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