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@29:00f839aad30e, 2016-07-07 (annotated)

Committer:
mjhaugsdal
Date:
Thu Jul 07 08:53:21 2016 +0000
Revision:
29:00f839aad30e
Parent:
28:dac262b7ab32
Child:
30:fac5b5624fe3
Testing better sync

Who changed what in which revision?

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