Penn State Abington Spring 2013 Version 1 6
Penn State Abington Spring 2013 Version 1. 6. 1 Programming VEX Cortex Robotics with ROBOTC Topics • • Sensors Motors ROBOTC programming examples Sample VEX robot projects Spring 2013 Instructor: R. Avanzato
Robots 3 categories of robots 1. Teleoperated (remote control) 1. 2. 3. 4. 5. Human operated No sensors needed on robot predator May or may not need on-board computer VEX is capable of remote control operation with a transmitter Example: surgical robots (at least for now) Davinci surgery 2. Semi-Autonomous 1. 2. 3. 4. 5. Combination of human operation and computer control Computer can stop human from making bad control decision VEX is capable of semi-autonomous Some sensors required Microprocessor on robot required; software is required Mars rover 3. Autonomous 1. 2. 3. 4. 5. Computer controlled; no human or operator control VEX is capable of autonomous control Microprocessor on robot required; software is required Sensors required Software generally more sophisticated; may require AI Roomba vacuum robot 2
VEX Sensors • Sensors can be places into 2 general categories: – Digital (2 values: either “on” or “off”; 1 or 0) – Analog (continuous range of values ex: light intensity, temperature) • • Pushbutton digital (2 states: On or OFF; 1 or 0) Lever (limit) switch digital • • • Light sensor analog (0 to 1024) Line -following (light) sensors analog (0 to 1024) Ultrasonic/sonar sensor “analog”; needs special hookup (0 to 255 in) • • What are applications of each? Understand difference between “digital sensor” (on/off only) and “analog sensor” (continuous range of values). How does it affect software? • Each type of sensor is plugged into special ports on VEX 3
VEX Robot & Sensors (side view) servomotor Potentiometer (pot) Top push button Light sensor Cortex controller Sonar (Ultrasound) Wheel encoder Front bump (push button) sensor • PSU Abington VEX Squarebot with Sensors 4
VEX Robot & Sensors (back view) Servo potentiometer LCD screen Optical Wheel (shaft) Encoder battery On/off power switch (slide up-down) • PSU Abington VEX Squarebot with Sensors 5
VEX Robot & Sensors (bottom view) 3 floor light sensors Optical Wheel (shaft) Encoders (2) Drive motors (2) Wheels (4) • PSU Abington VEX Squarebot with Sensors 6
VEX Robot & Sensors (front view) Top bump sensor Light sensor Sonar (ultrasonic) Front bump sensor • PSU Abington VEX Squarebot with Sensors 7
Warning!!! • Program execution begins immediately after program is downloaded onto robot (unless you add a delay such as a 2 second delay) • This can present a hazard to you and the robot. Do not let robot fall off table. Be prepared to stop robot every rime you download program. • Robot will start moving wheels or moving arm immediately • Place a delay or start button logic to allow you to place robot on floror before execution • Wear safety glasses if you have a robot with a moving arm 8
ROBOTC Guide • Connect robot to PC; turn on robot; last program stored will execute!!! • Open ROBOTC Always set Platform Type = VEX Cortex (default) • In ROBOTC, select “Robot” VEX Cortex Comm. Mode USB only • Use debugger windows (shows all motor, sensor and variable values) • ROBOTC has automatic formatting feature (sets proper indentation) …click on the magic wand. . use this feature • Start with Sample Code or use this PPT; build on examples • Configure all sensors in ROBOTC Select Robot Motors and Sensors Setup • Save all copies of programs; email to other team members; share on Angel, Google, etc. • First lines of every program must be “#pragma config” statements 9
VEX Guide Examples (ROBOTC) 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11. 12. Moving Forward with Time Moving Forward and Reverse with Time Bump Sensor Sonar (Ultrasonic) Sensor (ver 1 & ver 2) FOR loop Encoder (ver 1 & ver 2) Light sensor Pushbutton to start robot (ver 1 & ver 2) VEX Speaker (sounds and music) LCD (character display) Multi-robot communication (Xbee) with message ID 10
VEX Cortex Robot Ports Right Motor: Left Motor: Top Servo: Front Servo: Motor 2 Motor 3 Motor 4 Motor 5 Light sensor (front): Analog 1 Right line sensor: Analog 2 Center line sensor: Analog 3 Left line sensor: Analog 4 Potentiometer: Analog 5 Front Touch Sensor: Digital 1 Top Touch sensor: Digital 2 Sonar Input: Digital 3 Sonar Output: Digital 4 Right Encoder Front: Digital 5 Right Encoder Back: Digital 6 Left Encoder Front: Digital 7 Left Encoder Back: Digital 8 LED [Color may vary]: Digital 12 Xbee UART 1 LCD: UART 2 LCD RX: Yellow from Y cable LCD TX: White from Y cable 11
VEX Cortex Robot Ports #pragma config(Motor, port 2, right. Motor, tmotor. Servo. Continuous. Rotation, open. Loop, reversed #pragma config(Motor, port 3, left. Motor, tmotor. Servo. Continuous. Rotation, open. Loop) #pragma config(Motor, port 4, top. Servo, tmotor. Servo. Standard, open. Loop) #pragma config(Motor, port 5, front. Servo, tmotor. Servo. Standard, open. Loop) #pragma config(Sensor, in 1, light 1, sensor. Reflection) #pragma config(Sensor, in 2, line. Right, sensor. Line. Follower) #pragma config(Sensor, in 3, line. Center, sensor. Line. Follower) #pragma config(Sensor, in 4, line. Left, sensor. Line. Follower) #pragma config(Sensor, in 5, pot, sensor. Potentiometer) #pragma config(Sensor, dgtl 1, front. Touch, sensor. Touch) #pragma config(Sensor, dgtl 2, top. Touch, sensor. Touch) #pragma config(Sensor, dgtl 3, sonar, sensor. SONAR_inch) #pragma config(Sensor, dgtl 5, encoder. Right 1, sensor. Quad. Encoder) #pragma config(Sensor, dgtl 7, encoder. Left 1, sensor. Quad. Encoder) #pragma config(Sensor, dgtl 12, led, sensor. LEDto. VCC) //*!!Code automatically generated by 'ROBOTC' configuration wizard 12
Robot. C Program General Format #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) #pragma config …… define sensors #pragma config …… define sensors // comments …documentation task main() { Instructions; // comments } //Program ends, and the robot stops NOTE: pragma config statements must be at top of program (before any comments statements) 13
VEX Example #1: Move Forward with Time #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) task main() { wait 1 Msec(2000); //Robot waits for 2000 milliseconds (2 seconds) //Move forward at full power for 3 seconds, then stop motor[right. Motor] = 127; // run at full (127) power forward motor[left. Motor] = 127; // run at full (127) power forward wait 1 Msec(3000); //Wait 3000 ms (3 sec) -- motors stay on!! } //Program ends, and the robot stops // Note: be careful with parentheses and square brackets Questions: • Why is motor on port 2 reversed? What does that mean? What would happen if you omitted this? What if no motor is plugged into port 2? If right and left motors reversed? • Motor speed ranges from 127 (max fwd) to 0 (stop) to -127 (max reverse) • How do you make a robot turn? Does it depend on chassis construction? • What is command to wait 1. 5 seconds? 0. 76 seconds? 10. 5 seconds? 14
VEX Example #2 FWD and REV with Time #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) task main() { wait 1 Msec(2000); //Robot waits for 2000 milliseconds (2 seconds) //Move forward for 3 seconds (half power) motor[right. Motor] = 63; motor[left. Motor] = 63; wait 1 Msec(3000); //Move reverse for 2 seconds motor[right. Motor] = -63; motor[left. Motor] = - 63; wait 1 Msec(2000); } // How do you turn (pivot) robot? //Program ends, and the robot stops 15
VEX Program Example #3 Bump Sensor #pragma config(Sensor, dgtl 1, front. Touch, sensor. Touch) #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// // Move forward until robot hits front bump sensor, then stop // Assumes front bump sensor is connected to digital input port 1 // bump sensor value = 0 when not pressed; value = 1 when pressed (digital sensor) task main() { while (Sensor. Value[ front. Touch ] == 0) // loop until button is pressed { motor[right. Motor] = 63; motor[left. Motor] = 63; } } // stop motors when program ends NOTE: In ROBOTC, configure sensors by selecting Robot Motors and Sensors Setup 16
VEX Program Example #4 Sonar (v 1) #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) #pragma config(Sensor, dgtl 3, sonar, sensor. SONAR_inch) // Assume Square. Bot robot with sonar sensor on front of robot // Move robot forward for until ultrasonic (sonar) sensor detects object with 24 inches, // then, robot stops; Range of sonar 0 255 inches (sonar returns -1 when out of range) task main() { wait 1 Msec(2000); //Robot waits for 2 sec before executing program while(Sensor. Value[sonar] > 24) //Loop while further than 24 inches away { motor[right. Motor] = 63; // You can change the speed motor[left. Motor] = 63; } } // Robot stops when program ends // Question: How do you deal with out-of-range (-1) condition? 17
VEX Program Example #4 Sonar (v 2) #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) #pragma config(Sensor, dgtl 3, sonar, sensor. SONAR_inch) // Same operation as version 1, but turn off motors explicitly at end of program for clarity // This is unnecessary because motors are always turned off when program ends task main() { wait 1 Msec(2000); //Robot waits for 2 sec before executing program while(Sensor. Value[sonar] > 24) //Loop while further than 24 inches away { motor[right. Motor] = 63; motor[left. Motor] = 63; } motor[right. Motor] = 0; // turn off motors explicitly (optional in this case) motor[left. Motor] = 0 ; } // Note: you can explicitly turn off motors (as in above) for clarity, or all motors will be turned off automatically when program ends. 18
VEX Program Example #5 For Loop #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) // Move robot forward for 2 seconds, the reverse robot for 2 seconds , then repeat total of 5 times, // then, robot stops //NOTE: use a “for” loop when you want to repeat a fixed number of times task main() { wait 1 Msec(1000); //Robot waits for 1000 milliseconds (1 second) before starting for ( int k = 0; k < 5; k = k + 1) // loop (repeat 5 times; while k is less than 5) // k is a loop counter variable which is initially set at 0, and // increments by 1 for each loop; when k reaches 5, then the loop stops { motor[right. Motor] = 63; //Move forward for 2 seconds (half power) motor[left. Motor] = 63; wait 1 Msec(2000); motor[right. Motor] = -63; //Move reverse for 2 seconds motor[left. Motor] = -63; wait 1 Msec(2000); } } // in the above example, if ‘k < 5’ was changed to ‘k < 10’ then the loop would repeat 10 times 19
VEX Example #6 Encoder (v 1) #pragma config(Sensor, dgtl 5, encoder. Right 1, sensor. Rotation) #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) // Assume Square. Bot robot with at least one encoder connected of robot // An encoder (also called shaft encoder, optical encoder) counts the number of times an axle or shaft or wheel (depends on gears) rotates // For the VEX kit, the encoder counts 100 ticks every time an axle makes one complete revolution // Encoders can be used to move more precisely (why? ) // Encoders can be connected to one motor shaft or both motor shafts. // GOAL: Move forward until encoder reaches 100 counts (you can change this to any value), then stops task main() { wait 1 Msec(2000); // waits for 2000 milliseconds before executing program Sensor. Value[encoder. Right 1] = 0; // reset encoder value to 0 while (Sensor. Value[encoder. Right 1] < 100) // move forward until encoder count >= 100 { motor[right. Motor] = 63; motor[left. Motor] = 63; } } // Robot stops when program ends // Note: there is some overshoot when motor power is set to off. How to fix? 20
VEX Example #6 Encoder (v 2) #pragma config(Sensor, dgtl 5, encoder. Right 1, sensor. Rotation) #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) // Assume Square. Bot robot with at least one encoder connected of robot /// GOAL: Move forward until encoder reaches 100 counts (you can change this to any value), then rotate robot for 50 counts, then stop robot task main() { wait 1 Msec(2000); // waits for 2000 milliseconds before executing program Sensor. Value[encoder. Right 1] = 0; // reset encoder value to 0 while (Sensor. Value[encoder. Right 1] < 100) // move forward until encoder count >= 100 { motor[right. Motor] = 63; motor[left. Motor] = 63; } Sensor. Value[encoder. Right 1] = 0; // reset encoder value to 0 ****** important step ***** while (Sensor. Value[encoder. Right 1] < 50) // move forward until encoder count >= 100 { motor[right. Motor] = 63; motor[left. Motor] = -63; } } // Robot stops when program ends // Note: there is some overshoot when motor power is set to off. How to fix? 21
VEX Example #7 Light Sensor #pragma config(Sensor, in 1, light 1, sensor. Reflection) #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) // A light sensor is plugged into any of the analog ports of the VEX controller // There are 2 sets of light sensors on the VEX robots; 1 ) fwd facing light sensor (in 1) and 2) a set of 3 downward line following light sensors (in 2, in 3, in 4) // Note: light sensor values (0 = bright light; 1024 = dark) task main() { wait 1 Msec(1000); //Robot waits for 1 s before executing program while (Sensor. Value[ light 1 ] < 500) // while bright is light, go forward { motor[right. Motor] = 63; motor[left. Motor] = 63; } } // when light gets dark, loop ends, and program ends and motors stop 22
VEX Example #8 A : Pushbutton To Start Code (v 1) #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) #pragma config(Sensor, dgtl 2, top. Touch, sensor. Touch) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// // Use pushbutton at top of robot to start the program instead of using time delay // Assume pushbutton at top of robot is plugged into digital input port 2 task main() { while (Sensor. Value [top. Touch] == 0) // loop while button is not pressed { } // do nothing (just wait) wait 1 Msec(1000); motor[right. Motor] = 63; // go forward after the button is pressed by user motor[left. Motor] = 63; wait 1 Msec(1000); // go for 1 second , then stop when program ends } 23
VEX Example #8 B : Pushbutton To Start Code (v 2) #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) #pragma config(Sensor, dgtl 2, top. Touch, sensor. Touch) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// // Use pushbutton at top of robot to start the program instead of using time delay // Assume pushbutton at top of robot is plugged into digital input port 2 task main() { while ( 1 ) // loop forever { if (Sensor. Value [top. Touch] == 1) break; } wait 1 Msec(1000); motor[right. Motor] = 63; // go forward after the button is pressed by user motor[left. Motor] = 63; wait 1 Msec(1000); // go for 1 second , then stop when program ends } 24
VEX Example #9 Speaker (sounds) task main() { //Basic "Play Sound" commands Play. Sound(sound. Beep); wait 1 Msec(200); //Play a tone: //First Parameter: Frequence in Hz //Second Parameter: Length to play in 1/100 th of a seconds (50 =. 5 seconds) Play. Tone(440, 50); wait 1 Msec(500); //Play a Sound File (need to use the File Management to Upload First) Play. Sound. File("1. wav"); wait 1 Msec(1000); } Note: see ROBOTC documentation for more examples and songs for speaker http: //www. robotc. net/wiki/Tutorials/VEX_Speaker_module 25
VEX Example #10 LCD Display #pragma config(Sensor, in 1, light 1, sensor. Reflection) // A light sensor is plugged into any of the analog ports of the VEX controller // This program display the value of the light sensor on the VEX robot LCD display // Note: light sensor values (0 = bright light; 1024 = dark) task main() { wait 1 Msec(1000); //Robot waits for 1 s before executing program display. LCDCentered. String(0, "Light. Sensor"); // display message on line 0 while(true) { display. LCDNumber(1, 0, Sensor. Value(light 1)); //display value for the light sensor wait 1 Msec(200); clear. LCDLine(1); // clear screen and repeat (clear line 1) } } Note: You can display the values of any sensor or display any message to the LCD screen. It is not necessary to put the display command in a loop – it depends on the application. There are 2 lines on the display (designated 0 and 1) and there is a maximum of only 16 characters (letters or numbers) which can be displayed on each line. 26
VEX Example #11 A Multi-Robot (XBee) Robot A #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) #include "Xbee. Tools. h " // need this line for Multi-robot task main() { Init. RS 232(uart. One, baud. Rate 9600); // need this line for Multi-robot wait 1 Msec(1000); string message = “Robot. B Go"; Send. String(message); // send message to Robot B to move motor[right. Motor] = 63; motor[left. Motor] = 63; wait 1 Msec(1000); //Move forward for 1 second } //Program ends, and the robot stops 27
VEX Example #11 B Multi-Robot (XBee) Robot B #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) #include "Xbee. Tools. h " // need this line for Multi-robot task main() { Init. RS 232(uart. One, baud. Rate 9600); // need this line for Multi-robot wait 1 Msec(500); string message; Receive. String(message); // wait here for any message from Robot A, then go motor[right. Motor] = 63; motor[left. Motor] = 63; wait 1 Msec(1000); //Move forward for 1 second } //Program ends, and the robot stops 28
VEX Example #12 Multi-Robot (XBee) Robot B with ID #pragma config(Motor, port 2, right. Motor, tmotor. Normal, open. Loop, reversed) #pragma config(Motor, port 3, left. Motor, tmotor. Normal, open. Loop) #include "Xbee. Tools. h " // need this line for Multi-robot task main() // this version of Robot B waits for a message with unique ID { Init. RS 232(uart. One, baud. Rate 9600); // need this line for Multi-robot wait 1 Msec(500); string message; // declare a variable which can store a string (text) while(true) // keep repeating forever (or until you receive correct message) { Receive. String(message); // wait for a message from Robot. A; store in message if (String. Find(message, "Robot. B") >= 0) // if message contains “Robot. B” then move; { // otherwise, wait for another message motor[right. Motor] = 63; motor[left. Motor] = 63; wait 1 Msec(1000); //Move forward for 1 second break; // break out of while loop and stop robot } 29 } } //Program ends, and the robot stops
VEX Example #12 MR String Commands • • Xbee wireless commands always send and receive strings (not numbers). Remember, when you transmit a command, every robot within 300 feet will receive it – be careful. (Try to use unique robot names and put names in commands) Some numbers must be converted to string before transmitting, and strings need to be converted back to numbers after receiving them. It is sometimes useful to send compound commands, such as “FWD 500” to command another robot to move forward for 500 milliseconds, etc. Here is another example of a compound command “JOE FWD 500” this tells robot named JOE to go forward for 500 milliseconds A string is an array of characters. The first character is at index = 0 Reference: http: //www. robotc. net/firewiki/index. php? title=Creating, _Sending_and_Parsing_Parameterized_Messages Here are the important string functions for MR communication • • string message; // declare variable message to be a string Send. String(message); // transmits string and continue to next line Receive. String (message); // waits here until a string is received String. Format( message, “FWD%d”, time); //inserts integer into string String. Find( message, “JOE”); // returns index of where string is found. This function returns a -1 if string is not found; if != -1, then found int t = atoi( my. String ); // atoi() converts string into integer number 30 String. Delete(input. String, start. Pos, size); // deletes portion of a string;
VEX Example #12 MR String Commands Example #1 string message; int count = 350; String. Format(message, “%d”, count); // would produce string “ 350” (stored in message) Example #2 int count = 350; String. Format(message, “FWD%d”, count); // would produce string “FWD 350” Example #3 if (String. Find (message, “FWD”) != -1) //must have been found { go. Forward(); } Example #4 if (String. Find (message, “FWD”) != -1) // “FWD” must have been found in message { Delete. String(message, 0, 3); // delete the “FWD” part of the string (start at 0, delete 3 chars) int count = atoi(message); // convert string number into an integer } 31
Sample Programming Problems 1. 2. 3. 4. 5. Robot moves forward for 5 seconds, then stops Robot pivots for 3 seconds, then stops Robot moves forward until robot hits bump sensor, then stops Robot pivots until sonar senses object within 2 feet Robot moves forward for 2 seconds, stops for 1 second, then repeats these two actions 5 times, then stops 6. Robot moves forward for 2 seconds, then reverse for 2 seconds…. repeats these 2 tasks forever (robot never stops) 7. Robot follows a white line on a black background 8. Robot moves to a white line, then stops 9. Robot moves forward and counts white lines on surface 10. Robot moves a fixed distance using encoders • • • Be able to write C code for these examples – and extend It is not necessary to have any background in C programming to do these exercises; ask instructor for help if needed. Reference: ROBOTC website 32
- Slides: 32
