Mouse code for the MacroRat
Revision 23:690b0ca34ee9, committed 2017-05-20
- Comitter:
- christine222
- Date:
- Sat May 20 09:11:08 2017 +0000
- Parent:
- 22:681190ff98f0
- Child:
- 24:e7063765d6f0
- Commit message:
- ncellencoderirwall function working for all walls (both, 1 side, no sides) need to tune detecting the sides though, there's a very precise threshold that we need to find
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri May 19 01:15:36 2017 +0000 +++ b/main.cpp Sat May 20 09:11:08 2017 +0000 @@ -19,12 +19,13 @@ // #define IP_CONSTANT 8.85 // #define II_CONSTANT 0.005 // #define ID_CONSTANT 3.15 -#define IP_CONSTANT 6.85 +#define IP_CONSTANT 13.5 #define II_CONSTANT 0.095 -#define ID_CONSTANT 15.85 +#define ID_CONSTANT 8.85 -const int desiredCountR = 1400; -const int desiredCountL = 1475; +const int desiredCount180 = 2900; +const int desiredCountR = 1500; +const int desiredCountL = 1575; const int oneCellCount = 5400; const int oneCellCountMomentum = 4800; // one cell count is actually approximately 5400, but this value is considering momentum! @@ -34,16 +35,50 @@ float receiverThreeReading = 0.0; float receiverFourReading = 0.0; -float averageDiv = 170; -float initAverage = 4; +//IRSAvg= >: 0.143701, 0.128285 + + + + + + + + //facing wall ir2 and ir3 + //0.144562, 0.113971 in maze + + // normal hall ir2 and ir3 + //0.013665, 0.010889 in maze + + //0.014462, 0.009138 + // 0.013888, 0.010530 + + + //no walls ir2 and ir3 + //0.003265, 0.002904 in maze9 + + //0.003162, 0.003123 + //0.003795, + +//0.005297, 0.007064 + + + + +float averageDivL = 23.0; //blue +float initAverageL = 10.35; + +float averageDivR = 24; //red +float initAverageR = 12.82; //4.5 + +float averageDivUpper = 0.9; void pidOnEncoders(); void turnLeft() { - double speed0 = 0.15; - double speed1 = -0.15; + double speed0 = 0.11; + double speed1 = -0.13; int counter = 0; int initial0 = encoder0.getPulses(); @@ -90,8 +125,8 @@ void turnRight() { - double speed0 = -0.15; - double speed1 = 0.15; + double speed0 = -0.11; + double speed1 = 0.13; int counter = 0; int initial0 = encoder0.getPulses(); @@ -184,15 +219,15 @@ void turnRight180() { - double speed0 = -0.15; - double speed1 = 0.15; + double speed0 = -0.16; + double speed1 = 0.16; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - int desiredCount0 = initial0 + desiredCountR*2; - int desiredCount1 = initial1 - desiredCountR*2; + int desiredCount0 = initial0 + desiredCount180; + int desiredCount1 = initial1 - desiredCount180; int count0 = initial0; int count1 = initial1; @@ -239,12 +274,12 @@ receiverTwoReading = IRP_2.getSamples(100); receiverThreeReading = IRP_3.getSamples(100); // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading); - if (receiverThreeReading < IRP_3.sensorAvg/averageDiv){ + if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){ // redLed.write(1); // blueLed.write(0); turnFlag |= RIGHT_FLAG; } - else if (receiverTwoReading < IRP_2.sensorAvg/averageDiv){ + else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){ // redLed.write(0); // blueLed.write(1); turnFlag |= LEFT_FLAG; @@ -334,14 +369,14 @@ - double speed0 = 0.1; - double speed1 = 0.13; + double speed0 = 0.10; + double speed1 = 0.12; right_motor.move(speed0); left_motor.move(speed1); - while((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005) && ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.25) ) { - + while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { + //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 )); count0 = encoder0.getPulses() - initial0; @@ -372,9 +407,69 @@ } //pidOnEncoders(); - pidBrake(); - //right_motor.brake(); - //left_motor.brake(); + //pidBrake(); + right_motor.brake(); + left_motor.brake(); + return; +} + + +void moveForwardWallEncoder(){ + + int count0; + int count1; + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + int initial1 = count1; + int initial0 = count0; + int diff = count0 - count1; + double kp = 0.00015; + double kd = 0.00019; + int prev = 0; + + + + double speed0 = 0.10; + double speed1 = 0.12; + right_motor.move(speed0); + left_motor.move(speed1); + + + //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { + 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 )); + + count0 = encoder0.getPulses() - initial0; + count1 = encoder1.getPulses() - initial1; + int x = count0 - count1; + //double d = kp * x + kd * ( x - prev ); + double kppart = kp * x; + double kdpart = kd * (x-prev); + double d = kppart + kdpart; + + //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); + if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward + { + left_motor.move( speed1-0.8*d ); + right_motor.move( speed0+d ); + } + else if( x > diff + 40 ) + { + left_motor.move( speed1-0.8*d ); + right_motor.move( speed0+d ); + } + // else + // { + // left_motor.brake(); + // right_motor.brake(); + // } + prev = x; + } + + //pidOnEncoders(); + //pidBrake(); + right_motor.brake(); + left_motor.brake(); return; } @@ -397,7 +492,7 @@ while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) { - //moveForwardEncoder(); + //moveForwardWallEncoder(); }else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone //moveForwardRightWall(); }else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone @@ -413,7 +508,7 @@ blueLed.write(1); } sumError += currentError; - currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverage) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverage) ) ; + currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ; derivError = currentError - previousError; double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down @@ -552,8 +647,8 @@ int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount; - left_motor.forward(0.125); - right_motor.forward(0.10); + left_motor.forward(0.17); + right_motor.forward(0.15); float receiverTwoReading = 0.0; float receiverThreeReading = 0.0; @@ -561,26 +656,72 @@ float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); - float prevRec2 = 0.0; - float prevRec3 = 0.0; - + int state = 0; + + while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ receiverTwoReading = IRP_2.getSamples(100); receiverThreeReading = IRP_3.getSamples(100); - if (receiverTwoReading <= IRP_2.sensorAvg/averageDiv) - currentError = prevRec2 -(receiverThreeReading - IRP_3.sensorAvg/initAverage); - else if (receiverThreeReading <= IRP_3.sensorAvg/averageDiv) - currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverage) - prevRec3; - // else if (receiverTwoReading <= IRP_2.sensorAvg/2 && receiverThreeReading <= IRP_3.sensorAvg/2) // scenario when both left and right wall missing, use encoders only - // moveForwardCellEncoder(2); - else{ - currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverage) - ( receiverThreeReading - IRP_3.sensorAvg/initAverage); - prevRec2 = receiverTwoReading - IRP_2.sensorAvg/initAverage; - prevRec3 = receiverThreeReading - IRP_3.sensorAvg/initAverage; + + if ((IRP_1.getSamples(100) > IRP_1.sensorAvg*0.1) || (IRP_4.getSamples(100) > IRP_4.sensorAvg*0.1) ){ + // almost to the end + redLed.write(1); + greenLed.write(1); + blueLed.write(1); + moveForwardWallEncoder(); + break; + + }else if( (receiverThreeReading < IRP_3.sensorAvg/(averageDivR*0.8)) && (receiverTwoReading < IRP_2.sensorAvg/(averageDivL*0.8)) ){ + // both sides gone + redLed.write(1); + greenLed.write(1); + blueLed.write(1); + moveForwardEncoder(); + }else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone + // RED RED RED RED RED + state = 1; + redLed.write(0); + greenLed.write(1); + blueLed.write(1); + }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// 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))){ + // both walls there + state = 0; + redLed.write(1); + greenLed.write(0); + blueLed.write(1); } + + switch(state){ + case(0):{ // both walls there + currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + break; + } + case(1):{// RED RED RED RED RED + currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (0.6*IRP_2.sensorAvg/initAverageL); + break; + } + case(2):{// blue + currentError = (0.8*IRP_3.sensorAvg/initAverageL) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR); + break; + } + default:{ + currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + break; + } + } + + + sumError += currentError; derivError = currentError - previousError; @@ -604,20 +745,22 @@ previousError = currentError; ir2 = IRP_2.getSamples( SAMPLE_NUM ); ir3 = IRP_3.getSamples( SAMPLE_NUM ); + } left_motor.brake(); right_motor.brake(); + return; } int main() { //Set highest bandwidth. - gyro.setLpBandwidth(LPFBW_42HZ); + //gyro.setLpBandwidth(LPFBW_42HZ); serial.baud(9600); - serial.printf("The gyro's address is %s", gyro.getWhoAmI()); + //serial.printf("The gyro's address is %s", gyro.getWhoAmI()); wait (0.1); @@ -671,19 +814,98 @@ //right_motor.forward( 0.2 ); //left_motor.forward( 0.2 ); turnRight180(); - wait_ms(60); + wait_ms(1500); while (1) { - + //wait_ms(1500); + //turnRight(); + //wait_ms(1500); + //turnLeft(); + nCellEncoderAndIR(4); + wait_ms(2000); + // 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; - nCellEncoderAndIR(3); - break; + + + + /* + counter2++; + counter3++; + ir2tot += IRP_2.getSamples(100); + ir3tot += IRP_3.getSamples(100); + - // 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)); + 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(3); //break; - //moveForwardEncoder(); - //serial.printf("ded \n"); - //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 ));