Sille Van Landschoot / Mbed 2 deprecated racing_robots

Racing Robots Session

Dependencies:   m3pi mbed

Fork of racing_robots by Nico De Witte

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?

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;
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 }