Driving forwards and backwards
|
Video
Configuring ROBOTC
Since we want to control the drive servos, we need to configure ROBOTC so that it knows which pins control the motors. Like always, we need to create a new file. Then we can tell ROBOTC that we are using the Arduino UNO by selecting it from the Platform Type menu under Robot. Now we open up the Motors and Sensors Setup window and specify that the DFRobot Motor controller board is connected and apply the settings by clicking the Apply button. Now we can tell ROBOTC where the motors are connected. To do this we select the Motors tab.
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 also control additional component types. 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 5 and 6, we want the items that end with "_5" and "_6", 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 according to the servo they control. Since the right drive motor is connected to motor_5 we will name motor_ 5 "rightServo". So of course, that means that we will name the left drive motor (motor_ 6) "leftServo". This way we don't need to think about which pin they are connected to, we just think of which servo/motor it is. 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 Int. HBridge for both servos. Because the robot's configuration has both of the LEGO motors assembled "backwards" relative to the front of the robot (where all of the sensors have been placed), we need to reverse both of the motors by clicking on the two little boxes at the far right of the screen under "Reversed." What this does is automatically change the sign of any inputs that go to the servos. For example, a command that sets the speed of the motor to 100 would actually be read to the servo as -100. But, for the sake of the programmer's sanity, the value coded into the program is positive, since the servo would actually be going "forward" relative to the robot.
NOTE: You may find that your "rightServo" actually commands the left motor and that your "leftServo" commands the right motor. If this is the case, simply switch which motor pins are named "rightServo" and "leftServo"
Now that we have that all set, we can submit the configuration by clicking OK. Your source code file should now contain the following at the top.
#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!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, and 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(PluginCircuitBoard, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!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 from -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.
We found that for the LEGO motors, the appropriate range of speeds was from 80 to 127 and from -80 to -127. Any speed with a magnitude less than 80 was not powerful enough to overcome friction, and should not be used in order to prevent damage to your motors. Do not worry if you type in a value of greater magnitude than + or - 127, as RobotC automatically limits speed to + or - 127.
Make the servos go forward
To make the robot drive forward, we need to set both servos to go forward at the same speed. So that the robot does not get away from you, lets keep the power level at 100. We can start by setting the left motor to 100, then set the right motor to 100. Add a two second delay and we have the first part of the goal complete. To do this, we use the code
motor[leftServo] = 100; //Set the left servo to go forward at power level 100 motor[rightServo] = 100; //Set the right servo to go forward at power level 100 wait1Msec(2000); //pause code execution for 2000ms (2 seconds) |
However, 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 want to make the robot stop for one second, we need to set both servos to 0 and have a one second delay until the next command. By changing each "100" 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; //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
So 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 -100) to have them go in reverse.
motor[leftServo] = -100; //Set the left servo to go backwards at power level -100 (absolute power of 100) motor[rightServo] =- 100; //Set the right servo to go backwards at power level -100 (absolute power of 100) wait1Msec(2000); //pause code execution for 2000ms (2 seconds) |
Putting everything together
So 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, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!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(PluginCircuitBoard, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// task main() { while(true) //repeat indefinitely { // code to drive forward for 2 seconds motor[leftServo] = 100; //Set the left servo to go forward at power level 100 motor[rightServo] = 100; //Set the right servo to go forward at power level 100 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] = -100; //Set the left servo to go backwards at power level -100 (absolute power of 100) motor[rightServo] =- 100; //Set the right servo to go backwards at power level -100 (absolute power of 100) 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) } } |
