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@2:356bb8d99326, 2015-02-23 (annotated)

Committer:
dwini
Date:
Mon Feb 23 14:37:37 2015 +0000
Revision:
2:356bb8d99326
Parent:
0:c0ae66a0ec7a
Child:
3:b5d18690f357
Add implementation of calibrate, pid and drive control. Not working as it should !

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