Robot Bachelor / Mbed 2 deprecated MC

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 Robot Bachelor

main.cpp@26:57c62b925064, 2016-05-30 (annotated)

Committer:
mjhaugsdal
Date:
Mon May 30 09:15:51 2016 +0000
Revision:
26:57c62b925064
Parent:
25:321b970eb3ff
Child:
27:35c962e8e95b
Temp save. Tested stepcounter on bootup

Who changed what in which revision?

UserRevisionLine numberNew 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
rlanghbv 0:bd78e433db61 36
mjhaugsdal 26:57c62b925064 37 static int speed = 5000;
mjhaugsdal 25:321b970eb3ff 38 static int bootStep1 = 100;
mjhaugsdal 25:321b970eb3ff 39 static int bootStep2 = 100;
mjhaugsdal 25:321b970eb3ff 40 static int bootStep3 = 100;
mjhaugsdal 3:15be78948724 41 static char m_cmd = 'x';
mjhaugsdal 23:ad08a8eabc24 42 static bool e1 = true;
mjhaugsdal 23:ad08a8eabc24 43 static bool e2 = true;
mjhaugsdal 23:ad08a8eabc24 44 static bool e3 = true;
mjhaugsdal 25:321b970eb3ff 45 static bool rest = false;
rlanghbv 0:bd78e433db61 46
mjhaugsdal 26:57c62b925064 47 static bool sync1 = false;
mjhaugsdal 26:57c62b925064 48 static bool sync2 = false;
mjhaugsdal 26:57c62b925064 49 static bool sync3 = false;
mjhaugsdal 26:57c62b925064 50
mjhaugsdal 24:24c91a6ae6ba 51 static int steps1 = 0;
mjhaugsdal 24:24c91a6ae6ba 52 static int steps2 = 0;
mjhaugsdal 24:24c91a6ae6ba 53 static int steps3 = 0;
mjhaugsdal 24:24c91a6ae6ba 54
mjhaugsdal 24:24c91a6ae6ba 55
mjhaugsdal 21:9f3ae352ee63 56 //Methods to set pins to control direction.
mjhaugsdal 17:ff0021c4dcdc 57 void stepAllRight()
rlanghbv 0:bd78e433db61 58 {
mjhaugsdal 6:ac5d277bd497 59 //engine 1
rlanghbv 0:bd78e433db61 60 IN1=0;
rlanghbv 0:bd78e433db61 61 IN2=1;
rlanghbv 0:bd78e433db61 62 IN3=0;
rlanghbv 0:bd78e433db61 63 IN4=1;
mjhaugsdal 6:ac5d277bd497 64 //engine 2
mjhaugsdal 6:ac5d277bd497 65 IN5=0;
mjhaugsdal 6:ac5d277bd497 66 IN6=1;
mjhaugsdal 6:ac5d277bd497 67 IN7=0;
mjhaugsdal 6:ac5d277bd497 68 IN8=1;
mjhaugsdal 17:ff0021c4dcdc 69 //engine 3
mjhaugsdal 17:ff0021c4dcdc 70 IN9=0;
mjhaugsdal 17:ff0021c4dcdc 71 IN10=1;
mjhaugsdal 17:ff0021c4dcdc 72 IN11=0;
mjhaugsdal 17:ff0021c4dcdc 73 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 74 wait_us(speed); //legg som global variabel "fart"
mjhaugsdal 6:ac5d277bd497 75 //engine 1
rlanghbv 0:bd78e433db61 76 IN1=0;
rlanghbv 0:bd78e433db61 77 IN2=1;
rlanghbv 0:bd78e433db61 78 IN3=1;
rlanghbv 0:bd78e433db61 79 IN4=0;
mjhaugsdal 6:ac5d277bd497 80 //engine 2
mjhaugsdal 6:ac5d277bd497 81 IN5=0;
mjhaugsdal 6:ac5d277bd497 82 IN6=1;
mjhaugsdal 6:ac5d277bd497 83 IN7=1;
mjhaugsdal 6:ac5d277bd497 84 IN8=0;
mjhaugsdal 17:ff0021c4dcdc 85 //engine 3
mjhaugsdal 17:ff0021c4dcdc 86 IN9=0;
mjhaugsdal 17:ff0021c4dcdc 87 IN10=1;
mjhaugsdal 17:ff0021c4dcdc 88 IN11=1;
mjhaugsdal 17:ff0021c4dcdc 89 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 90 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 91 //engine 1
rlanghbv 0:bd78e433db61 92 IN1=1;
rlanghbv 0:bd78e433db61 93 IN2=0;
rlanghbv 0:bd78e433db61 94 IN3=1;
rlanghbv 0:bd78e433db61 95 IN4=0;
mjhaugsdal 6:ac5d277bd497 96 //engine 2
mjhaugsdal 6:ac5d277bd497 97 IN5=1;
mjhaugsdal 6:ac5d277bd497 98 IN6=0;
mjhaugsdal 6:ac5d277bd497 99 IN7=1;
mjhaugsdal 6:ac5d277bd497 100 IN8=0;
mjhaugsdal 17:ff0021c4dcdc 101 //engine 3
mjhaugsdal 17:ff0021c4dcdc 102 IN9=1;
mjhaugsdal 17:ff0021c4dcdc 103 IN10=0;
mjhaugsdal 17:ff0021c4dcdc 104 IN11=1;
mjhaugsdal 17:ff0021c4dcdc 105 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 106 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 107 //engine 1
rlanghbv 0:bd78e433db61 108 IN1=1;
rlanghbv 0:bd78e433db61 109 IN2=0;
rlanghbv 0:bd78e433db61 110 IN3=0;
rlanghbv 0:bd78e433db61 111 IN4=1;
mjhaugsdal 6:ac5d277bd497 112 //engine 2
mjhaugsdal 6:ac5d277bd497 113 IN5=1;
mjhaugsdal 6:ac5d277bd497 114 IN6=0;
mjhaugsdal 6:ac5d277bd497 115 IN7=0;
mjhaugsdal 6:ac5d277bd497 116 IN8=1;
mjhaugsdal 17:ff0021c4dcdc 117 //engine 3
mjhaugsdal 17:ff0021c4dcdc 118 IN9=1;
mjhaugsdal 17:ff0021c4dcdc 119 IN10=0;
mjhaugsdal 17:ff0021c4dcdc 120 IN11=0;
mjhaugsdal 17:ff0021c4dcdc 121 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 122 wait_us(speed);
rlanghbv 0:bd78e433db61 123 }
mjhaugsdal 17:ff0021c4dcdc 124 void stepAllLeft()
rlanghbv 0:bd78e433db61 125 {
mjhaugsdal 6:ac5d277bd497 126 //engine 1
rlanghbv 0:bd78e433db61 127 IN1=1;
rlanghbv 0:bd78e433db61 128 IN2=0;
rlanghbv 0:bd78e433db61 129 IN3=0;
rlanghbv 0:bd78e433db61 130 IN4=1;
mjhaugsdal 6:ac5d277bd497 131 //engine 2
mjhaugsdal 6:ac5d277bd497 132 IN5=1;
mjhaugsdal 6:ac5d277bd497 133 IN6=0;
mjhaugsdal 6:ac5d277bd497 134 IN7=0;
mjhaugsdal 6:ac5d277bd497 135 IN8=1;
mjhaugsdal 17:ff0021c4dcdc 136 //engine 3
mjhaugsdal 17:ff0021c4dcdc 137 IN9=1;
mjhaugsdal 17:ff0021c4dcdc 138 IN10=0;
mjhaugsdal 17:ff0021c4dcdc 139 IN11=0;
mjhaugsdal 17:ff0021c4dcdc 140 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 141 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 142 //engine 1
rlanghbv 0:bd78e433db61 143 IN1=1;
rlanghbv 0:bd78e433db61 144 IN2=0;
rlanghbv 0:bd78e433db61 145 IN3=1;
rlanghbv 0:bd78e433db61 146 IN4=0;
mjhaugsdal 6:ac5d277bd497 147 //engine 2
mjhaugsdal 6:ac5d277bd497 148 IN5=1;
mjhaugsdal 6:ac5d277bd497 149 IN6=0;
mjhaugsdal 6:ac5d277bd497 150 IN7=1;
mjhaugsdal 6:ac5d277bd497 151 IN8=0;
mjhaugsdal 17:ff0021c4dcdc 152 //engine 3
mjhaugsdal 17:ff0021c4dcdc 153 IN9=1;
mjhaugsdal 17:ff0021c4dcdc 154 IN10=0;
mjhaugsdal 17:ff0021c4dcdc 155 IN11=1;
mjhaugsdal 17:ff0021c4dcdc 156 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 157 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 158 //engine 1
rlanghbv 0:bd78e433db61 159 IN1=0;
rlanghbv 0:bd78e433db61 160 IN2=1;
rlanghbv 0:bd78e433db61 161 IN3=1;
rlanghbv 0:bd78e433db61 162 IN4=0;
mjhaugsdal 6:ac5d277bd497 163 //engine 2
mjhaugsdal 6:ac5d277bd497 164 IN5=0;
mjhaugsdal 6:ac5d277bd497 165 IN6=1;
mjhaugsdal 6:ac5d277bd497 166 IN7=1;
mjhaugsdal 6:ac5d277bd497 167 IN8=0;
mjhaugsdal 17:ff0021c4dcdc 168 //engine 3
mjhaugsdal 17:ff0021c4dcdc 169 IN9=0;
mjhaugsdal 17:ff0021c4dcdc 170 IN10=1;
mjhaugsdal 17:ff0021c4dcdc 171 IN11=1;
mjhaugsdal 17:ff0021c4dcdc 172 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 173 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 174 //engine 1
rlanghbv 0:bd78e433db61 175 IN1=0;
rlanghbv 0:bd78e433db61 176 IN2=1;
rlanghbv 0:bd78e433db61 177 IN3=0;
rlanghbv 0:bd78e433db61 178 IN4=1;
mjhaugsdal 6:ac5d277bd497 179 //engine 2
mjhaugsdal 6:ac5d277bd497 180 IN5=0;
mjhaugsdal 6:ac5d277bd497 181 IN6=1;
mjhaugsdal 6:ac5d277bd497 182 IN7=0;
mjhaugsdal 6:ac5d277bd497 183 IN8=1;
mjhaugsdal 17:ff0021c4dcdc 184 //engine 3
mjhaugsdal 17:ff0021c4dcdc 185 IN9=0;
mjhaugsdal 17:ff0021c4dcdc 186 IN10=1;
mjhaugsdal 17:ff0021c4dcdc 187 IN11=0;
mjhaugsdal 17:ff0021c4dcdc 188 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 189 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 190 }
mjhaugsdal 17:ff0021c4dcdc 191
mjhaugsdal 17:ff0021c4dcdc 192 void stepEngine1Left()
mjhaugsdal 17:ff0021c4dcdc 193 {
mjhaugsdal 17:ff0021c4dcdc 194 //engine 1
mjhaugsdal 17:ff0021c4dcdc 195 IN1=1;
mjhaugsdal 17:ff0021c4dcdc 196 IN2=0;
mjhaugsdal 17:ff0021c4dcdc 197 IN3=0;
mjhaugsdal 17:ff0021c4dcdc 198 IN4=1;
mjhaugsdal 24:24c91a6ae6ba 199 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 200 IN1=1;
mjhaugsdal 17:ff0021c4dcdc 201 IN2=0;
mjhaugsdal 17:ff0021c4dcdc 202 IN3=1;
mjhaugsdal 17:ff0021c4dcdc 203 IN4=0;
mjhaugsdal 24:24c91a6ae6ba 204 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 205 IN1=0;
mjhaugsdal 17:ff0021c4dcdc 206 IN2=1;
mjhaugsdal 17:ff0021c4dcdc 207 IN3=1;
mjhaugsdal 17:ff0021c4dcdc 208 IN4=0;
mjhaugsdal 24:24c91a6ae6ba 209 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 210 IN1=0;
mjhaugsdal 17:ff0021c4dcdc 211 IN2=1;
mjhaugsdal 17:ff0021c4dcdc 212 IN3=0;
mjhaugsdal 17:ff0021c4dcdc 213 IN4=1;
mjhaugsdal 24:24c91a6ae6ba 214 wait_us(speed);
rlanghbv 0:bd78e433db61 215 }
rlanghbv 0:bd78e433db61 216
mjhaugsdal 17:ff0021c4dcdc 217 void stepEngine1Right()
mjhaugsdal 17:ff0021c4dcdc 218 {
mjhaugsdal 17:ff0021c4dcdc 219 //engine 1
mjhaugsdal 17:ff0021c4dcdc 220 IN1=0;
mjhaugsdal 17:ff0021c4dcdc 221 IN2=1;
mjhaugsdal 17:ff0021c4dcdc 222 IN3=0;
mjhaugsdal 17:ff0021c4dcdc 223 IN4=1;
mjhaugsdal 24:24c91a6ae6ba 224 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 225 IN1=0;
mjhaugsdal 17:ff0021c4dcdc 226 IN2=1;
mjhaugsdal 17:ff0021c4dcdc 227 IN3=1;
mjhaugsdal 17:ff0021c4dcdc 228 IN4=0;
mjhaugsdal 24:24c91a6ae6ba 229 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 230 IN1=1;
mjhaugsdal 17:ff0021c4dcdc 231 IN2=0;
mjhaugsdal 17:ff0021c4dcdc 232 IN3=1;
mjhaugsdal 17:ff0021c4dcdc 233 IN4=0;
mjhaugsdal 24:24c91a6ae6ba 234 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 235 IN1=1;
mjhaugsdal 17:ff0021c4dcdc 236 IN2=0;
mjhaugsdal 17:ff0021c4dcdc 237 IN3=0;
mjhaugsdal 17:ff0021c4dcdc 238 IN4=1;
mjhaugsdal 24:24c91a6ae6ba 239 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 240 }
mjhaugsdal 17:ff0021c4dcdc 241
mjhaugsdal 17:ff0021c4dcdc 242 void stepEngine2Right()
mjhaugsdal 17:ff0021c4dcdc 243 {
mjhaugsdal 17:ff0021c4dcdc 244 //engine 1
mjhaugsdal 17:ff0021c4dcdc 245 IN5=0;
mjhaugsdal 17:ff0021c4dcdc 246 IN6=1;
mjhaugsdal 17:ff0021c4dcdc 247 IN7=0;
mjhaugsdal 17:ff0021c4dcdc 248 IN8=1;
mjhaugsdal 24:24c91a6ae6ba 249 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 250 IN5=0;
mjhaugsdal 17:ff0021c4dcdc 251 IN6=1;
mjhaugsdal 17:ff0021c4dcdc 252 IN7=1;
mjhaugsdal 17:ff0021c4dcdc 253 IN8=0;
mjhaugsdal 24:24c91a6ae6ba 254 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 255 IN5=1;
mjhaugsdal 17:ff0021c4dcdc 256 IN6=0;
mjhaugsdal 17:ff0021c4dcdc 257 IN7=1;
mjhaugsdal 17:ff0021c4dcdc 258 IN8=0;
mjhaugsdal 24:24c91a6ae6ba 259 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 260 IN5=1;
mjhaugsdal 17:ff0021c4dcdc 261 IN6=0;
mjhaugsdal 17:ff0021c4dcdc 262 IN7=0;
mjhaugsdal 17:ff0021c4dcdc 263 IN8=1;
mjhaugsdal 24:24c91a6ae6ba 264 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 265 }
mjhaugsdal 17:ff0021c4dcdc 266
mjhaugsdal 17:ff0021c4dcdc 267 void stepEngine2Left()
mjhaugsdal 17:ff0021c4dcdc 268 {
mjhaugsdal 17:ff0021c4dcdc 269 //engine 1
mjhaugsdal 17:ff0021c4dcdc 270 IN5=1;
mjhaugsdal 17:ff0021c4dcdc 271 IN6=0;
mjhaugsdal 17:ff0021c4dcdc 272 IN7=0;
mjhaugsdal 17:ff0021c4dcdc 273 IN8=1;
mjhaugsdal 24:24c91a6ae6ba 274 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 275 IN5=1;
mjhaugsdal 17:ff0021c4dcdc 276 IN6=0;
mjhaugsdal 17:ff0021c4dcdc 277 IN7=1;
mjhaugsdal 17:ff0021c4dcdc 278 IN8=0;
mjhaugsdal 24:24c91a6ae6ba 279 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 280 IN5=0;
mjhaugsdal 17:ff0021c4dcdc 281 IN6=1;
mjhaugsdal 17:ff0021c4dcdc 282 IN7=1;
mjhaugsdal 17:ff0021c4dcdc 283 IN8=0;
mjhaugsdal 24:24c91a6ae6ba 284 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 285 IN5=0;
mjhaugsdal 17:ff0021c4dcdc 286 IN6=1;
mjhaugsdal 17:ff0021c4dcdc 287 IN7=0;
mjhaugsdal 17:ff0021c4dcdc 288 IN8=1;
mjhaugsdal 24:24c91a6ae6ba 289 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 290 }
mjhaugsdal 17:ff0021c4dcdc 291
mjhaugsdal 19:79600d3509d7 292 void stepEngine3Right()
mjhaugsdal 19:79600d3509d7 293 {
mjhaugsdal 19:79600d3509d7 294 //engine 1
mjhaugsdal 19:79600d3509d7 295 IN9=0;
mjhaugsdal 19:79600d3509d7 296 IN10=1;
mjhaugsdal 19:79600d3509d7 297 IN11=0;
mjhaugsdal 19:79600d3509d7 298 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 299 wait_us(speed);
mjhaugsdal 19:79600d3509d7 300 IN9=0;
mjhaugsdal 19:79600d3509d7 301 IN10=1;
mjhaugsdal 19:79600d3509d7 302 IN11=1;
mjhaugsdal 19:79600d3509d7 303 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 304 wait_us(speed);
mjhaugsdal 19:79600d3509d7 305 IN9=1;
mjhaugsdal 19:79600d3509d7 306 IN10=0;
mjhaugsdal 19:79600d3509d7 307 IN11=1;
mjhaugsdal 19:79600d3509d7 308 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 309 wait_us(speed);
mjhaugsdal 19:79600d3509d7 310 IN9=1;
mjhaugsdal 19:79600d3509d7 311 IN10=0;
mjhaugsdal 19:79600d3509d7 312 IN11=0;
mjhaugsdal 19:79600d3509d7 313 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 314 wait_us(speed);
mjhaugsdal 19:79600d3509d7 315 }
mjhaugsdal 19:79600d3509d7 316
mjhaugsdal 19:79600d3509d7 317 void stepEngine3Left()
mjhaugsdal 19:79600d3509d7 318 {
mjhaugsdal 19:79600d3509d7 319 //engine 1
mjhaugsdal 19:79600d3509d7 320 IN9=1;
mjhaugsdal 19:79600d3509d7 321 IN10=0;
mjhaugsdal 19:79600d3509d7 322 IN11=0;
mjhaugsdal 19:79600d3509d7 323 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 324 wait_us(speed);
mjhaugsdal 19:79600d3509d7 325 IN9=1;
mjhaugsdal 19:79600d3509d7 326 IN10=0;
mjhaugsdal 19:79600d3509d7 327 IN11=1;
mjhaugsdal 19:79600d3509d7 328 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 329 wait_us(speed);
mjhaugsdal 19:79600d3509d7 330 IN9=0;
mjhaugsdal 19:79600d3509d7 331 IN10=1;
mjhaugsdal 19:79600d3509d7 332 IN11=1;
mjhaugsdal 19:79600d3509d7 333 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 334 wait_us(speed);
mjhaugsdal 19:79600d3509d7 335 IN9=0;
mjhaugsdal 19:79600d3509d7 336 IN10=1;
mjhaugsdal 19:79600d3509d7 337 IN11=0;
mjhaugsdal 19:79600d3509d7 338 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 339 wait_us(speed);
mjhaugsdal 19:79600d3509d7 340 }
mjhaugsdal 19:79600d3509d7 341
mjhaugsdal 23:ad08a8eabc24 342
mjhaugsdal 23:ad08a8eabc24 343 void step1()
mjhaugsdal 23:ad08a8eabc24 344 {
mjhaugsdal 23:ad08a8eabc24 345 //engine 1
mjhaugsdal 23:ad08a8eabc24 346 IN1=1;
mjhaugsdal 23:ad08a8eabc24 347 IN2=0;
mjhaugsdal 23:ad08a8eabc24 348 IN3=0;
mjhaugsdal 23:ad08a8eabc24 349 IN4=1;
mjhaugsdal 23:ad08a8eabc24 350 }
mjhaugsdal 23:ad08a8eabc24 351
mjhaugsdal 23:ad08a8eabc24 352 void step2()
mjhaugsdal 23:ad08a8eabc24 353 {
mjhaugsdal 23:ad08a8eabc24 354 //engine 1
mjhaugsdal 23:ad08a8eabc24 355 IN1=0;
mjhaugsdal 23:ad08a8eabc24 356 IN2=1;
mjhaugsdal 23:ad08a8eabc24 357 IN3=0;
mjhaugsdal 23:ad08a8eabc24 358 IN4=1;
mjhaugsdal 23:ad08a8eabc24 359 }
mjhaugsdal 23:ad08a8eabc24 360
mjhaugsdal 23:ad08a8eabc24 361 void wait()
mjhaugsdal 23:ad08a8eabc24 362 {
mjhaugsdal 24:24c91a6ae6ba 363 wait_us(speed);
mjhaugsdal 23:ad08a8eabc24 364
mjhaugsdal 23:ad08a8eabc24 365 }
mjhaugsdal 23:ad08a8eabc24 366
mjhaugsdal 21:9f3ae352ee63 367 //Method to release all engines.
mjhaugsdal 18:ef02a9014491 368 void stopAll()
mjhaugsdal 18:ef02a9014491 369 {
mjhaugsdal 18:ef02a9014491 370 IN1=0;
mjhaugsdal 18:ef02a9014491 371 IN2=0;
mjhaugsdal 18:ef02a9014491 372 IN3=0;
mjhaugsdal 18:ef02a9014491 373 IN4=0;
mjhaugsdal 18:ef02a9014491 374 IN5=0;
mjhaugsdal 18:ef02a9014491 375 IN6=0;
mjhaugsdal 18:ef02a9014491 376 IN7=0;
mjhaugsdal 18:ef02a9014491 377 IN8=0;
mjhaugsdal 18:ef02a9014491 378 IN9=0;
mjhaugsdal 18:ef02a9014491 379 IN10=0;
mjhaugsdal 18:ef02a9014491 380 IN11=0;
mjhaugsdal 18:ef02a9014491 381 IN12=0;
mjhaugsdal 18:ef02a9014491 382 }
mjhaugsdal 18:ef02a9014491 383
mjhaugsdal 21:9f3ae352ee63 384 //Methods to release single engines.
mjhaugsdal 20:d90bd1718078 385 void stopE1()
mjhaugsdal 20:d90bd1718078 386 {
mjhaugsdal 20:d90bd1718078 387 IN1=0;
mjhaugsdal 20:d90bd1718078 388 IN2=0;
mjhaugsdal 20:d90bd1718078 389 IN3=0;
mjhaugsdal 20:d90bd1718078 390 IN4=0;
mjhaugsdal 20:d90bd1718078 391
mjhaugsdal 20:d90bd1718078 392 }
mjhaugsdal 20:d90bd1718078 393 void stopE2()
mjhaugsdal 20:d90bd1718078 394 {
mjhaugsdal 20:d90bd1718078 395 IN5=0;
mjhaugsdal 20:d90bd1718078 396 IN6=0;
mjhaugsdal 20:d90bd1718078 397 IN7=0;
mjhaugsdal 20:d90bd1718078 398 IN8=0;
mjhaugsdal 20:d90bd1718078 399
mjhaugsdal 20:d90bd1718078 400 }
mjhaugsdal 20:d90bd1718078 401 void stopE3()
mjhaugsdal 20:d90bd1718078 402 {
mjhaugsdal 20:d90bd1718078 403 IN9=0;
mjhaugsdal 20:d90bd1718078 404 IN10=0;
mjhaugsdal 20:d90bd1718078 405 IN11=0;
mjhaugsdal 20:d90bd1718078 406 IN12=0;
mjhaugsdal 20:d90bd1718078 407
mjhaugsdal 20:d90bd1718078 408 }
mjhaugsdal 20:d90bd1718078 409
mjhaugsdal 17:ff0021c4dcdc 410
mjhaugsdal 17:ff0021c4dcdc 411
mjhaugsdal 19:79600d3509d7 412
mjhaugsdal 19:79600d3509d7 413
mjhaugsdal 23:ad08a8eabc24 414
mjhaugsdal 23:ad08a8eabc24 415
mjhaugsdal 1:a3287196a9b3 416 void input(void const *args)
mjhaugsdal 1:a3287196a9b3 417 {
mjhaugsdal 1:a3287196a9b3 418 while(true)
mjhaugsdal 1:a3287196a9b3 419 {
mjhaugsdal 3:15be78948724 420
rlanghbv 2:25bcc26f7a5b 421 if(pc.readable())
mjhaugsdal 3:15be78948724 422 { m_cmd=pc.getc();
mjhaugsdal 3:15be78948724 423
mjhaugsdal 17:ff0021c4dcdc 424 }
mjhaugsdal 17:ff0021c4dcdc 425
mjhaugsdal 23:ad08a8eabc24 426 Thread::wait(100);
mjhaugsdal 5:fdc7a779d525 427 //pc.printf("%d", steps);
mjhaugsdal 23:ad08a8eabc24 428
mjhaugsdal 1:a3287196a9b3 429 }
mjhaugsdal 1:a3287196a9b3 430 }
rlanghbv 0:bd78e433db61 431
mjhaugsdal 25:321b970eb3ff 432
mjhaugsdal 26:57c62b925064 433 //MOTOR 1 RIGHT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 434 void pb1_hit_callback (void)
mjhaugsdal 23:ad08a8eabc24 435 {
mjhaugsdal 25:321b970eb3ff 436 if(rest == false)
mjhaugsdal 25:321b970eb3ff 437 {
mjhaugsdal 25:321b970eb3ff 438 e1 = false;
mjhaugsdal 25:321b970eb3ff 439
mjhaugsdal 26:57c62b925064 440 //speed = 2000;
mjhaugsdal 26:57c62b925064 441 if (sync1 == true)
mjhaugsdal 25:321b970eb3ff 442 {
mjhaugsdal 26:57c62b925064 443 Thread::wait(1000);
mjhaugsdal 26:57c62b925064 444
mjhaugsdal 26:57c62b925064 445 int stepsToCenter = steps1/2;
mjhaugsdal 26:57c62b925064 446
mjhaugsdal 26:57c62b925064 447 while (stepsToCenter >= 0)
mjhaugsdal 26:57c62b925064 448 {
mjhaugsdal 26:57c62b925064 449 stepEngine1Left();
mjhaugsdal 26:57c62b925064 450 stepsToCenter --;
mjhaugsdal 26:57c62b925064 451 }
mjhaugsdal 26:57c62b925064 452 //break;
mjhaugsdal 26:57c62b925064 453
mjhaugsdal 25:321b970eb3ff 454 }
mjhaugsdal 25:321b970eb3ff 455 }
mjhaugsdal 25:321b970eb3ff 456
mjhaugsdal 23:ad08a8eabc24 457
mjhaugsdal 23:ad08a8eabc24 458
mjhaugsdal 25:321b970eb3ff 459 }
mjhaugsdal 25:321b970eb3ff 460
mjhaugsdal 25:321b970eb3ff 461
mjhaugsdal 26:57c62b925064 462 //MOTOR 1 LEFT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 463 void pb2_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 464 {
mjhaugsdal 25:321b970eb3ff 465 if(rest == false)
mjhaugsdal 25:321b970eb3ff 466 {
mjhaugsdal 25:321b970eb3ff 467 e1 = false;
mjhaugsdal 25:321b970eb3ff 468 //m_cmd = 'x';
mjhaugsdal 23:ad08a8eabc24 469
mjhaugsdal 26:57c62b925064 470 //speed = 2000;
mjhaugsdal 26:57c62b925064 471 if (sync1 == true)
mjhaugsdal 25:321b970eb3ff 472 {
mjhaugsdal 26:57c62b925064 473 Thread::wait(1000);
mjhaugsdal 26:57c62b925064 474 int stepsToCenter = 242;
mjhaugsdal 26:57c62b925064 475
mjhaugsdal 26:57c62b925064 476 while (stepsToCenter >= 0)
mjhaugsdal 26:57c62b925064 477 {
mjhaugsdal 26:57c62b925064 478 stepEngine1Right();
mjhaugsdal 26:57c62b925064 479 stepsToCenter --;
mjhaugsdal 26:57c62b925064 480 }
mjhaugsdal 25:321b970eb3ff 481 }
mjhaugsdal 25:321b970eb3ff 482 }
mjhaugsdal 25:321b970eb3ff 483
mjhaugsdal 25:321b970eb3ff 484 }
mjhaugsdal 25:321b970eb3ff 485 //MOTOR 2 LEFT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 486 void pb3_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 487 {
mjhaugsdal 25:321b970eb3ff 488 if(rest == false)
mjhaugsdal 25:321b970eb3ff 489 {
mjhaugsdal 25:321b970eb3ff 490 e2 = false;
mjhaugsdal 25:321b970eb3ff 491 //m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 492 speed = 2000;
mjhaugsdal 25:321b970eb3ff 493 Thread::wait(1000);
mjhaugsdal 25:321b970eb3ff 494 int stepsToCenter = 242;
mjhaugsdal 25:321b970eb3ff 495
mjhaugsdal 25:321b970eb3ff 496 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 497 {
mjhaugsdal 25:321b970eb3ff 498 stepEngine2Left();
mjhaugsdal 25:321b970eb3ff 499 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 500 }
mjhaugsdal 25:321b970eb3ff 501 }
mjhaugsdal 23:ad08a8eabc24 502
mjhaugsdal 23:ad08a8eabc24 503 }
mjhaugsdal 23:ad08a8eabc24 504
mjhaugsdal 25:321b970eb3ff 505 //MOTOR 2 RIGHT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 506 void pb4_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 507 {
mjhaugsdal 25:321b970eb3ff 508 if(rest == false)
mjhaugsdal 25:321b970eb3ff 509 {
mjhaugsdal 25:321b970eb3ff 510 e2 = false;
mjhaugsdal 25:321b970eb3ff 511 //m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 512 speed = 2000;
mjhaugsdal 25:321b970eb3ff 513 Thread::wait(1000);
mjhaugsdal 25:321b970eb3ff 514 int stepsToCenter = 242;
mjhaugsdal 25:321b970eb3ff 515
mjhaugsdal 25:321b970eb3ff 516 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 517 {
mjhaugsdal 25:321b970eb3ff 518 stepEngine2Right();
mjhaugsdal 25:321b970eb3ff 519 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 520 }
mjhaugsdal 25:321b970eb3ff 521 }
mjhaugsdal 25:321b970eb3ff 522
mjhaugsdal 25:321b970eb3ff 523
mjhaugsdal 25:321b970eb3ff 524 }
mjhaugsdal 25:321b970eb3ff 525
mjhaugsdal 25:321b970eb3ff 526 //MOTOR 3 LEFT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 527 void pb5_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 528 {
mjhaugsdal 25:321b970eb3ff 529 if(rest == false)
mjhaugsdal 25:321b970eb3ff 530 {
mjhaugsdal 25:321b970eb3ff 531 e3 = false;
mjhaugsdal 25:321b970eb3ff 532 //m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 533 speed = 2000;
mjhaugsdal 25:321b970eb3ff 534 Thread::wait(1000);
mjhaugsdal 25:321b970eb3ff 535 int stepsToCenter = 242;
mjhaugsdal 25:321b970eb3ff 536
mjhaugsdal 25:321b970eb3ff 537 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 538 {
mjhaugsdal 25:321b970eb3ff 539 stepEngine3Left();
mjhaugsdal 25:321b970eb3ff 540 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 541 }
mjhaugsdal 25:321b970eb3ff 542 }
mjhaugsdal 25:321b970eb3ff 543
mjhaugsdal 25:321b970eb3ff 544
mjhaugsdal 25:321b970eb3ff 545 }
mjhaugsdal 25:321b970eb3ff 546 //MOTOR 3 RIGHT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 547 void pb6_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 548 {
mjhaugsdal 25:321b970eb3ff 549 if(rest == false)
mjhaugsdal 25:321b970eb3ff 550 {
mjhaugsdal 25:321b970eb3ff 551 e3 = false;
mjhaugsdal 25:321b970eb3ff 552 //m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 553 speed = 2000;
mjhaugsdal 25:321b970eb3ff 554 Thread::wait(1000);
mjhaugsdal 25:321b970eb3ff 555 int stepsToCenter = 242;
mjhaugsdal 25:321b970eb3ff 556
mjhaugsdal 25:321b970eb3ff 557 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 558 {
mjhaugsdal 25:321b970eb3ff 559 stepEngine3Right();
mjhaugsdal 25:321b970eb3ff 560 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 561 }
mjhaugsdal 25:321b970eb3ff 562 }
mjhaugsdal 25:321b970eb3ff 563 }
mjhaugsdal 23:ad08a8eabc24 564
rlanghbv 0:bd78e433db61 565 int main()
rlanghbv 0:bd78e433db61 566 {
mjhaugsdal 24:24c91a6ae6ba 567
mjhaugsdal 25:321b970eb3ff 568 pb1.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 569 pb2.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 570 pb3.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 571 pb4.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 572 pb5.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 573 pb6.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 574
mjhaugsdal 25:321b970eb3ff 575 //Set up buttons 1 and 2
mjhaugsdal 25:321b970eb3ff 576 pb1.attach_deasserted(&pb1_hit_callback);
mjhaugsdal 25:321b970eb3ff 577 pb1.setSampleFrequency();
mjhaugsdal 25:321b970eb3ff 578 pb2.attach_deasserted(&pb2_hit_callback);
mjhaugsdal 25:321b970eb3ff 579 pb2.setSampleFrequency();
mjhaugsdal 23:ad08a8eabc24 580
mjhaugsdal 1:a3287196a9b3 581 //Thread 1 has constant feed from usb
rlanghbv 2:25bcc26f7a5b 582 Thread t1(input);
mjhaugsdal 22:f6e328f7bd28 583 //static char global_direction;
mjhaugsdal 3:15be78948724 584 printf("Started");
mjhaugsdal 22:f6e328f7bd28 585
mjhaugsdal 24:24c91a6ae6ba 586 // Move motor in one direction
mjhaugsdal 24:24c91a6ae6ba 587 while (true)
mjhaugsdal 24:24c91a6ae6ba 588 {
mjhaugsdal 24:24c91a6ae6ba 589 speed = 2000;
mjhaugsdal 25:321b970eb3ff 590
mjhaugsdal 25:321b970eb3ff 591 while(bootStep1 >= 0)
mjhaugsdal 25:321b970eb3ff 592 {
mjhaugsdal 25:321b970eb3ff 593 bootStep1--;
mjhaugsdal 25:321b970eb3ff 594 stepEngine1Right();
mjhaugsdal 25:321b970eb3ff 595 }
mjhaugsdal 24:24c91a6ae6ba 596 stepEngine1Left();
mjhaugsdal 26:57c62b925064 597 //Found corner!
mjhaugsdal 24:24c91a6ae6ba 598 if (e1 == false)
mjhaugsdal 24:24c91a6ae6ba 599 {
mjhaugsdal 26:57c62b925064 600 e1= true;
mjhaugsdal 26:57c62b925064 601 Thread::wait(1000);
mjhaugsdal 26:57c62b925064 602 while(true)
mjhaugsdal 26:57c62b925064 603 {
mjhaugsdal 26:57c62b925064 604 steps1++;
mjhaugsdal 26:57c62b925064 605 //Thread::wait(100);
mjhaugsdal 26:57c62b925064 606 pc.printf("%d", steps1);
mjhaugsdal 26:57c62b925064 607 stepEngine1Right();
mjhaugsdal 26:57c62b925064 608 if(e1==false)
mjhaugsdal 26:57c62b925064 609 break;
mjhaugsdal 26:57c62b925064 610 }
mjhaugsdal 26:57c62b925064 611
mjhaugsdal 24:24c91a6ae6ba 612 Thread::wait(1000);
mjhaugsdal 25:321b970eb3ff 613 break;
mjhaugsdal 24:24c91a6ae6ba 614 }//end if
mjhaugsdal 25:321b970eb3ff 615
mjhaugsdal 24:24c91a6ae6ba 616 }//end while
mjhaugsdal 25:321b970eb3ff 617
mjhaugsdal 25:321b970eb3ff 618 //Set up buttons 3 and 4
mjhaugsdal 25:321b970eb3ff 619 pb3.attach_deasserted(&pb3_hit_callback);
mjhaugsdal 25:321b970eb3ff 620 pb3.setSampleFrequency();
mjhaugsdal 25:321b970eb3ff 621 pb4.attach_deasserted(&pb4_hit_callback);
mjhaugsdal 25:321b970eb3ff 622 pb4.setSampleFrequency();
mjhaugsdal 25:321b970eb3ff 623
mjhaugsdal 24:24c91a6ae6ba 624 while (true)
mjhaugsdal 24:24c91a6ae6ba 625 {
mjhaugsdal 25:321b970eb3ff 626
mjhaugsdal 25:321b970eb3ff 627 speed = 2000;
mjhaugsdal 25:321b970eb3ff 628 while(bootStep2 >= 0)
mjhaugsdal 25:321b970eb3ff 629 {
mjhaugsdal 25:321b970eb3ff 630 bootStep2--;
mjhaugsdal 25:321b970eb3ff 631 stepEngine2Right();
mjhaugsdal 25:321b970eb3ff 632
mjhaugsdal 25:321b970eb3ff 633 }
mjhaugsdal 25:321b970eb3ff 634
mjhaugsdal 25:321b970eb3ff 635 stepEngine2Left();
mjhaugsdal 24:24c91a6ae6ba 636 if (e2 == false)
mjhaugsdal 24:24c91a6ae6ba 637 {
mjhaugsdal 25:321b970eb3ff 638 Thread::wait(1000);
mjhaugsdal 24:24c91a6ae6ba 639 break;
mjhaugsdal 24:24c91a6ae6ba 640 }//end if
mjhaugsdal 24:24c91a6ae6ba 641 }
mjhaugsdal 25:321b970eb3ff 642
mjhaugsdal 25:321b970eb3ff 643 pb5.attach_deasserted(&pb5_hit_callback);
mjhaugsdal 25:321b970eb3ff 644 pb5.setSampleFrequency();
mjhaugsdal 25:321b970eb3ff 645 pb6.attach_deasserted(&pb6_hit_callback);
mjhaugsdal 25:321b970eb3ff 646 pb6.setSampleFrequency();
mjhaugsdal 24:24c91a6ae6ba 647 while (true)
mjhaugsdal 24:24c91a6ae6ba 648 {
mjhaugsdal 25:321b970eb3ff 649 speed = 2000;
mjhaugsdal 25:321b970eb3ff 650 while(bootStep3 >= 0)
mjhaugsdal 25:321b970eb3ff 651 {
mjhaugsdal 25:321b970eb3ff 652 bootStep3--;
mjhaugsdal 25:321b970eb3ff 653 stepEngine3Right();
mjhaugsdal 25:321b970eb3ff 654
mjhaugsdal 25:321b970eb3ff 655 }
mjhaugsdal 25:321b970eb3ff 656 stepEngine3Left();
mjhaugsdal 24:24c91a6ae6ba 657 if (e3 == false)
mjhaugsdal 24:24c91a6ae6ba 658 {
mjhaugsdal 25:321b970eb3ff 659 pc.printf("HALLO");
mjhaugsdal 24:24c91a6ae6ba 660 Thread::wait(1000);
mjhaugsdal 24:24c91a6ae6ba 661 break;
mjhaugsdal 24:24c91a6ae6ba 662 }//end if
mjhaugsdal 24:24c91a6ae6ba 663 }
mjhaugsdal 24:24c91a6ae6ba 664
mjhaugsdal 1:a3287196a9b3 665 while (true)
mjhaugsdal 22:f6e328f7bd28 666 {
mjhaugsdal 24:24c91a6ae6ba 667 speed = 1500;
mjhaugsdal 24:24c91a6ae6ba 668
mjhaugsdal 24:24c91a6ae6ba 669
mjhaugsdal 21:9f3ae352ee63 670 //Release all motors / Set all pins to 0
mjhaugsdal 18:ef02a9014491 671 if (m_cmd == 'z')
mjhaugsdal 18:ef02a9014491 672 {
mjhaugsdal 18:ef02a9014491 673 stopAll();
mjhaugsdal 19:79600d3509d7 674 }
mjhaugsdal 21:9f3ae352ee63 675 //Make all engines halt and hold still.
mjhaugsdal 19:79600d3509d7 676 if (m_cmd == 'x')
mjhaugsdal 19:79600d3509d7 677 {
mjhaugsdal 18:ef02a9014491 678 }
mjhaugsdal 25:321b970eb3ff 679
mjhaugsdal 25:321b970eb3ff 680 //Rest mode. Make the engines ignore pushbuttons.
mjhaugsdal 25:321b970eb3ff 681 /*
mjhaugsdal 25:321b970eb3ff 682 if(m_cmd == 'r')
mjhaugsdal 25:321b970eb3ff 683 {
mjhaugsdal 25:321b970eb3ff 684 rest = true;
mjhaugsdal 25:321b970eb3ff 685 //Step motors to rest position
mjhaugsdal 25:321b970eb3ff 686
mjhaugsdal 25:321b970eb3ff 687
mjhaugsdal 25:321b970eb3ff 688 }
mjhaugsdal 25:321b970eb3ff 689 */
mjhaugsdal 19:79600d3509d7 690
mjhaugsdal 21:9f3ae352ee63 691 //Controlling each motor seperately.
mjhaugsdal 25:321b970eb3ff 692 //ENGINE 3
mjhaugsdal 24:24c91a6ae6ba 693 if(m_cmd == '7' && steps3 > -242)
mjhaugsdal 19:79600d3509d7 694 {
mjhaugsdal 25:321b970eb3ff 695 rest = false;
mjhaugsdal 22:f6e328f7bd28 696
mjhaugsdal 24:24c91a6ae6ba 697 steps3 --;
mjhaugsdal 24:24c91a6ae6ba 698 stepEngine3Left();
mjhaugsdal 24:24c91a6ae6ba 699 }
mjhaugsdal 24:24c91a6ae6ba 700 else if (m_cmd == '9' && steps3 < 242)
mjhaugsdal 24:24c91a6ae6ba 701 {
mjhaugsdal 25:321b970eb3ff 702 rest = false;
mjhaugsdal 23:ad08a8eabc24 703 stepEngine3Right();
mjhaugsdal 24:24c91a6ae6ba 704 steps3 ++;
mjhaugsdal 19:79600d3509d7 705 }
mjhaugsdal 19:79600d3509d7 706 //ENGINE 2
mjhaugsdal 24:24c91a6ae6ba 707 else if (m_cmd == '4' && steps2 > -242)
mjhaugsdal 19:79600d3509d7 708 {
mjhaugsdal 25:321b970eb3ff 709 rest = false;
mjhaugsdal 24:24c91a6ae6ba 710 steps2 --;
mjhaugsdal 19:79600d3509d7 711 stepEngine2Left();
mjhaugsdal 19:79600d3509d7 712 }
mjhaugsdal 24:24c91a6ae6ba 713 else if (m_cmd == '6' && steps2 < 242)
mjhaugsdal 19:79600d3509d7 714 {
mjhaugsdal 25:321b970eb3ff 715 rest = false;
mjhaugsdal 24:24c91a6ae6ba 716 steps2 ++;
mjhaugsdal 19:79600d3509d7 717 stepEngine2Right();
mjhaugsdal 19:79600d3509d7 718 }
mjhaugsdal 19:79600d3509d7 719 //ENGINE 3
mjhaugsdal 24:24c91a6ae6ba 720 else if (m_cmd == '1' && steps1 > -242)
mjhaugsdal 19:79600d3509d7 721 {
mjhaugsdal 25:321b970eb3ff 722 rest = false;
mjhaugsdal 24:24c91a6ae6ba 723 steps1--;
mjhaugsdal 23:ad08a8eabc24 724 stepEngine1Left();
mjhaugsdal 19:79600d3509d7 725 }
mjhaugsdal 24:24c91a6ae6ba 726 else if (m_cmd == '3' && steps1 < 242)
mjhaugsdal 19:79600d3509d7 727 {
mjhaugsdal 25:321b970eb3ff 728 rest = false;
mjhaugsdal 24:24c91a6ae6ba 729 steps1++;
mjhaugsdal 23:ad08a8eabc24 730 stepEngine1Right();
mjhaugsdal 19:79600d3509d7 731 }
mjhaugsdal 23:ad08a8eabc24 732
mjhaugsdal 25:321b970eb3ff 733
mjhaugsdal 25:321b970eb3ff 734
mjhaugsdal 23:ad08a8eabc24 735
mjhaugsdal 25:321b970eb3ff 736 //Thread::wait(10);
mjhaugsdal 25:321b970eb3ff 737 //pc.printf("%d", steps2);
mjhaugsdal 5:fdc7a779d525 738
mjhaugsdal 4:4d655fdae399 739
mjhaugsdal 24:24c91a6ae6ba 740 } //END WHILE TRUE
mjhaugsdal 1:a3287196a9b3 741
mjhaugsdal 3:15be78948724 742 } //END Main
mjhaugsdal 25:321b970eb3ff 743
mjhaugsdal 25:321b970eb3ff 744
mjhaugsdal 25:321b970eb3ff 745
mjhaugsdal 25:321b970eb3ff 746
mjhaugsdal 25:321b970eb3ff 747
mjhaugsdal 25:321b970eb3ff 748
mjhaugsdal 25:321b970eb3ff 749
mjhaugsdal 25:321b970eb3ff 750
mjhaugsdal 25:321b970eb3ff 751
mjhaugsdal 25:321b970eb3ff 752
mjhaugsdal 25:321b970eb3ff 753
mjhaugsdal 25:321b970eb3ff 754
mjhaugsdal 25:321b970eb3ff 755