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@7:a72215b1910b, 2015-02-25 (annotated)

Committer:
pcordemans
Date:
Wed Feb 25 15:56:14 2015 +0000
Revision:
7:a72215b1910b
Parent:
6:0dc4e4225881
Child:
8:597ce8a7d34b
PID works, updated the comments

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 7:a72215b1910b 6 #define MAX_REAL_SPEED 0.75
pcordemans 7:a72215b1910b 7
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) {
pcordemans 7:a72215b1910b 25 error("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 7:a72215b1910b 43 error("Turn 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 derivative, proportional, integral = 0;
dwini 2:356bb8d99326 93 float power;
pcordemans 6:0dc4e4225881 94
pcordemans 7:a72215b1910b 95 proportional = line_position / 100.0;
pcordemans 6:0dc4e4225881 96
dwini 2:356bb8d99326 97 // Compute the derivative
dwini 2:356bb8d99326 98 derivative = line_position - internal_previous_pos_of_line;
pcordemans 6:0dc4e4225881 99
dwini 2:356bb8d99326 100 // Compute the integral
dwini 2:356bb8d99326 101 integral += proportional;
pcordemans 6:0dc4e4225881 102
dwini 2:356bb8d99326 103 // Remember the last position.
dwini 2:356bb8d99326 104 internal_previous_pos_of_line = line_position;
pcordemans 6:0dc4e4225881 105
dwini 2:356bb8d99326 106 // Compute the power
dwini 2:356bb8d99326 107 power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ;
pcordemans 6:0dc4e4225881 108
pcordemans 7:a72215b1910b 109 power = power * MAX_SPEED;
pcordemans 7:a72215b1910b 110 if (power < -MAX_SPEED)
pcordemans 7:a72215b1910b 111 power = -MAX_SPEED;
pcordemans 7:a72215b1910b 112 else if (power > MAX_SPEED)
pcordemans 7:a72215b1910b 113 power = MAX_SPEED;
pcordemans 7:a72215b1910b 114 return power ;
dwini 2:356bb8d99326 115 }
dwini 0:c0ae66a0ec7a 116
pcordemans 6:0dc4e4225881 117 void show_stats(void)
pcordemans 6:0dc4e4225881 118 {
dwini 0:c0ae66a0ec7a 119 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 120
dwini 0:c0ae66a0ec7a 121 // Display speed
dwini 0:c0ae66a0ec7a 122 m3pi.locate(0, 0);
pcordemans 6:0dc4e4225881 123 m3pi.printf("S%d", internal_speed);
pcordemans 6:0dc4e4225881 124
pcordemans 6:0dc4e4225881 125 // Display turn
pcordemans 6:0dc4e4225881 126 m3pi.locate(4,0);
pcordemans 6:0dc4e4225881 127 m3pi.printf("T%d", internal_turnspeed);
dwini 0:c0ae66a0ec7a 128
dwini 0:c0ae66a0ec7a 129 // Display line
dwini 0:c0ae66a0ec7a 130 m3pi.locate(0, 1);
dwini 0:c0ae66a0ec7a 131 int line = line_sensor();
pcordemans 6:0dc4e4225881 132 m3pi.printf("POS %d", line);
dwini 0:c0ae66a0ec7a 133 }
dwini 0:c0ae66a0ec7a 134
pcordemans 6:0dc4e4225881 135 void show_name(char * name)
pcordemans 6:0dc4e4225881 136 {
dwini 2:356bb8d99326 137 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 138
dwini 2:356bb8d99326 139 // Display speed
dwini 2:356bb8d99326 140 m3pi.locate(0, 0);
pcordemans 6:0dc4e4225881 141 // x The horizontal position, from 0 to 7
pcordemans 6:0dc4e4225881 142 // y The vertical position, from 0 to 1
dwini 2:356bb8d99326 143 m3pi.printf("%s", name);
dwini 2:356bb8d99326 144 }
dwini 0:c0ae66a0ec7a 145
pcordemans 6:0dc4e4225881 146 void await(int milliseconds)
pcordemans 6:0dc4e4225881 147 {
dwini 0:c0ae66a0ec7a 148 wait_ms(milliseconds);
pcordemans 5:355240d7126b 149 }
pcordemans 5:355240d7126b 150
pcordemans 6:0dc4e4225881 151 void led(LedIndex i, LedState state)
pcordemans 6:0dc4e4225881 152 {
pcordemans 6:0dc4e4225881 153 if(state == LED_ON) {
pcordemans 5:355240d7126b 154 internal_led_state |= (1 << i);
pcordemans 6:0dc4e4225881 155 } else if(state == LED_OFF) {
pcordemans 5:355240d7126b 156 internal_led_state &= ~(1 << i);
pcordemans 6:0dc4e4225881 157 } else if(state == LED_TOGGLE) {
pcordemans 5:355240d7126b 158 internal_led_state ^= (1 << i);
pcordemans 6:0dc4e4225881 159 } else {
pcordemans 5:355240d7126b 160 error("Illegal LED state");
pcordemans 5:355240d7126b 161 }
pcordemans 5:355240d7126b 162 m3pi.leds(internal_led_state);
dwini 0:c0ae66a0ec7a 163 }