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?
User | Revision | Line number | New 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 | } |