2015-2016_Mouserat / Test

PID Test

Dependencies:   AVEncoder mbed-src-AV

main.cpp@7:b866e3aae05f, 2015-12-04 (annotated)

Committer:
jimmery
Date:
Fri Dec 04 02:51:37 2015 +0000
Revision:
7:b866e3aae05f
Parent:
6:61b503990cd6
Child:
8:03e5c3aaa9c9
ir changes;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jimmery 0:13d8a77fb1d7 1 #include "mbed.h"
jimmery 0:13d8a77fb1d7 2 #include "AVEncoder.h"
jimmery 0:13d8a77fb1d7 3
jimmery 0:13d8a77fb1d7 4 // set things
jimmery 0:13d8a77fb1d7 5 Serial pc(SERIAL_TX, SERIAL_RX);
jimmery 0:13d8a77fb1d7 6 Ticker Systicker;
jimmery 0:13d8a77fb1d7 7 Timer timer;
aduriseti 6:61b503990cd6 8 Ticker action_ticker;
aduriseti 6:61b503990cd6 9 Ticker algorithm_ticker;
jimmery 0:13d8a77fb1d7 10
jimmery 0:13d8a77fb1d7 11 PwmOut right_forward(PB_10);
jimmery 0:13d8a77fb1d7 12 PwmOut right_reverse(PA_6);
jimmery 0:13d8a77fb1d7 13 PwmOut left_forward(PA_7);
jimmery 0:13d8a77fb1d7 14 PwmOut left_reverse(PB_6);
jimmery 0:13d8a77fb1d7 15
jimmery 0:13d8a77fb1d7 16 // TODO: change our encoder pins from AnalogIn into:
jimmery 0:13d8a77fb1d7 17 // otherwise, we can also use the AVEncoder thing as well.
jimmery 0:13d8a77fb1d7 18 AVEncoder l_enco(PA_15, PB_3);
jimmery 0:13d8a77fb1d7 19 AVEncoder r_enco(PA_1, PA_10);
jimmery 0:13d8a77fb1d7 20
jimmery 0:13d8a77fb1d7 21 // gyro
jimmery 0:13d8a77fb1d7 22 AnalogIn _gyro(PA_0);
jimmery 0:13d8a77fb1d7 23 // AnalogIn gyro_cal(PC_1) ?? currently this isn't connected.
jimmery 0:13d8a77fb1d7 24
jimmery 0:13d8a77fb1d7 25 //Left Front IR
jimmery 0:13d8a77fb1d7 26 DigitalOut eLF(PC_3);
jimmery 0:13d8a77fb1d7 27 AnalogIn rLF(PC_0);
jimmery 0:13d8a77fb1d7 28 //PC_4 is an ADC
jimmery 0:13d8a77fb1d7 29 //Left Side IR
jimmery 0:13d8a77fb1d7 30 DigitalOut eLS(PC_2);
jimmery 0:13d8a77fb1d7 31 AnalogIn rLS(PC_1);
jimmery 0:13d8a77fb1d7 32
intgsull 3:40333f38771d 33 //Right Side IR
intgsull 3:40333f38771d 34 DigitalOut eRS(PC_12);
intgsull 3:40333f38771d 35 AnalogIn rRS(PA_4);
jimmery 0:13d8a77fb1d7 36
intgsull 3:40333f38771d 37 //Right Front IR
intgsull 3:40333f38771d 38 DigitalOut eRF(PC_15);
intgsull 3:40333f38771d 39 AnalogIn rRF(PB_0);
jimmery 0:13d8a77fb1d7 40
jimmery 0:13d8a77fb1d7 41 DigitalOut myled(LED1);
jimmery 0:13d8a77fb1d7 42
jimmery 0:13d8a77fb1d7 43 volatile float gyro_offset = 0;
jimmery 0:13d8a77fb1d7 44
jimmery 0:13d8a77fb1d7 45 volatile float line_prevError = 0;
jimmery 0:13d8a77fb1d7 46 volatile float enco_prevError = 0;
jimmery 0:13d8a77fb1d7 47 volatile float gyro_prevError = 0;
jimmery 0:13d8a77fb1d7 48
jimmery 0:13d8a77fb1d7 49 volatile float line_accumulator = 0;
jimmery 0:13d8a77fb1d7 50 volatile float line_decayFactor = 1;
jimmery 0:13d8a77fb1d7 51 volatile float enco_accumulator = 0;
intgsull 4:112f3d35bd2d 52 volatile float enco_decayFactor = 1.6;
jimmery 0:13d8a77fb1d7 53 volatile float gyro_accumulator = 0;
intgsull 3:40333f38771d 54 volatile float gyro_decayFactor = 1;
jimmery 0:13d8a77fb1d7 55
intgsull 4:112f3d35bd2d 56 volatile float set_speed = .75;
intgsull 4:112f3d35bd2d 57 volatile float left_speed = .75;
intgsull 4:112f3d35bd2d 58 volatile float right_speed = .75;
jimmery 0:13d8a77fb1d7 59
intgsull 4:112f3d35bd2d 60 const float left_max_speed = 5; // max speed is 6 encoder pulses per ms.
intgsull 4:112f3d35bd2d 61 const float right_max_speed = 5;
jimmery 0:13d8a77fb1d7 62
jimmery 2:82a11e992619 63 const float gyro_propo = 6.5;
jimmery 0:13d8a77fb1d7 64 const float gyro_integ = 0;
jimmery 2:82a11e992619 65 const float gyro_deriv = 10;
jimmery 0:13d8a77fb1d7 66
aduriseti 6:61b503990cd6 67 const float enco_propo = 6;
aduriseti 6:61b503990cd6 68 const float enco_integ = 10;//1;
jimmery 7:b866e3aae05f 69 const float enco_deriv = 1000;//.0002;
jimmery 0:13d8a77fb1d7 70
intgsull 3:40333f38771d 71 const float spin_enco_weight = 1;
jimmery 0:13d8a77fb1d7 72 const float spin_gyro_weight = 1 - spin_enco_weight;
jimmery 0:13d8a77fb1d7 73
jimmery 7:b866e3aae05f 74 const float base_RS = 0.2;
jimmery 7:b866e3aae05f 75 const float base_LS = 0.1;
jimmery 7:b866e3aae05f 76 const float base_RF = 0.002;
jimmery 7:b866e3aae05f 77 const float base_LF = 0.15;
jimmery 7:b866e3aae05f 78
jimmery 7:b866e3aae05f 79 const float thresh_LF = 0.25;
jimmery 7:b866e3aae05f 80 const float thresh_RF = 0.0045; // TODO: CAUTION USING THIS ALONE.
jimmery 7:b866e3aae05f 81 //
jimmery 7:b866e3aae05f 82 //const float
jimmery 7:b866e3aae05f 83
aduriseti 6:61b503990cd6 84 const float frontWall = 0.07; //need to calibrate this threshold to a value where mouse can stop in time
aduriseti 6:61b503990cd6 85 const float open_left = 0.20;
aduriseti 6:61b503990cd6 86 const float open_right = 0.17;
intgsull 1:98efd8dd9077 87 //something like this may be useful
intgsull 1:98efd8dd9077 88
jimmery 0:13d8a77fb1d7 89 volatile float enco_error;
jimmery 0:13d8a77fb1d7 90 volatile float enco_pid;
jimmery 0:13d8a77fb1d7 91 volatile float gyro_error;
jimmery 0:13d8a77fb1d7 92 volatile float gyro_pid;
jimmery 0:13d8a77fb1d7 93 volatile float w_error;
jimmery 0:13d8a77fb1d7 94
intgsull 4:112f3d35bd2d 95 ////////////////////////////////////ir PID constants////////////////////////////////////////////////
intgsull 4:112f3d35bd2d 96 const float leftWall= 0;
intgsull 4:112f3d35bd2d 97 const float rightWall= 0; //we need to find the threshold value for when
intgsull 4:112f3d35bd2d 98 //left or right walls are present
intgsull 4:112f3d35bd2d 99 const float irOffset= 0; // difference between right and left ir sensors when
intgsull 4:112f3d35bd2d 100 //mouse is in the middle
intgsull 4:112f3d35bd2d 101 const float lirOffset= 0; //middle value for when there is only a wall on one
intgsull 4:112f3d35bd2d 102 const float rirOffset= 0; //side of the mouse (left and right)
intgsull 4:112f3d35bd2d 103 volatile float irError = 0;
intgsull 4:112f3d35bd2d 104 volatile float irErrorD = 0;
intgsull 4:112f3d35bd2d 105 volatile float irErrorI = 0;
intgsull 4:112f3d35bd2d 106 volatile float oldirError = 0;
intgsull 4:112f3d35bd2d 107 volatile float totalirError= 0; //errors
intgsull 4:112f3d35bd2d 108 const float irKp = 0;
intgsull 4:112f3d35bd2d 109 const float irKd = 0;
intgsull 4:112f3d35bd2d 110 const float irKi = 0; //constants
intgsull 4:112f3d35bd2d 111 volatile float leftD;
intgsull 4:112f3d35bd2d 112 volatile float rightD;
aduriseti 5:f704940c9c7e 113
intgsull 4:112f3d35bd2d 114 void irPID()
intgsull 4:112f3d35bd2d 115 {
intgsull 4:112f3d35bd2d 116 eLS = 1;
intgsull 4:112f3d35bd2d 117 leftD = rLS.read();
intgsull 4:112f3d35bd2d 118 eLS = 0;
intgsull 4:112f3d35bd2d 119 eRS = 1;
intgsull 4:112f3d35bd2d 120 rightD = rRS.read();
intgsull 4:112f3d35bd2d 121 eRS = 0;
intgsull 4:112f3d35bd2d 122 if(leftD > leftWall && rightD > rightWall)//walls on both sides
intgsull 4:112f3d35bd2d 123 {
intgsull 4:112f3d35bd2d 124 irError = rightD-leftD-irOffset;
intgsull 4:112f3d35bd2d 125
intgsull 4:112f3d35bd2d 126 irErrorD = irError-oldirError;
intgsull 4:112f3d35bd2d 127
intgsull 4:112f3d35bd2d 128 irErrorI += irError;
intgsull 4:112f3d35bd2d 129 }
intgsull 4:112f3d35bd2d 130 else if(leftD > leftWall) //just left wall
intgsull 4:112f3d35bd2d 131 {
intgsull 4:112f3d35bd2d 132
intgsull 4:112f3d35bd2d 133 irError = 2*(lirOffset-leftD);
intgsull 4:112f3d35bd2d 134
intgsull 4:112f3d35bd2d 135 irErrorD=irError-oldirError;
intgsull 4:112f3d35bd2d 136
intgsull 4:112f3d35bd2d 137 irErrorI += irError;
intgsull 4:112f3d35bd2d 138 }
intgsull 4:112f3d35bd2d 139 else if(rightD > rightWall)//just right wall
intgsull 4:112f3d35bd2d 140 {
intgsull 4:112f3d35bd2d 141 irError=2*(rightD-rirOffset);
intgsull 4:112f3d35bd2d 142
intgsull 4:112f3d35bd2d 143 irError = irError-oldirError;
intgsull 4:112f3d35bd2d 144
intgsull 4:112f3d35bd2d 145 irErrorI += irError;
intgsull 4:112f3d35bd2d 146 }
intgsull 4:112f3d35bd2d 147 else if(leftD < leftWall && rightD < rightWall)//no walls!! Use encoder PID
intgsull 4:112f3d35bd2d 148 {
intgsull 4:112f3d35bd2d 149 irError = 0;
intgsull 4:112f3d35bd2d 150 irErrorD = 0;
intgsull 4:112f3d35bd2d 151 irErrorI += irError;
intgsull 4:112f3d35bd2d 152 }
intgsull 4:112f3d35bd2d 153 totalirError = irError*irKp + irErrorD*irKd + irErrorI*irKi;
intgsull 4:112f3d35bd2d 154 oldirError = irError;
intgsull 4:112f3d35bd2d 155 left_speed -= totalirError;
intgsull 4:112f3d35bd2d 156 right_speed += totalirError;
intgsull 4:112f3d35bd2d 157 }
intgsull 4:112f3d35bd2d 158
jimmery 2:82a11e992619 159 // this is just so that we can maintain what state our mouse is in.
jimmery 2:82a11e992619 160 // currently this has no real use, but it may in the future.
jimmery 2:82a11e992619 161 // or we could just remove this entirely.
jimmery 2:82a11e992619 162 typedef enum
jimmery 2:82a11e992619 163 {
jimmery 2:82a11e992619 164 STOPPED,
jimmery 2:82a11e992619 165 FORWARD,
aduriseti 5:f704940c9c7e 166 TURNING,
aduriseti 5:f704940c9c7e 167 FINISHED,
jimmery 2:82a11e992619 168 UNKNOWN
jimmery 2:82a11e992619 169 } STATE;
jimmery 2:82a11e992619 170 volatile STATE mouse_state;
jimmery 2:82a11e992619 171
aduriseti 6:61b503990cd6 172 volatile int wall_in_front = 0;
aduriseti 6:61b503990cd6 173 volatile int opening_left_ahead = 0;
aduriseti 6:61b503990cd6 174 volatile int opening_right_ahead = 0;
aduriseti 6:61b503990cd6 175 volatile int opening_left;
aduriseti 6:61b503990cd6 176 volatile int opening_right;
aduriseti 6:61b503990cd6 177
jimmery 2:82a11e992619 178 void watchOut();
jimmery 2:82a11e992619 179 void offsetCalc();
jimmery 2:82a11e992619 180 void stop();
jimmery 2:82a11e992619 181
aduriseti 5:f704940c9c7e 182 void encoder_reset()
jimmery 0:13d8a77fb1d7 183 {
jimmery 0:13d8a77fb1d7 184 l_enco.reset();
jimmery 0:13d8a77fb1d7 185 r_enco.reset();
jimmery 0:13d8a77fb1d7 186 }
jimmery 0:13d8a77fb1d7 187
aduriseti 6:61b503990cd6 188 void detect_opening() {
aduriseti 6:61b503990cd6 189 eRS = 1;
aduriseti 6:61b503990cd6 190 float right_side = rRS.read();
aduriseti 6:61b503990cd6 191 eRS = 0;
aduriseti 6:61b503990cd6 192 eLS = 1;
aduriseti 6:61b503990cd6 193 float left_side = rLS.read();
aduriseti 6:61b503990cd6 194 eLS = 0;
aduriseti 6:61b503990cd6 195 if (right_side < open_right) {
aduriseti 6:61b503990cd6 196 mouse_state = STOPPED;
aduriseti 6:61b503990cd6 197 opening_right_ahead = 1; //handler for this condition sets back to 0
aduriseti 6:61b503990cd6 198 } else if (left_side < open_left) {
aduriseti 6:61b503990cd6 199 mouse_state = STOPPED;
aduriseti 6:61b503990cd6 200 opening_left_ahead = 1; //handler fot this sets back to 0 like above
aduriseti 6:61b503990cd6 201 }
aduriseti 6:61b503990cd6 202 }
aduriseti 6:61b503990cd6 203
jimmery 0:13d8a77fb1d7 204 void systick()
jimmery 0:13d8a77fb1d7 205 {
aduriseti 5:f704940c9c7e 206 //watchOut();
jimmery 2:82a11e992619 207 if ( mouse_state == STOPPED )
jimmery 2:82a11e992619 208 {
jimmery 2:82a11e992619 209 offsetCalc();
jimmery 2:82a11e992619 210 stop();
aduriseti 6:61b503990cd6 211 encoder_reset(); // maybe problems with this, reconsider
jimmery 2:82a11e992619 212 return;
aduriseti 5:f704940c9c7e 213 } else if (mouse_state == FORWARD) {
aduriseti 5:f704940c9c7e 214 watchOut();
aduriseti 6:61b503990cd6 215 //detect_opening();
aduriseti 5:f704940c9c7e 216
aduriseti 5:f704940c9c7e 217 enco_error = l_enco.getPulses() - r_enco.getPulses();
aduriseti 5:f704940c9c7e 218 gyro_error = _gyro.read() - gyro_offset;
aduriseti 5:f704940c9c7e 219
aduriseti 5:f704940c9c7e 220 enco_accumulator += enco_error;
aduriseti 5:f704940c9c7e 221 gyro_accumulator += gyro_error;
aduriseti 5:f704940c9c7e 222
aduriseti 5:f704940c9c7e 223 enco_pid = 0;
aduriseti 5:f704940c9c7e 224 enco_pid += enco_propo * enco_error;
aduriseti 5:f704940c9c7e 225 enco_pid += enco_integ * enco_accumulator;
aduriseti 5:f704940c9c7e 226 enco_pid += enco_deriv * (enco_error - enco_prevError);
aduriseti 5:f704940c9c7e 227
aduriseti 5:f704940c9c7e 228 gyro_pid = 0;
aduriseti 5:f704940c9c7e 229 gyro_pid += gyro_propo * gyro_error;
aduriseti 5:f704940c9c7e 230 gyro_pid += gyro_integ * gyro_accumulator;
aduriseti 5:f704940c9c7e 231 gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
aduriseti 5:f704940c9c7e 232
aduriseti 5:f704940c9c7e 233 w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
aduriseti 5:f704940c9c7e 234 left_speed = set_speed + w_error;
aduriseti 5:f704940c9c7e 235 right_speed = set_speed - w_error;
aduriseti 5:f704940c9c7e 236
aduriseti 5:f704940c9c7e 237 left_forward = left_speed / left_max_speed;
aduriseti 5:f704940c9c7e 238 left_reverse = 0;
aduriseti 5:f704940c9c7e 239 right_forward = right_speed / right_max_speed;
aduriseti 5:f704940c9c7e 240 right_reverse = 0;
aduriseti 5:f704940c9c7e 241
aduriseti 5:f704940c9c7e 242 enco_prevError = enco_error;
aduriseti 5:f704940c9c7e 243 gyro_prevError = gyro_error;
aduriseti 5:f704940c9c7e 244
aduriseti 5:f704940c9c7e 245 enco_accumulator += enco_error;
aduriseti 5:f704940c9c7e 246 gyro_accumulator += gyro_error;
aduriseti 5:f704940c9c7e 247
aduriseti 5:f704940c9c7e 248 enco_accumulator /= enco_decayFactor;
aduriseti 5:f704940c9c7e 249 gyro_accumulator /= gyro_decayFactor;
aduriseti 5:f704940c9c7e 250
aduriseti 5:f704940c9c7e 251 //reset();
jimmery 2:82a11e992619 252 }
jimmery 0:13d8a77fb1d7 253 }
jimmery 0:13d8a77fb1d7 254
jimmery 0:13d8a77fb1d7 255 // computes gyro_offset
jimmery 0:13d8a77fb1d7 256 // uses a "weighted" average.
jimmery 0:13d8a77fb1d7 257 // idea is that the current gyro offset is weighted more than previous ones.
jimmery 0:13d8a77fb1d7 258 // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n).
jimmery 0:13d8a77fb1d7 259 // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
jimmery 0:13d8a77fb1d7 260 // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
jimmery 0:13d8a77fb1d7 261 // currently this is only in the setup function. we can run this when the mouse is running in a line
jimmery 0:13d8a77fb1d7 262 // when we figure out good line running pid.
jimmery 0:13d8a77fb1d7 263 void offsetCalc()
jimmery 0:13d8a77fb1d7 264 {
jimmery 0:13d8a77fb1d7 265 gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
jimmery 0:13d8a77fb1d7 266 }
jimmery 0:13d8a77fb1d7 267
jimmery 0:13d8a77fb1d7 268
intgsull 1:98efd8dd9077 269 void stop()
intgsull 1:98efd8dd9077 270 {
intgsull 1:98efd8dd9077 271 left_forward = 1;
intgsull 1:98efd8dd9077 272 left_reverse = 1;
intgsull 1:98efd8dd9077 273 right_forward = 1;
intgsull 1:98efd8dd9077 274 right_reverse = 1;
jimmery 2:82a11e992619 275 }
aduriseti 6:61b503990cd6 276
aduriseti 6:61b503990cd6 277 typedef enum
aduriseti 6:61b503990cd6 278 {
aduriseti 6:61b503990cd6 279 LEFT,
aduriseti 6:61b503990cd6 280 RIGHT
aduriseti 6:61b503990cd6 281 } TURN_DIRECTION;
aduriseti 6:61b503990cd6 282 volatile TURN_DIRECTION turn_direction;
aduriseti 6:61b503990cd6 283
aduriseti 6:61b503990cd6 284 volatile int turn_counter;
aduriseti 6:61b503990cd6 285 volatile int right_turn_count;
aduriseti 6:61b503990cd6 286 volatile int examined_left;
aduriseti 6:61b503990cd6 287 volatile int examined_right;
aduriseti 6:61b503990cd6 288
aduriseti 6:61b503990cd6 289 //time to make a 90 degree turn and move forward one cell length, repectively
aduriseti 6:61b503990cd6 290 //should be replaced with encoder counts
aduriseti 6:61b503990cd6 291 volatile int max_turn_count = 800;
aduriseti 6:61b503990cd6 292 volatile int cell_length_count = 800;
intgsull 1:98efd8dd9077 293
intgsull 1:98efd8dd9077 294 void watchOut()
intgsull 1:98efd8dd9077 295 {
aduriseti 5:f704940c9c7e 296 //if (mouse_state == FORWARD) {
aduriseti 5:f704940c9c7e 297 eRF = 1;
aduriseti 5:f704940c9c7e 298 float right = rRF.read();
aduriseti 5:f704940c9c7e 299 eRF = 0;
aduriseti 5:f704940c9c7e 300 eLF = 1;
aduriseti 5:f704940c9c7e 301 float left = rLF.read();
aduriseti 5:f704940c9c7e 302 eLF = 0;
jimmery 7:b866e3aae05f 303 if(left > thresh_LF || right > thresh_RF )
aduriseti 5:f704940c9c7e 304 {
aduriseti 5:f704940c9c7e 305 mouse_state = STOPPED;
aduriseti 6:61b503990cd6 306 wall_in_front = 1; //handler for this condition sets back to zero
aduriseti 6:61b503990cd6 307
aduriseti 6:61b503990cd6 308 encoder_reset();
aduriseti 6:61b503990cd6 309 mouse_state = TURNING;
aduriseti 6:61b503990cd6 310 turn_direction = LEFT;
aduriseti 6:61b503990cd6 311 }
aduriseti 5:f704940c9c7e 312 }
aduriseti 5:f704940c9c7e 313
aduriseti 5:f704940c9c7e 314
aduriseti 5:f704940c9c7e 315 void turn() {
aduriseti 5:f704940c9c7e 316 if (mouse_state == TURNING) {
aduriseti 5:f704940c9c7e 317 if (turn_counter < max_turn_count) {
aduriseti 5:f704940c9c7e 318 int left_pulses = l_enco.getPulses();
aduriseti 5:f704940c9c7e 319 int right_pulses = r_enco.getPulses();
aduriseti 5:f704940c9c7e 320 turn_counter = (left_pulses + right_pulses)/2;
aduriseti 6:61b503990cd6 321
aduriseti 6:61b503990cd6 322 //maybe eventually replace this with PID control -> systicker
aduriseti 5:f704940c9c7e 323 if (turn_direction == LEFT) {
aduriseti 6:61b503990cd6 324 left_forward = 0;
aduriseti 6:61b503990cd6 325 left_reverse = set_speed / left_max_speed;
aduriseti 6:61b503990cd6 326 right_forward = set_speed / right_max_speed;
aduriseti 6:61b503990cd6 327 right_reverse = 0;
aduriseti 5:f704940c9c7e 328 } else {
aduriseti 6:61b503990cd6 329 left_forward = set_speed / left_max_speed;
aduriseti 6:61b503990cd6 330 left_reverse = 0;
aduriseti 6:61b503990cd6 331 right_forward = 0;
aduriseti 6:61b503990cd6 332 right_reverse = set_speed / right_max_speed;
aduriseti 6:61b503990cd6 333 }
aduriseti 6:61b503990cd6 334 } else {
aduriseti 6:61b503990cd6 335
aduriseti 6:61b503990cd6 336 stop();
aduriseti 6:61b503990cd6 337 turn_counter = 0;
aduriseti 5:f704940c9c7e 338 encoder_reset();
aduriseti 5:f704940c9c7e 339 mouse_state = FORWARD;
aduriseti 5:f704940c9c7e 340 }
aduriseti 5:f704940c9c7e 341 }
aduriseti 5:f704940c9c7e 342 }
aduriseti 6:61b503990cd6 343
aduriseti 6:61b503990cd6 344 volatile int forward_counter;
aduriseti 6:61b503990cd6 345
aduriseti 6:61b503990cd6 346 void move_cell() {
aduriseti 6:61b503990cd6 347 mouse_state = FORWARD;
aduriseti 6:61b503990cd6 348 forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
aduriseti 6:61b503990cd6 349 if (forward_counter > 2000) {
aduriseti 6:61b503990cd6 350 action_ticker.detach();
aduriseti 6:61b503990cd6 351 forward_counter = 0;
aduriseti 6:61b503990cd6 352 //encoder_reset();
aduriseti 6:61b503990cd6 353 mouse_state = STOPPED;
aduriseti 6:61b503990cd6 354 if (opening_left_ahead) { //takes care of case where opening on both sides - defaults to left turn
aduriseti 6:61b503990cd6 355 opening_left_ahead = 0;
aduriseti 6:61b503990cd6 356 opening_left = 1;
aduriseti 6:61b503990cd6 357 } else if (opening_right_ahead) {
aduriseti 6:61b503990cd6 358 opening_right_ahead = 0;
aduriseti 6:61b503990cd6 359 opening_right = 1;
aduriseti 6:61b503990cd6 360 } else {
aduriseti 6:61b503990cd6 361 //we fucked up
aduriseti 6:61b503990cd6 362 }
aduriseti 6:61b503990cd6 363 }
aduriseti 6:61b503990cd6 364 }
aduriseti 5:f704940c9c7e 365
aduriseti 6:61b503990cd6 366
aduriseti 6:61b503990cd6 367 void algorithm() {
aduriseti 5:f704940c9c7e 368 if (mouse_state == STOPPED) {
aduriseti 5:f704940c9c7e 369 encoder_reset();
aduriseti 6:61b503990cd6 370 mouse_state = TURNING;
aduriseti 6:61b503990cd6 371 turn_direction = LEFT;
aduriseti 6:61b503990cd6 372 //action_ticker.attach_us(&turn, 1000);
aduriseti 6:61b503990cd6 373
aduriseti 6:61b503990cd6 374 /*if (opening_left_ahead || opening_right_ahead) {
aduriseti 6:61b503990cd6 375 action_ticker.attach_us(&move_cell, 1000);
aduriseti 6:61b503990cd6 376 }
aduriseti 6:61b503990cd6 377 if (opening_left) {
aduriseti 5:f704940c9c7e 378 mouse_state = TURNING;
aduriseti 6:61b503990cd6 379 turn_direction = LEFT;
aduriseti 6:61b503990cd6 380 action_ticker.attach_us(&turn, 1000);
aduriseti 5:f704940c9c7e 381 }
aduriseti 6:61b503990cd6 382 if (opening_right) {
aduriseti 6:61b503990cd6 383 mouse_state = TURNING;
aduriseti 6:61b503990cd6 384 turn_direction = RIGHT;
aduriseti 6:61b503990cd6 385 action_ticker.attach_us(&turn, 1000);
aduriseti 6:61b503990cd6 386 }*/
intgsull 1:98efd8dd9077 387 }
intgsull 1:98efd8dd9077 388 }
intgsull 1:98efd8dd9077 389
intgsull 4:112f3d35bd2d 390
jimmery 0:13d8a77fb1d7 391 void setup()
jimmery 0:13d8a77fb1d7 392 {
jimmery 0:13d8a77fb1d7 393 pc.printf("Hello World\r\n");
jimmery 0:13d8a77fb1d7 394
jimmery 0:13d8a77fb1d7 395 eRS = 0;
jimmery 0:13d8a77fb1d7 396 eRF = 0;
jimmery 0:13d8a77fb1d7 397 eLS = 0;
jimmery 0:13d8a77fb1d7 398 eLF = 0;
jimmery 0:13d8a77fb1d7 399
aduriseti 6:61b503990cd6 400
aduriseti 6:61b503990cd6 401
jimmery 2:82a11e992619 402 mouse_state = FORWARD;
jimmery 2:82a11e992619 403
jimmery 0:13d8a77fb1d7 404 myled = 1;
jimmery 0:13d8a77fb1d7 405
jimmery 0:13d8a77fb1d7 406 for ( int i = 0; i < 1000; i++ )
jimmery 0:13d8a77fb1d7 407 {
jimmery 0:13d8a77fb1d7 408 offsetCalc();
jimmery 0:13d8a77fb1d7 409 }
jimmery 0:13d8a77fb1d7 410
jimmery 0:13d8a77fb1d7 411 //left_forward = left_speed / left_max_speed;
jimmery 0:13d8a77fb1d7 412 // left_reverse = 0;
jimmery 0:13d8a77fb1d7 413 // right_forward = right_speed / right_max_speed;
jimmery 0:13d8a77fb1d7 414 // right_reverse = 0;
jimmery 0:13d8a77fb1d7 415
jimmery 0:13d8a77fb1d7 416 wait(2);
jimmery 0:13d8a77fb1d7 417
jimmery 0:13d8a77fb1d7 418 // repeat this for some time frame.
aduriseti 6:61b503990cd6 419
jimmery 0:13d8a77fb1d7 420 Systicker.attach_us(&systick, 1000);
aduriseti 6:61b503990cd6 421
aduriseti 6:61b503990cd6 422 //encoder_reset();
aduriseti 6:61b503990cd6 423 //mouse_state = TURNING;
aduriseti 6:61b503990cd6 424 //turn_direction = LEFT;
aduriseti 6:61b503990cd6 425 action_ticker.attach_us(&turn, 1000);
aduriseti 6:61b503990cd6 426
aduriseti 5:f704940c9c7e 427 //ir_ticker.attach_us(&irPID, 100000);
aduriseti 6:61b503990cd6 428
aduriseti 6:61b503990cd6 429 //algorithm_ticker.attach_us(&algorithm, 1000);
aduriseti 6:61b503990cd6 430
aduriseti 6:61b503990cd6 431
aduriseti 6:61b503990cd6 432
aduriseti 6:61b503990cd6 433
jimmery 0:13d8a77fb1d7 434 }
jimmery 0:13d8a77fb1d7 435
jimmery 0:13d8a77fb1d7 436
jimmery 0:13d8a77fb1d7 437 int main() {
jimmery 0:13d8a77fb1d7 438 setup();
intgsull 3:40333f38771d 439
intgsull 3:40333f38771d 440
jimmery 0:13d8a77fb1d7 441 while(1) {
intgsull 3:40333f38771d 442 //
intgsull 3:40333f38771d 443 // pc.printf("left pulss %f, right pulses %f \r\n", l_enco.getPulses(), r_enco.getPulses());
intgsull 3:40333f38771d 444 // //wait(1);
jimmery 7:b866e3aae05f 445 //wait(1);
jimmery 7:b866e3aae05f 446 /*eRS = 1;
jimmery 7:b866e3aae05f 447 wait_us(50);
aduriseti 6:61b503990cd6 448 float right_side = rRS.read();
jimmery 7:b866e3aae05f 449 eRS = 0;
jimmery 7:b866e3aae05f 450 eLS = 1;
jimmery 7:b866e3aae05f 451 wait_us(50);
aduriseti 6:61b503990cd6 452 float left_side = rLS.read();
aduriseti 6:61b503990cd6 453 eLS = 0;
jimmery 7:b866e3aae05f 454 eRF = 1;
jimmery 7:b866e3aae05f 455 wait_us(50);
jimmery 7:b866e3aae05f 456 float right_front = rRF.read();
jimmery 7:b866e3aae05f 457 eRF = 0;
jimmery 7:b866e3aae05f 458 eLF = 1;
jimmery 7:b866e3aae05f 459 wait_us(50);
jimmery 7:b866e3aae05f 460 float left_front = rLF.read();
jimmery 7:b866e3aae05f 461 eLF = 0;
jimmery 7:b866e3aae05f 462 pc.printf("right side: %f, left side: %f right_front: %f, left_front: %f \r\n", right_side, left_side, right_front, left_front);
jimmery 7:b866e3aae05f 463 wait(1);*/
intgsull 3:40333f38771d 464 }
intgsull 3:40333f38771d 465 // pc.printf("left pulses %d, right pulses %d \r\n", l_enco.getPulses(), r_enco.getPulses()); //encoder testing
jimmery 0:13d8a77fb1d7 466 //wait(1);
jimmery 0:13d8a77fb1d7 467 }