MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 42:75257e6c4c76
- Parent:
- 41:56a34315dd75
- Child:
- 43:f22168a05c3e
--- 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) {