hello
Dependencies: AVEncoder QEI mbed-src-AV
Diff: main.cpp
- Revision:
- 6:32d9b855b90f
- Parent:
- 5:f9837617817b
- Child:
- 7:f1bceb2bab70
diff -r f9837617817b -r 32d9b855b90f main.cpp --- a/main.cpp Sat Nov 14 01:12:47 2015 +0000 +++ b/main.cpp Wed Nov 18 06:49:06 2015 +0000 @@ -1,6 +1,7 @@ //Micromouse code #include "mbed.h" #include "AVEncoder.h" +#include "analogin_api.h" // set things Serial pc(SERIAL_TX, SERIAL_RX); @@ -50,24 +51,23 @@ volatile float gyro_prevError = 0; volatile float line_accumulator = 0; -volatile float line_decayFactor = 1.5; +volatile float line_decayFactor = 1; volatile float enco_accumulator = 0; -volatile float enco_decayFactor = 1.5; +volatile float enco_decayFactor = 1; volatile float gyro_accumulator = 0; -volatile float gyro_decayFactor = 1.5; +volatile float gyro_decayFactor = 1; volatile float left_speed = 0; volatile float right_speed = 0; -volatile unsigned long l_pulses = 0; -volatile unsigned long r_pulses = 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 float line_propo = 1; +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; @@ -92,7 +92,7 @@ TURNING, UNKNOWN } STATE; -STATE mouse_state; +volatile STATE mouse_state; // helper functions void reset(); @@ -101,15 +101,87 @@ void moveForward(); void turn(); -// interrupt stuff. -void incLENC() -{ - l_pulses++; + +//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 = rightDistance; //– leftDistance – irOffset; + irError -= leftDistance; + irError -= irOffset; + + irErrorD = irError; + irErrorD -= oldirError;//(irError – oldirError); + + irErrorI += irError; + } + else if(rLS > leftWall) //just left wall + { + leftDistance = rLS; + irError = lirOffset;//(2 * (lirOffset – leftDistance)); + + irErrorD = (irError – oldirError); + irErrorI += irError; + } + else if(rRS > rightWall)//just right wall + { + rightDistance = rRS(); + irError = (2 * (rightDistance – rirOffset)); + irErrorD = (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; } -void incRENC() +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() { - r_pulses++; + eRF = 1; + eLF = 1; + if(rRF > frontWall || rLF > frontWall) + { + eRF = 0; + eLF = 0; + mouse_state = STOPPED; + stop(); + return; + } } // PID Control @@ -117,21 +189,23 @@ // we do have to calibrate constants though. void systick() { - pc.printf("systick ran\r\n"); + //pc.printf("systick ran\r\n"); if ( mouse_state == STOPPED || mouse_state == UNKNOWN ) { // do nothing? // reset? - reset(); + //reset(); offsetCalc(); return; } - pc.printf("systick ran while state is FORWARD \r\n"); + //pc.printf("systick ran while state is FORWARD \r\n"); int dt = timer.read_us(); // should be around 1 ms. timer.reset(); - float line_error = line_speed * dt - 0.5 * ( l_enco.getPulses() - r_enco.getPulses()); + //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; @@ -152,17 +226,17 @@ 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; + 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 = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid; - left_speed += x_error + w_error; - right_speed += x_error - w_error; + 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); + //pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); moveForward(); // offsetCalc(); @@ -188,8 +262,12 @@ 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; @@ -228,12 +306,19 @@ int main() { setup(); - mouse_state = FORWARD; + pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); + + wait(2); - wait(1.5); + 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("DONE\r\n"); + //pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses()); //while(1) // { @@ -247,28 +332,28 @@ // movement functions. void moveForward() { - mouse_state = FORWARD; + //mouse_state = FORWARD; if ( left_speed > 0 ) // which should be always. { - motor1_forward = left_speed; + motor1_forward = left_speed / max_speed; motor1_reverse = 0; } else { motor1_forward = 0; - motor1_reverse = -left_speed; + motor1_reverse = -left_speed / max_speed; } if ( right_speed > 0 ) // which again should be always. { - motor2_forward = right_speed; + motor2_forward = right_speed / max_speed; motor2_reverse = 0; } else { motor2_forward = 0; - motor2_reverse = -right_speed; + motor2_reverse = -right_speed / max_speed; } }