Robot Flowchart and Code

Here is the Code for the robot written out, and farther below is a link to the file with the robots original code

#pragma config(StandardModel, "RVW SQUAREBOT")

// This program goes forward until the sonarSensor sensor sees an object closer than our 'threshold' and will try to find another way.

task main() {

int threshold = 20; // sets the minimum distance the robot can be from the nearest obstruction that is in front of it.

while(true) { // this will ensure that it continues moving until it reaches the end at which the program will stop it

// this while loop determines if it is too close to an obstruction and needs to turn or if it can continue going strait

while(SensorValue(sonarSensor) > threshold || SensorValue(sonarSensor) == -1) { // while the sonarSensor sensor is reading in values larger than our threshold, or

//reading out of range (-1 = no object in sight)

// move forward

motor[rightMotor] = 100; // set right motor speed to 100

motor[leftMotor] = 100; // set left motor speed to 100

}

// stop the robot

motor[rightMotor] = 0; // stop right motor

motor[leftMotor] = 0; // stop left motor

int left = 0; // declaring the variable for the distance the robot is to the closest obstruction to the left

int right = 0; // declaring the variable for the distance the robot is to the closest obstruction to the right

// turn ~90 degrees to left

motor[rightMotor] = 100; // set right motor speed to 100

motor[leftMotor] = -100; // set left motor speed to -100

wait1Msec(816); // exact time needed for a ~90 degree turn to the left

motor[rightMotor] = 0; // stop right motor

motor[leftMotor] = 0; // stop left motor

left = SensorValue[sonarSensor]; // sets sonar value (distance to closest obstruction or -1) for left side

// turn ~180 degrees to right

motor[rightMotor] = -100; // set right motor speed to -100

motor[leftMotor] = 100; // set left motor speed to 100

wait1Msec(1632); // exact time needed for a ~180 degree turn to the right

motor[rightMotor] = 0; // stop right motor

motor[leftMotor] = 0; // stop left motor

right = SensorValue[sonarSensor]; // sets sonar value (distance to closest obstruction or -1) for right side

// at this point the robot is pointing to the right, but if the left side has more space to traverse, then it needs to turn ~180 degrees to the left so that it can move in

// the optimal direction

if(left > right || left < 0) {

motor[rightMotor] = 100; // set right motor speed to 100

motor[leftMotor] = -100; // set left motor speed to -100

wait1Msec(1632); // exact time needed for a ~180 degree turn to the left

motor[rightMotor] = 0; // stop right motor

motor[leftMotor] = 0; // stop left motor

}

wait1Msec(1000); // so that one can see what is happening more easily, but is not needed

} // now it will go back to the start of the infinite for loop so that is can begin the process once again as outlined in the flowchart

}