PID Test
Dependencies: AVEncoder mbed-src-AV
main.cpp
- Committer:
- intgsull
- Date:
- 2015-12-01
- Revision:
- 4:112f3d35bd2d
- Parent:
- 3:40333f38771d
- Child:
- 5:f704940c9c7e
File content as of revision 4:112f3d35bd2d:
#include "mbed.h" #include "AVEncoder.h" // set things Serial pc(SERIAL_TX, SERIAL_RX); Ticker Systicker; Timer timer; 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.6; 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 = 10; const float enco_integ = 15;//1; const float enco_deriv = 2000;//.0002; const float spin_enco_weight = 1; const float spin_gyro_weight = 1 - spin_enco_weight; const float frontWall = 0.2; //need to calibrate this threshold to a value where mouse can stop in time //something like this may be useful volatile float enco_error; volatile float enco_pid; volatile float gyro_error; volatile float gyro_pid; volatile float w_error; ////////////////////////////////////ir PID constants//////////////////////////////////////////////// const float leftWall= 0; const float rightWall= 0; //we need to find the threshold value for when //left or right walls are present const float irOffset= 0; // difference between right and left ir sensors when //mouse is in the middle const float lirOffset= 0; //middle value for when there is only a wall on one const float rirOffset= 0; //side of the mouse (left and right) volatile float irError = 0; volatile float irErrorD = 0; volatile float irErrorI = 0; volatile float oldirError = 0; volatile float totalirError= 0; //errors const float irKp = 0; const float irKd = 0; const float irKi = 0; //constants volatile float leftD; volatile float rightD; void irPID() { eLS = 1; leftD = rLS.read(); eLS = 0; eRS = 1; rightD = rRS.read(); eRS = 0; if(leftD > leftWall && rightD > rightWall)//walls on both sides { irError = rightD-leftD-irOffset; irErrorD = irError-oldirError; irErrorI += irError; } else if(leftD > leftWall) //just left wall { irError = 2*(lirOffset-leftD); irErrorD=irError-oldirError; irErrorI += irError; } else if(rightD > rightWall)//just right wall { irError=2*(rightD-rirOffset); irError = irError-oldirError; irErrorI += irError; } else if(leftD < leftWall && rightD < rightWall)//no walls!! Use encoder PID { irError = 0; irErrorD = 0; irErrorI += irError; } totalirError = irError*irKp + irErrorD*irKd + irErrorI*irKi; oldirError = irError; left_speed -= totalirError; right_speed += totalirError; } // 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(); r_enco.reset(); } 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_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 // 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 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; } } void setup() { pc.printf("Hello World\r\n"); eRS = 0; eRF = 0; eLS = 0; eLF = 0; mouse_state = FORWARD; myled = 1; for ( int i = 0; i < 1000; i++ ) { offsetCalc(); } //left_forward = left_speed / left_max_speed; // left_reverse = 0; // right_forward = right_speed / right_max_speed; // right_reverse = 0; wait(2); // repeat this for some time frame. Systicker.attach_us(&systick, 1000); } int main() { setup(); while(1) { // // pc.printf("left pulss %f, right pulses %f \r\n", l_enco.getPulses(), r_enco.getPulses()); // //wait(1); } // pc.printf("left pulses %d, right pulses %d \r\n", l_enco.getPulses(), r_enco.getPulses()); //encoder testing //wait(1); }