Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 44:85bf2c0cd518
- Parent:
- 43:f22168a05c3e
- Child:
- 45:8b0bee6baf38
diff -r f22168a05c3e -r 85bf2c0cd518 main.cpp --- a/main.cpp Sun May 28 10:09:56 2017 +0000 +++ b/main.cpp Fri Jun 02 18:50:51 2017 +0000 @@ -38,9 +38,9 @@ */ -#define IP_CONSTANT 2.00 +#define IP_CONSTANT 1.6 #define II_CONSTANT 0.00001 -#define ID_CONSTANT 0.2 +#define ID_CONSTANT 0.24 void pidOnEncoders(); @@ -64,7 +64,7 @@ receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); left_motor.move(speed1); - wait_us(60000); + wait_ms(100); double distance_to_go = (oneCellCount)*num; @@ -143,7 +143,7 @@ right_motor.move(speed0); left_motor.move(speed1); - wait_us(60000); + wait_us(100000); double distance_to_go = (oneCellCount)*num; @@ -515,7 +515,7 @@ left_motor.brake(); right_motor.brake(); - if (encoder0.getPulses() >= 0.5*desiredCount0 && encoder1.getPulses() >= 0.5*desiredCount1) { + if (encoder0.getPulses() >= 0.4*desiredCount0 && encoder1.getPulses() >= 0.4*desiredCount1) { if (currDir % 4 == 0) { mouseY += 1; } else if (currDir % 4 == 1) { @@ -972,6 +972,14 @@ // prevEncoder0Count = encoder0.getPulses(); // prevEncoder1Count = encoder1.getPulses(); + // turnRight(); + // wait_ms(2000); + // turnLeft(); + // wait_ms(2000); + // turnRight(); + // wait_ms(2000); + // turnRight(); + // wait_ms(2000); if (!overrideFloodFill){ nextMovement = chooseNextMovement(); @@ -981,8 +989,9 @@ //redLed.write(1); //greenLed.write(0); //blueLed.write(1); - encoderAfterTurn(1); - // nCellEncoderAndIR(1); + // encoderAfterTurn(1); + moveForwardEncoder(0.4); + nCellEncoderAndIR(0.6); } else if (nextMovement == 1){ justTurned = true; @@ -1000,10 +1009,54 @@ justTurned = true; turn180(); } + }else{ + receiverOneReading = IRP_1.getSamples(100); + receiverTwoReading = IRP_2.getSamples(100); + receiverThreeReading = IRP_3.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); + + if(receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ){ + + int randomNum = rand() %2; + if(randomNum == 0){ + turnRight(); + }else{ + turnLeft(); + } + }else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { + // both walls there + nCellEncoderAndIR(1); + redLed.write(1); + greenLed.write(0); + blueLed.write(1); + }else if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) { + // both sides gone + + redLed.write(1); + greenLed.write(0); + blueLed.write(0); + // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass); + moveForwardEncoder(1); + continue; + } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED + turnRight(); + redLed.write(0); + greenLed.write(1); + blueLed.write(1); + // moveForwardEncoder(valToPass); + // break; + } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone + turnLeft(); + redLed.write(1); + greenLed.write(1); + blueLed.write(0); + // moveForwardEncoder(valToPass); + // break; + } } - wait_ms(700); // reduce this once we fine tune this! - + wait_ms(500); // reduce this once we fine tune this! + //////////////////////// TESTING CODE GOES BELOW ///////////////////////////