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@25:321b970eb3ff, 2016-05-30 (annotated)

Committer:
mjhaugsdal
Date:
Mon May 30 08:16:42 2016 +0000
Revision:
25:321b970eb3ff
Parent:
24:24c91a6ae6ba
Child:
26:57c62b925064
Made boolean rest variable. Made rest mode "r"

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