Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 20:82836745332e
- Parent:
- 19:7b66a518b6f8
- Child:
- 21:9a6cb07bdcb6
diff -r 7b66a518b6f8 -r 82836745332e main.cpp --- a/main.cpp Mon May 15 00:54:41 2017 +0000 +++ b/main.cpp Wed May 17 02:01:34 2017 +0000 @@ -16,15 +16,15 @@ */ // Constants for when HIGH_PWM_VOLTAGE = 0.1 -#define IP_CONSTANT 8.9 -#define II_CONSTANT 1 -#define ID_CONSTANT 3.1 +#define IP_CONSTANT 8.85 +#define II_CONSTANT 0.005 +#define ID_CONSTANT 3.15 const int desiredCountR = 1400; const int desiredCountL = 1475; const int oneCellCount = 5400; -const int oneCellCountMomentum = 4600; // one cell count is actually approximately 5400, but this value is considering momentum! +const int oneCellCountMomentum = 4700; // one cell count is actually approximately 5400, but this value is considering momentum! float receiverOneReading = 0.0; float receiverTwoReading = 0.0; @@ -78,6 +78,7 @@ right_motor.brake(); left_motor.brake(); turnFlag = 0; // zeroing out the flags! + currDir -= 1; } void turnRight() @@ -98,7 +99,6 @@ double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; - while(1) { if(!(abs(error0) < 1) && !(abs(error1) < 1)) { @@ -125,6 +125,7 @@ right_motor.brake(); left_motor.brake(); turnFlag = 0; + currDir += 1; } void turnLeft180() @@ -171,6 +172,7 @@ right_motor.brake(); left_motor.brake(); + currDir -= 2; } void turnRight180() @@ -216,23 +218,30 @@ } right_motor.brake(); left_motor.brake(); + currDir += 2; } void moveForwardCellEncoder(double cellNum){ int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum; - left_motor.forward(0.11); - right_motor.forward(0.10); + left_motor.forward(0.125); + right_motor.forward(0.095); wait_ms(1); while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ - receiverTwoReading = IRP_2.getSamples(5); - receiverThreeReading = IRP_3.getSamples(5); - if (receiverThreeReading < 0.0007f) + 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/4){ + // redLed.write(1); + // blueLed.write(0); turnFlag |= RIGHT_FLAG; - else if (receiverTwoReading < 0.0009f) + } + else if (receiverTwoReading < IRP_2.sensorAvg/4){ + // redLed.write(0); + // blueLed.write(1); turnFlag |= LEFT_FLAG; - // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1); + } pidOnEncoders(); } @@ -266,7 +275,7 @@ double HIGH_PWM_VOLTAGE = 0.1; double rightSpeed = 0.1; - double leftSpeed = 0.1; + double leftSpeed = 0.12; float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); @@ -279,7 +288,7 @@ } else { blueLed.write(1); } - + sumError += currentError; 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; @@ -305,8 +314,6 @@ ir3 = IRP_3.getSamples( SAMPLE_NUM ); } - //sensor1Turn = IR_Sensor1.read(); - //sensor4Turn = IR_Sensor4.read(); //backward(); wait_ms(40); @@ -369,8 +376,8 @@ count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int diff = count0 - count1; - double kp = 0.000075; - double kd = 0.000135; + double kp = 0.00011; + double kd = 0.00014; int prev = 0; int counter = 0; @@ -385,12 +392,12 @@ 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 - 50 ) // count1 is bigger, right wheel pushed forward + if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward { left_motor.move( -d ); right_motor.move( d ); } - else if( x > diff + 50 ) + else if( x > diff + 40 ) { left_motor.move( -d ); right_motor.move( d ); @@ -407,7 +414,7 @@ } } -void oneCellEncoderAndIR(){ +void oneCellEncoderAndIR(double cellCount){ double currentError = 0; double previousError = 0; double derivError = 0; @@ -417,17 +424,31 @@ double rightSpeed = 0.10; double leftSpeed = 0.10; - int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum; - int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum; + int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount; + int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount; - left_motor.forward(0.11); + left_motor.forward(0.125); right_motor.forward(0.10); + float receiverTwoReading = 0.0; + float receiverThreeReading = 0.0; + float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ - currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; + receiverTwoReading = IRP_2.getSamples(100); + receiverThreeReading = IRP_3.getSamples(100); + if (receiverTwoReading <= IRP_2.sensorAvg/4) + currentError = receiverThreeReading - IRP_3.sensorAvg; + else if (receiverThreeReading <= IRP_3.sensorAvg/4) + currentError = receiverTwoReading - IRP_2.sensorAvg; + // 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 = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg)); + + sumError += currentError; 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 @@ -515,18 +536,18 @@ //left_motor.forward( 0.2 ); while (1) { - oneCellEncoderAndIR(); - // moveForwardCellEncoder(1); - // wait(0.5); - // handleTurns(); - // wait(0.5); - // moveForwardCellEncoder(1); - // wait(0.5); - // handleTurns(); + // oneCellEncoderAndIR(5); + // break; + moveForwardCellEncoder(1); + wait(0.5); + handleTurns(); + wait(0.5); + moveForwardCellEncoder(1); + wait(0.5); + handleTurns(); break; - // moveForwardOneCellEncoder(); //pidOnEncoders(); - //moveForwardUntilWallIr(); + // moveForwardUntilWallIr(); //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) ) ; @@ -535,14 +556,5 @@ //reading = Rec_4.read(); // serial.printf("reading: %f\n", reading); -// redLed.write(0); -// wait_ms(1000); -// redLed.write(1); -// greenLed.write(0); -// wait_ms(1000); -// greenLed.write(1); -// blueLed.write(0); -// wait_ms(1000); -// blueLed.write(1); } }