MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 26:d20f1adac2d3
- Parent:
- 25:f827a8b7880e
- Child:
- 27:02ce1040f331
--- a/main.cpp Sun May 21 08:52:43 2017 +0000
+++ b/main.cpp Sun May 21 09:58:54 2017 +0000
@@ -1,20 +1,23 @@
#include "mbed.h"
-
+
#include "irpair.h"
#include "main.h"
#include "motor.h"
+
+#include <stdlib.h>
+#include <stack> // std::stack
+#include <utility> // std::pair, std::make_pair
-#include <stdlib.h>
#include "ITG3200.h"
#include "stm32f4xx.h"
#include "QEI.h"
-
+
/* Constants for when HIGH_PWM_VOLTAGE = 0.2
#define IP_CONSTANT 6
#define II_CONSTANT 0
#define ID_CONSTANT 1
*/
-
+
// Constants for when HIGH_PWM_VOLTAGE = 0.1
// #define IP_CONSTANT 8.85
// #define II_CONSTANT 0.005
@@ -22,104 +25,85 @@
#define IP_CONSTANT 8.5
#define II_CONSTANT 0.095
#define ID_CONSTANT 6.85
-
-const int desiredCount180 = 2850;
+
+const int desiredCount180 = 2800;
const int desiredCountR = 1575;
const int desiredCountL = 1650;
-
+
const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4700; // one cell count is actually approximately 5400, but this value is considering momentum!
-
+const int oneCellCountMomentum = 4700;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
+
float receiverOneReading = 0.0;
float receiverTwoReading = 0.0;
float receiverThreeReading = 0.0;
float receiverFourReading = 0.0;
+float ir1base = 0.0;
+float ir2base = 0.0;
+
+float ir3base = 0.0;
+
+float ir4base = 0.0;
float initAverageL = 8.28;
-float averageDivL = 29.5; //blue
+float averageDivL = 30.5; //blue
float initAverageR = 8.75; //4.5
-float averageDivR = 29.5; //red
+float averageDivR = 30.5; //red
float averageDivUpper = 0.9;
+//IRSAvg= >: 0.143701, 0.128285
+
+ //facing wall ir2 and ir3
+ //0.144562, 0.113971 in maze
+
+ // normal hall ir2 and ir3
+ //0.013665, 0.010889 in maze
+
+ //0.014462, 0.009138
+ // 0.013888, 0.010530
+
+
+ //no walls ir2 and ir3
+ //0.003265, 0.002904 in maze9
+
+ //0.003162, 0.003123
+ //0.003795,
+
+//0.005297, 0.007064
+
+float noWallR = 0.007;
+float noWallL = 0.007;
+
void pidOnEncoders();
-
-
+
+
void turnLeft()
{
double speed0 = 0.11;
double speed1 = -0.13;
-
+
int counter = 0;
int initial0 = encoder0.getPulses();
int initial1 = encoder1.getPulses();
-
+
int desiredCount0 = initial0 - desiredCountL;
int desiredCount1 = initial1 + desiredCountL;
-
+
int count0 = initial0;
int count1 = initial1;
-
+
double error0 = count0 - desiredCount0;
double error1 = count1 - desiredCount1;
-
-
+
while(1) {
-
+
if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
-
+
error0 = count0 - desiredCount0;
error1 = count1 - desiredCount1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
- counter = 0;
- } else {
- counter++;
- right_motor.brake();
- left_motor.brake();
- }
-
- if (counter > 60) {
- break;
- }
- }
-
- right_motor.brake();
- left_motor.brake();
- turnFlag = 0; // zeroing out the flags!
- currDir -= 1;
-}
-
-void turnRight()
-{
- double speed0 = -0.11;
- double speed1 = 0.13;
-
- int counter = 0;
- int initial0 = encoder0.getPulses();
- int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 + desiredCountR;
- int desiredCount1 = initial1 - desiredCountR;
-
- int count0 = initial0;
- int count1 = initial1;
-
- double error0 = count0 - desiredCount0;
- double error1 = count1 - desiredCount1;
-
- while(1) {
-
- if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
-
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
+
right_motor.move(speed0);
left_motor.move(speed1);
counter = 0;
@@ -128,93 +112,45 @@
right_motor.brake();
left_motor.brake();
}
-
+
if (counter > 60) {
break;
}
}
-
+
right_motor.brake();
left_motor.brake();
- turnFlag = 0;
- currDir += 1;
+ turnFlag = 0; // zeroing out the flags!
+ currDir -= 1;
}
-
-void turnLeft180()
+
+void turnRight()
{
- double speed0 = 0.15;
- double speed1 = -0.15;
-
+ double speed0 = -0.11;
+ double speed1 = 0.13;
+
int counter = 0;
int initial0 = encoder0.getPulses();
int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 - desiredCountL*2;
- int desiredCount1 = initial1 + desiredCountL*2;
-
+
+ int desiredCount0 = initial0 + desiredCountR;
+ int desiredCount1 = initial1 - desiredCountR;
+
int count0 = initial0;
int count1 = initial1;
-
+
double error0 = count0 - desiredCount0;
double error1 = count1 - desiredCount1;
-
-
+
while(1) {
-
+
if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
-
+
error0 = count0 - desiredCount0;
error1 = count1 - desiredCount1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
- counter = 0;
- } else {
- counter++;
- right_motor.brake();
- left_motor.brake();
- }
-
- if (counter > 60) {
- break;
- }
- }
-
- right_motor.brake();
- left_motor.brake();
- currDir -= 2;
-}
-
-void turnRight180()
-{
- double speed0 = -0.16;
- double speed1 = 0.16;
-
- int counter = 0;
- int initial0 = encoder0.getPulses();
- int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 + desiredCount180;
- int desiredCount1 = initial1 - desiredCount180;
-
- int count0 = initial0;
- int count1 = initial1;
-
- double error0 = count0 - desiredCount0;
- double error1 = count1 - desiredCount1;
-
-
- while(1) {
-
- if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
-
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
+
right_motor.move(speed0);
left_motor.move(speed1);
counter = 0;
@@ -223,7 +159,102 @@
right_motor.brake();
left_motor.brake();
}
-
+
+ if (counter > 60) {
+ break;
+ }
+ }
+
+ right_motor.brake();
+ left_motor.brake();
+ turnFlag = 0;
+ currDir += 1;
+}
+
+void turnLeft180()
+{
+ double speed0 = 0.15;
+ double speed1 = -0.15;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ int desiredCount0 = initial0 - desiredCountL*2;
+ int desiredCount1 = initial1 + desiredCountL*2;
+
+ int count0 = initial0;
+ int count1 = initial1;
+
+ double error0 = count0 - desiredCount0;
+ double error1 = count1 - desiredCount1;
+
+
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
+
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ } else {
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
+
+ if (counter > 60) {
+ break;
+ }
+ }
+
+ right_motor.brake();
+ left_motor.brake();
+ currDir -= 2;
+}
+
+void turnRight180()
+{
+ double speed0 = -0.16;
+ double speed1 = 0.16;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ int desiredCount0 = initial0 + desiredCount180;
+ int desiredCount1 = initial1 - desiredCount180;
+
+ int count0 = initial0;
+ int count1 = initial1;
+
+ double error0 = count0 - desiredCount0;
+ double error1 = count1 - desiredCount1;
+
+
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
+
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ } else {
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
+
if (counter > 60) {
break;
}
@@ -232,11 +263,11 @@
left_motor.brake();
currDir += 2;
}
-
+
void moveForwardCellEncoder(double cellNum){
int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
-
+
left_motor.forward(0.125);
right_motor.forward(0.095);
wait_ms(1);
@@ -244,88 +275,26 @@
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 < IRP_3.sensorAvg/averageDivR){
+ if (receiverThreeReading < ir3base){
// redLed.write(1);
// blueLed.write(0);
turnFlag |= RIGHT_FLAG;
}
- else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){
+ else if (receiverTwoReading < ir2base){
// redLed.write(0);
// blueLed.write(1);
turnFlag |= LEFT_FLAG;
}
pidOnEncoders();
}
-
+
left_motor.brake();
right_motor.brake();
}
-
-void handleTurns(){
- if (turnFlag == 0x1){
- // moveForwardCellEncoder(0.3);
- turnLeft();
- }
- else if (turnFlag == 0x2){
- // moveForwardCellEncoder(0.3);
- turnRight();
- }
- else if (turnFlag == 0x3){
- // moveForwardCellEncoder(0.3);
- turnLeft();
- turnRight();
- }
-}
-
-void pidBrake(){
-
- int count0;
- int count1;
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
- int initial0 = count0;
- int initial1 = count1;
- double kp = 0.00011;
-
-
-
- int error = count0 - count1;
-
- int counter = 0;
- right_motor.move(0.7);
- left_motor.move(0.7);
-
- double speed0 = 0.7;
- double speed1 = 0.7;
-
- while(1)
- {
- if(abs(error) < 3){
- right_motor.brake();
- left_motor.brake();
- counter++;
- }else{
- count0 = encoder0.getPulses() - initial0;
- count1 = encoder1.getPulses() - initial1;
- error = count0 - count1;
- speed0 = -error*kp + speed0;
- speed1 = error*kp + speed1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
-
- counter = 0;
- }
- if (counter > 10){
- break;
- }
-
- }
- return;
-}
-
-void moveForwardEncoder(){
-
+
+
+void moveForwardEncoder(double num){
+
int count0;
int count1;
count0 = encoder0.getPulses();
@@ -337,15 +306,13 @@
double kd = 0.00019;
int prev = 0;
-
-
double speed0 = 0.10;
double speed1 = 0.12;
right_motor.move(speed0);
left_motor.move(speed1);
-
-
- while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
+
+
+ while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) || IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){
//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 ));
@@ -375,18 +342,17 @@
// }
prev = x;
}
-
+
//pidOnEncoders();
//pidBrake();
right_motor.brake();
left_motor.brake();
return;
}
-
-
+
+
void moveForwardWallEncoder(){
-
- int count0;
+ int count0;
int count1;
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
@@ -451,7 +417,7 @@
left_motor.brake();
return;
}
-
+
void moveForwardUntilWallIr()
{
double currentError = 0;
@@ -521,12 +487,12 @@
right_motor.brake();
}
}
-
+
void printDipFlag()
{
if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
}
-
+
void enableButton1()
{
dipFlags |= BUTTON1_FLAG;
@@ -567,7 +533,7 @@
dipFlags &= ~BUTTON4_FLAG;
printDipFlag();
}
-
+
void pidOnEncoders()
{
int count0;
@@ -578,7 +544,7 @@
double kp = 0.00011;
double kd = 0.00014;
int prev = 0;
-
+
int counter = 0;
while(1)
{
@@ -612,7 +578,7 @@
break;
}
}
-
+
void nCellEncoderAndIR(double cellCount){
double currentError = 0;
double previousError = 0;
@@ -640,54 +606,31 @@
int state = 0;
-
- while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1 && receiverOneReading < IRP_1.sensorAvg*0.8 && receiverFourReading < IRP_4.sensorAvg*0.8){
+ 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);
-
- //if ((receiverOneReading+receiverFourReading)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.15 ){
- if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){
- // almost to the end
- right_motor.move(-0.15);
- left_motor.move(-0.15);
-
- wait_ms(150);
- right_motor.brake();
- left_motor.brake();
-
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(1);
- wait_ms(200);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- wait_ms(200);
-
-
+ if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){
+ if (currDir % 4 == 0){
+ wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= F_WALL;
+ }
+ else if (currDir % 4 == 1){
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= R_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] |= B_WALL;
+ }
+ break;
+ }
- redLed.write(1);
- greenLed.write(0);
- blueLed.write(1);
- wait_ms(200);
- redLed.write(0);
- greenLed.write(1);
- blueLed.write(1);
-
-
-
-
- //moveForwardWallEncoder();
-
-
- return;
-
- }else 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);
@@ -708,20 +651,67 @@
redLed.write(1);
greenLed.write(1);
blueLed.write(0);
- moveForwardEncoder();
- }else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone
+
+ int prev0 = encoder0.getPulses();
+ int prev1 = encoder1.getPulses();
+ int diff0 = desiredCount0 - prev0;
+ int diff1 = desiredCount1 - prev1;
+ int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
+ 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;
+ }
// 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;
+ }
// 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))){
+ if (currDir % 4 == 0){
+ wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
+ }
+ else if (currDir % 4 == 1){
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
+ }
+ else if (currDir % 4 == 2){
+ wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
+ }
+ else if (currDir % 4 == 3){
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
+ }
// both walls there
state = 0;
redLed.write(1);
@@ -748,12 +738,7 @@
break;
}
}
- //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
-
-
-
-
sumError += currentError;
derivError = currentError - previousError;
double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
@@ -768,55 +753,346 @@
if (leftSpeed < 0) leftSpeed = 0;
if (rightSpeed > 0.30) rightSpeed = 0.30;
if (rightSpeed < 0) rightSpeed = 0;
-
+
right_motor.forward(rightSpeed);
left_motor.forward(leftSpeed);
pidOnEncoders();
-
+
previousError = currentError;
ir2 = IRP_2.getSamples( SAMPLE_NUM );
ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
+ }
+ 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;
+ }
+
left_motor.brake();
right_motor.brake();
return;
}
+bool isWallInFront(int x, int y){
+ return (wallArray[MAZE_LEN - y - 1][x] & 0x1);
+}
+bool isWallInBack(int x, int y){
+ return (wallArray[MAZE_LEN - y - 1][x] & 0x8);
+}
+bool isWallOnRight(int x, int y){
+ return (wallArray[MAZE_LEN - y - 1][x] & 0x4);
+}
+bool isWallOnLeft(int x, int y){
+ return (wallArray[MAZE_LEN - y - 1][x] & 0x2);
+}
+
+int chooseNextMovement(){
+ int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
+ if (goingToCenter && (currentDistance == 0)){
+ goingToCenter = false;
+ changeManhattanDistance(goingToCenter);
+ }
+ else if (!goingToCenter && (currentDistance == 0)){
+ goingToCenter == true;
+ changeManhattanDistance(goingToCenter);
+ }
+
+ visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
+ int currX = 0;
+ int currY = 0;
+ int currDist = 0;
+ int dirToGo = 0;
+ if (!justTurned){
+ cellsToVisit.push(make_pair(mouseX, mouseY));
+ while (!cellsToVisit.empty()) {
+ pair<int, int> curr = cellsToVisit.top();
+ cellsToVisit.pop();
+ currX = curr.first;
+ currY = curr.second;
+ currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
+ int minDist = INT_MAX;
+
+ if (hasVisited(currX, currY)) { // then we want to actually see where the walls are, else we treat it as if there are no walls!
+ if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
+ minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
+ }
+ }
+ if (currX - 1 >= 0 && !isWallOnLeft((unsigned) currX, (unsigned) 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 (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 (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
+ minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
+ }
+ }
+ } else {
+ if (currX + 1 < MAZE_LEN) {
+ if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
+ minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
+ }
+ }
+ if (currX - 1 >= 0) {
+ if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
+ minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
+ }
+ }
+ if (currY + 1 < MAZE_LEN) {
+ if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
+ minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
+ }
+ }
+ if (currY - 1 >= 0) {
+ if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
+ minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
+ }
+ }
+ }
+
+ if (minDist == INT_MAX)
+ continue;
+ 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)) {
+ if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
+ cellsToVisit.push(make_pair(currX, currY + 1));
+ }
+ if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
+ cellsToVisit.push(make_pair(currX + 1, currY));
+ }
+ if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
+ cellsToVisit.push(make_pair(currX, currY - 1));
+ }
+ if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
+ cellsToVisit.push(make_pair(currX - 1, currY));
+ }
+ } else {
+ if (currY + 1 < MAZE_LEN) {
+ cellsToVisit.push(make_pair(currX, currY + 1));
+ }
+ if (currX + 1 < MAZE_LEN) {
+ cellsToVisit.push(make_pair(currX + 1, currY));
+ }
+ if (currY - 1 >= 0) {
+ cellsToVisit.push(make_pair(currX, currY - 1));
+ }
+ if (currX - 1 >= 0) {
+ cellsToVisit.push(make_pair(currX - 1, currY));
+ }
+ }
+ }
+
+ int minDistance = INT_MAX;
+ if (currDir % 4 == 0) {
+ if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
+ dirToGo = 1;
+ }
+ }
+ if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
+ dirToGo = 2;
+ }
+ }
+ if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
+ dirToGo = 3;
+ }
+ }
+ if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
+ dirToGo = 4;
+ }
+ }
+ } else if (currDir % 4 == 2) {
+ if (mouseX - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
+ dirToGo = 1;
+ }
+ }
+ if (mouseX + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
+ dirToGo = 2;
+ }
+ }
+ if (mouseY - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
+ dirToGo = 3;
+ }
+ }
+ if (mouseY + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
+ dirToGo = 4;
+ }
+ }
+ } else if (currDir % 4 == 1) {
+ if (mouseY - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
+ dirToGo = 1;
+ }
+ }
+ 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];
+ dirToGo = 2;
+ }
+ }
+ if (mouseX + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
+ dirToGo = 3;
+ }
+ }
+ if (mouseX - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
+ dirToGo = 4;
+ }
+ }
+ } else if (currDir % 4 == 3) {
+ if (mouseY + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
+ dirToGo = 1;
+ }
+ }
+ if (mouseY - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
+ dirToGo = 2;
+ }
+ }
+ if (mouseX - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
+ dirToGo = 3;
+ }
+ }
+ if (mouseX + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
+ if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
+ minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
+ dirToGo = 4;
+ }
+ }
+ }
+ }
+ else{
+ justTurned = false;
+ dirToGo = 0;
+ }
+
+ return dirToGo;
+}
+
+bool hasVisited(int x, int y){
+ return visitedCells[MAZE_LEN - 1 - y][x];
+}
+
+void changeManhattanDistance(bool headCenter){
+ if (headCenter){
+ for (int i = 0; i < MAZE_LEN / 2; i++) {
+ for (int j = 0; j < MAZE_LEN / 2; j++) {
+ manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(7 - i);
+ }
+ }
+ for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
+ for (int j = 0; j < MAZE_LEN / 2; j++) {
+ manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(8 - i);
+ }
+ }
+ for (int i = 0; i < MAZE_LEN / 2; i++) {
+ for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
+ manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(7 - i);
+ }
+ }
+ for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
+ for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
+ manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(8 - i);
+ }
+ }
+ }
+ else {
+ for (int i = 0; i < MAZE_LEN / 2; i++) {
+ for (int j = 0; j < MAZE_LEN / 2; j++) {
+ manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
+ }
+ }
+ for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
+ for (int j = 0; j < MAZE_LEN / 2; j++) {
+ manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
+ }
+ }
+ for (int i = 0; i < MAZE_LEN / 2; i++) {
+ for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
+ manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
+ }
+ }
+ for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
+ for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
+ manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
+ }
+ }
+ }
+}
+
int main()
{
//Set highest bandwidth.
//gyro.setLpBandwidth(LPFBW_42HZ);
serial.baud(9600);
//serial.printf("The gyro's address is %s", gyro.getWhoAmI());
-
+
wait (0.1);
-
-
+
redLed.write(1);
greenLed.write(0);
blueLed.write(1);
-
+
//left_motor.forward(0.1);
//right_motor.forward(0.1);
-
+
// PA_1 is A of right
// PA_0 is B of right
// PA_5 is A of left
// PB_3 is B of left
//QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
-// QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
-
+ // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
+
// TODO: Setting all the registers and what not for Quadrature Encoders
/* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
*/
-
+
// set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type
// of alternate function specified
// 4 modes sets AHB1ENR
@@ -830,100 +1106,71 @@
// SMCR - encoder mode
// CR1 reenabales
// then read CNT reg of timer
-
-
+
+
dipButton1.rise(&enableButton1);
dipButton2.rise(&enableButton2);
dipButton3.rise(&enableButton3);
dipButton4.rise(&enableButton4);
-
+
dipButton1.fall(&disableButton1);
dipButton2.fall(&disableButton2);
dipButton3.fall(&disableButton3);
dipButton4.fall(&disableButton4);
+ // if(dipFlags == 0x1){
+ turnRight180();
+ wait_ms(1000);
+ // }else{
+ // turnRight();
+ // IRP_1.calibrateSensor();
+ // IRP_4.calibrateSensor();
+ // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
+ // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
+
+ // wait_ms(300);
+ // turnLeft();
+ // wait_ms(300);
+ // }
+
+
+ // init the wall, and mouse loc arrays:
+ wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
+ visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
+
//right_motor.forward( 0.2 );
//left_motor.forward( 0.2 );
- turnRight180();
- wait_ms(1500);
+ //turnRight180();
+ //wait_ms(1500);
+ int nextMovement = 0;
+ while (1) {
+ nextMovement = chooseNextMovement();
+ if (nextMovement == 0){
+ nCellEncoderAndIR(1);
+ }
+ else if (nextMovement == 1){
+ justTurned = true;
+ turnRight();
+ }
+ else if (nextMovement == 2){
+ justTurned = true;
+ turnLeft();
+ }
+ else if (nextMovement == 3){
+ nCellEncoderAndIR(1);
+ }
+ else if (nextMovement == 4){
+ justTurned = true;
+ turnRight180();
+ }
+ wait_ms(1000); // reduce this once we fine tune this!
-
- while (1) {
//wait_ms(1500);
//turnRight();
//wait_ms(1500);
//turnLeft();
-
-
- // float ir2 = IRP_2.getSamples(100);
- // float ir3 = IRP_3.getSamples(100);
- // float ir1 = IRP_1.getSamples(100);
- // float ir4 = IRP_4.getSamples(100);
-
-
- // if( ir1 > IRP_1.sensorAvg*0.3 || ir4 > IRP_4.sensorAvg*0.3 ){
- // // almost to the end
- // redLed.write(1);
- // greenLed.write(1);
- // blueLed.write(1);
- // wait_ms(200);
- // redLed.write(1);
- // greenLed.write(1);
- // blueLed.write(0);
- // wait_ms(200);
- // redLed.write(1);
- // greenLed.write(0);
- // blueLed.write(1);
- // wait_ms(200);
- // redLed.write(0);
- // greenLed.write(1);
- // blueLed.write(1);
-
- // }else if((ir3 < IRP_3.sensorAvg/(averageDivR)) && (ir2 < IRP_2.sensorAvg/(averageDivL)) ){
- // // both sides gone
- // redLed.write(1);
- // greenLed.write(1);
- // blueLed.write(1);
- // wait_ms(100);
- // redLed.write(1);
- // greenLed.write(1);
- // blueLed.write(0);
- // wait_ms(200);
- // redLed.write(1);
- // greenLed.write(1);
- // blueLed.write(0);
- // wait_ms(200);
- // redLed.write(1);
- // greenLed.write(1);
- // blueLed.write(0);
- // wait_ms(200);
- // redLed.write(1);
- // greenLed.write(1);
- // blueLed.write(0);
- // }else if (ir3 < IRP_3.sensorAvg/averageDivR){// right wall gone
- // // RED RED RED RED RED
- // redLed.write(0);
- // greenLed.write(1);
- // blueLed.write(1);
- // }else if (ir2 < IRP_2.sensorAvg/averageDivL){// left wall gone
- // // BLUE BLUE BLUE BLUE
- // redLed.write(1);
- // greenLed.write(1);
- // blueLed.write(0);
- // }else if ((ir2 > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (ir3 > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
- // // both walls there
- // redLed.write(1);
- // greenLed.write(0);
- // blueLed.write(1);
- // }
-
-
-
-
- nCellEncoderAndIR(1);
- wait_ms(1000);
-
-
+ // nCellEncoderAndIR(1);
+ // wait_ms(500);
// turnRight();
// wait_ms(500);
// nCellEncoderAndIR(1);
@@ -945,9 +1192,7 @@
// nCellEncoderAndIR(5);
// break;
// turnRight180();
-
-
-
+
// int number = rand() % 4 + 1;
// switch(number){
// case(1):{//turn right
@@ -959,7 +1204,7 @@
// break;
// }
// case(3):{// keep going
-
+
// break;
// }
// case(4):{// turnaround
@@ -970,34 +1215,26 @@
// break;
// }
// }
-
+
// float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
// float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
-
+
// float ir3 = IRP_2.getSamples(100)/initAverageL;
// float ir2 = IRP_3.getSamples(100)/initAverageR;
- //serial.printf("%f, %f \n", IRP_1.sensorAvg, IRP_4.sensorAvg);
- //serial.printf("%f, %f \n", IRP_2.sensorAvg, IRP_3.sensorAvg);
- //break;
-
- //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
- //serial.printf("IRS= >: %f, %f \r\n", IRP_1.getSamples(100), IRP_4.getSamples(100));
-
-
-
+
/*
counter2++;
counter3++;
ir2tot += IRP_2.getSamples(100);
ir3tot += IRP_3.getSamples(100);
-
-
+
+
ir2 = ir2tot/counter2;
ir3 = ir3tot/counter3;
-
+
serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
*/
//serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
@@ -1006,20 +1243,17 @@
//serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
//serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
//serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
-
-
+
+
////////////////////////////////////////////////////////////////
-
+
//nCellEncoderAndIR(3);
//break;
-
+
//serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100));
-
-
//serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
-
-
+
//break;
// moveForwardCellEncoder(1);
// wait(0.5);
@@ -1035,8 +1269,8 @@
//serial.printf("Pulse Count=> e0:%d, e1:%d \r\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 );
-
+
//reading = Rec_4.read();
// serial.printf("reading: %f\n", reading);
}
-}
+}
\ No newline at end of file