hello
Dependencies: AVEncoder QEI mbed-src-AV
main.cpp@8:a254346f20aa, 2015-11-18 (annotated)
- Committer:
- intgsull
- Date:
- Wed Nov 18 07:55:13 2015 +0000
- Revision:
- 8:a254346f20aa
- Parent:
- 7:f1bceb2bab70
- Child:
- 9:e2feaadf504c
yay it's fixed
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 | 1:5b9fa1823663 | 103 | |
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 | 6:32d9b855b90f | 130 | leftDistance = rLS; |
intgsull | 6:32d9b855b90f | 131 | rightDistance = rRS; |
intgsull | 6:32d9b855b90f | 132 | irError = rightDistance; //– leftDistance – irOffset; |
intgsull | 6:32d9b855b90f | 133 | irError -= leftDistance; |
intgsull | 6:32d9b855b90f | 134 | irError -= irOffset; |
intgsull | 6:32d9b855b90f | 135 | |
intgsull | 6:32d9b855b90f | 136 | irErrorD = irError; |
intgsull | 6:32d9b855b90f | 137 | irErrorD -= oldirError;//(irError – oldirError); |
intgsull | 6:32d9b855b90f | 138 | |
intgsull | 6:32d9b855b90f | 139 | irErrorI += irError; |
intgsull | 6:32d9b855b90f | 140 | } |
intgsull | 6:32d9b855b90f | 141 | else if(rLS > leftWall) //just left wall |
intgsull | 6:32d9b855b90f | 142 | { |
intgsull | 6:32d9b855b90f | 143 | leftDistance = rLS; |
intgsull | 7:f1bceb2bab70 | 144 | irError = 2*(lirOffset-leftDistance);//(2 * (lirOffset – leftDistance)); |
intgsull | 6:32d9b855b90f | 145 | |
intgsull | 7:f1bceb2bab70 | 146 | irErrorD=irError-oldirError; |
intgsull | 7:f1bceb2bab70 | 147 | |
intgsull | 7:f1bceb2bab70 | 148 | irErrorI += irError; |
intgsull | 6:32d9b855b90f | 149 | } |
intgsull | 6:32d9b855b90f | 150 | else if(rRS > rightWall)//just right wall |
intgsull | 6:32d9b855b90f | 151 | { |
intgsull | 7:f1bceb2bab70 | 152 | rightDistance = rRS; |
intgsull | 8:a254346f20aa | 153 | irError=2*(rightDistance-rirOffset); |
intgsull | 8:a254346f20aa | 154 | irError = irError-oldirError; |
intgsull | 6:32d9b855b90f | 155 | irErrorI += irError; |
intgsull | 6:32d9b855b90f | 156 | } |
intgsull | 6:32d9b855b90f | 157 | else if(rLS < leftWall && rRS < rightWall)//no walls!! Use encoder PID |
intgsull | 6:32d9b855b90f | 158 | { |
intgsull | 6:32d9b855b90f | 159 | irError = 0; |
intgsull | 6:32d9b855b90f | 160 | irErrorD = 0; |
intgsull | 6:32d9b855b90f | 161 | irErrorI += irError; |
intgsull | 6:32d9b855b90f | 162 | } |
intgsull | 6:32d9b855b90f | 163 | totalirError = irError*irKp + irErrorD*irKd + irErrorI*irKi; |
intgsull | 6:32d9b855b90f | 164 | oldirError = irError; |
intgsull | 6:32d9b855b90f | 165 | left_speed -= totalirError; |
intgsull | 6:32d9b855b90f | 166 | right_speed += totalirError; |
intgsull | 6:32d9b855b90f | 167 | eLS = 0; |
intgsull | 6:32d9b855b90f | 168 | eRS = 0; |
intgsull | 4:453d534b1c1d | 169 | } |
intgsull | 4:453d534b1c1d | 170 | |
intgsull | 6:32d9b855b90f | 171 | const float frontWall = 0; //need to calibrate this threshold to a value where mouse can stop in time |
intgsull | 6:32d9b855b90f | 172 | //something like this may be useful for the very temporal present |
intgsull | 6:32d9b855b90f | 173 | //doesn't account for current speed/trajectory atm |
intgsull | 6:32d9b855b90f | 174 | void dontDriveStraightIntoAWall() |
intgsull | 4:453d534b1c1d | 175 | { |
intgsull | 6:32d9b855b90f | 176 | eRF = 1; |
intgsull | 6:32d9b855b90f | 177 | eLF = 1; |
intgsull | 6:32d9b855b90f | 178 | if(rRF > frontWall || rLF > frontWall) |
intgsull | 6:32d9b855b90f | 179 | { |
intgsull | 6:32d9b855b90f | 180 | eRF = 0; |
intgsull | 6:32d9b855b90f | 181 | eLF = 0; |
intgsull | 6:32d9b855b90f | 182 | mouse_state = STOPPED; |
intgsull | 6:32d9b855b90f | 183 | stop(); |
intgsull | 6:32d9b855b90f | 184 | return; |
intgsull | 6:32d9b855b90f | 185 | } |
intgsull | 4:453d534b1c1d | 186 | } |
intgsull | 2:5f9b78950a17 | 187 | |
intgsull | 4:453d534b1c1d | 188 | // PID Control |
intgsull | 4:453d534b1c1d | 189 | // this contains the simplistic PID control for the most part. |
intgsull | 4:453d534b1c1d | 190 | // we do have to calibrate constants though. |
intgsull | 4:453d534b1c1d | 191 | void systick() |
intgsull | 4:453d534b1c1d | 192 | { |
intgsull | 6:32d9b855b90f | 193 | //pc.printf("systick ran\r\n"); |
intgsull | 4:453d534b1c1d | 194 | if ( mouse_state == STOPPED || mouse_state == UNKNOWN ) |
intgsull | 4:453d534b1c1d | 195 | { |
intgsull | 4:453d534b1c1d | 196 | // do nothing? |
intgsull | 4:453d534b1c1d | 197 | // reset? |
intgsull | 6:32d9b855b90f | 198 | //reset(); |
intgsull | 4:453d534b1c1d | 199 | offsetCalc(); |
intgsull | 4:453d534b1c1d | 200 | return; |
intgsull | 2:5f9b78950a17 | 201 | } |
intgsull | 6:32d9b855b90f | 202 | //pc.printf("systick ran while state is FORWARD \r\n"); |
intgsull | 2:5f9b78950a17 | 203 | |
intgsull | 4:453d534b1c1d | 204 | int dt = timer.read_us(); // should be around 1 ms. |
intgsull | 4:453d534b1c1d | 205 | timer.reset(); |
intgsull | 4:453d534b1c1d | 206 | |
intgsull | 6:32d9b855b90f | 207 | //printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); |
intgsull | 6:32d9b855b90f | 208 | |
intgsull | 6:32d9b855b90f | 209 | float line_error = line_speed - 0.5 * ( l_enco.getPulses() + r_enco.getPulses()); |
intgsull | 4:453d534b1c1d | 210 | int enco_error = l_enco.getPulses() - r_enco.getPulses(); |
intgsull | 4:453d534b1c1d | 211 | float gyro_error = _gyro.read() - gyro_offset; |
intgsull | 4:453d534b1c1d | 212 | |
intgsull | 4:453d534b1c1d | 213 | line_accumulator += line_error; |
intgsull | 4:453d534b1c1d | 214 | enco_accumulator += enco_error; |
intgsull | 4:453d534b1c1d | 215 | gyro_accumulator += gyro_error; |
intgsull | 4:453d534b1c1d | 216 | |
intgsull | 4:453d534b1c1d | 217 | float line_pid = 0; |
intgsull | 4:453d534b1c1d | 218 | line_pid += line_propo * line_error; |
intgsull | 4:453d534b1c1d | 219 | line_pid += line_integ * line_accumulator; |
intgsull | 4:453d534b1c1d | 220 | line_pid += line_deriv * (line_error - line_prevError)/dt; |
intgsull | 4:453d534b1c1d | 221 | |
intgsull | 4:453d534b1c1d | 222 | float enco_pid = 0; |
intgsull | 4:453d534b1c1d | 223 | enco_pid += enco_propo * enco_error; |
intgsull | 4:453d534b1c1d | 224 | enco_pid += enco_integ * enco_accumulator; |
intgsull | 4:453d534b1c1d | 225 | enco_pid += enco_deriv * (enco_error - enco_prevError)/dt; |
intgsull | 4:453d534b1c1d | 226 | |
intgsull | 4:453d534b1c1d | 227 | float gyro_pid = 0; |
intgsull | 4:453d534b1c1d | 228 | gyro_pid += gyro_propo * gyro_error; |
intgsull | 4:453d534b1c1d | 229 | gyro_pid += gyro_integ * gyro_accumulator; |
intgsull | 6:32d9b855b90f | 230 | gyro_pid += gyro_deriv *(gyro_error - gyro_prevError)/dt; |
intgsull | 4:453d534b1c1d | 231 | |
intgsull | 4:453d534b1c1d | 232 | // forward moving pid control. |
intgsull | 4:453d534b1c1d | 233 | if ( mouse_state == FORWARD ) |
intgsull | 4:453d534b1c1d | 234 | { |
intgsull | 4:453d534b1c1d | 235 | float x_error = line_pid; |
intgsull | 6:32d9b855b90f | 236 | float w_error = 0; //spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid; |
intgsull | 6:32d9b855b90f | 237 | left_speed += (x_error - w_error); |
intgsull | 6:32d9b855b90f | 238 | right_speed += (x_error + w_error); //swapped |
aduriseti | 5:f9837617817b | 239 | |
intgsull | 6:32d9b855b90f | 240 | //pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); |
aduriseti | 5:f9837617817b | 241 | |
aduriseti | 5:f9837617817b | 242 | moveForward(); |
intgsull | 4:453d534b1c1d | 243 | // offsetCalc(); |
intgsull | 4:453d534b1c1d | 244 | } |
intgsull | 4:453d534b1c1d | 245 | if ( mouse_state == TURNING ) |
intgsull | 4:453d534b1c1d | 246 | { |
intgsull | 4:453d534b1c1d | 247 | // nothing for now. if we turn in place, we assume no pid control. |
intgsull | 4:453d534b1c1d | 248 | // this may have to change when we try curve turns. |
intgsull | 4:453d534b1c1d | 249 | } |
intgsull | 4:453d534b1c1d | 250 | |
intgsull | 4:453d534b1c1d | 251 | line_prevError = line_error; |
intgsull | 4:453d534b1c1d | 252 | enco_prevError = enco_error; |
intgsull | 4:453d534b1c1d | 253 | gyro_prevError = gyro_error; |
intgsull | 4:453d534b1c1d | 254 | |
intgsull | 4:453d534b1c1d | 255 | line_accumulator /= line_decayFactor; |
intgsull | 4:453d534b1c1d | 256 | enco_accumulator /= enco_decayFactor; |
intgsull | 4:453d534b1c1d | 257 | gyro_accumulator /= gyro_decayFactor; |
aduriseti | 5:f9837617817b | 258 | |
aduriseti | 5:f9837617817b | 259 | reset(); |
intgsull | 4:453d534b1c1d | 260 | } |
intgsull | 4:453d534b1c1d | 261 | |
intgsull | 4:453d534b1c1d | 262 | // setup stuff. |
intgsull | 4:453d534b1c1d | 263 | void setup() |
intgsull | 1:5b9fa1823663 | 264 | { |
aduriseti | 5:f9837617817b | 265 | pc.printf("Hello World\r\n"); |
intgsull | 6:32d9b855b90f | 266 | pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); |
intgsull | 6:32d9b855b90f | 267 | pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); |
intgsull | 4:453d534b1c1d | 268 | mouse_state = STOPPED; |
intgsull | 4:453d534b1c1d | 269 | |
intgsull | 6:32d9b855b90f | 270 | reset(); |
intgsull | 6:32d9b855b90f | 271 | |
intgsull | 4:453d534b1c1d | 272 | eRS = 0; |
intgsull | 4:453d534b1c1d | 273 | eRF = 0; |
intgsull | 4:453d534b1c1d | 274 | eLS = 0; |
intgsull | 4:453d534b1c1d | 275 | eLF = 0; |
intgsull | 4:453d534b1c1d | 276 | |
intgsull | 4:453d534b1c1d | 277 | myled = 1; |
intgsull | 1:5b9fa1823663 | 278 | |
intgsull | 4:453d534b1c1d | 279 | // repeat this for some time frame. |
aduriseti | 5:f9837617817b | 280 | for ( int i = 0; i < 200; i++ ) |
aduriseti | 5:f9837617817b | 281 | offsetCalc(); |
intgsull | 4:453d534b1c1d | 282 | Systicker.attach_us(&systick, 1000); |
intgsull | 4:453d534b1c1d | 283 | } |
intgsull | 4:453d534b1c1d | 284 | |
intgsull | 4:453d534b1c1d | 285 | void reset() |
intgsull | 4:453d534b1c1d | 286 | { |
aduriseti | 5:f9837617817b | 287 | l_enco.reset(); |
aduriseti | 5:f9837617817b | 288 | r_enco.reset(); |
intgsull | 1:5b9fa1823663 | 289 | } |
intgsull | 1:5b9fa1823663 | 290 | |
intgsull | 1:5b9fa1823663 | 291 | |
intgsull | 4:453d534b1c1d | 292 | // computes gyro_offset |
intgsull | 4:453d534b1c1d | 293 | // uses a "weighted" average. |
intgsull | 4:453d534b1c1d | 294 | // idea is that the current gyro offset is weighted more than previous ones. |
intgsull | 4:453d534b1c1d | 295 | // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n). |
intgsull | 4:453d534b1c1d | 296 | // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.) |
intgsull | 4:453d534b1c1d | 297 | // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight. |
intgsull | 4:453d534b1c1d | 298 | // currently this is only in the setup function. we can run this when the mouse is running in a line |
intgsull | 4:453d534b1c1d | 299 | // when we figure out good line running pid. |
intgsull | 4:453d534b1c1d | 300 | void offsetCalc() |
intgsull | 4:453d534b1c1d | 301 | { |
intgsull | 4:453d534b1c1d | 302 | gyro_offset = gyro_offset / 2 + _gyro.read() / 2; |
intgsull | 4:453d534b1c1d | 303 | } |
intgsull | 1:5b9fa1823663 | 304 | |
intgsull | 4:453d534b1c1d | 305 | |
intgsull | 0:fa523db3f4f5 | 306 | |
intgsull | 4:453d534b1c1d | 307 | int main() |
intgsull | 4:453d534b1c1d | 308 | { |
intgsull | 4:453d534b1c1d | 309 | setup(); |
intgsull | 6:32d9b855b90f | 310 | pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); |
intgsull | 6:32d9b855b90f | 311 | |
intgsull | 6:32d9b855b90f | 312 | wait(2); |
aduriseti | 5:f9837617817b | 313 | |
intgsull | 6:32d9b855b90f | 314 | mouse_state = FORWARD; |
intgsull | 6:32d9b855b90f | 315 | for ( int i = 0; i < 10; i++ ) |
intgsull | 6:32d9b855b90f | 316 | { |
intgsull | 6:32d9b855b90f | 317 | pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); |
intgsull | 6:32d9b855b90f | 318 | pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); |
intgsull | 6:32d9b855b90f | 319 | } |
aduriseti | 5:f9837617817b | 320 | stop(); |
aduriseti | 5:f9837617817b | 321 | |
intgsull | 6:32d9b855b90f | 322 | //pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); |
aduriseti | 5:f9837617817b | 323 | |
aduriseti | 5:f9837617817b | 324 | //while(1) |
aduriseti | 5:f9837617817b | 325 | // { |
aduriseti | 5:f9837617817b | 326 | // pc.printf("The left motor is going at speed: %d\r\n", left_speed); |
aduriseti | 5:f9837617817b | 327 | // pc.printf("The left motor is going at speed: %d\r\n", right_speed); |
aduriseti | 5:f9837617817b | 328 | // wait(1); |
aduriseti | 5:f9837617817b | 329 | // } |
intgsull | 0:fa523db3f4f5 | 330 | } |
intgsull | 0:fa523db3f4f5 | 331 | |
intgsull | 4:453d534b1c1d | 332 | |
intgsull | 4:453d534b1c1d | 333 | // movement functions. |
intgsull | 4:453d534b1c1d | 334 | void moveForward() |
intgsull | 4:453d534b1c1d | 335 | { |
intgsull | 6:32d9b855b90f | 336 | //mouse_state = FORWARD; |
intgsull | 4:453d534b1c1d | 337 | |
intgsull | 4:453d534b1c1d | 338 | if ( left_speed > 0 ) // which should be always. |
intgsull | 4:453d534b1c1d | 339 | { |
intgsull | 6:32d9b855b90f | 340 | motor1_forward = left_speed / max_speed; |
intgsull | 4:453d534b1c1d | 341 | motor1_reverse = 0; |
intgsull | 4:453d534b1c1d | 342 | } |
intgsull | 4:453d534b1c1d | 343 | else |
intgsull | 4:453d534b1c1d | 344 | { |
intgsull | 4:453d534b1c1d | 345 | motor1_forward = 0; |
intgsull | 6:32d9b855b90f | 346 | motor1_reverse = -left_speed / max_speed; |
intgsull | 0:fa523db3f4f5 | 347 | } |
intgsull | 0:fa523db3f4f5 | 348 | |
intgsull | 4:453d534b1c1d | 349 | if ( right_speed > 0 ) // which again should be always. |
intgsull | 4:453d534b1c1d | 350 | { |
intgsull | 6:32d9b855b90f | 351 | motor2_forward = right_speed / max_speed; |
intgsull | 0:fa523db3f4f5 | 352 | motor2_reverse = 0; |
intgsull | 0:fa523db3f4f5 | 353 | } |
intgsull | 4:453d534b1c1d | 354 | else |
intgsull | 4:453d534b1c1d | 355 | { |
intgsull | 0:fa523db3f4f5 | 356 | motor2_forward = 0; |
intgsull | 6:32d9b855b90f | 357 | motor2_reverse = -right_speed / max_speed; |
intgsull | 0:fa523db3f4f5 | 358 | } |
intgsull | 4:453d534b1c1d | 359 | } |
intgsull | 4:453d534b1c1d | 360 | |
intgsull | 4:453d534b1c1d | 361 | void stop() |
intgsull | 4:453d534b1c1d | 362 | { |
intgsull | 4:453d534b1c1d | 363 | mouse_state = STOPPED; |
intgsull | 4:453d534b1c1d | 364 | |
aduriseti | 5:f9837617817b | 365 | |
intgsull | 4:453d534b1c1d | 366 | motor1_forward = 1.0; |
intgsull | 4:453d534b1c1d | 367 | motor1_reverse = 1.0; |
intgsull | 4:453d534b1c1d | 368 | motor2_forward = 1.0; |
intgsull | 4:453d534b1c1d | 369 | motor2_reverse = 1.0; |
aduriseti | 5:f9837617817b | 370 | |
aduriseti | 5:f9837617817b | 371 | |
intgsull | 4:453d534b1c1d | 372 | } |
intgsull | 4:453d534b1c1d | 373 | |
aduriseti | 5:f9837617817b | 374 | void turn()// maybe split this into two functions? |
aduriseti | 5:f9837617817b | 375 | { |
aduriseti | 5:f9837617817b | 376 | mouse_state = TURNING; |
aduriseti | 5:f9837617817b | 377 | float angle = 0; |
aduriseti | 5:f9837617817b | 378 | while (angle < 0.9) |
aduriseti | 5:f9837617817b | 379 | { |
aduriseti | 5:f9837617817b | 380 | float gyro_val = _gyro.read() - gyro_offset; |
aduriseti | 5:f9837617817b | 381 | angle += gyro_val; |
aduriseti | 5:f9837617817b | 382 | |
aduriseti | 5:f9837617817b | 383 | pc.printf("%f\r\n", angle); |
aduriseti | 5:f9837617817b | 384 | |
aduriseti | 5:f9837617817b | 385 | motor1_forward = 0.5; |
aduriseti | 5:f9837617817b | 386 | motor1_reverse = 0; |
aduriseti | 5:f9837617817b | 387 | motor2_forward = 0; |
aduriseti | 5:f9837617817b | 388 | motor2_reverse = 0.5; |
aduriseti | 5:f9837617817b | 389 | } |
aduriseti | 5:f9837617817b | 390 | } |