Jack Daniels / Mbed 2 deprecated SantaSeeker

IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp@7:60e5e064698d, 2012-10-10 (annotated)

Committer:
vsavkin3
Date:
Wed Oct 10 06:28:46 2012 +0000
Revision:
7:60e5e064698d
Parent:
6:2b89c05628ce
Child:
8:c7d277d05f23
New Direction;

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 4:9011798d1a20 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 7:60e5e064698d 35 for(j=0;j<4;j++) stable[0][j] = readLeft;
vsavkin3 7:60e5e064698d 36 for(j=0;j<4;j++) stable[1][j] = readRight;
vsavkin3 7:60e5e064698d 37 for(j=0;j<4;j++) stable[2][j] = diagLeft;
vsavkin3 7:60e5e064698d 38 for(j=0;j<4;j++) stable[3][j] = diagRight;
vsavkin3 7:60e5e064698d 39 for(j=0;j<4;j++) stable[4][j] = readFront;
vsavkin3 7:60e5e064698d 40 j=1;
vsavkin3 4:9011798d1a20 41 while (1){
vsavkin3 7:60e5e064698d 42 if(j>=4) j=0;
vsavkin3 7:60e5e064698d 43
vsavkin3 4:9011798d1a20 44 readLeft=irLeft;
vsavkin3 7:60e5e064698d 45 readRight=irRight;
vsavkin3 7:60e5e064698d 46 readFront=irFront;
vsavkin3 7:60e5e064698d 47 diagRight=dirRight;
vsavkin3 7:60e5e064698d 48 diagLeft=dirLeft;
vsavkin3 4:9011798d1a20 49 readLeft=21/readLeft;
vsavkin3 4:9011798d1a20 50 readRight=21/readRight;
vsavkin3 4:9011798d1a20 51 readFront=21/readFront;
vsavkin3 7:60e5e064698d 52 diagRight=21/diagRight;
vsavkin3 7:60e5e064698d 53 diagLeft=21/diagLeft;
vsavkin3 4:9011798d1a20 54 float avgFront = 0, avgRight = 0, avgLeft = 0;
vsavkin3 4:9011798d1a20 55 avgFront = readFront;
vsavkin3 4:9011798d1a20 56 avgRight = readRight;
vsavkin3 7:60e5e064698d 57 avgLeft = readLeft;
vsavkin3 7:60e5e064698d 58 stable[0][j] = avgLeft;
vsavkin3 7:60e5e064698d 59 stable[1][j] = avgRight;
vsavkin3 7:60e5e064698d 60 stable[2][j] = diagLeft;
vsavkin3 7:60e5e064698d 61 stable[3][j] = diagRight;
vsavkin3 7:60e5e064698d 62 stable[4][j] = avgFront;
vsavkin3 7:60e5e064698d 63
vsavkin3 7:60e5e064698d 64 j++;
vsavkin3 7:60e5e064698d 65 printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight);
vsavkin3 6:2b89c05628ce 66 float globalDir = compass.sample()/10.0;
vsavkin3 6:2b89c05628ce 67 printf(" Heading: %f\n\r", globalDir);
vsavkin3 7:60e5e064698d 68
vsavkin3 4:9011798d1a20 69 switch(dir){
vsavkin3 4:9011798d1a20 70 case Forward:
vsavkin3 7:60e5e064698d 71 if ((avgFront<40&&stable[4][0]<40&&stable[4][1]<40&&stable[4][2]<40&&stable[4][3]<40)||
vsavkin3 7:60e5e064698d 72 (diagLeft<40&&stable[2][0]<40&&stable[2][1]<40&&stable[2][2]<40&&stable[2][3]<40)||
vsavkin3 7:60e5e064698d 73 /*(avgLeft<40&&stable[0][0]<40&&stable[0][1]<40&&stable[0][2]<40&&stable[0][3]<40)*/)
vsavkin3 4:9011798d1a20 74 {
vsavkin3 4:9011798d1a20 75 right.speed(0);
vsavkin3 4:9011798d1a20 76 left.speed(0);
vsavkin3 6:2b89c05628ce 77 printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 78 //wait(0.1);
vsavkin3 4:9011798d1a20 79 dir=Right;
vsavkin3 4:9011798d1a20 80 }
vsavkin3 7:60e5e064698d 81 else if((diagRight<40&&stable[3][0]<40&&stable[3][1]<40&&stable[3][2]<40&&stable[3][3]<40)||
vsavkin3 7:60e5e064698d 82 /*(avgRight<40&&stable[1][0]<40&&stable[1][1]<40&&stable[1][2]<40&&stable[1][3]<40))*/
vsavkin3 4:9011798d1a20 83 {
vsavkin3 7:60e5e064698d 84 right.speed(0);
vsavkin3 7:60e5e064698d 85 left.speed(0);
vsavkin3 7:60e5e064698d 86 printf("stopped2!\n\r");
vsavkin3 7:60e5e064698d 87 //wait(0.1);
vsavkin3 7:60e5e064698d 88 dir=Left;
vsavkin3 7:60e5e064698d 89 }
vsavkin3 7:60e5e064698d 90 else if(
vsavkin3 7:60e5e064698d 91 (((globalDir > 30 && globalDir <= 180)||(globalDir > 180 && globalDir <= 330))&&
vsavkin3 7:60e5e064698d 92 )
vsavkin3 7:60e5e064698d 93 )
vsavkin3 7:60e5e064698d 94 {
vsavkin3 6:2b89c05628ce 95 right.speed(0);
vsavkin3 6:2b89c05628ce 96 left.speed(0);
vsavkin3 6:2b89c05628ce 97 dir = TurnNorth;
vsavkin3 7:60e5e064698d 98 }
vsavkin3 7:60e5e064698d 99 else {right.speed(0.95); left.speed(1);
vsavkin3 7:60e5e064698d 100 printf("CHARGE!\n\r");
vsavkin3 4:9011798d1a20 101 }
vsavkin3 4:9011798d1a20 102 break;
vsavkin3 4:9011798d1a20 103 case Right:
vsavkin3 7:60e5e064698d 104 printf("TURN Right!\n\r");
vsavkin3 7:60e5e064698d 105 if (
vsavkin3 7:60e5e064698d 106 ((avgFront<=31.7||stable[4][0]<30||stable[4][1]<30||stable[4][2]<30||stable[4][3]<30)
vsavkin3 7:60e5e064698d 107 || (diagLeft<=31.7||stable[2][0]<30||stable[2][1]<30||stable[2][2]<30||stable[2][3]<30)
vsavkin3 7:60e5e064698d 108 )
vsavkin3 4:9011798d1a20 109 {
vsavkin3 4:9011798d1a20 110 right.speed(-1);
vsavkin3 7:60e5e064698d 111 left.speed(1);
vsavkin3 7:60e5e064698d 112 // printf("turning!\n\r");
vsavkin3 7:60e5e064698d 113 }
vsavkin3 7:60e5e064698d 114 else
vsavkin3 7:60e5e064698d 115 {
vsavkin3 7:60e5e064698d 116 right.speed(0);
vsavkin3 4:9011798d1a20 117 left.speed(0);
vsavkin3 7:60e5e064698d 118 // printf("stopped!\n\r");
vsavkin3 7:60e5e064698d 119 //wait(0.1);
vsavkin3 7:60e5e064698d 120 dir=Forward;
vsavkin3 7:60e5e064698d 121 }
vsavkin3 7:60e5e064698d 122 break;
vsavkin3 7:60e5e064698d 123
vsavkin3 7:60e5e064698d 124 case Left:
vsavkin3 7:60e5e064698d 125 printf("TURN Left!\n\r");
vsavkin3 7:60e5e064698d 126 if (
vsavkin3 7:60e5e064698d 127 /*(avgRight<=31.7 ||stable[1][0]<30||stable[1][1]<30||stable[1][2]<30||stable[1][3]<30)||*/
vsavkin3 7:60e5e064698d 128 (avgFront<=31.7||stable[4][0]<30||stable[4][1]<30||stable[4][2]<30||stable[4][3]<30)
vsavkin3 7:60e5e064698d 129 || (diagRight<=31.7||stable[3][0]<30||stable[3][1]<30||stable[3][2]<30||stable[3][3]<30)
vsavkin3 7:60e5e064698d 130 )
vsavkin3 7:60e5e064698d 131 {
vsavkin3 7:60e5e064698d 132 right.speed(1);
vsavkin3 7:60e5e064698d 133 left.speed(-1);
vsavkin3 4:9011798d1a20 134 // printf("turning!\n\r");
vsavkin3 4:9011798d1a20 135 }
vsavkin3 4:9011798d1a20 136 else
vsavkin3 4:9011798d1a20 137 {
vsavkin3 4:9011798d1a20 138 right.speed(0);
vsavkin3 4:9011798d1a20 139 left.speed(0);
vsavkin3 4:9011798d1a20 140 // printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 141 //wait(0.1);
vsavkin3 4:9011798d1a20 142 dir=Forward;
vsavkin3 4:9011798d1a20 143 }
vsavkin3 4:9011798d1a20 144 break;
vsavkin3 6:2b89c05628ce 145 case TurnNorth:
vsavkin3 6:2b89c05628ce 146 printf("TO THE NORTH!\n\r");
vsavkin3 6:2b89c05628ce 147 if (globalDir>330 || globalDir < 30)
vsavkin3 6:2b89c05628ce 148 {
vsavkin3 6:2b89c05628ce 149 //It's going north!!
vsavkin3 7:60e5e064698d 150 right.speed(0);
vsavkin3 7:60e5e064698d 151 left.speed(0);
vsavkin3 6:2b89c05628ce 152 dir=Forward;
vsavkin3 6:2b89c05628ce 153 }
vsavkin3 6:2b89c05628ce 154 else
vsavkin3 7:60e5e064698d 155 { if(globalDir<=180){
vsavkin3 7:60e5e064698d 156 right.speed(1);
vsavkin3 7:60e5e064698d 157 left.speed(-1);}
vsavkin3 7:60e5e064698d 158 else{right.speed(-1); left.speed(1);}
vsavkin3 6:2b89c05628ce 159 }
vsavkin3 6:2b89c05628ce 160 break;
vsavkin3 4:9011798d1a20 161 }
vsavkin3 4:9011798d1a20 162 wait(0.01);
vsavkin3 4:9011798d1a20 163 }
vsavkin3 4:9011798d1a20 164 }