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
Diff: main.cpp
- Revision:
- 10:d2907773f9a3
- Parent:
- 9:ad6f60953086
- Child:
- 11:cde87eaf3f0f
--- a/main.cpp Fri Dec 04 07:23:38 2015 +0000 +++ b/main.cpp Fri Dec 04 09:38:25 2015 +0000 @@ -84,6 +84,10 @@ const float wall_left = 0; const float wall_right = 0; +volatile float lf_val; +volatile float rf_val; +volatile float ls_val; +volatile float rs_val; volatile float enco_error; volatile float enco_pid; @@ -142,54 +146,51 @@ } //combining detect opening and watch out -void Scan() +bool forward_wall_detected() +{ + return lf_val > thresh_LF; +} + +bool side_wall_detected() +{ + if (rs_val < open_right) + { + mouse_state = MOVECELL; + turn_direction = RIGHT; + return true; + } + else if (ls_val < open_left) + { + mouse_state = MOVECELL; + turn_direction = LEFT; + return true; + } + + return false; +} + +// updates PID values. +void scan() { eRS = 1; wait_us(50); - float right_side = rRS.read(); + rs_val = rRS.read(); eRS = 0; eLS = 1; wait_us(50); - float left_side = rLS.read(); + ls_val = rLS.read(); eLS = 0; - //eRF = 1; -// wait_us(50); -// float right_front = rRF.read(); //right front not working so screw it -// eRF = 0; + eLF = 1; wait_us(50); - float left_front = rLF.read(); - eLF = 0; - - if ( left_front > thresh_LF ) - { - mouse_state = STOPPED; - } - - /*if (right_side < open_right) { - mouse_state = MOVECELL; - opening_right_ahead = 1; - } else if (left_side < open_left) { - mouse_state = MOVECELL; - opening_left_ahead = 1; - } - - if(left_front > thresh_LF ) - { - mouse_state = STOPPED; // does this actually do anything tho - wall_in_front = 1; //I don't think we're using this and it's not getting reset? - - encoder_reset(); - mouse_state = TURNING; - turn_direction = LEFT; - } */ + lf_val = rLF.read(); } void systick() { - Scan(); // get the IR values. + scan(); // get the IR values. // these values are useful regardless of what mouse_state we are in. switch (mouse_state) { @@ -198,6 +199,18 @@ encoder_reset(); break; case FORWARD: + if ( forward_wall_detected() ) + { + mouse_state = STOPPED; + turn_direction = LEFT; + return; + } + +// if ( side_wall_detected() ) +// { +// return; +// } + enco_error = l_enco.getPulses() - r_enco.getPulses(); gyro_error = _gyro.read() - gyro_offset; @@ -294,10 +307,9 @@ } } - - +// TODO move_cell is not complete. this is copied code from systick. void move_cell() { - //out of business moved to algorithm + //bacl in business boys. if (mouse_state == MOVECELL) { forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses()); @@ -363,6 +375,45 @@ //algorithm_ticker.attach_us(&algorithm, 1000); let's try to stick with one ticker } +int main() { + setup(); + while(1) { + switch (mouse_state) + { + case STOPPED: + stop(); + wait(1); + if ( turn_direction == INVALID ) + { + mouse_state = FORWARD; + } + else + { + mouse_state = TURNING; + } + break; + case FORWARD: + left_forward = left_speed / left_max_speed; + left_reverse = 0; + right_forward = right_speed / right_max_speed; + right_reverse = 0; + break; + case TURNING: + turn(); + mouse_state = STOPPED; + turn_direction = INVALID; + break; + case MOVECELL: + move_cell(); + break; + default: + // idk lmao. show some error message? + break; + } + } +} + + void test_IR_sensors() { eRS = 1; @@ -385,28 +436,15 @@ wait(1); } -int main() { - setup(); - while(1) { - switch (mouse_state) - { - case STOPPED: - stop(); - break; - case FORWARD: - left_forward = left_speed / left_max_speed; - left_reverse = 0; - right_forward = right_speed / right_max_speed; - right_reverse = 0; - break; - case TURNING: - turn(); - break; - case MOVECELL: - break; - default: - // idk lmao - break; - } - } -} +void test_motors() +{ + encoder_reset(); + + right_forward = 1; + left_forward = 1; + right_reverse = 0; + left_reverse = 0; + + wait(10); + pc.printf("left pulses: %d, right pulses: %d", l_enco.getPulses(), r_enco.getPulses()); +} \ No newline at end of file