MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 27:02ce1040f331
- Parent:
- 26:d20f1adac2d3
- Child:
- 28:8126a4d620e8
--- a/main.cpp Sun May 21 09:58:54 2017 +0000
+++ b/main.cpp Sun May 21 10:30:19 2017 +0000
@@ -46,9 +46,9 @@
float ir4base = 0.0;
float initAverageL = 8.28;
-float averageDivL = 30.5; //blue
+float averageDivL = 29.5; //blue
float initAverageR = 8.75; //4.5
-float averageDivR = 30.5; //red
+float averageDivR = 29.5; //red
float averageDivUpper = 0.9;
//IRSAvg= >: 0.143701, 0.128285
@@ -363,8 +363,6 @@
double kd = 0.00019;
int prev = 0;
-
-
double speed0 = 0.11;
double speed1 = 0.13;
right_motor.move(speed0);
@@ -607,30 +605,28 @@
int state = 0;
while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
- receiverTwoReading = IRP_2.getSamples(100);
- receiverThreeReading = IRP_3.getSamples(100);
- // previr2 = receiverTwoReading;
- // previr3 = receiverThreeReading;
- receiverOneReading = IRP_1.getSamples(100);
- receiverFourReading = IRP_4.getSamples(100);
+ receiverTwoReading = IRP_2.getSamples(60);
+ receiverThreeReading = IRP_3.getSamples(60);
+ receiverOneReading = IRP_1.getSamples(60);
+ receiverFourReading = IRP_4.getSamples(60);
- if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){
+ if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){
if (currDir % 4 == 0){
- wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= F_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
}
else if (currDir % 4 == 1){
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= R_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_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] |= B_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
}
break;
}
- if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL)) ){
+ if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL))){
// both sides gone
redLed.write(1);
greenLed.write(1);
@@ -660,42 +656,42 @@
moveForwardEncoder(valToPass);
}
else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone
- 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;
- }
+ // 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;
+ // }
// RED RED RED RED RED
state = 1;
redLed.write(0);
greenLed.write(1);
blueLed.write(1);
}else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone
- if (currDir % 4 == 0){
- wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
- }
- else if (currDir % 4 == 1){
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
- }
- else if (currDir % 4 == 2){
- wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
- }
- else if (currDir % 4 == 3){
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
- }
+ // if (currDir % 4 == 0){
+ // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
+ // }
+ // else if (currDir % 4 == 1){
+ // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
+ // }
+ // else if (currDir % 4 == 2){
+ // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
+ // }
+ // else if (currDir % 4 == 3){
+ // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
+ // }
// BLUE BLUE BLUE BLUE
state = 2;
redLed.write(1);
greenLed.write(1);
blueLed.write(0);
- }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
+ }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
if (currDir % 4 == 0){
wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
@@ -781,16 +777,16 @@
}
bool isWallInFront(int x, int y){
- return (wallArray[MAZE_LEN - y - 1][x] & 0x1);
+ return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
}
bool isWallInBack(int x, int y){
- return (wallArray[MAZE_LEN - y - 1][x] & 0x8);
+ return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
}
bool isWallOnRight(int x, int y){
- return (wallArray[MAZE_LEN - y - 1][x] & 0x4);
+ return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
}
bool isWallOnLeft(int x, int y){
- return (wallArray[MAZE_LEN - y - 1][x] & 0x2);
+ return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
}
int chooseNextMovement(){
@@ -825,17 +821,17 @@
minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
}
}
- if (currX - 1 >= 0 && !isWallOnLeft((unsigned) currX, (unsigned) currY)) {
+ if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) {
if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
}
}
- if (currY + 1 < MAZE_LEN && !isWallInFront((unsigned) currX, (unsigned) currY)) {
+ if (currY + 1 < MAZE_LEN && !isWallInFront( currX, currY)) {
if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
}
}
- if (currY - 1 >= 0 && !isWallInBack((unsigned) currX, (unsigned) currY)) {
+ if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
}
@@ -868,8 +864,6 @@
if (currDist > minDist)
continue;
if (currDist <= minDist) {
-// cout << "Changing the following coordinate ( " << currX << "," << currY << ") from "
-// << manhattanDistances[MAZE_LEN - 1 - currY][currX] << " to " << minDist + 1 << endl;
manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
}
if (hasVisited(currX, currY)) {