USN Robotics
Main Code
Dependencies: PinDetect Ulrasonic mbed-rtos mbed WTV020SD_Sound_Breakout_Library
Fork of
MC
by 
Robot Bachelor
main.cpp@36:3011c70a5c81, 2016-09-22 (annotated)
- Committer:
- benrammok
- Date:
- Thu Sep 22 13:48:34 2016 +0000
- Revision:
- 36:3011c70a5c81
- Parent:
- 35:64ed42e6f9a4
- Child:
- 37:22ce2b4040f7
Lagt til WTV020M01 Sound Board
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" |
| mjhaugsdal | 23:ad08a8eabc24 | 3 | #include "PinDetect.h" |
| benrammok | 36:3011c70a5c81 | 4 | #include "WTV020M01.h" |
| azadmehr | 33:6f500188e36d | 5 | |
| azadmehr | 32:5163db06f6cc | 6 | #include "ultrasonic.h" |
| rlanghbv | 0:bd78e433db61 | 7 | |
| benrammok | 35:64ed42e6f9a4 | 8 | //Needs to be define to be able to read from the Ultrasonic sensor |
| benrammok | 35:64ed42e6f9a4 | 9 | void dist(int); |
| benrammok | 35:64ed42e6f9a4 | 10 | ultrasonic mu(A2, A3, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9 |
| benrammok | 35:64ed42e6f9a4 | 11 | //have updates every .1 seconds and a timeout after 1 |
| benrammok | 35:64ed42e6f9a4 | 12 | //second, and call dist when the distance changes |
| benrammok | 35:64ed42e6f9a4 | 13 | |
| rlanghbv | 2:25bcc26f7a5b | 14 | Serial pc(USBTX, USBRX); // tx, rx |
| benrammok | 36:3011c70a5c81 | 15 | WTV020M01 sndBoard(PB_13, PB_14, PB_15, PB_1, NC); // NUCLEO F334R8 |
| benrammok | 36:3011c70a5c81 | 16 | //WTV020M01 sndBoard(PG_1, PF_9, PF_7, PF_8, NC); // Soundboard - NUCLEO F746ZG |
| azadmehr | 31:358d4c032d76 | 17 | //MidMagnetSensors |
| azadmehr | 31:358d4c032d76 | 18 | PinDetect pb3(D10); |
| azadmehr | 31:358d4c032d76 | 19 | PinDetect pb4(D11); |
| mjhaugsdal | 25:321b970eb3ff | 20 | |
| azadmehr | 31:358d4c032d76 | 21 | //BottomMagnetSensors |
| azadmehr | 31:358d4c032d76 | 22 | PinDetect pb1(D12); |
| azadmehr | 31:358d4c032d76 | 23 | PinDetect pb2(D13); |
| mjhaugsdal | 25:321b970eb3ff | 24 | |
| azadmehr | 31:358d4c032d76 | 25 | //TopMagnetSensors |
| azadmehr | 31:358d4c032d76 | 26 | PinDetect pb5(D8); |
| azadmehr | 31:358d4c032d76 | 27 | PinDetect pb6(D9); |
| azadmehr | 31:358d4c032d76 | 28 | |
| azadmehr | 31:358d4c032d76 | 29 | |
| rlanghbv | 0:bd78e433db61 | 30 | |
| mjhaugsdal | 1:a3287196a9b3 | 31 | //Analog Pins |
| azadmehr | 31:358d4c032d76 | 32 | //Bottomengine |
| mjhaugsdal | 5:fdc7a779d525 | 33 | DigitalOut IN1(A0); |
| mjhaugsdal | 5:fdc7a779d525 | 34 | DigitalOut IN2(A1); |
| mjhaugsdal | 5:fdc7a779d525 | 35 | DigitalOut IN3(A2); |
| mjhaugsdal | 5:fdc7a779d525 | 36 | DigitalOut IN4(A3); |
| azadmehr | 31:358d4c032d76 | 37 | //SecondEngine |
| azadmehr | 31:358d4c032d76 | 38 | DigitalOut IN5(D2); |
| azadmehr | 31:358d4c032d76 | 39 | DigitalOut IN6(D3); |
| azadmehr | 31:358d4c032d76 | 40 | DigitalOut IN7(A4); |
| azadmehr | 31:358d4c032d76 | 41 | DigitalOut IN8(A5); |
| azadmehr | 31:358d4c032d76 | 42 | //TopEngine |
| azadmehr | 31:358d4c032d76 | 43 | DigitalOut IN9(D4); |
| azadmehr | 31:358d4c032d76 | 44 | DigitalOut IN10(D5); |
| azadmehr | 31:358d4c032d76 | 45 | DigitalOut IN11(D6); |
| azadmehr | 31:358d4c032d76 | 46 | DigitalOut IN12(D7); |
| mjhaugsdal | 5:fdc7a779d525 | 47 | |
| mjhaugsdal | 28:dac262b7ab32 | 48 | //User button |
| mjhaugsdal | 28:dac262b7ab32 | 49 | DigitalIn SW(USER_BUTTON); |
| mjhaugsdal | 28:dac262b7ab32 | 50 | |
| rlanghbv | 0:bd78e433db61 | 51 | |
| mjhaugsdal | 30:fac5b5624fe3 | 52 | static int speed = 1000; |
| mjhaugsdal | 30:fac5b5624fe3 | 53 | |
| mjhaugsdal | 3:15be78948724 | 54 | static char m_cmd = 'x'; |
| mjhaugsdal | 27:35c962e8e95b | 55 | static char cmd; |
| mjhaugsdal | 23:ad08a8eabc24 | 56 | static bool e1 = true; |
| mjhaugsdal | 23:ad08a8eabc24 | 57 | static bool e2 = true; |
| mjhaugsdal | 23:ad08a8eabc24 | 58 | static bool e3 = true; |
| mjhaugsdal | 30:fac5b5624fe3 | 59 | static int i1 = 0; |
| mjhaugsdal | 30:fac5b5624fe3 | 60 | static int i2 = 0; |
| mjhaugsdal | 30:fac5b5624fe3 | 61 | static int i3 = 0; |
| rlanghbv | 0:bd78e433db61 | 62 | |
| mjhaugsdal | 24:24c91a6ae6ba | 63 | static int steps1 = 0; |
| mjhaugsdal | 24:24c91a6ae6ba | 64 | static int steps2 = 0; |
| mjhaugsdal | 24:24c91a6ae6ba | 65 | static int steps3 = 0; |
| mjhaugsdal | 24:24c91a6ae6ba | 66 | |
| mjhaugsdal | 24:24c91a6ae6ba | 67 | |
| benrammok | 35:64ed42e6f9a4 | 68 | //Distance stored from sensor |
| benrammok | 35:64ed42e6f9a4 | 69 | static int sDistance = 0; |
| benrammok | 36:3011c70a5c81 | 70 | //Delayed distance to compare with first meassurement |
| benrammok | 36:3011c70a5c81 | 71 | static int dDistance = 0; |
| benrammok | 36:3011c70a5c81 | 72 | static bool isPlaying = false; |
| azadmehr | 32:5163db06f6cc | 73 | |
| azadmehr | 32:5163db06f6cc | 74 | |
| azadmehr | 32:5163db06f6cc | 75 | |
| mjhaugsdal | 27:35c962e8e95b | 76 | |
| mjhaugsdal | 27:35c962e8e95b | 77 | void step2Left3Right() |
| mjhaugsdal | 27:35c962e8e95b | 78 | { |
| mjhaugsdal | 27:35c962e8e95b | 79 | //engine 2 |
| mjhaugsdal | 27:35c962e8e95b | 80 | IN5=1; |
| mjhaugsdal | 27:35c962e8e95b | 81 | IN6=0; |
| mjhaugsdal | 27:35c962e8e95b | 82 | IN7=0; |
| mjhaugsdal | 27:35c962e8e95b | 83 | IN8=1; |
| mjhaugsdal | 27:35c962e8e95b | 84 | //engine 3 |
| mjhaugsdal | 27:35c962e8e95b | 85 | IN9=0; |
| mjhaugsdal | 27:35c962e8e95b | 86 | IN10=1; |
| mjhaugsdal | 27:35c962e8e95b | 87 | IN11=0; |
| mjhaugsdal | 27:35c962e8e95b | 88 | IN12=1; |
| mjhaugsdal | 27:35c962e8e95b | 89 | |
| mjhaugsdal | 27:35c962e8e95b | 90 | wait_us(speed); |
| mjhaugsdal | 27:35c962e8e95b | 91 | IN5=1; |
| mjhaugsdal | 27:35c962e8e95b | 92 | IN6=0; |
| mjhaugsdal | 27:35c962e8e95b | 93 | IN7=1; |
| mjhaugsdal | 27:35c962e8e95b | 94 | IN8=0; |
| mjhaugsdal | 27:35c962e8e95b | 95 | IN9=0; |
| mjhaugsdal | 27:35c962e8e95b | 96 | IN10=1; |
| mjhaugsdal | 27:35c962e8e95b | 97 | IN11=1; |
| mjhaugsdal | 27:35c962e8e95b | 98 | IN12=0; |
| mjhaugsdal | 27:35c962e8e95b | 99 | wait_us(speed); |
| mjhaugsdal | 27:35c962e8e95b | 100 | IN5=0; |
| mjhaugsdal | 27:35c962e8e95b | 101 | IN6=1; |
| mjhaugsdal | 27:35c962e8e95b | 102 | IN7=1; |
| mjhaugsdal | 27:35c962e8e95b | 103 | IN8=0; |
| mjhaugsdal | 27:35c962e8e95b | 104 | IN9=1; |
| mjhaugsdal | 27:35c962e8e95b | 105 | IN10=0; |
| mjhaugsdal | 27:35c962e8e95b | 106 | IN11=1; |
| mjhaugsdal | 27:35c962e8e95b | 107 | IN12=0; |
| mjhaugsdal | 27:35c962e8e95b | 108 | wait_us(speed); |
| mjhaugsdal | 27:35c962e8e95b | 109 | IN5=0; |
| mjhaugsdal | 27:35c962e8e95b | 110 | IN6=1; |
| mjhaugsdal | 27:35c962e8e95b | 111 | IN7=0; |
| mjhaugsdal | 27:35c962e8e95b | 112 | IN8=1; |
| mjhaugsdal | 27:35c962e8e95b | 113 | IN9=1; |
| mjhaugsdal | 27:35c962e8e95b | 114 | IN10=0; |
| mjhaugsdal | 27:35c962e8e95b | 115 | IN11=0; |
| mjhaugsdal | 27:35c962e8e95b | 116 | IN12=1; |
| mjhaugsdal | 27:35c962e8e95b | 117 | wait_us(speed); |
| mjhaugsdal | 27:35c962e8e95b | 118 | } |
| mjhaugsdal | 27:35c962e8e95b | 119 | |
| mjhaugsdal | 17:ff0021c4dcdc | 120 | void stepAllRight() |
| rlanghbv | 0:bd78e433db61 | 121 | { |
| mjhaugsdal | 6:ac5d277bd497 | 122 | //engine 1 |
| rlanghbv | 0:bd78e433db61 | 123 | IN1=0; |
| rlanghbv | 0:bd78e433db61 | 124 | IN2=1; |
| rlanghbv | 0:bd78e433db61 | 125 | IN3=0; |
| rlanghbv | 0:bd78e433db61 | 126 | IN4=1; |
| mjhaugsdal | 6:ac5d277bd497 | 127 | //engine 2 |
| mjhaugsdal | 6:ac5d277bd497 | 128 | IN5=0; |
| mjhaugsdal | 6:ac5d277bd497 | 129 | IN6=1; |
| mjhaugsdal | 6:ac5d277bd497 | 130 | IN7=0; |
| mjhaugsdal | 6:ac5d277bd497 | 131 | IN8=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 132 | //engine 3 |
| mjhaugsdal | 17:ff0021c4dcdc | 133 | IN9=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 134 | IN10=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 135 | IN11=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 136 | IN12=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 137 | wait_us(speed); //legg som global variabel "fart" |
| mjhaugsdal | 6:ac5d277bd497 | 138 | //engine 1 |
| rlanghbv | 0:bd78e433db61 | 139 | IN1=0; |
| rlanghbv | 0:bd78e433db61 | 140 | IN2=1; |
| rlanghbv | 0:bd78e433db61 | 141 | IN3=1; |
| rlanghbv | 0:bd78e433db61 | 142 | IN4=0; |
| mjhaugsdal | 6:ac5d277bd497 | 143 | //engine 2 |
| mjhaugsdal | 6:ac5d277bd497 | 144 | IN5=0; |
| mjhaugsdal | 6:ac5d277bd497 | 145 | IN6=1; |
| mjhaugsdal | 6:ac5d277bd497 | 146 | IN7=1; |
| mjhaugsdal | 6:ac5d277bd497 | 147 | IN8=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 148 | //engine 3 |
| mjhaugsdal | 17:ff0021c4dcdc | 149 | IN9=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 150 | IN10=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 151 | IN11=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 152 | IN12=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 153 | wait_us(speed); |
| mjhaugsdal | 6:ac5d277bd497 | 154 | //engine 1 |
| rlanghbv | 0:bd78e433db61 | 155 | IN1=1; |
| rlanghbv | 0:bd78e433db61 | 156 | IN2=0; |
| rlanghbv | 0:bd78e433db61 | 157 | IN3=1; |
| rlanghbv | 0:bd78e433db61 | 158 | IN4=0; |
| mjhaugsdal | 6:ac5d277bd497 | 159 | //engine 2 |
| mjhaugsdal | 6:ac5d277bd497 | 160 | IN5=1; |
| mjhaugsdal | 6:ac5d277bd497 | 161 | IN6=0; |
| mjhaugsdal | 6:ac5d277bd497 | 162 | IN7=1; |
| mjhaugsdal | 6:ac5d277bd497 | 163 | IN8=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 164 | //engine 3 |
| mjhaugsdal | 17:ff0021c4dcdc | 165 | IN9=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 166 | IN10=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 167 | IN11=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 168 | IN12=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 169 | wait_us(speed); |
| mjhaugsdal | 6:ac5d277bd497 | 170 | //engine 1 |
| rlanghbv | 0:bd78e433db61 | 171 | IN1=1; |
| rlanghbv | 0:bd78e433db61 | 172 | IN2=0; |
| rlanghbv | 0:bd78e433db61 | 173 | IN3=0; |
| rlanghbv | 0:bd78e433db61 | 174 | IN4=1; |
| mjhaugsdal | 6:ac5d277bd497 | 175 | //engine 2 |
| mjhaugsdal | 6:ac5d277bd497 | 176 | IN5=1; |
| mjhaugsdal | 6:ac5d277bd497 | 177 | IN6=0; |
| mjhaugsdal | 6:ac5d277bd497 | 178 | IN7=0; |
| mjhaugsdal | 6:ac5d277bd497 | 179 | IN8=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 180 | //engine 3 |
| mjhaugsdal | 17:ff0021c4dcdc | 181 | IN9=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 182 | IN10=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 183 | IN11=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 184 | IN12=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 185 | wait_us(speed); |
| rlanghbv | 0:bd78e433db61 | 186 | } |
| mjhaugsdal | 17:ff0021c4dcdc | 187 | void stepAllLeft() |
| rlanghbv | 0:bd78e433db61 | 188 | { |
| mjhaugsdal | 6:ac5d277bd497 | 189 | //engine 1 |
| rlanghbv | 0:bd78e433db61 | 190 | IN1=1; |
| rlanghbv | 0:bd78e433db61 | 191 | IN2=0; |
| rlanghbv | 0:bd78e433db61 | 192 | IN3=0; |
| rlanghbv | 0:bd78e433db61 | 193 | IN4=1; |
| mjhaugsdal | 6:ac5d277bd497 | 194 | //engine 2 |
| mjhaugsdal | 6:ac5d277bd497 | 195 | IN5=1; |
| mjhaugsdal | 6:ac5d277bd497 | 196 | IN6=0; |
| mjhaugsdal | 6:ac5d277bd497 | 197 | IN7=0; |
| mjhaugsdal | 6:ac5d277bd497 | 198 | IN8=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 199 | //engine 3 |
| mjhaugsdal | 17:ff0021c4dcdc | 200 | IN9=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 201 | IN10=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 202 | IN11=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 203 | IN12=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 204 | wait_us(speed); |
| mjhaugsdal | 6:ac5d277bd497 | 205 | //engine 1 |
| rlanghbv | 0:bd78e433db61 | 206 | IN1=1; |
| rlanghbv | 0:bd78e433db61 | 207 | IN2=0; |
| rlanghbv | 0:bd78e433db61 | 208 | IN3=1; |
| rlanghbv | 0:bd78e433db61 | 209 | IN4=0; |
| mjhaugsdal | 6:ac5d277bd497 | 210 | //engine 2 |
| mjhaugsdal | 6:ac5d277bd497 | 211 | IN5=1; |
| mjhaugsdal | 6:ac5d277bd497 | 212 | IN6=0; |
| mjhaugsdal | 6:ac5d277bd497 | 213 | IN7=1; |
| mjhaugsdal | 6:ac5d277bd497 | 214 | IN8=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 215 | //engine 3 |
| mjhaugsdal | 17:ff0021c4dcdc | 216 | IN9=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 217 | IN10=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 218 | IN11=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 219 | IN12=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 220 | wait_us(speed); |
| mjhaugsdal | 6:ac5d277bd497 | 221 | //engine 1 |
| rlanghbv | 0:bd78e433db61 | 222 | IN1=0; |
| rlanghbv | 0:bd78e433db61 | 223 | IN2=1; |
| rlanghbv | 0:bd78e433db61 | 224 | IN3=1; |
| rlanghbv | 0:bd78e433db61 | 225 | IN4=0; |
| mjhaugsdal | 6:ac5d277bd497 | 226 | //engine 2 |
| mjhaugsdal | 6:ac5d277bd497 | 227 | IN5=0; |
| mjhaugsdal | 6:ac5d277bd497 | 228 | IN6=1; |
| mjhaugsdal | 6:ac5d277bd497 | 229 | IN7=1; |
| mjhaugsdal | 6:ac5d277bd497 | 230 | IN8=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 231 | //engine 3 |
| mjhaugsdal | 17:ff0021c4dcdc | 232 | IN9=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 233 | IN10=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 234 | IN11=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 235 | IN12=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 236 | wait_us(speed); |
| mjhaugsdal | 6:ac5d277bd497 | 237 | //engine 1 |
| rlanghbv | 0:bd78e433db61 | 238 | IN1=0; |
| rlanghbv | 0:bd78e433db61 | 239 | IN2=1; |
| rlanghbv | 0:bd78e433db61 | 240 | IN3=0; |
| rlanghbv | 0:bd78e433db61 | 241 | IN4=1; |
| mjhaugsdal | 6:ac5d277bd497 | 242 | //engine 2 |
| mjhaugsdal | 6:ac5d277bd497 | 243 | IN5=0; |
| mjhaugsdal | 6:ac5d277bd497 | 244 | IN6=1; |
| mjhaugsdal | 6:ac5d277bd497 | 245 | IN7=0; |
| mjhaugsdal | 6:ac5d277bd497 | 246 | IN8=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 247 | //engine 3 |
| mjhaugsdal | 17:ff0021c4dcdc | 248 | IN9=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 249 | IN10=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 250 | IN11=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 251 | IN12=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 252 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 253 | } |
| mjhaugsdal | 17:ff0021c4dcdc | 254 | |
| mjhaugsdal | 17:ff0021c4dcdc | 255 | void stepEngine1Left() |
| mjhaugsdal | 17:ff0021c4dcdc | 256 | { |
| mjhaugsdal | 17:ff0021c4dcdc | 257 | //engine 1 |
| mjhaugsdal | 17:ff0021c4dcdc | 258 | IN1=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 259 | IN2=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 260 | IN3=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 261 | IN4=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 262 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 263 | IN1=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 264 | IN2=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 265 | IN3=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 266 | IN4=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 267 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 268 | IN1=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 269 | IN2=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 270 | IN3=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 271 | IN4=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 272 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 273 | IN1=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 274 | IN2=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 275 | IN3=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 276 | IN4=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 277 | wait_us(speed); |
| rlanghbv | 0:bd78e433db61 | 278 | } |
| rlanghbv | 0:bd78e433db61 | 279 | |
| mjhaugsdal | 17:ff0021c4dcdc | 280 | void stepEngine1Right() |
| mjhaugsdal | 17:ff0021c4dcdc | 281 | { |
| mjhaugsdal | 17:ff0021c4dcdc | 282 | //engine 1 |
| mjhaugsdal | 17:ff0021c4dcdc | 283 | IN1=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 284 | IN2=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 285 | IN3=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 286 | IN4=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 287 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 288 | IN1=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 289 | IN2=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 290 | IN3=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 291 | IN4=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 292 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 293 | IN1=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 294 | IN2=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 295 | IN3=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 296 | IN4=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 297 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 298 | IN1=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 299 | IN2=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 300 | IN3=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 301 | IN4=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 302 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 303 | } |
| mjhaugsdal | 17:ff0021c4dcdc | 304 | |
| mjhaugsdal | 17:ff0021c4dcdc | 305 | void stepEngine2Right() |
| mjhaugsdal | 17:ff0021c4dcdc | 306 | { |
| mjhaugsdal | 17:ff0021c4dcdc | 307 | //engine 1 |
| mjhaugsdal | 17:ff0021c4dcdc | 308 | IN5=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 309 | IN6=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 310 | IN7=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 311 | IN8=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 312 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 313 | IN5=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 314 | IN6=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 315 | IN7=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 316 | IN8=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 317 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 318 | IN5=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 319 | IN6=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 320 | IN7=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 321 | IN8=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 322 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 323 | IN5=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 324 | IN6=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 325 | IN7=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 326 | IN8=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 327 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 328 | } |
| mjhaugsdal | 17:ff0021c4dcdc | 329 | |
| mjhaugsdal | 17:ff0021c4dcdc | 330 | void stepEngine2Left() |
| mjhaugsdal | 17:ff0021c4dcdc | 331 | { |
| mjhaugsdal | 17:ff0021c4dcdc | 332 | //engine 1 |
| mjhaugsdal | 17:ff0021c4dcdc | 333 | IN5=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 334 | IN6=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 335 | IN7=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 336 | IN8=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 337 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 338 | IN5=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 339 | IN6=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 340 | IN7=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 341 | IN8=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 342 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 343 | IN5=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 344 | IN6=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 345 | IN7=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 346 | IN8=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 347 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 348 | IN5=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 349 | IN6=1; |
| mjhaugsdal | 17:ff0021c4dcdc | 350 | IN7=0; |
| mjhaugsdal | 17:ff0021c4dcdc | 351 | IN8=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 352 | wait_us(speed); |
| mjhaugsdal | 17:ff0021c4dcdc | 353 | } |
| mjhaugsdal | 17:ff0021c4dcdc | 354 | |
| mjhaugsdal | 19:79600d3509d7 | 355 | void stepEngine3Right() |
| mjhaugsdal | 19:79600d3509d7 | 356 | { |
| mjhaugsdal | 19:79600d3509d7 | 357 | //engine 1 |
| mjhaugsdal | 19:79600d3509d7 | 358 | IN9=0; |
| mjhaugsdal | 19:79600d3509d7 | 359 | IN10=1; |
| mjhaugsdal | 19:79600d3509d7 | 360 | IN11=0; |
| mjhaugsdal | 19:79600d3509d7 | 361 | IN12=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 362 | wait_us(speed); |
| mjhaugsdal | 19:79600d3509d7 | 363 | IN9=0; |
| mjhaugsdal | 19:79600d3509d7 | 364 | IN10=1; |
| mjhaugsdal | 19:79600d3509d7 | 365 | IN11=1; |
| mjhaugsdal | 19:79600d3509d7 | 366 | IN12=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 367 | wait_us(speed); |
| mjhaugsdal | 19:79600d3509d7 | 368 | IN9=1; |
| mjhaugsdal | 19:79600d3509d7 | 369 | IN10=0; |
| mjhaugsdal | 19:79600d3509d7 | 370 | IN11=1; |
| mjhaugsdal | 19:79600d3509d7 | 371 | IN12=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 372 | wait_us(speed); |
| mjhaugsdal | 19:79600d3509d7 | 373 | IN9=1; |
| mjhaugsdal | 19:79600d3509d7 | 374 | IN10=0; |
| mjhaugsdal | 19:79600d3509d7 | 375 | IN11=0; |
| mjhaugsdal | 19:79600d3509d7 | 376 | IN12=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 377 | wait_us(speed); |
| mjhaugsdal | 19:79600d3509d7 | 378 | } |
| mjhaugsdal | 19:79600d3509d7 | 379 | |
| mjhaugsdal | 19:79600d3509d7 | 380 | void stepEngine3Left() |
| mjhaugsdal | 19:79600d3509d7 | 381 | { |
| mjhaugsdal | 19:79600d3509d7 | 382 | //engine 1 |
| mjhaugsdal | 19:79600d3509d7 | 383 | IN9=1; |
| mjhaugsdal | 19:79600d3509d7 | 384 | IN10=0; |
| mjhaugsdal | 19:79600d3509d7 | 385 | IN11=0; |
| mjhaugsdal | 19:79600d3509d7 | 386 | IN12=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 387 | wait_us(speed); |
| mjhaugsdal | 19:79600d3509d7 | 388 | IN9=1; |
| mjhaugsdal | 19:79600d3509d7 | 389 | IN10=0; |
| mjhaugsdal | 19:79600d3509d7 | 390 | IN11=1; |
| mjhaugsdal | 19:79600d3509d7 | 391 | IN12=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 392 | wait_us(speed); |
| mjhaugsdal | 19:79600d3509d7 | 393 | IN9=0; |
| mjhaugsdal | 19:79600d3509d7 | 394 | IN10=1; |
| mjhaugsdal | 19:79600d3509d7 | 395 | IN11=1; |
| mjhaugsdal | 19:79600d3509d7 | 396 | IN12=0; |
| mjhaugsdal | 24:24c91a6ae6ba | 397 | wait_us(speed); |
| mjhaugsdal | 19:79600d3509d7 | 398 | IN9=0; |
| mjhaugsdal | 19:79600d3509d7 | 399 | IN10=1; |
| mjhaugsdal | 19:79600d3509d7 | 400 | IN11=0; |
| mjhaugsdal | 19:79600d3509d7 | 401 | IN12=1; |
| mjhaugsdal | 24:24c91a6ae6ba | 402 | wait_us(speed); |
| mjhaugsdal | 19:79600d3509d7 | 403 | } |
| mjhaugsdal | 19:79600d3509d7 | 404 | |
| mjhaugsdal | 21:9f3ae352ee63 | 405 | //Method to release all engines. |
| mjhaugsdal | 18:ef02a9014491 | 406 | void stopAll() |
| mjhaugsdal | 18:ef02a9014491 | 407 | { |
| mjhaugsdal | 18:ef02a9014491 | 408 | IN1=0; |
| mjhaugsdal | 18:ef02a9014491 | 409 | IN2=0; |
| mjhaugsdal | 18:ef02a9014491 | 410 | IN3=0; |
| mjhaugsdal | 18:ef02a9014491 | 411 | IN4=0; |
| mjhaugsdal | 18:ef02a9014491 | 412 | IN5=0; |
| mjhaugsdal | 18:ef02a9014491 | 413 | IN6=0; |
| mjhaugsdal | 18:ef02a9014491 | 414 | IN7=0; |
| mjhaugsdal | 18:ef02a9014491 | 415 | IN8=0; |
| mjhaugsdal | 18:ef02a9014491 | 416 | IN9=0; |
| mjhaugsdal | 18:ef02a9014491 | 417 | IN10=0; |
| mjhaugsdal | 18:ef02a9014491 | 418 | IN11=0; |
| mjhaugsdal | 18:ef02a9014491 | 419 | IN12=0; |
| mjhaugsdal | 18:ef02a9014491 | 420 | } |
| mjhaugsdal | 18:ef02a9014491 | 421 | |
| mjhaugsdal | 21:9f3ae352ee63 | 422 | //Methods to release single engines. |
| mjhaugsdal | 20:d90bd1718078 | 423 | void stopE1() |
| mjhaugsdal | 20:d90bd1718078 | 424 | { |
| mjhaugsdal | 20:d90bd1718078 | 425 | IN1=0; |
| mjhaugsdal | 20:d90bd1718078 | 426 | IN2=0; |
| mjhaugsdal | 20:d90bd1718078 | 427 | IN3=0; |
| mjhaugsdal | 20:d90bd1718078 | 428 | IN4=0; |
| mjhaugsdal | 20:d90bd1718078 | 429 | |
| mjhaugsdal | 20:d90bd1718078 | 430 | } |
| mjhaugsdal | 20:d90bd1718078 | 431 | void stopE2() |
| mjhaugsdal | 20:d90bd1718078 | 432 | { |
| mjhaugsdal | 20:d90bd1718078 | 433 | IN5=0; |
| mjhaugsdal | 20:d90bd1718078 | 434 | IN6=0; |
| mjhaugsdal | 20:d90bd1718078 | 435 | IN7=0; |
| mjhaugsdal | 20:d90bd1718078 | 436 | IN8=0; |
| mjhaugsdal | 20:d90bd1718078 | 437 | |
| mjhaugsdal | 20:d90bd1718078 | 438 | } |
| mjhaugsdal | 20:d90bd1718078 | 439 | void stopE3() |
| mjhaugsdal | 20:d90bd1718078 | 440 | { |
| mjhaugsdal | 20:d90bd1718078 | 441 | IN9=0; |
| mjhaugsdal | 20:d90bd1718078 | 442 | IN10=0; |
| mjhaugsdal | 20:d90bd1718078 | 443 | IN11=0; |
| mjhaugsdal | 20:d90bd1718078 | 444 | IN12=0; |
| mjhaugsdal | 20:d90bd1718078 | 445 | |
| mjhaugsdal | 20:d90bd1718078 | 446 | } |
| mjhaugsdal | 20:d90bd1718078 | 447 | |
| mjhaugsdal | 27:35c962e8e95b | 448 | void hold() |
| mjhaugsdal | 27:35c962e8e95b | 449 | { |
| mjhaugsdal | 27:35c962e8e95b | 450 | IN1=1; |
| mjhaugsdal | 27:35c962e8e95b | 451 | IN2=0; |
| mjhaugsdal | 27:35c962e8e95b | 452 | IN3=0; |
| mjhaugsdal | 27:35c962e8e95b | 453 | IN4=1; |
| mjhaugsdal | 27:35c962e8e95b | 454 | IN5=1; |
| mjhaugsdal | 27:35c962e8e95b | 455 | IN6=0; |
| mjhaugsdal | 27:35c962e8e95b | 456 | IN7=0; |
| mjhaugsdal | 27:35c962e8e95b | 457 | IN8=1; |
| mjhaugsdal | 27:35c962e8e95b | 458 | IN9=1; |
| mjhaugsdal | 27:35c962e8e95b | 459 | IN10=0; |
| mjhaugsdal | 27:35c962e8e95b | 460 | IN11=0; |
| mjhaugsdal | 27:35c962e8e95b | 461 | IN12=1; |
| mjhaugsdal | 27:35c962e8e95b | 462 | } |
| mjhaugsdal | 23:ad08a8eabc24 | 463 | |
| mjhaugsdal | 23:ad08a8eabc24 | 464 | |
| mjhaugsdal | 1:a3287196a9b3 | 465 | void input(void const *args) |
| mjhaugsdal | 1:a3287196a9b3 | 466 | { |
| mjhaugsdal | 1:a3287196a9b3 | 467 | while(true) |
| mjhaugsdal | 1:a3287196a9b3 | 468 | { |
| mjhaugsdal | 3:15be78948724 | 469 | |
| rlanghbv | 2:25bcc26f7a5b | 470 | if(pc.readable()) |
| mjhaugsdal | 27:35c962e8e95b | 471 | { |
| mjhaugsdal | 27:35c962e8e95b | 472 | m_cmd=pc.getc(); |
| mjhaugsdal | 27:35c962e8e95b | 473 | } |
| mjhaugsdal | 23:ad08a8eabc24 | 474 | Thread::wait(100); |
| mjhaugsdal | 5:fdc7a779d525 | 475 | //pc.printf("%d", steps); |
| benrammok | 35:64ed42e6f9a4 | 476 | //pc.printf("%c", m_cmd); |
| mjhaugsdal | 1:a3287196a9b3 | 477 | } |
| mjhaugsdal | 1:a3287196a9b3 | 478 | } |
| rlanghbv | 0:bd78e433db61 | 479 | |
| benrammok | 35:64ed42e6f9a4 | 480 | //Tråd for ultralyd |
| benrammok | 35:64ed42e6f9a4 | 481 | void ultraSound(void const *args) |
| benrammok | 35:64ed42e6f9a4 | 482 | { |
| benrammok | 35:64ed42e6f9a4 | 483 | while(true) |
| benrammok | 35:64ed42e6f9a4 | 484 | { |
| benrammok | 36:3011c70a5c81 | 485 | |
| benrammok | 36:3011c70a5c81 | 486 | sDistance = mu.getCurrentDistance(); |
| benrammok | 36:3011c70a5c81 | 487 | Thread::wait(100); |
| benrammok | 36:3011c70a5c81 | 488 | dDistance = mu.getCurrentDistance(); |
| benrammok | 35:64ed42e6f9a4 | 489 | |
| benrammok | 36:3011c70a5c81 | 490 | printf("Current Distance: %dmm\r\n", sDistance); |
| benrammok | 36:3011c70a5c81 | 491 | printf("Current Change in Distance: %dmm\r\n", abs(sDistance-dDistance)); |
| benrammok | 36:3011c70a5c81 | 492 | if(abs(sDistance-dDistance) > 30 && abs(sDistance-dDistance) < 1000){ |
| benrammok | 36:3011c70a5c81 | 493 | /* Control structure for different audio playback. |
| benrammok | 36:3011c70a5c81 | 494 | sndBoard.nextTrack(); |
| benrammok | 36:3011c70a5c81 | 495 | sndBoard.nextTrack(); |
| benrammok | 36:3011c70a5c81 | 496 | sndBoard.nextTrack(); |
| benrammok | 35:64ed42e6f9a4 | 497 | |
| benrammok | 36:3011c70a5c81 | 498 | if(sDistance >= 500){ |
| benrammok | 36:3011c70a5c81 | 499 | sndBoard.resetTrack(); |
| benrammok | 36:3011c70a5c81 | 500 | Thread::wait(200); |
| benrammok | 36:3011c70a5c81 | 501 | sndBoard.nextTrack(); |
| benrammok | 36:3011c70a5c81 | 502 | Thread::wait(200); |
| benrammok | 36:3011c70a5c81 | 503 | sndBoard.nextTrack(); |
| benrammok | 35:64ed42e6f9a4 | 504 | } |
| benrammok | 36:3011c70a5c81 | 505 | if(sDistance < 500){ |
| benrammok | 36:3011c70a5c81 | 506 | sndBoard.resetTrack(); |
| benrammok | 36:3011c70a5c81 | 507 | Thread::wait(200); |
| benrammok | 36:3011c70a5c81 | 508 | sndBoard.nextTrack(); |
| benrammok | 36:3011c70a5c81 | 509 | Thread::wait(200); |
| benrammok | 36:3011c70a5c81 | 510 | sndBoard.nextTrack(); |
| benrammok | 36:3011c70a5c81 | 511 | Thread::wait(200); |
| benrammok | 36:3011c70a5c81 | 512 | sndBoard.nextTrack(); |
| benrammok | 36:3011c70a5c81 | 513 | isPlaying = true; |
| benrammok | 36:3011c70a5c81 | 514 | } |
| benrammok | 36:3011c70a5c81 | 515 | */ |
| benrammok | 36:3011c70a5c81 | 516 | } |
| benrammok | 36:3011c70a5c81 | 517 | |
| benrammok | 35:64ed42e6f9a4 | 518 | Thread::wait(200); |
| benrammok | 35:64ed42e6f9a4 | 519 | } |
| benrammok | 35:64ed42e6f9a4 | 520 | } |
| benrammok | 35:64ed42e6f9a4 | 521 | |
| benrammok | 35:64ed42e6f9a4 | 522 | |
| benrammok | 35:64ed42e6f9a4 | 523 | |
| mjhaugsdal | 27:35c962e8e95b | 524 | //MOTOR 1 LEFT SIDE BUTTON |
| mjhaugsdal | 25:321b970eb3ff | 525 | void pb1_hit_callback (void) |
| mjhaugsdal | 23:ad08a8eabc24 | 526 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 527 | |
| mjhaugsdal | 25:321b970eb3ff | 528 | e1 = false; |
| mjhaugsdal | 25:321b970eb3ff | 529 | |
| mjhaugsdal | 29:00f839aad30e | 530 | speed = 1000; |
| mjhaugsdal | 30:fac5b5624fe3 | 531 | |
| mjhaugsdal | 30:fac5b5624fe3 | 532 | wait(2); |
| mjhaugsdal | 30:fac5b5624fe3 | 533 | int stepsToCenter = i1/2; |
| mjhaugsdal | 27:35c962e8e95b | 534 | |
| mjhaugsdal | 27:35c962e8e95b | 535 | while (stepsToCenter >= 0) |
| mjhaugsdal | 25:321b970eb3ff | 536 | { |
| mjhaugsdal | 27:35c962e8e95b | 537 | stepEngine1Right(); |
| mjhaugsdal | 27:35c962e8e95b | 538 | stepsToCenter --; |
| mjhaugsdal | 25:321b970eb3ff | 539 | } |
| mjhaugsdal | 27:35c962e8e95b | 540 | steps1 = 0; |
| mjhaugsdal | 27:35c962e8e95b | 541 | //break; |
| mjhaugsdal | 27:35c962e8e95b | 542 | //end if |
| mjhaugsdal | 27:35c962e8e95b | 543 | m_cmd = 'x'; |
| mjhaugsdal | 30:fac5b5624fe3 | 544 | |
| mjhaugsdal | 25:321b970eb3ff | 545 | } |
| mjhaugsdal | 25:321b970eb3ff | 546 | |
| mjhaugsdal | 27:35c962e8e95b | 547 | //MOTOR 1 RIGHT SIDE BUTTON |
| mjhaugsdal | 25:321b970eb3ff | 548 | void pb2_hit_callback (void) |
| mjhaugsdal | 25:321b970eb3ff | 549 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 550 | |
| mjhaugsdal | 25:321b970eb3ff | 551 | e1 = false; |
| mjhaugsdal | 25:321b970eb3ff | 552 | //m_cmd = 'x'; |
| mjhaugsdal | 23:ad08a8eabc24 | 553 | |
| mjhaugsdal | 29:00f839aad30e | 554 | speed = 1000; |
| mjhaugsdal | 30:fac5b5624fe3 | 555 | |
| mjhaugsdal | 30:fac5b5624fe3 | 556 | wait(2); |
| mjhaugsdal | 30:fac5b5624fe3 | 557 | int stepsToCenter = i1/2; |
| mjhaugsdal | 27:35c962e8e95b | 558 | |
| mjhaugsdal | 27:35c962e8e95b | 559 | while (stepsToCenter >= 0) |
| mjhaugsdal | 25:321b970eb3ff | 560 | { |
| mjhaugsdal | 27:35c962e8e95b | 561 | stepEngine1Left(); |
| mjhaugsdal | 27:35c962e8e95b | 562 | stepsToCenter --; |
| mjhaugsdal | 25:321b970eb3ff | 563 | } |
| mjhaugsdal | 27:35c962e8e95b | 564 | steps1 = 0; |
| mjhaugsdal | 27:35c962e8e95b | 565 | m_cmd = 'x'; |
| mjhaugsdal | 25:321b970eb3ff | 566 | } |
| mjhaugsdal | 30:fac5b5624fe3 | 567 | |
| mjhaugsdal | 30:fac5b5624fe3 | 568 | |
| mjhaugsdal | 30:fac5b5624fe3 | 569 | |
| mjhaugsdal | 25:321b970eb3ff | 570 | //MOTOR 2 LEFT SIDE BUTTON |
| mjhaugsdal | 25:321b970eb3ff | 571 | void pb3_hit_callback (void) |
| mjhaugsdal | 25:321b970eb3ff | 572 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 573 | |
| mjhaugsdal | 25:321b970eb3ff | 574 | { |
| mjhaugsdal | 25:321b970eb3ff | 575 | e2 = false; |
| mjhaugsdal | 25:321b970eb3ff | 576 | //m_cmd = 'x'; |
| mjhaugsdal | 29:00f839aad30e | 577 | speed = 1000; |
| mjhaugsdal | 30:fac5b5624fe3 | 578 | wait(1); |
| mjhaugsdal | 30:fac5b5624fe3 | 579 | int stepsToCenter = i2/2; |
| mjhaugsdal | 25:321b970eb3ff | 580 | |
| mjhaugsdal | 25:321b970eb3ff | 581 | while (stepsToCenter >= 0) |
| mjhaugsdal | 25:321b970eb3ff | 582 | { |
| azadmehr | 32:5163db06f6cc | 583 | stepEngine2Left(); |
| mjhaugsdal | 25:321b970eb3ff | 584 | stepsToCenter --; |
| mjhaugsdal | 25:321b970eb3ff | 585 | } |
| mjhaugsdal | 27:35c962e8e95b | 586 | steps2 = 0; |
| mjhaugsdal | 30:fac5b5624fe3 | 587 | //m_cmd = 'x'; |
| mjhaugsdal | 30:fac5b5624fe3 | 588 | stopE2(); |
| mjhaugsdal | 25:321b970eb3ff | 589 | } |
| mjhaugsdal | 23:ad08a8eabc24 | 590 | |
| mjhaugsdal | 23:ad08a8eabc24 | 591 | } |
| mjhaugsdal | 23:ad08a8eabc24 | 592 | |
| mjhaugsdal | 25:321b970eb3ff | 593 | //MOTOR 2 RIGHT SIDE BUTTON |
| mjhaugsdal | 25:321b970eb3ff | 594 | void pb4_hit_callback (void) |
| mjhaugsdal | 25:321b970eb3ff | 595 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 596 | |
| mjhaugsdal | 25:321b970eb3ff | 597 | { |
| mjhaugsdal | 25:321b970eb3ff | 598 | e2 = false; |
| mjhaugsdal | 25:321b970eb3ff | 599 | //m_cmd = 'x'; |
| mjhaugsdal | 29:00f839aad30e | 600 | speed = 1000; |
| mjhaugsdal | 30:fac5b5624fe3 | 601 | wait(1); |
| mjhaugsdal | 30:fac5b5624fe3 | 602 | int stepsToCenter = i2/2; |
| mjhaugsdal | 25:321b970eb3ff | 603 | |
| mjhaugsdal | 25:321b970eb3ff | 604 | while (stepsToCenter >= 0) |
| mjhaugsdal | 25:321b970eb3ff | 605 | { |
| azadmehr | 32:5163db06f6cc | 606 | stepEngine2Right(); |
| mjhaugsdal | 25:321b970eb3ff | 607 | stepsToCenter --; |
| mjhaugsdal | 25:321b970eb3ff | 608 | } |
| mjhaugsdal | 27:35c962e8e95b | 609 | steps2 = 0; |
| mjhaugsdal | 30:fac5b5624fe3 | 610 | //m_cmd = 'x'; |
| mjhaugsdal | 30:fac5b5624fe3 | 611 | stopE2(); |
| mjhaugsdal | 25:321b970eb3ff | 612 | } |
| mjhaugsdal | 25:321b970eb3ff | 613 | |
| mjhaugsdal | 25:321b970eb3ff | 614 | |
| mjhaugsdal | 25:321b970eb3ff | 615 | } |
| mjhaugsdal | 25:321b970eb3ff | 616 | |
| mjhaugsdal | 30:fac5b5624fe3 | 617 | |
| mjhaugsdal | 25:321b970eb3ff | 618 | //MOTOR 3 LEFT SIDE BUTTON |
| mjhaugsdal | 25:321b970eb3ff | 619 | void pb5_hit_callback (void) |
| mjhaugsdal | 25:321b970eb3ff | 620 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 621 | |
| mjhaugsdal | 25:321b970eb3ff | 622 | e3 = false; |
| mjhaugsdal | 25:321b970eb3ff | 623 | //m_cmd = 'x'; |
| mjhaugsdal | 29:00f839aad30e | 624 | speed = 1000; |
| mjhaugsdal | 30:fac5b5624fe3 | 625 | wait(1); |
| mjhaugsdal | 30:fac5b5624fe3 | 626 | int stepsToCenter = i3/2; |
| mjhaugsdal | 25:321b970eb3ff | 627 | |
| mjhaugsdal | 25:321b970eb3ff | 628 | while (stepsToCenter >= 0) |
| mjhaugsdal | 25:321b970eb3ff | 629 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 630 | if(stepsToCenter < i3/10) |
| mjhaugsdal | 30:fac5b5624fe3 | 631 | speed = 1500; |
| mjhaugsdal | 30:fac5b5624fe3 | 632 | |
| mjhaugsdal | 25:321b970eb3ff | 633 | stepEngine3Left(); |
| mjhaugsdal | 25:321b970eb3ff | 634 | stepsToCenter --; |
| mjhaugsdal | 25:321b970eb3ff | 635 | } |
| mjhaugsdal | 27:35c962e8e95b | 636 | steps3 = 0; |
| mjhaugsdal | 27:35c962e8e95b | 637 | m_cmd = 'x'; |
| mjhaugsdal | 25:321b970eb3ff | 638 | |
| mjhaugsdal | 25:321b970eb3ff | 639 | } |
| mjhaugsdal | 25:321b970eb3ff | 640 | //MOTOR 3 RIGHT SIDE BUTTON |
| mjhaugsdal | 25:321b970eb3ff | 641 | void pb6_hit_callback (void) |
| mjhaugsdal | 25:321b970eb3ff | 642 | { |
| mjhaugsdal | 27:35c962e8e95b | 643 | |
| mjhaugsdal | 30:fac5b5624fe3 | 644 | |
| mjhaugsdal | 25:321b970eb3ff | 645 | e3 = false; |
| mjhaugsdal | 25:321b970eb3ff | 646 | //m_cmd = 'x'; |
| mjhaugsdal | 29:00f839aad30e | 647 | speed = 1000; |
| mjhaugsdal | 30:fac5b5624fe3 | 648 | wait(1); |
| mjhaugsdal | 30:fac5b5624fe3 | 649 | int stepsToCenter = i3/2; |
| mjhaugsdal | 25:321b970eb3ff | 650 | |
| mjhaugsdal | 25:321b970eb3ff | 651 | while (stepsToCenter >= 0) |
| mjhaugsdal | 25:321b970eb3ff | 652 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 653 | if(stepsToCenter < i3/10) |
| mjhaugsdal | 30:fac5b5624fe3 | 654 | speed = 1500; |
| mjhaugsdal | 30:fac5b5624fe3 | 655 | |
| mjhaugsdal | 25:321b970eb3ff | 656 | stepEngine3Right(); |
| mjhaugsdal | 25:321b970eb3ff | 657 | stepsToCenter --; |
| mjhaugsdal | 25:321b970eb3ff | 658 | } |
| mjhaugsdal | 27:35c962e8e95b | 659 | //reset steps and stop engine |
| mjhaugsdal | 27:35c962e8e95b | 660 | steps3 = 0; |
| mjhaugsdal | 27:35c962e8e95b | 661 | m_cmd = 'x'; |
| mjhaugsdal | 30:fac5b5624fe3 | 662 | |
| mjhaugsdal | 25:321b970eb3ff | 663 | } |
| azadmehr | 32:5163db06f6cc | 664 | void dist(int distance) |
| azadmehr | 32:5163db06f6cc | 665 | { |
| azadmehr | 32:5163db06f6cc | 666 | //put code here to happen when the distance is changed |
| benrammok | 35:64ed42e6f9a4 | 667 | //stepEngine2Right(); |
| azadmehr | 32:5163db06f6cc | 668 | } |
| azadmehr | 32:5163db06f6cc | 669 | |
| mjhaugsdal | 23:ad08a8eabc24 | 670 | |
| rlanghbv | 0:bd78e433db61 | 671 | int main() |
| rlanghbv | 0:bd78e433db61 | 672 | { |
| azadmehr | 32:5163db06f6cc | 673 | |
| benrammok | 35:64ed42e6f9a4 | 674 | |
| azadmehr | 32:5163db06f6cc | 675 | |
| benrammok | 35:64ed42e6f9a4 | 676 | mu.startUpdates();//start mesuring the distance |
| azadmehr | 32:5163db06f6cc | 677 | |
| azadmehr | 32:5163db06f6cc | 678 | |
| azadmehr | 32:5163db06f6cc | 679 | |
| mjhaugsdal | 25:321b970eb3ff | 680 | pb1.mode(PullUp); |
| mjhaugsdal | 25:321b970eb3ff | 681 | pb2.mode(PullUp); |
| mjhaugsdal | 25:321b970eb3ff | 682 | pb3.mode(PullUp); |
| mjhaugsdal | 25:321b970eb3ff | 683 | pb4.mode(PullUp); |
| mjhaugsdal | 25:321b970eb3ff | 684 | pb5.mode(PullUp); |
| mjhaugsdal | 25:321b970eb3ff | 685 | pb6.mode(PullUp); |
| mjhaugsdal | 30:fac5b5624fe3 | 686 | |
| mjhaugsdal | 25:321b970eb3ff | 687 | //Set up buttons 1 and 2 |
| mjhaugsdal | 25:321b970eb3ff | 688 | pb1.attach_deasserted(&pb1_hit_callback); |
| mjhaugsdal | 25:321b970eb3ff | 689 | pb1.setSampleFrequency(); |
| mjhaugsdal | 25:321b970eb3ff | 690 | pb2.attach_deasserted(&pb2_hit_callback); |
| mjhaugsdal | 30:fac5b5624fe3 | 691 | pb2.setSampleFrequency(); |
| mjhaugsdal | 30:fac5b5624fe3 | 692 | |
| mjhaugsdal | 23:ad08a8eabc24 | 693 | |
| mjhaugsdal | 1:a3287196a9b3 | 694 | //Thread 1 has constant feed from usb |
| rlanghbv | 2:25bcc26f7a5b | 695 | Thread t1(input); |
| benrammok | 35:64ed42e6f9a4 | 696 | |
| benrammok | 35:64ed42e6f9a4 | 697 | //Thread 2 - Ultrasound reading |
| benrammok | 35:64ed42e6f9a4 | 698 | Thread t2(ultraSound); |
| benrammok | 35:64ed42e6f9a4 | 699 | |
| mjhaugsdal | 22:f6e328f7bd28 | 700 | //static char global_direction; |
| mjhaugsdal | 3:15be78948724 | 701 | printf("Started"); |
| azadmehr | 31:358d4c032d76 | 702 | /* |
| azadmehr | 31:358d4c032d76 | 703 | //Prosedyre for å midtstille laveste ledd |
| mjhaugsdal | 30:fac5b5624fe3 | 704 | |
| mjhaugsdal | 24:24c91a6ae6ba | 705 | while (true) |
| mjhaugsdal | 24:24c91a6ae6ba | 706 | { |
| mjhaugsdal | 28:dac262b7ab32 | 707 | |
| mjhaugsdal | 30:fac5b5624fe3 | 708 | |
| mjhaugsdal | 24:24c91a6ae6ba | 709 | stepEngine1Left(); |
| mjhaugsdal | 24:24c91a6ae6ba | 710 | if (e1 == false) |
| mjhaugsdal | 24:24c91a6ae6ba | 711 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 712 | Thread::wait(2000); |
| mjhaugsdal | 29:00f839aad30e | 713 | e1 = true; |
| mjhaugsdal | 29:00f839aad30e | 714 | while(true) |
| mjhaugsdal | 29:00f839aad30e | 715 | { |
| mjhaugsdal | 29:00f839aad30e | 716 | |
| mjhaugsdal | 29:00f839aad30e | 717 | stepEngine1Right(); |
| mjhaugsdal | 30:fac5b5624fe3 | 718 | i1++; |
| mjhaugsdal | 29:00f839aad30e | 719 | if(e1 == false) |
| mjhaugsdal | 29:00f839aad30e | 720 | break; |
| mjhaugsdal | 29:00f839aad30e | 721 | |
| mjhaugsdal | 29:00f839aad30e | 722 | } |
| mjhaugsdal | 29:00f839aad30e | 723 | |
| mjhaugsdal | 24:24c91a6ae6ba | 724 | Thread::wait(1000); |
| mjhaugsdal | 25:321b970eb3ff | 725 | break; |
| mjhaugsdal | 24:24c91a6ae6ba | 726 | }//end if |
| mjhaugsdal | 25:321b970eb3ff | 727 | |
| mjhaugsdal | 30:fac5b5624fe3 | 728 | }//end while |
| mjhaugsdal | 30:fac5b5624fe3 | 729 | |
| azadmehr | 31:358d4c032d76 | 730 | */ |
| azadmehr | 31:358d4c032d76 | 731 | |
| mjhaugsdal | 25:321b970eb3ff | 732 | //Set up buttons 3 and 4 |
| mjhaugsdal | 25:321b970eb3ff | 733 | pb3.attach_deasserted(&pb3_hit_callback); |
| mjhaugsdal | 25:321b970eb3ff | 734 | pb3.setSampleFrequency(); |
| mjhaugsdal | 25:321b970eb3ff | 735 | pb4.attach_deasserted(&pb4_hit_callback); |
| mjhaugsdal | 25:321b970eb3ff | 736 | pb4.setSampleFrequency(); |
| azadmehr | 31:358d4c032d76 | 737 | |
| azadmehr | 31:358d4c032d76 | 738 | //midstille midterste ledd |
| mjhaugsdal | 24:24c91a6ae6ba | 739 | while (true) |
| mjhaugsdal | 24:24c91a6ae6ba | 740 | { |
| mjhaugsdal | 25:321b970eb3ff | 741 | |
| mjhaugsdal | 29:00f839aad30e | 742 | speed = 1000; |
| mjhaugsdal | 29:00f839aad30e | 743 | /* while(bootStep2 >= 0) |
| mjhaugsdal | 25:321b970eb3ff | 744 | { |
| mjhaugsdal | 25:321b970eb3ff | 745 | bootStep2--; |
| mjhaugsdal | 25:321b970eb3ff | 746 | stepEngine2Right(); |
| mjhaugsdal | 25:321b970eb3ff | 747 | |
| mjhaugsdal | 29:00f839aad30e | 748 | } */ |
| mjhaugsdal | 25:321b970eb3ff | 749 | |
| mjhaugsdal | 25:321b970eb3ff | 750 | stepEngine2Left(); |
| mjhaugsdal | 24:24c91a6ae6ba | 751 | if (e2 == false) |
| mjhaugsdal | 24:24c91a6ae6ba | 752 | { |
| mjhaugsdal | 29:00f839aad30e | 753 | e2 = true; |
| mjhaugsdal | 29:00f839aad30e | 754 | while(true) |
| mjhaugsdal | 29:00f839aad30e | 755 | { |
| mjhaugsdal | 29:00f839aad30e | 756 | |
| mjhaugsdal | 29:00f839aad30e | 757 | stepEngine2Right(); |
| mjhaugsdal | 30:fac5b5624fe3 | 758 | i2++; |
| mjhaugsdal | 29:00f839aad30e | 759 | if(e2 == false) |
| mjhaugsdal | 29:00f839aad30e | 760 | break; |
| mjhaugsdal | 29:00f839aad30e | 761 | |
| mjhaugsdal | 29:00f839aad30e | 762 | } |
| mjhaugsdal | 27:35c962e8e95b | 763 | |
| mjhaugsdal | 25:321b970eb3ff | 764 | Thread::wait(1000); |
| azadmehr | 32:5163db06f6cc | 765 | stopAll(); |
| mjhaugsdal | 24:24c91a6ae6ba | 766 | break; |
| mjhaugsdal | 24:24c91a6ae6ba | 767 | }//end if |
| mjhaugsdal | 24:24c91a6ae6ba | 768 | } |
| mjhaugsdal | 25:321b970eb3ff | 769 | |
| mjhaugsdal | 30:fac5b5624fe3 | 770 | |
| azadmehr | 32:5163db06f6cc | 771 | //checking buttons 5 and 6 |
| mjhaugsdal | 25:321b970eb3ff | 772 | pb5.attach_deasserted(&pb5_hit_callback); |
| mjhaugsdal | 25:321b970eb3ff | 773 | pb5.setSampleFrequency(); |
| mjhaugsdal | 25:321b970eb3ff | 774 | pb6.attach_deasserted(&pb6_hit_callback); |
| mjhaugsdal | 25:321b970eb3ff | 775 | pb6.setSampleFrequency(); |
| azadmehr | 31:358d4c032d76 | 776 | //Midstille toppledd |
| azadmehr | 31:358d4c032d76 | 777 | |
| mjhaugsdal | 24:24c91a6ae6ba | 778 | while (true) |
| mjhaugsdal | 24:24c91a6ae6ba | 779 | { |
| mjhaugsdal | 25:321b970eb3ff | 780 | stepEngine3Left(); |
| mjhaugsdal | 24:24c91a6ae6ba | 781 | if (e3 == false) |
| mjhaugsdal | 24:24c91a6ae6ba | 782 | { |
| mjhaugsdal | 29:00f839aad30e | 783 | e3 = true; |
| mjhaugsdal | 29:00f839aad30e | 784 | while(true) |
| mjhaugsdal | 29:00f839aad30e | 785 | { |
| mjhaugsdal | 29:00f839aad30e | 786 | |
| mjhaugsdal | 29:00f839aad30e | 787 | stepEngine3Right(); |
| mjhaugsdal | 30:fac5b5624fe3 | 788 | i3++; |
| mjhaugsdal | 29:00f839aad30e | 789 | if(e3 == false) |
| mjhaugsdal | 29:00f839aad30e | 790 | break; |
| mjhaugsdal | 29:00f839aad30e | 791 | |
| mjhaugsdal | 29:00f839aad30e | 792 | } |
| mjhaugsdal | 27:35c962e8e95b | 793 | //pc.printf("HALLO"); |
| mjhaugsdal | 30:fac5b5624fe3 | 794 | //m_cmd = 'z'; |
| mjhaugsdal | 24:24c91a6ae6ba | 795 | Thread::wait(1000); |
| azadmehr | 32:5163db06f6cc | 796 | stopAll(); |
| mjhaugsdal | 24:24c91a6ae6ba | 797 | break; |
| mjhaugsdal | 24:24c91a6ae6ba | 798 | }//end if |
| mjhaugsdal | 24:24c91a6ae6ba | 799 | } |
| azadmehr | 32:5163db06f6cc | 800 | stopAll(); |
| mjhaugsdal | 28:dac262b7ab32 | 801 | //normal runtime |
| mjhaugsdal | 1:a3287196a9b3 | 802 | while (true) |
| mjhaugsdal | 22:f6e328f7bd28 | 803 | { |
| mjhaugsdal | 28:dac262b7ab32 | 804 | |
| mjhaugsdal | 28:dac262b7ab32 | 805 | //user button to turn off engines |
| mjhaugsdal | 30:fac5b5624fe3 | 806 | if(SW==0) |
| mjhaugsdal | 28:dac262b7ab32 | 807 | { |
| azadmehr | 32:5163db06f6cc | 808 | while(true) |
| azadmehr | 32:5163db06f6cc | 809 | { |
| azadmehr | 32:5163db06f6cc | 810 | e3=true; |
| azadmehr | 32:5163db06f6cc | 811 | //stopAll(); |
| azadmehr | 32:5163db06f6cc | 812 | //Do something else here |
| azadmehr | 32:5163db06f6cc | 813 | //mu.checkDistance(); //call checkDistance() as much as possible, as this is where |
| azadmehr | 32:5163db06f6cc | 814 | //the class checks if dist needs to be called. |
| mjhaugsdal | 28:dac262b7ab32 | 815 | //put engines to sleep |
| azadmehr | 32:5163db06f6cc | 816 | stepEngine3Right(); |
| azadmehr | 32:5163db06f6cc | 817 | if(e3==false) |
| azadmehr | 32:5163db06f6cc | 818 | break; |
| azadmehr | 32:5163db06f6cc | 819 | } |
| benrammok | 35:64ed42e6f9a4 | 820 | |
| azadmehr | 32:5163db06f6cc | 821 | stopAll(); |
| azadmehr | 32:5163db06f6cc | 822 | //stepEngine2Right(); |
| azadmehr | 32:5163db06f6cc | 823 | // speed=1000; |
| azadmehr | 32:5163db06f6cc | 824 | // m_cmd ='z'; |
| mjhaugsdal | 28:dac262b7ab32 | 825 | |
| mjhaugsdal | 28:dac262b7ab32 | 826 | |
| mjhaugsdal | 28:dac262b7ab32 | 827 | } |
| azadmehr | 32:5163db06f6cc | 828 | /* //set the speed. Higher = slower |
| mjhaugsdal | 29:00f839aad30e | 829 | speed = 1000; |
| mjhaugsdal | 27:35c962e8e95b | 830 | //speed = 1500; |
| mjhaugsdal | 24:24c91a6ae6ba | 831 | |
| mjhaugsdal | 27:35c962e8e95b | 832 | switch (m_cmd) |
| mjhaugsdal | 27:35c962e8e95b | 833 | { |
| mjhaugsdal | 27:35c962e8e95b | 834 | case '7': |
| mjhaugsdal | 27:35c962e8e95b | 835 | cmd = '7'; |
| mjhaugsdal | 27:35c962e8e95b | 836 | break; |
| mjhaugsdal | 27:35c962e8e95b | 837 | |
| mjhaugsdal | 27:35c962e8e95b | 838 | case '9': |
| mjhaugsdal | 27:35c962e8e95b | 839 | cmd = '9'; |
| mjhaugsdal | 27:35c962e8e95b | 840 | break; |
| mjhaugsdal | 27:35c962e8e95b | 841 | |
| mjhaugsdal | 27:35c962e8e95b | 842 | case '4': |
| mjhaugsdal | 27:35c962e8e95b | 843 | cmd= '4'; |
| mjhaugsdal | 27:35c962e8e95b | 844 | break; |
| mjhaugsdal | 27:35c962e8e95b | 845 | |
| mjhaugsdal | 27:35c962e8e95b | 846 | case '6': |
| mjhaugsdal | 27:35c962e8e95b | 847 | cmd = '6'; |
| mjhaugsdal | 27:35c962e8e95b | 848 | break; |
| mjhaugsdal | 27:35c962e8e95b | 849 | |
| mjhaugsdal | 27:35c962e8e95b | 850 | case '1': |
| mjhaugsdal | 27:35c962e8e95b | 851 | cmd = '1'; |
| mjhaugsdal | 27:35c962e8e95b | 852 | break; |
| mjhaugsdal | 27:35c962e8e95b | 853 | |
| mjhaugsdal | 27:35c962e8e95b | 854 | case '3': |
| mjhaugsdal | 27:35c962e8e95b | 855 | cmd = '3'; |
| mjhaugsdal | 27:35c962e8e95b | 856 | break; |
| mjhaugsdal | 27:35c962e8e95b | 857 | |
| mjhaugsdal | 27:35c962e8e95b | 858 | case 'x': |
| mjhaugsdal | 27:35c962e8e95b | 859 | cmd = 'x'; |
| mjhaugsdal | 27:35c962e8e95b | 860 | break; |
| mjhaugsdal | 27:35c962e8e95b | 861 | |
| mjhaugsdal | 27:35c962e8e95b | 862 | //release motors |
| mjhaugsdal | 27:35c962e8e95b | 863 | case 'z': |
| mjhaugsdal | 27:35c962e8e95b | 864 | |
| mjhaugsdal | 27:35c962e8e95b | 865 | //step all motors to the middle |
| mjhaugsdal | 27:35c962e8e95b | 866 | |
| mjhaugsdal | 27:35c962e8e95b | 867 | while(steps1 > 0) |
| mjhaugsdal | 27:35c962e8e95b | 868 | { |
| mjhaugsdal | 27:35c962e8e95b | 869 | stepEngine1Left(); |
| mjhaugsdal | 27:35c962e8e95b | 870 | steps1--; |
| mjhaugsdal | 27:35c962e8e95b | 871 | } |
| mjhaugsdal | 27:35c962e8e95b | 872 | while(steps1 < 0) |
| mjhaugsdal | 27:35c962e8e95b | 873 | { |
| mjhaugsdal | 27:35c962e8e95b | 874 | stepEngine1Right(); |
| mjhaugsdal | 27:35c962e8e95b | 875 | steps1++; |
| mjhaugsdal | 27:35c962e8e95b | 876 | } |
| mjhaugsdal | 27:35c962e8e95b | 877 | |
| mjhaugsdal | 27:35c962e8e95b | 878 | while(steps2 > 0) |
| mjhaugsdal | 27:35c962e8e95b | 879 | { |
| mjhaugsdal | 27:35c962e8e95b | 880 | stepEngine2Left(); |
| mjhaugsdal | 27:35c962e8e95b | 881 | steps2--; |
| mjhaugsdal | 27:35c962e8e95b | 882 | } |
| mjhaugsdal | 27:35c962e8e95b | 883 | while(steps2 < 0) |
| mjhaugsdal | 27:35c962e8e95b | 884 | { |
| mjhaugsdal | 27:35c962e8e95b | 885 | stepEngine2Right(); |
| mjhaugsdal | 27:35c962e8e95b | 886 | steps2++; |
| mjhaugsdal | 27:35c962e8e95b | 887 | } |
| mjhaugsdal | 27:35c962e8e95b | 888 | |
| mjhaugsdal | 27:35c962e8e95b | 889 | while(steps3 > 0) |
| mjhaugsdal | 27:35c962e8e95b | 890 | { |
| mjhaugsdal | 27:35c962e8e95b | 891 | stepEngine3Left(); |
| mjhaugsdal | 27:35c962e8e95b | 892 | steps3--; |
| mjhaugsdal | 27:35c962e8e95b | 893 | } |
| mjhaugsdal | 27:35c962e8e95b | 894 | while(steps3 < 0) |
| mjhaugsdal | 27:35c962e8e95b | 895 | { |
| mjhaugsdal | 27:35c962e8e95b | 896 | stepEngine3Right(); |
| mjhaugsdal | 27:35c962e8e95b | 897 | steps3++; |
| mjhaugsdal | 27:35c962e8e95b | 898 | } |
| mjhaugsdal | 27:35c962e8e95b | 899 | |
| mjhaugsdal | 27:35c962e8e95b | 900 | cmd = 'z'; |
| mjhaugsdal | 27:35c962e8e95b | 901 | |
| mjhaugsdal | 27:35c962e8e95b | 902 | |
| mjhaugsdal | 27:35c962e8e95b | 903 | break; |
| mjhaugsdal | 27:35c962e8e95b | 904 | |
| mjhaugsdal | 27:35c962e8e95b | 905 | } |
| mjhaugsdal | 27:35c962e8e95b | 906 | |
| mjhaugsdal | 21:9f3ae352ee63 | 907 | //Release all motors / Set all pins to 0 |
| mjhaugsdal | 27:35c962e8e95b | 908 | if (cmd == 'z') |
| mjhaugsdal | 18:ef02a9014491 | 909 | { |
| mjhaugsdal | 18:ef02a9014491 | 910 | stopAll(); |
| mjhaugsdal | 19:79600d3509d7 | 911 | } |
| mjhaugsdal | 21:9f3ae352ee63 | 912 | //Make all engines halt and hold still. |
| mjhaugsdal | 27:35c962e8e95b | 913 | if (cmd == 'x') |
| mjhaugsdal | 19:79600d3509d7 | 914 | { |
| mjhaugsdal | 27:35c962e8e95b | 915 | hold(); |
| mjhaugsdal | 28:dac262b7ab32 | 916 | Thread::wait(1000); |
| mjhaugsdal | 28:dac262b7ab32 | 917 | m_cmd = 'z'; |
| mjhaugsdal | 27:35c962e8e95b | 918 | |
| mjhaugsdal | 18:ef02a9014491 | 919 | } |
| mjhaugsdal | 25:321b970eb3ff | 920 | |
| mjhaugsdal | 25:321b970eb3ff | 921 | //Rest mode. Make the engines ignore pushbuttons. |
| azadmehr | 32:5163db06f6cc | 922 | |
| mjhaugsdal | 25:321b970eb3ff | 923 | if(m_cmd == 'r') |
| mjhaugsdal | 25:321b970eb3ff | 924 | { |
| mjhaugsdal | 25:321b970eb3ff | 925 | rest = true; |
| mjhaugsdal | 25:321b970eb3ff | 926 | //Step motors to rest position |
| mjhaugsdal | 25:321b970eb3ff | 927 | |
| mjhaugsdal | 25:321b970eb3ff | 928 | } |
| mjhaugsdal | 25:321b970eb3ff | 929 | */ |
| mjhaugsdal | 19:79600d3509d7 | 930 | |
| mjhaugsdal | 30:fac5b5624fe3 | 931 | /* |
| mjhaugsdal | 21:9f3ae352ee63 | 932 | //Controlling each motor seperately. |
| mjhaugsdal | 25:321b970eb3ff | 933 | //ENGINE 3 |
| mjhaugsdal | 27:35c962e8e95b | 934 | if(cmd == '7' && steps3 > -200) |
| mjhaugsdal | 19:79600d3509d7 | 935 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 936 | |
| mjhaugsdal | 22:f6e328f7bd28 | 937 | |
| mjhaugsdal | 24:24c91a6ae6ba | 938 | steps3 --; |
| mjhaugsdal | 24:24c91a6ae6ba | 939 | stepEngine3Left(); |
| mjhaugsdal | 27:35c962e8e95b | 940 | |
| mjhaugsdal | 24:24c91a6ae6ba | 941 | } |
| mjhaugsdal | 27:35c962e8e95b | 942 | else if (cmd == '9' && steps3 < 280) |
| mjhaugsdal | 24:24c91a6ae6ba | 943 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 944 | |
| mjhaugsdal | 23:ad08a8eabc24 | 945 | stepEngine3Right(); |
| mjhaugsdal | 24:24c91a6ae6ba | 946 | steps3 ++; |
| mjhaugsdal | 19:79600d3509d7 | 947 | } |
| mjhaugsdal | 19:79600d3509d7 | 948 | //ENGINE 2 |
| mjhaugsdal | 27:35c962e8e95b | 949 | else if (cmd == '4' && steps2 > -242) |
| mjhaugsdal | 19:79600d3509d7 | 950 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 951 | |
| mjhaugsdal | 24:24c91a6ae6ba | 952 | steps2 --; |
| mjhaugsdal | 19:79600d3509d7 | 953 | stepEngine2Left(); |
| mjhaugsdal | 19:79600d3509d7 | 954 | } |
| mjhaugsdal | 27:35c962e8e95b | 955 | else if (cmd == '6' && steps2 < 242) |
| mjhaugsdal | 19:79600d3509d7 | 956 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 957 | |
| mjhaugsdal | 24:24c91a6ae6ba | 958 | steps2 ++; |
| mjhaugsdal | 19:79600d3509d7 | 959 | stepEngine2Right(); |
| mjhaugsdal | 19:79600d3509d7 | 960 | } |
| mjhaugsdal | 19:79600d3509d7 | 961 | //ENGINE 3 |
| mjhaugsdal | 27:35c962e8e95b | 962 | else if (cmd == '1' && steps1 > -242) |
| mjhaugsdal | 19:79600d3509d7 | 963 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 964 | |
| mjhaugsdal | 24:24c91a6ae6ba | 965 | steps1--; |
| mjhaugsdal | 23:ad08a8eabc24 | 966 | stepEngine1Left(); |
| mjhaugsdal | 19:79600d3509d7 | 967 | } |
| mjhaugsdal | 27:35c962e8e95b | 968 | else if (cmd == '3' && steps1 < 242) |
| mjhaugsdal | 19:79600d3509d7 | 969 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 970 | |
| mjhaugsdal | 30:fac5b5624fe3 | 971 | steps1++; |
| mjhaugsdal | 30:fac5b5624fe3 | 972 | stepEngine1Right(); |
| mjhaugsdal | 30:fac5b5624fe3 | 973 | } |
| azadmehr | 32:5163db06f6cc | 974 | |
| mjhaugsdal | 30:fac5b5624fe3 | 975 | |
| mjhaugsdal | 30:fac5b5624fe3 | 976 | if(cmd == '7') |
| mjhaugsdal | 30:fac5b5624fe3 | 977 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 978 | |
| mjhaugsdal | 30:fac5b5624fe3 | 979 | |
| mjhaugsdal | 30:fac5b5624fe3 | 980 | steps3 --; |
| mjhaugsdal | 30:fac5b5624fe3 | 981 | stepEngine3Left(); |
| mjhaugsdal | 30:fac5b5624fe3 | 982 | |
| mjhaugsdal | 30:fac5b5624fe3 | 983 | } |
| mjhaugsdal | 30:fac5b5624fe3 | 984 | else if (cmd == '9') |
| mjhaugsdal | 30:fac5b5624fe3 | 985 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 986 | |
| mjhaugsdal | 30:fac5b5624fe3 | 987 | stepEngine3Right(); |
| mjhaugsdal | 30:fac5b5624fe3 | 988 | steps3 ++; |
| mjhaugsdal | 30:fac5b5624fe3 | 989 | } |
| mjhaugsdal | 30:fac5b5624fe3 | 990 | //ENGINE 2 |
| mjhaugsdal | 30:fac5b5624fe3 | 991 | else if (cmd == '4') |
| mjhaugsdal | 30:fac5b5624fe3 | 992 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 993 | |
| mjhaugsdal | 30:fac5b5624fe3 | 994 | steps2 --; |
| mjhaugsdal | 30:fac5b5624fe3 | 995 | stepEngine2Left(); |
| mjhaugsdal | 30:fac5b5624fe3 | 996 | } |
| mjhaugsdal | 30:fac5b5624fe3 | 997 | else if (cmd == '6') |
| mjhaugsdal | 30:fac5b5624fe3 | 998 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 999 | |
| mjhaugsdal | 30:fac5b5624fe3 | 1000 | steps2 ++; |
| mjhaugsdal | 30:fac5b5624fe3 | 1001 | stepEngine2Right(); |
| mjhaugsdal | 30:fac5b5624fe3 | 1002 | } |
| mjhaugsdal | 30:fac5b5624fe3 | 1003 | //ENGINE 3 |
| mjhaugsdal | 30:fac5b5624fe3 | 1004 | else if (cmd == '1') |
| mjhaugsdal | 30:fac5b5624fe3 | 1005 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 1006 | |
| mjhaugsdal | 30:fac5b5624fe3 | 1007 | steps1--; |
| mjhaugsdal | 30:fac5b5624fe3 | 1008 | stepEngine1Left(); |
| mjhaugsdal | 30:fac5b5624fe3 | 1009 | } |
| mjhaugsdal | 30:fac5b5624fe3 | 1010 | else if (cmd == '3') |
| mjhaugsdal | 30:fac5b5624fe3 | 1011 | { |
| mjhaugsdal | 30:fac5b5624fe3 | 1012 | |
| mjhaugsdal | 24:24c91a6ae6ba | 1013 | steps1++; |
| mjhaugsdal | 23:ad08a8eabc24 | 1014 | stepEngine1Right(); |
| mjhaugsdal | 19:79600d3509d7 | 1015 | } |
| mjhaugsdal | 23:ad08a8eabc24 | 1016 | |
| mjhaugsdal | 30:fac5b5624fe3 | 1017 | if (cmd == 'y') |
| mjhaugsdal | 27:35c962e8e95b | 1018 | { |
| mjhaugsdal | 27:35c962e8e95b | 1019 | int step = 100; |
| mjhaugsdal | 27:35c962e8e95b | 1020 | while (step >= 0) |
| mjhaugsdal | 27:35c962e8e95b | 1021 | { |
| mjhaugsdal | 27:35c962e8e95b | 1022 | step--; |
| mjhaugsdal | 27:35c962e8e95b | 1023 | step2Left3Right(); |
| mjhaugsdal | 27:35c962e8e95b | 1024 | } |
| mjhaugsdal | 27:35c962e8e95b | 1025 | m_cmd = 'x'; |
| mjhaugsdal | 27:35c962e8e95b | 1026 | |
| mjhaugsdal | 27:35c962e8e95b | 1027 | } |
| mjhaugsdal | 27:35c962e8e95b | 1028 | |
| mjhaugsdal | 30:fac5b5624fe3 | 1029 | if (cmd == 't') |
| mjhaugsdal | 27:35c962e8e95b | 1030 | { |
| mjhaugsdal | 27:35c962e8e95b | 1031 | int step1, step2, step3; |
| mjhaugsdal | 27:35c962e8e95b | 1032 | step1 = 100; |
| mjhaugsdal | 27:35c962e8e95b | 1033 | step2 = 60; |
| mjhaugsdal | 27:35c962e8e95b | 1034 | step3 = 70; |
| mjhaugsdal | 27:35c962e8e95b | 1035 | |
| mjhaugsdal | 27:35c962e8e95b | 1036 | while (step1 > 0) |
| mjhaugsdal | 27:35c962e8e95b | 1037 | { |
| mjhaugsdal | 27:35c962e8e95b | 1038 | stepEngine1Left(); |
| mjhaugsdal | 27:35c962e8e95b | 1039 | step1--; |
| mjhaugsdal | 27:35c962e8e95b | 1040 | //global stepcounter |
| mjhaugsdal | 27:35c962e8e95b | 1041 | steps1--; |
| mjhaugsdal | 27:35c962e8e95b | 1042 | } |
| mjhaugsdal | 27:35c962e8e95b | 1043 | while(step2 > 0) |
| mjhaugsdal | 27:35c962e8e95b | 1044 | { |
| mjhaugsdal | 27:35c962e8e95b | 1045 | stepEngine2Right(); |
| mjhaugsdal | 27:35c962e8e95b | 1046 | step2--; |
| mjhaugsdal | 27:35c962e8e95b | 1047 | //global stepcounter |
| mjhaugsdal | 27:35c962e8e95b | 1048 | steps2++; |
| mjhaugsdal | 27:35c962e8e95b | 1049 | } |
| mjhaugsdal | 27:35c962e8e95b | 1050 | while(step3 > 0) |
| mjhaugsdal | 27:35c962e8e95b | 1051 | { |
| mjhaugsdal | 27:35c962e8e95b | 1052 | stepEngine3Left(); |
| mjhaugsdal | 27:35c962e8e95b | 1053 | step3--; |
| mjhaugsdal | 27:35c962e8e95b | 1054 | //global stepcounter |
| mjhaugsdal | 27:35c962e8e95b | 1055 | steps3--; |
| mjhaugsdal | 27:35c962e8e95b | 1056 | } |
| mjhaugsdal | 27:35c962e8e95b | 1057 | |
| mjhaugsdal | 28:dac262b7ab32 | 1058 | //m_cmd = 'x'; |
| mjhaugsdal | 27:35c962e8e95b | 1059 | |
| mjhaugsdal | 27:35c962e8e95b | 1060 | } |
| mjhaugsdal | 25:321b970eb3ff | 1061 | |
| mjhaugsdal | 25:321b970eb3ff | 1062 | //Thread::wait(10); |
| mjhaugsdal | 25:321b970eb3ff | 1063 | //pc.printf("%d", steps2); |
| azadmehr | 32:5163db06f6cc | 1064 | */ |
| mjhaugsdal | 4:4d655fdae399 | 1065 | |
| mjhaugsdal | 24:24c91a6ae6ba | 1066 | } //END WHILE TRUE |
| mjhaugsdal | 1:a3287196a9b3 | 1067 | |
| mjhaugsdal | 3:15be78948724 | 1068 | } //END Main |