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