Program the robot to drive forwards and backwards
|
Configuring ROBOTC
Before we start programming for the servos we need to configure ROBOTC so that it knows which pins control the motors. Open an new filed and change the Platform Type (under the Robot menu in ROBOTC) to Arduino UNO.
Next, open the Motors and Sensors Setup window and change the Controller Board Type under the Controller Board tab to Arduino Uno and click Apply. Then, click on the Motors tab to bring up the Motors 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_" 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 an Audio Speaker). 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 change the items that end with "_10" and "_11" (which happen to be the top two items). Since the left drive servo is connected to servo_10 we will name it "leftServo"; the right drive servo (servo_11) will be named "rightServo". This will help make coding much simpler.
Now we need to tell ROBOTC that we have continuous rotation servos connected to those ports. To do this, click the drop-down list in the rows for the servos and select Servo - Cont. Rotation for both servos. We also need to let ROBOTC know that right servo is flipped by checking the box at the end of the row for servo_11.
We are reversing servo_11 due to the way the servos are mounted. Each servo will rotate the same direction with respect to itself when sent the same value. While it is good that we don't have to manually check which way is forward for each individual servo, the problem arises when the servos are 'mirrored' from one another, as is the case in the BoeBot. If you look at each servo's 'hubcap' while it is moving forward, you will notice that both are rotating in the same direction (clockwise). However, since the servos are mirrored from one another and 'flipped', one of the servos (in this case, the right servo) is spinning the robot backwards. As such when you send both servos the same signal, one will move the robot forward and the other will move it backwards, resulting in the robot spinning in place. By telling ROBOTC that the right servo is reversed, it will flip the "direction" of the signal going to the right servo, allowing the same forward command to move the robot straight forward.
We can now apply the configuration to the program by clicking Apply or OK. Your source code file should now contain the following at the top.
#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldParallaxBoeBot) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, servo_10, leftServo, tmotorServoContinuousRotation, openLoop, IOPins, dgtl10, None) #pragma config(Motor, servo_11, rightServo, tmotorServoContinuousRotation, openLoop, reversed, 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 will start by putting a while(true) loop in the task main().
#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldParallaxBoeBot) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, servo_10, leftServo, tmotorServoContinuousRotation, openLoop, IOPins, dgtl10, None) #pragma config(Motor, servo_11, rightServo, tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl11, None) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// task main() { while(true) //repeat indefinitely { } } |
Controlling the Servos
To control a servo, in this case the left drive servo, you would use
motor[leftServo] = power; |
where power is an integer (-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, 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 moving too quickly we will use a power level of 20. Setting both motors to a power level of 20 and adding a two second delay will complete the first part of the goal:
motor[leftServo] = 20; //Set the left servo to go forward at power level 20 motor[rightServo] = 20; //Set the right servo to go forward at power level 20 wait1Msec(2000); //pause code execution for 2000ms (2 seconds) |
If we were to put this code into the while loop, it would continue to just drive forward. This is because the code would 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 servos to 0 and have a one second wait command. We can use a modified version of the 'drive forward' code by changing each servo power level to "0" and the 2000 ms wait value to 1000 ms:
motor[leftServo] = 0; //Set the left servo stop motor[rightServo] = 0; //Set the right servo stop wait1Msec(1000); //Pause code execution for 1000ms (1 second) |
Make the servos go backward
We have segments of code to make the robot go forward for two seconds and stop for one second. However, we still need code to make the robot go backwards for two seconds. To do this, we can again use the code to drive forward, but instead of setting the servos to a positive value we will set them to a negative value (in this case we will use -20) to have them go in reverse.
motor[leftServo] = -20; //Set the left servo to go backwards at power level -20 (absolute power of 20) motor[rightServo] =- 20; //Set the right servo to go backwards at power level -20 (absolute power of 20) 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. So what we need to do is put the above behavior code into the following program.
#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldParallaxBoeBot) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, servo_10, leftServo, tmotorServoContinuousRotation, openLoop, IOPins, dgtl10, None) #pragma config(Motor, servo_11, rightServo, tmotorServoContinuousRotation, openLoop, reversed, 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 organized together you end up with something like this:
#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldParallaxBoeBot) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, servo_10, leftServo, tmotorServoContinuousRotation, openLoop, IOPins, dgtl10, None) #pragma config(Motor, servo_11, rightServo, tmotorServoContinuousRotation, openLoop, reversed, 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] = 20; //Set the left servo to go forward at power level 20 motor[rightServo] = 20; //Set the right servo to go forward at power level 20 wait1Msec(2000); //pause code execution for 2000ms (2 seconds) // code to stop the robot for 1 second motor[leftServo] = 0; //Set the left servo stop motor[rightServo] = 0; //Set the right servo stop wait1Msec(1000); //pause code execution for 1000ms (1 second) // code to drive backwards for 2 seconds motor[leftServo] = -20; //Set the left servo to go backwards at power level -20 (absolute power of 20) motor[rightServo] =- 20; //Set the right servo to go backwards at power level -20 (absolute power of 20) wait1Msec(2000); //pause code execution for 2000ms (2 seconds) // code to stop the robot for 1 second motor[leftServo] = 0; //Set the left servo stop motor[rightServo] = 0; //Set the right servo stop wait1Msec(1000); //pause code execution for 1000ms (1 second) } } |
NOTE: Due to the design of the Parallax Boe Shield, you will need to move the switch to position 2 for the servos to work.
