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