Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 32:69acb14778ea
- Parent:
- 31:9b71b44e0867
- Child:
- 33:68ce1f74ab5f
diff -r 9b71b44e0867 -r 69acb14778ea main.cpp --- a/main.cpp Wed May 24 21:47:21 2017 +0000 +++ b/main.cpp Fri May 26 03:46:03 2017 +0000 @@ -136,8 +136,8 @@ right_motor.move(speed0); left_motor.move(speed1); - float ir1 = IRP_1.getSamples(50); - float ir4 = IRP_4.getSamples(50); + double ir1 = IRP_1.getSamples(50); + double ir4 = IRP_4.getSamples(50); if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4) { return; @@ -340,6 +340,7 @@ void nCellEncoderAndIR(double cellCount) { + //serial.printf("I'm inside IR!"); double currentError = 0; double previousError = 0; double derivError = 0; @@ -353,8 +354,9 @@ int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount; + serial.printf("%d, %d\n", desiredCount0, desiredCount1 ); - left_motor.forward(0.16); + left_motor.forward(0.15); right_motor.forward(0.15); float ir2 = IRP_2.getSamples( SAMPLE_NUM ); @@ -365,13 +367,16 @@ 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) { - receiverTwoReading = IRP_2.getSamples(100); - receiverThreeReading = IRP_3.getSamples(100); - receiverOneReading = IRP_1.getSamples(100); - receiverFourReading = IRP_4.getSamples(100); + //serial.printf("Entered encoder while loop\n"); + 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*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ) { + if( receiverOneReading > IRP_1.sensorAvg * 3 || receiverFourReading > IRP_4.sensorAvg * 3 ) { break; } @@ -402,6 +407,7 @@ blueLed.write(1); } + //serial.printf("Entering switch\n"); switch(state) { case(0): { // both walls there currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); @@ -420,6 +426,7 @@ break; } } + //serial.printf("Exiting switch"); sumError += currentError; derivError = currentError - previousError; @@ -431,6 +438,8 @@ rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R); leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L); } + + //serial.printf("%f, %f\n", leftSpeed, rightSpeed); if (leftSpeed > 0.30) leftSpeed = 0.30; if (leftSpeed < 0) leftSpeed = 0; if (rightSpeed > 0.30) rightSpeed = 0.30; @@ -438,7 +447,7 @@ right_motor.forward(rightSpeed); left_motor.forward(leftSpeed); - pidOnEncoders(); + //pidOnEncoders(); previousError = currentError; } @@ -446,7 +455,7 @@ if (currDir % 4 == 0) { mouseY += 1; } else if (currDir % 4 == 1) { - mouseX + 1; + mouseX += 1; } else if (currDir % 4 == 2) { mouseY -= 1; } else if (currDir % 4 == 3) { @@ -494,6 +503,7 @@ } } + //serial.printf("What?\n"); left_motor.brake(); right_motor.brake(); } @@ -522,7 +532,7 @@ goingToCenter = false; changeManhattanDistance(goingToCenter); } else if (!goingToCenter && (currentDistance == 0)) { - goingToCenter == true; + goingToCenter = true; changeManhattanDistance(goingToCenter); } @@ -861,17 +871,19 @@ wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE; visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1; - int prevEncoder0Count = 0; - int prevEncoder1Count = 0; - int currEncoder0Count = 0; - int currEncoder1Count = 0; + //int prevEncoder0Count = 0; + //int prevEncoder1Count = 0; + //int currEncoder0Count = 0; + //int currEncoder1Count = 0; - bool overrideFloodFill = false; + //bool overrideFloodFill = false; //right_motor.forward( 0.2 ); //left_motor.forward( 0.2 ); //turnRight180(); //wait_ms(1500); - int nextMovement = 0; + //int nextMovement = 0; + //serial.printf("I'm about to enter!"); + nCellEncoderAndIR(3); while (1) { // prevEncoder0Count = encoder0.getPulses(); // prevEncoder1Count = encoder1.getPulses(); @@ -999,7 +1011,8 @@ //////////////////////////////////////////////////////////////// - //nCellEncoderAndIR(3); + //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)); @@ -1018,8 +1031,8 @@ //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 ); + //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);