Mouse code for the MacroRat
Revision 40:465d2b565977, committed 2017-05-28
- Comitter:
- sahilmgandhi
- Date:
- Sun May 28 06:22:34 2017 +0000
- Parent:
- 39:058fb32c24e0
- Child:
- 41:56a34315dd75
- Commit message:
- RIP Harambe
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun May 28 03:42:59 2017 +0000 +++ b/main.cpp Sun May 28 06:22:34 2017 +0000 @@ -44,35 +44,6 @@ void pidOnEncoders(); -void moveForwardCellEncoder(double cellNum) -{ - int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum; - int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum; - - left_motor.forward(WHEEL_SPEED); - right_motor.forward(WHEEL_SPEED); - wait_ms(1); - while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) { - 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 < ir3base) { - // redLed.write(1); - // blueLed.write(0); - turnFlag |= RIGHT_FLAG; - } else if (receiverTwoReading < ir2base) { - // redLed.write(0); - // blueLed.write(1); - turnFlag |= LEFT_FLAG; - } - pidOnEncoders(); - } - - left_motor.brake(); - right_motor.brake(); -} - - void moveForwardEncoder(double num) { int count0; @@ -82,18 +53,19 @@ int initial1 = count1; int initial0 = count0; int diff = count0 - count1; - double kp = 0.00015; - double kd = 0.00019; + double kp = 0.00022; + double kd = 0.00020; int prev = 0; - double speed0 = WHEEL_SPEED; + double speed0 = WHEEL_SPEED+0.015; double speed1 = WHEEL_SPEED; + + receiverOneReading = IRP_1.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); left_motor.move(speed1); - receiverOneReading = IRP_1.getSamples(100); - receiverTwoReading = IRP_4.getSamples(100); - while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { + while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-270)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-270)*num) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); @@ -107,10 +79,10 @@ //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 - left_motor.move( speed1-0.8*d ); + left_motor.move( speed1-d ); right_motor.move( speed0+d ); } else if( x > diff + 40 ) { - left_motor.move( speed1-0.8*d ); + left_motor.move( speed1-d ); right_motor.move( speed0+d ); } prev = x; @@ -125,9 +97,7 @@ return; } - -void moveForwardWallEncoder() -{ +void encoderAfterTurn(double num){ int count0; int count1; count0 = encoder0.getPulses(); @@ -135,25 +105,20 @@ int initial1 = count1; int initial0 = count0; int diff = count0 - count1; - double kp = 0.00015; - double kd = 0.00019; + double kp = 0.00022; + double kd = 0.00020; int prev = 0; - double speed0 = WHEEL_SPEED; + double speed0 = WHEEL_SPEED+0.015; double speed1 = WHEEL_SPEED; + receiverOneReading = IRP_1.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); + right_motor.move(speed0); left_motor.move(speed1); - double ir1 = IRP_1.getSamples(50); - double ir4 = IRP_4.getSamples(50); - - if(ir1 > IRP_1.sensorAvg*4 || ir4 > IRP_2.sensorAvg*4){ - return; - } - - //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { - //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){ - while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) { + while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-270)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-270)*num) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { + //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); count0 = encoder0.getPulses() - initial0; @@ -166,30 +131,96 @@ //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 - left_motor.move( speed1-0.8*d ); + left_motor.move( speed1-d ); right_motor.move( speed0+d ); } else if( x > diff + 40 ) { - left_motor.move( speed1-0.8*d ); + left_motor.move( speed1-d ); right_motor.move( speed0+d ); } - // else - // { - // left_motor.brake(); - // right_motor.brake(); - // } prev = x; + receiverOneReading = IRP_1.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); } - //pidOnEncoders(); - //pidBrake(); right_motor.brake(); left_motor.brake(); - return; + 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; + } + + // the mouse has moved forward, we need to update the wall map now + receiverOneReading = IRP_1.getSamples(100); + receiverTwoReading = IRP_2.getSamples(100); + receiverThreeReading = IRP_3.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); + + serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg); + serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg); + + if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) { + serial.printf("Front wall is there\n"); + if (currDir % 4 == 0) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; + } else if (currDir % 4 == 1) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; + } else if (currDir % 4 == 2) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL; + } else if (currDir % 4 == 3) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL; + } + } + if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) { + // do nothing, the walls are not there + } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there RED + serial.printf("Left wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX); + if (currDir % 4 == 0) { + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; + } else if (currDir % 4 == 1) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; + } else if (currDir % 4 == 2) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; + } else if (currDir % 4 == 3) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL; + } + } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there + serial.printf("Right wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX); + if (currDir % 4 == 0) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; + } else if (currDir % 4 == 1) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL; + } else if (currDir % 4 == 2) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL; + } else if (currDir % 4 == 3) { + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; + } + } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { + serial.printf("Both walls \n"); + if (currDir %4 == 0){ + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; + } else if (currDir %4 == 1){ + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; + } else if (currDir % 4 == 2){ + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; + } else if (currDir %4 == 3){ + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; + wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; + } + } } void printDipFlag() { - if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); + if (DEBUGGING) + serial.printf("Flag value is %d", dipFlags); } void enableButton1() @@ -324,15 +355,29 @@ continue; } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED state = 1; + // double prev0 = encoder0.getPulses(); + // double prev1 = encoder1.getPulses(); + // double diff0 = desiredCount0 - prev0; + // double diff1 = desiredCount1 - prev1; + // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); redLed.write(0); greenLed.write(1); blueLed.write(1); + // moveForwardEncoder(valToPass); + // break; } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone // BLUE BLUE BLUE BLUE state = 2; + // double prev0 = encoder0.getPulses(); + // double prev1 = encoder1.getPulses(); + // double diff0 = desiredCount0 - prev0; + // double diff1 = desiredCount1 - prev1; + // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); redLed.write(1); greenLed.write(1); blueLed.write(0); + // moveForwardEncoder(valToPass); + // break; } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { // both walls there state = 0; @@ -410,7 +455,7 @@ left_motor.brake(); right_motor.brake(); - if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1) { + if (encoder0.getPulses() >= 0.5*desiredCount0 && encoder1.getPulses() >= 0.5*desiredCount1) { if (currDir % 4 == 0) { mouseY += 1; } else if (currDir % 4 == 1) { @@ -422,16 +467,16 @@ } // the mouse has moved forward, we need to update the wall map now - receiverOneReading = IRP_1.getSamples(75); - receiverTwoReading = IRP_2.getSamples(75); - receiverThreeReading = IRP_3.getSamples(75); - receiverFourReading = IRP_4.getSamples(75); + receiverOneReading = IRP_1.getSamples(100); + receiverTwoReading = IRP_2.getSamples(100); + receiverThreeReading = IRP_3.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); - // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg); - // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg); + serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg); + serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg); if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) { - // serial.printf("Front wall is there\n"); + serial.printf("Front wall is there\n"); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } else if (currDir % 4 == 1) { @@ -445,7 +490,7 @@ if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) { // do nothing, the walls are not there } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there RED - // serial.printf("Left wall\n"); + serial.printf("Left wall\n"); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; } else if (currDir % 4 == 1) { @@ -456,7 +501,7 @@ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL; } } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there - // serial.printf("Right wall\n"); + serial.printf("Right wall\n"); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; } else if (currDir % 4 == 1) { @@ -467,7 +512,7 @@ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { - // serial.printf("Both walls \n"); + serial.printf("Both walls \n"); if (currDir %4 == 0){ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; @@ -669,6 +714,7 @@ if (mouseY + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) { if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) { minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX]; + serial.printf("Looks like the left wall was not there\n"); dirToGo = 2; } } @@ -859,16 +905,23 @@ //wait_ms(1500); int nextMovement = 0; + // moveForwardEncoder(1); while (1) { - prevEncoder0Count = encoder0.getPulses(); - prevEncoder1Count = encoder1.getPulses(); + // break; + // prevEncoder0Count = encoder0.getPulses(); + // prevEncoder1Count = encoder1.getPulses(); if (!overrideFloodFill){ nextMovement = chooseNextMovement(); - serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY); + // serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY); if (nextMovement == 0){ - nCellEncoderAndIR(1); + // serial.printf("Just Turned, want to go forward now\n"); + redLed.write(1); + greenLed.write(0); + blueLed.write(1); + encoderAfterTurn(1); + // nCellEncoderAndIR(1); } else if (nextMovement == 1){ justTurned = true; @@ -886,23 +939,12 @@ turn180(); } } - else{ - overrideFloodFill = false; - } - currEncoder0Count = encoder0.getPulses(); - currEncoder1Count = encoder1.getPulses(); - - if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){ - overrideFloodFill = true; - waitButton4(); - } - wait_ms(500); // reduce this once we fine tune this! //////////////////////// TESTING CODE GOES BELOW /////////////////////////// - // nCellEncoderAndIR(2); + // encoderAfterTurn(1); // waitButton4(); // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() ); // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
--- a/main.h Sun May 28 03:42:59 2017 +0000 +++ b/main.h Sun May 28 06:22:34 2017 +0000 @@ -177,14 +177,14 @@ const int desiredCountL = 1500; const int oneCellCount = 5400; -const int oneCellCountMomentum = 4630;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! +const int oneCellCountMomentum = 4590;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! // const int oneCellCountMomentum = 4400; double receiverOneReading = 0.0; double receiverTwoReading = 0.0; double receiverThreeReading = 0.0; double receiverFourReading = 0.0; -const double frontStop = 7.2; +const double frontStop = 7.55; const double LRAvg = 3.5; float ir1base = 0.0;