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@27:35c962e8e95b, 2016-06-01 (annotated)

Committer:
mjhaugsdal
Date:
Wed Jun 01 17:40:54 2016 +0000
Revision:
27:35c962e8e95b
Parent:
26:57c62b925064
Child:
28:dac262b7ab32
Added some simultaneous rotation capabilities

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