Mouse code for the MacroRat
Revision 39:058fb32c24e0, committed 2017-05-28
- Comitter:
- vanshg
- Date:
- Sun May 28 03:42:59 2017 +0000
- Parent:
- 38:fe05f93009a2
- Child:
- 40:465d2b565977
- Commit message:
- calibration n shit
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 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 );
--- a/main.h Sat May 27 21:29:55 2017 +0000 +++ b/main.h Sun May 28 03:42:59 2017 +0000 @@ -172,67 +172,76 @@ //#define II_CONSTANT 0.06 //#define ID_CONSTANT 7.55 -const int desiredCount180 = 3380; // change accordingly to the terrain -const int desiredCountR = 1600; -const int desiredCountL = 1600; - +const int desiredCount180 = 2900; // change accordingly to the terrain +const int desiredCountR = 1490; +const int desiredCountL = 1500; + const int oneCellCount = 5400; -const int oneCellCountMomentum = 5070;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! - +const int oneCellCountMomentum = 4630;//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 LRAvg = 3.5; + float ir1base = 0.0; float ir2base = 0.0; float ir3base = 0.0; float ir4base = 0.0; float averageDivUpper = 0.5; - + inline void turnLeft() { double speed0 = 0.11; double speed1 = -0.12; // change back to 0.13 if turns stop working, testing something out! - - double kp = 0.000082; - + + //double kp = 0.000082; + double kp = 0.00010; + int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - + int desiredCount0 = initial0 - desiredCountL; // left wheel int desiredCount1 = initial1 + desiredCountL; // right wheel - + int count0 = initial0; int count1 = initial1; - + double error0 = desiredCount0 - count0; // is negative double error1 = desiredCount1 - count1; // is positive - + while(1) { - if(!(abs(error0) < 3) && !(abs(error1) < 3)) { + if(!(abs(error0) < 4) && !(abs(error1) < 4)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); - + error0 = desiredCount0 - count0; // is negative error1 = desiredCount1 - count1; // is positive - + right_motor.move(error1*kp); left_motor.move(error0*kp); counter = 0; } else { counter++; + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = desiredCount0 - count0; // is negative + error1 = desiredCount1 - count1; // is positive right_motor.brake(); left_motor.brake(); } - if (counter > 60) { + if (counter > 100) { break; } } - + right_motor.brake(); left_motor.brake(); turnFlag = 0; // zeroing out the flags! @@ -245,7 +254,8 @@ double speed0 = -0.11; double speed1 = 0.12; // change back to 0.13 if turns stop working, testing something out! - double kp = 0.00009; +// double kp = 0.00009; + double kp = 0.0002; int counter = 0; int initial0 = encoder0.getPulses(); @@ -262,7 +272,7 @@ while(1) { - if(!(abs(error0) < 3) && !(abs(error1) < 3)) { + if(!(abs(error0) < 2) && !(abs(error1) < 2)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); @@ -274,10 +284,15 @@ counter = 0; } else { counter++; + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = desiredCount0 - count0; // is positive + error1 = desiredCount1 - count1; // is negative right_motor.brake(); left_motor.brake(); } - if (counter > 60) { + if (counter > 100) { break; } } @@ -293,7 +308,8 @@ double speed0 = -0.10; double speed1 = 0.11; - double kp = 0.000055; +// double kp = 0.000055; + double kp = 0.00006; int counter = 0; int initial0 = encoder0.getPulses(); @@ -322,10 +338,17 @@ counter = 0; } else { counter++; + + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = desiredCount0 - count0; + error1 = desiredCount1 - count1; + right_motor.brake(); left_motor.brake(); } - if (counter > 60) { + if (counter > 150) { break; } }