hello
Dependencies: AVEncoder QEI mbed-src-AV
main.cpp
- Committer:
- intgsull
- Date:
- 2015-11-20
- Revision:
- 9:e2feaadf504c
- Parent:
- 8:a254346f20aa
File content as of revision 9:e2feaadf504c:
//Micromouse code #include "mbed.h" #include "AVEncoder.h" #include "analogin_api.h" // set things Serial pc(SERIAL_TX, SERIAL_RX); Ticker Systicker; Timer timer; PwmOut motor1_forward(PB_10); PwmOut motor1_reverse(PA_6); PwmOut motor2_forward(PA_7); PwmOut motor2_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 Front IR DigitalOut eRF(PC_12); AnalogIn rRF(PA_4); //Right Side IR DigitalOut eRS(PC_15); AnalogIn rRS(PB_0); DigitalOut myled(LED1); // global variables. volatile float gyro_offset = 0; volatile float line_speed = 1; // currently is in terms of encoder pulses / ms. volatile float angl_speed = 0; // should not turn while moving forward. volatile float line_prevError = 0; volatile int 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 left_speed = 0; volatile float right_speed = 0; // pid constants. these need to be tuned. but i set them as a default val for now. // line refers to the translational speed. // enco and gyro will be used primarily for angular speed. // (we can change the names later, // i added line in after i realized that i already had the angular code) const int max_speed = 6; // max speed is 6 encoder pulses per ms. const float line_propo = 0; const float line_integ = 0; const float line_deriv = 0; const float gyro_propo = 1; const float gyro_integ = 0; const float gyro_deriv = 0; const float enco_propo = 1; const float enco_integ = 0; const float enco_deriv = 0; const float spin_enco_weight = 0.5; const float spin_gyro_weight = 1 - spin_enco_weight; // 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; // helper functions void reset(); void offsetCalc(); void stop(); void moveForward(); void turn(); void irReset(); //irPID function to not crash into stuff 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 leftDistance = 0; volatile float rightDistance= 0; void irPID() { eLS = 1; eRS = 1; if(rLS > leftWall && rRS > rightWall)//walls on both sides { //leftDistance = rLS; // rightDistance = rRS; irError = rRS-rLS-irOffset; //– leftDistance – irOffset; irErrorD = irError-oldirError; //irErrorD -= oldirError;//(irError – oldirError); irErrorI += irError; } else if(rLS > leftWall) //just left wall { leftDistance = rLS; irError = 2*(lirOffset-leftDistance);//(2 * (lirOffset – leftDistance)); irErrorD=irError-oldirError; irErrorI += irError; } else if(rRS > rightWall)//just right wall { rightDistance = rRS; irError=2*(rightDistance-rirOffset); irError = irError-oldirError; irErrorI += irError; } else if(rLS < leftWall && rRS < 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; eLS = 0; eRS = 0; } const float frontWall = 0; //need to calibrate this threshold to a value where mouse can stop in time //something like this may be useful for the very temporal present //doesn't account for current speed/trajectory atm void dontDriveStraightIntoAWall() { eRF = 1; eLF = 1; if(rRF > frontWall || rLF > frontWall) { eRF = 0; eLF = 0; mouse_state = STOPPED; stop(); return; } } // PID Control // this contains the simplistic PID control for the most part. // we do have to calibrate constants though. void systick() { //pc.printf("systick ran\r\n"); if ( mouse_state == STOPPED || mouse_state == UNKNOWN ) { // do nothing? // reset? //reset(); offsetCalc(); return; } //pc.printf("systick ran while state is FORWARD \r\n"); int dt = timer.read_us(); // should be around 1 ms. timer.reset(); //printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); float line_error = line_speed - 0.5 * ( l_enco.getPulses() + r_enco.getPulses()); int enco_error = l_enco.getPulses() - r_enco.getPulses(); float gyro_error = _gyro.read() - gyro_offset; line_accumulator += line_error; enco_accumulator += enco_error; gyro_accumulator += gyro_error; float line_pid = 0; line_pid += line_propo * line_error; line_pid += line_integ * line_accumulator; line_pid += line_deriv * (line_error - line_prevError)/dt; float enco_pid = 0; enco_pid += enco_propo * enco_error; enco_pid += enco_integ * enco_accumulator; enco_pid += enco_deriv * (enco_error - enco_prevError)/dt; float gyro_pid = 0; gyro_pid += gyro_propo * gyro_error; gyro_pid += gyro_integ * gyro_accumulator; gyro_pid += gyro_deriv *(gyro_error - gyro_prevError)/dt; // forward moving pid control. if ( mouse_state == FORWARD ) { float x_error = line_pid; float w_error = 0; //spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid; left_speed += (x_error - w_error); right_speed += (x_error + w_error); //swapped //pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); moveForward(); // offsetCalc(); } if ( mouse_state == TURNING ) { // nothing for now. if we turn in place, we assume no pid control. // this may have to change when we try curve turns. } line_prevError = line_error; enco_prevError = enco_error; gyro_prevError = gyro_error; line_accumulator /= line_decayFactor; enco_accumulator /= enco_decayFactor; gyro_accumulator /= gyro_decayFactor; reset(); } // setup stuff. void setup() { pc.printf("Hello World\r\n"); pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); mouse_state = STOPPED; reset(); eRS = 0; eRF = 0; eLS = 0; eLF = 0; myled = 1; // repeat this for some time frame. for ( int i = 0; i < 200; i++ ) offsetCalc(); Systicker.attach_us(&systick, 1000); } void reset() { l_enco.reset(); r_enco.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; } int main() { setup(); pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); wait(2); mouse_state = FORWARD; for ( int i = 0; i < 10; i++ ) { pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); } stop(); //pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); //while(1) // { // pc.printf("The left motor is going at speed: %d\r\n", left_speed); // pc.printf("The left motor is going at speed: %d\r\n", right_speed); // wait(1); // } } // movement functions. void moveForward() { //mouse_state = FORWARD; if ( left_speed > 0 ) // which should be always. { motor1_forward = left_speed / max_speed; motor1_reverse = 0; } else { motor1_forward = 0; motor1_reverse = -left_speed / max_speed; } if ( right_speed > 0 ) // which again should be always. { motor2_forward = right_speed / max_speed; motor2_reverse = 0; } else { motor2_forward = 0; motor2_reverse = -right_speed / max_speed; } } void stop() { mouse_state = STOPPED; motor1_forward = 1.0; motor1_reverse = 1.0; motor2_forward = 1.0; motor2_reverse = 1.0; } void turn()// maybe split this into two functions? { mouse_state = TURNING; float angle = 0; while (angle < 0.90) { float gyro_val = _gyro.read() - gyro_offset; angle += gyro_val; pc.printf("%f\r\n", angle); motor1_forward = 0.5; motor1_reverse = 0; motor2_forward = 0; motor2_reverse = 0.5; } } void irReset() //maybe split resets { eRF = 0; eRS = 0; eLF = 0; eRF = 0; }