Racing Robots Session
Dependencies: MbedJSONValue m3pi
This is the library for the Racing Robots session. It supports the M3PI robot of Polulu.
It is based on the "Arduino" principle of the init and loop function.
Just add a main.cpp file which contains:
Racing Robots main file
#include "robot_logic.h" void init() { //put your initialization logic here } void loop() { //put your robot control logic here }
Features include:
- Controlling the LEDS
- Move forward and backward
- Turn
- Read the sensor values
- Use a PID controller
robot_logic.cpp@8:597ce8a7d34b, 2015-06-01 (annotated)
- Committer:
- sillevl
- Date:
- Mon Jun 01 14:47:32 2015 +0000
- Revision:
- 8:597ce8a7d34b
- Parent:
- 7:a72215b1910b
Added support for start/stop via Xbee
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dwini | 0:c0ae66a0ec7a | 1 | #include "robot_logic.h" |
sillevl | 8:597ce8a7d34b | 2 | #include "xbee.h" |
dwini | 0:c0ae66a0ec7a | 3 | |
dwini | 2:356bb8d99326 | 4 | // Some defines |
dwini | 2:356bb8d99326 | 5 | #define MAX_SPEED 100 |
dwini | 2:356bb8d99326 | 6 | #define MAX_SENSOR 100 |
pcordemans | 7:a72215b1910b | 7 | #define MAX_REAL_SPEED 0.75 |
pcordemans | 7:a72215b1910b | 8 | |
pcordemans | 3:b5d18690f357 | 9 | #define MIN 0 |
dwini | 2:356bb8d99326 | 10 | |
dwini | 2:356bb8d99326 | 11 | // Static scope variables |
dwini | 2:356bb8d99326 | 12 | static m3pi m3pi; |
dwini | 0:c0ae66a0ec7a | 13 | |
sillevl | 8:597ce8a7d34b | 14 | #ifdef XBEE |
sillevl | 8:597ce8a7d34b | 15 | Xbee xbee(p28, p27); |
sillevl | 8:597ce8a7d34b | 16 | #endif |
sillevl | 8:597ce8a7d34b | 17 | |
dwini | 2:356bb8d99326 | 18 | // Static scope variables for keeping internal state |
dwini | 2:356bb8d99326 | 19 | static int internal_speed = 0; // [-100, +100] |
pcordemans | 6:0dc4e4225881 | 20 | static int internal_turnspeed = 0; // [-100, +100] |
dwini | 2:356bb8d99326 | 21 | static int internal_p = 0; |
dwini | 2:356bb8d99326 | 22 | static int internal_i = 0; |
dwini | 2:356bb8d99326 | 23 | static int internal_d = 0; |
dwini | 2:356bb8d99326 | 24 | static int internal_previous_pos_of_line = 0; |
pcordemans | 5:355240d7126b | 25 | static int internal_led_state = 0; |
dwini | 0:c0ae66a0ec7a | 26 | |
pcordemans | 6:0dc4e4225881 | 27 | void drive(int speed) |
pcordemans | 6:0dc4e4225881 | 28 | { |
dwini | 2:356bb8d99326 | 29 | if (speed > MAX_SPEED || speed < -MAX_SPEED) { |
pcordemans | 7:a72215b1910b | 30 | error("Speed out of range"); |
dwini | 0:c0ae66a0ec7a | 31 | return; |
dwini | 0:c0ae66a0ec7a | 32 | } |
dwini | 0:c0ae66a0ec7a | 33 | |
dwini | 2:356bb8d99326 | 34 | internal_speed = speed; |
dwini | 2:356bb8d99326 | 35 | |
sillevl | 8:597ce8a7d34b | 36 | if (speed == 0 || !canDrive()) { |
dwini | 0:c0ae66a0ec7a | 37 | m3pi.stop(); |
dwini | 0:c0ae66a0ec7a | 38 | } else if (speed > 0) { |
dwini | 2:356bb8d99326 | 39 | m3pi.forward(MAX_REAL_SPEED*speed/MAX_SPEED); |
dwini | 0:c0ae66a0ec7a | 40 | } else if (speed < 0) { |
dwini | 2:356bb8d99326 | 41 | m3pi.backward(MAX_REAL_SPEED*(-speed)/MAX_SPEED); |
dwini | 0:c0ae66a0ec7a | 42 | } |
dwini | 0:c0ae66a0ec7a | 43 | } |
dwini | 0:c0ae66a0ec7a | 44 | |
pcordemans | 6:0dc4e4225881 | 45 | void turn(int turnspeed) |
pcordemans | 6:0dc4e4225881 | 46 | { |
dwini | 2:356bb8d99326 | 47 | if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) { |
pcordemans | 7:a72215b1910b | 48 | error("Turn out of range"); |
dwini | 0:c0ae66a0ec7a | 49 | return; |
dwini | 0:c0ae66a0ec7a | 50 | } |
pcordemans | 6:0dc4e4225881 | 51 | internal_turnspeed = turnspeed; |
dwini | 2:356bb8d99326 | 52 | |
pcordemans | 6:0dc4e4225881 | 53 | float left = internal_speed; |
pcordemans | 6:0dc4e4225881 | 54 | float right = internal_speed; |
pcordemans | 6:0dc4e4225881 | 55 | |
pcordemans | 6:0dc4e4225881 | 56 | left -= turnspeed; |
pcordemans | 6:0dc4e4225881 | 57 | right += turnspeed; |
dwini | 2:356bb8d99326 | 58 | |
pcordemans | 6:0dc4e4225881 | 59 | if (right < MIN) |
pcordemans | 6:0dc4e4225881 | 60 | right = MIN; |
pcordemans | 6:0dc4e4225881 | 61 | else if (right > MAX_SPEED) |
pcordemans | 6:0dc4e4225881 | 62 | right = MAX_SPEED; |
dwini | 2:356bb8d99326 | 63 | |
pcordemans | 6:0dc4e4225881 | 64 | if (left < MIN) |
pcordemans | 6:0dc4e4225881 | 65 | left = MIN; |
pcordemans | 6:0dc4e4225881 | 66 | else if (left > MAX_SPEED) |
pcordemans | 6:0dc4e4225881 | 67 | left = MAX_SPEED; |
sillevl | 8:597ce8a7d34b | 68 | |
sillevl | 8:597ce8a7d34b | 69 | if(!canDrive()){ |
sillevl | 8:597ce8a7d34b | 70 | left = right = 0; |
sillevl | 8:597ce8a7d34b | 71 | } |
sillevl | 8:597ce8a7d34b | 72 | |
pcordemans | 6:0dc4e4225881 | 73 | m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED); |
pcordemans | 6:0dc4e4225881 | 74 | m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED); |
dwini | 0:c0ae66a0ec7a | 75 | } |
dwini | 0:c0ae66a0ec7a | 76 | |
pcordemans | 6:0dc4e4225881 | 77 | void stop(void) |
pcordemans | 6:0dc4e4225881 | 78 | { |
dwini | 0:c0ae66a0ec7a | 79 | m3pi.stop(); |
dwini | 0:c0ae66a0ec7a | 80 | } |
pcordemans | 6:0dc4e4225881 | 81 | void sensor_calibrate(void) |
pcordemans | 6:0dc4e4225881 | 82 | { |
dwini | 2:356bb8d99326 | 83 | m3pi.sensor_auto_calibrate(); |
dwini | 2:356bb8d99326 | 84 | } |
dwini | 2:356bb8d99326 | 85 | |
pcordemans | 6:0dc4e4225881 | 86 | int line_sensor(void) |
pcordemans | 6:0dc4e4225881 | 87 | { |
dwini | 0:c0ae66a0ec7a | 88 | // Get position of line [-1.0, +1.0] |
dwini | 0:c0ae66a0ec7a | 89 | float pos = m3pi.line_position(); |
dwini | 2:356bb8d99326 | 90 | return ((int)(pos * MAX_SENSOR)); |
dwini | 2:356bb8d99326 | 91 | } |
dwini | 2:356bb8d99326 | 92 | |
pcordemans | 6:0dc4e4225881 | 93 | void pid_init(int p, int i, int d) |
pcordemans | 6:0dc4e4225881 | 94 | { |
dwini | 2:356bb8d99326 | 95 | internal_p = p; |
dwini | 2:356bb8d99326 | 96 | internal_i = i; |
dwini | 2:356bb8d99326 | 97 | internal_d = d; |
dwini | 0:c0ae66a0ec7a | 98 | } |
dwini | 0:c0ae66a0ec7a | 99 | |
pcordemans | 6:0dc4e4225881 | 100 | int pid_turn(int line_position) |
pcordemans | 6:0dc4e4225881 | 101 | { |
dwini | 2:356bb8d99326 | 102 | float derivative, proportional, integral = 0; |
dwini | 2:356bb8d99326 | 103 | float power; |
pcordemans | 6:0dc4e4225881 | 104 | |
pcordemans | 7:a72215b1910b | 105 | proportional = line_position / 100.0; |
pcordemans | 6:0dc4e4225881 | 106 | |
dwini | 2:356bb8d99326 | 107 | // Compute the derivative |
dwini | 2:356bb8d99326 | 108 | derivative = line_position - internal_previous_pos_of_line; |
pcordemans | 6:0dc4e4225881 | 109 | |
dwini | 2:356bb8d99326 | 110 | // Compute the integral |
dwini | 2:356bb8d99326 | 111 | integral += proportional; |
pcordemans | 6:0dc4e4225881 | 112 | |
dwini | 2:356bb8d99326 | 113 | // Remember the last position. |
dwini | 2:356bb8d99326 | 114 | internal_previous_pos_of_line = line_position; |
pcordemans | 6:0dc4e4225881 | 115 | |
dwini | 2:356bb8d99326 | 116 | // Compute the power |
dwini | 2:356bb8d99326 | 117 | power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ; |
pcordemans | 6:0dc4e4225881 | 118 | |
pcordemans | 7:a72215b1910b | 119 | power = power * MAX_SPEED; |
pcordemans | 7:a72215b1910b | 120 | if (power < -MAX_SPEED) |
pcordemans | 7:a72215b1910b | 121 | power = -MAX_SPEED; |
pcordemans | 7:a72215b1910b | 122 | else if (power > MAX_SPEED) |
pcordemans | 7:a72215b1910b | 123 | power = MAX_SPEED; |
pcordemans | 7:a72215b1910b | 124 | return power ; |
dwini | 2:356bb8d99326 | 125 | } |
dwini | 0:c0ae66a0ec7a | 126 | |
pcordemans | 6:0dc4e4225881 | 127 | void show_stats(void) |
pcordemans | 6:0dc4e4225881 | 128 | { |
dwini | 0:c0ae66a0ec7a | 129 | m3pi.cls(); // Clear display |
dwini | 0:c0ae66a0ec7a | 130 | |
dwini | 0:c0ae66a0ec7a | 131 | // Display speed |
dwini | 0:c0ae66a0ec7a | 132 | m3pi.locate(0, 0); |
pcordemans | 6:0dc4e4225881 | 133 | m3pi.printf("S%d", internal_speed); |
pcordemans | 6:0dc4e4225881 | 134 | |
pcordemans | 6:0dc4e4225881 | 135 | // Display turn |
pcordemans | 6:0dc4e4225881 | 136 | m3pi.locate(4,0); |
pcordemans | 6:0dc4e4225881 | 137 | m3pi.printf("T%d", internal_turnspeed); |
dwini | 0:c0ae66a0ec7a | 138 | |
dwini | 0:c0ae66a0ec7a | 139 | // Display line |
dwini | 0:c0ae66a0ec7a | 140 | m3pi.locate(0, 1); |
dwini | 0:c0ae66a0ec7a | 141 | int line = line_sensor(); |
pcordemans | 6:0dc4e4225881 | 142 | m3pi.printf("POS %d", line); |
dwini | 0:c0ae66a0ec7a | 143 | } |
dwini | 0:c0ae66a0ec7a | 144 | |
pcordemans | 6:0dc4e4225881 | 145 | void show_name(char * name) |
pcordemans | 6:0dc4e4225881 | 146 | { |
dwini | 2:356bb8d99326 | 147 | m3pi.cls(); // Clear display |
dwini | 0:c0ae66a0ec7a | 148 | |
dwini | 2:356bb8d99326 | 149 | // Display speed |
dwini | 2:356bb8d99326 | 150 | m3pi.locate(0, 0); |
pcordemans | 6:0dc4e4225881 | 151 | // x The horizontal position, from 0 to 7 |
pcordemans | 6:0dc4e4225881 | 152 | // y The vertical position, from 0 to 1 |
dwini | 2:356bb8d99326 | 153 | m3pi.printf("%s", name); |
dwini | 2:356bb8d99326 | 154 | } |
dwini | 0:c0ae66a0ec7a | 155 | |
pcordemans | 6:0dc4e4225881 | 156 | void await(int milliseconds) |
pcordemans | 6:0dc4e4225881 | 157 | { |
dwini | 0:c0ae66a0ec7a | 158 | wait_ms(milliseconds); |
pcordemans | 5:355240d7126b | 159 | } |
pcordemans | 5:355240d7126b | 160 | |
pcordemans | 6:0dc4e4225881 | 161 | void led(LedIndex i, LedState state) |
pcordemans | 6:0dc4e4225881 | 162 | { |
pcordemans | 6:0dc4e4225881 | 163 | if(state == LED_ON) { |
pcordemans | 5:355240d7126b | 164 | internal_led_state |= (1 << i); |
pcordemans | 6:0dc4e4225881 | 165 | } else if(state == LED_OFF) { |
pcordemans | 5:355240d7126b | 166 | internal_led_state &= ~(1 << i); |
pcordemans | 6:0dc4e4225881 | 167 | } else if(state == LED_TOGGLE) { |
pcordemans | 5:355240d7126b | 168 | internal_led_state ^= (1 << i); |
pcordemans | 6:0dc4e4225881 | 169 | } else { |
pcordemans | 5:355240d7126b | 170 | error("Illegal LED state"); |
pcordemans | 5:355240d7126b | 171 | } |
pcordemans | 5:355240d7126b | 172 | m3pi.leds(internal_led_state); |
sillevl | 8:597ce8a7d34b | 173 | } |
sillevl | 8:597ce8a7d34b | 174 | |
sillevl | 8:597ce8a7d34b | 175 | int canDrive() |
sillevl | 8:597ce8a7d34b | 176 | { |
sillevl | 8:597ce8a7d34b | 177 | #ifdef XBEE |
sillevl | 8:597ce8a7d34b | 178 | return !xbee.stopped(); |
sillevl | 8:597ce8a7d34b | 179 | #endif |
sillevl | 8:597ce8a7d34b | 180 | return true; |
sillevl | 8:597ce8a7d34b | 181 | } |
sillevl | 8:597ce8a7d34b | 182 | |
sillevl | 8:597ce8a7d34b | 183 | |
sillevl | 8:597ce8a7d34b | 184 | void setCode(int code) |
sillevl | 8:597ce8a7d34b | 185 | { |
sillevl | 8:597ce8a7d34b | 186 | #ifdef XBEE |
sillevl | 8:597ce8a7d34b | 187 | xbee.setCode(code); |
sillevl | 8:597ce8a7d34b | 188 | #endif |
dwini | 0:c0ae66a0ec7a | 189 | } |