Sille Van Landschoot / Mbed 2 deprecated RacingRobotsLib

Racing Robots Session

Dependencies:   m3pi mbed

Dependents:   RacingRobots

Fork of racing_robots by Nico De Witte

robot_logic.cpp@5:355240d7126b, 2015-02-24 (annotated)

Committer:
pcordemans
Date:
Tue Feb 24 09:38:40 2015 +0000
Revision:
5:355240d7126b
Parent:
3:b5d18690f357
Child:
6:0dc4e4225881
added led implementation; cleaned up error messages

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dwini 0:c0ae66a0ec7a 1 #include "robot_logic.h"
dwini 0:c0ae66a0ec7a 2
dwini 2:356bb8d99326 3 // Some defines
dwini 2:356bb8d99326 4 #define MAX_SPEED 100
dwini 2:356bb8d99326 5 #define MAX_SENSOR 100
dwini 2:356bb8d99326 6 #define MAX_REAL_SPEED 1.0
pcordemans 3:b5d18690f357 7 #define MAX 0.3
pcordemans 3:b5d18690f357 8 #define MIN 0
dwini 2:356bb8d99326 9
dwini 2:356bb8d99326 10 // Static scope variables
dwini 2:356bb8d99326 11 static m3pi m3pi;
dwini 0:c0ae66a0ec7a 12
dwini 2:356bb8d99326 13 // Static scope variables for keeping internal state
dwini 2:356bb8d99326 14 static int internal_speed = 0; // [-100, +100]
dwini 2:356bb8d99326 15 static int internal_turn_speed = 0; // [-100, +100]
dwini 2:356bb8d99326 16 static int internal_p = 0;
dwini 2:356bb8d99326 17 static int internal_i = 0;
dwini 2:356bb8d99326 18 static int internal_d = 0;
dwini 2:356bb8d99326 19 static int internal_previous_pos_of_line = 0;
pcordemans 5:355240d7126b 20 static int internal_led_state = 0;
dwini 0:c0ae66a0ec7a 21
dwini 0:c0ae66a0ec7a 22 /*
dwini 0:c0ae66a0ec7a 23 * Drive the robot forward or backward.
dwini 2:356bb8d99326 24 * If the robot was turning it will stop turning and drive in a straight line.
dwini 0:c0ae66a0ec7a 25 *
dwini 0:c0ae66a0ec7a 26 * @speed The speed percentage with which to drive forward or backward.
dwini 0:c0ae66a0ec7a 27 * Can range from -100 (full throttle backward) to +100 (full throttle forward).
dwini 0:c0ae66a0ec7a 28 */
dwini 0:c0ae66a0ec7a 29 void drive(int speed) {
dwini 2:356bb8d99326 30 if (speed > MAX_SPEED || speed < -MAX_SPEED) {
dwini 0:c0ae66a0ec7a 31 printf("[ERROR] Drive speed out of range");
dwini 0:c0ae66a0ec7a 32 return;
dwini 0:c0ae66a0ec7a 33 }
dwini 0:c0ae66a0ec7a 34
dwini 2:356bb8d99326 35 internal_speed = speed;
dwini 2:356bb8d99326 36
dwini 0:c0ae66a0ec7a 37 if (speed == 0) {
dwini 0:c0ae66a0ec7a 38 m3pi.stop();
dwini 0:c0ae66a0ec7a 39 } else if (speed > 0) {
dwini 2:356bb8d99326 40 m3pi.forward(MAX_REAL_SPEED*speed/MAX_SPEED);
dwini 0:c0ae66a0ec7a 41 } else if (speed < 0) {
dwini 2:356bb8d99326 42 m3pi.backward(MAX_REAL_SPEED*(-speed)/MAX_SPEED);
dwini 0:c0ae66a0ec7a 43 }
dwini 0:c0ae66a0ec7a 44 }
dwini 0:c0ae66a0ec7a 45
dwini 0:c0ae66a0ec7a 46 /*
dwini 2:356bb8d99326 47 * Turn the robot left or right while driving.
dwini 0:c0ae66a0ec7a 48 *
dwini 2:356bb8d99326 49 * @turnspeed The percentage with which to turn the robot.
dwini 0:c0ae66a0ec7a 50 * Can range from -100 (full throttle left) to +100 (full throttle right).
dwini 0:c0ae66a0ec7a 51 */
dwini 0:c0ae66a0ec7a 52 void turn(int turnspeed) {
dwini 2:356bb8d99326 53 if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
pcordemans 5:355240d7126b 54 error("Speed out of range");
dwini 0:c0ae66a0ec7a 55 return;
dwini 0:c0ae66a0ec7a 56 }
dwini 0:c0ae66a0ec7a 57
dwini 2:356bb8d99326 58 internal_turn_speed = turnspeed;
dwini 2:356bb8d99326 59
dwini 2:356bb8d99326 60 if (turnspeed == 0) { // Drive straight
dwini 2:356bb8d99326 61 drive(internal_speed);
dwini 2:356bb8d99326 62 } else {
dwini 2:356bb8d99326 63 // int left = 0;
dwini 2:356bb8d99326 64 // int right = 0;
dwini 2:356bb8d99326 65 // if (internal_speed > 0) { // Right | Left forward
dwini 2:356bb8d99326 66 // left = internal_speed + turnspeed / 2;
dwini 2:356bb8d99326 67 // right = internal_speed - turnspeed / 2;
dwini 2:356bb8d99326 68
dwini 2:356bb8d99326 69 // if (left > MAX_SPEED) {
dwini 2:356bb8d99326 70 // left = MAX_SPEED;
dwini 2:356bb8d99326 71 // right = MAX_SPEED - turnspeed;
dwini 2:356bb8d99326 72 // } else if (right > MAX_SPEED) {
dwini 2:356bb8d99326 73 // right = MAX_SPEED;
dwini 2:356bb8d99326 74 // left = MAX_SPEED + turnspeed;
dwini 2:356bb8d99326 75 // }
dwini 2:356bb8d99326 76 // }
dwini 2:356bb8d99326 77 // else if (internal_speed < 0) { // Right | Left backward
dwini 2:356bb8d99326 78 // left = internal_speed - turnspeed / 2;
dwini 2:356bb8d99326 79 // right = internal_speed + turnspeed / 2;
dwini 2:356bb8d99326 80
dwini 2:356bb8d99326 81 // if (left < -MAX_SPEED) {
dwini 2:356bb8d99326 82 // left = -MAX_SPEED;
dwini 2:356bb8d99326 83 // right = -MAX_SPEED - turnspeed;
dwini 2:356bb8d99326 84 // } else if (right < -MAX_SPEED) {
dwini 2:356bb8d99326 85 // right = -MAX_SPEED;
dwini 2:356bb8d99326 86 // left = -MAX_SPEED + turnspeed;
dwini 2:356bb8d99326 87 // }
dwini 2:356bb8d99326 88 // }
dwini 2:356bb8d99326 89
dwini 2:356bb8d99326 90 // Compute new speeds
dwini 2:356bb8d99326 91 int right = internal_speed+turnspeed;
dwini 2:356bb8d99326 92 int left = internal_speed-turnspeed;
dwini 2:356bb8d99326 93
dwini 2:356bb8d99326 94 // limit checks
dwini 2:356bb8d99326 95 if (right < MIN)
dwini 2:356bb8d99326 96 right = MIN;
dwini 2:356bb8d99326 97 else if (right > MAX)
dwini 2:356bb8d99326 98 right = MAX;
dwini 2:356bb8d99326 99
dwini 2:356bb8d99326 100 if (left < MIN)
dwini 2:356bb8d99326 101 left = MIN;
dwini 2:356bb8d99326 102 else if (left > MAX)
dwini 2:356bb8d99326 103 left = MAX;
dwini 2:356bb8d99326 104
dwini 2:356bb8d99326 105 m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED);
dwini 2:356bb8d99326 106 m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED);
dwini 0:c0ae66a0ec7a 107 }
dwini 0:c0ae66a0ec7a 108 }
dwini 0:c0ae66a0ec7a 109
dwini 0:c0ae66a0ec7a 110 /*
dwini 0:c0ae66a0ec7a 111 * Stop the robot.
dwini 0:c0ae66a0ec7a 112 */
dwini 0:c0ae66a0ec7a 113 void stop(void) {
dwini 0:c0ae66a0ec7a 114 m3pi.stop();
dwini 0:c0ae66a0ec7a 115 }
dwini 0:c0ae66a0ec7a 116
dwini 0:c0ae66a0ec7a 117 /*
dwini 2:356bb8d99326 118 * Calibrate the line follow sensors.
dwini 2:356bb8d99326 119 * Take note that the pololu should be placed over the line
dwini 2:356bb8d99326 120 * before this function is called and that it will rotate to
dwini 2:356bb8d99326 121 * both sides.
dwini 2:356bb8d99326 122 */
dwini 2:356bb8d99326 123 void sensor_calibrate(void) {
dwini 2:356bb8d99326 124 m3pi.sensor_auto_calibrate();
dwini 2:356bb8d99326 125 }
dwini 2:356bb8d99326 126
dwini 2:356bb8d99326 127 /*
dwini 0:c0ae66a0ec7a 128 * Read the value from the line sensor. The returned value indicates the
dwini 0:c0ae66a0ec7a 129 * position of the line. The value ranges from -100 to +100 where -100 is
dwini 0:c0ae66a0ec7a 130 * fully left, +100 is fully right and 0 means the line is detected in the middle.
dwini 0:c0ae66a0ec7a 131 *
dwini 0:c0ae66a0ec7a 132 * @return The position of the line with a range of -100 to +100.
dwini 0:c0ae66a0ec7a 133 */
dwini 0:c0ae66a0ec7a 134 int line_sensor(void) {
dwini 0:c0ae66a0ec7a 135 // Get position of line [-1.0, +1.0]
dwini 0:c0ae66a0ec7a 136 float pos = m3pi.line_position();
dwini 2:356bb8d99326 137 return ((int)(pos * MAX_SENSOR));
dwini 2:356bb8d99326 138 }
dwini 2:356bb8d99326 139
dwini 2:356bb8d99326 140 /*
dwini 2:356bb8d99326 141 * Initialize the PID drive control with
dwini 2:356bb8d99326 142 * the P, I and T factors.
dwini 2:356bb8d99326 143 *
dwini 2:356bb8d99326 144 * @p The P factor
dwini 2:356bb8d99326 145 * @i The I factor
dwini 2:356bb8d99326 146 * @d The D factor
dwini 2:356bb8d99326 147 */
dwini 2:356bb8d99326 148 void pid_init(int p, int i, int d) {
dwini 2:356bb8d99326 149 internal_p = p;
dwini 2:356bb8d99326 150 internal_i = i;
dwini 2:356bb8d99326 151 internal_d = d;
dwini 0:c0ae66a0ec7a 152 }
dwini 0:c0ae66a0ec7a 153
dwini 2:356bb8d99326 154 /*
dwini 2:356bb8d99326 155 * Determine PID turnspeed with which the pololu should
dwini 2:356bb8d99326 156 * turn to follow the line at the given position.
dwini 2:356bb8d99326 157 *
dwini 2:356bb8d99326 158 * @line_position The position of the line in a range of [-100, +100]
dwini 2:356bb8d99326 159 *
dwini 2:356bb8d99326 160 * @returns The turnspeed in a range of [-100, +100]
dwini 2:356bb8d99326 161 */
dwini 2:356bb8d99326 162 int pid_turn(int line_position) {
dwini 2:356bb8d99326 163 float right;
dwini 2:356bb8d99326 164 float left;
dwini 2:356bb8d99326 165
dwini 2:356bb8d99326 166 float derivative, proportional, integral = 0;
dwini 2:356bb8d99326 167 float power;
dwini 2:356bb8d99326 168 float speed = internal_speed;
dwini 2:356bb8d99326 169
dwini 2:356bb8d99326 170 proportional = line_position;
dwini 2:356bb8d99326 171
dwini 2:356bb8d99326 172 // Compute the derivative
dwini 2:356bb8d99326 173 derivative = line_position - internal_previous_pos_of_line;
dwini 2:356bb8d99326 174
dwini 2:356bb8d99326 175 // Compute the integral
dwini 2:356bb8d99326 176 integral += proportional;
dwini 2:356bb8d99326 177
dwini 2:356bb8d99326 178 // Remember the last position.
dwini 2:356bb8d99326 179 internal_previous_pos_of_line = line_position;
dwini 2:356bb8d99326 180
dwini 2:356bb8d99326 181 // Compute the power
dwini 2:356bb8d99326 182 power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ;
dwini 2:356bb8d99326 183
dwini 2:356bb8d99326 184 return power;
dwini 2:356bb8d99326 185 }
dwini 0:c0ae66a0ec7a 186
dwini 0:c0ae66a0ec7a 187 // Show drive speed and sensor data
dwini 2:356bb8d99326 188 void show_stats(void) {
dwini 0:c0ae66a0ec7a 189 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 190
dwini 0:c0ae66a0ec7a 191 // Display speed
dwini 0:c0ae66a0ec7a 192 m3pi.locate(0, 0);
dwini 0:c0ae66a0ec7a 193 // x The horizontal position, from 0 to 7
dwini 0:c0ae66a0ec7a 194 // y The vertical position, from 0 to 1
dwini 0:c0ae66a0ec7a 195 m3pi.printf("FOR 100%%");
dwini 0:c0ae66a0ec7a 196
dwini 0:c0ae66a0ec7a 197 // Display line
dwini 0:c0ae66a0ec7a 198 m3pi.locate(0, 1);
dwini 0:c0ae66a0ec7a 199 int line = line_sensor();
dwini 0:c0ae66a0ec7a 200 m3pi.printf("LINE %d", line);
dwini 0:c0ae66a0ec7a 201 }
dwini 0:c0ae66a0ec7a 202
dwini 2:356bb8d99326 203 /*
dwini 2:356bb8d99326 204 * Shows the name of the robot on the display.
dwini 2:356bb8d99326 205 *
dwini 2:356bb8d99326 206 * @name C character string (null-terminated) with the name of the robot (max 8 chars)
dwini 2:356bb8d99326 207 */
dwini 2:356bb8d99326 208 void show_name(char * name) {
dwini 2:356bb8d99326 209 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 210
dwini 2:356bb8d99326 211 // Display speed
dwini 2:356bb8d99326 212 m3pi.locate(0, 0);
dwini 2:356bb8d99326 213 // x The horizontal position, from 0 to 7
dwini 2:356bb8d99326 214 // y The vertical position, from 0 to 1
dwini 2:356bb8d99326 215 m3pi.printf("%s", name);
dwini 2:356bb8d99326 216 }
dwini 0:c0ae66a0ec7a 217
dwini 0:c0ae66a0ec7a 218
dwini 0:c0ae66a0ec7a 219 /*
dwini 0:c0ae66a0ec7a 220 * Wait for an approximate number of milliseconds.
dwini 0:c0ae66a0ec7a 221 *
dwini 0:c0ae66a0ec7a 222 * @milliseconds The number of milliseconds to wait.
dwini 0:c0ae66a0ec7a 223 */
dwini 2:356bb8d99326 224 void await(int milliseconds) {
dwini 0:c0ae66a0ec7a 225 wait_ms(milliseconds);
pcordemans 5:355240d7126b 226 }
pcordemans 5:355240d7126b 227
pcordemans 5:355240d7126b 228
pcordemans 5:355240d7126b 229 void led(LedIndex i, LedState state) {
pcordemans 5:355240d7126b 230 if(state == LED_ON){
pcordemans 5:355240d7126b 231 internal_led_state |= (1 << i);
pcordemans 5:355240d7126b 232 }
pcordemans 5:355240d7126b 233 else if(state == LED_OFF) {
pcordemans 5:355240d7126b 234 internal_led_state &= ~(1 << i);
pcordemans 5:355240d7126b 235 }
pcordemans 5:355240d7126b 236 else if(state == LED_TOGGLE){
pcordemans 5:355240d7126b 237 internal_led_state ^= (1 << i);
pcordemans 5:355240d7126b 238 }
pcordemans 5:355240d7126b 239 else{
pcordemans 5:355240d7126b 240 error("Illegal LED state");
pcordemans 5:355240d7126b 241 }
pcordemans 5:355240d7126b 242 m3pi.leds(internal_led_state);
dwini 0:c0ae66a0ec7a 243 }