hello
Dependencies: AVEncoder QEI mbed-src-AV
main.cpp@4:453d534b1c1d, 2015-11-14 (annotated)
- Committer:
- intgsull
- Date:
- Sat Nov 14 00:15:41 2015 +0000
- Revision:
- 4:453d534b1c1d
- Parent:
- 2:5f9b78950a17
- Child:
- 5:f9837617817b
This is pre-Lab3, with the skeleton PID code. Just in case we need to reset the entire process.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
intgsull | 4:453d534b1c1d | 1 | //Micromouse code |
intgsull | 0:fa523db3f4f5 | 2 | #include "mbed.h" |
intgsull | 4:453d534b1c1d | 3 | #include "AVEncoder.h" |
intgsull | 0:fa523db3f4f5 | 4 | |
intgsull | 4:453d534b1c1d | 5 | // set things |
intgsull | 4:453d534b1c1d | 6 | Serial pc(SERIAL_TX, SERIAL_RX); |
intgsull | 4:453d534b1c1d | 7 | Ticker Systicker; |
intgsull | 4:453d534b1c1d | 8 | Timer timer; |
intgsull | 4:453d534b1c1d | 9 | |
intgsull | 4:453d534b1c1d | 10 | PwmOut motor1_forward(PB_10); |
intgsull | 2:5f9b78950a17 | 11 | PwmOut motor1_reverse(PA_6); |
intgsull | 0:fa523db3f4f5 | 12 | PwmOut motor2_forward(PA_7); |
intgsull | 0:fa523db3f4f5 | 13 | PwmOut motor2_reverse(PB_6); |
intgsull | 0:fa523db3f4f5 | 14 | |
intgsull | 4:453d534b1c1d | 15 | // TODO: change our encoder pins from AnalogIn into: |
intgsull | 4:453d534b1c1d | 16 | // otherwise, we can also use the AVEncoder thing as well. |
intgsull | 4:453d534b1c1d | 17 | AVEncoder l_enco(PA_15, PB_3); |
intgsull | 4:453d534b1c1d | 18 | AVEncoder r_enco(PA_1, PA_10); |
intgsull | 2:5f9b78950a17 | 19 | |
intgsull | 4:453d534b1c1d | 20 | // gyro |
intgsull | 2:5f9b78950a17 | 21 | AnalogIn _gyro(PA_0); |
intgsull | 4:453d534b1c1d | 22 | // AnalogIn gyro_cal(PC_1) ?? currently this isn't connected. |
intgsull | 1:5b9fa1823663 | 23 | |
intgsull | 2:5f9b78950a17 | 24 | //Left Front IR |
intgsull | 2:5f9b78950a17 | 25 | DigitalOut eLF(PC_3); |
intgsull | 2:5f9b78950a17 | 26 | AnalogIn rLF(PC_0); |
intgsull | 2:5f9b78950a17 | 27 | //PC_4 is an ADC |
intgsull | 2:5f9b78950a17 | 28 | //Left Side IR |
intgsull | 2:5f9b78950a17 | 29 | DigitalOut eLS(PC_2); |
intgsull | 2:5f9b78950a17 | 30 | AnalogIn rLS(PC_1); |
intgsull | 2:5f9b78950a17 | 31 | |
intgsull | 2:5f9b78950a17 | 32 | //Right Front IR |
intgsull | 2:5f9b78950a17 | 33 | DigitalOut eRF(PC_12); |
intgsull | 2:5f9b78950a17 | 34 | AnalogIn rRF(PA_4); |
intgsull | 2:5f9b78950a17 | 35 | |
intgsull | 2:5f9b78950a17 | 36 | //Right Side IR |
intgsull | 2:5f9b78950a17 | 37 | DigitalOut eRS(PC_15); |
intgsull | 2:5f9b78950a17 | 38 | AnalogIn rRS(PB_0); |
intgsull | 1:5b9fa1823663 | 39 | |
intgsull | 0:fa523db3f4f5 | 40 | DigitalOut myled(LED1); |
intgsull | 0:fa523db3f4f5 | 41 | |
intgsull | 4:453d534b1c1d | 42 | // global variables. |
intgsull | 4:453d534b1c1d | 43 | volatile float gyro_offset = 0; |
intgsull | 4:453d534b1c1d | 44 | |
intgsull | 4:453d534b1c1d | 45 | volatile float line_speed = 1; // currently is in terms of encoder pulses / ms. |
intgsull | 4:453d534b1c1d | 46 | volatile float angl_speed = 0; // should not turn while moving forward. |
intgsull | 4:453d534b1c1d | 47 | |
intgsull | 4:453d534b1c1d | 48 | volatile float line_prevError = 0; |
intgsull | 4:453d534b1c1d | 49 | volatile int enco_prevError = 0; |
intgsull | 4:453d534b1c1d | 50 | volatile float gyro_prevError = 0; |
intgsull | 1:5b9fa1823663 | 51 | |
intgsull | 4:453d534b1c1d | 52 | volatile float line_accumulator = 0; |
intgsull | 4:453d534b1c1d | 53 | volatile float line_decayFactor = 1.5; |
intgsull | 4:453d534b1c1d | 54 | volatile float enco_accumulator = 0; |
intgsull | 4:453d534b1c1d | 55 | volatile float enco_decayFactor = 1.5; |
intgsull | 4:453d534b1c1d | 56 | volatile float gyro_accumulator = 0; |
intgsull | 4:453d534b1c1d | 57 | volatile float gyro_decayFactor = 1.5; |
intgsull | 4:453d534b1c1d | 58 | |
intgsull | 4:453d534b1c1d | 59 | volatile float left_speed = 0; |
intgsull | 4:453d534b1c1d | 60 | volatile float right_speed = 0; |
intgsull | 1:5b9fa1823663 | 61 | |
intgsull | 4:453d534b1c1d | 62 | volatile unsigned long l_pulses = 0; |
intgsull | 4:453d534b1c1d | 63 | volatile unsigned long r_pulses = 0; |
intgsull | 0:fa523db3f4f5 | 64 | |
intgsull | 4:453d534b1c1d | 65 | // pid constants. these need to be tuned. but i set them as a default val for now. |
intgsull | 4:453d534b1c1d | 66 | // line refers to the translational speed. |
intgsull | 4:453d534b1c1d | 67 | // enco and gyro will be used primarily for angular speed. |
intgsull | 4:453d534b1c1d | 68 | // (we can change the names later, |
intgsull | 4:453d534b1c1d | 69 | // i added line in after i realized that i already had the angular code) |
intgsull | 4:453d534b1c1d | 70 | const float line_propo = 1; |
intgsull | 4:453d534b1c1d | 71 | const float line_integ = 1; |
intgsull | 4:453d534b1c1d | 72 | const float line_deriv = 1; |
intgsull | 2:5f9b78950a17 | 73 | |
intgsull | 4:453d534b1c1d | 74 | const float gyro_propo = 1; |
intgsull | 4:453d534b1c1d | 75 | const float gyro_integ = 1; |
intgsull | 4:453d534b1c1d | 76 | const float gyro_deriv = 1; |
intgsull | 4:453d534b1c1d | 77 | |
intgsull | 4:453d534b1c1d | 78 | const float enco_propo = 1; |
intgsull | 4:453d534b1c1d | 79 | const float enco_integ = 1; |
intgsull | 4:453d534b1c1d | 80 | const float enco_deriv = 1; |
intgsull | 4:453d534b1c1d | 81 | |
intgsull | 4:453d534b1c1d | 82 | const float spin_enco_weight = 0.5; |
intgsull | 4:453d534b1c1d | 83 | const float spin_gyro_weight = 1 - spin_enco_weight; |
intgsull | 1:5b9fa1823663 | 84 | |
intgsull | 4:453d534b1c1d | 85 | // this is just so that we can maintain what state our mouse is in. |
intgsull | 4:453d534b1c1d | 86 | // currently this has no real use, but it may in the future. |
intgsull | 4:453d534b1c1d | 87 | // or we could just remove this entirely. |
intgsull | 4:453d534b1c1d | 88 | typedef enum |
intgsull | 4:453d534b1c1d | 89 | { |
intgsull | 4:453d534b1c1d | 90 | STOPPED, |
intgsull | 4:453d534b1c1d | 91 | FORWARD, |
intgsull | 4:453d534b1c1d | 92 | TURNING, |
intgsull | 4:453d534b1c1d | 93 | UNKNOWN |
intgsull | 4:453d534b1c1d | 94 | } STATE; |
intgsull | 4:453d534b1c1d | 95 | STATE mouse_state; |
intgsull | 4:453d534b1c1d | 96 | |
intgsull | 4:453d534b1c1d | 97 | // helper functions |
intgsull | 4:453d534b1c1d | 98 | void reset(); |
intgsull | 4:453d534b1c1d | 99 | void offsetCalc(); |
intgsull | 4:453d534b1c1d | 100 | void stop(); |
intgsull | 4:453d534b1c1d | 101 | void moveForward(); |
intgsull | 4:453d534b1c1d | 102 | void turn(); |
intgsull | 1:5b9fa1823663 | 103 | |
intgsull | 4:453d534b1c1d | 104 | // interrupt stuff. |
intgsull | 4:453d534b1c1d | 105 | void incLENC() |
intgsull | 4:453d534b1c1d | 106 | { |
intgsull | 4:453d534b1c1d | 107 | l_pulses++; |
intgsull | 4:453d534b1c1d | 108 | } |
intgsull | 4:453d534b1c1d | 109 | |
intgsull | 4:453d534b1c1d | 110 | void incRENC() |
intgsull | 4:453d534b1c1d | 111 | { |
intgsull | 4:453d534b1c1d | 112 | r_pulses++; |
intgsull | 4:453d534b1c1d | 113 | } |
intgsull | 2:5f9b78950a17 | 114 | |
intgsull | 4:453d534b1c1d | 115 | // PID Control |
intgsull | 4:453d534b1c1d | 116 | // this contains the simplistic PID control for the most part. |
intgsull | 4:453d534b1c1d | 117 | // we do have to calibrate constants though. |
intgsull | 4:453d534b1c1d | 118 | void systick() |
intgsull | 4:453d534b1c1d | 119 | { |
intgsull | 4:453d534b1c1d | 120 | if ( mouse_state == STOPPED || mouse_state == UNKNOWN ) |
intgsull | 4:453d534b1c1d | 121 | { |
intgsull | 4:453d534b1c1d | 122 | // do nothing? |
intgsull | 4:453d534b1c1d | 123 | // reset? |
intgsull | 4:453d534b1c1d | 124 | reset(); |
intgsull | 4:453d534b1c1d | 125 | offsetCalc(); |
intgsull | 4:453d534b1c1d | 126 | return; |
intgsull | 2:5f9b78950a17 | 127 | } |
intgsull | 2:5f9b78950a17 | 128 | |
intgsull | 4:453d534b1c1d | 129 | int dt = timer.read_us(); // should be around 1 ms. |
intgsull | 4:453d534b1c1d | 130 | timer.reset(); |
intgsull | 4:453d534b1c1d | 131 | |
intgsull | 4:453d534b1c1d | 132 | float line_error = line_speed * dt - 0.5 * ( l_enco.getPulses() - r_enco.getPulses()); |
intgsull | 4:453d534b1c1d | 133 | int enco_error = l_enco.getPulses() - r_enco.getPulses(); |
intgsull | 4:453d534b1c1d | 134 | float gyro_error = _gyro.read() - gyro_offset; |
intgsull | 4:453d534b1c1d | 135 | |
intgsull | 4:453d534b1c1d | 136 | line_accumulator += line_error; |
intgsull | 4:453d534b1c1d | 137 | enco_accumulator += enco_error; |
intgsull | 4:453d534b1c1d | 138 | gyro_accumulator += gyro_error; |
intgsull | 4:453d534b1c1d | 139 | |
intgsull | 4:453d534b1c1d | 140 | float line_pid = 0; |
intgsull | 4:453d534b1c1d | 141 | line_pid += line_propo * line_error; |
intgsull | 4:453d534b1c1d | 142 | line_pid += line_integ * line_accumulator; |
intgsull | 4:453d534b1c1d | 143 | line_pid += line_deriv * (line_error - line_prevError)/dt; |
intgsull | 4:453d534b1c1d | 144 | |
intgsull | 4:453d534b1c1d | 145 | float enco_pid = 0; |
intgsull | 4:453d534b1c1d | 146 | enco_pid += enco_propo * enco_error; |
intgsull | 4:453d534b1c1d | 147 | enco_pid += enco_integ * enco_accumulator; |
intgsull | 4:453d534b1c1d | 148 | enco_pid += enco_deriv * (enco_error - enco_prevError)/dt; |
intgsull | 4:453d534b1c1d | 149 | |
intgsull | 4:453d534b1c1d | 150 | float gyro_pid = 0; |
intgsull | 4:453d534b1c1d | 151 | gyro_pid += gyro_propo * gyro_error; |
intgsull | 4:453d534b1c1d | 152 | gyro_pid += gyro_integ * gyro_accumulator; |
intgsull | 4:453d534b1c1d | 153 | gyro_pid += gyro_deriv * (gyro_error - gyro_prevError)/dt; |
intgsull | 4:453d534b1c1d | 154 | |
intgsull | 4:453d534b1c1d | 155 | // forward moving pid control. |
intgsull | 4:453d534b1c1d | 156 | if ( mouse_state == FORWARD ) |
intgsull | 4:453d534b1c1d | 157 | { |
intgsull | 4:453d534b1c1d | 158 | float x_error = line_pid; |
intgsull | 4:453d534b1c1d | 159 | float w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid; |
intgsull | 4:453d534b1c1d | 160 | left_speed += x_error + w_error; |
intgsull | 4:453d534b1c1d | 161 | right_speed += x_error - w_error; |
intgsull | 4:453d534b1c1d | 162 | // offsetCalc(); |
intgsull | 4:453d534b1c1d | 163 | } |
intgsull | 4:453d534b1c1d | 164 | if ( mouse_state == TURNING ) |
intgsull | 4:453d534b1c1d | 165 | { |
intgsull | 4:453d534b1c1d | 166 | // nothing for now. if we turn in place, we assume no pid control. |
intgsull | 4:453d534b1c1d | 167 | // this may have to change when we try curve turns. |
intgsull | 4:453d534b1c1d | 168 | } |
intgsull | 4:453d534b1c1d | 169 | |
intgsull | 4:453d534b1c1d | 170 | line_prevError = line_error; |
intgsull | 4:453d534b1c1d | 171 | enco_prevError = enco_error; |
intgsull | 4:453d534b1c1d | 172 | gyro_prevError = gyro_error; |
intgsull | 4:453d534b1c1d | 173 | |
intgsull | 4:453d534b1c1d | 174 | line_accumulator /= line_decayFactor; |
intgsull | 4:453d534b1c1d | 175 | enco_accumulator /= enco_decayFactor; |
intgsull | 4:453d534b1c1d | 176 | gyro_accumulator /= gyro_decayFactor; |
intgsull | 4:453d534b1c1d | 177 | } |
intgsull | 4:453d534b1c1d | 178 | |
intgsull | 4:453d534b1c1d | 179 | // setup stuff. |
intgsull | 4:453d534b1c1d | 180 | void setup() |
intgsull | 1:5b9fa1823663 | 181 | { |
intgsull | 4:453d534b1c1d | 182 | mouse_state = STOPPED; |
intgsull | 4:453d534b1c1d | 183 | |
intgsull | 4:453d534b1c1d | 184 | eRS = 0; |
intgsull | 4:453d534b1c1d | 185 | eRF = 0; |
intgsull | 4:453d534b1c1d | 186 | eLS = 0; |
intgsull | 4:453d534b1c1d | 187 | eLF = 0; |
intgsull | 4:453d534b1c1d | 188 | |
intgsull | 4:453d534b1c1d | 189 | myled = 1; |
intgsull | 1:5b9fa1823663 | 190 | |
intgsull | 4:453d534b1c1d | 191 | // repeat this for some time frame. |
intgsull | 4:453d534b1c1d | 192 | offsetCalc(); |
intgsull | 4:453d534b1c1d | 193 | Systicker.attach_us(&systick, 1000); |
intgsull | 4:453d534b1c1d | 194 | } |
intgsull | 4:453d534b1c1d | 195 | |
intgsull | 4:453d534b1c1d | 196 | void reset() |
intgsull | 4:453d534b1c1d | 197 | { |
intgsull | 4:453d534b1c1d | 198 | l_pulses = 0; |
intgsull | 4:453d534b1c1d | 199 | r_pulses = 0; |
intgsull | 1:5b9fa1823663 | 200 | } |
intgsull | 1:5b9fa1823663 | 201 | |
intgsull | 1:5b9fa1823663 | 202 | |
intgsull | 4:453d534b1c1d | 203 | // computes gyro_offset |
intgsull | 4:453d534b1c1d | 204 | // uses a "weighted" average. |
intgsull | 4:453d534b1c1d | 205 | // idea is that the current gyro offset is weighted more than previous ones. |
intgsull | 4:453d534b1c1d | 206 | // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n). |
intgsull | 4:453d534b1c1d | 207 | // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.) |
intgsull | 4:453d534b1c1d | 208 | // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight. |
intgsull | 4:453d534b1c1d | 209 | // currently this is only in the setup function. we can run this when the mouse is running in a line |
intgsull | 4:453d534b1c1d | 210 | // when we figure out good line running pid. |
intgsull | 4:453d534b1c1d | 211 | void offsetCalc() |
intgsull | 4:453d534b1c1d | 212 | { |
intgsull | 4:453d534b1c1d | 213 | gyro_offset = gyro_offset / 2 + _gyro.read() / 2; |
intgsull | 4:453d534b1c1d | 214 | } |
intgsull | 1:5b9fa1823663 | 215 | |
intgsull | 4:453d534b1c1d | 216 | |
intgsull | 0:fa523db3f4f5 | 217 | |
intgsull | 4:453d534b1c1d | 218 | int main() |
intgsull | 4:453d534b1c1d | 219 | { |
intgsull | 4:453d534b1c1d | 220 | setup(); |
intgsull | 4:453d534b1c1d | 221 | while(1) |
intgsull | 4:453d534b1c1d | 222 | { |
intgsull | 4:453d534b1c1d | 223 | pc.printf("The left motor is going at speed: %d\r\n", left_speed); |
intgsull | 4:453d534b1c1d | 224 | pc.printf("The left motor is going at speed: %d\r\n", right_speed); |
intgsull | 4:453d534b1c1d | 225 | wait(1); |
intgsull | 0:fa523db3f4f5 | 226 | } |
intgsull | 0:fa523db3f4f5 | 227 | } |
intgsull | 0:fa523db3f4f5 | 228 | |
intgsull | 4:453d534b1c1d | 229 | |
intgsull | 4:453d534b1c1d | 230 | // movement functions. |
intgsull | 4:453d534b1c1d | 231 | void moveForward() |
intgsull | 4:453d534b1c1d | 232 | { |
intgsull | 4:453d534b1c1d | 233 | mouse_state = FORWARD; |
intgsull | 4:453d534b1c1d | 234 | |
intgsull | 4:453d534b1c1d | 235 | if ( left_speed > 0 ) // which should be always. |
intgsull | 4:453d534b1c1d | 236 | { |
intgsull | 4:453d534b1c1d | 237 | motor1_forward = left_speed; |
intgsull | 4:453d534b1c1d | 238 | motor1_reverse = 0; |
intgsull | 4:453d534b1c1d | 239 | } |
intgsull | 4:453d534b1c1d | 240 | else |
intgsull | 4:453d534b1c1d | 241 | { |
intgsull | 4:453d534b1c1d | 242 | motor1_forward = 0; |
intgsull | 4:453d534b1c1d | 243 | motor1_reverse = -left_speed; |
intgsull | 0:fa523db3f4f5 | 244 | } |
intgsull | 0:fa523db3f4f5 | 245 | |
intgsull | 4:453d534b1c1d | 246 | if ( right_speed > 0 ) // which again should be always. |
intgsull | 4:453d534b1c1d | 247 | { |
intgsull | 4:453d534b1c1d | 248 | motor2_forward = right_speed; |
intgsull | 0:fa523db3f4f5 | 249 | motor2_reverse = 0; |
intgsull | 0:fa523db3f4f5 | 250 | } |
intgsull | 4:453d534b1c1d | 251 | else |
intgsull | 4:453d534b1c1d | 252 | { |
intgsull | 0:fa523db3f4f5 | 253 | motor2_forward = 0; |
intgsull | 4:453d534b1c1d | 254 | motor2_reverse = -right_speed; |
intgsull | 0:fa523db3f4f5 | 255 | } |
intgsull | 4:453d534b1c1d | 256 | } |
intgsull | 4:453d534b1c1d | 257 | |
intgsull | 4:453d534b1c1d | 258 | void stop() |
intgsull | 4:453d534b1c1d | 259 | { |
intgsull | 4:453d534b1c1d | 260 | mouse_state = STOPPED; |
intgsull | 4:453d534b1c1d | 261 | |
intgsull | 4:453d534b1c1d | 262 | motor1_forward = 1.0; |
intgsull | 4:453d534b1c1d | 263 | motor1_reverse = 1.0; |
intgsull | 4:453d534b1c1d | 264 | motor2_forward = 1.0; |
intgsull | 4:453d534b1c1d | 265 | motor2_reverse = 1.0; |
intgsull | 4:453d534b1c1d | 266 | } |
intgsull | 4:453d534b1c1d | 267 | |
intgsull | 4:453d534b1c1d | 268 | void turn(int dir); // maybe split this into two functions? |