Sille Van Landschoot
/
RacingRobotsLib
Racing Robots Session
Fork of racing_robots by
robot_logic.cpp@3:b5d18690f357, 2015-02-24 (annotated)
- Committer:
- pcordemans
- Date:
- Tue Feb 24 09:01:06 2015 +0000
- Revision:
- 3:b5d18690f357
- Parent:
- 2:356bb8d99326
- Child:
- 5:355240d7126b
added mbed and m3pi sublibraries
Who changed what in which revision?
User | Revision | Line number | New 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; |
dwini | 0:c0ae66a0ec7a | 20 | |
dwini | 0:c0ae66a0ec7a | 21 | /* |
dwini | 0:c0ae66a0ec7a | 22 | * Drive the robot forward or backward. |
dwini | 2:356bb8d99326 | 23 | * If the robot was turning it will stop turning and drive in a straight line. |
dwini | 0:c0ae66a0ec7a | 24 | * |
dwini | 0:c0ae66a0ec7a | 25 | * @speed The speed percentage with which to drive forward or backward. |
dwini | 0:c0ae66a0ec7a | 26 | * Can range from -100 (full throttle backward) to +100 (full throttle forward). |
dwini | 0:c0ae66a0ec7a | 27 | */ |
dwini | 0:c0ae66a0ec7a | 28 | void drive(int speed) { |
dwini | 2:356bb8d99326 | 29 | if (speed > MAX_SPEED || speed < -MAX_SPEED) { |
dwini | 0:c0ae66a0ec7a | 30 | printf("[ERROR] Drive 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 | |
dwini | 0:c0ae66a0ec7a | 36 | if (speed == 0) { |
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 | |
dwini | 0:c0ae66a0ec7a | 45 | /* |
dwini | 2:356bb8d99326 | 46 | * Turn the robot left or right while driving. |
dwini | 0:c0ae66a0ec7a | 47 | * |
dwini | 2:356bb8d99326 | 48 | * @turnspeed The percentage with which to turn the robot. |
dwini | 0:c0ae66a0ec7a | 49 | * Can range from -100 (full throttle left) to +100 (full throttle right). |
dwini | 0:c0ae66a0ec7a | 50 | */ |
dwini | 0:c0ae66a0ec7a | 51 | void turn(int turnspeed) { |
dwini | 2:356bb8d99326 | 52 | if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) { |
dwini | 0:c0ae66a0ec7a | 53 | printf("[ERROR] Turn speed out of range"); |
dwini | 0:c0ae66a0ec7a | 54 | return; |
dwini | 0:c0ae66a0ec7a | 55 | } |
dwini | 0:c0ae66a0ec7a | 56 | |
dwini | 2:356bb8d99326 | 57 | internal_turn_speed = turnspeed; |
dwini | 2:356bb8d99326 | 58 | |
dwini | 2:356bb8d99326 | 59 | if (turnspeed == 0) { // Drive straight |
dwini | 2:356bb8d99326 | 60 | drive(internal_speed); |
dwini | 2:356bb8d99326 | 61 | } else { |
dwini | 2:356bb8d99326 | 62 | // int left = 0; |
dwini | 2:356bb8d99326 | 63 | // int right = 0; |
dwini | 2:356bb8d99326 | 64 | // if (internal_speed > 0) { // Right | Left forward |
dwini | 2:356bb8d99326 | 65 | // left = internal_speed + turnspeed / 2; |
dwini | 2:356bb8d99326 | 66 | // right = internal_speed - turnspeed / 2; |
dwini | 2:356bb8d99326 | 67 | |
dwini | 2:356bb8d99326 | 68 | // if (left > MAX_SPEED) { |
dwini | 2:356bb8d99326 | 69 | // left = MAX_SPEED; |
dwini | 2:356bb8d99326 | 70 | // right = MAX_SPEED - turnspeed; |
dwini | 2:356bb8d99326 | 71 | // } else if (right > MAX_SPEED) { |
dwini | 2:356bb8d99326 | 72 | // right = MAX_SPEED; |
dwini | 2:356bb8d99326 | 73 | // left = MAX_SPEED + turnspeed; |
dwini | 2:356bb8d99326 | 74 | // } |
dwini | 2:356bb8d99326 | 75 | // } |
dwini | 2:356bb8d99326 | 76 | // else if (internal_speed < 0) { // Right | Left backward |
dwini | 2:356bb8d99326 | 77 | // left = internal_speed - turnspeed / 2; |
dwini | 2:356bb8d99326 | 78 | // right = internal_speed + turnspeed / 2; |
dwini | 2:356bb8d99326 | 79 | |
dwini | 2:356bb8d99326 | 80 | // if (left < -MAX_SPEED) { |
dwini | 2:356bb8d99326 | 81 | // left = -MAX_SPEED; |
dwini | 2:356bb8d99326 | 82 | // right = -MAX_SPEED - turnspeed; |
dwini | 2:356bb8d99326 | 83 | // } else if (right < -MAX_SPEED) { |
dwini | 2:356bb8d99326 | 84 | // right = -MAX_SPEED; |
dwini | 2:356bb8d99326 | 85 | // left = -MAX_SPEED + turnspeed; |
dwini | 2:356bb8d99326 | 86 | // } |
dwini | 2:356bb8d99326 | 87 | // } |
dwini | 2:356bb8d99326 | 88 | |
dwini | 2:356bb8d99326 | 89 | // Compute new speeds |
dwini | 2:356bb8d99326 | 90 | int right = internal_speed+turnspeed; |
dwini | 2:356bb8d99326 | 91 | int left = internal_speed-turnspeed; |
dwini | 2:356bb8d99326 | 92 | |
dwini | 2:356bb8d99326 | 93 | // limit checks |
dwini | 2:356bb8d99326 | 94 | if (right < MIN) |
dwini | 2:356bb8d99326 | 95 | right = MIN; |
dwini | 2:356bb8d99326 | 96 | else if (right > MAX) |
dwini | 2:356bb8d99326 | 97 | right = MAX; |
dwini | 2:356bb8d99326 | 98 | |
dwini | 2:356bb8d99326 | 99 | if (left < MIN) |
dwini | 2:356bb8d99326 | 100 | left = MIN; |
dwini | 2:356bb8d99326 | 101 | else if (left > MAX) |
dwini | 2:356bb8d99326 | 102 | left = MAX; |
dwini | 2:356bb8d99326 | 103 | |
dwini | 2:356bb8d99326 | 104 | m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED); |
dwini | 2:356bb8d99326 | 105 | m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED); |
dwini | 0:c0ae66a0ec7a | 106 | } |
dwini | 0:c0ae66a0ec7a | 107 | } |
dwini | 0:c0ae66a0ec7a | 108 | |
dwini | 0:c0ae66a0ec7a | 109 | /* |
dwini | 0:c0ae66a0ec7a | 110 | * Stop the robot. |
dwini | 0:c0ae66a0ec7a | 111 | */ |
dwini | 0:c0ae66a0ec7a | 112 | void stop(void) { |
dwini | 0:c0ae66a0ec7a | 113 | m3pi.stop(); |
dwini | 0:c0ae66a0ec7a | 114 | } |
dwini | 0:c0ae66a0ec7a | 115 | |
dwini | 0:c0ae66a0ec7a | 116 | /* |
dwini | 2:356bb8d99326 | 117 | * Calibrate the line follow sensors. |
dwini | 2:356bb8d99326 | 118 | * Take note that the pololu should be placed over the line |
dwini | 2:356bb8d99326 | 119 | * before this function is called and that it will rotate to |
dwini | 2:356bb8d99326 | 120 | * both sides. |
dwini | 2:356bb8d99326 | 121 | */ |
dwini | 2:356bb8d99326 | 122 | void sensor_calibrate(void) { |
dwini | 2:356bb8d99326 | 123 | m3pi.sensor_auto_calibrate(); |
dwini | 2:356bb8d99326 | 124 | } |
dwini | 2:356bb8d99326 | 125 | |
dwini | 2:356bb8d99326 | 126 | /* |
dwini | 0:c0ae66a0ec7a | 127 | * Read the value from the line sensor. The returned value indicates the |
dwini | 0:c0ae66a0ec7a | 128 | * position of the line. The value ranges from -100 to +100 where -100 is |
dwini | 0:c0ae66a0ec7a | 129 | * fully left, +100 is fully right and 0 means the line is detected in the middle. |
dwini | 0:c0ae66a0ec7a | 130 | * |
dwini | 0:c0ae66a0ec7a | 131 | * @return The position of the line with a range of -100 to +100. |
dwini | 0:c0ae66a0ec7a | 132 | */ |
dwini | 0:c0ae66a0ec7a | 133 | int line_sensor(void) { |
dwini | 0:c0ae66a0ec7a | 134 | // Get position of line [-1.0, +1.0] |
dwini | 0:c0ae66a0ec7a | 135 | float pos = m3pi.line_position(); |
dwini | 2:356bb8d99326 | 136 | return ((int)(pos * MAX_SENSOR)); |
dwini | 2:356bb8d99326 | 137 | } |
dwini | 2:356bb8d99326 | 138 | |
dwini | 2:356bb8d99326 | 139 | /* |
dwini | 2:356bb8d99326 | 140 | * Initialize the PID drive control with |
dwini | 2:356bb8d99326 | 141 | * the P, I and T factors. |
dwini | 2:356bb8d99326 | 142 | * |
dwini | 2:356bb8d99326 | 143 | * @p The P factor |
dwini | 2:356bb8d99326 | 144 | * @i The I factor |
dwini | 2:356bb8d99326 | 145 | * @d The D factor |
dwini | 2:356bb8d99326 | 146 | */ |
dwini | 2:356bb8d99326 | 147 | void pid_init(int p, int i, int d) { |
dwini | 2:356bb8d99326 | 148 | internal_p = p; |
dwini | 2:356bb8d99326 | 149 | internal_i = i; |
dwini | 2:356bb8d99326 | 150 | internal_d = d; |
dwini | 0:c0ae66a0ec7a | 151 | } |
dwini | 0:c0ae66a0ec7a | 152 | |
dwini | 2:356bb8d99326 | 153 | /* |
dwini | 2:356bb8d99326 | 154 | * Determine PID turnspeed with which the pololu should |
dwini | 2:356bb8d99326 | 155 | * turn to follow the line at the given position. |
dwini | 2:356bb8d99326 | 156 | * |
dwini | 2:356bb8d99326 | 157 | * @line_position The position of the line in a range of [-100, +100] |
dwini | 2:356bb8d99326 | 158 | * |
dwini | 2:356bb8d99326 | 159 | * @returns The turnspeed in a range of [-100, +100] |
dwini | 2:356bb8d99326 | 160 | */ |
dwini | 2:356bb8d99326 | 161 | int pid_turn(int line_position) { |
dwini | 2:356bb8d99326 | 162 | float right; |
dwini | 2:356bb8d99326 | 163 | float left; |
dwini | 2:356bb8d99326 | 164 | |
dwini | 2:356bb8d99326 | 165 | float derivative, proportional, integral = 0; |
dwini | 2:356bb8d99326 | 166 | float power; |
dwini | 2:356bb8d99326 | 167 | float speed = internal_speed; |
dwini | 2:356bb8d99326 | 168 | |
dwini | 2:356bb8d99326 | 169 | proportional = line_position; |
dwini | 2:356bb8d99326 | 170 | |
dwini | 2:356bb8d99326 | 171 | // Compute the derivative |
dwini | 2:356bb8d99326 | 172 | derivative = line_position - internal_previous_pos_of_line; |
dwini | 2:356bb8d99326 | 173 | |
dwini | 2:356bb8d99326 | 174 | // Compute the integral |
dwini | 2:356bb8d99326 | 175 | integral += proportional; |
dwini | 2:356bb8d99326 | 176 | |
dwini | 2:356bb8d99326 | 177 | // Remember the last position. |
dwini | 2:356bb8d99326 | 178 | internal_previous_pos_of_line = line_position; |
dwini | 2:356bb8d99326 | 179 | |
dwini | 2:356bb8d99326 | 180 | // Compute the power |
dwini | 2:356bb8d99326 | 181 | power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ; |
dwini | 2:356bb8d99326 | 182 | |
dwini | 2:356bb8d99326 | 183 | return power; |
dwini | 2:356bb8d99326 | 184 | } |
dwini | 0:c0ae66a0ec7a | 185 | |
dwini | 0:c0ae66a0ec7a | 186 | // Show drive speed and sensor data |
dwini | 2:356bb8d99326 | 187 | void show_stats(void) { |
dwini | 0:c0ae66a0ec7a | 188 | m3pi.cls(); // Clear display |
dwini | 0:c0ae66a0ec7a | 189 | |
dwini | 0:c0ae66a0ec7a | 190 | // Display speed |
dwini | 0:c0ae66a0ec7a | 191 | m3pi.locate(0, 0); |
dwini | 0:c0ae66a0ec7a | 192 | // x The horizontal position, from 0 to 7 |
dwini | 0:c0ae66a0ec7a | 193 | // y The vertical position, from 0 to 1 |
dwini | 0:c0ae66a0ec7a | 194 | m3pi.printf("FOR 100%%"); |
dwini | 0:c0ae66a0ec7a | 195 | |
dwini | 0:c0ae66a0ec7a | 196 | // Display line |
dwini | 0:c0ae66a0ec7a | 197 | m3pi.locate(0, 1); |
dwini | 0:c0ae66a0ec7a | 198 | int line = line_sensor(); |
dwini | 0:c0ae66a0ec7a | 199 | m3pi.printf("LINE %d", line); |
dwini | 0:c0ae66a0ec7a | 200 | } |
dwini | 0:c0ae66a0ec7a | 201 | |
dwini | 2:356bb8d99326 | 202 | /* |
dwini | 2:356bb8d99326 | 203 | * Shows the name of the robot on the display. |
dwini | 2:356bb8d99326 | 204 | * |
dwini | 2:356bb8d99326 | 205 | * @name C character string (null-terminated) with the name of the robot (max 8 chars) |
dwini | 2:356bb8d99326 | 206 | */ |
dwini | 2:356bb8d99326 | 207 | void show_name(char * name) { |
dwini | 2:356bb8d99326 | 208 | m3pi.cls(); // Clear display |
dwini | 0:c0ae66a0ec7a | 209 | |
dwini | 2:356bb8d99326 | 210 | // Display speed |
dwini | 2:356bb8d99326 | 211 | m3pi.locate(0, 0); |
dwini | 2:356bb8d99326 | 212 | // x The horizontal position, from 0 to 7 |
dwini | 2:356bb8d99326 | 213 | // y The vertical position, from 0 to 1 |
dwini | 2:356bb8d99326 | 214 | m3pi.printf("%s", name); |
dwini | 2:356bb8d99326 | 215 | } |
dwini | 0:c0ae66a0ec7a | 216 | |
dwini | 0:c0ae66a0ec7a | 217 | |
dwini | 0:c0ae66a0ec7a | 218 | /* |
dwini | 0:c0ae66a0ec7a | 219 | * Wait for an approximate number of milliseconds. |
dwini | 0:c0ae66a0ec7a | 220 | * |
dwini | 0:c0ae66a0ec7a | 221 | * @milliseconds The number of milliseconds to wait. |
dwini | 0:c0ae66a0ec7a | 222 | */ |
dwini | 2:356bb8d99326 | 223 | void await(int milliseconds) { |
dwini | 0:c0ae66a0ec7a | 224 | wait_ms(milliseconds); |
dwini | 0:c0ae66a0ec7a | 225 | } |