Navigating the simple maze using switches

From ROBOTC API Guide
Jump to: navigation, search
ArduinoArduino Tutorials and Guided ProjectsVEX + Arduino, Mobile Robotics Platform → Navigating the simple maze using switches

Concepts

We have already navigated a maze using timing. However, that only works it you know that the path will always be the same length. What if you made the maze 2 times bigger? The times to drive straight would no longer be correct. We can solve this problem by using the whiskers to detect the walls of the maze. By using the walls to find the distance, it is possible to change the lengths of the straight paths to just about any value. This works since the robot only needs to turn where there is a wall. So you just replace the time based pause with one that waits for a wall, and add some code to back way from the wall before turning and you are good to go.

Configuration

The robot configuration for this program will be the same as the one in the previous program, where the robot avoided obstacles. leftServo on pin 11, rightServo reversed on pin 10, leftSwitch Digital Input on pin 8, and rightSwitch Digital Input on pin 7.


Programming

We need to add a function to back up away from the wall so that we don't hit it when turning. For this function, let's go backward at speed 40 for 400 ms.

//drive backward at speed 40 for 400 ms to back away from the wall
void DriveBackward()
{
  motor[leftServo] = -40;
  motor[rightServo] = -40;
  wait1Msec(400);
}

Now we need to take the code to navigate the maze and remove the wait in the drive forward function and replace it with the code to wait for the whiskers to find a wall and code to back away from the wall. So

// drive forward at speed 40 for the passed time in milliseconds
void DriveForward(int time)
{
  motor[leftServo] = 40;
  motor[rightServo] = 40;
  wait1Msec(time);
}

becomes something like

// drive forward at speed 40 until a wall is found
void DriveForwardUntilWall()
{
  motor[leftServo] = 40;
  motor[rightServo] = 40;
  while(SensorValue[leftSwitch] == 0 && SensorValue[rightSwitch] == 0) {}
  DriveBakward(); //could be replaced with the code in the function
}

Notice that this if statement has the "&&" Boolean logic operator 'and' which only returns true if the values on both sides evaluate to true.

Notepad.gif TODO: add link to the details for the boolean operators.

Putting it all together

Now all that remains is to remove the time parameters from the calls to "DriveForward()" in task main(). Once you have done that you end up with the following program. Keep in mind that, as before, you may need to tune the power values to each motor to keep it driving straight and accurately.

#pragma config(CircuitBoardType, typeCktBoardUNO)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Sensor, dgtl8,  leftSwitch,     sensorTouch)
#pragma config(Sensor, dgtl7,  rightSwitch,    sensorTouch)
#pragma config(Motor,  motor_11,        leftServo,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl10, None)
#pragma config(Motor,  servo_10,        rightServo,    tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl11, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
//Function to drive backwards
void DriveBackward()
{
  motor[leftServo] = -40;
  motor[rightServo] = -40;
  wait1Msec(400);
}
 
//Function to drive forwards until a wall is hit
void DriveForwardUntilWall()
{
  motor[leftServo] = 40;
  motor[rightServo] = 40;
 
  //Test to see if either switch has been pressed. If so, reverse.
  while(SensorValue[leftSwitch] == 0 && SensorValue[rightSwitch] == 0) {}
  DriveBackward();
}
 
//Function to turn right
void TurnRight()
{
  motor[leftServo] = 40;
  motor[rightServo] = -40;
  wait1Msec(470);  //will probably need to adjust time for your robot
}
 
//Function to turn left
void TurnLeft()
{
  motor[leftServo] = -40;
  motor[rightServo] = 40;
  wait1Msec(475);  //will probably need to adjust time for your robot
}
 
//Function to stop the robot
void Stop()
{
  motor[leftServo] = 0;
  motor[rightServo] = 0;
}
 
task main()
{
  //wait 1 seconds before starting.
  wait1Msec(1000);
 
  //Sequence of events to complete the maze.
  DriveForwardUntilWall();
  TurnLeft();
 
  DriveForwardUntilWall();
  TurnRight();
 
  DriveForwardUntilWall();
  TurnRight();
 
  DriveForwardUntilWall();
  Stop();
}