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:
- 5:f704940c9c7e
- Parent:
- 4:112f3d35bd2d
- Child:
- 6:61b503990cd6
--- a/main.cpp Tue Dec 01 19:38:27 2015 +0000 +++ b/main.cpp Wed Dec 02 01:53:48 2015 +0000 @@ -96,6 +96,7 @@ const float irKi = 0; //constants volatile float leftD; volatile float rightD; + void irPID() { eLS = 1; @@ -148,7 +149,8 @@ { STOPPED, FORWARD, - TURNING, + TURNING, + FINISHED, UNKNOWN } STATE; volatile STATE mouse_state; @@ -157,7 +159,7 @@ void offsetCalc(); void stop(); -void reset() +void encoder_reset() { l_enco.reset(); r_enco.reset(); @@ -165,49 +167,51 @@ void systick() { - watchOut(); + //watchOut(); if ( mouse_state == STOPPED ) { offsetCalc(); stop(); return; + } else if (mouse_state == FORWARD) { + watchOut(); + + enco_error = l_enco.getPulses() - r_enco.getPulses(); + gyro_error = _gyro.read() - gyro_offset; + + enco_accumulator += enco_error; + gyro_accumulator += gyro_error; + + enco_pid = 0; + enco_pid += enco_propo * enco_error; + enco_pid += enco_integ * enco_accumulator; + enco_pid += enco_deriv * (enco_error - enco_prevError); + + gyro_pid = 0; + gyro_pid += gyro_propo * gyro_error; + gyro_pid += gyro_integ * gyro_accumulator; + gyro_pid += gyro_deriv * (gyro_error - gyro_prevError); + + w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid; + left_speed = set_speed + w_error; + right_speed = set_speed - w_error; + + left_forward = left_speed / left_max_speed; + left_reverse = 0; + right_forward = right_speed / right_max_speed; + right_reverse = 0; + + enco_prevError = enco_error; + gyro_prevError = gyro_error; + + enco_accumulator += enco_error; + gyro_accumulator += gyro_error; + + enco_accumulator /= enco_decayFactor; + gyro_accumulator /= gyro_decayFactor; + + //reset(); } - - enco_error = l_enco.getPulses() - r_enco.getPulses(); - gyro_error = _gyro.read() - gyro_offset; - - enco_accumulator += enco_error; - gyro_accumulator += gyro_error; - - enco_pid = 0; - enco_pid += enco_propo * enco_error; - enco_pid += enco_integ * enco_accumulator; - enco_pid += enco_deriv * (enco_error - enco_prevError); - - gyro_pid = 0; - gyro_pid += gyro_propo * gyro_error; - gyro_pid += gyro_integ * gyro_accumulator; - gyro_pid += gyro_deriv * (gyro_error - gyro_prevError); - - w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid; - left_speed = set_speed + w_error; - right_speed = set_speed - w_error; - - left_forward = left_speed / left_max_speed; - left_reverse = 0; - right_forward = right_speed / right_max_speed; - right_reverse = 0; - - enco_prevError = enco_error; - gyro_prevError = gyro_error; - - enco_accumulator += enco_error; - gyro_accumulator += gyro_error; - - enco_accumulator /= enco_decayFactor; - gyro_accumulator /= gyro_decayFactor; - - //reset(); } // computes gyro_offset @@ -234,19 +238,102 @@ void watchOut() { - eRF = 1; - float right = rRF.read(); - eRF = 0; - eLF = 1; - float left = rLF.read(); - eLF = 0; - if(left > frontWall || right > frontWall) - { - mouse_state = STOPPED; + //if (mouse_state == FORWARD) { + eRF = 1; + float right = rRF.read(); + eRF = 0; + eLF = 1; + float left = rLF.read(); + eLF = 0; + if(left > frontWall || right > frontWall) + { + mouse_state = STOPPED; + } + //} +} + + +typedef enum +{ + LEFT, + RIGHT +} TURN_DIRECTION; +volatile TURN_DIRECTION turn_direction; + +volatile int turn_counter; +volatile int forward_counter; +volatile int right_turn_count; +volatile int examined_left; +volatile int examined_right; + +//time to make a 90 degree turn and move forward one cell length, repectively +//should be replaced with encoder counts +volatile int max_turn_count = 100; +volatile int cell_length_count = 100; + +void turn() { + if (mouse_state == TURNING) { + if (turn_counter < max_turn_count) { + int left_pulses = l_enco.getPulses(); + int right_pulses = r_enco.getPulses(); + if (left_pulses < 0) left_pulses = -left_pulses; + if (right_pulses < 0) right_pulses = -right_pulses; + turn_counter = (left_pulses + right_pulses)/2; + } else { + if (turn_direction == LEFT) { + examined_left = 1; + } else { + if (right_turn_count > 1) { + examined_right = 1; + } else { + right_turn_count++; + mouse_state = STOPPED; + return; + } + } + encoder_reset(); + mouse_state = FORWARD; + } + } +} + +void find_opening() { + if (mouse_state == STOPPED) { + encoder_reset(); + if (!examined_left && !examined_right) { + turn_direction = LEFT; + left_forward = 0; + left_reverse = set_speed / left_max_speed; + right_forward = set_speed / right_max_speed; + right_reverse = 0; + mouse_state = TURNING; + } else if (examined_left && !examined_right) { + turn_direction = RIGHT; + left_forward = set_speed / left_max_speed; + left_reverse = 0; + right_forward = 0; + right_reverse = set_speed / right_max_speed; + mouse_state = TURNING; + } else if (!examined_left && examined_right) { + //this should never happen + mouse_state = FINISHED; + } else { + //we are done + mouse_state = FINISHED; + } + } else if (mouse_state == FORWARD) { + if (forward_counter > cell_length_count) { + examined_left = 0; + examined_right = 0; + right_turn_count = 0; + forward_counter = 0; + } + forward_counter = (l_enco.getPulses() + r_enco.getPulses())/2; } } - +Ticker turn_ticker; +Ticker algorithm_ticker; void setup() { @@ -275,6 +362,9 @@ // repeat this for some time frame. Systicker.attach_us(&systick, 1000); + //ir_ticker.attach_us(&irPID, 100000); + algorithm_ticker.attach_us(&find_opening, 1000); + turn_ticker.attach_us(&turn, 1000); }