Nico De Witte / racing_robots

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MbedJSONValue m3pi

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 }