2015-2016_Mouserat / Test

PID Test

Dependencies:   AVEncoder mbed-src-AV

main.cpp@5:f704940c9c7e, 2015-12-02 (annotated)

Committer:
aduriseti
Date:
Wed Dec 02 01:53:48 2015 +0000
Revision:
5:f704940c9c7e
Parent:
4:112f3d35bd2d
Child:
6:61b503990cd6
turning added - and logic for navigating from stopped mouse state;

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