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@11:cde87eaf3f0f, 2015-12-04 (annotated)

Committer:
intgsull
Date:
Fri Dec 04 22:53:53 2015 +0000
Revision:
11:cde87eaf3f0f
Parent:
10:d2907773f9a3
Child:
12:0849b16c2672
updated

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
jimmery 0:13d8a77fb1d7 8 PwmOut right_forward(PB_10);
jimmery 0:13d8a77fb1d7 9 PwmOut right_reverse(PA_6);
jimmery 0:13d8a77fb1d7 10 PwmOut left_forward(PA_7);
jimmery 0:13d8a77fb1d7 11 PwmOut left_reverse(PB_6);
jimmery 0:13d8a77fb1d7 12
jimmery 0:13d8a77fb1d7 13 // TODO: change our encoder pins from AnalogIn into:
jimmery 0:13d8a77fb1d7 14 // otherwise, we can also use the AVEncoder thing as well.
jimmery 0:13d8a77fb1d7 15 AVEncoder l_enco(PA_15, PB_3);
jimmery 0:13d8a77fb1d7 16 AVEncoder r_enco(PA_1, PA_10);
jimmery 0:13d8a77fb1d7 17
jimmery 0:13d8a77fb1d7 18 // gyro
jimmery 0:13d8a77fb1d7 19 AnalogIn _gyro(PA_0);
jimmery 0:13d8a77fb1d7 20 // AnalogIn gyro_cal(PC_1) ?? currently this isn't connected.
jimmery 0:13d8a77fb1d7 21
jimmery 0:13d8a77fb1d7 22 //Left Front IR
jimmery 0:13d8a77fb1d7 23 DigitalOut eLF(PC_3);
jimmery 0:13d8a77fb1d7 24 AnalogIn rLF(PC_0);
jimmery 0:13d8a77fb1d7 25 //PC_4 is an ADC
jimmery 0:13d8a77fb1d7 26 //Left Side IR
jimmery 0:13d8a77fb1d7 27 DigitalOut eLS(PC_2);
jimmery 0:13d8a77fb1d7 28 AnalogIn rLS(PC_1);
jimmery 0:13d8a77fb1d7 29
intgsull 3:40333f38771d 30 //Right Side IR
intgsull 3:40333f38771d 31 DigitalOut eRS(PC_12);
intgsull 3:40333f38771d 32 AnalogIn rRS(PA_4);
jimmery 0:13d8a77fb1d7 33
intgsull 3:40333f38771d 34 //Right Front IR
intgsull 3:40333f38771d 35 DigitalOut eRF(PC_15);
intgsull 3:40333f38771d 36 AnalogIn rRF(PB_0);
jimmery 0:13d8a77fb1d7 37
jimmery 0:13d8a77fb1d7 38 DigitalOut myled(LED1);
jimmery 0:13d8a77fb1d7 39
jimmery 0:13d8a77fb1d7 40 volatile float gyro_offset = 0;
jimmery 0:13d8a77fb1d7 41
jimmery 0:13d8a77fb1d7 42 volatile float line_prevError = 0;
jimmery 0:13d8a77fb1d7 43 volatile float enco_prevError = 0;
jimmery 0:13d8a77fb1d7 44 volatile float gyro_prevError = 0;
jimmery 0:13d8a77fb1d7 45
jimmery 0:13d8a77fb1d7 46 volatile float line_accumulator = 0;
jimmery 0:13d8a77fb1d7 47 volatile float line_decayFactor = 1;
jimmery 0:13d8a77fb1d7 48 volatile float enco_accumulator = 0;
jimmery 9:ad6f60953086 49 volatile float enco_decayFactor = 1;
jimmery 0:13d8a77fb1d7 50 volatile float gyro_accumulator = 0;
intgsull 3:40333f38771d 51 volatile float gyro_decayFactor = 1;
jimmery 0:13d8a77fb1d7 52
intgsull 4:112f3d35bd2d 53 volatile float set_speed = .75;
intgsull 4:112f3d35bd2d 54 volatile float left_speed = .75;
intgsull 4:112f3d35bd2d 55 volatile float right_speed = .75;
jimmery 0:13d8a77fb1d7 56
intgsull 4:112f3d35bd2d 57 const float left_max_speed = 5; // max speed is 6 encoder pulses per ms.
intgsull 4:112f3d35bd2d 58 const float right_max_speed = 5;
jimmery 0:13d8a77fb1d7 59
jimmery 2:82a11e992619 60 const float gyro_propo = 6.5;
jimmery 0:13d8a77fb1d7 61 const float gyro_integ = 0;
jimmery 2:82a11e992619 62 const float gyro_deriv = 10;
jimmery 0:13d8a77fb1d7 63
aduriseti 6:61b503990cd6 64 const float enco_propo = 6;
jimmery 9:ad6f60953086 65 const float enco_integ = 0;//1;
jimmery 7:b866e3aae05f 66 const float enco_deriv = 1000;//.0002;
jimmery 0:13d8a77fb1d7 67
intgsull 3:40333f38771d 68 const float spin_enco_weight = 1;
jimmery 0:13d8a77fb1d7 69 const float spin_gyro_weight = 1 - spin_enco_weight;
jimmery 0:13d8a77fb1d7 70
jimmery 7:b866e3aae05f 71 const float base_RS = 0.2;
jimmery 7:b866e3aae05f 72 const float base_LS = 0.1;
jimmery 7:b866e3aae05f 73 const float base_RF = 0.002;
jimmery 7:b866e3aae05f 74 const float base_LF = 0.15;
jimmery 7:b866e3aae05f 75
jimmery 7:b866e3aae05f 76 const float thresh_LF = 0.25;
jimmery 7:b866e3aae05f 77 const float thresh_RF = 0.0045; // TODO: CAUTION USING THIS ALONE.
intgsull 8:03e5c3aaa9c9 78
intgsull 8:03e5c3aaa9c9 79 const float open_left = 0.22; //calibrate these two
intgsull 8:03e5c3aaa9c9 80 const float open_right = 0.18;
intgsull 8:03e5c3aaa9c9 81 const float wall_left = 0;
intgsull 8:03e5c3aaa9c9 82 const float wall_right = 0;
intgsull 8:03e5c3aaa9c9 83
jimmery 10:d2907773f9a3 84 volatile float lf_val;
jimmery 10:d2907773f9a3 85 volatile float rf_val;
jimmery 10:d2907773f9a3 86 volatile float ls_val;
jimmery 10:d2907773f9a3 87 volatile float rs_val;
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,
jimmery 9:ad6f60953086 98 RIGHT,
jimmery 9:ad6f60953086 99 INVALID
intgsull 8:03e5c3aaa9c9 100 } TURN_DIRECTION;
intgsull 8:03e5c3aaa9c9 101 volatile TURN_DIRECTION turn_direction;
aduriseti 5:f704940c9c7e 102
jimmery 9:ad6f60953086 103 //volatile int turn_counter;
intgsull 8:03e5c3aaa9c9 104 volatile int right_turn_count;
intgsull 8:03e5c3aaa9c9 105 volatile int examined_left;
intgsull 8:03e5c3aaa9c9 106 volatile int examined_right;
intgsull 8:03e5c3aaa9c9 107
intgsull 8:03e5c3aaa9c9 108 //time to make a 90 degree turn and move forward one cell length, repectively
intgsull 8:03e5c3aaa9c9 109 //should be replaced with encoder counts
intgsull 11:cde87eaf3f0f 110 const int lmax_turn_count = 620; //I think we need different constants for left/right
intgsull 11:cde87eaf3f0f 111 const int rmax_turn_count = 0;
intgsull 11:cde87eaf3f0f 112 const int cell_length_count = 800;
intgsull 4:112f3d35bd2d 113
intgsull 8:03e5c3aaa9c9 114 volatile int forward_counter = 0;
intgsull 4:112f3d35bd2d 115
jimmery 2:82a11e992619 116 // this is just so that we can maintain what state our mouse is in.
jimmery 2:82a11e992619 117 // currently this has no real use, but it may in the future.
jimmery 2:82a11e992619 118 // or we could just remove this entirely.
jimmery 2:82a11e992619 119 typedef enum
jimmery 2:82a11e992619 120 {
jimmery 2:82a11e992619 121 STOPPED,
jimmery 2:82a11e992619 122 FORWARD,
aduriseti 5:f704940c9c7e 123 TURNING,
intgsull 8:03e5c3aaa9c9 124 MOVECELL,
aduriseti 5:f704940c9c7e 125 FINISHED,
jimmery 2:82a11e992619 126 UNKNOWN
jimmery 2:82a11e992619 127 } STATE;
jimmery 2:82a11e992619 128 volatile STATE mouse_state;
jimmery 2:82a11e992619 129
aduriseti 6:61b503990cd6 130 volatile int wall_in_front = 0;
aduriseti 6:61b503990cd6 131 volatile int opening_left_ahead = 0;
aduriseti 6:61b503990cd6 132 volatile int opening_right_ahead = 0;
intgsull 8:03e5c3aaa9c9 133 volatile int opening_left = 0;
intgsull 8:03e5c3aaa9c9 134 volatile int opening_right = 0;
aduriseti 6:61b503990cd6 135
intgsull 8:03e5c3aaa9c9 136 void Scan();
jimmery 2:82a11e992619 137 void offsetCalc();
jimmery 2:82a11e992619 138 void stop();
jimmery 2:82a11e992619 139
aduriseti 5:f704940c9c7e 140 void encoder_reset()
jimmery 0:13d8a77fb1d7 141 {
jimmery 0:13d8a77fb1d7 142 l_enco.reset();
jimmery 0:13d8a77fb1d7 143 r_enco.reset();
jimmery 0:13d8a77fb1d7 144 }
jimmery 0:13d8a77fb1d7 145
intgsull 8:03e5c3aaa9c9 146 //combining detect opening and watch out
jimmery 10:d2907773f9a3 147 bool forward_wall_detected()
jimmery 10:d2907773f9a3 148 {
jimmery 10:d2907773f9a3 149 return lf_val > thresh_LF;
jimmery 10:d2907773f9a3 150 }
jimmery 10:d2907773f9a3 151
jimmery 10:d2907773f9a3 152 bool side_wall_detected()
jimmery 10:d2907773f9a3 153 {
jimmery 10:d2907773f9a3 154 if (rs_val < open_right)
jimmery 10:d2907773f9a3 155 {
jimmery 10:d2907773f9a3 156 mouse_state = MOVECELL;
jimmery 10:d2907773f9a3 157 turn_direction = RIGHT;
jimmery 10:d2907773f9a3 158 return true;
jimmery 10:d2907773f9a3 159 }
jimmery 10:d2907773f9a3 160 else if (ls_val < open_left)
jimmery 10:d2907773f9a3 161 {
jimmery 10:d2907773f9a3 162 mouse_state = MOVECELL;
jimmery 10:d2907773f9a3 163 turn_direction = LEFT;
jimmery 10:d2907773f9a3 164 return true;
jimmery 10:d2907773f9a3 165 }
jimmery 10:d2907773f9a3 166
jimmery 10:d2907773f9a3 167 return false;
jimmery 10:d2907773f9a3 168 }
jimmery 10:d2907773f9a3 169
jimmery 10:d2907773f9a3 170 // updates PID values.
jimmery 10:d2907773f9a3 171 void scan()
intgsull 8:03e5c3aaa9c9 172 {
jimmery 9:ad6f60953086 173 eRS = 1;
jimmery 9:ad6f60953086 174 wait_us(50);
jimmery 10:d2907773f9a3 175 rs_val = rRS.read();
aduriseti 6:61b503990cd6 176 eRS = 0;
intgsull 8:03e5c3aaa9c9 177
aduriseti 6:61b503990cd6 178 eLS = 1;
intgsull 8:03e5c3aaa9c9 179 wait_us(50);
jimmery 10:d2907773f9a3 180 ls_val = rLS.read();
aduriseti 6:61b503990cd6 181 eLS = 0;
jimmery 10:d2907773f9a3 182
intgsull 8:03e5c3aaa9c9 183 eLF = 1;
jimmery 9:ad6f60953086 184 wait_us(50);
jimmery 10:d2907773f9a3 185 lf_val = rLF.read();
intgsull 8:03e5c3aaa9c9 186 }
intgsull 8:03e5c3aaa9c9 187
aduriseti 6:61b503990cd6 188
jimmery 0:13d8a77fb1d7 189 void systick()
jimmery 0:13d8a77fb1d7 190 {
jimmery 10:d2907773f9a3 191 scan(); // get the IR values.
jimmery 9:ad6f60953086 192 // these values are useful regardless of what mouse_state we are in.
jimmery 9:ad6f60953086 193 switch (mouse_state)
jimmery 2:82a11e992619 194 {
jimmery 9:ad6f60953086 195 case STOPPED:
jimmery 9:ad6f60953086 196 offsetCalc();
jimmery 9:ad6f60953086 197 encoder_reset();
jimmery 9:ad6f60953086 198 break;
jimmery 9:ad6f60953086 199 case FORWARD:
jimmery 10:d2907773f9a3 200 if ( forward_wall_detected() )
jimmery 10:d2907773f9a3 201 {
jimmery 10:d2907773f9a3 202 mouse_state = STOPPED;
jimmery 10:d2907773f9a3 203 turn_direction = LEFT;
jimmery 10:d2907773f9a3 204 return;
jimmery 10:d2907773f9a3 205 }
jimmery 10:d2907773f9a3 206
jimmery 10:d2907773f9a3 207 // if ( side_wall_detected() )
jimmery 10:d2907773f9a3 208 // {
jimmery 10:d2907773f9a3 209 // return;
jimmery 10:d2907773f9a3 210 // }
jimmery 10:d2907773f9a3 211
jimmery 9:ad6f60953086 212 enco_error = l_enco.getPulses() - r_enco.getPulses();
jimmery 9:ad6f60953086 213 gyro_error = _gyro.read() - gyro_offset;
jimmery 9:ad6f60953086 214
jimmery 9:ad6f60953086 215 enco_accumulator += enco_error;
jimmery 9:ad6f60953086 216 gyro_accumulator += gyro_error;
jimmery 9:ad6f60953086 217
jimmery 9:ad6f60953086 218 enco_pid = 0;
jimmery 9:ad6f60953086 219 enco_pid += enco_propo * enco_error;
jimmery 9:ad6f60953086 220 enco_pid += enco_integ * enco_accumulator;
jimmery 9:ad6f60953086 221 enco_pid += enco_deriv * (enco_error - enco_prevError);
aduriseti 5:f704940c9c7e 222
jimmery 9:ad6f60953086 223 gyro_pid = 0;
jimmery 9:ad6f60953086 224 gyro_pid += gyro_propo * gyro_error;
jimmery 9:ad6f60953086 225 gyro_pid += gyro_integ * gyro_accumulator;
jimmery 9:ad6f60953086 226 gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
jimmery 9:ad6f60953086 227
jimmery 9:ad6f60953086 228 w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
jimmery 9:ad6f60953086 229 left_speed = set_speed + w_error;
jimmery 9:ad6f60953086 230 right_speed = set_speed - w_error;
jimmery 9:ad6f60953086 231
jimmery 9:ad6f60953086 232 enco_prevError = enco_error;
jimmery 9:ad6f60953086 233 gyro_prevError = gyro_error;
jimmery 9:ad6f60953086 234
jimmery 9:ad6f60953086 235 enco_accumulator += enco_error;
jimmery 9:ad6f60953086 236 gyro_accumulator += gyro_error;
jimmery 9:ad6f60953086 237
jimmery 9:ad6f60953086 238 enco_accumulator /= enco_decayFactor;
jimmery 9:ad6f60953086 239 gyro_accumulator /= gyro_decayFactor;
jimmery 9:ad6f60953086 240 break;
jimmery 9:ad6f60953086 241 default:
jimmery 9:ad6f60953086 242 // don't do anything.
jimmery 9:ad6f60953086 243 break;
jimmery 2:82a11e992619 244 }
jimmery 0:13d8a77fb1d7 245 }
jimmery 0:13d8a77fb1d7 246
jimmery 0:13d8a77fb1d7 247 // computes gyro_offset
jimmery 0:13d8a77fb1d7 248 // uses a "weighted" average.
jimmery 0:13d8a77fb1d7 249 // idea is that the current gyro offset is weighted more than previous ones.
jimmery 0:13d8a77fb1d7 250 // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n).
jimmery 0:13d8a77fb1d7 251 // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
jimmery 0:13d8a77fb1d7 252 // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
jimmery 0:13d8a77fb1d7 253 // currently this is only in the setup function. we can run this when the mouse is running in a line
jimmery 0:13d8a77fb1d7 254 // when we figure out good line running pid.
jimmery 0:13d8a77fb1d7 255 void offsetCalc()
jimmery 0:13d8a77fb1d7 256 {
jimmery 0:13d8a77fb1d7 257 gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
jimmery 0:13d8a77fb1d7 258 }
jimmery 0:13d8a77fb1d7 259
jimmery 0:13d8a77fb1d7 260
intgsull 1:98efd8dd9077 261 void stop()
intgsull 1:98efd8dd9077 262 {
intgsull 11:cde87eaf3f0f 263 left_forward = 0;
intgsull 11:cde87eaf3f0f 264 left_reverse = 0;
intgsull 11:cde87eaf3f0f 265 right_forward = 0;
intgsull 11:cde87eaf3f0f 266 right_reverse = 0;
jimmery 2:82a11e992619 267 }
aduriseti 6:61b503990cd6 268
aduriseti 5:f704940c9c7e 269 void turn() {
jimmery 9:ad6f60953086 270 // reopen for business.
jimmery 9:ad6f60953086 271
jimmery 9:ad6f60953086 272 int turn_counter = 0;
jimmery 9:ad6f60953086 273 int base_left_pulses = l_enco.getPulses();
jimmery 9:ad6f60953086 274 int base_right_pulses = r_enco.getPulses();
jimmery 9:ad6f60953086 275
jimmery 9:ad6f60953086 276 int left_pulses;
jimmery 9:ad6f60953086 277 int right_pulses;
jimmery 9:ad6f60953086 278
intgsull 11:cde87eaf3f0f 279 while ( turn_counter < lmax_turn_count )
jimmery 9:ad6f60953086 280 {
jimmery 9:ad6f60953086 281 left_pulses = l_enco.getPulses();
jimmery 9:ad6f60953086 282 right_pulses = r_enco.getPulses();
jimmery 9:ad6f60953086 283
jimmery 9:ad6f60953086 284 turn_counter = ((left_pulses - base_left_pulses) + (right_pulses - base_right_pulses)) / 2;
jimmery 9:ad6f60953086 285
jimmery 9:ad6f60953086 286 switch(turn_direction)
jimmery 9:ad6f60953086 287 {
jimmery 9:ad6f60953086 288 case LEFT:
jimmery 9:ad6f60953086 289 left_forward = 0;
jimmery 9:ad6f60953086 290 right_forward = set_speed / right_max_speed;
jimmery 9:ad6f60953086 291 left_reverse = set_speed / left_max_speed;
jimmery 9:ad6f60953086 292 right_reverse = 0;
jimmery 9:ad6f60953086 293 break;
jimmery 9:ad6f60953086 294 case RIGHT:
jimmery 9:ad6f60953086 295 left_forward = set_speed / left_max_speed;
jimmery 9:ad6f60953086 296 right_forward = 0;
jimmery 9:ad6f60953086 297 left_reverse = 0;
jimmery 9:ad6f60953086 298 right_reverse = set_speed / right_max_speed;
jimmery 9:ad6f60953086 299 break;
jimmery 9:ad6f60953086 300 default:
jimmery 9:ad6f60953086 301 // invalid state. did not set a turn direction.
jimmery 9:ad6f60953086 302 mouse_state = STOPPED;
intgsull 11:cde87eaf3f0f 303 break;
jimmery 9:ad6f60953086 304 }
jimmery 9:ad6f60953086 305
jimmery 9:ad6f60953086 306 }
aduriseti 5:f704940c9c7e 307 }
aduriseti 6:61b503990cd6 308
jimmery 10:d2907773f9a3 309 // TODO move_cell is not complete. this is copied code from systick.
aduriseti 6:61b503990cd6 310 void move_cell() {
jimmery 10:d2907773f9a3 311 //bacl in business boys.
jimmery 9:ad6f60953086 312 if (mouse_state == MOVECELL)
jimmery 9:ad6f60953086 313 {
jimmery 9:ad6f60953086 314 forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
jimmery 9:ad6f60953086 315 if (forward_counter > 2000)
jimmery 9:ad6f60953086 316 {
jimmery 9:ad6f60953086 317 forward_counter = 0;
jimmery 9:ad6f60953086 318 mouse_state = TURNING;
jimmery 9:ad6f60953086 319 if (opening_left_ahead)
jimmery 9:ad6f60953086 320 {
jimmery 9:ad6f60953086 321 opening_left_ahead = 0;
jimmery 9:ad6f60953086 322 turn_direction = LEFT;
jimmery 9:ad6f60953086 323 }
jimmery 9:ad6f60953086 324 else if (opening_right_ahead)
jimmery 9:ad6f60953086 325 {
jimmery 9:ad6f60953086 326 opening_right_ahead = 0;
jimmery 9:ad6f60953086 327 }
jimmery 9:ad6f60953086 328 encoder_reset(); //right? prepare for turn
jimmery 9:ad6f60953086 329 }
jimmery 9:ad6f60953086 330 }
aduriseti 6:61b503990cd6 331 }
aduriseti 5:f704940c9c7e 332
jimmery 0:13d8a77fb1d7 333 void setup()
jimmery 0:13d8a77fb1d7 334 {
jimmery 9:ad6f60953086 335 eRS = 0;
jimmery 9:ad6f60953086 336 eRF = 0;
jimmery 9:ad6f60953086 337 eLS = 0;
jimmery 9:ad6f60953086 338 eLF = 0;
jimmery 0:13d8a77fb1d7 339
jimmery 2:82a11e992619 340 mouse_state = FORWARD;
jimmery 9:ad6f60953086 341 turn_direction = INVALID;
jimmery 0:13d8a77fb1d7 342 myled = 1;
jimmery 0:13d8a77fb1d7 343 for ( int i = 0; i < 1000; i++ )
jimmery 0:13d8a77fb1d7 344 {
jimmery 0:13d8a77fb1d7 345 offsetCalc();
jimmery 0:13d8a77fb1d7 346 }
jimmery 0:13d8a77fb1d7 347 wait(2);
jimmery 0:13d8a77fb1d7 348
jimmery 0:13d8a77fb1d7 349 Systicker.attach_us(&systick, 1000);
jimmery 0:13d8a77fb1d7 350 }
jimmery 9:ad6f60953086 351
jimmery 10:d2907773f9a3 352 int main() {
jimmery 10:d2907773f9a3 353 setup();
jimmery 10:d2907773f9a3 354 while(1) {
jimmery 10:d2907773f9a3 355 switch (mouse_state)
jimmery 10:d2907773f9a3 356 {
jimmery 10:d2907773f9a3 357 case STOPPED:
jimmery 10:d2907773f9a3 358 stop();
jimmery 10:d2907773f9a3 359 wait(1);
jimmery 10:d2907773f9a3 360 if ( turn_direction == INVALID )
jimmery 10:d2907773f9a3 361 {
jimmery 10:d2907773f9a3 362 mouse_state = FORWARD;
jimmery 10:d2907773f9a3 363 }
jimmery 10:d2907773f9a3 364 else
jimmery 10:d2907773f9a3 365 {
jimmery 10:d2907773f9a3 366 mouse_state = TURNING;
jimmery 10:d2907773f9a3 367 }
jimmery 10:d2907773f9a3 368 break;
jimmery 10:d2907773f9a3 369 case FORWARD:
jimmery 10:d2907773f9a3 370 left_forward = left_speed / left_max_speed;
jimmery 10:d2907773f9a3 371 left_reverse = 0;
jimmery 10:d2907773f9a3 372 right_forward = right_speed / right_max_speed;
jimmery 10:d2907773f9a3 373 right_reverse = 0;
jimmery 10:d2907773f9a3 374 break;
jimmery 10:d2907773f9a3 375 case TURNING:
jimmery 10:d2907773f9a3 376 turn();
jimmery 10:d2907773f9a3 377 mouse_state = STOPPED;
jimmery 10:d2907773f9a3 378 turn_direction = INVALID;
jimmery 10:d2907773f9a3 379 break;
jimmery 10:d2907773f9a3 380 case MOVECELL:
jimmery 10:d2907773f9a3 381 move_cell();
jimmery 10:d2907773f9a3 382 break;
jimmery 10:d2907773f9a3 383 default:
jimmery 10:d2907773f9a3 384 // idk lmao. show some error message?
jimmery 10:d2907773f9a3 385 break;
jimmery 10:d2907773f9a3 386 }
jimmery 10:d2907773f9a3 387 }
jimmery 10:d2907773f9a3 388 }
jimmery 10:d2907773f9a3 389
jimmery 10:d2907773f9a3 390
jimmery 9:ad6f60953086 391 void test_IR_sensors()
jimmery 9:ad6f60953086 392 {
jimmery 9:ad6f60953086 393 eRS = 1;
jimmery 9:ad6f60953086 394 wait_us(50);
jimmery 9:ad6f60953086 395 float right_side = rRS.read();
jimmery 9:ad6f60953086 396 eRS = 0;
jimmery 9:ad6f60953086 397 eLS = 1;
jimmery 9:ad6f60953086 398 wait_us(50);
jimmery 9:ad6f60953086 399 float left_side = rLS.read();
jimmery 9:ad6f60953086 400 eLS = 0;
jimmery 9:ad6f60953086 401 eRF = 1;
jimmery 9:ad6f60953086 402 wait_us(50);
jimmery 9:ad6f60953086 403 float right_front = rRF.read();
jimmery 9:ad6f60953086 404 eRF = 0;
jimmery 9:ad6f60953086 405 eLF = 1;
jimmery 9:ad6f60953086 406 wait_us(50);
jimmery 9:ad6f60953086 407 float left_front = rLF.read();
jimmery 9:ad6f60953086 408 eLF = 0;
jimmery 9:ad6f60953086 409 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 410 wait(1);
jimmery 9:ad6f60953086 411 }
jimmery 0:13d8a77fb1d7 412
jimmery 10:d2907773f9a3 413 void test_motors()
jimmery 10:d2907773f9a3 414 {
jimmery 10:d2907773f9a3 415 encoder_reset();
jimmery 10:d2907773f9a3 416
jimmery 10:d2907773f9a3 417 right_forward = 1;
jimmery 10:d2907773f9a3 418 left_forward = 1;
jimmery 10:d2907773f9a3 419 right_reverse = 0;
jimmery 10:d2907773f9a3 420 left_reverse = 0;
jimmery 10:d2907773f9a3 421
jimmery 10:d2907773f9a3 422 wait(10);
jimmery 10:d2907773f9a3 423 pc.printf("left pulses: %d, right pulses: %d", l_enco.getPulses(), r_enco.getPulses());
jimmery 10:d2907773f9a3 424 }