Main Code
Dependencies: PinDetect Ulrasonic mbed-rtos mbed WTV020SD_Sound_Breakout_Library
Fork of MC by
main.cpp@16:a3555918c025, 2016-04-26 (annotated)
- Committer:
- mjhaugsdal
- Date:
- Tue Apr 26 14:13:34 2016 +0000
- Revision:
- 16:a3555918c025
- Parent:
- 6:ac5d277bd497
- Child:
- 17:ff0021c4dcdc
Went back to revision 6
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rlanghbv | 0:bd78e433db61 | 1 | #include "mbed.h" |
mjhaugsdal | 1:a3287196a9b3 | 2 | #include "rtos.h" |
rlanghbv | 0:bd78e433db61 | 3 | |
rlanghbv | 2:25bcc26f7a5b | 4 | Serial pc(USBTX, USBRX); // tx, rx |
rlanghbv | 0:bd78e433db61 | 5 | |
mjhaugsdal | 1:a3287196a9b3 | 6 | //Analog Pins |
mjhaugsdal | 5:fdc7a779d525 | 7 | //First engine |
mjhaugsdal | 5:fdc7a779d525 | 8 | DigitalOut IN1(A0); |
mjhaugsdal | 5:fdc7a779d525 | 9 | DigitalOut IN2(A1); |
mjhaugsdal | 5:fdc7a779d525 | 10 | DigitalOut IN3(A2); |
mjhaugsdal | 5:fdc7a779d525 | 11 | DigitalOut IN4(A3); |
mjhaugsdal | 5:fdc7a779d525 | 12 | //Second engine |
mjhaugsdal | 6:ac5d277bd497 | 13 | DigitalOut IN5(D3); |
mjhaugsdal | 6:ac5d277bd497 | 14 | DigitalOut IN6(D4); |
mjhaugsdal | 6:ac5d277bd497 | 15 | DigitalOut IN7(D5); |
mjhaugsdal | 6:ac5d277bd497 | 16 | DigitalOut IN8(D6); |
mjhaugsdal | 16:a3555918c025 | 17 | //Third Engine |
mjhaugsdal | 16:a3555918c025 | 18 | DigitalOut IN9(D8); |
mjhaugsdal | 16:a3555918c025 | 19 | DigitalOut IN10(D9); |
mjhaugsdal | 16:a3555918c025 | 20 | DigitalOut IN11(D10); |
mjhaugsdal | 16:a3555918c025 | 21 | DigitalOut IN12(D11); |
mjhaugsdal | 5:fdc7a779d525 | 22 | |
rlanghbv | 0:bd78e433db61 | 23 | |
mjhaugsdal | 1:a3287196a9b3 | 24 | static int fart = 1000; |
mjhaugsdal | 3:15be78948724 | 25 | static char m_cmd = 'x'; |
rlanghbv | 0:bd78e433db61 | 26 | |
rlanghbv | 0:bd78e433db61 | 27 | |
mjhaugsdal | 5:fdc7a779d525 | 28 | |
rlanghbv | 0:bd78e433db61 | 29 | void step4Right() |
rlanghbv | 0:bd78e433db61 | 30 | { |
mjhaugsdal | 6:ac5d277bd497 | 31 | //engine 1 |
rlanghbv | 0:bd78e433db61 | 32 | IN1=0; |
rlanghbv | 0:bd78e433db61 | 33 | IN2=1; |
rlanghbv | 0:bd78e433db61 | 34 | IN3=0; |
rlanghbv | 0:bd78e433db61 | 35 | IN4=1; |
mjhaugsdal | 6:ac5d277bd497 | 36 | //engine 2 |
mjhaugsdal | 6:ac5d277bd497 | 37 | IN5=0; |
mjhaugsdal | 6:ac5d277bd497 | 38 | IN6=1; |
mjhaugsdal | 6:ac5d277bd497 | 39 | IN7=0; |
mjhaugsdal | 6:ac5d277bd497 | 40 | IN8=1; |
mjhaugsdal | 1:a3287196a9b3 | 41 | wait_us(fart); //legg som global variabel "fart" |
mjhaugsdal | 6:ac5d277bd497 | 42 | //engine 1 |
rlanghbv | 0:bd78e433db61 | 43 | IN1=0; |
rlanghbv | 0:bd78e433db61 | 44 | IN2=1; |
rlanghbv | 0:bd78e433db61 | 45 | IN3=1; |
rlanghbv | 0:bd78e433db61 | 46 | IN4=0; |
mjhaugsdal | 6:ac5d277bd497 | 47 | //engine 2 |
mjhaugsdal | 6:ac5d277bd497 | 48 | IN5=0; |
mjhaugsdal | 6:ac5d277bd497 | 49 | IN6=1; |
mjhaugsdal | 6:ac5d277bd497 | 50 | IN7=1; |
mjhaugsdal | 6:ac5d277bd497 | 51 | IN8=0; |
mjhaugsdal | 1:a3287196a9b3 | 52 | wait_us(fart); |
mjhaugsdal | 6:ac5d277bd497 | 53 | //engine 1 |
rlanghbv | 0:bd78e433db61 | 54 | IN1=1; |
rlanghbv | 0:bd78e433db61 | 55 | IN2=0; |
rlanghbv | 0:bd78e433db61 | 56 | IN3=1; |
rlanghbv | 0:bd78e433db61 | 57 | IN4=0; |
mjhaugsdal | 6:ac5d277bd497 | 58 | //engine 2 |
mjhaugsdal | 6:ac5d277bd497 | 59 | IN5=1; |
mjhaugsdal | 6:ac5d277bd497 | 60 | IN6=0; |
mjhaugsdal | 6:ac5d277bd497 | 61 | IN7=1; |
mjhaugsdal | 6:ac5d277bd497 | 62 | IN8=0; |
mjhaugsdal | 1:a3287196a9b3 | 63 | wait_us(fart); |
mjhaugsdal | 6:ac5d277bd497 | 64 | //engine 1 |
rlanghbv | 0:bd78e433db61 | 65 | IN1=1; |
rlanghbv | 0:bd78e433db61 | 66 | IN2=0; |
rlanghbv | 0:bd78e433db61 | 67 | IN3=0; |
rlanghbv | 0:bd78e433db61 | 68 | IN4=1; |
mjhaugsdal | 6:ac5d277bd497 | 69 | //engine 2 |
mjhaugsdal | 6:ac5d277bd497 | 70 | IN5=1; |
mjhaugsdal | 6:ac5d277bd497 | 71 | IN6=0; |
mjhaugsdal | 6:ac5d277bd497 | 72 | IN7=0; |
mjhaugsdal | 6:ac5d277bd497 | 73 | IN8=1; |
mjhaugsdal | 1:a3287196a9b3 | 74 | wait_us(fart); |
rlanghbv | 0:bd78e433db61 | 75 | } |
rlanghbv | 0:bd78e433db61 | 76 | void step4Left() |
rlanghbv | 0:bd78e433db61 | 77 | { |
mjhaugsdal | 6:ac5d277bd497 | 78 | //engine 1 |
rlanghbv | 0:bd78e433db61 | 79 | IN1=1; |
rlanghbv | 0:bd78e433db61 | 80 | IN2=0; |
rlanghbv | 0:bd78e433db61 | 81 | IN3=0; |
rlanghbv | 0:bd78e433db61 | 82 | IN4=1; |
mjhaugsdal | 6:ac5d277bd497 | 83 | //engine 2 |
mjhaugsdal | 6:ac5d277bd497 | 84 | IN5=1; |
mjhaugsdal | 6:ac5d277bd497 | 85 | IN6=0; |
mjhaugsdal | 6:ac5d277bd497 | 86 | IN7=0; |
mjhaugsdal | 6:ac5d277bd497 | 87 | IN8=1; |
mjhaugsdal | 1:a3287196a9b3 | 88 | wait_us(fart); |
mjhaugsdal | 6:ac5d277bd497 | 89 | //engine 1 |
rlanghbv | 0:bd78e433db61 | 90 | IN1=1; |
rlanghbv | 0:bd78e433db61 | 91 | IN2=0; |
rlanghbv | 0:bd78e433db61 | 92 | IN3=1; |
rlanghbv | 0:bd78e433db61 | 93 | IN4=0; |
mjhaugsdal | 6:ac5d277bd497 | 94 | //engine 2 |
mjhaugsdal | 6:ac5d277bd497 | 95 | IN5=1; |
mjhaugsdal | 6:ac5d277bd497 | 96 | IN6=0; |
mjhaugsdal | 6:ac5d277bd497 | 97 | IN7=1; |
mjhaugsdal | 6:ac5d277bd497 | 98 | IN8=0; |
mjhaugsdal | 1:a3287196a9b3 | 99 | wait_us(fart); |
mjhaugsdal | 6:ac5d277bd497 | 100 | //engine 1 |
rlanghbv | 0:bd78e433db61 | 101 | IN1=0; |
rlanghbv | 0:bd78e433db61 | 102 | IN2=1; |
rlanghbv | 0:bd78e433db61 | 103 | IN3=1; |
rlanghbv | 0:bd78e433db61 | 104 | IN4=0; |
mjhaugsdal | 6:ac5d277bd497 | 105 | //engine 2 |
mjhaugsdal | 6:ac5d277bd497 | 106 | IN5=0; |
mjhaugsdal | 6:ac5d277bd497 | 107 | IN6=1; |
mjhaugsdal | 6:ac5d277bd497 | 108 | IN7=1; |
mjhaugsdal | 6:ac5d277bd497 | 109 | IN8=0; |
mjhaugsdal | 1:a3287196a9b3 | 110 | wait_us(fart); |
mjhaugsdal | 6:ac5d277bd497 | 111 | //engine 1 |
rlanghbv | 0:bd78e433db61 | 112 | IN1=0; |
rlanghbv | 0:bd78e433db61 | 113 | IN2=1; |
rlanghbv | 0:bd78e433db61 | 114 | IN3=0; |
rlanghbv | 0:bd78e433db61 | 115 | IN4=1; |
mjhaugsdal | 6:ac5d277bd497 | 116 | //engine 2 |
mjhaugsdal | 6:ac5d277bd497 | 117 | IN5=0; |
mjhaugsdal | 6:ac5d277bd497 | 118 | IN6=1; |
mjhaugsdal | 6:ac5d277bd497 | 119 | IN7=0; |
mjhaugsdal | 6:ac5d277bd497 | 120 | IN8=1; |
mjhaugsdal | 1:a3287196a9b3 | 121 | wait_us(fart); |
rlanghbv | 0:bd78e433db61 | 122 | } |
rlanghbv | 0:bd78e433db61 | 123 | |
mjhaugsdal | 1:a3287196a9b3 | 124 | |
mjhaugsdal | 1:a3287196a9b3 | 125 | void input(void const *args) |
mjhaugsdal | 1:a3287196a9b3 | 126 | { |
mjhaugsdal | 1:a3287196a9b3 | 127 | while(true) |
mjhaugsdal | 1:a3287196a9b3 | 128 | { |
mjhaugsdal | 3:15be78948724 | 129 | |
rlanghbv | 2:25bcc26f7a5b | 130 | if(pc.readable()) |
mjhaugsdal | 3:15be78948724 | 131 | { m_cmd=pc.getc(); |
mjhaugsdal | 3:15be78948724 | 132 | |
mjhaugsdal | 3:15be78948724 | 133 | } |
rlanghbv | 2:25bcc26f7a5b | 134 | |
rlanghbv | 2:25bcc26f7a5b | 135 | Thread::wait(10); |
mjhaugsdal | 5:fdc7a779d525 | 136 | //pc.printf("%d", steps); |
mjhaugsdal | 1:a3287196a9b3 | 137 | } |
mjhaugsdal | 1:a3287196a9b3 | 138 | } |
rlanghbv | 0:bd78e433db61 | 139 | |
rlanghbv | 0:bd78e433db61 | 140 | int main() |
rlanghbv | 0:bd78e433db61 | 141 | { |
mjhaugsdal | 1:a3287196a9b3 | 142 | //Thread 1 has constant feed from usb |
rlanghbv | 2:25bcc26f7a5b | 143 | Thread t1(input); |
mjhaugsdal | 3:15be78948724 | 144 | static char global_direction; |
mjhaugsdal | 3:15be78948724 | 145 | printf("Started"); |
mjhaugsdal | 3:15be78948724 | 146 | |
mjhaugsdal | 5:fdc7a779d525 | 147 | |
mjhaugsdal | 1:a3287196a9b3 | 148 | while (true) |
mjhaugsdal | 1:a3287196a9b3 | 149 | { |
mjhaugsdal | 4:4d655fdae399 | 150 | //Thread::wait(5); |
mjhaugsdal | 3:15be78948724 | 151 | |
mjhaugsdal | 4:4d655fdae399 | 152 | //0 STEPS LEFT AT MAX SPEED |
mjhaugsdal | 3:15be78948724 | 153 | if (m_cmd == '0') |
mjhaugsdal | 3:15be78948724 | 154 | { |
mjhaugsdal | 4:4d655fdae399 | 155 | fart = 1000; |
mjhaugsdal | 5:fdc7a779d525 | 156 | //global_direction = '0'; |
mjhaugsdal | 3:15be78948724 | 157 | step4Left(); |
mjhaugsdal | 5:fdc7a779d525 | 158 | //steps = steps + step; |
mjhaugsdal | 5:fdc7a779d525 | 159 | |
mjhaugsdal | 5:fdc7a779d525 | 160 | |
mjhaugsdal | 4:4d655fdae399 | 161 | //printf("LEFT \n"); |
mjhaugsdal | 3:15be78948724 | 162 | //printf("%c \n" + global_direction); |
mjhaugsdal | 3:15be78948724 | 163 | } |
mjhaugsdal | 4:4d655fdae399 | 164 | //1 STEPS RIGHT AT MAX SPEED |
mjhaugsdal | 3:15be78948724 | 165 | else if (m_cmd == '1') |
mjhaugsdal | 3:15be78948724 | 166 | { |
mjhaugsdal | 4:4d655fdae399 | 167 | fart = 1000; |
mjhaugsdal | 5:fdc7a779d525 | 168 | //global_direction = '1'; |
mjhaugsdal | 6:ac5d277bd497 | 169 | step4Right(); |
mjhaugsdal | 3:15be78948724 | 170 | } |
mjhaugsdal | 4:4d655fdae399 | 171 | else |
mjhaugsdal | 4:4d655fdae399 | 172 | { |
mjhaugsdal | 5:fdc7a779d525 | 173 | //global_direction = 'x'; |
mjhaugsdal | 4:4d655fdae399 | 174 | } |
mjhaugsdal | 6:ac5d277bd497 | 175 | |
mjhaugsdal | 3:15be78948724 | 176 | if (m_cmd == 'a') |
mjhaugsdal | 3:15be78948724 | 177 | { |
mjhaugsdal | 4:4d655fdae399 | 178 | fart = 2000; |
mjhaugsdal | 5:fdc7a779d525 | 179 | //global_direction = '1'; |
mjhaugsdal | 4:4d655fdae399 | 180 | step4Right(); |
mjhaugsdal | 5:fdc7a779d525 | 181 | //steps = steps + step; |
mjhaugsdal | 5:fdc7a779d525 | 182 | // printf("%i", &steps); |
mjhaugsdal | 3:15be78948724 | 183 | |
mjhaugsdal | 3:15be78948724 | 184 | } |
mjhaugsdal | 4:4d655fdae399 | 185 | |
mjhaugsdal | 4:4d655fdae399 | 186 | else if (m_cmd == 'c') |
mjhaugsdal | 3:15be78948724 | 187 | { |
mjhaugsdal | 4:4d655fdae399 | 188 | fart = 2000; |
mjhaugsdal | 5:fdc7a779d525 | 189 | //global_direction = '0'; |
mjhaugsdal | 1:a3287196a9b3 | 190 | step4Left(); |
mjhaugsdal | 5:fdc7a779d525 | 191 | |
mjhaugsdal | 5:fdc7a779d525 | 192 | //steps = steps + step; |
mjhaugsdal | 5:fdc7a779d525 | 193 | // printf("%i", &steps); |
mjhaugsdal | 3:15be78948724 | 194 | } |
mjhaugsdal | 5:fdc7a779d525 | 195 | else if (m_cmd == 'k') |
mjhaugsdal | 1:a3287196a9b3 | 196 | { |
mjhaugsdal | 6:ac5d277bd497 | 197 | int steps = 242; |
mjhaugsdal | 5:fdc7a779d525 | 198 | |
mjhaugsdal | 5:fdc7a779d525 | 199 | |
mjhaugsdal | 6:ac5d277bd497 | 200 | //Steps = 242 gives a 60' rotation to the left. |
mjhaugsdal | 5:fdc7a779d525 | 201 | while (steps >= 0) |
mjhaugsdal | 5:fdc7a779d525 | 202 | { |
mjhaugsdal | 5:fdc7a779d525 | 203 | step4Left(); |
mjhaugsdal | 5:fdc7a779d525 | 204 | steps--; |
mjhaugsdal | 5:fdc7a779d525 | 205 | } |
mjhaugsdal | 5:fdc7a779d525 | 206 | m_cmd = 'x'; |
mjhaugsdal | 3:15be78948724 | 207 | } |
mjhaugsdal | 5:fdc7a779d525 | 208 | else if (m_cmd == 'l') |
mjhaugsdal | 5:fdc7a779d525 | 209 | { |
mjhaugsdal | 6:ac5d277bd497 | 210 | int steps = 242; |
mjhaugsdal | 6:ac5d277bd497 | 211 | //Steps = 242 gives a 60' rotation to the right. |
mjhaugsdal | 5:fdc7a779d525 | 212 | while (steps >= 0) |
mjhaugsdal | 5:fdc7a779d525 | 213 | { |
mjhaugsdal | 5:fdc7a779d525 | 214 | step4Right(); |
mjhaugsdal | 5:fdc7a779d525 | 215 | steps--; |
mjhaugsdal | 5:fdc7a779d525 | 216 | } |
mjhaugsdal | 5:fdc7a779d525 | 217 | m_cmd = 'x'; |
mjhaugsdal | 5:fdc7a779d525 | 218 | |
mjhaugsdal | 5:fdc7a779d525 | 219 | } |
mjhaugsdal | 5:fdc7a779d525 | 220 | |
mjhaugsdal | 5:fdc7a779d525 | 221 | |
mjhaugsdal | 5:fdc7a779d525 | 222 | |
mjhaugsdal | 4:4d655fdae399 | 223 | |
mjhaugsdal | 1:a3287196a9b3 | 224 | } |
mjhaugsdal | 1:a3287196a9b3 | 225 | |
mjhaugsdal | 3:15be78948724 | 226 | } //END Main |