Jack Daniels / Mbed 2 deprecated SantaSeeker

IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp@15:ff95ff245fc0, 2012-10-10 (annotated)

Committer:
vsavkin3
Date:
Wed Oct 10 20:17:04 2012 +0000
Revision:
15:ff95ff245fc0
Parent:
14:9f5ce2fd2cfc 
Final Santa Seeker

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