David Buck
main.cpp@0:eff3ddf69354, 2010-02-05 (annotated)
- Committer:
- Kaikestu
- Date:
- Fri Feb 05 12:25:12 2010 +0000
- Revision:
- 0:eff3ddf69354
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Kaikestu | 0:eff3ddf69354 | 1 | #include "mbed.h" |
| Kaikestu | 0:eff3ddf69354 | 2 | #include "Servo.h" |
| Kaikestu | 0:eff3ddf69354 | 3 | #include "SRF05.h" |
| Kaikestu | 0:eff3ddf69354 | 4 | |
| Kaikestu | 0:eff3ddf69354 | 5 | Servo motor (p21); |
| Kaikestu | 0:eff3ddf69354 | 6 | Servo turn (p22); |
| Kaikestu | 0:eff3ddf69354 | 7 | |
| Kaikestu | 0:eff3ddf69354 | 8 | SRF05 front (p24,p23); |
| Kaikestu | 0:eff3ddf69354 | 9 | SRF05 back (p26,p25); |
| Kaikestu | 0:eff3ddf69354 | 10 | |
| Kaikestu | 0:eff3ddf69354 | 11 | DigitalOut b (p20); |
| Kaikestu | 0:eff3ddf69354 | 12 | DigitalOut led1 (LED1); |
| Kaikestu | 0:eff3ddf69354 | 13 | DigitalOut led2 (LED2); |
| Kaikestu | 0:eff3ddf69354 | 14 | |
| Kaikestu | 0:eff3ddf69354 | 15 | void backward (float); |
| Kaikestu | 0:eff3ddf69354 | 16 | void forward (float); |
| Kaikestu | 0:eff3ddf69354 | 17 | void right (void); |
| Kaikestu | 0:eff3ddf69354 | 18 | void left (void); |
| Kaikestu | 0:eff3ddf69354 | 19 | void reset (void); |
| Kaikestu | 0:eff3ddf69354 | 20 | void stop (void); |
| Kaikestu | 0:eff3ddf69354 | 21 | |
| Kaikestu | 0:eff3ddf69354 | 22 | |
| Kaikestu | 0:eff3ddf69354 | 23 | int i = 0; |
| Kaikestu | 0:eff3ddf69354 | 24 | int j = 0; |
| Kaikestu | 0:eff3ddf69354 | 25 | |
| Kaikestu | 0:eff3ddf69354 | 26 | float sample_delay = 0.01; |
| Kaikestu | 0:eff3ddf69354 | 27 | |
| Kaikestu | 0:eff3ddf69354 | 28 | int main() { |
| Kaikestu | 0:eff3ddf69354 | 29 | |
| Kaikestu | 0:eff3ddf69354 | 30 | b = 0; |
| Kaikestu | 0:eff3ddf69354 | 31 | reset(); |
| Kaikestu | 0:eff3ddf69354 | 32 | |
| Kaikestu | 0:eff3ddf69354 | 33 | //main loop |
| Kaikestu | 0:eff3ddf69354 | 34 | while (1) { |
| Kaikestu | 0:eff3ddf69354 | 35 | |
| Kaikestu | 0:eff3ddf69354 | 36 | //init |
| Kaikestu | 0:eff3ddf69354 | 37 | i = front.read(); |
| Kaikestu | 0:eff3ddf69354 | 38 | j = back.read(); |
| Kaikestu | 0:eff3ddf69354 | 39 | |
| Kaikestu | 0:eff3ddf69354 | 40 | |
| Kaikestu | 0:eff3ddf69354 | 41 | //can go forward? |
| Kaikestu | 0:eff3ddf69354 | 42 | if (i > 100) { |
| Kaikestu | 0:eff3ddf69354 | 43 | while (i > 100) { |
| Kaikestu | 0:eff3ddf69354 | 44 | |
| Kaikestu | 0:eff3ddf69354 | 45 | forward(20); |
| Kaikestu | 0:eff3ddf69354 | 46 | i = front.read(); |
| Kaikestu | 0:eff3ddf69354 | 47 | j = back.read(); |
| Kaikestu | 0:eff3ddf69354 | 48 | } |
| Kaikestu | 0:eff3ddf69354 | 49 | |
| Kaikestu | 0:eff3ddf69354 | 50 | stop(); |
| Kaikestu | 0:eff3ddf69354 | 51 | } |
| Kaikestu | 0:eff3ddf69354 | 52 | |
| Kaikestu | 0:eff3ddf69354 | 53 | //can it turn to avoid? |
| Kaikestu | 0:eff3ddf69354 | 54 | if (i > 10 && i < 100) { |
| Kaikestu | 0:eff3ddf69354 | 55 | led1 = 1; |
| Kaikestu | 0:eff3ddf69354 | 56 | while (i > 10 && i < 100) { |
| Kaikestu | 0:eff3ddf69354 | 57 | |
| Kaikestu | 0:eff3ddf69354 | 58 | forward(20); |
| Kaikestu | 0:eff3ddf69354 | 59 | turn.write(0); |
| Kaikestu | 0:eff3ddf69354 | 60 | |
| Kaikestu | 0:eff3ddf69354 | 61 | i = front.read(); |
| Kaikestu | 0:eff3ddf69354 | 62 | j = back.read(); |
| Kaikestu | 0:eff3ddf69354 | 63 | } |
| Kaikestu | 0:eff3ddf69354 | 64 | led1 = 0; |
| Kaikestu | 0:eff3ddf69354 | 65 | stop(); |
| Kaikestu | 0:eff3ddf69354 | 66 | } |
| Kaikestu | 0:eff3ddf69354 | 67 | |
| Kaikestu | 0:eff3ddf69354 | 68 | //is there room to ververse and turn? |
| Kaikestu | 0:eff3ddf69354 | 69 | if (i < 20 && j > 10) { |
| Kaikestu | 0:eff3ddf69354 | 70 | led2 = 1; |
| Kaikestu | 0:eff3ddf69354 | 71 | while (i < 20 && j > 10) { |
| Kaikestu | 0:eff3ddf69354 | 72 | |
| Kaikestu | 0:eff3ddf69354 | 73 | backward(20); |
| Kaikestu | 0:eff3ddf69354 | 74 | turn.write(1); |
| Kaikestu | 0:eff3ddf69354 | 75 | |
| Kaikestu | 0:eff3ddf69354 | 76 | i = front.read(); |
| Kaikestu | 0:eff3ddf69354 | 77 | j = back.read(); |
| Kaikestu | 0:eff3ddf69354 | 78 | } |
| Kaikestu | 0:eff3ddf69354 | 79 | led2 = 0; |
| Kaikestu | 0:eff3ddf69354 | 80 | stop(); |
| Kaikestu | 0:eff3ddf69354 | 81 | } |
| Kaikestu | 0:eff3ddf69354 | 82 | } |
| Kaikestu | 0:eff3ddf69354 | 83 | } |
| Kaikestu | 0:eff3ddf69354 | 84 | |
| Kaikestu | 0:eff3ddf69354 | 85 | void reset (void) { |
| Kaikestu | 0:eff3ddf69354 | 86 | turn.write(0.5); |
| Kaikestu | 0:eff3ddf69354 | 87 | motor.write(0.48); |
| Kaikestu | 0:eff3ddf69354 | 88 | } |
| Kaikestu | 0:eff3ddf69354 | 89 | |
| Kaikestu | 0:eff3ddf69354 | 90 | // 0.5 = 0 speed & 0 = full speed |
| Kaikestu | 0:eff3ddf69354 | 91 | void forward (float speed) { |
| Kaikestu | 0:eff3ddf69354 | 92 | float y = speed / 100; |
| Kaikestu | 0:eff3ddf69354 | 93 | float x = y * 0.5; |
| Kaikestu | 0:eff3ddf69354 | 94 | float z = 0.5 - x; |
| Kaikestu | 0:eff3ddf69354 | 95 | |
| Kaikestu | 0:eff3ddf69354 | 96 | motor.write(z); |
| Kaikestu | 0:eff3ddf69354 | 97 | } |
| Kaikestu | 0:eff3ddf69354 | 98 | |
| Kaikestu | 0:eff3ddf69354 | 99 | // 0.5 = 0 speed & 1 = full speed |
| Kaikestu | 0:eff3ddf69354 | 100 | |
| Kaikestu | 0:eff3ddf69354 | 101 | void backward (float speed) { |
| Kaikestu | 0:eff3ddf69354 | 102 | float y = speed / 100; |
| Kaikestu | 0:eff3ddf69354 | 103 | float x = y * 0.5; |
| Kaikestu | 0:eff3ddf69354 | 104 | float z = 0.5 + x; |
| Kaikestu | 0:eff3ddf69354 | 105 | |
| Kaikestu | 0:eff3ddf69354 | 106 | motor.write(z); |
| Kaikestu | 0:eff3ddf69354 | 107 | } |
| Kaikestu | 0:eff3ddf69354 | 108 | |
| Kaikestu | 0:eff3ddf69354 | 109 | // 0 = right 0.5 = straight 1 = left |
| Kaikestu | 0:eff3ddf69354 | 110 | void left (void) { |
| Kaikestu | 0:eff3ddf69354 | 111 | turn.write(1); |
| Kaikestu | 0:eff3ddf69354 | 112 | } |
| Kaikestu | 0:eff3ddf69354 | 113 | void right (void) { |
| Kaikestu | 0:eff3ddf69354 | 114 | turn.write(0); |
| Kaikestu | 0:eff3ddf69354 | 115 | } |
| Kaikestu | 0:eff3ddf69354 | 116 | |
| Kaikestu | 0:eff3ddf69354 | 117 | void stop (void) { |
| Kaikestu | 0:eff3ddf69354 | 118 | b = 1; |
| Kaikestu | 0:eff3ddf69354 | 119 | wait(0.3); |
| Kaikestu | 0:eff3ddf69354 | 120 | reset(); |
| Kaikestu | 0:eff3ddf69354 | 121 | wait(0.1); |
| Kaikestu | 0:eff3ddf69354 | 122 | b = 0; |
| Kaikestu | 0:eff3ddf69354 | 123 | wait(1); |
| Kaikestu | 0:eff3ddf69354 | 124 | } |