Mouse code for the MacroRat
Revision 42:75257e6c4c76, committed 2017-05-28
- Comitter:
- kyleliangus
- Date:
- Sun May 28 06:50:22 2017 +0000
- Parent:
- 41:56a34315dd75
- Child:
- 43:f22168a05c3e
- Commit message:
- Deaccel on encoder move forward
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun May 28 06:48:16 2017 +0000 +++ b/main.cpp Sun May 28 06:50:22 2017 +0000 @@ -65,7 +65,9 @@ right_motor.move(speed0); left_motor.move(speed1); - while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-270)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-270)*num) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { + double distance_to_go = (oneCellCountMomentum-270)*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 )); @@ -77,13 +79,22 @@ double kdpart = kd * (x-prev); double d = kppart + kdpart; + double leftspeed = speed1; + double rightspeed = speed0; + + if( ( distance_to_go - (encoder0.getPulses() - initial0) ) <= 1000 || (distance_to_go - (encoder1.getPulses() - initial1)) <= 1000 ) + { + leftspeed *= .1/1000; + rightspeed *= .1/1000; + } + //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-d ); - right_motor.move( speed0+d ); + left_motor.move( leftspeed-d ); + right_motor.move( rightspeed+d ); } else if( x > diff + 40 ) { - left_motor.move( speed1-d ); - right_motor.move( speed0+d ); + left_motor.move( leftspeed-d ); + right_motor.move( rightspeed+d ); } prev = x; receiverOneReading = IRP_1.getSamples(100); @@ -117,7 +128,9 @@ right_motor.move(speed0); left_motor.move(speed1); - while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-270)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-270)*num) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { + double distance_to_go = (oneCellCountMomentum-270)*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 )); @@ -129,13 +142,22 @@ double kdpart = kd * (x-prev); double d = kppart + kdpart; + double leftspeed = speed1; + double rightspeed = speed0; + + if( ( distance_to_go - (encoder0.getPulses() - initial0) ) <= 1000 || (distance_to_go - (encoder1.getPulses() - initial1)) <= 1000 ) + { + leftspeed *= .1/1000; + rightspeed *= .1/1000; + } + //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-d ); - right_motor.move( speed0+d ); + left_motor.move( leftspeed-d ); + right_motor.move( rightspeed+d ); } else if( x > diff + 40 ) { - left_motor.move( speed1-d ); - right_motor.move( speed0+d ); + left_motor.move( leftspeed-d ); + right_motor.move( rightspeed+d ); } prev = x; receiverOneReading = IRP_1.getSamples(100); @@ -160,11 +182,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) { @@ -178,7 +200,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, currDir = %d\n", mouseY, mouseX, currDir%4); + 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) { @@ -189,7 +211,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) {