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
Committer:
pcordemans
Date:
Wed Feb 25 09:10:13 2015 +0000
Revision:
6:0dc4e4225881
Parent:
5:355240d7126b
Child:
7:a72215b1910b
implemented show stats and cleaned up the documentation

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
pcordemans 6:0dc4e4225881 6 #define MAX_REAL_SPEED 0.3
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]
pcordemans 6:0dc4e4225881 15 static int internal_turnspeed = 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
pcordemans 6:0dc4e4225881 22 void drive(int speed)
pcordemans 6:0dc4e4225881 23 {
dwini 2:356bb8d99326 24 if (speed > MAX_SPEED || speed < -MAX_SPEED) {
dwini 0:c0ae66a0ec7a 25 printf("[ERROR] Drive speed out of range");
dwini 0:c0ae66a0ec7a 26 return;
dwini 0:c0ae66a0ec7a 27 }
dwini 0:c0ae66a0ec7a 28
dwini 2:356bb8d99326 29 internal_speed = speed;
dwini 2:356bb8d99326 30
dwini 0:c0ae66a0ec7a 31 if (speed == 0) {
dwini 0:c0ae66a0ec7a 32 m3pi.stop();
dwini 0:c0ae66a0ec7a 33 } else if (speed > 0) {
dwini 2:356bb8d99326 34 m3pi.forward(MAX_REAL_SPEED*speed/MAX_SPEED);
dwini 0:c0ae66a0ec7a 35 } else if (speed < 0) {
dwini 2:356bb8d99326 36 m3pi.backward(MAX_REAL_SPEED*(-speed)/MAX_SPEED);
dwini 0:c0ae66a0ec7a 37 }
dwini 0:c0ae66a0ec7a 38 }
dwini 0:c0ae66a0ec7a 39
pcordemans 6:0dc4e4225881 40 void turn(int turnspeed)
pcordemans 6:0dc4e4225881 41 {
dwini 2:356bb8d99326 42 if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
pcordemans 5:355240d7126b 43 error("Speed out of range");
dwini 0:c0ae66a0ec7a 44 return;
dwini 0:c0ae66a0ec7a 45 }
pcordemans 6:0dc4e4225881 46 internal_turnspeed = turnspeed;
dwini 2:356bb8d99326 47
pcordemans 6:0dc4e4225881 48 float left = internal_speed;
pcordemans 6:0dc4e4225881 49 float right = internal_speed;
pcordemans 6:0dc4e4225881 50
pcordemans 6:0dc4e4225881 51 left -= turnspeed;
pcordemans 6:0dc4e4225881 52 right += turnspeed;
dwini 2:356bb8d99326 53
pcordemans 6:0dc4e4225881 54 if (right < MIN)
pcordemans 6:0dc4e4225881 55 right = MIN;
pcordemans 6:0dc4e4225881 56 else if (right > MAX_SPEED)
pcordemans 6:0dc4e4225881 57 right = MAX_SPEED;
dwini 2:356bb8d99326 58
pcordemans 6:0dc4e4225881 59 if (left < MIN)
pcordemans 6:0dc4e4225881 60 left = MIN;
pcordemans 6:0dc4e4225881 61 else if (left > MAX_SPEED)
pcordemans 6:0dc4e4225881 62 left = MAX_SPEED;
pcordemans 6:0dc4e4225881 63 m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED);
pcordemans 6:0dc4e4225881 64 m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED);
dwini 0:c0ae66a0ec7a 65 }
dwini 0:c0ae66a0ec7a 66
pcordemans 6:0dc4e4225881 67 void stop(void)
pcordemans 6:0dc4e4225881 68 {
dwini 0:c0ae66a0ec7a 69 m3pi.stop();
dwini 0:c0ae66a0ec7a 70 }
pcordemans 6:0dc4e4225881 71 void sensor_calibrate(void)
pcordemans 6:0dc4e4225881 72 {
dwini 2:356bb8d99326 73 m3pi.sensor_auto_calibrate();
dwini 2:356bb8d99326 74 }
dwini 2:356bb8d99326 75
pcordemans 6:0dc4e4225881 76 int line_sensor(void)
pcordemans 6:0dc4e4225881 77 {
dwini 0:c0ae66a0ec7a 78 // Get position of line [-1.0, +1.0]
dwini 0:c0ae66a0ec7a 79 float pos = m3pi.line_position();
dwini 2:356bb8d99326 80 return ((int)(pos * MAX_SENSOR));
dwini 2:356bb8d99326 81 }
dwini 2:356bb8d99326 82
pcordemans 6:0dc4e4225881 83 void pid_init(int p, int i, int d)
pcordemans 6:0dc4e4225881 84 {
dwini 2:356bb8d99326 85 internal_p = p;
dwini 2:356bb8d99326 86 internal_i = i;
dwini 2:356bb8d99326 87 internal_d = d;
dwini 0:c0ae66a0ec7a 88 }
dwini 0:c0ae66a0ec7a 89
pcordemans 6:0dc4e4225881 90 int pid_turn(int line_position)
pcordemans 6:0dc4e4225881 91 {
dwini 2:356bb8d99326 92 float right;
dwini 2:356bb8d99326 93 float left;
pcordemans 6:0dc4e4225881 94
dwini 2:356bb8d99326 95 float derivative, proportional, integral = 0;
dwini 2:356bb8d99326 96 float power;
dwini 2:356bb8d99326 97 float speed = internal_speed;
pcordemans 6:0dc4e4225881 98
dwini 2:356bb8d99326 99 proportional = line_position;
pcordemans 6:0dc4e4225881 100
dwini 2:356bb8d99326 101 // Compute the derivative
dwini 2:356bb8d99326 102 derivative = line_position - internal_previous_pos_of_line;
pcordemans 6:0dc4e4225881 103
dwini 2:356bb8d99326 104 // Compute the integral
dwini 2:356bb8d99326 105 integral += proportional;
pcordemans 6:0dc4e4225881 106
dwini 2:356bb8d99326 107 // Remember the last position.
dwini 2:356bb8d99326 108 internal_previous_pos_of_line = line_position;
pcordemans 6:0dc4e4225881 109
dwini 2:356bb8d99326 110 // Compute the power
dwini 2:356bb8d99326 111 power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ;
pcordemans 6:0dc4e4225881 112
dwini 2:356bb8d99326 113 return power;
dwini 2:356bb8d99326 114 }
dwini 0:c0ae66a0ec7a 115
pcordemans 6:0dc4e4225881 116 /**
pcordemans 6:0dc4e4225881 117 *Show speed, turn and sensor data
pcordemans 6:0dc4e4225881 118 */
pcordemans 6:0dc4e4225881 119 void show_stats(void)
pcordemans 6:0dc4e4225881 120 {
dwini 0:c0ae66a0ec7a 121 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 122
dwini 0:c0ae66a0ec7a 123 // Display speed
dwini 0:c0ae66a0ec7a 124 m3pi.locate(0, 0);
pcordemans 6:0dc4e4225881 125 m3pi.printf("S%d", internal_speed);
pcordemans 6:0dc4e4225881 126
pcordemans 6:0dc4e4225881 127 // Display turn
pcordemans 6:0dc4e4225881 128 m3pi.locate(4,0);
pcordemans 6:0dc4e4225881 129 m3pi.printf("T%d", internal_turnspeed);
dwini 0:c0ae66a0ec7a 130
dwini 0:c0ae66a0ec7a 131 // Display line
dwini 0:c0ae66a0ec7a 132 m3pi.locate(0, 1);
dwini 0:c0ae66a0ec7a 133 int line = line_sensor();
pcordemans 6:0dc4e4225881 134 m3pi.printf("POS %d", line);
dwini 0:c0ae66a0ec7a 135 }
dwini 0:c0ae66a0ec7a 136
pcordemans 6:0dc4e4225881 137 void show_name(char * name)
pcordemans 6:0dc4e4225881 138 {
dwini 2:356bb8d99326 139 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 140
dwini 2:356bb8d99326 141 // Display speed
dwini 2:356bb8d99326 142 m3pi.locate(0, 0);
pcordemans 6:0dc4e4225881 143 // x The horizontal position, from 0 to 7
pcordemans 6:0dc4e4225881 144 // y The vertical position, from 0 to 1
dwini 2:356bb8d99326 145 m3pi.printf("%s", name);
dwini 2:356bb8d99326 146 }
dwini 0:c0ae66a0ec7a 147
pcordemans 6:0dc4e4225881 148 void await(int milliseconds)
pcordemans 6:0dc4e4225881 149 {
dwini 0:c0ae66a0ec7a 150 wait_ms(milliseconds);
pcordemans 5:355240d7126b 151 }
pcordemans 5:355240d7126b 152
pcordemans 6:0dc4e4225881 153 void led(LedIndex i, LedState state)
pcordemans 6:0dc4e4225881 154 {
pcordemans 6:0dc4e4225881 155 if(state == LED_ON) {
pcordemans 5:355240d7126b 156 internal_led_state |= (1 << i);
pcordemans 6:0dc4e4225881 157 } else if(state == LED_OFF) {
pcordemans 5:355240d7126b 158 internal_led_state &= ~(1 << i);
pcordemans 6:0dc4e4225881 159 } else if(state == LED_TOGGLE) {
pcordemans 5:355240d7126b 160 internal_led_state ^= (1 << i);
pcordemans 6:0dc4e4225881 161 } else {
pcordemans 5:355240d7126b 162 error("Illegal LED state");
pcordemans 5:355240d7126b 163 }
pcordemans 5:355240d7126b 164 m3pi.leds(internal_led_state);
dwini 0:c0ae66a0ec7a 165 }