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?
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 | 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 | } |