IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
main.cpp@5:99545cf4dff4, 2012-10-04 (annotated)
- Committer:
- vsavkin3
- Date:
- Thu Oct 04 22:05:37 2012 +0000
- Revision:
- 5:99545cf4dff4
- Parent:
- 4:9011798d1a20
- Child:
- 6:2b89c05628ce
Compass interfaced with the robot. Heading can be read.
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 | 4:9011798d1a20 | 12 | enum direction{ Right, Forward }; |
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 | printf("Heading: %f\n\r", compass.sample()/10.0); |
vsavkin3 | 5:99545cf4dff4 | 56 | |
vsavkin3 | 4:9011798d1a20 | 57 | |
vsavkin3 | 4:9011798d1a20 | 58 | |
vsavkin3 | 4:9011798d1a20 | 59 | if (i>=10) |
vsavkin3 | 4:9011798d1a20 | 60 | { |
vsavkin3 | 4:9011798d1a20 | 61 | i=0; |
vsavkin3 | 4:9011798d1a20 | 62 | } |
vsavkin3 | 4:9011798d1a20 | 63 | readFrontVals[i] = readFront; |
vsavkin3 | 4:9011798d1a20 | 64 | readRightVals[i] = readRight; |
vsavkin3 | 4:9011798d1a20 | 65 | readLeftVals[i] = readLeft; |
vsavkin3 | 4:9011798d1a20 | 66 | i++; |
vsavkin3 | 4:9011798d1a20 | 67 | |
vsavkin3 | 4:9011798d1a20 | 68 | float avgFront = 0, avgRight = 0, avgLeft = 0; |
vsavkin3 | 4:9011798d1a20 | 69 | |
vsavkin3 | 4:9011798d1a20 | 70 | for (int j=0; j<10; j++) |
vsavkin3 | 4:9011798d1a20 | 71 | { |
vsavkin3 | 4:9011798d1a20 | 72 | avgFront += readFrontVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 73 | avgLeft += readLeftVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 74 | avgRight += readRightVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 75 | } |
vsavkin3 | 4:9011798d1a20 | 76 | avgFront = readFront; |
vsavkin3 | 4:9011798d1a20 | 77 | avgRight = readRight; |
vsavkin3 | 4:9011798d1a20 | 78 | //printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft); |
vsavkin3 | 4:9011798d1a20 | 79 | |
vsavkin3 | 4:9011798d1a20 | 80 | switch(dir){ |
vsavkin3 | 4:9011798d1a20 | 81 | case Forward: |
vsavkin3 | 4:9011798d1a20 | 82 | if (avgFront <= 40) |
vsavkin3 | 4:9011798d1a20 | 83 | { |
vsavkin3 | 4:9011798d1a20 | 84 | right.speed(0); |
vsavkin3 | 4:9011798d1a20 | 85 | left.speed(0); |
vsavkin3 | 4:9011798d1a20 | 86 | // printf("stopped!\n\r"); |
vsavkin3 | 4:9011798d1a20 | 87 | //wait(0.1); |
vsavkin3 | 4:9011798d1a20 | 88 | dir=Right; |
vsavkin3 | 4:9011798d1a20 | 89 | // printf("less than 18\n\r"); |
vsavkin3 | 4:9011798d1a20 | 90 | } |
vsavkin3 | 4:9011798d1a20 | 91 | else |
vsavkin3 | 4:9011798d1a20 | 92 | { |
vsavkin3 | 4:9011798d1a20 | 93 | right.speed(1); |
vsavkin3 | 4:9011798d1a20 | 94 | left.speed(1); |
vsavkin3 | 4:9011798d1a20 | 95 | } |
vsavkin3 | 4:9011798d1a20 | 96 | break; |
vsavkin3 | 4:9011798d1a20 | 97 | case Right: |
vsavkin3 | 4:9011798d1a20 | 98 | if (avgLeft>=50 || avgFront<=40) |
vsavkin3 | 4:9011798d1a20 | 99 | { |
vsavkin3 | 4:9011798d1a20 | 100 | right.speed(-1); |
vsavkin3 | 4:9011798d1a20 | 101 | left.speed(0); |
vsavkin3 | 4:9011798d1a20 | 102 | // printf("turning!\n\r"); |
vsavkin3 | 4:9011798d1a20 | 103 | } |
vsavkin3 | 4:9011798d1a20 | 104 | else |
vsavkin3 | 4:9011798d1a20 | 105 | { |
vsavkin3 | 4:9011798d1a20 | 106 | right.speed(0); |
vsavkin3 | 4:9011798d1a20 | 107 | left.speed(0); |
vsavkin3 | 4:9011798d1a20 | 108 | // printf("stopped!\n\r"); |
vsavkin3 | 4:9011798d1a20 | 109 | //wait(0.1); |
vsavkin3 | 4:9011798d1a20 | 110 | dir=Forward; |
vsavkin3 | 4:9011798d1a20 | 111 | } |
vsavkin3 | 4:9011798d1a20 | 112 | break; |
vsavkin3 | 4:9011798d1a20 | 113 | } |
vsavkin3 | 4:9011798d1a20 | 114 | wait(0.01); |
vsavkin3 | 4:9011798d1a20 | 115 | } |
vsavkin3 | 4:9011798d1a20 | 116 | } |