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@28:dac262b7ab32, 2016-06-02 (annotated)

Committer:
mjhaugsdal
Date:
Thu Jun 02 07:53:41 2016 +0000
Revision:
28:dac262b7ab32
Parent:
27:35c962e8e95b
Child:
29:00f839aad30e
made a change to make the motors loosen after running and holding

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;
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 27:35c962e8e95b 457 /*
mjhaugsdal 27:35c962e8e95b 458 if (m_cmd == 'x')
mjhaugsdal 27:35c962e8e95b 459 {
mjhaugsdal 27:35c962e8e95b 460 while(speed < 7000)
mjhaugsdal 27:35c962e8e95b 461 {
mjhaugsdal 27:35c962e8e95b 462 //Thread::wait(1);
mjhaugsdal 27:35c962e8e95b 463 speed ++;
mjhaugsdal 27:35c962e8e95b 464 }
mjhaugsdal 27:35c962e8e95b 465 speed = 2000;
mjhaugsdal 27:35c962e8e95b 466 cmd = 'x';
mjhaugsdal 27:35c962e8e95b 467 break;
mjhaugsdal 27:35c962e8e95b 468 }
mjhaugsdal 27:35c962e8e95b 469 */
mjhaugsdal 27:35c962e8e95b 470
mjhaugsdal 27:35c962e8e95b 471
mjhaugsdal 23:ad08a8eabc24 472
mjhaugsdal 1:a3287196a9b3 473 }
mjhaugsdal 1:a3287196a9b3 474 }
rlanghbv 0:bd78e433db61 475
mjhaugsdal 25:321b970eb3ff 476
mjhaugsdal 27:35c962e8e95b 477 //MOTOR 1 LEFT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 478 void pb1_hit_callback (void)
mjhaugsdal 23:ad08a8eabc24 479 {
mjhaugsdal 25:321b970eb3ff 480 if(rest == false)
mjhaugsdal 25:321b970eb3ff 481 {
mjhaugsdal 25:321b970eb3ff 482 e1 = false;
mjhaugsdal 25:321b970eb3ff 483
mjhaugsdal 27:35c962e8e95b 484 speed = 2000;
mjhaugsdal 27:35c962e8e95b 485 Thread::wait(1000);
mjhaugsdal 27:35c962e8e95b 486 int stepsToCenter = 242;
mjhaugsdal 27:35c962e8e95b 487
mjhaugsdal 27:35c962e8e95b 488 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 489 {
mjhaugsdal 27:35c962e8e95b 490 stepEngine1Right();
mjhaugsdal 27:35c962e8e95b 491 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 492 }
mjhaugsdal 27:35c962e8e95b 493 steps1 = 0;
mjhaugsdal 27:35c962e8e95b 494 //break;
mjhaugsdal 27:35c962e8e95b 495 //end if
mjhaugsdal 27:35c962e8e95b 496 m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 497 }
mjhaugsdal 25:321b970eb3ff 498 }
mjhaugsdal 25:321b970eb3ff 499
mjhaugsdal 27:35c962e8e95b 500 //MOTOR 1 RIGHT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 501 void pb2_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 502 {
mjhaugsdal 25:321b970eb3ff 503 if(rest == false)
mjhaugsdal 25:321b970eb3ff 504 {
mjhaugsdal 25:321b970eb3ff 505 e1 = false;
mjhaugsdal 25:321b970eb3ff 506 //m_cmd = 'x';
mjhaugsdal 23:ad08a8eabc24 507
mjhaugsdal 27:35c962e8e95b 508 speed = 2000;
mjhaugsdal 27:35c962e8e95b 509 Thread::wait(1000);
mjhaugsdal 27:35c962e8e95b 510 int stepsToCenter = 242;
mjhaugsdal 27:35c962e8e95b 511
mjhaugsdal 27:35c962e8e95b 512 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 513 {
mjhaugsdal 27:35c962e8e95b 514 stepEngine1Left();
mjhaugsdal 27:35c962e8e95b 515 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 516 }
mjhaugsdal 27:35c962e8e95b 517 steps1 = 0;
mjhaugsdal 27:35c962e8e95b 518 m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 519 }
mjhaugsdal 25:321b970eb3ff 520 }
mjhaugsdal 25:321b970eb3ff 521 //MOTOR 2 LEFT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 522 void pb3_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 523 {
mjhaugsdal 25:321b970eb3ff 524 if(rest == false)
mjhaugsdal 25:321b970eb3ff 525 {
mjhaugsdal 25:321b970eb3ff 526 e2 = false;
mjhaugsdal 25:321b970eb3ff 527 //m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 528 speed = 2000;
mjhaugsdal 25:321b970eb3ff 529 Thread::wait(1000);
mjhaugsdal 25:321b970eb3ff 530 int stepsToCenter = 242;
mjhaugsdal 25:321b970eb3ff 531
mjhaugsdal 25:321b970eb3ff 532 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 533 {
mjhaugsdal 27:35c962e8e95b 534 stepEngine2Right();
mjhaugsdal 25:321b970eb3ff 535 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 536 }
mjhaugsdal 27:35c962e8e95b 537 steps2 = 0;
mjhaugsdal 27:35c962e8e95b 538 m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 539 }
mjhaugsdal 23:ad08a8eabc24 540
mjhaugsdal 23:ad08a8eabc24 541 }
mjhaugsdal 23:ad08a8eabc24 542
mjhaugsdal 25:321b970eb3ff 543 //MOTOR 2 RIGHT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 544 void pb4_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 545 {
mjhaugsdal 25:321b970eb3ff 546 if(rest == false)
mjhaugsdal 25:321b970eb3ff 547 {
mjhaugsdal 25:321b970eb3ff 548 e2 = false;
mjhaugsdal 25:321b970eb3ff 549 //m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 550 speed = 2000;
mjhaugsdal 25:321b970eb3ff 551 Thread::wait(1000);
mjhaugsdal 25:321b970eb3ff 552 int stepsToCenter = 242;
mjhaugsdal 25:321b970eb3ff 553
mjhaugsdal 25:321b970eb3ff 554 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 555 {
mjhaugsdal 27:35c962e8e95b 556 stepEngine2Left();
mjhaugsdal 25:321b970eb3ff 557 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 558 }
mjhaugsdal 27:35c962e8e95b 559 steps2 = 0;
mjhaugsdal 27:35c962e8e95b 560 m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 561 }
mjhaugsdal 25:321b970eb3ff 562
mjhaugsdal 25:321b970eb3ff 563
mjhaugsdal 25:321b970eb3ff 564 }
mjhaugsdal 25:321b970eb3ff 565
mjhaugsdal 25:321b970eb3ff 566 //MOTOR 3 LEFT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 567 void pb5_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 568 {
mjhaugsdal 25:321b970eb3ff 569 if(rest == false)
mjhaugsdal 25:321b970eb3ff 570 {
mjhaugsdal 25:321b970eb3ff 571 e3 = false;
mjhaugsdal 25:321b970eb3ff 572 //m_cmd = 'x';
mjhaugsdal 27:35c962e8e95b 573 speed = 2500;
mjhaugsdal 25:321b970eb3ff 574 Thread::wait(1000);
mjhaugsdal 27:35c962e8e95b 575 int stepsToCenter = 200;
mjhaugsdal 25:321b970eb3ff 576
mjhaugsdal 25:321b970eb3ff 577 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 578 {
mjhaugsdal 25:321b970eb3ff 579 stepEngine3Left();
mjhaugsdal 25:321b970eb3ff 580 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 581 }
mjhaugsdal 27:35c962e8e95b 582 steps3 = 0;
mjhaugsdal 27:35c962e8e95b 583 m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 584 }
mjhaugsdal 25:321b970eb3ff 585
mjhaugsdal 25:321b970eb3ff 586
mjhaugsdal 25:321b970eb3ff 587 }
mjhaugsdal 25:321b970eb3ff 588 //MOTOR 3 RIGHT SIDE BUTTON
mjhaugsdal 25:321b970eb3ff 589 void pb6_hit_callback (void)
mjhaugsdal 25:321b970eb3ff 590 {
mjhaugsdal 27:35c962e8e95b 591
mjhaugsdal 25:321b970eb3ff 592 if(rest == false)
mjhaugsdal 25:321b970eb3ff 593 {
mjhaugsdal 25:321b970eb3ff 594 e3 = false;
mjhaugsdal 25:321b970eb3ff 595 //m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 596 speed = 2000;
mjhaugsdal 25:321b970eb3ff 597 Thread::wait(1000);
mjhaugsdal 27:35c962e8e95b 598 int stepsToCenter = 280;
mjhaugsdal 25:321b970eb3ff 599
mjhaugsdal 25:321b970eb3ff 600 while (stepsToCenter >= 0)
mjhaugsdal 25:321b970eb3ff 601 {
mjhaugsdal 25:321b970eb3ff 602 stepEngine3Right();
mjhaugsdal 25:321b970eb3ff 603 stepsToCenter --;
mjhaugsdal 25:321b970eb3ff 604 }
mjhaugsdal 27:35c962e8e95b 605 //reset steps and stop engine
mjhaugsdal 27:35c962e8e95b 606 steps3 = 0;
mjhaugsdal 27:35c962e8e95b 607 m_cmd = 'x';
mjhaugsdal 25:321b970eb3ff 608 }
mjhaugsdal 25:321b970eb3ff 609 }
mjhaugsdal 23:ad08a8eabc24 610
rlanghbv 0:bd78e433db61 611 int main()
rlanghbv 0:bd78e433db61 612 {
mjhaugsdal 27:35c962e8e95b 613 int slowDown;
mjhaugsdal 24:24c91a6ae6ba 614
mjhaugsdal 25:321b970eb3ff 615 pb1.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 616 pb2.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 617 pb3.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 618 pb4.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 619 pb5.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 620 pb6.mode(PullUp);
mjhaugsdal 25:321b970eb3ff 621
mjhaugsdal 25:321b970eb3ff 622 //Set up buttons 1 and 2
mjhaugsdal 25:321b970eb3ff 623 pb1.attach_deasserted(&pb1_hit_callback);
mjhaugsdal 25:321b970eb3ff 624 pb1.setSampleFrequency();
mjhaugsdal 25:321b970eb3ff 625 pb2.attach_deasserted(&pb2_hit_callback);
mjhaugsdal 25:321b970eb3ff 626 pb2.setSampleFrequency();
mjhaugsdal 23:ad08a8eabc24 627
mjhaugsdal 1:a3287196a9b3 628 //Thread 1 has constant feed from usb
rlanghbv 2:25bcc26f7a5b 629 Thread t1(input);
mjhaugsdal 22:f6e328f7bd28 630 //static char global_direction;
mjhaugsdal 3:15be78948724 631 printf("Started");
mjhaugsdal 22:f6e328f7bd28 632
mjhaugsdal 24:24c91a6ae6ba 633 // Move motor in one direction
mjhaugsdal 24:24c91a6ae6ba 634 while (true)
mjhaugsdal 24:24c91a6ae6ba 635 {
mjhaugsdal 28:dac262b7ab32 636
mjhaugsdal 25:321b970eb3ff 637
mjhaugsdal 25:321b970eb3ff 638 while(bootStep1 >= 0)
mjhaugsdal 25:321b970eb3ff 639 {
mjhaugsdal 25:321b970eb3ff 640 bootStep1--;
mjhaugsdal 25:321b970eb3ff 641 stepEngine1Right();
mjhaugsdal 25:321b970eb3ff 642 }
mjhaugsdal 24:24c91a6ae6ba 643 stepEngine1Left();
mjhaugsdal 24:24c91a6ae6ba 644 if (e1 == false)
mjhaugsdal 24:24c91a6ae6ba 645 {
mjhaugsdal 24:24c91a6ae6ba 646 Thread::wait(1000);
mjhaugsdal 25:321b970eb3ff 647 break;
mjhaugsdal 24:24c91a6ae6ba 648 }//end if
mjhaugsdal 25:321b970eb3ff 649
mjhaugsdal 24:24c91a6ae6ba 650 }//end while
mjhaugsdal 25:321b970eb3ff 651
mjhaugsdal 25:321b970eb3ff 652 //Set up buttons 3 and 4
mjhaugsdal 25:321b970eb3ff 653 pb3.attach_deasserted(&pb3_hit_callback);
mjhaugsdal 25:321b970eb3ff 654 pb3.setSampleFrequency();
mjhaugsdal 25:321b970eb3ff 655 pb4.attach_deasserted(&pb4_hit_callback);
mjhaugsdal 25:321b970eb3ff 656 pb4.setSampleFrequency();
mjhaugsdal 25:321b970eb3ff 657
mjhaugsdal 24:24c91a6ae6ba 658 while (true)
mjhaugsdal 24:24c91a6ae6ba 659 {
mjhaugsdal 25:321b970eb3ff 660
mjhaugsdal 25:321b970eb3ff 661 speed = 2000;
mjhaugsdal 25:321b970eb3ff 662 while(bootStep2 >= 0)
mjhaugsdal 25:321b970eb3ff 663 {
mjhaugsdal 25:321b970eb3ff 664 bootStep2--;
mjhaugsdal 25:321b970eb3ff 665 stepEngine2Right();
mjhaugsdal 25:321b970eb3ff 666
mjhaugsdal 25:321b970eb3ff 667 }
mjhaugsdal 25:321b970eb3ff 668
mjhaugsdal 25:321b970eb3ff 669 stepEngine2Left();
mjhaugsdal 24:24c91a6ae6ba 670 if (e2 == false)
mjhaugsdal 24:24c91a6ae6ba 671 {
mjhaugsdal 27:35c962e8e95b 672
mjhaugsdal 25:321b970eb3ff 673 Thread::wait(1000);
mjhaugsdal 24:24c91a6ae6ba 674 break;
mjhaugsdal 24:24c91a6ae6ba 675 }//end if
mjhaugsdal 24:24c91a6ae6ba 676 }
mjhaugsdal 25:321b970eb3ff 677
mjhaugsdal 25:321b970eb3ff 678 pb5.attach_deasserted(&pb5_hit_callback);
mjhaugsdal 25:321b970eb3ff 679 pb5.setSampleFrequency();
mjhaugsdal 25:321b970eb3ff 680 pb6.attach_deasserted(&pb6_hit_callback);
mjhaugsdal 25:321b970eb3ff 681 pb6.setSampleFrequency();
mjhaugsdal 24:24c91a6ae6ba 682 while (true)
mjhaugsdal 24:24c91a6ae6ba 683 {
mjhaugsdal 27:35c962e8e95b 684 //speed = 2000;
mjhaugsdal 25:321b970eb3ff 685 while(bootStep3 >= 0)
mjhaugsdal 25:321b970eb3ff 686 {
mjhaugsdal 25:321b970eb3ff 687 bootStep3--;
mjhaugsdal 25:321b970eb3ff 688 stepEngine3Right();
mjhaugsdal 25:321b970eb3ff 689
mjhaugsdal 25:321b970eb3ff 690 }
mjhaugsdal 25:321b970eb3ff 691 stepEngine3Left();
mjhaugsdal 24:24c91a6ae6ba 692 if (e3 == false)
mjhaugsdal 24:24c91a6ae6ba 693 {
mjhaugsdal 27:35c962e8e95b 694 //pc.printf("HALLO");
mjhaugsdal 28:dac262b7ab32 695 m_cmd = "z";
mjhaugsdal 24:24c91a6ae6ba 696 Thread::wait(1000);
mjhaugsdal 24:24c91a6ae6ba 697 break;
mjhaugsdal 24:24c91a6ae6ba 698 }//end if
mjhaugsdal 24:24c91a6ae6ba 699 }
mjhaugsdal 24:24c91a6ae6ba 700
mjhaugsdal 28:dac262b7ab32 701 //normal runtime
mjhaugsdal 1:a3287196a9b3 702 while (true)
mjhaugsdal 22:f6e328f7bd28 703 {
mjhaugsdal 28:dac262b7ab32 704
mjhaugsdal 28:dac262b7ab32 705 //user button to turn off engines
mjhaugsdal 28:dac262b7ab32 706 if(SW==1)
mjhaugsdal 28:dac262b7ab32 707 {
mjhaugsdal 28:dac262b7ab32 708 //put engines to sleep
mjhaugsdal 28:dac262b7ab32 709 m_cmd ="z";
mjhaugsdal 28:dac262b7ab32 710
mjhaugsdal 28:dac262b7ab32 711
mjhaugsdal 28:dac262b7ab32 712 }
mjhaugsdal 28:dac262b7ab32 713 //set the speed. Higher = slower
mjhaugsdal 28:dac262b7ab32 714 speed = 2000;
mjhaugsdal 27:35c962e8e95b 715 //speed = 1500;
mjhaugsdal 24:24c91a6ae6ba 716
mjhaugsdal 27:35c962e8e95b 717 switch (m_cmd)
mjhaugsdal 27:35c962e8e95b 718 {
mjhaugsdal 27:35c962e8e95b 719 case '7':
mjhaugsdal 27:35c962e8e95b 720 cmd = '7';
mjhaugsdal 27:35c962e8e95b 721 break;
mjhaugsdal 27:35c962e8e95b 722
mjhaugsdal 27:35c962e8e95b 723 case '9':
mjhaugsdal 27:35c962e8e95b 724 cmd = '9';
mjhaugsdal 27:35c962e8e95b 725 break;
mjhaugsdal 27:35c962e8e95b 726
mjhaugsdal 27:35c962e8e95b 727 case '4':
mjhaugsdal 27:35c962e8e95b 728 cmd= '4';
mjhaugsdal 27:35c962e8e95b 729 break;
mjhaugsdal 27:35c962e8e95b 730
mjhaugsdal 27:35c962e8e95b 731 case '6':
mjhaugsdal 27:35c962e8e95b 732 cmd = '6';
mjhaugsdal 27:35c962e8e95b 733 break;
mjhaugsdal 27:35c962e8e95b 734
mjhaugsdal 27:35c962e8e95b 735 case '1':
mjhaugsdal 27:35c962e8e95b 736 cmd = '1';
mjhaugsdal 27:35c962e8e95b 737 break;
mjhaugsdal 27:35c962e8e95b 738
mjhaugsdal 27:35c962e8e95b 739 case '3':
mjhaugsdal 27:35c962e8e95b 740 cmd = '3';
mjhaugsdal 27:35c962e8e95b 741 break;
mjhaugsdal 27:35c962e8e95b 742
mjhaugsdal 27:35c962e8e95b 743 case 'x':
mjhaugsdal 27:35c962e8e95b 744 cmd = 'x';
mjhaugsdal 27:35c962e8e95b 745 break;
mjhaugsdal 27:35c962e8e95b 746
mjhaugsdal 27:35c962e8e95b 747 //release motors
mjhaugsdal 27:35c962e8e95b 748 case 'z':
mjhaugsdal 27:35c962e8e95b 749
mjhaugsdal 27:35c962e8e95b 750 //step all motors to the middle
mjhaugsdal 27:35c962e8e95b 751
mjhaugsdal 27:35c962e8e95b 752 while(steps1 > 0)
mjhaugsdal 27:35c962e8e95b 753 {
mjhaugsdal 27:35c962e8e95b 754 stepEngine1Left();
mjhaugsdal 27:35c962e8e95b 755 steps1--;
mjhaugsdal 27:35c962e8e95b 756 }
mjhaugsdal 27:35c962e8e95b 757 while(steps1 < 0)
mjhaugsdal 27:35c962e8e95b 758 {
mjhaugsdal 27:35c962e8e95b 759 stepEngine1Right();
mjhaugsdal 27:35c962e8e95b 760 steps1++;
mjhaugsdal 27:35c962e8e95b 761 }
mjhaugsdal 27:35c962e8e95b 762
mjhaugsdal 27:35c962e8e95b 763 while(steps2 > 0)
mjhaugsdal 27:35c962e8e95b 764 {
mjhaugsdal 27:35c962e8e95b 765 stepEngine2Left();
mjhaugsdal 27:35c962e8e95b 766 steps2--;
mjhaugsdal 27:35c962e8e95b 767 }
mjhaugsdal 27:35c962e8e95b 768 while(steps2 < 0)
mjhaugsdal 27:35c962e8e95b 769 {
mjhaugsdal 27:35c962e8e95b 770 stepEngine2Right();
mjhaugsdal 27:35c962e8e95b 771 steps2++;
mjhaugsdal 27:35c962e8e95b 772 }
mjhaugsdal 27:35c962e8e95b 773
mjhaugsdal 27:35c962e8e95b 774 while(steps3 > 0)
mjhaugsdal 27:35c962e8e95b 775 {
mjhaugsdal 27:35c962e8e95b 776 stepEngine3Left();
mjhaugsdal 27:35c962e8e95b 777 steps3--;
mjhaugsdal 27:35c962e8e95b 778 }
mjhaugsdal 27:35c962e8e95b 779 while(steps3 < 0)
mjhaugsdal 27:35c962e8e95b 780 {
mjhaugsdal 27:35c962e8e95b 781 stepEngine3Right();
mjhaugsdal 27:35c962e8e95b 782 steps3++;
mjhaugsdal 27:35c962e8e95b 783 }
mjhaugsdal 27:35c962e8e95b 784
mjhaugsdal 27:35c962e8e95b 785 cmd = 'z';
mjhaugsdal 27:35c962e8e95b 786
mjhaugsdal 27:35c962e8e95b 787
mjhaugsdal 27:35c962e8e95b 788 break;
mjhaugsdal 27:35c962e8e95b 789
mjhaugsdal 27:35c962e8e95b 790 }
mjhaugsdal 27:35c962e8e95b 791
mjhaugsdal 21:9f3ae352ee63 792 //Release all motors / Set all pins to 0
mjhaugsdal 27:35c962e8e95b 793 if (cmd == 'z')
mjhaugsdal 18:ef02a9014491 794 {
mjhaugsdal 18:ef02a9014491 795 stopAll();
mjhaugsdal 19:79600d3509d7 796 }
mjhaugsdal 21:9f3ae352ee63 797 //Make all engines halt and hold still.
mjhaugsdal 27:35c962e8e95b 798 if (cmd == 'x')
mjhaugsdal 19:79600d3509d7 799 {
mjhaugsdal 27:35c962e8e95b 800 hold();
mjhaugsdal 28:dac262b7ab32 801 Thread::wait(1000);
mjhaugsdal 28:dac262b7ab32 802 m_cmd = 'z';
mjhaugsdal 27:35c962e8e95b 803
mjhaugsdal 18:ef02a9014491 804 }
mjhaugsdal 25:321b970eb3ff 805
mjhaugsdal 25:321b970eb3ff 806 //Rest mode. Make the engines ignore pushbuttons.
mjhaugsdal 25:321b970eb3ff 807 /*
mjhaugsdal 25:321b970eb3ff 808 if(m_cmd == 'r')
mjhaugsdal 25:321b970eb3ff 809 {
mjhaugsdal 25:321b970eb3ff 810 rest = true;
mjhaugsdal 25:321b970eb3ff 811 //Step motors to rest position
mjhaugsdal 25:321b970eb3ff 812
mjhaugsdal 25:321b970eb3ff 813 }
mjhaugsdal 25:321b970eb3ff 814 */
mjhaugsdal 19:79600d3509d7 815
mjhaugsdal 21:9f3ae352ee63 816 //Controlling each motor seperately.
mjhaugsdal 25:321b970eb3ff 817 //ENGINE 3
mjhaugsdal 27:35c962e8e95b 818 if(cmd == '7' && steps3 > -200)
mjhaugsdal 19:79600d3509d7 819 {
mjhaugsdal 25:321b970eb3ff 820 rest = false;
mjhaugsdal 22:f6e328f7bd28 821
mjhaugsdal 24:24c91a6ae6ba 822 steps3 --;
mjhaugsdal 24:24c91a6ae6ba 823 stepEngine3Left();
mjhaugsdal 27:35c962e8e95b 824
mjhaugsdal 24:24c91a6ae6ba 825 }
mjhaugsdal 27:35c962e8e95b 826 else if (cmd == '9' && steps3 < 280)
mjhaugsdal 24:24c91a6ae6ba 827 {
mjhaugsdal 25:321b970eb3ff 828 rest = false;
mjhaugsdal 23:ad08a8eabc24 829 stepEngine3Right();
mjhaugsdal 24:24c91a6ae6ba 830 steps3 ++;
mjhaugsdal 19:79600d3509d7 831 }
mjhaugsdal 19:79600d3509d7 832 //ENGINE 2
mjhaugsdal 27:35c962e8e95b 833 else if (cmd == '4' && steps2 > -242)
mjhaugsdal 19:79600d3509d7 834 {
mjhaugsdal 25:321b970eb3ff 835 rest = false;
mjhaugsdal 24:24c91a6ae6ba 836 steps2 --;
mjhaugsdal 19:79600d3509d7 837 stepEngine2Left();
mjhaugsdal 19:79600d3509d7 838 }
mjhaugsdal 27:35c962e8e95b 839 else if (cmd == '6' && steps2 < 242)
mjhaugsdal 19:79600d3509d7 840 {
mjhaugsdal 25:321b970eb3ff 841 rest = false;
mjhaugsdal 24:24c91a6ae6ba 842 steps2 ++;
mjhaugsdal 19:79600d3509d7 843 stepEngine2Right();
mjhaugsdal 19:79600d3509d7 844 }
mjhaugsdal 19:79600d3509d7 845 //ENGINE 3
mjhaugsdal 27:35c962e8e95b 846 else if (cmd == '1' && steps1 > -242)
mjhaugsdal 19:79600d3509d7 847 {
mjhaugsdal 25:321b970eb3ff 848 rest = false;
mjhaugsdal 24:24c91a6ae6ba 849 steps1--;
mjhaugsdal 23:ad08a8eabc24 850 stepEngine1Left();
mjhaugsdal 19:79600d3509d7 851 }
mjhaugsdal 27:35c962e8e95b 852 else if (cmd == '3' && steps1 < 242)
mjhaugsdal 19:79600d3509d7 853 {
mjhaugsdal 25:321b970eb3ff 854 rest = false;
mjhaugsdal 24:24c91a6ae6ba 855 steps1++;
mjhaugsdal 23:ad08a8eabc24 856 stepEngine1Right();
mjhaugsdal 19:79600d3509d7 857 }
mjhaugsdal 23:ad08a8eabc24 858
mjhaugsdal 27:35c962e8e95b 859 if (m_cmd == 'y')
mjhaugsdal 27:35c962e8e95b 860 {
mjhaugsdal 27:35c962e8e95b 861 int step = 100;
mjhaugsdal 27:35c962e8e95b 862 while (step >= 0)
mjhaugsdal 27:35c962e8e95b 863 {
mjhaugsdal 27:35c962e8e95b 864 step--;
mjhaugsdal 27:35c962e8e95b 865 step2Left3Right();
mjhaugsdal 27:35c962e8e95b 866 }
mjhaugsdal 27:35c962e8e95b 867 m_cmd = 'x';
mjhaugsdal 27:35c962e8e95b 868
mjhaugsdal 27:35c962e8e95b 869 }
mjhaugsdal 27:35c962e8e95b 870
mjhaugsdal 27:35c962e8e95b 871 if (m_cmd == 't')
mjhaugsdal 27:35c962e8e95b 872 {
mjhaugsdal 27:35c962e8e95b 873 int step1, step2, step3;
mjhaugsdal 27:35c962e8e95b 874 step1 = 100;
mjhaugsdal 27:35c962e8e95b 875 step2 = 60;
mjhaugsdal 27:35c962e8e95b 876 step3 = 70;
mjhaugsdal 27:35c962e8e95b 877
mjhaugsdal 27:35c962e8e95b 878 while (step1 > 0)
mjhaugsdal 27:35c962e8e95b 879 {
mjhaugsdal 27:35c962e8e95b 880 stepEngine1Left();
mjhaugsdal 27:35c962e8e95b 881 step1--;
mjhaugsdal 27:35c962e8e95b 882 //global stepcounter
mjhaugsdal 27:35c962e8e95b 883 steps1--;
mjhaugsdal 27:35c962e8e95b 884 }
mjhaugsdal 27:35c962e8e95b 885 while(step2 > 0)
mjhaugsdal 27:35c962e8e95b 886 {
mjhaugsdal 27:35c962e8e95b 887 stepEngine2Right();
mjhaugsdal 27:35c962e8e95b 888 step2--;
mjhaugsdal 27:35c962e8e95b 889 //global stepcounter
mjhaugsdal 27:35c962e8e95b 890 steps2++;
mjhaugsdal 27:35c962e8e95b 891 }
mjhaugsdal 27:35c962e8e95b 892 while(step3 > 0)
mjhaugsdal 27:35c962e8e95b 893 {
mjhaugsdal 27:35c962e8e95b 894 stepEngine3Left();
mjhaugsdal 27:35c962e8e95b 895 step3--;
mjhaugsdal 27:35c962e8e95b 896 //global stepcounter
mjhaugsdal 27:35c962e8e95b 897 steps3--;
mjhaugsdal 27:35c962e8e95b 898 }
mjhaugsdal 27:35c962e8e95b 899
mjhaugsdal 28:dac262b7ab32 900 //m_cmd = 'x';
mjhaugsdal 27:35c962e8e95b 901
mjhaugsdal 27:35c962e8e95b 902 }
mjhaugsdal 25:321b970eb3ff 903
mjhaugsdal 25:321b970eb3ff 904
mjhaugsdal 23:ad08a8eabc24 905
mjhaugsdal 25:321b970eb3ff 906 //Thread::wait(10);
mjhaugsdal 25:321b970eb3ff 907 //pc.printf("%d", steps2);
mjhaugsdal 5:fdc7a779d525 908
mjhaugsdal 4:4d655fdae399 909
mjhaugsdal 24:24c91a6ae6ba 910 } //END WHILE TRUE
mjhaugsdal 1:a3287196a9b3 911
mjhaugsdal 3:15be78948724 912 } //END Main