Nico De Witte / racing_robots

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:

  1. Controlling the LEDS
  2. Move forward and backward
  3. Turn
  4. Read the sensor values
  5. Use a PID controller

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 }