hello
Dependencies: AVEncoder QEI mbed-src-AV
main.cpp@9:e2feaadf504c, 2015-11-20 (annotated)
- Committer:
- intgsull
- Date:
- Fri Nov 20 04:18:00 2015 +0000
- Revision:
- 9:e2feaadf504c
- Parent:
- 8:a254346f20aa
IR stuff added
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 | 6:32d9b855b90f | 4 | #include "analogin_api.h" |
intgsull | 0:fa523db3f4f5 | 5 | |
intgsull | 4:453d534b1c1d | 6 | // set things |
intgsull | 4:453d534b1c1d | 7 | Serial pc(SERIAL_TX, SERIAL_RX); |
intgsull | 4:453d534b1c1d | 8 | Ticker Systicker; |
intgsull | 4:453d534b1c1d | 9 | Timer timer; |
intgsull | 4:453d534b1c1d | 10 | |
intgsull | 4:453d534b1c1d | 11 | PwmOut motor1_forward(PB_10); |
intgsull | 2:5f9b78950a17 | 12 | PwmOut motor1_reverse(PA_6); |
intgsull | 0:fa523db3f4f5 | 13 | PwmOut motor2_forward(PA_7); |
intgsull | 0:fa523db3f4f5 | 14 | PwmOut motor2_reverse(PB_6); |
intgsull | 0:fa523db3f4f5 | 15 | |
intgsull | 4:453d534b1c1d | 16 | // TODO: change our encoder pins from AnalogIn into: |
intgsull | 4:453d534b1c1d | 17 | // otherwise, we can also use the AVEncoder thing as well. |
intgsull | 4:453d534b1c1d | 18 | AVEncoder l_enco(PA_15, PB_3); |
intgsull | 4:453d534b1c1d | 19 | AVEncoder r_enco(PA_1, PA_10); |
intgsull | 2:5f9b78950a17 | 20 | |
intgsull | 4:453d534b1c1d | 21 | // gyro |
intgsull | 2:5f9b78950a17 | 22 | AnalogIn _gyro(PA_0); |
intgsull | 4:453d534b1c1d | 23 | // AnalogIn gyro_cal(PC_1) ?? currently this isn't connected. |
intgsull | 1:5b9fa1823663 | 24 | |
intgsull | 2:5f9b78950a17 | 25 | //Left Front IR |
intgsull | 2:5f9b78950a17 | 26 | DigitalOut eLF(PC_3); |
intgsull | 2:5f9b78950a17 | 27 | AnalogIn rLF(PC_0); |
intgsull | 2:5f9b78950a17 | 28 | //PC_4 is an ADC |
intgsull | 2:5f9b78950a17 | 29 | //Left Side IR |
intgsull | 2:5f9b78950a17 | 30 | DigitalOut eLS(PC_2); |
intgsull | 2:5f9b78950a17 | 31 | AnalogIn rLS(PC_1); |
intgsull | 2:5f9b78950a17 | 32 | |
intgsull | 2:5f9b78950a17 | 33 | //Right Front IR |
intgsull | 2:5f9b78950a17 | 34 | DigitalOut eRF(PC_12); |
intgsull | 2:5f9b78950a17 | 35 | AnalogIn rRF(PA_4); |
intgsull | 2:5f9b78950a17 | 36 | |
intgsull | 2:5f9b78950a17 | 37 | //Right Side IR |
intgsull | 2:5f9b78950a17 | 38 | DigitalOut eRS(PC_15); |
intgsull | 2:5f9b78950a17 | 39 | AnalogIn rRS(PB_0); |
intgsull | 1:5b9fa1823663 | 40 | |
intgsull | 0:fa523db3f4f5 | 41 | DigitalOut myled(LED1); |
intgsull | 0:fa523db3f4f5 | 42 | |
intgsull | 4:453d534b1c1d | 43 | // global variables. |
intgsull | 4:453d534b1c1d | 44 | volatile float gyro_offset = 0; |
intgsull | 4:453d534b1c1d | 45 | |
intgsull | 4:453d534b1c1d | 46 | volatile float line_speed = 1; // currently is in terms of encoder pulses / ms. |
intgsull | 4:453d534b1c1d | 47 | volatile float angl_speed = 0; // should not turn while moving forward. |
intgsull | 4:453d534b1c1d | 48 | |
intgsull | 4:453d534b1c1d | 49 | volatile float line_prevError = 0; |
intgsull | 4:453d534b1c1d | 50 | volatile int enco_prevError = 0; |
intgsull | 4:453d534b1c1d | 51 | volatile float gyro_prevError = 0; |
intgsull | 1:5b9fa1823663 | 52 | |
intgsull | 4:453d534b1c1d | 53 | volatile float line_accumulator = 0; |
intgsull | 6:32d9b855b90f | 54 | volatile float line_decayFactor = 1; |
intgsull | 4:453d534b1c1d | 55 | volatile float enco_accumulator = 0; |
intgsull | 6:32d9b855b90f | 56 | volatile float enco_decayFactor = 1; |
intgsull | 4:453d534b1c1d | 57 | volatile float gyro_accumulator = 0; |
intgsull | 6:32d9b855b90f | 58 | volatile float gyro_decayFactor = 1; |
intgsull | 4:453d534b1c1d | 59 | |
intgsull | 4:453d534b1c1d | 60 | volatile float left_speed = 0; |
intgsull | 4:453d534b1c1d | 61 | volatile float right_speed = 0; |
intgsull | 1:5b9fa1823663 | 62 | |
intgsull | 4:453d534b1c1d | 63 | // pid constants. these need to be tuned. but i set them as a default val for now. |
intgsull | 4:453d534b1c1d | 64 | // line refers to the translational speed. |
intgsull | 4:453d534b1c1d | 65 | // enco and gyro will be used primarily for angular speed. |
intgsull | 4:453d534b1c1d | 66 | // (we can change the names later, |
intgsull | 4:453d534b1c1d | 67 | // i added line in after i realized that i already had the angular code) |
intgsull | 6:32d9b855b90f | 68 | const int max_speed = 6; // max speed is 6 encoder pulses per ms. |
intgsull | 6:32d9b855b90f | 69 | |
intgsull | 6:32d9b855b90f | 70 | const float line_propo = 0; |
aduriseti | 5:f9837617817b | 71 | const float line_integ = 0; |
aduriseti | 5:f9837617817b | 72 | const float line_deriv = 0; |
intgsull | 2:5f9b78950a17 | 73 | |
intgsull | 4:453d534b1c1d | 74 | const float gyro_propo = 1; |
aduriseti | 5:f9837617817b | 75 | const float gyro_integ = 0; |
aduriseti | 5:f9837617817b | 76 | const float gyro_deriv = 0; |
intgsull | 4:453d534b1c1d | 77 | |
intgsull | 4:453d534b1c1d | 78 | const float enco_propo = 1; |
aduriseti | 5:f9837617817b | 79 | const float enco_integ = 0; |
aduriseti | 5:f9837617817b | 80 | const float enco_deriv = 0; |
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 | 6:32d9b855b90f | 95 | volatile 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 | 9:e2feaadf504c | 103 | void irReset(); |
intgsull | 6:32d9b855b90f | 104 | |
intgsull | 6:32d9b855b90f | 105 | //irPID function to not crash into stuff |
intgsull | 6:32d9b855b90f | 106 | |
intgsull | 6:32d9b855b90f | 107 | const float leftWall= 0; |
intgsull | 6:32d9b855b90f | 108 | const float rightWall= 0; //we need to find the threshold value for when |
intgsull | 6:32d9b855b90f | 109 | //left or right walls are present |
intgsull | 6:32d9b855b90f | 110 | const float irOffset= 0; // difference between right and left ir sensors when |
intgsull | 6:32d9b855b90f | 111 | //mouse is in the middle |
intgsull | 6:32d9b855b90f | 112 | const float lirOffset= 0; //middle value for when there is only a wall on one |
intgsull | 6:32d9b855b90f | 113 | const float rirOffset= 0; //side of the mouse (left and right) |
intgsull | 6:32d9b855b90f | 114 | volatile float irError = 0; |
intgsull | 6:32d9b855b90f | 115 | volatile float irErrorD = 0; |
intgsull | 6:32d9b855b90f | 116 | volatile float irErrorI = 0; |
intgsull | 6:32d9b855b90f | 117 | volatile float oldirError = 0; |
intgsull | 6:32d9b855b90f | 118 | volatile float totalirError= 0; //errors |
intgsull | 6:32d9b855b90f | 119 | const float irKp = 0; |
intgsull | 6:32d9b855b90f | 120 | const float irKd = 0; |
intgsull | 6:32d9b855b90f | 121 | const float irKi = 0; //constants |
intgsull | 6:32d9b855b90f | 122 | volatile float leftDistance = 0; |
intgsull | 6:32d9b855b90f | 123 | volatile float rightDistance= 0; |
intgsull | 6:32d9b855b90f | 124 | void irPID() |
intgsull | 6:32d9b855b90f | 125 | { |
intgsull | 6:32d9b855b90f | 126 | eLS = 1; |
intgsull | 6:32d9b855b90f | 127 | eRS = 1; |
intgsull | 6:32d9b855b90f | 128 | if(rLS > leftWall && rRS > rightWall)//walls on both sides |
intgsull | 6:32d9b855b90f | 129 | { |
intgsull | 9:e2feaadf504c | 130 | //leftDistance = rLS; |
intgsull | 9:e2feaadf504c | 131 | // rightDistance = rRS; |
intgsull | 9:e2feaadf504c | 132 | irError = rRS-rLS-irOffset; //– leftDistance – irOffset; |
intgsull | 6:32d9b855b90f | 133 | |
intgsull | 9:e2feaadf504c | 134 | irErrorD = irError-oldirError; |
intgsull | 9:e2feaadf504c | 135 | //irErrorD -= oldirError;//(irError – oldirError); |
intgsull | 6:32d9b855b90f | 136 | |
intgsull | 6:32d9b855b90f | 137 | irErrorI += irError; |
intgsull | 6:32d9b855b90f | 138 | } |
intgsull | 6:32d9b855b90f | 139 | else if(rLS > leftWall) //just left wall |
intgsull | 6:32d9b855b90f | 140 | { |
intgsull | 6:32d9b855b90f | 141 | leftDistance = rLS; |
intgsull | 7:f1bceb2bab70 | 142 | irError = 2*(lirOffset-leftDistance);//(2 * (lirOffset – leftDistance)); |
intgsull | 6:32d9b855b90f | 143 | |
intgsull | 7:f1bceb2bab70 | 144 | irErrorD=irError-oldirError; |
intgsull | 7:f1bceb2bab70 | 145 | |
intgsull | 7:f1bceb2bab70 | 146 | irErrorI += irError; |
intgsull | 6:32d9b855b90f | 147 | } |
intgsull | 6:32d9b855b90f | 148 | else if(rRS > rightWall)//just right wall |
intgsull | 6:32d9b855b90f | 149 | { |
intgsull | 7:f1bceb2bab70 | 150 | rightDistance = rRS; |
intgsull | 8:a254346f20aa | 151 | irError=2*(rightDistance-rirOffset); |
intgsull | 8:a254346f20aa | 152 | irError = irError-oldirError; |
intgsull | 6:32d9b855b90f | 153 | irErrorI += irError; |
intgsull | 6:32d9b855b90f | 154 | } |
intgsull | 6:32d9b855b90f | 155 | else if(rLS < leftWall && rRS < rightWall)//no walls!! Use encoder PID |
intgsull | 6:32d9b855b90f | 156 | { |
intgsull | 6:32d9b855b90f | 157 | irError = 0; |
intgsull | 6:32d9b855b90f | 158 | irErrorD = 0; |
intgsull | 6:32d9b855b90f | 159 | irErrorI += irError; |
intgsull | 6:32d9b855b90f | 160 | } |
intgsull | 6:32d9b855b90f | 161 | totalirError = irError*irKp + irErrorD*irKd + irErrorI*irKi; |
intgsull | 6:32d9b855b90f | 162 | oldirError = irError; |
intgsull | 6:32d9b855b90f | 163 | left_speed -= totalirError; |
intgsull | 6:32d9b855b90f | 164 | right_speed += totalirError; |
intgsull | 6:32d9b855b90f | 165 | eLS = 0; |
intgsull | 6:32d9b855b90f | 166 | eRS = 0; |
intgsull | 4:453d534b1c1d | 167 | } |
intgsull | 4:453d534b1c1d | 168 | |
intgsull | 6:32d9b855b90f | 169 | const float frontWall = 0; //need to calibrate this threshold to a value where mouse can stop in time |
intgsull | 6:32d9b855b90f | 170 | //something like this may be useful for the very temporal present |
intgsull | 6:32d9b855b90f | 171 | //doesn't account for current speed/trajectory atm |
intgsull | 6:32d9b855b90f | 172 | void dontDriveStraightIntoAWall() |
intgsull | 4:453d534b1c1d | 173 | { |
intgsull | 6:32d9b855b90f | 174 | eRF = 1; |
intgsull | 6:32d9b855b90f | 175 | eLF = 1; |
intgsull | 6:32d9b855b90f | 176 | if(rRF > frontWall || rLF > frontWall) |
intgsull | 6:32d9b855b90f | 177 | { |
intgsull | 6:32d9b855b90f | 178 | eRF = 0; |
intgsull | 6:32d9b855b90f | 179 | eLF = 0; |
intgsull | 6:32d9b855b90f | 180 | mouse_state = STOPPED; |
intgsull | 6:32d9b855b90f | 181 | stop(); |
intgsull | 6:32d9b855b90f | 182 | return; |
intgsull | 6:32d9b855b90f | 183 | } |
intgsull | 4:453d534b1c1d | 184 | } |
intgsull | 2:5f9b78950a17 | 185 | |
intgsull | 4:453d534b1c1d | 186 | // PID Control |
intgsull | 4:453d534b1c1d | 187 | // this contains the simplistic PID control for the most part. |
intgsull | 4:453d534b1c1d | 188 | // we do have to calibrate constants though. |
intgsull | 4:453d534b1c1d | 189 | void systick() |
intgsull | 4:453d534b1c1d | 190 | { |
intgsull | 6:32d9b855b90f | 191 | //pc.printf("systick ran\r\n"); |
intgsull | 4:453d534b1c1d | 192 | if ( mouse_state == STOPPED || mouse_state == UNKNOWN ) |
intgsull | 4:453d534b1c1d | 193 | { |
intgsull | 4:453d534b1c1d | 194 | // do nothing? |
intgsull | 4:453d534b1c1d | 195 | // reset? |
intgsull | 6:32d9b855b90f | 196 | //reset(); |
intgsull | 4:453d534b1c1d | 197 | offsetCalc(); |
intgsull | 4:453d534b1c1d | 198 | return; |
intgsull | 2:5f9b78950a17 | 199 | } |
intgsull | 6:32d9b855b90f | 200 | //pc.printf("systick ran while state is FORWARD \r\n"); |
intgsull | 2:5f9b78950a17 | 201 | |
intgsull | 4:453d534b1c1d | 202 | int dt = timer.read_us(); // should be around 1 ms. |
intgsull | 4:453d534b1c1d | 203 | timer.reset(); |
intgsull | 4:453d534b1c1d | 204 | |
intgsull | 6:32d9b855b90f | 205 | //printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); |
intgsull | 6:32d9b855b90f | 206 | |
intgsull | 6:32d9b855b90f | 207 | float line_error = line_speed - 0.5 * ( l_enco.getPulses() + r_enco.getPulses()); |
intgsull | 4:453d534b1c1d | 208 | int enco_error = l_enco.getPulses() - r_enco.getPulses(); |
intgsull | 4:453d534b1c1d | 209 | float gyro_error = _gyro.read() - gyro_offset; |
intgsull | 4:453d534b1c1d | 210 | |
intgsull | 4:453d534b1c1d | 211 | line_accumulator += line_error; |
intgsull | 4:453d534b1c1d | 212 | enco_accumulator += enco_error; |
intgsull | 4:453d534b1c1d | 213 | gyro_accumulator += gyro_error; |
intgsull | 4:453d534b1c1d | 214 | |
intgsull | 4:453d534b1c1d | 215 | float line_pid = 0; |
intgsull | 4:453d534b1c1d | 216 | line_pid += line_propo * line_error; |
intgsull | 4:453d534b1c1d | 217 | line_pid += line_integ * line_accumulator; |
intgsull | 4:453d534b1c1d | 218 | line_pid += line_deriv * (line_error - line_prevError)/dt; |
intgsull | 4:453d534b1c1d | 219 | |
intgsull | 4:453d534b1c1d | 220 | float enco_pid = 0; |
intgsull | 4:453d534b1c1d | 221 | enco_pid += enco_propo * enco_error; |
intgsull | 4:453d534b1c1d | 222 | enco_pid += enco_integ * enco_accumulator; |
intgsull | 4:453d534b1c1d | 223 | enco_pid += enco_deriv * (enco_error - enco_prevError)/dt; |
intgsull | 4:453d534b1c1d | 224 | |
intgsull | 4:453d534b1c1d | 225 | float gyro_pid = 0; |
intgsull | 4:453d534b1c1d | 226 | gyro_pid += gyro_propo * gyro_error; |
intgsull | 4:453d534b1c1d | 227 | gyro_pid += gyro_integ * gyro_accumulator; |
intgsull | 6:32d9b855b90f | 228 | gyro_pid += gyro_deriv *(gyro_error - gyro_prevError)/dt; |
intgsull | 4:453d534b1c1d | 229 | |
intgsull | 4:453d534b1c1d | 230 | // forward moving pid control. |
intgsull | 4:453d534b1c1d | 231 | if ( mouse_state == FORWARD ) |
intgsull | 4:453d534b1c1d | 232 | { |
intgsull | 4:453d534b1c1d | 233 | float x_error = line_pid; |
intgsull | 6:32d9b855b90f | 234 | float w_error = 0; //spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid; |
intgsull | 6:32d9b855b90f | 235 | left_speed += (x_error - w_error); |
intgsull | 6:32d9b855b90f | 236 | right_speed += (x_error + w_error); //swapped |
aduriseti | 5:f9837617817b | 237 | |
intgsull | 6:32d9b855b90f | 238 | //pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); |
aduriseti | 5:f9837617817b | 239 | |
aduriseti | 5:f9837617817b | 240 | moveForward(); |
intgsull | 4:453d534b1c1d | 241 | // offsetCalc(); |
intgsull | 4:453d534b1c1d | 242 | } |
intgsull | 4:453d534b1c1d | 243 | if ( mouse_state == TURNING ) |
intgsull | 4:453d534b1c1d | 244 | { |
intgsull | 4:453d534b1c1d | 245 | // nothing for now. if we turn in place, we assume no pid control. |
intgsull | 4:453d534b1c1d | 246 | // this may have to change when we try curve turns. |
intgsull | 4:453d534b1c1d | 247 | } |
intgsull | 4:453d534b1c1d | 248 | |
intgsull | 4:453d534b1c1d | 249 | line_prevError = line_error; |
intgsull | 4:453d534b1c1d | 250 | enco_prevError = enco_error; |
intgsull | 4:453d534b1c1d | 251 | gyro_prevError = gyro_error; |
intgsull | 4:453d534b1c1d | 252 | |
intgsull | 4:453d534b1c1d | 253 | line_accumulator /= line_decayFactor; |
intgsull | 4:453d534b1c1d | 254 | enco_accumulator /= enco_decayFactor; |
intgsull | 4:453d534b1c1d | 255 | gyro_accumulator /= gyro_decayFactor; |
aduriseti | 5:f9837617817b | 256 | |
aduriseti | 5:f9837617817b | 257 | reset(); |
intgsull | 4:453d534b1c1d | 258 | } |
intgsull | 4:453d534b1c1d | 259 | |
intgsull | 4:453d534b1c1d | 260 | // setup stuff. |
intgsull | 4:453d534b1c1d | 261 | void setup() |
intgsull | 1:5b9fa1823663 | 262 | { |
aduriseti | 5:f9837617817b | 263 | pc.printf("Hello World\r\n"); |
intgsull | 6:32d9b855b90f | 264 | pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); |
intgsull | 6:32d9b855b90f | 265 | pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); |
intgsull | 4:453d534b1c1d | 266 | mouse_state = STOPPED; |
intgsull | 4:453d534b1c1d | 267 | |
intgsull | 6:32d9b855b90f | 268 | reset(); |
intgsull | 6:32d9b855b90f | 269 | |
intgsull | 4:453d534b1c1d | 270 | eRS = 0; |
intgsull | 4:453d534b1c1d | 271 | eRF = 0; |
intgsull | 4:453d534b1c1d | 272 | eLS = 0; |
intgsull | 4:453d534b1c1d | 273 | eLF = 0; |
intgsull | 4:453d534b1c1d | 274 | |
intgsull | 4:453d534b1c1d | 275 | myled = 1; |
intgsull | 1:5b9fa1823663 | 276 | |
intgsull | 4:453d534b1c1d | 277 | // repeat this for some time frame. |
aduriseti | 5:f9837617817b | 278 | for ( int i = 0; i < 200; i++ ) |
aduriseti | 5:f9837617817b | 279 | offsetCalc(); |
intgsull | 4:453d534b1c1d | 280 | Systicker.attach_us(&systick, 1000); |
intgsull | 4:453d534b1c1d | 281 | } |
intgsull | 4:453d534b1c1d | 282 | |
intgsull | 4:453d534b1c1d | 283 | void reset() |
intgsull | 4:453d534b1c1d | 284 | { |
aduriseti | 5:f9837617817b | 285 | l_enco.reset(); |
aduriseti | 5:f9837617817b | 286 | r_enco.reset(); |
intgsull | 1:5b9fa1823663 | 287 | } |
intgsull | 1:5b9fa1823663 | 288 | |
intgsull | 1:5b9fa1823663 | 289 | |
intgsull | 4:453d534b1c1d | 290 | // computes gyro_offset |
intgsull | 4:453d534b1c1d | 291 | // uses a "weighted" average. |
intgsull | 4:453d534b1c1d | 292 | // idea is that the current gyro offset is weighted more than previous ones. |
intgsull | 4:453d534b1c1d | 293 | // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n). |
intgsull | 4:453d534b1c1d | 294 | // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.) |
intgsull | 4:453d534b1c1d | 295 | // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight. |
intgsull | 4:453d534b1c1d | 296 | // currently this is only in the setup function. we can run this when the mouse is running in a line |
intgsull | 4:453d534b1c1d | 297 | // when we figure out good line running pid. |
intgsull | 4:453d534b1c1d | 298 | void offsetCalc() |
intgsull | 4:453d534b1c1d | 299 | { |
intgsull | 4:453d534b1c1d | 300 | gyro_offset = gyro_offset / 2 + _gyro.read() / 2; |
intgsull | 4:453d534b1c1d | 301 | } |
intgsull | 1:5b9fa1823663 | 302 | |
intgsull | 4:453d534b1c1d | 303 | |
intgsull | 0:fa523db3f4f5 | 304 | |
intgsull | 4:453d534b1c1d | 305 | int main() |
intgsull | 4:453d534b1c1d | 306 | { |
intgsull | 4:453d534b1c1d | 307 | setup(); |
intgsull | 6:32d9b855b90f | 308 | pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); |
intgsull | 6:32d9b855b90f | 309 | |
intgsull | 6:32d9b855b90f | 310 | wait(2); |
aduriseti | 5:f9837617817b | 311 | |
intgsull | 6:32d9b855b90f | 312 | mouse_state = FORWARD; |
intgsull | 6:32d9b855b90f | 313 | for ( int i = 0; i < 10; i++ ) |
intgsull | 6:32d9b855b90f | 314 | { |
intgsull | 6:32d9b855b90f | 315 | pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); |
intgsull | 6:32d9b855b90f | 316 | pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); |
intgsull | 6:32d9b855b90f | 317 | } |
aduriseti | 5:f9837617817b | 318 | stop(); |
aduriseti | 5:f9837617817b | 319 | |
intgsull | 6:32d9b855b90f | 320 | //pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); |
aduriseti | 5:f9837617817b | 321 | |
aduriseti | 5:f9837617817b | 322 | //while(1) |
aduriseti | 5:f9837617817b | 323 | // { |
aduriseti | 5:f9837617817b | 324 | // pc.printf("The left motor is going at speed: %d\r\n", left_speed); |
aduriseti | 5:f9837617817b | 325 | // pc.printf("The left motor is going at speed: %d\r\n", right_speed); |
aduriseti | 5:f9837617817b | 326 | // wait(1); |
aduriseti | 5:f9837617817b | 327 | // } |
intgsull | 0:fa523db3f4f5 | 328 | } |
intgsull | 0:fa523db3f4f5 | 329 | |
intgsull | 4:453d534b1c1d | 330 | |
intgsull | 4:453d534b1c1d | 331 | // movement functions. |
intgsull | 4:453d534b1c1d | 332 | void moveForward() |
intgsull | 4:453d534b1c1d | 333 | { |
intgsull | 6:32d9b855b90f | 334 | //mouse_state = FORWARD; |
intgsull | 4:453d534b1c1d | 335 | |
intgsull | 4:453d534b1c1d | 336 | if ( left_speed > 0 ) // which should be always. |
intgsull | 4:453d534b1c1d | 337 | { |
intgsull | 6:32d9b855b90f | 338 | motor1_forward = left_speed / max_speed; |
intgsull | 4:453d534b1c1d | 339 | motor1_reverse = 0; |
intgsull | 4:453d534b1c1d | 340 | } |
intgsull | 4:453d534b1c1d | 341 | else |
intgsull | 4:453d534b1c1d | 342 | { |
intgsull | 4:453d534b1c1d | 343 | motor1_forward = 0; |
intgsull | 6:32d9b855b90f | 344 | motor1_reverse = -left_speed / max_speed; |
intgsull | 0:fa523db3f4f5 | 345 | } |
intgsull | 0:fa523db3f4f5 | 346 | |
intgsull | 4:453d534b1c1d | 347 | if ( right_speed > 0 ) // which again should be always. |
intgsull | 4:453d534b1c1d | 348 | { |
intgsull | 6:32d9b855b90f | 349 | motor2_forward = right_speed / max_speed; |
intgsull | 0:fa523db3f4f5 | 350 | motor2_reverse = 0; |
intgsull | 0:fa523db3f4f5 | 351 | } |
intgsull | 4:453d534b1c1d | 352 | else |
intgsull | 4:453d534b1c1d | 353 | { |
intgsull | 0:fa523db3f4f5 | 354 | motor2_forward = 0; |
intgsull | 6:32d9b855b90f | 355 | motor2_reverse = -right_speed / max_speed; |
intgsull | 0:fa523db3f4f5 | 356 | } |
intgsull | 4:453d534b1c1d | 357 | } |
intgsull | 4:453d534b1c1d | 358 | |
intgsull | 4:453d534b1c1d | 359 | void stop() |
intgsull | 4:453d534b1c1d | 360 | { |
intgsull | 4:453d534b1c1d | 361 | mouse_state = STOPPED; |
intgsull | 4:453d534b1c1d | 362 | |
aduriseti | 5:f9837617817b | 363 | |
intgsull | 4:453d534b1c1d | 364 | motor1_forward = 1.0; |
intgsull | 4:453d534b1c1d | 365 | motor1_reverse = 1.0; |
intgsull | 4:453d534b1c1d | 366 | motor2_forward = 1.0; |
intgsull | 4:453d534b1c1d | 367 | motor2_reverse = 1.0; |
aduriseti | 5:f9837617817b | 368 | |
aduriseti | 5:f9837617817b | 369 | |
intgsull | 4:453d534b1c1d | 370 | } |
intgsull | 4:453d534b1c1d | 371 | |
aduriseti | 5:f9837617817b | 372 | void turn()// maybe split this into two functions? |
aduriseti | 5:f9837617817b | 373 | { |
aduriseti | 5:f9837617817b | 374 | mouse_state = TURNING; |
aduriseti | 5:f9837617817b | 375 | float angle = 0; |
intgsull | 9:e2feaadf504c | 376 | while (angle < 0.90) |
aduriseti | 5:f9837617817b | 377 | { |
aduriseti | 5:f9837617817b | 378 | float gyro_val = _gyro.read() - gyro_offset; |
aduriseti | 5:f9837617817b | 379 | angle += gyro_val; |
aduriseti | 5:f9837617817b | 380 | |
aduriseti | 5:f9837617817b | 381 | pc.printf("%f\r\n", angle); |
aduriseti | 5:f9837617817b | 382 | |
aduriseti | 5:f9837617817b | 383 | motor1_forward = 0.5; |
aduriseti | 5:f9837617817b | 384 | motor1_reverse = 0; |
aduriseti | 5:f9837617817b | 385 | motor2_forward = 0; |
aduriseti | 5:f9837617817b | 386 | motor2_reverse = 0.5; |
aduriseti | 5:f9837617817b | 387 | } |
intgsull | 9:e2feaadf504c | 388 | } |
intgsull | 9:e2feaadf504c | 389 | void irReset() //maybe split resets |
intgsull | 9:e2feaadf504c | 390 | { |
intgsull | 9:e2feaadf504c | 391 | eRF = 0; |
intgsull | 9:e2feaadf504c | 392 | eRS = 0; |
intgsull | 9:e2feaadf504c | 393 | eLF = 0; |
intgsull | 9:e2feaadf504c | 394 | eRF = 0; |
intgsull | 9:e2feaadf504c | 395 | } |