Sille Van Landschoot
/
racing_robots
Racing Robots Session
Fork of racing_robots by
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?
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 |
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 | } |