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:
- 2:82a11e992619
- Parent:
- 1:98efd8dd9077
- Child:
- 3:40333f38771d
--- a/main.cpp Sat Nov 21 03:43:15 2015 +0000 +++ b/main.cpp Tue Nov 24 04:13:10 2015 +0000 @@ -47,25 +47,26 @@ volatile float line_accumulator = 0; volatile float line_decayFactor = 1; volatile float enco_accumulator = 0; -volatile float enco_decayFactor = 1; +volatile float enco_decayFactor = 1.2; volatile float gyro_accumulator = 0; -volatile float gyro_decayFactor = 1; +volatile float gyro_decayFactor = 1.2; +volatile float set_speed = 0.5; volatile float left_speed = 0.5; volatile float right_speed = 0.5; const float left_max_speed = 6; // max speed is 6 encoder pulses per ms. const float right_max_speed = 6.2; -const float gyro_propo = 0.75; +const float gyro_propo = 6.5; const float gyro_integ = 0; -const float gyro_deriv = 1; +const float gyro_deriv = 10; -const float enco_propo = 0.05; +const float enco_propo = .0005; const float enco_integ = 0; -const float enco_deriv = 5; +const float enco_deriv = .0002; -const float spin_enco_weight = 0.5; +const float spin_enco_weight = 0.75; const float spin_gyro_weight = 1 - spin_enco_weight; const float frontWall = 0.7; //need to calibrate this threshold to a value where mouse can stop in time @@ -77,6 +78,22 @@ volatile float gyro_pid; volatile float w_error; +// this is just so that we can maintain what state our mouse is in. +// currently this has no real use, but it may in the future. +// or we could just remove this entirely. +typedef enum +{ + STOPPED, + FORWARD, + TURNING, + UNKNOWN +} STATE; +volatile STATE mouse_state; + +void watchOut(); +void offsetCalc(); +void stop(); + void reset() { l_enco.reset(); @@ -86,28 +103,48 @@ void systick() { watchOut(); + if ( mouse_state == STOPPED ) + { + offsetCalc(); + stop(); + return; + } 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_deriv * (enco_error - enco_prevError); + 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 += w_error; - right_speed -= w_error; + 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; - reset(); + 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 @@ -130,18 +167,19 @@ left_reverse = 1; right_forward = 1; right_reverse = 1; - } +} void watchOut() { eRF = 1; + float right = rRF.read(); + eRF = 0; eLF = 1; - if(rRF > frontWall || rLF > frontWall) + float left = rLF.read(); + eLF = 0; + if(left > frontWall || right > frontWall) { - eRF = 0; - eLF = 0; - stop(); - return; + mouse_state = STOPPED; } } @@ -155,6 +193,8 @@ eLS = 0; eLF = 0; + mouse_state = FORWARD; + myled = 1; for ( int i = 0; i < 1000; i++ ) @@ -178,7 +218,7 @@ setup(); while(1) { - pc.printf("enco_error: %f, gyro_error: %f, w_error: %f\r\n", enco_error, gyro_error, w_error); + pc.printf("enco_pid: %f, gyro_pid: %f, w_error: %f\r\n", enco_pid, gyro_pid, w_error); //wait(1); } }