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@12:0849b16c2672, 2015-12-05 (annotated)

Committer:
jimmery
Date:
Sat Dec 05 00:26:26 2015 +0000
Revision:
12:0849b16c2672
Parent:
11:cde87eaf3f0f
Child:
13:5f08195456a4
gyro turn readded. more robust;

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