IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
main.cpp@4:9011798d1a20, 2012-10-04 (annotated)
- Committer:
- vsavkin3
- Date:
- Thu Oct 04 22:05:11 2012 +0000
- Revision:
- 4:9011798d1a20
- Parent:
- 3:4ee5247f47ec
- Child:
- 5:99545cf4dff4
Robot correctly turns away from obstacles and then moves forward. Can follow a wall! (Kind of)
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 | 4:9011798d1a20 | 10 | |
vsavkin3 | 4:9011798d1a20 | 11 | enum direction{ Right, Forward }; |
vsavkin3 | 4:9011798d1a20 | 12 | |
vsavkin3 | 4:9011798d1a20 | 13 | int main() { |
vsavkin3 | 4:9011798d1a20 | 14 | enum direction dir=Forward; |
vsavkin3 | 4:9011798d1a20 | 15 | |
vsavkin3 | 4:9011798d1a20 | 16 | printf("Front Right Left\n\r"); |
vsavkin3 | 4:9011798d1a20 | 17 | int i=0; |
vsavkin3 | 4:9011798d1a20 | 18 | float readLeftVals[10]; |
vsavkin3 | 4:9011798d1a20 | 19 | float readRightVals[10]; |
vsavkin3 | 4:9011798d1a20 | 20 | float readFrontVals[10]; |
vsavkin3 | 4:9011798d1a20 | 21 | float readLeft; |
vsavkin3 | 4:9011798d1a20 | 22 | float readRight; |
vsavkin3 | 4:9011798d1a20 | 23 | float readFront; |
vsavkin3 | 4:9011798d1a20 | 24 | |
vsavkin3 | 4:9011798d1a20 | 25 | //Initialize the averager before the robot even moves |
vsavkin3 | 4:9011798d1a20 | 26 | for (int j=0; j<10; j++); |
vsavkin3 | 4:9011798d1a20 | 27 | { |
vsavkin3 | 4:9011798d1a20 | 28 | readLeft=irLeft; |
vsavkin3 | 4:9011798d1a20 | 29 | readLeft=21/readLeft; |
vsavkin3 | 4:9011798d1a20 | 30 | |
vsavkin3 | 4:9011798d1a20 | 31 | readRight=irRight; |
vsavkin3 | 4:9011798d1a20 | 32 | readRight=21/readRight; |
vsavkin3 | 4:9011798d1a20 | 33 | |
vsavkin3 | 4:9011798d1a20 | 34 | readFront=irFront; |
vsavkin3 | 4:9011798d1a20 | 35 | readFront=21/readFront; |
vsavkin3 | 4:9011798d1a20 | 36 | |
vsavkin3 | 4:9011798d1a20 | 37 | readFrontVals[i] = readFront; |
vsavkin3 | 4:9011798d1a20 | 38 | readRightVals[i] = readRight; |
vsavkin3 | 4:9011798d1a20 | 39 | readLeftVals[i] = readLeft; |
vsavkin3 | 4:9011798d1a20 | 40 | } |
vsavkin3 | 4:9011798d1a20 | 41 | |
vsavkin3 | 4:9011798d1a20 | 42 | while (1){ |
vsavkin3 | 4:9011798d1a20 | 43 | readLeft=irLeft; |
vsavkin3 | 4:9011798d1a20 | 44 | readLeft=21/readLeft; |
vsavkin3 | 4:9011798d1a20 | 45 | |
vsavkin3 | 4:9011798d1a20 | 46 | readRight=irRight; |
vsavkin3 | 4:9011798d1a20 | 47 | readRight=21/readRight; |
vsavkin3 | 4:9011798d1a20 | 48 | |
vsavkin3 | 4:9011798d1a20 | 49 | readFront=irFront; |
vsavkin3 | 4:9011798d1a20 | 50 | readFront=21/readFront; |
vsavkin3 | 4:9011798d1a20 | 51 | |
vsavkin3 | 4:9011798d1a20 | 52 | |
vsavkin3 | 4:9011798d1a20 | 53 | |
vsavkin3 | 4:9011798d1a20 | 54 | if (i>=10) |
vsavkin3 | 4:9011798d1a20 | 55 | { |
vsavkin3 | 4:9011798d1a20 | 56 | i=0; |
vsavkin3 | 4:9011798d1a20 | 57 | } |
vsavkin3 | 4:9011798d1a20 | 58 | readFrontVals[i] = readFront; |
vsavkin3 | 4:9011798d1a20 | 59 | readRightVals[i] = readRight; |
vsavkin3 | 4:9011798d1a20 | 60 | readLeftVals[i] = readLeft; |
vsavkin3 | 4:9011798d1a20 | 61 | i++; |
vsavkin3 | 4:9011798d1a20 | 62 | |
vsavkin3 | 4:9011798d1a20 | 63 | float avgFront = 0, avgRight = 0, avgLeft = 0; |
vsavkin3 | 4:9011798d1a20 | 64 | |
vsavkin3 | 4:9011798d1a20 | 65 | for (int j=0; j<10; j++) |
vsavkin3 | 4:9011798d1a20 | 66 | { |
vsavkin3 | 4:9011798d1a20 | 67 | avgFront += readFrontVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 68 | avgLeft += readLeftVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 69 | avgRight += readRightVals[j]/10; |
vsavkin3 | 4:9011798d1a20 | 70 | } |
vsavkin3 | 4:9011798d1a20 | 71 | avgFront = readFront; |
vsavkin3 | 4:9011798d1a20 | 72 | avgRight = readRight; |
vsavkin3 | 4:9011798d1a20 | 73 | //printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft); |
vsavkin3 | 4:9011798d1a20 | 74 | |
vsavkin3 | 4:9011798d1a20 | 75 | switch(dir){ |
vsavkin3 | 4:9011798d1a20 | 76 | case Forward: |
vsavkin3 | 4:9011798d1a20 | 77 | if (avgFront <= 40) |
vsavkin3 | 4:9011798d1a20 | 78 | { |
vsavkin3 | 4:9011798d1a20 | 79 | right.speed(0); |
vsavkin3 | 4:9011798d1a20 | 80 | left.speed(0); |
vsavkin3 | 4:9011798d1a20 | 81 | // printf("stopped!\n\r"); |
vsavkin3 | 4:9011798d1a20 | 82 | //wait(0.1); |
vsavkin3 | 4:9011798d1a20 | 83 | dir=Right; |
vsavkin3 | 4:9011798d1a20 | 84 | // printf("less than 18\n\r"); |
vsavkin3 | 4:9011798d1a20 | 85 | } |
vsavkin3 | 4:9011798d1a20 | 86 | else |
vsavkin3 | 4:9011798d1a20 | 87 | { |
vsavkin3 | 4:9011798d1a20 | 88 | right.speed(1); |
vsavkin3 | 4:9011798d1a20 | 89 | left.speed(1); |
vsavkin3 | 4:9011798d1a20 | 90 | } |
vsavkin3 | 4:9011798d1a20 | 91 | break; |
vsavkin3 | 4:9011798d1a20 | 92 | case Right: |
vsavkin3 | 4:9011798d1a20 | 93 | if (avgLeft>=50 || avgFront<=40) |
vsavkin3 | 4:9011798d1a20 | 94 | { |
vsavkin3 | 4:9011798d1a20 | 95 | right.speed(-1); |
vsavkin3 | 4:9011798d1a20 | 96 | left.speed(0); |
vsavkin3 | 4:9011798d1a20 | 97 | // printf("turning!\n\r"); |
vsavkin3 | 4:9011798d1a20 | 98 | } |
vsavkin3 | 4:9011798d1a20 | 99 | else |
vsavkin3 | 4:9011798d1a20 | 100 | { |
vsavkin3 | 4:9011798d1a20 | 101 | right.speed(0); |
vsavkin3 | 4:9011798d1a20 | 102 | left.speed(0); |
vsavkin3 | 4:9011798d1a20 | 103 | // printf("stopped!\n\r"); |
vsavkin3 | 4:9011798d1a20 | 104 | //wait(0.1); |
vsavkin3 | 4:9011798d1a20 | 105 | dir=Forward; |
vsavkin3 | 4:9011798d1a20 | 106 | } |
vsavkin3 | 4:9011798d1a20 | 107 | break; |
vsavkin3 | 4:9011798d1a20 | 108 | } |
vsavkin3 | 4:9011798d1a20 | 109 | wait(0.01); |
vsavkin3 | 4:9011798d1a20 | 110 | } |
vsavkin3 | 4:9011798d1a20 | 111 | } |