2015-2016_Mouserat / Test

PID Test

Dependencies:   AVEncoder mbed-src-AV

main.cpp@8:03e5c3aaa9c9, 2015-12-04 (annotated)

Committer:
intgsull
Date:
Fri Dec 04 04:21:55 2015 +0000
Revision:
8:03e5c3aaa9c9
Parent:
7:b866e3aae05f
Child:
9:ad6f60953086
Consolidated to one ticker, doesn't work as expected mouse is detecting walls but is doing a lot of left/right turns, it might be good to enumerate the right turn case instead of leaving it as an else statement without a TURN_STATE thing.

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.
intgsull 8:03e5c3aaa9c9 81
jimmery 7:b866e3aae05f 82
intgsull 8:03e5c3aaa9c9 83 const float open_left = 0.22; //calibrate these two
intgsull 8:03e5c3aaa9c9 84 const float open_right = 0.18;
intgsull 8:03e5c3aaa9c9 85 const float wall_left = 0;
intgsull 8:03e5c3aaa9c9 86 const float wall_right = 0;
intgsull 8:03e5c3aaa9c9 87
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 8:03e5c3aaa9c9 95 typedef enum
intgsull 8:03e5c3aaa9c9 96 {
intgsull 8:03e5c3aaa9c9 97 LEFT,
intgsull 8:03e5c3aaa9c9 98 RIGHT
intgsull 8:03e5c3aaa9c9 99 } TURN_DIRECTION;
intgsull 8:03e5c3aaa9c9 100 volatile TURN_DIRECTION turn_direction;
aduriseti 5:f704940c9c7e 101
intgsull 8:03e5c3aaa9c9 102 volatile int turn_counter;
intgsull 8:03e5c3aaa9c9 103 volatile int right_turn_count;
intgsull 8:03e5c3aaa9c9 104 volatile int examined_left;
intgsull 8:03e5c3aaa9c9 105 volatile int examined_right;
intgsull 8:03e5c3aaa9c9 106
intgsull 8:03e5c3aaa9c9 107 //time to make a 90 degree turn and move forward one cell length, repectively
intgsull 8:03e5c3aaa9c9 108 //should be replaced with encoder counts
intgsull 8:03e5c3aaa9c9 109 volatile int max_turn_count = 800;
intgsull 8:03e5c3aaa9c9 110 volatile int cell_length_count = 800;
intgsull 4:112f3d35bd2d 111
intgsull 8:03e5c3aaa9c9 112 volatile int forward_counter = 0;
intgsull 4:112f3d35bd2d 113
jimmery 2:82a11e992619 114 // this is just so that we can maintain what state our mouse is in.
jimmery 2:82a11e992619 115 // currently this has no real use, but it may in the future.
jimmery 2:82a11e992619 116 // or we could just remove this entirely.
jimmery 2:82a11e992619 117 typedef enum
jimmery 2:82a11e992619 118 {
jimmery 2:82a11e992619 119 STOPPED,
jimmery 2:82a11e992619 120 FORWARD,
aduriseti 5:f704940c9c7e 121 TURNING,
intgsull 8:03e5c3aaa9c9 122 MOVECELL,
aduriseti 5:f704940c9c7e 123 FINISHED,
jimmery 2:82a11e992619 124 UNKNOWN
jimmery 2:82a11e992619 125 } STATE;
jimmery 2:82a11e992619 126 volatile STATE mouse_state;
jimmery 2:82a11e992619 127
aduriseti 6:61b503990cd6 128 volatile int wall_in_front = 0;
aduriseti 6:61b503990cd6 129 volatile int opening_left_ahead = 0;
aduriseti 6:61b503990cd6 130 volatile int opening_right_ahead = 0;
intgsull 8:03e5c3aaa9c9 131 volatile int opening_left = 0;
intgsull 8:03e5c3aaa9c9 132 volatile int opening_right = 0;
aduriseti 6:61b503990cd6 133
intgsull 8:03e5c3aaa9c9 134 void Scan();
jimmery 2:82a11e992619 135 void offsetCalc();
jimmery 2:82a11e992619 136 void stop();
jimmery 2:82a11e992619 137
aduriseti 5:f704940c9c7e 138 void encoder_reset()
jimmery 0:13d8a77fb1d7 139 {
jimmery 0:13d8a77fb1d7 140 l_enco.reset();
jimmery 0:13d8a77fb1d7 141 r_enco.reset();
jimmery 0:13d8a77fb1d7 142 }
jimmery 0:13d8a77fb1d7 143
intgsull 8:03e5c3aaa9c9 144 //combining detect opening and watch out
intgsull 8:03e5c3aaa9c9 145 void Scan()
intgsull 8:03e5c3aaa9c9 146 {
intgsull 8:03e5c3aaa9c9 147 eRS = 1;
intgsull 8:03e5c3aaa9c9 148 wait_us(50);
aduriseti 6:61b503990cd6 149 float right_side = rRS.read();
aduriseti 6:61b503990cd6 150 eRS = 0;
intgsull 8:03e5c3aaa9c9 151
aduriseti 6:61b503990cd6 152 eLS = 1;
intgsull 8:03e5c3aaa9c9 153 wait_us(50);
aduriseti 6:61b503990cd6 154 float left_side = rLS.read();
aduriseti 6:61b503990cd6 155 eLS = 0;
intgsull 8:03e5c3aaa9c9 156 //eRF = 1;
intgsull 8:03e5c3aaa9c9 157 // wait_us(50);
intgsull 8:03e5c3aaa9c9 158 // float right_front = rRF.read(); //right front not working so screw it
intgsull 8:03e5c3aaa9c9 159 // eRF = 0;
intgsull 8:03e5c3aaa9c9 160 eLF = 1;
intgsull 8:03e5c3aaa9c9 161 wait_us(50);
intgsull 8:03e5c3aaa9c9 162 float left_front = rLF.read();
intgsull 8:03e5c3aaa9c9 163 eLF = 0;
intgsull 8:03e5c3aaa9c9 164
aduriseti 6:61b503990cd6 165 if (right_side < open_right) {
intgsull 8:03e5c3aaa9c9 166 mouse_state = MOVECELL;
intgsull 8:03e5c3aaa9c9 167 opening_right_ahead = 1;
aduriseti 6:61b503990cd6 168 } else if (left_side < open_left) {
intgsull 8:03e5c3aaa9c9 169 mouse_state = MOVECELL;
intgsull 8:03e5c3aaa9c9 170 opening_left_ahead = 1;
aduriseti 6:61b503990cd6 171 }
intgsull 8:03e5c3aaa9c9 172
intgsull 8:03e5c3aaa9c9 173 if(left_front > thresh_LF )
intgsull 8:03e5c3aaa9c9 174 {
intgsull 8:03e5c3aaa9c9 175 mouse_state = STOPPED; // does this actually do anything tho
intgsull 8:03e5c3aaa9c9 176 wall_in_front = 1; //I don't think we're using this and it's not getting reset?
intgsull 8:03e5c3aaa9c9 177
intgsull 8:03e5c3aaa9c9 178 encoder_reset();
intgsull 8:03e5c3aaa9c9 179 mouse_state = TURNING;
intgsull 8:03e5c3aaa9c9 180 turn_direction = LEFT;
intgsull 8:03e5c3aaa9c9 181 }
intgsull 8:03e5c3aaa9c9 182 }
intgsull 8:03e5c3aaa9c9 183
aduriseti 6:61b503990cd6 184
jimmery 0:13d8a77fb1d7 185 void systick()
jimmery 0:13d8a77fb1d7 186 {
jimmery 2:82a11e992619 187 if ( mouse_state == STOPPED )
jimmery 2:82a11e992619 188 {
jimmery 2:82a11e992619 189 offsetCalc();
jimmery 2:82a11e992619 190 stop();
aduriseti 6:61b503990cd6 191 encoder_reset(); // maybe problems with this, reconsider
jimmery 2:82a11e992619 192 return;
aduriseti 5:f704940c9c7e 193 } else if (mouse_state == FORWARD) {
intgsull 8:03e5c3aaa9c9 194 Scan();
intgsull 8:03e5c3aaa9c9 195
aduriseti 5:f704940c9c7e 196
aduriseti 5:f704940c9c7e 197 enco_error = l_enco.getPulses() - r_enco.getPulses();
aduriseti 5:f704940c9c7e 198 gyro_error = _gyro.read() - gyro_offset;
aduriseti 5:f704940c9c7e 199
aduriseti 5:f704940c9c7e 200 enco_accumulator += enco_error;
aduriseti 5:f704940c9c7e 201 gyro_accumulator += gyro_error;
aduriseti 5:f704940c9c7e 202
aduriseti 5:f704940c9c7e 203 enco_pid = 0;
aduriseti 5:f704940c9c7e 204 enco_pid += enco_propo * enco_error;
aduriseti 5:f704940c9c7e 205 enco_pid += enco_integ * enco_accumulator;
aduriseti 5:f704940c9c7e 206 enco_pid += enco_deriv * (enco_error - enco_prevError);
aduriseti 5:f704940c9c7e 207
aduriseti 5:f704940c9c7e 208 gyro_pid = 0;
aduriseti 5:f704940c9c7e 209 gyro_pid += gyro_propo * gyro_error;
aduriseti 5:f704940c9c7e 210 gyro_pid += gyro_integ * gyro_accumulator;
aduriseti 5:f704940c9c7e 211 gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
aduriseti 5:f704940c9c7e 212
aduriseti 5:f704940c9c7e 213 w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
aduriseti 5:f704940c9c7e 214 left_speed = set_speed + w_error;
aduriseti 5:f704940c9c7e 215 right_speed = set_speed - w_error;
aduriseti 5:f704940c9c7e 216
aduriseti 5:f704940c9c7e 217 left_forward = left_speed / left_max_speed;
aduriseti 5:f704940c9c7e 218 left_reverse = 0;
aduriseti 5:f704940c9c7e 219 right_forward = right_speed / right_max_speed;
aduriseti 5:f704940c9c7e 220 right_reverse = 0;
aduriseti 5:f704940c9c7e 221
aduriseti 5:f704940c9c7e 222 enco_prevError = enco_error;
aduriseti 5:f704940c9c7e 223 gyro_prevError = gyro_error;
aduriseti 5:f704940c9c7e 224
aduriseti 5:f704940c9c7e 225 enco_accumulator += enco_error;
aduriseti 5:f704940c9c7e 226 gyro_accumulator += gyro_error;
aduriseti 5:f704940c9c7e 227
aduriseti 5:f704940c9c7e 228 enco_accumulator /= enco_decayFactor;
aduriseti 5:f704940c9c7e 229 gyro_accumulator /= gyro_decayFactor;
aduriseti 5:f704940c9c7e 230
intgsull 8:03e5c3aaa9c9 231
intgsull 8:03e5c3aaa9c9 232 }
intgsull 8:03e5c3aaa9c9 233 else if (mouse_state == TURNING) {
intgsull 8:03e5c3aaa9c9 234 if (turn_counter < max_turn_count) {
intgsull 8:03e5c3aaa9c9 235 int left_pulses = l_enco.getPulses();
intgsull 8:03e5c3aaa9c9 236 int right_pulses = r_enco.getPulses();
intgsull 8:03e5c3aaa9c9 237 turn_counter = (left_pulses + right_pulses)/2;
intgsull 8:03e5c3aaa9c9 238
intgsull 8:03e5c3aaa9c9 239 //maybe eventually replace this with PID control -> systicker
intgsull 8:03e5c3aaa9c9 240 if (turn_direction == LEFT) {
intgsull 8:03e5c3aaa9c9 241 left_forward = 0;
intgsull 8:03e5c3aaa9c9 242 left_reverse = set_speed / left_max_speed;
intgsull 8:03e5c3aaa9c9 243 right_forward = set_speed / right_max_speed;
intgsull 8:03e5c3aaa9c9 244 right_reverse = 0;
intgsull 8:03e5c3aaa9c9 245 } else {
intgsull 8:03e5c3aaa9c9 246 left_forward = set_speed / left_max_speed;
intgsull 8:03e5c3aaa9c9 247 left_reverse = 0;
intgsull 8:03e5c3aaa9c9 248 right_forward = 0;
intgsull 8:03e5c3aaa9c9 249 right_reverse = set_speed / right_max_speed;
intgsull 8:03e5c3aaa9c9 250 }
intgsull 8:03e5c3aaa9c9 251 } else {
intgsull 8:03e5c3aaa9c9 252
intgsull 8:03e5c3aaa9c9 253 stop();
intgsull 8:03e5c3aaa9c9 254 turn_counter = 0;
intgsull 8:03e5c3aaa9c9 255 encoder_reset();
intgsull 8:03e5c3aaa9c9 256 mouse_state = FORWARD;
intgsull 8:03e5c3aaa9c9 257
intgsull 8:03e5c3aaa9c9 258 }
intgsull 8:03e5c3aaa9c9 259 }
intgsull 8:03e5c3aaa9c9 260 else if (mouse_state == MOVECELL)
intgsull 8:03e5c3aaa9c9 261 {
intgsull 8:03e5c3aaa9c9 262 forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
intgsull 8:03e5c3aaa9c9 263 if (forward_counter > 2000)
intgsull 8:03e5c3aaa9c9 264 {
intgsull 8:03e5c3aaa9c9 265 forward_counter = 0;
intgsull 8:03e5c3aaa9c9 266 mouse_state = TURNING;
intgsull 8:03e5c3aaa9c9 267 if (opening_left_ahead)
intgsull 8:03e5c3aaa9c9 268 {
intgsull 8:03e5c3aaa9c9 269 opening_left_ahead = 0;
intgsull 8:03e5c3aaa9c9 270 turn_direction = LEFT;
intgsull 8:03e5c3aaa9c9 271 }
intgsull 8:03e5c3aaa9c9 272 else if (opening_right_ahead)
intgsull 8:03e5c3aaa9c9 273 {
intgsull 8:03e5c3aaa9c9 274 opening_right_ahead = 0;
intgsull 8:03e5c3aaa9c9 275 }
intgsull 8:03e5c3aaa9c9 276 encoder_reset(); //right? prepare for turn
intgsull 8:03e5c3aaa9c9 277 }
jimmery 2:82a11e992619 278 }
jimmery 0:13d8a77fb1d7 279 }
jimmery 0:13d8a77fb1d7 280
jimmery 0:13d8a77fb1d7 281 // computes gyro_offset
jimmery 0:13d8a77fb1d7 282 // uses a "weighted" average.
jimmery 0:13d8a77fb1d7 283 // idea is that the current gyro offset is weighted more than previous ones.
jimmery 0:13d8a77fb1d7 284 // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n).
jimmery 0:13d8a77fb1d7 285 // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
jimmery 0:13d8a77fb1d7 286 // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
jimmery 0:13d8a77fb1d7 287 // currently this is only in the setup function. we can run this when the mouse is running in a line
jimmery 0:13d8a77fb1d7 288 // when we figure out good line running pid.
jimmery 0:13d8a77fb1d7 289 void offsetCalc()
jimmery 0:13d8a77fb1d7 290 {
jimmery 0:13d8a77fb1d7 291 gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
jimmery 0:13d8a77fb1d7 292 }
jimmery 0:13d8a77fb1d7 293
jimmery 0:13d8a77fb1d7 294
intgsull 1:98efd8dd9077 295 void stop()
intgsull 1:98efd8dd9077 296 {
intgsull 1:98efd8dd9077 297 left_forward = 1;
intgsull 1:98efd8dd9077 298 left_reverse = 1;
intgsull 1:98efd8dd9077 299 right_forward = 1;
intgsull 1:98efd8dd9077 300 right_reverse = 1;
jimmery 2:82a11e992619 301 }
aduriseti 6:61b503990cd6 302
aduriseti 6:61b503990cd6 303
aduriseti 5:f704940c9c7e 304
aduriseti 5:f704940c9c7e 305
aduriseti 5:f704940c9c7e 306 void turn() {
intgsull 8:03e5c3aaa9c9 307 //closed for business moved to systick
aduriseti 5:f704940c9c7e 308 }
aduriseti 6:61b503990cd6 309
intgsull 8:03e5c3aaa9c9 310
aduriseti 6:61b503990cd6 311
aduriseti 6:61b503990cd6 312 void move_cell() {
intgsull 8:03e5c3aaa9c9 313 //out of business moved to algorithm
intgsull 8:03e5c3aaa9c9 314
aduriseti 6:61b503990cd6 315 }
aduriseti 5:f704940c9c7e 316
aduriseti 6:61b503990cd6 317
intgsull 8:03e5c3aaa9c9 318 void algorithm() { //moved to systick
intgsull 8:03e5c3aaa9c9 319
intgsull 8:03e5c3aaa9c9 320
intgsull 8:03e5c3aaa9c9 321
intgsull 8:03e5c3aaa9c9 322 //encoder_reset();
intgsull 8:03e5c3aaa9c9 323 // mouse_state = TURNING;
intgsull 8:03e5c3aaa9c9 324 // turn_direction = LEFT;
aduriseti 6:61b503990cd6 325 //action_ticker.attach_us(&turn, 1000);
aduriseti 6:61b503990cd6 326
aduriseti 6:61b503990cd6 327 /*if (opening_left_ahead || opening_right_ahead) {
aduriseti 6:61b503990cd6 328 action_ticker.attach_us(&move_cell, 1000);
aduriseti 6:61b503990cd6 329 }
aduriseti 6:61b503990cd6 330 if (opening_left) {
aduriseti 5:f704940c9c7e 331 mouse_state = TURNING;
aduriseti 6:61b503990cd6 332 turn_direction = LEFT;
aduriseti 6:61b503990cd6 333 action_ticker.attach_us(&turn, 1000);
aduriseti 5:f704940c9c7e 334 }
aduriseti 6:61b503990cd6 335 if (opening_right) {
aduriseti 6:61b503990cd6 336 mouse_state = TURNING;
aduriseti 6:61b503990cd6 337 turn_direction = RIGHT;
aduriseti 6:61b503990cd6 338 action_ticker.attach_us(&turn, 1000);
aduriseti 6:61b503990cd6 339 }*/
intgsull 8:03e5c3aaa9c9 340
intgsull 1:98efd8dd9077 341 }
intgsull 1:98efd8dd9077 342
jimmery 0:13d8a77fb1d7 343 void setup()
jimmery 0:13d8a77fb1d7 344 {
intgsull 8:03e5c3aaa9c9 345 eRS = 0;
intgsull 8:03e5c3aaa9c9 346 eRF = 0;
intgsull 8:03e5c3aaa9c9 347 eLS = 0;
intgsull 8:03e5c3aaa9c9 348 eLF = 0;
jimmery 0:13d8a77fb1d7 349
jimmery 2:82a11e992619 350 mouse_state = FORWARD;
jimmery 0:13d8a77fb1d7 351 myled = 1;
jimmery 0:13d8a77fb1d7 352 for ( int i = 0; i < 1000; i++ )
jimmery 0:13d8a77fb1d7 353 {
jimmery 0:13d8a77fb1d7 354 offsetCalc();
jimmery 0:13d8a77fb1d7 355 }
jimmery 0:13d8a77fb1d7 356 wait(2);
jimmery 0:13d8a77fb1d7 357
jimmery 0:13d8a77fb1d7 358 Systicker.attach_us(&systick, 1000);
intgsull 8:03e5c3aaa9c9 359 //action_ticker.attach_us(&turn, 1000);
intgsull 8:03e5c3aaa9c9 360 //algorithm_ticker.attach_us(&algorithm, 1000); let's try to stick with one ticker
jimmery 0:13d8a77fb1d7 361 }
jimmery 0:13d8a77fb1d7 362
jimmery 0:13d8a77fb1d7 363
jimmery 0:13d8a77fb1d7 364 int main() {
jimmery 0:13d8a77fb1d7 365 setup();
jimmery 0:13d8a77fb1d7 366 while(1) {
intgsull 3:40333f38771d 367 //
intgsull 3:40333f38771d 368 // pc.printf("left pulss %f, right pulses %f \r\n", l_enco.getPulses(), r_enco.getPulses());
intgsull 3:40333f38771d 369 // //wait(1);
jimmery 7:b866e3aae05f 370 //wait(1);
jimmery 7:b866e3aae05f 371 /*eRS = 1;
jimmery 7:b866e3aae05f 372 wait_us(50);
aduriseti 6:61b503990cd6 373 float right_side = rRS.read();
jimmery 7:b866e3aae05f 374 eRS = 0;
jimmery 7:b866e3aae05f 375 eLS = 1;
jimmery 7:b866e3aae05f 376 wait_us(50);
aduriseti 6:61b503990cd6 377 float left_side = rLS.read();
aduriseti 6:61b503990cd6 378 eLS = 0;
jimmery 7:b866e3aae05f 379 eRF = 1;
jimmery 7:b866e3aae05f 380 wait_us(50);
jimmery 7:b866e3aae05f 381 float right_front = rRF.read();
jimmery 7:b866e3aae05f 382 eRF = 0;
jimmery 7:b866e3aae05f 383 eLF = 1;
jimmery 7:b866e3aae05f 384 wait_us(50);
jimmery 7:b866e3aae05f 385 float left_front = rLF.read();
jimmery 7:b866e3aae05f 386 eLF = 0;
jimmery 7:b866e3aae05f 387 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 388 wait(1);*/
intgsull 3:40333f38771d 389 }
intgsull 3:40333f38771d 390 // pc.printf("left pulses %d, right pulses %d \r\n", l_enco.getPulses(), r_enco.getPulses()); //encoder testing
jimmery 0:13d8a77fb1d7 391 //wait(1);
jimmery 0:13d8a77fb1d7 392 }