Sille Van Landschoot
/
RacingRobotsLib
Racing Robots Session
Fork of racing_robots by
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?
User | Revision | Line number | New 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 | } |