2015-2016_Mouserat / Restructured_Test

this code is completely restructured, but should do the same thing. did not want to directly commit, since it may not work at all. compiles though.

Dependencies:   AVEncoder mbed-src-AV

Fork of Test by 2015-2016_Mouserat

main.cpp@10:d2907773f9a3, 2015-12-04 (annotated)

Committer:
jimmery
Date:
Fri Dec 04 09:38:25 2015 +0000
Revision:
10:d2907773f9a3
Parent:
9:ad6f60953086
Child:
11:cde87eaf3f0f
changed code structure up a lot. (no need to account for systick other things, gave more clear definitions to mouse_state and turn_direction); removed i-controller because it created a large amount of drift.

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