IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
main.cpp@1:6812f830e5a1, 2012-10-04 (annotated)
- Committer:
- vsavkin3
- Date:
- Thu Oct 04 19:11:02 2012 +0000
- Revision:
- 1:6812f830e5a1
- Parent:
- 0:1a0fa585e36c
- Child:
- 2:25d2332ec855
- Child:
- 3:4ee5247f47ec
Robot sometimes sees front and turns right appropriately until there's nothing stopping it again.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsavkin3 | 0:1a0fa585e36c | 1 | #include "mbed.h" |
vsavkin3 | 0:1a0fa585e36c | 2 | #include "Motor.h" |
vsavkin3 | 0:1a0fa585e36c | 3 | |
vsavkin3 | 0:1a0fa585e36c | 4 | Motor right(p21, p23, p22); // pwm, fwd, rev |
vsavkin3 | 0:1a0fa585e36c | 5 | Motor left(p26, p24, p25); // pwm, fwd, rev |
vsavkin3 | 1:6812f830e5a1 | 6 | AnalogIn irLeft(p20); |
vsavkin3 | 1:6812f830e5a1 | 7 | AnalogIn irFront(p19); |
vsavkin3 | 1:6812f830e5a1 | 8 | AnalogIn irRight(p18); |
vsavkin3 | 1:6812f830e5a1 | 9 | AnalogIn sonarLeft(p17); |
vsavkin3 | 1:6812f830e5a1 | 10 | |
vsavkin3 | 1:6812f830e5a1 | 11 | enum direction{ Right, Forward }; |
vsavkin3 | 0:1a0fa585e36c | 12 | |
vsavkin3 | 0:1a0fa585e36c | 13 | int main() { |
vsavkin3 | 1:6812f830e5a1 | 14 | enum direction dir=Forward; |
vsavkin3 | 1:6812f830e5a1 | 15 | |
vsavkin3 | 1:6812f830e5a1 | 16 | printf("Front Right Left\n\r"); |
vsavkin3 | 1:6812f830e5a1 | 17 | int i=0; |
vsavkin3 | 1:6812f830e5a1 | 18 | int readLeftVals[10]; |
vsavkin3 | 1:6812f830e5a1 | 19 | int readRightVals[10]; |
vsavkin3 | 1:6812f830e5a1 | 20 | int readFrontVals[10]; |
vsavkin3 | 1:6812f830e5a1 | 21 | float readLeft; |
vsavkin3 | 1:6812f830e5a1 | 22 | float readRight; |
vsavkin3 | 1:6812f830e5a1 | 23 | float readFront; |
vsavkin3 | 1:6812f830e5a1 | 24 | |
vsavkin3 | 1:6812f830e5a1 | 25 | //Initialize the averager before the robot even moves |
vsavkin3 | 1:6812f830e5a1 | 26 | for (int j=0; j<10; j++); |
vsavkin3 | 1:6812f830e5a1 | 27 | { |
vsavkin3 | 1:6812f830e5a1 | 28 | readLeft=irLeft; |
vsavkin3 | 1:6812f830e5a1 | 29 | readLeft=10/readLeft; |
vsavkin3 | 1:6812f830e5a1 | 30 | |
vsavkin3 | 1:6812f830e5a1 | 31 | readRight=irRight; |
vsavkin3 | 1:6812f830e5a1 | 32 | readRight=10/readRight; |
vsavkin3 | 1:6812f830e5a1 | 33 | |
vsavkin3 | 1:6812f830e5a1 | 34 | readFront=irFront; |
vsavkin3 | 1:6812f830e5a1 | 35 | readFront=10/readFront; |
vsavkin3 | 1:6812f830e5a1 | 36 | |
vsavkin3 | 1:6812f830e5a1 | 37 | readFrontVals[i] = readFront; |
vsavkin3 | 1:6812f830e5a1 | 38 | readRightVals[i] = readRight; |
vsavkin3 | 1:6812f830e5a1 | 39 | readLeftVals[i] = readLeft; |
vsavkin3 | 1:6812f830e5a1 | 40 | } |
vsavkin3 | 1:6812f830e5a1 | 41 | |
vsavkin3 | 0:1a0fa585e36c | 42 | while (1){ |
vsavkin3 | 1:6812f830e5a1 | 43 | readLeft=irLeft; |
vsavkin3 | 1:6812f830e5a1 | 44 | readLeft=10/readLeft; |
vsavkin3 | 1:6812f830e5a1 | 45 | |
vsavkin3 | 1:6812f830e5a1 | 46 | readRight=irRight; |
vsavkin3 | 1:6812f830e5a1 | 47 | readRight=10/readRight; |
vsavkin3 | 1:6812f830e5a1 | 48 | |
vsavkin3 | 1:6812f830e5a1 | 49 | readFront=irFront; |
vsavkin3 | 1:6812f830e5a1 | 50 | readFront=10/readFront; |
vsavkin3 | 1:6812f830e5a1 | 51 | |
vsavkin3 | 1:6812f830e5a1 | 52 | |
vsavkin3 | 1:6812f830e5a1 | 53 | |
vsavkin3 | 1:6812f830e5a1 | 54 | |
vsavkin3 | 1:6812f830e5a1 | 55 | if (i>10) |
vsavkin3 | 1:6812f830e5a1 | 56 | { |
vsavkin3 | 1:6812f830e5a1 | 57 | i=0; |
vsavkin3 | 1:6812f830e5a1 | 58 | } |
vsavkin3 | 1:6812f830e5a1 | 59 | readFrontVals[i] = readFront; |
vsavkin3 | 1:6812f830e5a1 | 60 | readRightVals[i] = readRight; |
vsavkin3 | 1:6812f830e5a1 | 61 | readLeftVals[i] = readLeft; |
vsavkin3 | 1:6812f830e5a1 | 62 | i++; |
vsavkin3 | 1:6812f830e5a1 | 63 | |
vsavkin3 | 1:6812f830e5a1 | 64 | int avgFront = 0, avgRight = 0, avgLeft = 0; |
vsavkin3 | 1:6812f830e5a1 | 65 | |
vsavkin3 | 1:6812f830e5a1 | 66 | for (int j=0; j<10; j++) |
vsavkin3 | 1:6812f830e5a1 | 67 | { |
vsavkin3 | 1:6812f830e5a1 | 68 | avgFront += readFrontVals[j]/10; |
vsavkin3 | 1:6812f830e5a1 | 69 | avgLeft += readLeftVals[j]/10; |
vsavkin3 | 1:6812f830e5a1 | 70 | avgRight += readRightVals[j]/10; |
vsavkin3 | 0:1a0fa585e36c | 71 | } |
vsavkin3 | 1:6812f830e5a1 | 72 | |
vsavkin3 | 1:6812f830e5a1 | 73 | printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft); |
vsavkin3 | 1:6812f830e5a1 | 74 | |
vsavkin3 | 1:6812f830e5a1 | 75 | switch(dir){ |
vsavkin3 | 1:6812f830e5a1 | 76 | case Forward: |
vsavkin3 | 1:6812f830e5a1 | 77 | if (avgFront <= 18) |
vsavkin3 | 1:6812f830e5a1 | 78 | { |
vsavkin3 | 1:6812f830e5a1 | 79 | wait(0.5); |
vsavkin3 | 1:6812f830e5a1 | 80 | dir=Right; |
vsavkin3 | 1:6812f830e5a1 | 81 | printf("less than 18\n\r"); |
vsavkin3 | 1:6812f830e5a1 | 82 | } |
vsavkin3 | 1:6812f830e5a1 | 83 | else |
vsavkin3 | 1:6812f830e5a1 | 84 | { |
vsavkin3 | 1:6812f830e5a1 | 85 | right.speed(1); |
vsavkin3 | 1:6812f830e5a1 | 86 | left.speed(1); |
vsavkin3 | 1:6812f830e5a1 | 87 | } |
vsavkin3 | 1:6812f830e5a1 | 88 | break; |
vsavkin3 | 1:6812f830e5a1 | 89 | case Right: |
vsavkin3 | 1:6812f830e5a1 | 90 | if (avgLeft>30 || avgFront<=18) |
vsavkin3 | 1:6812f830e5a1 | 91 | { |
vsavkin3 | 1:6812f830e5a1 | 92 | right.speed(-1); |
vsavkin3 | 1:6812f830e5a1 | 93 | left.speed(1); |
vsavkin3 | 1:6812f830e5a1 | 94 | printf("turning!\n\r"); |
vsavkin3 | 1:6812f830e5a1 | 95 | } |
vsavkin3 | 1:6812f830e5a1 | 96 | else |
vsavkin3 | 1:6812f830e5a1 | 97 | { |
vsavkin3 | 1:6812f830e5a1 | 98 | wait(0.5); |
vsavkin3 | 1:6812f830e5a1 | 99 | dir=Forward; |
vsavkin3 | 1:6812f830e5a1 | 100 | } |
vsavkin3 | 1:6812f830e5a1 | 101 | break; |
vsavkin3 | 1:6812f830e5a1 | 102 | } |
vsavkin3 | 1:6812f830e5a1 | 103 | wait(0.01); |
vsavkin3 | 0:1a0fa585e36c | 104 | } |
vsavkin3 | 0:1a0fa585e36c | 105 | } |