IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
main.cpp@6:2b89c05628ce, 2012-10-04 (annotated)
- Committer:
- vsavkin3
- Date:
- Thu Oct 04 22:53:42 2012 +0000
- Revision:
- 6:2b89c05628ce
- Parent:
- 5:99545cf4dff4
- Child:
- 7:60e5e064698d
Bot successfully went around an obstacle and turned north! Once... good stopping point, however :D
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 | 5:99545cf4dff4 | 10 | HMC6352 compass(p9, p10); |
vsavkin3 | 4:9011798d1a20 | 11 | |
vsavkin3 | 6:2b89c05628ce | 12 | enum direction{ Right, Forward, Left, TurnNorth }; |
vsavkin3 | 4:9011798d1a20 | 13 | |
vsavkin3 | 4:9011798d1a20 | 14 | int main() { |
vsavkin3 | 4:9011798d1a20 | 15 | enum direction dir=Forward; |
vsavkin3 | 5:99545cf4dff4 | 16 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
vsavkin3 | 4:9011798d1a20 | 17 | |
vsavkin3 | 4:9011798d1a20 | 18 | printf("Front Right Left\n\r"); |
vsavkin3 | 4:9011798d1a20 | 19 | int i=0; |
vsavkin3 | 4:9011798d1a20 | 20 | float readLeftVals[10]; |
vsavkin3 | 4:9011798d1a20 | 21 | float readRightVals[10]; |
vsavkin3 | 4:9011798d1a20 | 22 | float readFrontVals[10]; |
vsavkin3 | 4:9011798d1a20 | 23 | float readLeft; |
vsavkin3 | 4:9011798d1a20 | 24 | float readRight; |
vsavkin3 | 4:9011798d1a20 | 25 | float readFront; |
vsavkin3 | 4:9011798d1a20 | 26 | |
vsavkin3 | 4:9011798d1a20 | 27 | //Initialize the averager before the robot even moves |
vsavkin3 | 4:9011798d1a20 | 28 | for (int j=0; j<10; j++); |
vsavkin3 | 4:9011798d1a20 | 29 | { |
vsavkin3 | 4:9011798d1a20 | 30 | readLeft=irLeft; |
vsavkin3 | 4:9011798d1a20 | 31 | readLeft=21/readLeft; |
vsavkin3 | 4:9011798d1a20 | 32 | |
vsavkin3 | 4:9011798d1a20 | 33 | readRight=irRight; |
vsavkin3 | 4:9011798d1a20 | 34 | readRight=21/readRight; |
vsavkin3 | 4:9011798d1a20 | 35 | |
vsavkin3 | 4:9011798d1a20 | 36 | readFront=irFront; |
vsavkin3 | 4:9011798d1a20 | 37 | readFront=21/readFront; |
vsavkin3 | 4:9011798d1a20 | 38 | |
vsavkin3 | 4:9011798d1a20 | 39 | readFrontVals[i] = readFront; |
vsavkin3 | 4:9011798d1a20 | 40 | readRightVals[i] = readRight; |
vsavkin3 | 4:9011798d1a20 | 41 | readLeftVals[i] = readLeft; |
vsavkin3 | 4:9011798d1a20 | 42 | } |
vsavkin3 | 4:9011798d1a20 | 43 | |
vsavkin3 | 4:9011798d1a20 | 44 | while (1){ |
vsavkin3 | 4:9011798d1a20 | 45 | readLeft=irLeft; |
vsavkin3 | 4:9011798d1a20 | 46 | readLeft=21/readLeft; |
vsavkin3 | 4:9011798d1a20 | 47 | |
vsavkin3 | 4:9011798d1a20 | 48 | readRight=irRight; |
vsavkin3 | 4:9011798d1a20 | 49 | readRight=21/readRight; |
vsavkin3 | 4:9011798d1a20 | 50 | |
vsavkin3 | 4:9011798d1a20 | 51 | readFront=irFront; |
vsavkin3 | 4:9011798d1a20 | 52 | readFront=21/readFront; |
vsavkin3 | 4:9011798d1a20 | 53 | |
vsavkin3 | 5:99545cf4dff4 | 54 | |
vsavkin3 | 5:99545cf4dff4 | 55 | |
vsavkin3 | 4:9011798d1a20 | 56 | |
vsavkin3 | 4:9011798d1a20 | 57 | |
vsavkin3 | 4:9011798d1a20 | 58 | if (i>=10) |
vsavkin3 | 4:9011798d1a20 | 59 | { |
vsavkin3 | 4:9011798d1a20 | 60 | i=0; |
vsavkin3 | 4:9011798d1a20 | 61 | } |
vsavkin3 | 4:9011798d1a20 | 62 | readFrontVals[i] = readFront; |
vsavkin3 | 4:9011798d1a20 | 63 | readRightVals[i] = readRight; |
vsavkin3 | 4:9011798d1a20 | 64 | readLeftVals[i] = readLeft; |
vsavkin3 | 4:9011798d1a20 | 65 | i++; |
vsavkin3 | 4:9011798d1a20 | 66 | |
vsavkin3 | 4:9011798d1a20 | 67 | float avgFront = 0, avgRight = 0, avgLeft = 0; |
vsavkin3 | 4:9011798d1a20 | 68 | |
vsavkin3 | 4:9011798d1a20 | 69 | for (int j=0; j<10; j++) |
vsavkin3 | 4:9011798d1a20 | 70 | { |
vsavkin3 | 4:9011798d1a20 | 71 | avgFront += readFrontVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 72 | avgLeft += readLeftVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 73 | avgRight += readRightVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 74 | } |
vsavkin3 | 4:9011798d1a20 | 75 | avgFront = readFront; |
vsavkin3 | 4:9011798d1a20 | 76 | avgRight = readRight; |
vsavkin3 | 6:2b89c05628ce | 77 | printf("Front: %5f Right: %5f Left: %5f", avgFront, avgRight, avgLeft); |
vsavkin3 | 6:2b89c05628ce | 78 | float globalDir = compass.sample()/10.0; |
vsavkin3 | 6:2b89c05628ce | 79 | printf(" Heading: %f\n\r", globalDir); |
vsavkin3 | 4:9011798d1a20 | 80 | |
vsavkin3 | 4:9011798d1a20 | 81 | switch(dir){ |
vsavkin3 | 4:9011798d1a20 | 82 | case Forward: |
vsavkin3 | 6:2b89c05628ce | 83 | printf("CHARGE!n\r"); |
vsavkin3 | 4:9011798d1a20 | 84 | if (avgFront <= 40) |
vsavkin3 | 4:9011798d1a20 | 85 | { |
vsavkin3 | 4:9011798d1a20 | 86 | right.speed(0); |
vsavkin3 | 4:9011798d1a20 | 87 | left.speed(0); |
vsavkin3 | 6:2b89c05628ce | 88 | printf("stopped!\n\r"); |
vsavkin3 | 4:9011798d1a20 | 89 | //wait(0.1); |
vsavkin3 | 4:9011798d1a20 | 90 | dir=Right; |
vsavkin3 | 4:9011798d1a20 | 91 | } |
vsavkin3 | 4:9011798d1a20 | 92 | else |
vsavkin3 | 4:9011798d1a20 | 93 | { |
vsavkin3 | 6:2b89c05628ce | 94 | if (globalDir > 45 && globalDir <= 135) |
vsavkin3 | 6:2b89c05628ce | 95 | { |
vsavkin3 | 6:2b89c05628ce | 96 | //East |
vsavkin3 | 6:2b89c05628ce | 97 | if (avgLeft >= 50) |
vsavkin3 | 6:2b89c05628ce | 98 | { |
vsavkin3 | 6:2b89c05628ce | 99 | right.speed(0); |
vsavkin3 | 6:2b89c05628ce | 100 | left.speed(0); |
vsavkin3 | 6:2b89c05628ce | 101 | dir = TurnNorth; |
vsavkin3 | 6:2b89c05628ce | 102 | } |
vsavkin3 | 6:2b89c05628ce | 103 | else |
vsavkin3 | 6:2b89c05628ce | 104 | { |
vsavkin3 | 6:2b89c05628ce | 105 | right.speed(1); |
vsavkin3 | 6:2b89c05628ce | 106 | left.speed(1); |
vsavkin3 | 6:2b89c05628ce | 107 | } |
vsavkin3 | 6:2b89c05628ce | 108 | } |
vsavkin3 | 6:2b89c05628ce | 109 | else if (globalDir > 135 && globalDir <= 225) |
vsavkin3 | 6:2b89c05628ce | 110 | { |
vsavkin3 | 6:2b89c05628ce | 111 | //It's going south O.o |
vsavkin3 | 6:2b89c05628ce | 112 | right.speed(1); |
vsavkin3 | 6:2b89c05628ce | 113 | left.speed(1); |
vsavkin3 | 6:2b89c05628ce | 114 | } |
vsavkin3 | 6:2b89c05628ce | 115 | else if (globalDir > 225 && globalDir <= 315) |
vsavkin3 | 6:2b89c05628ce | 116 | { |
vsavkin3 | 6:2b89c05628ce | 117 | //West |
vsavkin3 | 6:2b89c05628ce | 118 | if (avgRight >= 50) |
vsavkin3 | 6:2b89c05628ce | 119 | { |
vsavkin3 | 6:2b89c05628ce | 120 | right.speed(0); |
vsavkin3 | 6:2b89c05628ce | 121 | left.speed(0); |
vsavkin3 | 6:2b89c05628ce | 122 | dir = TurnNorth; |
vsavkin3 | 6:2b89c05628ce | 123 | } |
vsavkin3 | 6:2b89c05628ce | 124 | else |
vsavkin3 | 6:2b89c05628ce | 125 | { |
vsavkin3 | 6:2b89c05628ce | 126 | right.speed(1); |
vsavkin3 | 6:2b89c05628ce | 127 | left.speed(1); |
vsavkin3 | 6:2b89c05628ce | 128 | } |
vsavkin3 | 6:2b89c05628ce | 129 | } |
vsavkin3 | 6:2b89c05628ce | 130 | else |
vsavkin3 | 6:2b89c05628ce | 131 | { |
vsavkin3 | 6:2b89c05628ce | 132 | //It's going north! |
vsavkin3 | 6:2b89c05628ce | 133 | right.speed(1); |
vsavkin3 | 6:2b89c05628ce | 134 | left.speed(1); |
vsavkin3 | 6:2b89c05628ce | 135 | } |
vsavkin3 | 4:9011798d1a20 | 136 | } |
vsavkin3 | 4:9011798d1a20 | 137 | break; |
vsavkin3 | 4:9011798d1a20 | 138 | case Right: |
vsavkin3 | 6:2b89c05628ce | 139 | printf("TURN!\n\r"); |
vsavkin3 | 6:2b89c05628ce | 140 | if (/*avgLeft>=50 ||*/ avgFront<=40) |
vsavkin3 | 4:9011798d1a20 | 141 | { |
vsavkin3 | 4:9011798d1a20 | 142 | right.speed(-1); |
vsavkin3 | 4:9011798d1a20 | 143 | left.speed(0); |
vsavkin3 | 4:9011798d1a20 | 144 | // printf("turning!\n\r"); |
vsavkin3 | 4:9011798d1a20 | 145 | } |
vsavkin3 | 4:9011798d1a20 | 146 | else |
vsavkin3 | 4:9011798d1a20 | 147 | { |
vsavkin3 | 4:9011798d1a20 | 148 | right.speed(0); |
vsavkin3 | 4:9011798d1a20 | 149 | left.speed(0); |
vsavkin3 | 4:9011798d1a20 | 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 | 6:2b89c05628ce | 156 | printf("TO THE NORTH!\n\r"); |
vsavkin3 | 6:2b89c05628ce | 157 | if (globalDir>330 || globalDir < 30) |
vsavkin3 | 6:2b89c05628ce | 158 | { |
vsavkin3 | 6:2b89c05628ce | 159 | //It's going north!! |
vsavkin3 | 6:2b89c05628ce | 160 | right.speed(1); |
vsavkin3 | 6:2b89c05628ce | 161 | left.speed(1); |
vsavkin3 | 6:2b89c05628ce | 162 | dir=Forward; |
vsavkin3 | 6:2b89c05628ce | 163 | } |
vsavkin3 | 6:2b89c05628ce | 164 | else |
vsavkin3 | 6:2b89c05628ce | 165 | { |
vsavkin3 | 6:2b89c05628ce | 166 | right.speed(.5); |
vsavkin3 | 6:2b89c05628ce | 167 | left.speed(-.5); |
vsavkin3 | 6:2b89c05628ce | 168 | } |
vsavkin3 | 6:2b89c05628ce | 169 | break; |
vsavkin3 | 4:9011798d1a20 | 170 | } |
vsavkin3 | 4:9011798d1a20 | 171 | wait(0.01); |
vsavkin3 | 4:9011798d1a20 | 172 | } |
vsavkin3 | 4:9011798d1a20 | 173 | } |