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