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

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 }