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@30:fac5b5624fe3, 2016-07-07 (annotated)

Committer:
mjhaugsdal
Date:
Thu Jul 07 11:13:35 2016 +0000
Revision:
30:fac5b5624fe3
Parent:
29:00f839aad30e 
07072016

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