Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 28:8126a4d620e8
- Parent:
- 27:02ce1040f331
- Child:
- 29:ec2c5a69acd6
diff -r 02ce1040f331 -r 8126a4d620e8 main.cpp --- a/main.cpp Sun May 21 10:30:19 2017 +0000 +++ b/main.cpp Sun May 21 13:04:21 2017 +0000 @@ -22,16 +22,16 @@ // #define IP_CONSTANT 8.85 // #define II_CONSTANT 0.005 // #define ID_CONSTANT 3.15 -#define IP_CONSTANT 8.5 -#define II_CONSTANT 0.095 -#define ID_CONSTANT 6.85 +#define IP_CONSTANT 8.15 +#define II_CONSTANT 0.06 +#define ID_CONSTANT 7.5 -const int desiredCount180 = 2800; +const int desiredCount180 = 2700; const int desiredCountR = 1575; const int desiredCountL = 1650; const int oneCellCount = 5400; -const int oneCellCountMomentum = 4700;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! +const int oneCellCountMomentum = 4450;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! float receiverOneReading = 0.0; float receiverTwoReading = 0.0; @@ -45,11 +45,11 @@ float ir4base = 0.0; -float initAverageL = 8.28; -float averageDivL = 29.5; //blue +float initAverageL = 8.25; +float averageDivL = 27.8; //blue float initAverageR = 8.75; //4.5 -float averageDivR = 29.5; //red -float averageDivUpper = 0.9; +float averageDivR = 28.5; //red +float averageDivUpper = 0.5; //IRSAvg= >: 0.143701, 0.128285 @@ -539,8 +539,8 @@ count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int diff = count0 - count1; - double kp = 0.00011; - double kd = 0.00014; + double kp = 0.00013; + double kd = 0.00016; int prev = 0; int counter = 0; @@ -555,12 +555,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 - 40 ) // count1 is bigger, right wheel pushed forward + if( x < diff - 60 ) // count1 is bigger, right wheel pushed forward { left_motor.move( -d ); right_motor.move( d ); } - else if( x > diff + 40 ) + else if( x > diff + 60 ) { left_motor.move( -d ); right_motor.move( d ); @@ -572,7 +572,7 @@ // } prev = x; counter++; - if (counter == 5) + if (counter == 10) break; } } @@ -583,18 +583,15 @@ double derivError = 0; double sumError = 0; - double HIGH_PWM_VOLTAGE = 0.1; - double rightSpeed = 0.10; - double leftSpeed = 0.10; + double HIGH_PWM_VOLTAGE = 0.12; + double rightSpeed = 0.12; + double leftSpeed = 0.12; int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount; - left_motor.forward(0.28); - right_motor.forward(0.25); - - float receiverTwoReading = 0.0; - float receiverThreeReading = 0.0; + left_motor.forward(0.12); + right_motor.forward(0.10); float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); @@ -605,10 +602,10 @@ int state = 0; while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ - receiverTwoReading = IRP_2.getSamples(60); - receiverThreeReading = IRP_3.getSamples(60); - receiverOneReading = IRP_1.getSamples(60); - receiverFourReading = IRP_4.getSamples(60); + receiverTwoReading = IRP_2.getSamples(50); + receiverThreeReading = IRP_3.getSamples(50); + receiverOneReading = IRP_1.getSamples(50); + receiverFourReading = IRP_4.getSamples(50); if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){ if (currDir % 4 == 0){ @@ -626,28 +623,8 @@ break; } - if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL))){ + if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))){ // both sides gone - redLed.write(1); - greenLed.write(1); - blueLed.write(1); - wait_ms(100); - redLed.write(1); - greenLed.write(1); - blueLed.write(0); - wait_ms(200); - redLed.write(1); - greenLed.write(1); - blueLed.write(0); - wait_ms(200); - redLed.write(1); - greenLed.write(1); - blueLed.write(0); - wait_ms(200); - redLed.write(1); - greenLed.write(1); - blueLed.write(0); - int prev0 = encoder0.getPulses(); int prev1 = encoder1.getPulses(); int diff0 = desiredCount0 - prev0; @@ -655,7 +632,7 @@ int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); moveForwardEncoder(valToPass); } - else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone + else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR){// right wall gone // if (currDir % 4 == 0){ // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL; // } @@ -673,7 +650,7 @@ redLed.write(0); greenLed.write(1); blueLed.write(1); - }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone + }else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL){// left wall gone // if (currDir % 4 == 0){ // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL; // } @@ -692,22 +669,22 @@ greenLed.write(1); blueLed.write(0); }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){ - if (currDir % 4 == 0){ - wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL; - wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL; - } - else if (currDir % 4 == 1){ - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL; - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL; - } - else if (currDir % 4 == 2){ - wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL; - wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL; - } - else if (currDir % 4 == 3){ - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL; - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL; - } + // if (currDir % 4 == 0){ + // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL; + // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL; + // } + // else if (currDir % 4 == 1){ + // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL; + // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL; + // } + // else if (currDir % 4 == 2){ + // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL; + // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL; + // } + // else if (currDir % 4 == 3){ + // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL; + // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL; + // } // both walls there state = 0; redLed.write(1); @@ -730,7 +707,6 @@ } default:{ currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); - //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); break; } } @@ -755,25 +731,24 @@ pidOnEncoders(); previousError = currentError; - ir2 = IRP_2.getSamples( SAMPLE_NUM ); - ir3 = IRP_3.getSamples( SAMPLE_NUM ); } - if (currDir % 4 == 0){ + if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1){ + if (currDir % 4 == 0){ mouseY += 1; - } - else if (currDir % 4 == 1){ - mouseX + 1; - } - else if (currDir % 4 == 2){ - mouseY -= 1; - } - else if (currDir % 4 == 3){ - mouseX -= 1; + } + else if (currDir % 4 == 1){ + mouseX + 1; + } + else if (currDir % 4 == 2){ + mouseY -= 1; + } + else if (currDir % 4 == 3){ + mouseX -= 1; + } } left_motor.brake(); right_motor.brake(); - return; } bool isWallInFront(int x, int y){ @@ -1132,31 +1107,60 @@ 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; + + bool overrideFloodFill = false; //right_motor.forward( 0.2 ); //left_motor.forward( 0.2 ); //turnRight180(); //wait_ms(1500); int nextMovement = 0; while (1) { - nextMovement = chooseNextMovement(); - if (nextMovement == 0){ - nCellEncoderAndIR(1); - } - else if (nextMovement == 1){ - justTurned = true; - turnRight(); + prevEncoder0Count = encoder0.getPulses(); + prevEncoder1Count = encoder1.getPulses(); + + if (!overrideFloodFill){ + nextMovement = chooseNextMovement(); + if (nextMovement == 0){ + nCellEncoderAndIR(1); + } + else if (nextMovement == 1){ + justTurned = true; + turnRight(); + } + else if (nextMovement == 2){ + justTurned = true; + turnLeft(); + } + else if (nextMovement == 3){ + nCellEncoderAndIR(1); + } + else if (nextMovement == 4){ + justTurned = true; + turnRight180(); + } } - else if (nextMovement == 2){ - justTurned = true; - turnLeft(); + else{ + overrideFloodFill = false; + if ((rand()%2 + 1) == 1){ + justTurned = true; + turnLeft(); + } + else{ + justTurned = true; + turnRight(); + } } - else if (nextMovement == 3){ - nCellEncoderAndIR(1); + currEncoder0Count = encoder0.getPulses(); + currEncoder1Count = encoder1.getPulses(); + + if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){ + overrideFloodFill = true; } - else if (nextMovement == 4){ - justTurned = true; - turnRight180(); - } + wait_ms(1000); // reduce this once we fine tune this! //wait_ms(1500);