Mouse code for the MacroRat
Revision 33:68ce1f74ab5f, committed 2017-05-26
- Comitter:
- sahilmgandhi
- Date:
- Fri May 26 06:23:19 2017 +0000
- Parent:
- 32:69acb14778ea
- Child:
- 34:69342782fb68
- Commit message:
- PID working with the new IRs now ... need to tune it a bit though.
Changed in this revision
--- a/irpair.cpp Fri May 26 03:46:03 2017 +0000 +++ b/irpair.cpp Fri May 26 06:23:19 2017 +0000 @@ -1,8 +1,6 @@ #include "irpair.h" #include "mbed.h" -//Ticker toggleIr; - #define dark_samples 10 #define busy_wait 10
--- a/irpair.h Fri May 26 03:46:03 2017 +0000 +++ b/irpair.h Fri May 26 06:23:19 2017 +0000 @@ -13,7 +13,7 @@ calibrateSensor(); } - float getSamples( int i ); + double getSamples( int samples ); float sensorAvg; void calibrateSensor();
--- a/main.cpp Fri May 26 03:46:03 2017 +0000 +++ b/main.cpp Fri May 26 06:23:19 2017 +0000 @@ -12,25 +12,35 @@ #include "stm32f4xx.h" #include "QEI.h" -//IRSAvg= >: 0.143701, 0.128285 +/*LEFT/RIGHT BOTH WALLS +0.34 0.12 + +LEFT/RIGHT LEFT WALL GONE +0.02 0.11 -//facing wall ir2 and ir3 -//0.144562, 0.113971 in maze +LEFT/RIGHT RIGTH WALL GONE +0.33 0.008 -// normal hall ir2 and ir3 -//0.013665, 0.010889 in maze +LEFT/RIGHT BOTH WALLS GONE +0.02 0.008 + +LEFT/RIGHT CLOSE TO RIGHT WALL +0.14 0.47 -//0.014462, 0.009138 -// 0.013888, 0.010530 +LEFT/RIGHT CLOSE TO LEFT WALL +0.89 0.05 +FRONT IRS NEAR WALL (STOPPING DISTANCE) +0.41 0.49 -//no walls ir2 and ir3 -//0.003265, 0.002904 in maze9 +FRONT IRS NO WALL AHEAD (FOR ATLEAST 1 FULL CELL) +0.07 0.06 -//0.003162, 0.003123 -//0.003795, - -//0.005297, 0.007064 +*/ + +#define IP_CONSTANT 1.93 +#define II_CONSTANT 0.00001 +#define ID_CONSTANT 0.175 void pidOnEncoders(); @@ -65,7 +75,6 @@ void moveForwardEncoder(double num) { - int count0; int count1; count0 = encoder0.getPulses(); @@ -77,12 +86,12 @@ double kd = 0.00019; int prev = 0; - double speed0 = 0.10; - double speed1 = 0.12; + double speed0 = 0.11; + double speed1 = 0.13; right_motor.move(speed0); left_motor.move(speed1); - while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) || IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8) { + while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num)) { //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); @@ -304,7 +313,7 @@ count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int diff = count0 - count1; - double kp = 0.00013; + double kp = 0.00016; double kd = 0.00016; int prev = 0; @@ -340,7 +349,6 @@ void nCellEncoderAndIR(double cellCount) { - //serial.printf("I'm inside IR!"); double currentError = 0; double previousError = 0; double derivError = 0; @@ -362,44 +370,41 @@ float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); - // float previr2 = ir2; - // float previr3 = ir3; - int state = 0; - //serial.printf("Enter encoder while loop\n"); - //serial.printf("%d, %d\n", encoder0.getPulses(), encoder1.getPulses() ); while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) { - //serial.printf("Entered encoder while loop\n"); + // serial.printf("The desiredCount0 is: %d \t The desiredCount1 is: %d\t the 0encoderval is :%d\t the 1encoderval is : %d\t\n", desiredCount0, desiredCount1, encoder0.getPulses(), encoder1.getPulses()); receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM ); receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM ); receiverOneReading = IRP_1.getSamples( SAMPLE_NUM ); receiverFourReading = IRP_4.getSamples( SAMPLE_NUM ); - - if( receiverOneReading > IRP_1.sensorAvg * 3 || receiverFourReading > IRP_4.sensorAvg * 3 ) { + // serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg); + if( receiverOneReading > IRP_1.sensorAvg * 3.4 || receiverFourReading > IRP_4.sensorAvg * 3.4) { break; } - if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))) { + if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) { // both sides gone - int prev0 = encoder0.getPulses(); - int prev1 = encoder1.getPulses(); - int diff0 = desiredCount0 - prev0; - int diff1 = desiredCount1 - prev1; - int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); + double prev0 = encoder0.getPulses(); + double prev1 = encoder1.getPulses(); + double diff0 = desiredCount0 - prev0; + double diff1 = desiredCount1 - prev1; + double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); + // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass); moveForwardEncoder(valToPass); - } else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR) { // right wall gone + continue; + } else if (receiverThreeReading < IRP_3.sensorAvg/3.5) { // right wall gone RED state = 1; redLed.write(0); greenLed.write(1); blueLed.write(1); - } else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL) { // left wall gone + } else if (receiverTwoReading < IRP_2.sensorAvg/3.5) { // left wall gone // BLUE BLUE BLUE BLUE state = 2; redLed.write(1); greenLed.write(1); blueLed.write(0); - } else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))) { + } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { // both walls there state = 0; redLed.write(1); @@ -410,19 +415,19 @@ //serial.printf("Entering switch\n"); switch(state) { case(0): { // both walls there - currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + currentError = ( receiverTwoReading - IRP_2.sensorAvg) - ( receiverThreeReading - IRP_3.sensorAvg); break; } case(1): { // RED RED RED RED RED - currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL); + currentError = (receiverTwoReading - IRP_2.sensorAvg) - (IRP_3.sensorAvg*0.001); break; } case(2): { // blue - currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR); + currentError = (IRP_2.sensorAvg*0.001) - (receiverThreeReading - IRP_3.sensorAvg); break; } default: { - currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + currentError = ( receiverTwoReading - IRP_2.sensorAvg) - ( receiverThreeReading - IRP_3.sensorAvg); break; } } @@ -447,7 +452,7 @@ right_motor.forward(rightSpeed); left_motor.forward(leftSpeed); - //pidOnEncoders(); + pidOnEncoders(); previousError = currentError; } @@ -463,12 +468,12 @@ } // the mouse has moved forward, we need to update the wall map now - receiverOneReading = IRP_1.getSamples(50); - receiverTwoReading = IRP_2.getSamples(50); - receiverThreeReading = IRP_3.getSamples(50); - receiverFourReading = IRP_4.getSamples(50); + receiverOneReading = IRP_1.getSamples(75); + receiverTwoReading = IRP_2.getSamples(75); + receiverThreeReading = IRP_3.getSamples(75); + receiverFourReading = IRP_4.getSamples(75); - if (receiverOneReading >= 0.5f && receiverFourReading >= 0.5f) { + if (receiverOneReading >= 0.3f && receiverFourReading >= 0.3f) { if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } else if (currDir % 4 == 1) { @@ -479,7 +484,7 @@ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL; } } - if (receiverThreeReading >= 0.1f) { + if (receiverThreeReading >= IRP_3.sensorAvg*0.6) { if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL; } else if (currDir % 4 == 1) { @@ -490,7 +495,7 @@ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL; } } - if (receiverTwoReading >= 0.1f) { + if (receiverTwoReading >= IRP_2.sensorAvg*0.6) { if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL; } else if (currDir % 4 == 1) { @@ -508,20 +513,16 @@ right_motor.brake(); } -bool isWallInFront(int x, int y) -{ +bool isWallInFront(int x, int y){ return (wallArray[MAZE_LEN - y - 1][x] & F_WALL); } -bool isWallInBack(int x, int y) -{ +bool isWallInBack(int x, int y){ return (wallArray[MAZE_LEN - y - 1][x] & B_WALL); } -bool isWallOnRight(int x, int y) -{ +bool isWallOnRight(int x, int y){ return (wallArray[MAZE_LEN - y - 1][x] & R_WALL); } -bool isWallOnLeft(int x, int y) -{ +bool isWallOnLeft(int x, int y){ return (wallArray[MAZE_LEN - y - 1][x] & L_WALL); } @@ -741,8 +742,7 @@ return dirToGo; } -bool hasVisited(int x, int y) -{ +bool hasVisited(int x, int y){ return visitedCells[MAZE_LEN - 1 - y][x]; } @@ -842,7 +842,6 @@ // CR1 reenabales // then read CNT reg of timer - dipButton1.rise(&enableButton1); dipButton2.rise(&enableButton2); dipButton3.rise(&enableButton3); @@ -853,20 +852,6 @@ dipButton3.fall(&disableButton3); dipButton4.fall(&disableButton4); - // if(dipFlags == 0x1){ - // }else{ - // turnRight(); - // IRP_1.calibrateSensor(); - // IRP_4.calibrateSensor(); - // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL; - // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL; - - // wait_ms(300); - // turnLeft(); - // wait_ms(300); - // } - - // init the wall, and mouse loc arrays: wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE; visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1; @@ -877,13 +862,11 @@ //int currEncoder1Count = 0; //bool overrideFloodFill = false; - //right_motor.forward( 0.2 ); - //left_motor.forward( 0.2 ); + right_motor.forward( 0.2 ); + left_motor.forward( 0.2 ); //turnRight180(); //wait_ms(1500); //int nextMovement = 0; - //serial.printf("I'm about to enter!"); - nCellEncoderAndIR(3); while (1) { // prevEncoder0Count = encoder0.getPulses(); // prevEncoder1Count = encoder1.getPulses(); @@ -929,112 +912,34 @@ // // wait_ms(300); // reduce this once we fine tune this! - //wait_ms(1500); - //turnRight(); - //wait_ms(1500); - //turnLeft(); - // nCellEncoderAndIR(1); - // wait_ms(500); - // turnRight(); - // wait_ms(500); - // nCellEncoderAndIR(1); - // wait_ms(500); - // turnRight(); - // wait_ms(500); - // nCellEncoderAndIR(1); - // wait_ms(500); - // turnLeft(); - // wait_ms(500); - // nCellEncoderAndIR(2); - // wait_ms(500); - // turnRight(); - // wait_ms(500); - // nCellEncoderAndIR(1); - // wait_ms(500); - // turnRight(); - // wait_ms(500); - // nCellEncoderAndIR(5); - // break; - // turnRight180(); - // int number = rand() % 4 + 1; - // switch(number){ - // case(1):{//turn right - // turnRight(); - // break; - // } - // case(2):{ // turn left - // turnLeft(); - // break; - // } - // case(3):{// keep going - - // break; - // } - // case(4):{// turnaround - // turnRight180(); - // break; - // } - // default:{// keep going - // break; - // } - // } - - // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL; - // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR; - - // float ir3 = IRP_2.getSamples(100)/initAverageL; - // float ir2 = IRP_3.getSamples(100)/initAverageR; - - - - /* - counter2++; - counter3++; - ir2tot += IRP_2.getSamples(100); - ir3tot += IRP_3.getSamples(100); - - - ir2 = ir2tot/counter2; - ir3 = ir3tot/counter3; - - - serial.printf("IRS= >: %f, %f \r\n", ir2, ir3); - */ - //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR); - //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3); - //break; - //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100)); - //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3); - //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg); - - - //////////////////////////////////////////////////////////////// - - //nCellEncoderAndIR(4); - //while(1); - //break; - - //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100)); - - //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); - - //break; - // moveForwardCellEncoder(1); - // wait(0.5); - // handleTurns(); - // wait(0.5); - // moveForwardCellEncoder(1); - // wait(0.5); - // handleTurns(); - //break; - //pidOnEncoders(); - // moveForwardUntilWallIr(); - //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); - //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; - //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); - - //reading = Rec_4.read(); -// serial.printf("reading: %f\n", reading); + //////////////////////// TESTING CODE GOES BELOW /////////////////////////// + nCellEncoderAndIR(4); + wait_ms(200); + turnRight(); + wait_ms(200); + nCellEncoderAndIR(3); + wait_ms(200); + turnRight(); + wait_ms(200); + nCellEncoderAndIR(2); + wait_ms(200); + turnRight(); + wait_ms(200); + nCellEncoderAndIR(1); + wait_ms(200); + turnLeft(); + wait_ms(200); + nCellEncoderAndIR(1); + wait_ms(200); + turnLeft(); + wait_ms(200); + nCellEncoderAndIR(1); + wait_ms(200); + turnRight180(); + break; + // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() ); + // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; + // serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); } } \ No newline at end of file
--- a/main.h Fri May 26 03:46:03 2017 +0000 +++ b/main.h Fri May 26 06:23:19 2017 +0000 @@ -170,22 +170,18 @@ //#define IP_CONSTANT 8.2 //#define II_CONSTANT 0.06 //#define ID_CONSTANT 7.55 - -#define IP_CONSTANT 0.25 -#define II_CONSTANT 0 -#define ID_CONSTANT 0.00 -const int desiredCount180 = 2870; +const int desiredCount180 = 3000; // change accordingly to the terrain const int desiredCountR = 1700; const int desiredCountL = 1700; const int oneCellCount = 5400; -const int oneCellCountMomentum = 4550;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! +const int oneCellCountMomentum = 4570;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! -float receiverOneReading = 0.0; -float receiverTwoReading = 0.0; -float receiverThreeReading = 0.0; -float receiverFourReading = 0.0; +double receiverOneReading = 0.0; +double receiverTwoReading = 0.0; +double receiverThreeReading = 0.0; +double receiverFourReading = 0.0; float ir1base = 0.0; float ir2base = 0.0; @@ -206,7 +202,7 @@ inline void turnLeft() { double speed0 = 0.11; - double speed1 = -0.13; + double speed1 = -0.12; // change back to 0.13 if turns stop working, testing something out! int counter = 0; int initial0 = encoder0.getPulses(); @@ -254,7 +250,7 @@ inline void turnRight() { double speed0 = -0.11; - double speed1 = 0.13; + double speed1 = 0.12; // change back to 0.13 if turns stop working, testing something out! int counter = 0; int initial0 = encoder0.getPulses(); @@ -297,57 +293,10 @@ turnFlag = 0; currDir += 1; } - -inline void turnLeft180() -{ - double speed0 = 0.15; - double speed1 = -0.15; - - int counter = 0; - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 - desiredCountL*2; - int desiredCount1 = initial1 + desiredCountL*2; - - int count0 = initial0; - int count1 = initial1; - - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; - - - while(1) { - - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); - - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; - - right_motor.move(speed0); - left_motor.move(speed1); - counter = 0; - } else { - counter++; - right_motor.brake(); - left_motor.brake(); - } - - if (counter > 60) { - break; - } - } - - right_motor.brake(); - left_motor.brake(); - currDir -= 2; -} - + inline void turnRight180() { - double speed0 = -0.16; + double speed0 = -0.15; double speed1 = 0.16; int counter = 0;