Sille Van Landschoot / Mbed 2 deprecated racing_robots

Racing Robots Session

Dependencies:   m3pi mbed

Fork of racing_robots by Nico De Witte

robot_logic.cpp@11:dbec260681a7, 2016-05-19 (annotated)

Committer:
sillevl
Date:
Thu May 19 13:16:14 2016 +0000
Revision:
11:dbec260681a7
Parent:
7:a72215b1910b 
add xbee support to racing robots library

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
sillevl 11:dbec260681a7 6 #define MAX_REAL_SPEED 0.3
pcordemans 7:a72215b1910b 7
sillevl 11:dbec260681a7 8 #define MIN -MAX_SPEED
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;
sillevl 11:dbec260681a7 63
pcordemans 6:0dc4e4225881 64 m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED);
pcordemans 6:0dc4e4225881 65 m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED);
dwini 0:c0ae66a0ec7a 66 }
dwini 0:c0ae66a0ec7a 67
pcordemans 6:0dc4e4225881 68 void stop(void)
pcordemans 6:0dc4e4225881 69 {
dwini 0:c0ae66a0ec7a 70 m3pi.stop();
dwini 0:c0ae66a0ec7a 71 }
pcordemans 6:0dc4e4225881 72 void sensor_calibrate(void)
pcordemans 6:0dc4e4225881 73 {
dwini 2:356bb8d99326 74 m3pi.sensor_auto_calibrate();
dwini 2:356bb8d99326 75 }
dwini 2:356bb8d99326 76
pcordemans 6:0dc4e4225881 77 int line_sensor(void)
pcordemans 6:0dc4e4225881 78 {
dwini 0:c0ae66a0ec7a 79 // Get position of line [-1.0, +1.0]
dwini 0:c0ae66a0ec7a 80 float pos = m3pi.line_position();
dwini 2:356bb8d99326 81 return ((int)(pos * MAX_SENSOR));
dwini 2:356bb8d99326 82 }
dwini 2:356bb8d99326 83
pcordemans 6:0dc4e4225881 84 void pid_init(int p, int i, int d)
pcordemans 6:0dc4e4225881 85 {
dwini 2:356bb8d99326 86 internal_p = p;
dwini 2:356bb8d99326 87 internal_i = i;
dwini 2:356bb8d99326 88 internal_d = d;
dwini 0:c0ae66a0ec7a 89 }
dwini 0:c0ae66a0ec7a 90
pcordemans 6:0dc4e4225881 91 int pid_turn(int line_position)
pcordemans 6:0dc4e4225881 92 {
dwini 2:356bb8d99326 93 float derivative, proportional, integral = 0;
dwini 2:356bb8d99326 94 float power;
pcordemans 6:0dc4e4225881 95
pcordemans 7:a72215b1910b 96 proportional = line_position / 100.0;
pcordemans 6:0dc4e4225881 97
dwini 2:356bb8d99326 98 // Compute the derivative
dwini 2:356bb8d99326 99 derivative = line_position - internal_previous_pos_of_line;
pcordemans 6:0dc4e4225881 100
dwini 2:356bb8d99326 101 // Compute the integral
dwini 2:356bb8d99326 102 integral += proportional;
pcordemans 6:0dc4e4225881 103
dwini 2:356bb8d99326 104 // Remember the last position.
dwini 2:356bb8d99326 105 internal_previous_pos_of_line = line_position;
pcordemans 6:0dc4e4225881 106
dwini 2:356bb8d99326 107 // Compute the power
dwini 2:356bb8d99326 108 power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ;
pcordemans 6:0dc4e4225881 109
pcordemans 7:a72215b1910b 110 power = power * MAX_SPEED;
pcordemans 7:a72215b1910b 111 if (power < -MAX_SPEED)
pcordemans 7:a72215b1910b 112 power = -MAX_SPEED;
pcordemans 7:a72215b1910b 113 else if (power > MAX_SPEED)
pcordemans 7:a72215b1910b 114 power = MAX_SPEED;
pcordemans 7:a72215b1910b 115 return power ;
dwini 2:356bb8d99326 116 }
dwini 0:c0ae66a0ec7a 117
pcordemans 6:0dc4e4225881 118 void show_stats(void)
pcordemans 6:0dc4e4225881 119 {
dwini 0:c0ae66a0ec7a 120 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 121
dwini 0:c0ae66a0ec7a 122 // Display speed
dwini 0:c0ae66a0ec7a 123 m3pi.locate(0, 0);
pcordemans 6:0dc4e4225881 124 m3pi.printf("S%d", internal_speed);
pcordemans 6:0dc4e4225881 125
pcordemans 6:0dc4e4225881 126 // Display turn
pcordemans 6:0dc4e4225881 127 m3pi.locate(4,0);
pcordemans 6:0dc4e4225881 128 m3pi.printf("T%d", internal_turnspeed);
dwini 0:c0ae66a0ec7a 129
dwini 0:c0ae66a0ec7a 130 // Display line
dwini 0:c0ae66a0ec7a 131 m3pi.locate(0, 1);
dwini 0:c0ae66a0ec7a 132 int line = line_sensor();
pcordemans 6:0dc4e4225881 133 m3pi.printf("POS %d", line);
dwini 0:c0ae66a0ec7a 134 }
dwini 0:c0ae66a0ec7a 135
pcordemans 6:0dc4e4225881 136 void show_name(char * name)
pcordemans 6:0dc4e4225881 137 {
dwini 2:356bb8d99326 138 m3pi.cls(); // Clear display
dwini 0:c0ae66a0ec7a 139
dwini 2:356bb8d99326 140 // Display speed
dwini 2:356bb8d99326 141 m3pi.locate(0, 0);
pcordemans 6:0dc4e4225881 142 // x The horizontal position, from 0 to 7
pcordemans 6:0dc4e4225881 143 // y The vertical position, from 0 to 1
dwini 2:356bb8d99326 144 m3pi.printf("%s", name);
dwini 2:356bb8d99326 145 }
dwini 0:c0ae66a0ec7a 146
pcordemans 6:0dc4e4225881 147 void await(int milliseconds)
pcordemans 6:0dc4e4225881 148 {
dwini 0:c0ae66a0ec7a 149 wait_ms(milliseconds);
pcordemans 5:355240d7126b 150 }
pcordemans 5:355240d7126b 151
pcordemans 6:0dc4e4225881 152 void led(LedIndex i, LedState state)
pcordemans 6:0dc4e4225881 153 {
pcordemans 6:0dc4e4225881 154 if(state == LED_ON) {
pcordemans 5:355240d7126b 155 internal_led_state |= (1 << i);
pcordemans 6:0dc4e4225881 156 } else if(state == LED_OFF) {
pcordemans 5:355240d7126b 157 internal_led_state &= ~(1 << i);
pcordemans 6:0dc4e4225881 158 } else if(state == LED_TOGGLE) {
pcordemans 5:355240d7126b 159 internal_led_state ^= (1 << i);
pcordemans 6:0dc4e4225881 160 } else {
pcordemans 5:355240d7126b 161 error("Illegal LED state");
pcordemans 5:355240d7126b 162 }
pcordemans 5:355240d7126b 163 m3pi.leds(internal_led_state);
dwini 0:c0ae66a0ec7a 164 }