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@9:0385d1bfc38b, 2015-06-01 (annotated)

Committer:
sillevl
Date:
Mon Jun 01 14:53:39 2015 +0000
Revision:
9:0385d1bfc38b
Parent:
8:597ce8a7d34b 
added Xbee support for start/stop

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dwini 0:c0ae66a0ec7a 1 #include "robot_logic.h"
sillevl 8:597ce8a7d34b 2 #include "xbee.h"
dwini 0:c0ae66a0ec7a 3
dwini 2:356bb8d99326 4 // Some defines
dwini 2:356bb8d99326 5 #define MAX_SPEED 100
dwini 2:356bb8d99326 6 #define MAX_SENSOR 100
pcordemans 7:a72215b1910b 7 #define MAX_REAL_SPEED 0.75
pcordemans 7:a72215b1910b 8
pcordemans 3:b5d18690f357 9 #define MIN 0
dwini 2:356bb8d99326 10
dwini 2:356bb8d99326 11 // Static scope variables
dwini 2:356bb8d99326 12 static m3pi m3pi;
dwini 0:c0ae66a0ec7a 13
sillevl 8:597ce8a7d34b 14 #ifdef XBEE
sillevl 8:597ce8a7d34b 15 Xbee xbee(p28, p27);
sillevl 8:597ce8a7d34b 16 #endif
sillevl 8:597ce8a7d34b 17
dwini 2:356bb8d99326 18 // Static scope variables for keeping internal state
dwini 2:356bb8d99326 19 static int internal_speed = 0; // [-100, +100]
pcordemans 6:0dc4e4225881 20 static int internal_turnspeed = 0; // [-100, +100]
dwini 2:356bb8d99326 21 static int internal_p = 0;
dwini 2:356bb8d99326 22 static int internal_i = 0;
dwini 2:356bb8d99326 23 static int internal_d = 0;
dwini 2:356bb8d99326 24 static int internal_previous_pos_of_line = 0;
pcordemans 5:355240d7126b 25 static int internal_led_state = 0;
dwini 0:c0ae66a0ec7a 26
pcordemans 6:0dc4e4225881 27 void drive(int speed)
pcordemans 6:0dc4e4225881 28 {
dwini 2:356bb8d99326 29 if (speed > MAX_SPEED || speed < -MAX_SPEED) {
pcordemans 7:a72215b1910b 30 error("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
sillevl 8:597ce8a7d34b 36 if (speed == 0 || !canDrive()) {
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
pcordemans 6:0dc4e4225881 45 void turn(int turnspeed)
pcordemans 6:0dc4e4225881 46 {
dwini 2:356bb8d99326 47 if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
pcordemans 7:a72215b1910b 48 error("Turn out of range");
dwini 0:c0ae66a0ec7a 49 return;
dwini 0:c0ae66a0ec7a 50 }
pcordemans 6:0dc4e4225881 51 internal_turnspeed = turnspeed;
dwini 2:356bb8d99326 52
pcordemans 6:0dc4e4225881 53 float left = internal_speed;
pcordemans 6:0dc4e4225881 54 float right = internal_speed;
pcordemans 6:0dc4e4225881 55
pcordemans 6:0dc4e4225881 56 left -= turnspeed;
pcordemans 6:0dc4e4225881 57 right += turnspeed;
dwini 2:356bb8d99326 58
pcordemans 6:0dc4e4225881 59 if (right < MIN)
pcordemans 6:0dc4e4225881 60 right = MIN;
pcordemans 6:0dc4e4225881 61 else if (right > MAX_SPEED)
pcordemans 6:0dc4e4225881 62 right = MAX_SPEED;
dwini 2:356bb8d99326 63
pcordemans 6:0dc4e4225881 64 if (left < MIN)
pcordemans 6:0dc4e4225881 65 left = MIN;
pcordemans 6:0dc4e4225881 66 else if (left > MAX_SPEED)
pcordemans 6:0dc4e4225881 67 left = MAX_SPEED;
sillevl 8:597ce8a7d34b 68
sillevl 8:597ce8a7d34b 69 if(!canDrive()){
sillevl 8:597ce8a7d34b 70 left = right = 0;
sillevl 8:597ce8a7d34b 71 }
sillevl 8:597ce8a7d34b 72
pcordemans 6:0dc4e4225881 73 m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED);
pcordemans 6:0dc4e4225881 74 m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED);
dwini 0:c0ae66a0ec7a 75 }
dwini 0:c0ae66a0ec7a 76
pcordemans 6:0dc4e4225881 77 void stop(void)
pcordemans 6:0dc4e4225881 78 {
dwini 0:c0ae66a0ec7a 79 m3pi.stop();
dwini 0:c0ae66a0ec7a 80 }
pcordemans 6:0dc4e4225881 81 void sensor_calibrate(void)
pcordemans 6:0dc4e4225881 82 {
dwini 2:356bb8d99326 83 m3pi.sensor_auto_calibrate();
dwini 2:356bb8d99326 84 }
dwini 2:356bb8d99326 85
pcordemans 6:0dc4e4225881 86 int line_sensor(void)
pcordemans 6:0dc4e4225881 87 {
dwini 0:c0ae66a0ec7a 88 // Get position of line [-1.0, +1.0]
dwini 0:c0ae66a0ec7a 89 float pos = m3pi.line_position();
dwini 2:356bb8d99326 90 return ((int)(pos * MAX_SENSOR));
dwini 2:356bb8d99326 91 }
dwini 2:356bb8d99326 92
pcordemans 6:0dc4e4225881 93 void pid_init(int p, int i, int d)
pcordemans 6:0dc4e4225881 94 {
dwini 2:356bb8d99326 95 internal_p = p;
dwini 2:356bb8d99326 96 internal_i = i;
dwini 2:356bb8d99326 97 internal_d = d;
dwini 0:c0ae66a0ec7a 98 }
dwini 0:c0ae66a0ec7a 99
pcordemans 6:0dc4e4225881 100 int pid_turn(int line_position)
pcordemans 6:0dc4e4225881 101 {
dwini 2:356bb8d99326 102 float derivative, proportional, integral = 0;
dwini 2:356bb8d99326 103 float power;
pcordemans 6:0dc4e4225881 104
pcordemans 7:a72215b1910b 105 proportional = line_position / 100.0;
pcordemans 6:0dc4e4225881 106
dwini 2:356bb8d99326 107 // Compute the derivative
dwini 2:356bb8d99326 108 derivative = line_position - internal_previous_pos_of_line;
pcordemans 6:0dc4e4225881 109
dwini 2:356bb8d99326 110 // Compute the integral
dwini 2:356bb8d99326 111 integral += proportional;
pcordemans 6:0dc4e4225881 112
dwini 2:356bb8d99326 113 // Remember the last position.
dwini 2:356bb8d99326 114 internal_previous_pos_of_line = line_position;
pcordemans 6:0dc4e4225881 115
dwini 2:356bb8d99326 116 // Compute the power
dwini 2:356bb8d99326 117 power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ;
pcordemans 6:0dc4e4225881 118
pcordemans 7:a72215b1910b 119 power = power * MAX_SPEED;
pcordemans 7:a72215b1910b 120 if (power < -MAX_SPEED)
pcordemans 7:a72215b1910b 121 power = -MAX_SPEED;
pcordemans 7:a72215b1910b 122 else if (power > MAX_SPEED)
pcordemans 7:a72215b1910b 123 power = MAX_SPEED;
pcordemans 7:a72215b1910b 124 return power ;
dwini 2:356bb8d99326 125 }
dwini 0:c0ae66a0ec7a 126
pcordemans 6:0dc4e4225881 127 void show_stats(void)
pcordemans 6:0dc4e4225881 128 {
dwini 0:c0ae66a0ec7a 129 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 130
dwini 0:c0ae66a0ec7a 131 // Display speed
dwini 0:c0ae66a0ec7a 132 m3pi.locate(0, 0);
pcordemans 6:0dc4e4225881 133 m3pi.printf("S%d", internal_speed);
pcordemans 6:0dc4e4225881 134
pcordemans 6:0dc4e4225881 135 // Display turn
pcordemans 6:0dc4e4225881 136 m3pi.locate(4,0);
pcordemans 6:0dc4e4225881 137 m3pi.printf("T%d", internal_turnspeed);
dwini 0:c0ae66a0ec7a 138
dwini 0:c0ae66a0ec7a 139 // Display line
dwini 0:c0ae66a0ec7a 140 m3pi.locate(0, 1);
dwini 0:c0ae66a0ec7a 141 int line = line_sensor();
pcordemans 6:0dc4e4225881 142 m3pi.printf("POS %d", line);
dwini 0:c0ae66a0ec7a 143 }
dwini 0:c0ae66a0ec7a 144
pcordemans 6:0dc4e4225881 145 void show_name(char * name)
pcordemans 6:0dc4e4225881 146 {
dwini 2:356bb8d99326 147 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 148
dwini 2:356bb8d99326 149 // Display speed
dwini 2:356bb8d99326 150 m3pi.locate(0, 0);
pcordemans 6:0dc4e4225881 151 // x The horizontal position, from 0 to 7
pcordemans 6:0dc4e4225881 152 // y The vertical position, from 0 to 1
dwini 2:356bb8d99326 153 m3pi.printf("%s", name);
dwini 2:356bb8d99326 154 }
dwini 0:c0ae66a0ec7a 155
pcordemans 6:0dc4e4225881 156 void await(int milliseconds)
pcordemans 6:0dc4e4225881 157 {
dwini 0:c0ae66a0ec7a 158 wait_ms(milliseconds);
pcordemans 5:355240d7126b 159 }
pcordemans 5:355240d7126b 160
pcordemans 6:0dc4e4225881 161 void led(LedIndex i, LedState state)
pcordemans 6:0dc4e4225881 162 {
pcordemans 6:0dc4e4225881 163 if(state == LED_ON) {
pcordemans 5:355240d7126b 164 internal_led_state |= (1 << i);
pcordemans 6:0dc4e4225881 165 } else if(state == LED_OFF) {
pcordemans 5:355240d7126b 166 internal_led_state &= ~(1 << i);
pcordemans 6:0dc4e4225881 167 } else if(state == LED_TOGGLE) {
pcordemans 5:355240d7126b 168 internal_led_state ^= (1 << i);
pcordemans 6:0dc4e4225881 169 } else {
pcordemans 5:355240d7126b 170 error("Illegal LED state");
pcordemans 5:355240d7126b 171 }
pcordemans 5:355240d7126b 172 m3pi.leds(internal_led_state);
sillevl 8:597ce8a7d34b 173 }
sillevl 8:597ce8a7d34b 174
sillevl 8:597ce8a7d34b 175 int canDrive()
sillevl 8:597ce8a7d34b 176 {
sillevl 8:597ce8a7d34b 177 #ifdef XBEE
sillevl 8:597ce8a7d34b 178 return !xbee.stopped();
sillevl 8:597ce8a7d34b 179 #endif
sillevl 8:597ce8a7d34b 180 return true;
sillevl 8:597ce8a7d34b 181 }
sillevl 8:597ce8a7d34b 182
sillevl 8:597ce8a7d34b 183
sillevl 8:597ce8a7d34b 184 void setCode(int code)
sillevl 8:597ce8a7d34b 185 {
sillevl 8:597ce8a7d34b 186 #ifdef XBEE
sillevl 8:597ce8a7d34b 187 xbee.setCode(code);
sillevl 8:597ce8a7d34b 188 #endif
dwini 0:c0ae66a0ec7a 189 }