Jack Daniels
IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
Diff: main.cpp
- Revision:
- 7:60e5e064698d
- Parent:
- 6:2b89c05628ce
- Child:
- 8:c7d277d05f23
--- a/main.cpp Thu Oct 04 22:53:42 2012 +0000
+++ b/main.cpp Wed Oct 10 06:28:46 2012 +0000
@@ -7,6 +7,8 @@
AnalogIn irLeft(p20);
AnalogIn irFront(p19);
AnalogIn irRight(p18);
+AnalogIn dirLeft(p17);
+AnalogIn dirRight(p16);
HMC6352 compass(p9, p10);
enum direction{ Right, Forward, Left, TurnNorth };
@@ -16,72 +18,59 @@
compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
printf("Front Right Left\n\r");
- int i=0;
- float readLeftVals[10];
- float readRightVals[10];
- float readFrontVals[10];
+ int j=0;
float readLeft;
float readRight;
float readFront;
-
- //Initialize the averager before the robot even moves
- for (int j=0; j<10; j++);
- {
+ float diagRight;
+ float diagLeft;
+ float stable[5][4];
readLeft=irLeft;
- readLeft=21/readLeft;
-
readRight=irRight;
- readRight=21/readRight;
-
readFront=irFront;
- readFront=21/readFront;
-
- readFrontVals[i] = readFront;
- readRightVals[i] = readRight;
- readLeftVals[i] = readLeft;
- }
+ diagRight=dirRight;
+ diagLeft=dirLeft;
+ int NorthOn=0;
+ for(j=0;j<4;j++) stable[0][j] = readLeft;
+ for(j=0;j<4;j++) stable[1][j] = readRight;
+ for(j=0;j<4;j++) stable[2][j] = diagLeft;
+ for(j=0;j<4;j++) stable[3][j] = diagRight;
+ for(j=0;j<4;j++) stable[4][j] = readFront;
+ j=1;
while (1){
+ if(j>=4) j=0;
+
readLeft=irLeft;
+ readRight=irRight;
+ readFront=irFront;
+ diagRight=dirRight;
+ diagLeft=dirLeft;
readLeft=21/readLeft;
-
- readRight=irRight;
readRight=21/readRight;
-
- readFront=irFront;
readFront=21/readFront;
-
-
-
-
-
- if (i>=10)
- {
- i=0;
- }
- readFrontVals[i] = readFront;
- readRightVals[i] = readRight;
- readLeftVals[i] = readLeft;
- i++;
-
+ diagRight=21/diagRight;
+ diagLeft=21/diagLeft;
float avgFront = 0, avgRight = 0, avgLeft = 0;
-
- for (int j=0; j<10; j++)
- {
- avgFront += readFrontVals[j]/10;
- avgLeft += readLeftVals[j]/10;
- avgRight += readRightVals[j]/10;
- }
avgFront = readFront;
avgRight = readRight;
- printf("Front: %5f Right: %5f Left: %5f", avgFront, avgRight, avgLeft);
+ avgLeft = readLeft;
+stable[0][j] = avgLeft;
+stable[1][j] = avgRight;
+stable[2][j] = diagLeft;
+stable[3][j] = diagRight;
+stable[4][j] = avgFront;
+
+j++;
+ printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight);
float globalDir = compass.sample()/10.0;
printf(" Heading: %f\n\r", globalDir);
-
+
switch(dir){
case Forward:
- printf("CHARGE!n\r");
- if (avgFront <= 40)
+ if ((avgFront<40&&stable[4][0]<40&&stable[4][1]<40&&stable[4][2]<40&&stable[4][3]<40)||
+ (diagLeft<40&&stable[2][0]<40&&stable[2][1]<40&&stable[2][2]<40&&stable[2][3]<40)||
+ /*(avgLeft<40&&stable[0][0]<40&&stable[0][1]<40&&stable[0][2]<40&&stable[0][3]<40)*/)
{
right.speed(0);
left.speed(0);
@@ -89,58 +78,59 @@
//wait(0.1);
dir=Right;
}
- else
+ else if((diagRight<40&&stable[3][0]<40&&stable[3][1]<40&&stable[3][2]<40&&stable[3][3]<40)||
+ /*(avgRight<40&&stable[1][0]<40&&stable[1][1]<40&&stable[1][2]<40&&stable[1][3]<40))*/
{
- if (globalDir > 45 && globalDir <= 135)
- {
- //East
- if (avgLeft >= 50)
- {
+ right.speed(0);
+ left.speed(0);
+ printf("stopped2!\n\r");
+ //wait(0.1);
+ dir=Left;
+ }
+ else if(
+ (((globalDir > 30 && globalDir <= 180)||(globalDir > 180 && globalDir <= 330))&&
+ )
+ )
+ {
right.speed(0);
left.speed(0);
dir = TurnNorth;
- }
- else
- {
- right.speed(1);
- left.speed(1);
- }
- }
- else if (globalDir > 135 && globalDir <= 225)
- {
- //It's going south O.o
- right.speed(1);
- left.speed(1);
- }
- else if (globalDir > 225 && globalDir <= 315)
- {
- //West
- if (avgRight >= 50)
- {
- right.speed(0);
- left.speed(0);
- dir = TurnNorth;
- }
- else
- {
- right.speed(1);
- left.speed(1);
- }
- }
- else
- {
- //It's going north!
- right.speed(1);
- left.speed(1);
- }
+ }
+ else {right.speed(0.95); left.speed(1);
+ printf("CHARGE!\n\r");
}
break;
case Right:
- printf("TURN!\n\r");
- if (/*avgLeft>=50 ||*/ avgFront<=40)
+ printf("TURN Right!\n\r");
+ if (
+ ((avgFront<=31.7||stable[4][0]<30||stable[4][1]<30||stable[4][2]<30||stable[4][3]<30)
+ || (diagLeft<=31.7||stable[2][0]<30||stable[2][1]<30||stable[2][2]<30||stable[2][3]<30)
+ )
{
right.speed(-1);
+ left.speed(1);
+ // printf("turning!\n\r");
+ }
+ else
+ {
+ right.speed(0);
left.speed(0);
+ // printf("stopped!\n\r");
+ //wait(0.1);
+ dir=Forward;
+ }
+ break;
+
+ case Left:
+ printf("TURN Left!\n\r");
+ if (
+ /*(avgRight<=31.7 ||stable[1][0]<30||stable[1][1]<30||stable[1][2]<30||stable[1][3]<30)||*/
+ (avgFront<=31.7||stable[4][0]<30||stable[4][1]<30||stable[4][2]<30||stable[4][3]<30)
+ || (diagRight<=31.7||stable[3][0]<30||stable[3][1]<30||stable[3][2]<30||stable[3][3]<30)
+ )
+ {
+ right.speed(1);
+ left.speed(-1);
// printf("turning!\n\r");
}
else
@@ -157,14 +147,15 @@
if (globalDir>330 || globalDir < 30)
{
//It's going north!!
- right.speed(1);
- left.speed(1);
+ right.speed(0);
+ left.speed(0);
dir=Forward;
}
else
- {
- right.speed(.5);
- left.speed(-.5);
+ { if(globalDir<=180){
+ right.speed(1);
+ left.speed(-1);}
+ else{right.speed(-1); left.speed(1);}
}
break;
}