Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 39:058fb32c24e0
- Parent:
- 38:fe05f93009a2
- Child:
- 40:465d2b565977
diff -r fe05f93009a2 -r 058fb32c24e0 main.cpp --- a/main.cpp Sat May 27 21:29:55 2017 +0000 +++ b/main.cpp Sun May 28 03:42:59 2017 +0000 @@ -40,7 +40,7 @@ #define IP_CONSTANT 2.00 #define II_CONSTANT 0.00001 -#define ID_CONSTANT 0.190 +#define ID_CONSTANT 0.2 void pidOnEncoders(); @@ -93,7 +93,7 @@ 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*4 && receiverFourReading < IRP_4.sensorAvg*4) { + while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*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 )); @@ -289,7 +289,6 @@ int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount; - serial.printf("%d, %d\n", desiredCount0, desiredCount1 ); left_motor.forward(WHEEL_SPEED); right_motor.forward(WHEEL_SPEED); @@ -306,7 +305,7 @@ receiverOneReading = IRP_1.getSamples( SAMPLE_NUM ); receiverFourReading = IRP_4.getSamples( SAMPLE_NUM ); // serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg); - if( receiverOneReading > IRP_1.sensorAvg * 5.5 || receiverFourReading > IRP_4.sensorAvg * 5.5) { + if( receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) { break; } @@ -317,15 +316,18 @@ double diff0 = desiredCount0 - prev0; double diff1 = desiredCount1 - prev1; double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); + 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(valToPass); continue; - } else if (receiverThreeReading < IRP_3.sensorAvg/3.5) { // right wall gone RED + } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED state = 1; redLed.write(0); greenLed.write(1); blueLed.write(1); - } else if (receiverTwoReading < IRP_2.sensorAvg/3.5) { // left wall gone + } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone // BLUE BLUE BLUE BLUE state = 2; redLed.write(1); @@ -363,14 +365,30 @@ sumError += currentError; derivError = currentError - previousError; double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; + + double prev0 = encoder0.getPulses(); + double prev1 = encoder1.getPulses(); + double diff0 = desiredCount0 - prev0; + double diff1 = desiredCount1 - prev1; + double kp = .1/1000; + + + rightSpeed = HIGH_PWM_VOLTAGE_R; + leftSpeed = HIGH_PWM_VOLTAGE_L; + if (diff1 < 1000 || diff0 < 1000) { + rightSpeed = kp * HIGH_PWM_VOLTAGE_R; + leftSpeed = kp * HIGH_PWM_VOLTAGE_L; + } + if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down - rightSpeed = HIGH_PWM_VOLTAGE_R - abs(PIDSum*HIGH_PWM_VOLTAGE_R); - leftSpeed = HIGH_PWM_VOLTAGE_L + abs(PIDSum*HIGH_PWM_VOLTAGE_L); + rightSpeed = rightSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_R); + leftSpeed = leftSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_L); } else { // r is faster than L. speed up l, slow down r - rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R); - leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L); + rightSpeed = rightSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_R); + leftSpeed = leftSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_L); } + //serial.printf("%f, %f\n", leftSpeed, rightSpeed); if (leftSpeed > 0.30) leftSpeed = 0.30; if (leftSpeed < 0) leftSpeed = 0; @@ -409,37 +427,59 @@ receiverThreeReading = IRP_3.getSamples(75); receiverFourReading = IRP_4.getSamples(75); - if (receiverOneReading >= 0.3f && receiverFourReading >= 0.3f) { + // 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\n"); + 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; } - } - if (receiverThreeReading >= IRP_3.sensorAvg*0.6) { + } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there + // serial.printf("Right wall\n"); if (currDir % 4 == 0) { - wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL; + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; } else if (currDir % 4 == 1) { - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL; + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL; } else if (currDir % 4 == 2) { - wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL; + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL; } else if (currDir % 4 == 3) { - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL; + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } - } - if (receiverTwoReading >= IRP_2.sensorAvg*0.6) { - if (currDir % 4 == 0) { - wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL; - } else if (currDir % 4 == 1) { - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL; - } else if (currDir % 4 == 2) { - wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL; - } else if (currDir % 4 == 3) { - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_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; } } } @@ -743,7 +783,7 @@ // do nothing until ready } //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG); - wait( 2 ); + wait( 3 ); } int main() @@ -807,89 +847,63 @@ 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; + int prevEncoder0Count = 0; + int prevEncoder1Count = 0; + int currEncoder0Count = 0; + int currEncoder1Count = 0; - //bool overrideFloodFill = false; + bool overrideFloodFill = false; right_motor.forward( WHEEL_SPEED ); left_motor.forward( WHEEL_SPEED ); //turn180(); //wait_ms(1500); - //int nextMovement = 0; + int nextMovement = 0; + + while (1) { - // 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; -// turn180(); -// } -// } -// else{ -// overrideFloodFill = false; -// if ((rand()%2 + 1) == 1){ -// justTurned = true; -// turnLeft(); -// } -// else{ -// justTurned = true; -// turnRight(); -// } -// } -// currEncoder0Count = encoder0.getPulses(); -// currEncoder1Count = encoder1.getPulses(); -// -// if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){ -// overrideFloodFill = true; -// } -// -// wait_ms(300); // reduce this once we fine tune this! + prevEncoder0Count = encoder0.getPulses(); + prevEncoder1Count = encoder1.getPulses(); + + if (!overrideFloodFill){ + nextMovement = chooseNextMovement(); + serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY); + 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; + 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(4); - wait_ms(300); - turnRight(); - nCellEncoderAndIR(1); - wait_ms(300); - turnRight(); - nCellEncoderAndIR(4); - wait_ms(300); - turnLeft(); - wait_ms(300); - nCellEncoderAndIR(1); - wait_ms(300); - turnLeft(); - wait_ms(300); - nCellEncoderAndIR(3); - wait_ms(300); - turnRight(); - wait_ms(300); - nCellEncoderAndIR(3); - wait_ms(300); - turnRight(); - wait_ms(300); - nCellEncoderAndIR(3); - waitButton4(); + // nCellEncoderAndIR(2); + // 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) ) ; // 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 );