PID Test
Dependencies: AVEncoder mbed-src-AV
main.cpp
- Committer:
- jimmery
- Date:
- 2015-12-04
- Revision:
- 10:d2907773f9a3
- Parent:
- 9:ad6f60953086
- Child:
- 11:cde87eaf3f0f
File content as of revision 10:d2907773f9a3:
#include "mbed.h" #include "AVEncoder.h" // set things Serial pc(SERIAL_TX, SERIAL_RX); Ticker Systicker; Timer timer; Ticker action_ticker; Ticker algorithm_ticker; PwmOut right_forward(PB_10); PwmOut right_reverse(PA_6); PwmOut left_forward(PA_7); PwmOut left_reverse(PB_6); // TODO: change our encoder pins from AnalogIn into: // otherwise, we can also use the AVEncoder thing as well. AVEncoder l_enco(PA_15, PB_3); AVEncoder r_enco(PA_1, PA_10); // gyro AnalogIn _gyro(PA_0); // AnalogIn gyro_cal(PC_1) ?? currently this isn't connected. //Left Front IR DigitalOut eLF(PC_3); AnalogIn rLF(PC_0); //PC_4 is an ADC //Left Side IR DigitalOut eLS(PC_2); AnalogIn rLS(PC_1); //Right Side IR DigitalOut eRS(PC_12); AnalogIn rRS(PA_4); //Right Front IR DigitalOut eRF(PC_15); AnalogIn rRF(PB_0); DigitalOut myled(LED1); volatile float gyro_offset = 0; volatile float line_prevError = 0; volatile float enco_prevError = 0; volatile float gyro_prevError = 0; volatile float line_accumulator = 0; volatile float line_decayFactor = 1; volatile float enco_accumulator = 0; volatile float enco_decayFactor = 1; volatile float gyro_accumulator = 0; volatile float gyro_decayFactor = 1; volatile float set_speed = .75; volatile float left_speed = .75; volatile float right_speed = .75; const float left_max_speed = 5; // max speed is 6 encoder pulses per ms. const float right_max_speed = 5; const float gyro_propo = 6.5; const float gyro_integ = 0; const float gyro_deriv = 10; const float enco_propo = 6; const float enco_integ = 0;//1; const float enco_deriv = 1000;//.0002; const float spin_enco_weight = 1; const float spin_gyro_weight = 1 - spin_enco_weight; const float base_RS = 0.2; const float base_LS = 0.1; const float base_RF = 0.002; const float base_LF = 0.15; const float thresh_LF = 0.25; const float thresh_RF = 0.0045; // TODO: CAUTION USING THIS ALONE. const float open_left = 0.22; //calibrate these two const float open_right = 0.18; 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; volatile float gyro_error; volatile float gyro_pid; volatile float w_error; typedef enum { LEFT, RIGHT, INVALID } TURN_DIRECTION; volatile TURN_DIRECTION turn_direction; //volatile int turn_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 = 800; volatile int cell_length_count = 800; volatile int forward_counter = 0; // 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, MOVECELL, FINISHED, UNKNOWN } STATE; volatile STATE mouse_state; volatile int wall_in_front = 0; volatile int opening_left_ahead = 0; volatile int opening_right_ahead = 0; volatile int opening_left = 0; volatile int opening_right = 0; void Scan(); void offsetCalc(); void stop(); void encoder_reset() { l_enco.reset(); r_enco.reset(); } //combining detect opening and watch out 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); rs_val = rRS.read(); eRS = 0; eLS = 1; wait_us(50); ls_val = rLS.read(); eLS = 0; eLF = 1; wait_us(50); lf_val = rLF.read(); } void systick() { scan(); // get the IR values. // these values are useful regardless of what mouse_state we are in. switch (mouse_state) { case STOPPED: offsetCalc(); 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; 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; enco_prevError = enco_error; gyro_prevError = gyro_error; enco_accumulator += enco_error; gyro_accumulator += gyro_error; enco_accumulator /= enco_decayFactor; gyro_accumulator /= gyro_decayFactor; break; default: // don't do anything. break; } } // computes gyro_offset // uses a "weighted" average. // idea is that the current gyro offset is weighted more than previous ones. // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n). // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.) // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight. // currently this is only in the setup function. we can run this when the mouse is running in a line // when we figure out good line running pid. void offsetCalc() { gyro_offset = gyro_offset / 2 + _gyro.read() / 2; } void stop() { left_forward = 1; left_reverse = 1; right_forward = 1; right_reverse = 1; } void turn() { // reopen for business. int turn_counter = 0; int base_left_pulses = l_enco.getPulses(); int base_right_pulses = r_enco.getPulses(); int left_pulses; int right_pulses; while ( turn_counter < max_turn_count ) { left_pulses = l_enco.getPulses(); right_pulses = r_enco.getPulses(); turn_counter = ((left_pulses - base_left_pulses) + (right_pulses - base_right_pulses)) / 2; switch(turn_direction) { case LEFT: left_forward = 0; right_forward = set_speed / right_max_speed; left_reverse = set_speed / left_max_speed; right_reverse = 0; break; case RIGHT: left_forward = set_speed / left_max_speed; right_forward = 0; left_reverse = 0; right_reverse = set_speed / right_max_speed; break; default: // invalid state. did not set a turn direction. mouse_state = STOPPED; } } } // TODO move_cell is not complete. this is copied code from systick. void move_cell() { //bacl in business boys. if (mouse_state == MOVECELL) { forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses()); if (forward_counter > 2000) { forward_counter = 0; mouse_state = TURNING; if (opening_left_ahead) { opening_left_ahead = 0; turn_direction = LEFT; } else if (opening_right_ahead) { opening_right_ahead = 0; } encoder_reset(); //right? prepare for turn } } } void algorithm() { //moved to systick //encoder_reset(); // mouse_state = TURNING; // turn_direction = LEFT; //action_ticker.attach_us(&turn, 1000); /*if (opening_left_ahead || opening_right_ahead) { action_ticker.attach_us(&move_cell, 1000); } if (opening_left) { mouse_state = TURNING; turn_direction = LEFT; action_ticker.attach_us(&turn, 1000); } if (opening_right) { mouse_state = TURNING; turn_direction = RIGHT; action_ticker.attach_us(&turn, 1000); }*/ } void setup() { eRS = 0; eRF = 0; eLS = 0; eLF = 0; mouse_state = FORWARD; turn_direction = INVALID; myled = 1; for ( int i = 0; i < 1000; i++ ) { offsetCalc(); } wait(2); Systicker.attach_us(&systick, 1000); //action_ticker.attach_us(&turn, 1000); //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; wait_us(50); float right_side = rRS.read(); eRS = 0; eLS = 1; wait_us(50); float left_side = rLS.read(); eLS = 0; eRF = 1; wait_us(50); float right_front = rRF.read(); eRF = 0; eLF = 1; wait_us(50); float left_front = rLF.read(); eLF = 0; pc.printf("right side: %f, left side: %f right_front: %f, left_front: %f \r\n", right_side, left_side, right_front, left_front); wait(1); } 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()); }