Thread: Struggling with VEX programing

I'm trying to right a program to make my VEX robot move through a simple obstacle. It consists of 4, 2 by 4 pieces of lumber, set on their side. I have available a sonar sensor and 2 button sensors. I want to "walk" the maze by going in the "entrance" and coming out the "exit". Then start over on the other side.

Here's what I have so far. Any help would be greatly appreciated I'm very lost with this.

Code:

#pragma config(Sensor, in7, Ultrasonic, sensorSONAR, int1)

Code:

#pragma config(Sensor, in1, touchSensor1, sensorTouch)
#pragma config(Sensor, in2, touchSensor2, sensorTouch)




/*************************************************************************
 vex 5/9
 *************************************************************************/


void MoveForward()             //Robot moves forward at 65
{
    motor[port2] = 65;
    motor[port3] = 65;
    motor[port4] = 65;
    motor[port5] = 65;
}


void MoveForwardTillBump()
{
        if(SensorValue(touchSensor1) == 1)
        {
            motor[port2] = 0;
            motor[port3] = 0;
            motor[port4] = 0;
            motor[port5] = 0;
        }
}




void MoveReverse2(int msec)   //Robot moves forward at 65
{
    motor[port2] = -90;
    motor[port3] = -60;
    motor[port4] = -90;
    motor[port5] = -60;
}


void MoveReverse()             //Robot moves in reverse
{
    motor[port2] = -90;
    motor[port3] = -60;
    motor[port4] = -90;
    motor[port5] = -60;
}


void StopMotors()              //Stops motors
{
    motor[port2] = 0;
    motor[port3] = 0;
    motor[port4] = 0;
    motor[port5] = 0;
}


void TurnAround(int msec)      //Robot turns around
{
StopMotors();
    wait1Msec(500);
    motor[port2] = 40;
    motor[port3] = -35;
    motor[port4] = 40;
    motor[port5] = -35;
    wait1Msec(msec);
StopMotors();
    wait1Msec(500);
}


void RotateRobot()
{
    motor[port2] = 40;
    motor[port3] = -40;
    motor[port4] = 40;
    motor[port5] = -40;

    wait1Msec(0140);
}


void TurnRobot90()
{
    motor[port2] = 40;
    motor[port3] = -40;
    motor[port4] = 40;
    motor[port5] = -40;

    wait1Msec(0035);
}

void TurnRobot270()
{
        motor[port2] = 40;
        motor[port3] = -40;
        motor[port4] = 40;
        motor[port5] = -40;

        wait1Msec(0105);
}

void MoveFurthest()
{
    int passes;
    int distance;
    int maxDistance;
    int passesAtMax;


    do
    {
        StopMotors();
    } while (SensorValue(touchSensor1) == 0);

    do
    {
        distance = SensorValue(Ultrasonic);
        if(distance > maxDistance)
        {
            maxDistance = distance;
            passesAtMax = passes;
        }

        RotateRobot();
        passes = passes + 1;
    } while(passes < 12);

StopMotors();
    wait1Msec(2000);

    do
    {
        RotateRobot();
    } while (passes < passesAtMax);

StopMotors();
    wait1Msec(2000);

    do
    {
        MoveForward();
    }while (SensorValue(touchSensor1) == 0);

StopMotors();


}


task main()
{
    wait1Msec(2000);            //Robot waits for 2000 milliseconds before executing program
    bMotorReflected[port2] = 1;    //Reflects the direction of the motor on port2
    bMotorReflected[port4] = 1; //Reflects trhe direction of the motor on port4

    int passes;
    int distance;
    int maxDistance;
    int passesAtMax;

    passes = 0;
    maxDistance = 0;

    do
    {
MoveForwardTillBump();
        wait1Msec();
MoveReverse2(0200);
        wait1Msec();
MoveFurthest();

        if (passes > 0 && < 6)
        {
            TurnRobot270();
            MoveForward();
        }
        else if (passes > 6 && < 12)
        {
            MoveForwardTillBump();
            MoveReverse2(0100);
            TurnRobot90();
            MoveForward();
        }

    }while (passes < 20);


}

So have you managed the simpler task of say moving in a line and stopping when the sensor activates?

Or did you just try and write the whole thing, and then wonder why it didn't work?