Sille Van Landschoot / Mbed 2 deprecated RacingRobotsLib

Racing Robots Session

Dependencies:   m3pi mbed

Dependents:   RacingRobots

Fork of racing_robots by Nico De Witte

Logic.cpp@8:c5dccd557aab, 2015-06-01 (annotated)

Committer:
sillevl
Date:
Mon Jun 01 10:13:18 2015 +0000
Revision:
8:c5dccd557aab
Parent:
robot_logic.cpp@7:a72215b1910b 
renamed files to mbed standard

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sillevl 8:c5dccd557aab 1 #include "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 }