Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 17:f713758f6238
- Parent:
- 16:d9252437bd92
- Child:
- 18:6a4db94011d3
--- a/main.cpp Sun May 14 20:58:55 2017 +0000 +++ b/main.cpp Sun May 14 23:07:23 2017 +0000 @@ -26,6 +26,11 @@ const int oneCellCount = 5400; const int oneCellCountMomentum = 4600; // 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; + void pidOnEncoders(); void turnLeft() @@ -72,6 +77,7 @@ right_motor.brake(); left_motor.brake(); + turnFlag = 0; // zeroing out the flags! } void turnRight() @@ -118,6 +124,7 @@ right_motor.brake(); left_motor.brake(); + turnFlag = 0; } void turnLeft180() @@ -211,14 +218,20 @@ left_motor.brake(); } -void moveForwardOneCellEncoder(){ - int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum; - int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum; +void moveForwardCellEncoder(double cellNum){ + int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum; + int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum; left_motor.forward(0.12); right_motor.forward(0.10); wait_ms(2); while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ + receiverTwoReading = IRP_2.getSamples(20); + receiverThreeReading = IRP_3.getSamples(20); + if (receiverTwoReading <= IRP_2.sensorAvg/2.5) + turnFlag |= LEFT_FLAG; + else if (receiverThreeReading <= IRP_3.sensorAvg/2.5) + turnFlag |= RIGHT_FLAG; // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1); pidOnEncoders(); } @@ -227,6 +240,21 @@ right_motor.brake(); } +void handleTurns(){ + if (turnFlag == 0x1){ + moveForwardCellEncoder(0.5); + turnLeft(); + } + else if (turnFlag == 0x2){ + moveForwardCellEncoder(0.5); + turnRight(); + } + else if (turnFlag == 0x3){ + moveForwardCellEncoder(0.5); + turnLeft(); + } +} + void moveForwardUntilWallIr() { double currentError = 0; @@ -251,7 +279,6 @@ blueLed.write(1); } - currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; derivError = currentError - previousError; double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; @@ -379,6 +406,10 @@ } } +void oneCellEncoderAndIR(){ + +} + int main() { //Set highest bandwidth. @@ -439,21 +470,26 @@ //left_motor.forward( 0.2 ); while (1) { - moveForwardOneCellEncoder(); + moveForwardCellEncoder(1); + wait(0.3); + handleTurns(); + wait_ms(5); + moveForwardCellEncoder(1); + wait(0.3); + handleTurns(); + break; + // moveForwardOneCellEncoder(); //pidOnEncoders(); //moveForwardUntilWallIr(); // wait(2); // turnRight(); // wait(1); // moveForwardUntilWallIr(); - - break; //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ()); //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);