Jack Daniels / Mbed 2 deprecated SantaSeeker

IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp@11:0fd03f66af0d, 2012-10-10 (annotated)

Committer:
vsavkin3
Date:
Wed Oct 10 08:27:10 2012 +0000
Revision:
11:0fd03f66af0d
Parent:
10:168a1b176c77
Child:
12:204600e080c4
FUUUUU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsavkin3 4:9011798d1a20 1 #include "mbed.h"
vsavkin3 4:9011798d1a20 2 #include "Motor.h"
vsavkin3 4:9011798d1a20 3 #include "HMC6352.h"
vsavkin3 4:9011798d1a20 4
vsavkin3 4:9011798d1a20 5 Motor right(p21, p23, p22); // pwm, fwd, rev
vsavkin3 4:9011798d1a20 6 Motor left(p26, p24, p25); // pwm, fwd, rev
vsavkin3 4:9011798d1a20 7 AnalogIn irLeft(p20);
vsavkin3 4:9011798d1a20 8 AnalogIn irFront(p19);
vsavkin3 4:9011798d1a20 9 AnalogIn irRight(p18);
vsavkin3 7:60e5e064698d 10 AnalogIn dirLeft(p17);
vsavkin3 7:60e5e064698d 11 AnalogIn dirRight(p16);
vsavkin3 5:99545cf4dff4 12 HMC6352 compass(p9, p10);
vsavkin3 4:9011798d1a20 13
vsavkin3 6:2b89c05628ce 14 enum direction{ Right, Forward, Left, TurnNorth };
vsavkin3 4:9011798d1a20 15
vsavkin3 4:9011798d1a20 16 int main() {
vsavkin3 4:9011798d1a20 17 enum direction dir=Forward;
vsavkin3 5:99545cf4dff4 18 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
vsavkin3 4:9011798d1a20 19
vsavkin3 11:0fd03f66af0d 20 //printf("Front Right Left\n\r");
vsavkin3 7:60e5e064698d 21 int j=0;
vsavkin3 4:9011798d1a20 22 float readLeft;
vsavkin3 4:9011798d1a20 23 float readRight;
vsavkin3 4:9011798d1a20 24 float readFront;
vsavkin3 7:60e5e064698d 25 float diagRight;
vsavkin3 7:60e5e064698d 26 float diagLeft;
vsavkin3 7:60e5e064698d 27 float stable[5][4];
vsavkin3 4:9011798d1a20 28 readLeft=irLeft;
vsavkin3 4:9011798d1a20 29 readRight=irRight;
vsavkin3 4:9011798d1a20 30 readFront=irFront;
vsavkin3 7:60e5e064698d 31 diagRight=dirRight;
vsavkin3 7:60e5e064698d 32 diagLeft=dirLeft;
vsavkin3 4:9011798d1a20 33
vsavkin3 7:60e5e064698d 34 int NorthOn=0;
vsavkin3 8:c7d277d05f23 35 int NorthCount=0;
vsavkin3 9:6d663bdfee7c 36 for(j=0;j<2;j++) stable[0][j] = readLeft;
vsavkin3 9:6d663bdfee7c 37 for(j=0;j<2;j++) stable[1][j] = readRight;
vsavkin3 9:6d663bdfee7c 38 for(j=0;j<2;j++) stable[2][j] = diagLeft;
vsavkin3 9:6d663bdfee7c 39 for(j=0;j<2;j++) stable[3][j] = diagRight;
vsavkin3 9:6d663bdfee7c 40 for(j=0;j<2;j++) stable[4][j] = readFront;
vsavkin3 7:60e5e064698d 41 j=1;
vsavkin3 4:9011798d1a20 42 while (1){
vsavkin3 9:6d663bdfee7c 43 if(j>=2) j=0;
vsavkin3 7:60e5e064698d 44
vsavkin3 4:9011798d1a20 45 readLeft=irLeft;
vsavkin3 7:60e5e064698d 46 readRight=irRight;
vsavkin3 7:60e5e064698d 47 readFront=irFront;
vsavkin3 7:60e5e064698d 48 diagRight=dirRight;
vsavkin3 7:60e5e064698d 49 diagLeft=dirLeft;
vsavkin3 4:9011798d1a20 50 readLeft=21/readLeft;
vsavkin3 4:9011798d1a20 51 readRight=21/readRight;
vsavkin3 4:9011798d1a20 52 readFront=21/readFront;
vsavkin3 7:60e5e064698d 53 diagRight=21/diagRight;
vsavkin3 7:60e5e064698d 54 diagLeft=21/diagLeft;
vsavkin3 4:9011798d1a20 55 float avgFront = 0, avgRight = 0, avgLeft = 0;
vsavkin3 4:9011798d1a20 56 avgFront = readFront;
vsavkin3 4:9011798d1a20 57 avgRight = readRight;
vsavkin3 7:60e5e064698d 58 avgLeft = readLeft;
vsavkin3 7:60e5e064698d 59 stable[0][j] = avgLeft;
vsavkin3 7:60e5e064698d 60 stable[1][j] = avgRight;
vsavkin3 7:60e5e064698d 61 stable[2][j] = diagLeft;
vsavkin3 7:60e5e064698d 62 stable[3][j] = diagRight;
vsavkin3 7:60e5e064698d 63 stable[4][j] = avgFront;
vsavkin3 7:60e5e064698d 64
vsavkin3 7:60e5e064698d 65 j++;
vsavkin3 11:0fd03f66af0d 66 //printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight);
vsavkin3 6:2b89c05628ce 67 float globalDir = compass.sample()/10.0;
vsavkin3 11:0fd03f66af0d 68 //printf(" Heading: %f\n\r", globalDir);
vsavkin3 7:60e5e064698d 69
vsavkin3 4:9011798d1a20 70 switch(dir){
vsavkin3 4:9011798d1a20 71 case Forward:
vsavkin3 9:6d663bdfee7c 72 if ((avgFront<35&&stable[4][0]<35&&stable[4][1]<35)/*&&stable[4][2]<35&&stable[4][3]<35)*/||
vsavkin3 9:6d663bdfee7c 73 (diagLeft<35&&stable[2][0]<35&&stable[2][1]<35/*&&stable[2][2]<35&&stable[2][3]<35)*/
vsavkin3 9:6d663bdfee7c 74 /*(avgLeft<40&&stable[0][0]<40&&stable[0][1]<40&&stable[0][2]<40&&stable[0][3]<40)*/))
vsavkin3 4:9011798d1a20 75 {
vsavkin3 4:9011798d1a20 76 right.speed(0);
vsavkin3 4:9011798d1a20 77 left.speed(0);
vsavkin3 11:0fd03f66af0d 78 //printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 79 //wait(0.1);
vsavkin3 8:c7d277d05f23 80 NorthOn=0;
vsavkin3 4:9011798d1a20 81 dir=Right;
vsavkin3 4:9011798d1a20 82 }
vsavkin3 9:6d663bdfee7c 83 else if((diagRight<35&&stable[3][0]<35&&stable[3][1]<35/*&&stable[3][2]<35&&stable[3][3]<35)*/
vsavkin3 9:6d663bdfee7c 84 /*(avgRight<40&&stable[1][0]<40&&stable[1][1]<40&&stable[1][2]<40&&stable[1][3]<40))*/))
vsavkin3 4:9011798d1a20 85 {
vsavkin3 7:60e5e064698d 86 right.speed(0);
vsavkin3 7:60e5e064698d 87 left.speed(0);
vsavkin3 11:0fd03f66af0d 88 //printf("stopped2!\n\r");
vsavkin3 7:60e5e064698d 89 //wait(0.1);
vsavkin3 8:c7d277d05f23 90 NorthOn=0;
vsavkin3 7:60e5e064698d 91 dir=Left;
vsavkin3 7:60e5e064698d 92 }
vsavkin3 10:168a1b176c77 93 else if(((globalDir > 30 && globalDir <= 180)||(globalDir > 180 && globalDir <= 330))&&NorthOn)
vsavkin3 10:168a1b176c77 94
vsavkin3 7:60e5e064698d 95 {
vsavkin3 6:2b89c05628ce 96 right.speed(0);
vsavkin3 6:2b89c05628ce 97 left.speed(0);
vsavkin3 6:2b89c05628ce 98 dir = TurnNorth;
vsavkin3 7:60e5e064698d 99 }
vsavkin3 11:0fd03f66af0d 100 else {right.speed(1); left.speed(1);
vsavkin3 11:0fd03f66af0d 101 //printf("CHARGE!\n\r");
vsavkin3 11:0fd03f66af0d 102 if((avgLeft>35&&stable[0][0]>35&&stable[0][1]>35)
vsavkin3 11:0fd03f66af0d 103 &&(avgRight>35&&stable[1][0]>35&&stable[1][1]>35)) NorthOn=1;
vsavkin3 4:9011798d1a20 104 }
vsavkin3 4:9011798d1a20 105 break;
vsavkin3 4:9011798d1a20 106 case Right:
vsavkin3 11:0fd03f66af0d 107 ////printf("TURN Right!\n\r");
vsavkin3 7:60e5e064698d 108 if (
vsavkin3 9:6d663bdfee7c 109 (avgFront<=35||stable[4][0]<35||stable[4][1]<35)/*||stable[4][2]<35||stable[4][3]<35)*/
vsavkin3 9:6d663bdfee7c 110 || (diagLeft<=35||stable[2][0]<35||stable[2][1]<35)/*||stable[2][2]<35||stable[2][3]<35)*/
vsavkin3 7:60e5e064698d 111 )
vsavkin3 4:9011798d1a20 112 {
vsavkin3 4:9011798d1a20 113 right.speed(-1);
vsavkin3 8:c7d277d05f23 114 left.speed(0);
vsavkin3 11:0fd03f66af0d 115 // //printf("turning!\n\r");
vsavkin3 7:60e5e064698d 116 }
vsavkin3 7:60e5e064698d 117 else
vsavkin3 9:6d663bdfee7c 118 {
vsavkin3 7:60e5e064698d 119 right.speed(0);
vsavkin3 4:9011798d1a20 120 left.speed(0);
vsavkin3 11:0fd03f66af0d 121 // //printf("stopped!\n\r");
vsavkin3 7:60e5e064698d 122 //wait(0.1);
vsavkin3 7:60e5e064698d 123 dir=Forward;
vsavkin3 7:60e5e064698d 124 }
vsavkin3 7:60e5e064698d 125 break;
vsavkin3 7:60e5e064698d 126
vsavkin3 7:60e5e064698d 127 case Left:
vsavkin3 11:0fd03f66af0d 128 //printf("TURN Left!\n\r");
vsavkin3 7:60e5e064698d 129 if (
vsavkin3 7:60e5e064698d 130 /*(avgRight<=31.7 ||stable[1][0]<30||stable[1][1]<30||stable[1][2]<30||stable[1][3]<30)||*/
vsavkin3 9:6d663bdfee7c 131 (avgFront<=35||(stable[4][0]<35||stable[4][1]<35))/*&&(stable[4][2]<35||stable[4][3]<35))*/
vsavkin3 9:6d663bdfee7c 132 ||(diagRight<=35||(stable[3][0]<35||stable[3][1]<35))/*&&(stable[3][2]<35||stable[3][3]<35))*/
vsavkin3 7:60e5e064698d 133 )
vsavkin3 7:60e5e064698d 134 {
vsavkin3 8:c7d277d05f23 135 right.speed(0);
vsavkin3 7:60e5e064698d 136 left.speed(-1);
vsavkin3 11:0fd03f66af0d 137 // //printf("turning!\n\r");
vsavkin3 4:9011798d1a20 138 }
vsavkin3 4:9011798d1a20 139 else
vsavkin3 4:9011798d1a20 140 {
vsavkin3 9:6d663bdfee7c 141
vsavkin3 4:9011798d1a20 142 right.speed(0);
vsavkin3 4:9011798d1a20 143 left.speed(0);
vsavkin3 11:0fd03f66af0d 144 // //printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 145 //wait(0.1);
vsavkin3 4:9011798d1a20 146 dir=Forward;
vsavkin3 4:9011798d1a20 147 }
vsavkin3 4:9011798d1a20 148 break;
vsavkin3 6:2b89c05628ce 149 case TurnNorth:
vsavkin3 11:0fd03f66af0d 150 //printf("TO THE NORTH!\n\r");
vsavkin3 10:168a1b176c77 151 if(
vsavkin3 10:168a1b176c77 152 (avgLeft<35||stable[0][0]<35||stable[0][1]<35)
vsavkin3 10:168a1b176c77 153 ||
vsavkin3 10:168a1b176c77 154 (avgRight<35||stable[1][0]<35||stable[1][1]<35)
vsavkin3 10:168a1b176c77 155 ||
vsavkin3 10:168a1b176c77 156 (diagLeft<35||stable[2][0]<35||stable[2][1]<35)
vsavkin3 10:168a1b176c77 157 ||
vsavkin3 10:168a1b176c77 158 (diagRight<35||stable[3][0]<35||stable[3][1]<35)
vsavkin3 10:168a1b176c77 159 ||
vsavkin3 10:168a1b176c77 160 (avgFront<35||stable[4][0]<35||stable[4][1]<35)
vsavkin3 11:0fd03f66af0d 161 )
vsavkin3 11:0fd03f66af0d 162 {right.speed(0);
vsavkin3 11:0fd03f66af0d 163 left.speed(0);
vsavkin3 11:0fd03f66af0d 164 dir=Forward;}
vsavkin3 6:2b89c05628ce 165 if (globalDir>330 || globalDir < 30)
vsavkin3 6:2b89c05628ce 166 {
vsavkin3 6:2b89c05628ce 167 //It's going north!!
vsavkin3 7:60e5e064698d 168 right.speed(0);
vsavkin3 7:60e5e064698d 169 left.speed(0);
vsavkin3 6:2b89c05628ce 170 dir=Forward;
vsavkin3 6:2b89c05628ce 171 }
vsavkin3 6:2b89c05628ce 172 else
vsavkin3 7:60e5e064698d 173 { if(globalDir<=180){
vsavkin3 7:60e5e064698d 174 right.speed(1);
vsavkin3 7:60e5e064698d 175 left.speed(-1);}
vsavkin3 7:60e5e064698d 176 else{right.speed(-1); left.speed(1);}
vsavkin3 6:2b89c05628ce 177 }
vsavkin3 6:2b89c05628ce 178 break;
vsavkin3 4:9011798d1a20 179 }
vsavkin3 4:9011798d1a20 180 wait(0.01);
vsavkin3 4:9011798d1a20 181 }
vsavkin3 4:9011798d1a20 182 }