Using a set of switches to detect and avoid obstacles

From ROBOTC API Guide
Jump to: navigation, search
ArduinoArduino Tutorials and Guided ProjectsVEX + Arduino, Mobile Robotics Platform → Tutorials/Arduino Projects/Mobile Robotics/VEX/Obstacle Detection and Avoidance using a Switch

Concepts

Since we know how a VEX switch can be used to start a program, let's expand on that and use that same idea, to detect when the robot hits a wall. Since a momentary switch will be always be in the same state (in this case open) when not pressed, then if we setup a switch to be "pressed" whenever the robot body is about to hit the wall, we can use the state of the switch to detect the wall and stop driving forward. With a little extra code, we can even make the robot back up and avoid the wall.

Setup

We will be using VEX limit switches to detect obstacles. You will need to set them up so that one is upside down, as there is no way to reverse them and they are all manufactured the same way. Here is a series of images showing you how to construct a simple 'whisker' apparatus for the front of the Swervebot that will be able to detect obstacles in front of the robot.


Step 1
Step 2
Step 3
Step 5
Completed
Completed

Wiring

To wire the VEX limit switches, you will need four wires.

schematic diagram for the whisker switches
Virtual breadboard layout
breadboard layout

Configuration

Since the Robot has 2 switches we need to configure both of them. The first one will be for pin 7 and will be named "rightSwitch". The second will be pin 8 and will be named "leftSwitch". Since we want to be able to drive, we also need to configure the drive servos. One that is done, your code file should have something like the following code at the top.

#pragma config(CircuitBoardType, typeCktBoardUNO)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Sensor, dgtl8,  leftSwitch,     sensorDigitalIn)
#pragma config(Sensor, dgtl7,  rightSwitch,    sensorDigitalIn)
#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               !!*//

Programming

Now that everything is configured, it is time to plan the behavior. We want the robot to start of driving forward at speed 50. If the robot detects a wall via the whiskers, then it will back up for 0.5 seconds, make a left point turn for 0.5 seconds, then resume driving forward. The robot should do this indefinitely. Since we want it to continue indefinitely, we know that we will need a while loop, But how do we make the code change what it is doing based on sensor inputs? To do this we need to use an If statement. There are several forms of If statements, but for this code we will be using the base form:

if (condition)
{
  //code to run if condition is true
}

Notepad.gif TODO: add link to the details of the if statement.

By placing an if statement inside the while loop, and checking the whisker states, we can make the program respond to a whisker detecting a wall. However, we have 2 whiskers, so how do we check the state of both inside of the if statements' condition? We use something called Boolean logic. There are several Boolean logic operators, but for now, all we care about is the 'or' operator "||". The or operator will return true if one or both of values to either side of the operator evaluate to true.

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

task main()
{
  while(true)
  {
    if (SensorValue[leftSwitch] == true || SensorValue[rightSwitch] == true)
    {
      //code to avoid the wall
    }
 
    //drive forward at speed 50
    motor[leftServo] = 50;
    motor[rightServo] = 50;
  }
}

Now we just place the "Avoid Wall" code inside the If statement, and we get the following code.

task main()
{
  while(true)
  {
    //Check that either whisker is pressed. If true, get away from the wall with the following code.
    if (SensorValue[leftSwitch] == 0 || SensorValue[rightSwitch] == 0)
    {
      //drive backwards at speed 50 for 0.5 seconds
      motor[leftServo] = -50;
      motor[rightServo] = -50;
      wait1Msec(500);
 
      //left point turn at speed 50 for 0.5 seconds
      motor[leftServo] = -50;
      motor[rightServo] = 50;
      wait1Msec(500);
    }
 
    //drive forward at speed 50
    motor[leftServo] = 50;
    motor[rightServo] = 50;
  }
}