#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
}