MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 40:465d2b565977
- Parent:
- 39:058fb32c24e0
- Child:
- 41:56a34315dd75
--- a/main.cpp Sun May 28 03:42:59 2017 +0000
+++ b/main.cpp Sun May 28 06:22:34 2017 +0000
@@ -44,35 +44,6 @@
void pidOnEncoders();
-void moveForwardCellEncoder(double cellNum)
-{
- int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
- int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
-
- left_motor.forward(WHEEL_SPEED);
- right_motor.forward(WHEEL_SPEED);
- wait_ms(1);
- while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
- receiverTwoReading = IRP_2.getSamples(100);
- receiverThreeReading = IRP_3.getSamples(100);
- // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading);
- if (receiverThreeReading < ir3base) {
- // redLed.write(1);
- // blueLed.write(0);
- turnFlag |= RIGHT_FLAG;
- } else if (receiverTwoReading < ir2base) {
- // redLed.write(0);
- // blueLed.write(1);
- turnFlag |= LEFT_FLAG;
- }
- pidOnEncoders();
- }
-
- left_motor.brake();
- right_motor.brake();
-}
-
-
void moveForwardEncoder(double num)
{
int count0;
@@ -82,18 +53,19 @@
int initial1 = count1;
int initial0 = count0;
int diff = count0 - count1;
- double kp = 0.00015;
- double kd = 0.00019;
+ double kp = 0.00022;
+ double kd = 0.00020;
int prev = 0;
- double speed0 = WHEEL_SPEED;
+ double speed0 = WHEEL_SPEED+0.015;
double speed1 = WHEEL_SPEED;
+
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverFourReading = IRP_4.getSamples(100);
right_motor.move(speed0);
left_motor.move(speed1);
- 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*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) {
+ while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-270)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-270)*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 ));
@@ -107,10 +79,10 @@
//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-0.8*d );
+ left_motor.move( speed1-d );
right_motor.move( speed0+d );
} else if( x > diff + 40 ) {
- left_motor.move( speed1-0.8*d );
+ left_motor.move( speed1-d );
right_motor.move( speed0+d );
}
prev = x;
@@ -125,9 +97,7 @@
return;
}
-
-void moveForwardWallEncoder()
-{
+void encoderAfterTurn(double num){
int count0;
int count1;
count0 = encoder0.getPulses();
@@ -135,25 +105,20 @@
int initial1 = count1;
int initial0 = count0;
int diff = count0 - count1;
- double kp = 0.00015;
- double kd = 0.00019;
+ double kp = 0.00022;
+ double kd = 0.00020;
int prev = 0;
- double speed0 = WHEEL_SPEED;
+ double speed0 = WHEEL_SPEED+0.015;
double speed1 = WHEEL_SPEED;
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverFourReading = IRP_4.getSamples(100);
+
right_motor.move(speed0);
left_motor.move(speed1);
- double ir1 = IRP_1.getSamples(50);
- double ir4 = IRP_4.getSamples(50);
-
- if(ir1 > IRP_1.sensorAvg*4 || ir4 > IRP_2.sensorAvg*4){
- return;
- }
-
- //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
- //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){
- while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) {
+ while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-270)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-270)*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 ));
count0 = encoder0.getPulses() - initial0;
@@ -166,30 +131,96 @@
//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-0.8*d );
+ left_motor.move( speed1-d );
right_motor.move( speed0+d );
} else if( x > diff + 40 ) {
- left_motor.move( speed1-0.8*d );
+ left_motor.move( speed1-d );
right_motor.move( speed0+d );
}
- // else
- // {
- // left_motor.brake();
- // right_motor.brake();
- // }
prev = x;
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverFourReading = IRP_4.getSamples(100);
}
- //pidOnEncoders();
- //pidBrake();
right_motor.brake();
left_motor.brake();
- return;
+ if (currDir % 4 == 0) {
+ mouseY += 1;
+ } else if (currDir % 4 == 1) {
+ mouseX += 1;
+ } else if (currDir % 4 == 2) {
+ mouseY -= 1;
+ } else if (currDir % 4 == 3) {
+ mouseX -= 1;
+ }
+
+ // the mouse has moved forward, we need to update the wall map now
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverTwoReading = IRP_2.getSamples(100);
+ 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);
+
+ 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, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
+ 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;
+ }
+ } 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);
+ if (currDir % 4 == 0) {
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
+ } else if (currDir % 4 == 1) {
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
+ } else if (currDir % 4 == 2) {
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
+ } else if (currDir % 4 == 3) {
+ 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");
+ 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;
+ }
+ }
}
void printDipFlag()
{
- if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
+ if (DEBUGGING)
+ serial.printf("Flag value is %d", dipFlags);
}
void enableButton1()
@@ -324,15 +355,29 @@
continue;
} else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED
state = 1;
+ // double prev0 = encoder0.getPulses();
+ // double prev1 = encoder1.getPulses();
+ // double diff0 = desiredCount0 - prev0;
+ // double diff1 = desiredCount1 - prev1;
+ // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
redLed.write(0);
greenLed.write(1);
blueLed.write(1);
+ // moveForwardEncoder(valToPass);
+ // break;
} else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
// BLUE BLUE BLUE BLUE
state = 2;
+ // double prev0 = encoder0.getPulses();
+ // double prev1 = encoder1.getPulses();
+ // double diff0 = desiredCount0 - prev0;
+ // double diff1 = desiredCount1 - prev1;
+ // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
redLed.write(1);
greenLed.write(1);
blueLed.write(0);
+ // moveForwardEncoder(valToPass);
+ // break;
} else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
// both walls there
state = 0;
@@ -410,7 +455,7 @@
left_motor.brake();
right_motor.brake();
- if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1) {
+ if (encoder0.getPulses() >= 0.5*desiredCount0 && encoder1.getPulses() >= 0.5*desiredCount1) {
if (currDir % 4 == 0) {
mouseY += 1;
} else if (currDir % 4 == 1) {
@@ -422,16 +467,16 @@
}
// the mouse has moved forward, we need to update the wall map now
- receiverOneReading = IRP_1.getSamples(75);
- receiverTwoReading = IRP_2.getSamples(75);
- receiverThreeReading = IRP_3.getSamples(75);
- receiverFourReading = IRP_4.getSamples(75);
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverTwoReading = IRP_2.getSamples(100);
+ 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) {
@@ -445,7 +490,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\n");
+ serial.printf("Left wall\n");
if (currDir % 4 == 0) {
wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
} else if (currDir % 4 == 1) {
@@ -456,7 +501,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\n");
+ serial.printf("Right wall\n");
if (currDir % 4 == 0) {
wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
} else if (currDir % 4 == 1) {
@@ -467,7 +512,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;
@@ -669,6 +714,7 @@
if (mouseY + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
+ serial.printf("Looks like the left wall was not there\n");
dirToGo = 2;
}
}
@@ -859,16 +905,23 @@
//wait_ms(1500);
int nextMovement = 0;
+ // moveForwardEncoder(1);
while (1) {
- prevEncoder0Count = encoder0.getPulses();
- prevEncoder1Count = encoder1.getPulses();
+ // break;
+ // prevEncoder0Count = encoder0.getPulses();
+ // prevEncoder1Count = encoder1.getPulses();
if (!overrideFloodFill){
nextMovement = chooseNextMovement();
- serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY);
+ // serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY);
if (nextMovement == 0){
- nCellEncoderAndIR(1);
+ // serial.printf("Just Turned, want to go forward now\n");
+ redLed.write(1);
+ greenLed.write(0);
+ blueLed.write(1);
+ encoderAfterTurn(1);
+ // nCellEncoderAndIR(1);
}
else if (nextMovement == 1){
justTurned = true;
@@ -886,23 +939,12 @@
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(2);
+ // encoderAfterTurn(1);
// 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) ) ;