David Buck
3rd instalment of car automation
main.cpp@0:c6cbcb90e098, 2010-02-12 (annotated)
- Committer:
- Kaikestu
- Date:
- Fri Feb 12 12:57:20 2010 +0000
- Revision:
- 0:c6cbcb90e098
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Kaikestu | 0:c6cbcb90e098 | 1 | #include "mbed.h" |
| Kaikestu | 0:c6cbcb90e098 | 2 | #include "Servo.h" |
| Kaikestu | 0:c6cbcb90e098 | 3 | #include "SRF05.h" |
| Kaikestu | 0:c6cbcb90e098 | 4 | |
| Kaikestu | 0:c6cbcb90e098 | 5 | Servo motor (p21); |
| Kaikestu | 0:c6cbcb90e098 | 6 | Servo turn (p22); |
| Kaikestu | 0:c6cbcb90e098 | 7 | |
| Kaikestu | 0:c6cbcb90e098 | 8 | SRF05 front (p24,p23); |
| Kaikestu | 0:c6cbcb90e098 | 9 | SRF05 back (p26,p25); |
| Kaikestu | 0:c6cbcb90e098 | 10 | |
| Kaikestu | 0:c6cbcb90e098 | 11 | DigitalOut b (p20); |
| Kaikestu | 0:c6cbcb90e098 | 12 | DigitalOut led1 (LED1); |
| Kaikestu | 0:c6cbcb90e098 | 13 | DigitalOut led2 (LED2); |
| Kaikestu | 0:c6cbcb90e098 | 14 | |
| Kaikestu | 0:c6cbcb90e098 | 15 | LocalFileSystem local("local"); |
| Kaikestu | 0:c6cbcb90e098 | 16 | InterruptIn w(p16); |
| Kaikestu | 0:c6cbcb90e098 | 17 | Timer timer; |
| Kaikestu | 0:c6cbcb90e098 | 18 | |
| Kaikestu | 0:c6cbcb90e098 | 19 | void backward (float); |
| Kaikestu | 0:c6cbcb90e098 | 20 | void forward (float); |
| Kaikestu | 0:c6cbcb90e098 | 21 | void right (void); |
| Kaikestu | 0:c6cbcb90e098 | 22 | void left (void); |
| Kaikestu | 0:c6cbcb90e098 | 23 | void reset (void); |
| Kaikestu | 0:c6cbcb90e098 | 24 | void stop (void); |
| Kaikestu | 0:c6cbcb90e098 | 25 | |
| Kaikestu | 0:c6cbcb90e098 | 26 | |
| Kaikestu | 0:c6cbcb90e098 | 27 | //servo vars |
| Kaikestu | 0:c6cbcb90e098 | 28 | int i = 0; |
| Kaikestu | 0:c6cbcb90e098 | 29 | int j = 0; |
| Kaikestu | 0:c6cbcb90e098 | 30 | |
| Kaikestu | 0:c6cbcb90e098 | 31 | //timer/sensor vars |
| Kaikestu | 0:c6cbcb90e098 | 32 | int count; |
| Kaikestu | 0:c6cbcb90e098 | 33 | int begin; |
| Kaikestu | 0:c6cbcb90e098 | 34 | int end; |
| Kaikestu | 0:c6cbcb90e098 | 35 | int writeBegin; |
| Kaikestu | 0:c6cbcb90e098 | 36 | int writeEnd; |
| Kaikestu | 0:c6cbcb90e098 | 37 | int single_spin; |
| Kaikestu | 0:c6cbcb90e098 | 38 | int ws; |
| Kaikestu | 0:c6cbcb90e098 | 39 | //const int wheel = 257; //in mm |
| Kaikestu | 0:c6cbcb90e098 | 40 | //int Tdist = 0; |
| Kaikestu | 0:c6cbcb90e098 | 41 | |
| Kaikestu | 0:c6cbcb90e098 | 42 | //intrupt program exicuted |
| Kaikestu | 0:c6cbcb90e098 | 43 | void trigger() { |
| Kaikestu | 0:c6cbcb90e098 | 44 | |
| Kaikestu | 0:c6cbcb90e098 | 45 | begin = timer.read_ms(); |
| Kaikestu | 0:c6cbcb90e098 | 46 | single_spin = begin - end; |
| Kaikestu | 0:c6cbcb90e098 | 47 | end = timer.read_ms(); |
| Kaikestu | 0:c6cbcb90e098 | 48 | |
| Kaikestu | 0:c6cbcb90e098 | 49 | |
| Kaikestu | 0:c6cbcb90e098 | 50 | writeBegin = timer.read_ms(); |
| Kaikestu | 0:c6cbcb90e098 | 51 | FILE *fp = fopen("/local/out.csv", "a"); |
| Kaikestu | 0:c6cbcb90e098 | 52 | count = count + 1; |
| Kaikestu | 0:c6cbcb90e098 | 53 | //myled2 = !myled2; |
| Kaikestu | 0:c6cbcb90e098 | 54 | fprintf(fp, "%d, %d,%d\n", count, single_spin, ws); |
| Kaikestu | 0:c6cbcb90e098 | 55 | fclose(fp); |
| Kaikestu | 0:c6cbcb90e098 | 56 | |
| Kaikestu | 0:c6cbcb90e098 | 57 | writeEnd = timer.read_ms(); |
| Kaikestu | 0:c6cbcb90e098 | 58 | ws = writeEnd - writeBegin; |
| Kaikestu | 0:c6cbcb90e098 | 59 | end = end - ws; |
| Kaikestu | 0:c6cbcb90e098 | 60 | |
| Kaikestu | 0:c6cbcb90e098 | 61 | } |
| Kaikestu | 0:c6cbcb90e098 | 62 | |
| Kaikestu | 0:c6cbcb90e098 | 63 | |
| Kaikestu | 0:c6cbcb90e098 | 64 | int main() { |
| Kaikestu | 0:c6cbcb90e098 | 65 | |
| Kaikestu | 0:c6cbcb90e098 | 66 | timer.start(); //start timer |
| Kaikestu | 0:c6cbcb90e098 | 67 | w.rise(&trigger); //set trigger |
| Kaikestu | 0:c6cbcb90e098 | 68 | |
| Kaikestu | 0:c6cbcb90e098 | 69 | b = 0; |
| Kaikestu | 0:c6cbcb90e098 | 70 | reset(); |
| Kaikestu | 0:c6cbcb90e098 | 71 | |
| Kaikestu | 0:c6cbcb90e098 | 72 | //main loop |
| Kaikestu | 0:c6cbcb90e098 | 73 | while (1) { |
| Kaikestu | 0:c6cbcb90e098 | 74 | |
| Kaikestu | 0:c6cbcb90e098 | 75 | //init |
| Kaikestu | 0:c6cbcb90e098 | 76 | i = front.read(); |
| Kaikestu | 0:c6cbcb90e098 | 77 | j = back.read(); |
| Kaikestu | 0:c6cbcb90e098 | 78 | |
| Kaikestu | 0:c6cbcb90e098 | 79 | |
| Kaikestu | 0:c6cbcb90e098 | 80 | //can go forward? |
| Kaikestu | 0:c6cbcb90e098 | 81 | if (i > 100) { |
| Kaikestu | 0:c6cbcb90e098 | 82 | while (i > 100) { |
| Kaikestu | 0:c6cbcb90e098 | 83 | |
| Kaikestu | 0:c6cbcb90e098 | 84 | forward(20); |
| Kaikestu | 0:c6cbcb90e098 | 85 | i = front.read(); |
| Kaikestu | 0:c6cbcb90e098 | 86 | j = back.read(); |
| Kaikestu | 0:c6cbcb90e098 | 87 | } |
| Kaikestu | 0:c6cbcb90e098 | 88 | |
| Kaikestu | 0:c6cbcb90e098 | 89 | stop(); |
| Kaikestu | 0:c6cbcb90e098 | 90 | } |
| Kaikestu | 0:c6cbcb90e098 | 91 | |
| Kaikestu | 0:c6cbcb90e098 | 92 | //can it turn to avoid? |
| Kaikestu | 0:c6cbcb90e098 | 93 | if (i > 10 && i < 100) { |
| Kaikestu | 0:c6cbcb90e098 | 94 | led1 = 1; |
| Kaikestu | 0:c6cbcb90e098 | 95 | while (i > 10 && i < 100) { |
| Kaikestu | 0:c6cbcb90e098 | 96 | |
| Kaikestu | 0:c6cbcb90e098 | 97 | forward(20); |
| Kaikestu | 0:c6cbcb90e098 | 98 | turn.write(0); |
| Kaikestu | 0:c6cbcb90e098 | 99 | |
| Kaikestu | 0:c6cbcb90e098 | 100 | i = front.read(); |
| Kaikestu | 0:c6cbcb90e098 | 101 | j = back.read(); |
| Kaikestu | 0:c6cbcb90e098 | 102 | } |
| Kaikestu | 0:c6cbcb90e098 | 103 | led1 = 0; |
| Kaikestu | 0:c6cbcb90e098 | 104 | stop(); |
| Kaikestu | 0:c6cbcb90e098 | 105 | } |
| Kaikestu | 0:c6cbcb90e098 | 106 | |
| Kaikestu | 0:c6cbcb90e098 | 107 | //is there room to ververse and turn? |
| Kaikestu | 0:c6cbcb90e098 | 108 | if (i < 20 && j > 10) { |
| Kaikestu | 0:c6cbcb90e098 | 109 | led2 = 1; |
| Kaikestu | 0:c6cbcb90e098 | 110 | while (i < 20 && j > 10) { |
| Kaikestu | 0:c6cbcb90e098 | 111 | |
| Kaikestu | 0:c6cbcb90e098 | 112 | backward(20); |
| Kaikestu | 0:c6cbcb90e098 | 113 | turn.write(1); |
| Kaikestu | 0:c6cbcb90e098 | 114 | |
| Kaikestu | 0:c6cbcb90e098 | 115 | i = front.read(); |
| Kaikestu | 0:c6cbcb90e098 | 116 | j = back.read(); |
| Kaikestu | 0:c6cbcb90e098 | 117 | } |
| Kaikestu | 0:c6cbcb90e098 | 118 | led2 = 0; |
| Kaikestu | 0:c6cbcb90e098 | 119 | stop(); |
| Kaikestu | 0:c6cbcb90e098 | 120 | } |
| Kaikestu | 0:c6cbcb90e098 | 121 | } |
| Kaikestu | 0:c6cbcb90e098 | 122 | } |
| Kaikestu | 0:c6cbcb90e098 | 123 | |
| Kaikestu | 0:c6cbcb90e098 | 124 | void reset (void) { |
| Kaikestu | 0:c6cbcb90e098 | 125 | turn.write(0.5); |
| Kaikestu | 0:c6cbcb90e098 | 126 | motor.write(0.48); |
| Kaikestu | 0:c6cbcb90e098 | 127 | } |
| Kaikestu | 0:c6cbcb90e098 | 128 | |
| Kaikestu | 0:c6cbcb90e098 | 129 | // 0.5 = 0 speed & 0 = full speed |
| Kaikestu | 0:c6cbcb90e098 | 130 | void forward (float speed) { |
| Kaikestu | 0:c6cbcb90e098 | 131 | float y = speed / 100; |
| Kaikestu | 0:c6cbcb90e098 | 132 | float x = y * 0.5; |
| Kaikestu | 0:c6cbcb90e098 | 133 | float z = 0.5 - x; |
| Kaikestu | 0:c6cbcb90e098 | 134 | |
| Kaikestu | 0:c6cbcb90e098 | 135 | motor.write(z); |
| Kaikestu | 0:c6cbcb90e098 | 136 | } |
| Kaikestu | 0:c6cbcb90e098 | 137 | |
| Kaikestu | 0:c6cbcb90e098 | 138 | // 0.5 = 0 speed & 1 = full speed |
| Kaikestu | 0:c6cbcb90e098 | 139 | |
| Kaikestu | 0:c6cbcb90e098 | 140 | void backward (float speed) { |
| Kaikestu | 0:c6cbcb90e098 | 141 | float y = speed / 100; |
| Kaikestu | 0:c6cbcb90e098 | 142 | float x = y * 0.5; |
| Kaikestu | 0:c6cbcb90e098 | 143 | float z = 0.5 + x; |
| Kaikestu | 0:c6cbcb90e098 | 144 | |
| Kaikestu | 0:c6cbcb90e098 | 145 | motor.write(z); |
| Kaikestu | 0:c6cbcb90e098 | 146 | } |
| Kaikestu | 0:c6cbcb90e098 | 147 | |
| Kaikestu | 0:c6cbcb90e098 | 148 | // 0 = right 0.5 = straight 1 = left |
| Kaikestu | 0:c6cbcb90e098 | 149 | void left (void) { |
| Kaikestu | 0:c6cbcb90e098 | 150 | turn.write(1); |
| Kaikestu | 0:c6cbcb90e098 | 151 | } |
| Kaikestu | 0:c6cbcb90e098 | 152 | void right (void) { |
| Kaikestu | 0:c6cbcb90e098 | 153 | turn.write(0); |
| Kaikestu | 0:c6cbcb90e098 | 154 | } |
| Kaikestu | 0:c6cbcb90e098 | 155 | |
| Kaikestu | 0:c6cbcb90e098 | 156 | void stop (void) { |
| Kaikestu | 0:c6cbcb90e098 | 157 | b = 1; |
| Kaikestu | 0:c6cbcb90e098 | 158 | wait(0.3); |
| Kaikestu | 0:c6cbcb90e098 | 159 | reset(); |
| Kaikestu | 0:c6cbcb90e098 | 160 | wait(0.1); |
| Kaikestu | 0:c6cbcb90e098 | 161 | b = 0; |
| Kaikestu | 0:c6cbcb90e098 | 162 | wait(1); |
| Kaikestu | 0:c6cbcb90e098 | 163 | } |