MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 31:9b71b44e0867
- Parent:
- 29:ec2c5a69acd6
- Child:
- 32:69acb14778ea
--- a/main.cpp Wed May 24 02:40:36 2017 +0000
+++ b/main.cpp Wed May 24 21:47:21 2017 +0000
@@ -1,9 +1,9 @@
#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
@@ -11,62 +11,61 @@
#include "ITG3200.h"
#include "stm32f4xx.h"
#include "QEI.h"
-
-
//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
+
+//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
void pidOnEncoders();
-void moveForwardCellEncoder(double cellNum){
+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);
- while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
+ 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){
+ if (receiverThreeReading < ir3base) {
// redLed.write(1);
// blueLed.write(0);
turnFlag |= RIGHT_FLAG;
- }
- else if (receiverTwoReading < ir2base){
+ } else if (receiverTwoReading < ir2base) {
// redLed.write(0);
// blueLed.write(1);
turnFlag |= LEFT_FLAG;
}
pidOnEncoders();
}
-
+
left_motor.brake();
right_motor.brake();
}
-
-
-void moveForwardEncoder(double num){
-
+
+
+void moveForwardEncoder(double num)
+{
+
int count0;
int count1;
count0 = encoder0.getPulses();
@@ -82,12 +81,11 @@
double speed1 = 0.12;
right_motor.move(speed0);
left_motor.move(speed1);
-
-
- 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 ){
+
+ 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 ));
-
+
count0 = encoder0.getPulses() - initial0;
count1 = encoder1.getPulses() - initial1;
int x = count0 - count1;
@@ -95,36 +93,34 @@
double kppart = kp * x;
double kdpart = kd * (x-prev);
double d = kppart + kdpart;
-
+
//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
- {
+ if( x < diff - 40 ) { // count1 is bigger, right wheel pushed forward
left_motor.move( speed1-0.8*d );
right_motor.move( speed0+d );
- }
- else if( x > diff + 40 )
- {
+ } else if( x > diff + 40 ) {
left_motor.move( speed1-0.8*d );
right_motor.move( speed0+d );
}
// else
// {
// left_motor.brake();
- // right_motor.brake();
+ // right_motor.brake();
// }
- prev = x;
+ prev = x;
}
-
+
//pidOnEncoders();
//pidBrake();
right_motor.brake();
left_motor.brake();
return;
}
-
-
-void moveForwardWallEncoder(){
- int count0;
+
+
+void moveForwardWallEncoder()
+{
+ int count0;
int count1;
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
@@ -143,15 +139,15 @@
float ir1 = IRP_1.getSamples(50);
float ir4 = IRP_4.getSamples(50);
- if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4){
+ if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.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( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) {
//serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
-
+
count0 = encoder0.getPulses() - initial0;
count1 = encoder1.getPulses() - initial1;
int x = count0 - count1;
@@ -159,24 +155,21 @@
double kppart = kp * x;
double kdpart = kd * (x-prev);
double d = kppart + kdpart;
-
+
//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
- {
+ if( x < diff - 40 ) { // count1 is bigger, right wheel pushed forward
left_motor.move( speed1-0.8*d );
right_motor.move( speed0+d );
- }
- else if( x > diff + 40 )
- {
+ } else if( x > diff + 40 ) {
left_motor.move( speed1-0.8*d );
right_motor.move( speed0+d );
}
// else
// {
// left_motor.brake();
- // right_motor.brake();
+ // right_motor.brake();
// }
- prev = x;
+ prev = x;
ir1 = IRP_1.getSamples(50);
ir4 = IRP_4.getSamples(50);
}
@@ -187,7 +180,7 @@
left_motor.brake();
return;
}
-
+
void moveForwardUntilWallIr()
{
double currentError = 0;
@@ -205,16 +198,16 @@
int count = encoder0.getPulses();
while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered
-
+
if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
- //moveForwardWallEncoder();
- }else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone
+ //moveForwardWallEncoder();
+ } else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005) { // left wall gone
//moveForwardRightWall();
- }else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone
+ } else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005) { // right wall gone
//moveForwardLeftWall();
- }else{
- // will move forward using encoders only
- // if cell ahead doesn't have a wall on either one side or both sides
+ } else {
+ // will move forward using encoders only
+ // if cell ahead doesn't have a wall on either one side or both sides
int pulseCount = (encoder0.getPulses()-count) % 5600;
if (pulseCount > 5400 && pulseCount < 5800) {
@@ -257,12 +250,12 @@
right_motor.brake();
}
}
-
+
void printDipFlag()
{
if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
}
-
+
void enableButton1()
{
dipFlags |= BUTTON1_FLAG;
@@ -303,7 +296,7 @@
dipFlags &= ~BUTTON4_FLAG;
printDipFlag();
}
-
+
void pidOnEncoders()
{
int count0;
@@ -314,10 +307,9 @@
double kp = 0.00013;
double kd = 0.00016;
int prev = 0;
-
+
int counter = 0;
- while(1)
- {
+ while(1) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
int x = count0 - count1;
@@ -325,22 +317,19 @@
double kppart = kp * x;
double kdpart = kd * (x-prev);
double d = kppart + kdpart;
-
+
//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 - 60 ) // count1 is bigger, right wheel pushed forward
- {
+ if( x < diff - 60 ) { // count1 is bigger, right wheel pushed forward
left_motor.move( -d );
right_motor.move( d );
- }
- else if( x > diff + 60 )
- {
+ } else if( x > diff + 60 ) {
left_motor.move( -d );
right_motor.move( d );
}
// else
// {
// left_motor.brake();
- // right_motor.brake();
+ // right_motor.brake();
// }
prev = x;
counter++;
@@ -348,8 +337,9 @@
break;
}
}
-
-void nCellEncoderAndIR(double cellCount){
+
+void nCellEncoderAndIR(double cellCount)
+{
double currentError = 0;
double previousError = 0;
double derivError = 0;
@@ -375,29 +365,17 @@
int state = 0;
- while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
+ while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
receiverTwoReading = IRP_2.getSamples(100);
receiverThreeReading = IRP_3.getSamples(100);
receiverOneReading = IRP_1.getSamples(100);
receiverFourReading = IRP_4.getSamples(100);
- if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){
- 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] |= L_WALL;
- }
- else if (currDir % 4 == 3){
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
- }
+ if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ) {
break;
}
- if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))){
+ if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))) {
// both sides gone
int prev0 = encoder0.getPulses();
int prev1 = encoder1.getPulses();
@@ -405,60 +383,18 @@
int diff1 = desiredCount1 - prev1;
int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
moveForwardEncoder(valToPass);
- }
- else if (receiverThreeReading < 3*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
+ } else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR) { // right wall gone
state = 1;
redLed.write(0);
greenLed.write(1);
blueLed.write(1);
- }else if (receiverTwoReading < 3*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;
- // }
+ } else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL) { // left wall gone
// 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;
- // }
+ } else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))) {
// both walls there
state = 0;
redLed.write(1);
@@ -466,20 +402,20 @@
blueLed.write(1);
}
- switch(state){
- case(0):{ // both walls there
+ switch(state) {
+ case(0): { // both walls there
currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
break;
}
- case(1):{// RED RED RED RED RED
+ case(1): { // RED RED RED RED RED
currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
- break;
+ break;
}
- case(2):{// blue
+ case(2): { // blue
currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
break;
}
- default:{
+ default: {
currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
break;
}
@@ -494,58 +430,98 @@
} else { // r is faster than L. speed up l, slow down r
rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
- }
+ }
if (leftSpeed > 0.30) leftSpeed = 0.30;
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;
}
- if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1){
- if (currDir % 4 == 0){
- mouseY += 1;
- }
- else if (currDir % 4 == 1){
+ if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1) {
+ if (currDir % 4 == 0) {
+ mouseY += 1;
+ } else if (currDir % 4 == 1) {
mouseX + 1;
- }
- else if (currDir % 4 == 2){
+ } else if (currDir % 4 == 2) {
mouseY -= 1;
- }
- else if (currDir % 4 == 3){
+ } else if (currDir % 4 == 3) {
mouseX -= 1;
}
+
+ // the mouse has moved forward, we need to update the wall map now
+ receiverOneReading = IRP_1.getSamples(50);
+ receiverTwoReading = IRP_2.getSamples(50);
+ receiverThreeReading = IRP_3.getSamples(50);
+ receiverFourReading = IRP_4.getSamples(50);
+
+ if (receiverOneReading >= 0.5f && receiverFourReading >= 0.5f) {
+ 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] |= L_WALL;
+ } else if (currDir % 4 == 3) {
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
+ }
+ }
+ if (receiverThreeReading >= 0.1f) {
+ 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 (receiverTwoReading >= 0.1f) {
+ 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;
+ }
+ }
}
-
-
+
left_motor.brake();
right_motor.brake();
}
-bool isWallInFront(int x, int y){
+bool isWallInFront(int x, int y)
+{
return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
}
-bool isWallInBack(int x, int y){
+bool isWallInBack(int x, int y)
+{
return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
}
-bool isWallOnRight(int x, int y){
+bool isWallOnRight(int x, int y)
+{
return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
}
-bool isWallOnLeft(int x, int y){
+bool isWallOnLeft(int x, int y)
+{
return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
}
-int chooseNextMovement(){
+int chooseNextMovement()
+{
int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
- if (goingToCenter && (currentDistance == 0)){
+ if (goingToCenter && (currentDistance == 0)) {
goingToCenter = false;
changeManhattanDistance(goingToCenter);
- }
- else if (!goingToCenter && (currentDistance == 0)){
+ } else if (!goingToCenter && (currentDistance == 0)) {
goingToCenter == true;
changeManhattanDistance(goingToCenter);
}
@@ -555,7 +531,7 @@
int currY = 0;
int currDist = 0;
int dirToGo = 0;
- if (!justTurned){
+ if (!justTurned) {
cellsToVisit.push(make_pair(mouseX, mouseY));
while (!cellsToVisit.empty()) {
pair<int, int> curr = cellsToVisit.top();
@@ -565,7 +541,7 @@
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 (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];
@@ -747,8 +723,7 @@
}
}
}
- }
- else{
+ } else {
justTurned = false;
dirToGo = 0;
}
@@ -756,12 +731,14 @@
return dirToGo;
}
-bool hasVisited(int x, int y){
+bool hasVisited(int x, int y)
+{
return visitedCells[MAZE_LEN - 1 - y][x];
}
-void changeManhattanDistance(bool headCenter){
- if (headCenter){
+void changeManhattanDistance(bool headCenter)
+{
+ if (headCenter) {
for (int i = 0; i < MAZE_LEN; i++) {
for (int j = 0; j < MAZE_LEN; j++) {
distanceToCenter[i][j] = manhattanDistances[i][j];
@@ -772,9 +749,8 @@
manhattanDistances[i][j] = distanceToStart[i][j];
}
}
- }
- else {
- for (int i = 0; i < MAZE_LEN; i++) {
+ } else {
+ for (int i = 0; i < MAZE_LEN; i++) {
for (int j = 0; j < MAZE_LEN; j++) {
distanceToStart[i][j] = manhattanDistances[i][j];
}
@@ -787,29 +763,30 @@
}
}
-void initializeDistances(){
+void initializeDistances()
+{
for (int i = 0; i < MAZE_LEN / 2; i++) {
- for (int j = 0; j < MAZE_LEN / 2; j++) {
- distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
- }
+ for (int j = 0; j < MAZE_LEN / 2; j++) {
+ distanceToStart[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++) {
- distanceToStart[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++) {
+ distanceToStart[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++) {
- distanceToStart[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++) {
+ distanceToStart[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++) {
- distanceToStart[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++) {
+ distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
}
+ }
}
-
+
int main()
{
//Set highest bandwidth.
@@ -817,30 +794,30 @@
initializeDistances();
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 );
-
+
// 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
@@ -854,21 +831,19 @@
// 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();
@@ -898,7 +873,7 @@
//wait_ms(1500);
int nextMovement = 0;
while (1) {
- // prevEncoder0Count = encoder0.getPulses();
+ // prevEncoder0Count = encoder0.getPulses();
// prevEncoder1Count = encoder1.getPulses();
//
// if (!overrideFloodFill){
@@ -935,7 +910,7 @@
// }
// currEncoder0Count = encoder0.getPulses();
// currEncoder1Count = encoder1.getPulses();
-//
+//
// if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
// overrideFloodFill = true;
// }
@@ -969,7 +944,7 @@
// nCellEncoderAndIR(5);
// break;
// turnRight180();
-
+
// int number = rand() % 4 + 1;
// switch(number){
// case(1):{//turn right
@@ -981,7 +956,7 @@
// break;
// }
// case(3):{// keep going
-
+
// break;
// }
// case(4):{// turnaround
@@ -992,26 +967,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;
-
-
+
+
/*
counter2++;
- counter3++;
+ 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);
@@ -1020,17 +995,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);
@@ -1041,11 +1016,11 @@
// handleTurns();
//break;
//pidOnEncoders();
- // moveForwardUntilWallIr();
+ // moveForwardUntilWallIr();
//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) ) ;
+ 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);
}