Program the robot to drive forwards and backwards

From ROBOTC API Guide
Jump to: navigation, search
ArduinoArduino Tutorials and Guided ProjectsVEX + Arduino, Mobile Robotics Platform → Program the robot to drive forwards and backwards

Configuring ROBOTC

Since we want to control the drive motors we need to configure ROBOTC so that it knows which pin controls which motor. First, create a new file. Once we have that we can tell ROBOTC that we are using the Arduino UNO by selecting it from the Platform Type menu under Robot. Now we can tell ROBOTC where the motors are connected. To do this we select the Motors tab.

The Motors tab in the Motors and Sensors setup window

In the Motor column you will find a list of all the Motor type outputs. You will notice that all the names are in the form of "servo_" followed by a number or "motor_" followed by a number. All the servo_ and motor_ items can control servos, however the motor_ items can control additional items, such as the variable intensity LED program we made earlier. The numbers at the end of the motor item refer to the pin that it uses. Since we have the drive servos connected to pins 10 and 11, we want the items that end with "_10" and "_11", which happen to be the first two items on the list. To make it easier to reference the drive servos while programing, we will name them. This is good practice and you should make a habit of. Since the left drive motor is connected to servo_11 we will name servo_11 "leftServo". So, of course, that means we will rename the right drive motor (servo_10) to "rightServo". This way, we don't need to think about which pin they are connected to, we just issue commands according to which side the motor is on. Now, we need to tell ROBOTC that we have continuous rotation servos connected to those ports. to do this we click the drop-down list in the rows for the servos and select Servo - Cont. Rotation for both motors.

We also need to let ROBOTC know that one of the motors is flipped. Since we have two motors facing in opposite directions, they will both turn the same direction if we tell the robot to go forwards. If they both turn the same direction, the robot will spin around because the motor has been flipped.

The motor flipping effect.

We can fix this issue by checking the 'reversed' box at the end of the row for servo_10 - RobotC will compensate for the flipping.

The drive servos are configured

Once this is all configured, we can submit by clicking OK. Your source code file should now contain the following at the top.

#pragma config(CircuitBoardType, typeCktBoardUNO)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Motor,  servo_10,        rightServo,    tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl10, None)
#pragma config(Motor,  motor_11,        leftServo,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl11, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


Programming the Behavior

For now, we just want the robot to drive forward for two seconds, stop for one second, drive backwards for two seconds then stop for another second. We want the robot to do this indefinitely so we can start by putting a while(true) loop in the task main().

#pragma config(CircuitBoardType, typeCktBoardUNO)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Motor,  servo_10,        rightServo,    tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl10, None)
#pragma config(Motor,  motor_11,        leftServo,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl11, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
task main() 
{
  while(true) //repeat indefinitely
  {
 
  }
}

Controlling the Servos

To control a motor, in this case the left drive servo, you would use

motor[leftServo] = power;

where power is an int (-127 to 127). When used with a motor (continuous rotation servo), this command is used to set the speed (-127 to +127), where negative values are reverse; positive forward, and zero is stopped. When used with a standard servo (VEX servo module), this command sets the position (-127 to +127) of the servo. Zero is the "center" position of the servo, and -127 to +127 is the range of the servo. Note that your servo may not be able to travel the full -127 to +127 range, so please experiment with smaller values to avoid breaking your servo.

Make the servos go forward

To make the robot drive forward, we need to set both servos to go forward at the same speed. To keep the robot from getting away from you we will keep the power level lower, at 50. You can adjust this if you would like your robot to go faster or slower. Let's start by setting the left motor to 50, then we can set the right motor to 50. Add a two second delay and we have the first part of the program complete. To do this, we use the code

motor[leftServo] = 50;   //Set the left servo to go forward at power level 50
motor[rightServo] = 50;  //Set the right servo to go forward at power level 50
wait1Msec(2000);         //pause code execution for 2000ms (2 seconds)

If we were to put this code into the while loop, it would just continue to drive forward. This is because the code would just keep looping over the "drive forward" code without any code to make it stop.

Make the servos stop

Since we need to make the robot stop for one second, we need to set both motors to 0, and have a one second delay after that. We can do this simply by changing each "50" to "0" and the "2000" to "1000", we get the following code, and have a code fragment to make the robot stop for one second.

motor[leftServo] = 0;    //Stop the left motor
motor[rightServo] = 0;   //Stop the right motor
wait1Msec(1000);         //pause code execution for 1000ms (1 second)

Make the robot go backward

Now, we have bits of code to make the robot go forward for two seconds, and code to make the robot stop for one second. However, we still need code to make the robot go backwards for two seconds. To do this, we can use the code to drive forward, but instead of setting the motors to a positive value to drive forward, we set them to a negative value (in this case we will use -20) to have them go in reverse.

motor[leftServo] = -50;  //Set the left servo to go backwards at power level -50 
motor[rightServo] = -50; //Set the right servo to go backwards at power level -50 
wait1Msec(2000);         //pause code execution for 2000ms (2 seconds)

Putting everything together

Now we have blocks of code to make the robot drive forward for two seconds, stop for one second, and to drive backwards for two seconds. However, we still need to put the blocks together to go forward, stop, go backwards, stop and repeat. Therefore, the next thing to do is put the above behavior code into the following program.

#pragma config(CircuitBoardType, typeCktBoardUNO)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Motor,  servo_10,        rightServo,    tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl10, None)
#pragma config(Motor,  motor_11,        leftServo,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl11, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
task main() 
{
  while(true) //repeat indefinitely
  {
    // code to drive forward for 2 seconds
 
 
    // code to stop the robot for 1 second
 
 
    // code to drive backwards for 2 seconds
 
 
    // code to stop the robot for 1 second
 
 
  }
}

Once everything is put together you end up with something like this.

#pragma config(CircuitBoardType, typeCktBoardUNO)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Motor,  servo_10,        rightServo,    tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl10, None)
#pragma config(Motor,  motor_11,        leftServo,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl11, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
task main() 
{
  while(true) //repeat indefinitely
  {
    // code to drive forward for 2 seconds
    motor[leftServo] = 50;   //Set the left servo to go forward at power level 50
    motor[rightServo] = 50;  //Set the right servo to go forward at power level 50
    wait1Msec(2000);         //pause code execution for 2000ms (2 seconds)
 
 
    // code to stop the robot for 1 second
    motor[leftServo] = 0;    //Stop the left servo
    motor[rightServo] = 0;   //Stop the right servo
    wait1Msec(1000);         //pause code execution for 1000ms (1 second)    
 
 
    // code to drive backwards for 2 seconds
    motor[leftServo] = -50;  //Set the left servo to go backwards at power level -50 
    motor[rightServo] =- 50; //Set the right servo to go backwards at power level -50 
    wait1Msec(2000);         //pause code execution for 2000ms (2 seconds)
 
 
    // code to stop the robot for 1 second
    motor[leftServo] = 0;    //Stop the left servo
    motor[rightServo] = 0;   //Stop the right servo
    wait1Msec(1000);         //pause code execution for 1000ms (1 second)
  }
}