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