Mouse code for the MacroRat
Revision 43:f22168a05c3e, committed 2017-05-28
- Comitter:
- kyleliangus
- Date:
- Sun May 28 10:09:56 2017 +0000
- Parent:
- 42:75257e6c4c76
- Child:
- 44:85bf2c0cd518
- Commit message:
- Changed some values around
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 06:50:22 2017 +0000 +++ b/main.cpp Sun May 28 10:09:56 2017 +0000 @@ -57,50 +57,66 @@ double kd = 0.00020; int prev = 0; - double speed0 = WHEEL_SPEED+0.015; + double speed0 = WHEEL_SPEED; double speed1 = WHEEL_SPEED; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); left_motor.move(speed1); + wait_us(60000); - double distance_to_go = (oneCellCountMomentum-270)*num; + double distance_to_go = (oneCellCount)*num; while( ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && 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; - count1 = encoder1.getPulses() - initial1; - int x = count0 - count1; - //double d = kp * x + kd * ( x - prev ); - double kppart = kp * x; - double kdpart = kd * (x-prev); - double d = kppart + kdpart; + // count0 = encoder0.getPulses() - initial0; // left + // count1 = encoder1.getPulses() - initial1; // right + // int x = count0 - count1; // negative if right spins faster, positive if left spins faster + // //double d = kp * x + kd * ( x - prev ); + // double kppart = kp * x; + // double kdpart = kd * (x-prev); + // double d = kppart + kdpart; - double leftspeed = speed1; - double rightspeed = speed0; + // double leftspeed = speed0; + // double rightspeed = speed1; - if( ( distance_to_go - (encoder0.getPulses() - initial0) ) <= 1000 || (distance_to_go - (encoder1.getPulses() - initial1)) <= 1000 ) + + if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) { - leftspeed *= .1/1000; - rightspeed *= .1/1000; + //leftspeed = speed0/900; + //rightspeed = speed1/900; + left_motor.move( speed0/900 ); + right_motor.move( speed1/900 ); + } + else + { + left_motor.move(speed0); + right_motor.move(speed1); + wait_us(16000); + pidOnEncoders(); } - + // else + // { + // if( x < diff + 3 ) { // count1 is bigger, right wheel pushed forward + // leftspeed -= d/4; + // rightspeed += d; + // } else if( x > diff + 3 ) { + // leftspeed -= d; + // rightspeed += d; + // } + // } + // //serial.printf("%d, %f, %f\n", x, leftspeed, rightspeed ); + // left_motor.move( leftspeed ); + // right_motor.move( rightspeed ); //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( leftspeed-d ); - right_motor.move( rightspeed+d ); - } else if( x > diff + 40 ) { - left_motor.move( leftspeed-d ); - right_motor.move( rightspeed+d ); - } - prev = x; + + // prev = x; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); } - //pidOnEncoders(); //pidBrake(); right_motor.brake(); @@ -116,56 +132,77 @@ int initial1 = count1; int initial0 = count0; int diff = count0 - count1; - double kp = 0.00022; - double kd = 0.00020; + double kp = 0.00008; + double kd = 0.0000; int prev = 0; - double speed0 = WHEEL_SPEED+0.015; + double speed0 = WHEEL_SPEED; // left wheel double speed1 = WHEEL_SPEED; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); left_motor.move(speed1); + wait_us(60000); - double distance_to_go = (oneCellCountMomentum-270)*num; + double distance_to_go = (oneCellCount)*num; while( ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && 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; - count1 = encoder1.getPulses() - initial1; - int x = count0 - count1; - //double d = kp * x + kd * ( x - prev ); - double kppart = kp * x; - double kdpart = kd * (x-prev); - double d = kppart + kdpart; + // count0 = encoder0.getPulses() - initial0; // left + // count1 = encoder1.getPulses() - initial1; // right + // int x = count0 - count1; // negative if right spins faster, positive if left spins faster + // //double d = kp * x + kd * ( x - prev ); + // double kppart = kp * x; + // double kdpart = kd * (x-prev); + // double d = kppart + kdpart; - double leftspeed = speed1; - double rightspeed = speed0; + // double leftspeed = speed0; + // double rightspeed = speed1; - if( ( distance_to_go - (encoder0.getPulses() - initial0) ) <= 1000 || (distance_to_go - (encoder1.getPulses() - initial1)) <= 1000 ) + + if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) { - leftspeed *= .1/1000; - rightspeed *= .1/1000; + //leftspeed = speed0/900; + //rightspeed = speed1/900; + left_motor.move( speed0/900 ); + right_motor.move( speed1/900 ); + } + else + { + left_motor.move(speed0); + right_motor.move(speed1); + wait_us(16000); + pidOnEncoders(); } - + // else + // { + // if( x < diff + 3 ) { // count1 is bigger, right wheel pushed forward + // leftspeed -= d/4; + // rightspeed += d; + // } else if( x > diff + 3 ) { + // leftspeed -= d; + // rightspeed += d; + // } + // } + // //serial.printf("%d, %f, %f\n", x, leftspeed, rightspeed ); + // left_motor.move( leftspeed ); + // right_motor.move( rightspeed ); //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( leftspeed-d ); - right_motor.move( rightspeed+d ); - } else if( x > diff + 40 ) { - left_motor.move( leftspeed-d ); - right_motor.move( rightspeed+d ); - } - prev = x; + + // prev = x; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); } right_motor.brake(); left_motor.brake(); + + double curr0 = encoder0.getPulses(); + double curr1 = encoder1.getPulses(); + if ((curr0 - initial0) >= distance_to_go*0.6 && (curr1 - initial1) >= distance_to_go*0.6){ if (currDir % 4 == 0) { mouseY += 1; } else if (currDir % 4 == 1) { @@ -182,11 +219,11 @@ 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) { @@ -200,7 +237,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, mouseY: %d, mouseX: %d\n", mouseY, mouseX); + // 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) { @@ -211,7 +248,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, mouseY: %d, mouseX: %d\n", mouseY, mouseX); + // 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) { @@ -222,7 +259,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; @@ -237,6 +274,7 @@ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; } } + } } void printDipFlag() @@ -934,14 +972,15 @@ // prevEncoder0Count = encoder0.getPulses(); // prevEncoder1Count = encoder1.getPulses(); + if (!overrideFloodFill){ nextMovement = chooseNextMovement(); // serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY); if (nextMovement == 0){ // serial.printf("Just Turned, want to go forward now\n"); - redLed.write(1); - greenLed.write(0); - blueLed.write(1); + //redLed.write(1); + //greenLed.write(0); + //blueLed.write(1); encoderAfterTurn(1); // nCellEncoderAndIR(1); } @@ -955,14 +994,16 @@ } else if (nextMovement == 3){ nCellEncoderAndIR(1); + //encoderAfterTurn(1); } else if (nextMovement == 4){ justTurned = true; turn180(); } } - wait_ms(500); // reduce this once we fine tune this! + wait_ms(700); // reduce this once we fine tune this! + //////////////////////// TESTING CODE GOES BELOW /////////////////////////// @@ -971,5 +1012,7 @@ // 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 ); + //encoderAfterTurn(1); + //waitButton4(); } } \ No newline at end of file
--- a/main.h Sun May 28 06:50:22 2017 +0000 +++ b/main.h Sun May 28 10:09:56 2017 +0000 @@ -172,11 +172,11 @@ //#define II_CONSTANT 0.06 //#define ID_CONSTANT 7.55 -const int desiredCount180 = 2900; // change accordingly to the terrain -const int desiredCountR = 1475; -const int desiredCountL = 1495; +const int desiredCount180 = 3120; // change accordingly to the terrain +const int desiredCountR = 1390; +const int desiredCountL = 1475; -const int oneCellCount = 5400; +const int oneCellCount = 5480; const int oneCellCountMomentum = 4590;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! double receiverOneReading = 0.0; @@ -184,7 +184,7 @@ double receiverThreeReading = 0.0; double receiverFourReading = 0.0; -const double frontStop = 7.55; +const double frontStop = 7.9; const double LRAvg = 3.5; float ir1base = 0.0; @@ -255,7 +255,7 @@ double speed1 = 0.12; // change back to 0.13 if turns stop working, testing something out! // double kp = 0.00009; - double kp = 0.0002; + double kp = 0.00015; int counter = 0; int initial0 = encoder0.getPulses();