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@24:24c91a6ae6ba, 2016-05-11 (annotated)

Committer:
mjhaugsdal
Date:
Wed May 11 19:39:46 2016 +0000
Revision:
24:24c91a6ae6ba
Parent:
23:ad08a8eabc24
Child:
25:321b970eb3ff
Added boot-up routine and step-counters.

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 23:ad08a8eabc24 6 PinDetect pb(A5);
rlanghbv 0:bd78e433db61 7
mjhaugsdal 1:a3287196a9b3 8 //Analog Pins
mjhaugsdal 5:fdc7a779d525 9 //First engine
mjhaugsdal 5:fdc7a779d525 10 DigitalOut IN1(A0);
mjhaugsdal 5:fdc7a779d525 11 DigitalOut IN2(A1);
mjhaugsdal 5:fdc7a779d525 12 DigitalOut IN3(A2);
mjhaugsdal 5:fdc7a779d525 13 DigitalOut IN4(A3);
mjhaugsdal 5:fdc7a779d525 14 //Second engine
mjhaugsdal 6:ac5d277bd497 15 DigitalOut IN5(D3);
mjhaugsdal 6:ac5d277bd497 16 DigitalOut IN6(D4);
mjhaugsdal 6:ac5d277bd497 17 DigitalOut IN7(D5);
mjhaugsdal 6:ac5d277bd497 18 DigitalOut IN8(D6);
mjhaugsdal 16:a3555918c025 19 //Third Engine
mjhaugsdal 16:a3555918c025 20 DigitalOut IN9(D8);
mjhaugsdal 16:a3555918c025 21 DigitalOut IN10(D9);
mjhaugsdal 16:a3555918c025 22 DigitalOut IN11(D10);
mjhaugsdal 16:a3555918c025 23 DigitalOut IN12(D11);
mjhaugsdal 5:fdc7a779d525 24
rlanghbv 0:bd78e433db61 25
mjhaugsdal 24:24c91a6ae6ba 26 static int speed = 1500;
mjhaugsdal 3:15be78948724 27 static char m_cmd = 'x';
mjhaugsdal 23:ad08a8eabc24 28 static bool e1 = true;
mjhaugsdal 23:ad08a8eabc24 29 static bool e2 = true;
mjhaugsdal 23:ad08a8eabc24 30 static bool e3 = true;
rlanghbv 0:bd78e433db61 31
mjhaugsdal 24:24c91a6ae6ba 32 static int steps1 = 0;
mjhaugsdal 24:24c91a6ae6ba 33 static int steps2 = 0;
mjhaugsdal 24:24c91a6ae6ba 34 static int steps3 = 0;
mjhaugsdal 24:24c91a6ae6ba 35
mjhaugsdal 24:24c91a6ae6ba 36
mjhaugsdal 21:9f3ae352ee63 37 //Methods to set pins to control direction.
mjhaugsdal 17:ff0021c4dcdc 38 void stepAllRight()
rlanghbv 0:bd78e433db61 39 {
mjhaugsdal 6:ac5d277bd497 40 //engine 1
rlanghbv 0:bd78e433db61 41 IN1=0;
rlanghbv 0:bd78e433db61 42 IN2=1;
rlanghbv 0:bd78e433db61 43 IN3=0;
rlanghbv 0:bd78e433db61 44 IN4=1;
mjhaugsdal 6:ac5d277bd497 45 //engine 2
mjhaugsdal 6:ac5d277bd497 46 IN5=0;
mjhaugsdal 6:ac5d277bd497 47 IN6=1;
mjhaugsdal 6:ac5d277bd497 48 IN7=0;
mjhaugsdal 6:ac5d277bd497 49 IN8=1;
mjhaugsdal 17:ff0021c4dcdc 50 //engine 3
mjhaugsdal 17:ff0021c4dcdc 51 IN9=0;
mjhaugsdal 17:ff0021c4dcdc 52 IN10=1;
mjhaugsdal 17:ff0021c4dcdc 53 IN11=0;
mjhaugsdal 17:ff0021c4dcdc 54 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 55 wait_us(speed); //legg som global variabel "fart"
mjhaugsdal 6:ac5d277bd497 56 //engine 1
rlanghbv 0:bd78e433db61 57 IN1=0;
rlanghbv 0:bd78e433db61 58 IN2=1;
rlanghbv 0:bd78e433db61 59 IN3=1;
rlanghbv 0:bd78e433db61 60 IN4=0;
mjhaugsdal 6:ac5d277bd497 61 //engine 2
mjhaugsdal 6:ac5d277bd497 62 IN5=0;
mjhaugsdal 6:ac5d277bd497 63 IN6=1;
mjhaugsdal 6:ac5d277bd497 64 IN7=1;
mjhaugsdal 6:ac5d277bd497 65 IN8=0;
mjhaugsdal 17:ff0021c4dcdc 66 //engine 3
mjhaugsdal 17:ff0021c4dcdc 67 IN9=0;
mjhaugsdal 17:ff0021c4dcdc 68 IN10=1;
mjhaugsdal 17:ff0021c4dcdc 69 IN11=1;
mjhaugsdal 17:ff0021c4dcdc 70 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 71 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 72 //engine 1
rlanghbv 0:bd78e433db61 73 IN1=1;
rlanghbv 0:bd78e433db61 74 IN2=0;
rlanghbv 0:bd78e433db61 75 IN3=1;
rlanghbv 0:bd78e433db61 76 IN4=0;
mjhaugsdal 6:ac5d277bd497 77 //engine 2
mjhaugsdal 6:ac5d277bd497 78 IN5=1;
mjhaugsdal 6:ac5d277bd497 79 IN6=0;
mjhaugsdal 6:ac5d277bd497 80 IN7=1;
mjhaugsdal 6:ac5d277bd497 81 IN8=0;
mjhaugsdal 17:ff0021c4dcdc 82 //engine 3
mjhaugsdal 17:ff0021c4dcdc 83 IN9=1;
mjhaugsdal 17:ff0021c4dcdc 84 IN10=0;
mjhaugsdal 17:ff0021c4dcdc 85 IN11=1;
mjhaugsdal 17:ff0021c4dcdc 86 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 87 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 88 //engine 1
rlanghbv 0:bd78e433db61 89 IN1=1;
rlanghbv 0:bd78e433db61 90 IN2=0;
rlanghbv 0:bd78e433db61 91 IN3=0;
rlanghbv 0:bd78e433db61 92 IN4=1;
mjhaugsdal 6:ac5d277bd497 93 //engine 2
mjhaugsdal 6:ac5d277bd497 94 IN5=1;
mjhaugsdal 6:ac5d277bd497 95 IN6=0;
mjhaugsdal 6:ac5d277bd497 96 IN7=0;
mjhaugsdal 6:ac5d277bd497 97 IN8=1;
mjhaugsdal 17:ff0021c4dcdc 98 //engine 3
mjhaugsdal 17:ff0021c4dcdc 99 IN9=1;
mjhaugsdal 17:ff0021c4dcdc 100 IN10=0;
mjhaugsdal 17:ff0021c4dcdc 101 IN11=0;
mjhaugsdal 17:ff0021c4dcdc 102 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 103 wait_us(speed);
rlanghbv 0:bd78e433db61 104 }
mjhaugsdal 17:ff0021c4dcdc 105 void stepAllLeft()
rlanghbv 0:bd78e433db61 106 {
mjhaugsdal 6:ac5d277bd497 107 //engine 1
rlanghbv 0:bd78e433db61 108 IN1=1;
rlanghbv 0:bd78e433db61 109 IN2=0;
rlanghbv 0:bd78e433db61 110 IN3=0;
rlanghbv 0:bd78e433db61 111 IN4=1;
mjhaugsdal 6:ac5d277bd497 112 //engine 2
mjhaugsdal 6:ac5d277bd497 113 IN5=1;
mjhaugsdal 6:ac5d277bd497 114 IN6=0;
mjhaugsdal 6:ac5d277bd497 115 IN7=0;
mjhaugsdal 6:ac5d277bd497 116 IN8=1;
mjhaugsdal 17:ff0021c4dcdc 117 //engine 3
mjhaugsdal 17:ff0021c4dcdc 118 IN9=1;
mjhaugsdal 17:ff0021c4dcdc 119 IN10=0;
mjhaugsdal 17:ff0021c4dcdc 120 IN11=0;
mjhaugsdal 17:ff0021c4dcdc 121 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 122 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 123 //engine 1
rlanghbv 0:bd78e433db61 124 IN1=1;
rlanghbv 0:bd78e433db61 125 IN2=0;
rlanghbv 0:bd78e433db61 126 IN3=1;
rlanghbv 0:bd78e433db61 127 IN4=0;
mjhaugsdal 6:ac5d277bd497 128 //engine 2
mjhaugsdal 6:ac5d277bd497 129 IN5=1;
mjhaugsdal 6:ac5d277bd497 130 IN6=0;
mjhaugsdal 6:ac5d277bd497 131 IN7=1;
mjhaugsdal 6:ac5d277bd497 132 IN8=0;
mjhaugsdal 17:ff0021c4dcdc 133 //engine 3
mjhaugsdal 17:ff0021c4dcdc 134 IN9=1;
mjhaugsdal 17:ff0021c4dcdc 135 IN10=0;
mjhaugsdal 17:ff0021c4dcdc 136 IN11=1;
mjhaugsdal 17:ff0021c4dcdc 137 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 138 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 139 //engine 1
rlanghbv 0:bd78e433db61 140 IN1=0;
rlanghbv 0:bd78e433db61 141 IN2=1;
rlanghbv 0:bd78e433db61 142 IN3=1;
rlanghbv 0:bd78e433db61 143 IN4=0;
mjhaugsdal 6:ac5d277bd497 144 //engine 2
mjhaugsdal 6:ac5d277bd497 145 IN5=0;
mjhaugsdal 6:ac5d277bd497 146 IN6=1;
mjhaugsdal 6:ac5d277bd497 147 IN7=1;
mjhaugsdal 6:ac5d277bd497 148 IN8=0;
mjhaugsdal 17:ff0021c4dcdc 149 //engine 3
mjhaugsdal 17:ff0021c4dcdc 150 IN9=0;
mjhaugsdal 17:ff0021c4dcdc 151 IN10=1;
mjhaugsdal 17:ff0021c4dcdc 152 IN11=1;
mjhaugsdal 17:ff0021c4dcdc 153 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 154 wait_us(speed);
mjhaugsdal 6:ac5d277bd497 155 //engine 1
rlanghbv 0:bd78e433db61 156 IN1=0;
rlanghbv 0:bd78e433db61 157 IN2=1;
rlanghbv 0:bd78e433db61 158 IN3=0;
rlanghbv 0:bd78e433db61 159 IN4=1;
mjhaugsdal 6:ac5d277bd497 160 //engine 2
mjhaugsdal 6:ac5d277bd497 161 IN5=0;
mjhaugsdal 6:ac5d277bd497 162 IN6=1;
mjhaugsdal 6:ac5d277bd497 163 IN7=0;
mjhaugsdal 6:ac5d277bd497 164 IN8=1;
mjhaugsdal 17:ff0021c4dcdc 165 //engine 3
mjhaugsdal 17:ff0021c4dcdc 166 IN9=0;
mjhaugsdal 17:ff0021c4dcdc 167 IN10=1;
mjhaugsdal 17:ff0021c4dcdc 168 IN11=0;
mjhaugsdal 17:ff0021c4dcdc 169 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 170 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 171 }
mjhaugsdal 17:ff0021c4dcdc 172
mjhaugsdal 17:ff0021c4dcdc 173 void stepEngine1Left()
mjhaugsdal 17:ff0021c4dcdc 174 {
mjhaugsdal 17:ff0021c4dcdc 175 //engine 1
mjhaugsdal 17:ff0021c4dcdc 176 IN1=1;
mjhaugsdal 17:ff0021c4dcdc 177 IN2=0;
mjhaugsdal 17:ff0021c4dcdc 178 IN3=0;
mjhaugsdal 17:ff0021c4dcdc 179 IN4=1;
mjhaugsdal 24:24c91a6ae6ba 180 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 181 IN1=1;
mjhaugsdal 17:ff0021c4dcdc 182 IN2=0;
mjhaugsdal 17:ff0021c4dcdc 183 IN3=1;
mjhaugsdal 17:ff0021c4dcdc 184 IN4=0;
mjhaugsdal 24:24c91a6ae6ba 185 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 186 IN1=0;
mjhaugsdal 17:ff0021c4dcdc 187 IN2=1;
mjhaugsdal 17:ff0021c4dcdc 188 IN3=1;
mjhaugsdal 17:ff0021c4dcdc 189 IN4=0;
mjhaugsdal 24:24c91a6ae6ba 190 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 191 IN1=0;
mjhaugsdal 17:ff0021c4dcdc 192 IN2=1;
mjhaugsdal 17:ff0021c4dcdc 193 IN3=0;
mjhaugsdal 17:ff0021c4dcdc 194 IN4=1;
mjhaugsdal 24:24c91a6ae6ba 195 wait_us(speed);
rlanghbv 0:bd78e433db61 196 }
rlanghbv 0:bd78e433db61 197
mjhaugsdal 17:ff0021c4dcdc 198 void stepEngine1Right()
mjhaugsdal 17:ff0021c4dcdc 199 {
mjhaugsdal 17:ff0021c4dcdc 200 //engine 1
mjhaugsdal 17:ff0021c4dcdc 201 IN1=0;
mjhaugsdal 17:ff0021c4dcdc 202 IN2=1;
mjhaugsdal 17:ff0021c4dcdc 203 IN3=0;
mjhaugsdal 17:ff0021c4dcdc 204 IN4=1;
mjhaugsdal 24:24c91a6ae6ba 205 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 206 IN1=0;
mjhaugsdal 17:ff0021c4dcdc 207 IN2=1;
mjhaugsdal 17:ff0021c4dcdc 208 IN3=1;
mjhaugsdal 17:ff0021c4dcdc 209 IN4=0;
mjhaugsdal 24:24c91a6ae6ba 210 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 211 IN1=1;
mjhaugsdal 17:ff0021c4dcdc 212 IN2=0;
mjhaugsdal 17:ff0021c4dcdc 213 IN3=1;
mjhaugsdal 17:ff0021c4dcdc 214 IN4=0;
mjhaugsdal 24:24c91a6ae6ba 215 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 216 IN1=1;
mjhaugsdal 17:ff0021c4dcdc 217 IN2=0;
mjhaugsdal 17:ff0021c4dcdc 218 IN3=0;
mjhaugsdal 17:ff0021c4dcdc 219 IN4=1;
mjhaugsdal 24:24c91a6ae6ba 220 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 221 }
mjhaugsdal 17:ff0021c4dcdc 222
mjhaugsdal 17:ff0021c4dcdc 223 void stepEngine2Right()
mjhaugsdal 17:ff0021c4dcdc 224 {
mjhaugsdal 17:ff0021c4dcdc 225 //engine 1
mjhaugsdal 17:ff0021c4dcdc 226 IN5=0;
mjhaugsdal 17:ff0021c4dcdc 227 IN6=1;
mjhaugsdal 17:ff0021c4dcdc 228 IN7=0;
mjhaugsdal 17:ff0021c4dcdc 229 IN8=1;
mjhaugsdal 24:24c91a6ae6ba 230 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 231 IN5=0;
mjhaugsdal 17:ff0021c4dcdc 232 IN6=1;
mjhaugsdal 17:ff0021c4dcdc 233 IN7=1;
mjhaugsdal 17:ff0021c4dcdc 234 IN8=0;
mjhaugsdal 24:24c91a6ae6ba 235 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 236 IN5=1;
mjhaugsdal 17:ff0021c4dcdc 237 IN6=0;
mjhaugsdal 17:ff0021c4dcdc 238 IN7=1;
mjhaugsdal 17:ff0021c4dcdc 239 IN8=0;
mjhaugsdal 24:24c91a6ae6ba 240 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 241 IN5=1;
mjhaugsdal 17:ff0021c4dcdc 242 IN6=0;
mjhaugsdal 17:ff0021c4dcdc 243 IN7=0;
mjhaugsdal 17:ff0021c4dcdc 244 IN8=1;
mjhaugsdal 24:24c91a6ae6ba 245 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 246 }
mjhaugsdal 17:ff0021c4dcdc 247
mjhaugsdal 17:ff0021c4dcdc 248 void stepEngine2Left()
mjhaugsdal 17:ff0021c4dcdc 249 {
mjhaugsdal 17:ff0021c4dcdc 250 //engine 1
mjhaugsdal 17:ff0021c4dcdc 251 IN5=1;
mjhaugsdal 17:ff0021c4dcdc 252 IN6=0;
mjhaugsdal 17:ff0021c4dcdc 253 IN7=0;
mjhaugsdal 17:ff0021c4dcdc 254 IN8=1;
mjhaugsdal 24:24c91a6ae6ba 255 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 256 IN5=1;
mjhaugsdal 17:ff0021c4dcdc 257 IN6=0;
mjhaugsdal 17:ff0021c4dcdc 258 IN7=1;
mjhaugsdal 17:ff0021c4dcdc 259 IN8=0;
mjhaugsdal 24:24c91a6ae6ba 260 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 261 IN5=0;
mjhaugsdal 17:ff0021c4dcdc 262 IN6=1;
mjhaugsdal 17:ff0021c4dcdc 263 IN7=1;
mjhaugsdal 17:ff0021c4dcdc 264 IN8=0;
mjhaugsdal 24:24c91a6ae6ba 265 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 266 IN5=0;
mjhaugsdal 17:ff0021c4dcdc 267 IN6=1;
mjhaugsdal 17:ff0021c4dcdc 268 IN7=0;
mjhaugsdal 17:ff0021c4dcdc 269 IN8=1;
mjhaugsdal 24:24c91a6ae6ba 270 wait_us(speed);
mjhaugsdal 17:ff0021c4dcdc 271 }
mjhaugsdal 17:ff0021c4dcdc 272
mjhaugsdal 19:79600d3509d7 273 void stepEngine3Right()
mjhaugsdal 19:79600d3509d7 274 {
mjhaugsdal 19:79600d3509d7 275 //engine 1
mjhaugsdal 19:79600d3509d7 276 IN9=0;
mjhaugsdal 19:79600d3509d7 277 IN10=1;
mjhaugsdal 19:79600d3509d7 278 IN11=0;
mjhaugsdal 19:79600d3509d7 279 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 280 wait_us(speed);
mjhaugsdal 19:79600d3509d7 281 IN9=0;
mjhaugsdal 19:79600d3509d7 282 IN10=1;
mjhaugsdal 19:79600d3509d7 283 IN11=1;
mjhaugsdal 19:79600d3509d7 284 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 285 wait_us(speed);
mjhaugsdal 19:79600d3509d7 286 IN9=1;
mjhaugsdal 19:79600d3509d7 287 IN10=0;
mjhaugsdal 19:79600d3509d7 288 IN11=1;
mjhaugsdal 19:79600d3509d7 289 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 290 wait_us(speed);
mjhaugsdal 19:79600d3509d7 291 IN9=1;
mjhaugsdal 19:79600d3509d7 292 IN10=0;
mjhaugsdal 19:79600d3509d7 293 IN11=0;
mjhaugsdal 19:79600d3509d7 294 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 295 wait_us(speed);
mjhaugsdal 19:79600d3509d7 296 }
mjhaugsdal 19:79600d3509d7 297
mjhaugsdal 19:79600d3509d7 298 void stepEngine3Left()
mjhaugsdal 19:79600d3509d7 299 {
mjhaugsdal 19:79600d3509d7 300 //engine 1
mjhaugsdal 19:79600d3509d7 301 IN9=1;
mjhaugsdal 19:79600d3509d7 302 IN10=0;
mjhaugsdal 19:79600d3509d7 303 IN11=0;
mjhaugsdal 19:79600d3509d7 304 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 305 wait_us(speed);
mjhaugsdal 19:79600d3509d7 306 IN9=1;
mjhaugsdal 19:79600d3509d7 307 IN10=0;
mjhaugsdal 19:79600d3509d7 308 IN11=1;
mjhaugsdal 19:79600d3509d7 309 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 310 wait_us(speed);
mjhaugsdal 19:79600d3509d7 311 IN9=0;
mjhaugsdal 19:79600d3509d7 312 IN10=1;
mjhaugsdal 19:79600d3509d7 313 IN11=1;
mjhaugsdal 19:79600d3509d7 314 IN12=0;
mjhaugsdal 24:24c91a6ae6ba 315 wait_us(speed);
mjhaugsdal 19:79600d3509d7 316 IN9=0;
mjhaugsdal 19:79600d3509d7 317 IN10=1;
mjhaugsdal 19:79600d3509d7 318 IN11=0;
mjhaugsdal 19:79600d3509d7 319 IN12=1;
mjhaugsdal 24:24c91a6ae6ba 320 wait_us(speed);
mjhaugsdal 19:79600d3509d7 321 }
mjhaugsdal 19:79600d3509d7 322
mjhaugsdal 23:ad08a8eabc24 323
mjhaugsdal 23:ad08a8eabc24 324 void step1()
mjhaugsdal 23:ad08a8eabc24 325 {
mjhaugsdal 23:ad08a8eabc24 326 //engine 1
mjhaugsdal 23:ad08a8eabc24 327 IN1=1;
mjhaugsdal 23:ad08a8eabc24 328 IN2=0;
mjhaugsdal 23:ad08a8eabc24 329 IN3=0;
mjhaugsdal 23:ad08a8eabc24 330 IN4=1;
mjhaugsdal 23:ad08a8eabc24 331 }
mjhaugsdal 23:ad08a8eabc24 332
mjhaugsdal 23:ad08a8eabc24 333 void step2()
mjhaugsdal 23:ad08a8eabc24 334 {
mjhaugsdal 23:ad08a8eabc24 335 //engine 1
mjhaugsdal 23:ad08a8eabc24 336 IN1=0;
mjhaugsdal 23:ad08a8eabc24 337 IN2=1;
mjhaugsdal 23:ad08a8eabc24 338 IN3=0;
mjhaugsdal 23:ad08a8eabc24 339 IN4=1;
mjhaugsdal 23:ad08a8eabc24 340 }
mjhaugsdal 23:ad08a8eabc24 341
mjhaugsdal 23:ad08a8eabc24 342 void wait()
mjhaugsdal 23:ad08a8eabc24 343 {
mjhaugsdal 24:24c91a6ae6ba 344 wait_us(speed);
mjhaugsdal 23:ad08a8eabc24 345
mjhaugsdal 23:ad08a8eabc24 346 }
mjhaugsdal 23:ad08a8eabc24 347
mjhaugsdal 21:9f3ae352ee63 348 //Method to release all engines.
mjhaugsdal 18:ef02a9014491 349 void stopAll()
mjhaugsdal 18:ef02a9014491 350 {
mjhaugsdal 18:ef02a9014491 351 IN1=0;
mjhaugsdal 18:ef02a9014491 352 IN2=0;
mjhaugsdal 18:ef02a9014491 353 IN3=0;
mjhaugsdal 18:ef02a9014491 354 IN4=0;
mjhaugsdal 18:ef02a9014491 355 IN5=0;
mjhaugsdal 18:ef02a9014491 356 IN6=0;
mjhaugsdal 18:ef02a9014491 357 IN7=0;
mjhaugsdal 18:ef02a9014491 358 IN8=0;
mjhaugsdal 18:ef02a9014491 359 IN9=0;
mjhaugsdal 18:ef02a9014491 360 IN10=0;
mjhaugsdal 18:ef02a9014491 361 IN11=0;
mjhaugsdal 18:ef02a9014491 362 IN12=0;
mjhaugsdal 18:ef02a9014491 363 }
mjhaugsdal 18:ef02a9014491 364
mjhaugsdal 21:9f3ae352ee63 365 //Methods to release single engines.
mjhaugsdal 20:d90bd1718078 366 void stopE1()
mjhaugsdal 20:d90bd1718078 367 {
mjhaugsdal 20:d90bd1718078 368 IN1=0;
mjhaugsdal 20:d90bd1718078 369 IN2=0;
mjhaugsdal 20:d90bd1718078 370 IN3=0;
mjhaugsdal 20:d90bd1718078 371 IN4=0;
mjhaugsdal 20:d90bd1718078 372
mjhaugsdal 20:d90bd1718078 373 }
mjhaugsdal 20:d90bd1718078 374 void stopE2()
mjhaugsdal 20:d90bd1718078 375 {
mjhaugsdal 20:d90bd1718078 376 IN5=0;
mjhaugsdal 20:d90bd1718078 377 IN6=0;
mjhaugsdal 20:d90bd1718078 378 IN7=0;
mjhaugsdal 20:d90bd1718078 379 IN8=0;
mjhaugsdal 20:d90bd1718078 380
mjhaugsdal 20:d90bd1718078 381 }
mjhaugsdal 20:d90bd1718078 382 void stopE3()
mjhaugsdal 20:d90bd1718078 383 {
mjhaugsdal 20:d90bd1718078 384 IN9=0;
mjhaugsdal 20:d90bd1718078 385 IN10=0;
mjhaugsdal 20:d90bd1718078 386 IN11=0;
mjhaugsdal 20:d90bd1718078 387 IN12=0;
mjhaugsdal 20:d90bd1718078 388
mjhaugsdal 20:d90bd1718078 389 }
mjhaugsdal 20:d90bd1718078 390
mjhaugsdal 17:ff0021c4dcdc 391
mjhaugsdal 17:ff0021c4dcdc 392
mjhaugsdal 19:79600d3509d7 393
mjhaugsdal 19:79600d3509d7 394
mjhaugsdal 23:ad08a8eabc24 395
mjhaugsdal 23:ad08a8eabc24 396
mjhaugsdal 1:a3287196a9b3 397 void input(void const *args)
mjhaugsdal 1:a3287196a9b3 398 {
mjhaugsdal 1:a3287196a9b3 399 while(true)
mjhaugsdal 1:a3287196a9b3 400 {
mjhaugsdal 3:15be78948724 401
rlanghbv 2:25bcc26f7a5b 402 if(pc.readable())
mjhaugsdal 3:15be78948724 403 { m_cmd=pc.getc();
mjhaugsdal 3:15be78948724 404
mjhaugsdal 17:ff0021c4dcdc 405 }
mjhaugsdal 17:ff0021c4dcdc 406
mjhaugsdal 23:ad08a8eabc24 407 Thread::wait(100);
mjhaugsdal 5:fdc7a779d525 408 //pc.printf("%d", steps);
mjhaugsdal 23:ad08a8eabc24 409
mjhaugsdal 1:a3287196a9b3 410 }
mjhaugsdal 1:a3287196a9b3 411 }
rlanghbv 0:bd78e433db61 412
mjhaugsdal 23:ad08a8eabc24 413 //SPECIFY WHICH BUTTON IS PRESSED. NEED ONE CALLBACK FOR EACH BUTTON.
mjhaugsdal 23:ad08a8eabc24 414 void pb_hit_callback (void)
mjhaugsdal 23:ad08a8eabc24 415 {
mjhaugsdal 24:24c91a6ae6ba 416 e1 = false;
mjhaugsdal 24:24c91a6ae6ba 417 //m_cmd = 'x';
mjhaugsdal 23:ad08a8eabc24 418 /*
mjhaugsdal 23:ad08a8eabc24 419
mjhaugsdal 23:ad08a8eabc24 420
mjhaugsdal 23:ad08a8eabc24 421 */
mjhaugsdal 23:ad08a8eabc24 422
mjhaugsdal 23:ad08a8eabc24 423
mjhaugsdal 23:ad08a8eabc24 424 }
mjhaugsdal 23:ad08a8eabc24 425
mjhaugsdal 23:ad08a8eabc24 426
rlanghbv 0:bd78e433db61 427 int main()
rlanghbv 0:bd78e433db61 428 {
mjhaugsdal 24:24c91a6ae6ba 429
mjhaugsdal 23:ad08a8eabc24 430 pb.mode(PullUp);
mjhaugsdal 23:ad08a8eabc24 431 pb.attach_deasserted(&pb_hit_callback);
mjhaugsdal 23:ad08a8eabc24 432 pb.setSampleFrequency();
mjhaugsdal 23:ad08a8eabc24 433
mjhaugsdal 1:a3287196a9b3 434 //Thread 1 has constant feed from usb
rlanghbv 2:25bcc26f7a5b 435 Thread t1(input);
mjhaugsdal 22:f6e328f7bd28 436 //static char global_direction;
mjhaugsdal 3:15be78948724 437 printf("Started");
mjhaugsdal 23:ad08a8eabc24 438
mjhaugsdal 3:15be78948724 439
mjhaugsdal 22:f6e328f7bd28 440 /*
mjhaugsdal 22:f6e328f7bd28 441
mjhaugsdal 22:f6e328f7bd28 442 BOOT UP AND SYNCHRONIZE
mjhaugsdal 23:ad08a8eabc24 443 while (true)
mjhaugsdal 23:ad08a8eabc24 444 {
mjhaugsdal 23:ad08a8eabc24 445 should move one engine at a time. If not, alot of work has to be done in if/else statements as well as new engine methods..
mjhaugsdal 23:ad08a8eabc24 446 while(moving bottom part 180 degrees to one side / steps < 0)
mjhaugsdal 23:ad08a8eabc24 447 {
mjhaugsdal 23:ad08a8eabc24 448 step bottom engine in one direction.
mjhaugsdal 23:ad08a8eabc24 449 callback will fire and stop the engine
mjhaugsdal 23:ad08a8eabc24 450 }
mjhaugsdal 23:ad08a8eabc24 451 while(moving middle part 180 degrees to one side / steps < 0)
mjhaugsdal 23:ad08a8eabc24 452 {
mjhaugsdal 23:ad08a8eabc24 453 step middle engine in one direction
mjhaugsdal 23:ad08a8eabc24 454 callback fires and stops the engine
mjhaugsdal 23:ad08a8eabc24 455 }
mjhaugsdal 23:ad08a8eabc24 456 while(moving middle part 180 degrees to one side / steps < 0)
mjhaugsdal 23:ad08a8eabc24 457 {
mjhaugsdal 23:ad08a8eabc24 458 step middle engine in one direction
mjhaugsdal 23:ad08a8eabc24 459 callback fires and stops the engine
mjhaugsdal 23:ad08a8eabc24 460 }
mjhaugsdal 23:ad08a8eabc24 461 //Break the while loop (FALSE)
mjhaugsdal 23:ad08a8eabc24 462 break;
mjhaugsdal 23:ad08a8eabc24 463
mjhaugsdal 23:ad08a8eabc24 464
mjhaugsdal 23:ad08a8eabc24 465 }
mjhaugsdal 22:f6e328f7bd28 466
mjhaugsdal 22:f6e328f7bd28 467 */
mjhaugsdal 22:f6e328f7bd28 468
mjhaugsdal 24:24c91a6ae6ba 469 // Move motor in one direction
mjhaugsdal 24:24c91a6ae6ba 470 while (true)
mjhaugsdal 24:24c91a6ae6ba 471 {
mjhaugsdal 24:24c91a6ae6ba 472 speed = 2000;
mjhaugsdal 24:24c91a6ae6ba 473 stepEngine1Left();
mjhaugsdal 24:24c91a6ae6ba 474 if (e1 == false)
mjhaugsdal 24:24c91a6ae6ba 475 {
mjhaugsdal 24:24c91a6ae6ba 476 Thread::wait(1000);
mjhaugsdal 24:24c91a6ae6ba 477 int stepsToCenter = 242;
mjhaugsdal 24:24c91a6ae6ba 478
mjhaugsdal 24:24c91a6ae6ba 479 while (stepsToCenter >= 0)
mjhaugsdal 24:24c91a6ae6ba 480 {
mjhaugsdal 24:24c91a6ae6ba 481 stepEngine1Right();
mjhaugsdal 24:24c91a6ae6ba 482 stepsToCenter --;
mjhaugsdal 24:24c91a6ae6ba 483 }
mjhaugsdal 24:24c91a6ae6ba 484 break;
mjhaugsdal 24:24c91a6ae6ba 485 }//end if
mjhaugsdal 24:24c91a6ae6ba 486
mjhaugsdal 24:24c91a6ae6ba 487 }//end while
mjhaugsdal 24:24c91a6ae6ba 488 /*
mjhaugsdal 24:24c91a6ae6ba 489 while (true)
mjhaugsdal 24:24c91a6ae6ba 490 {
mjhaugsdal 24:24c91a6ae6ba 491 stepEngine2Right();
mjhaugsdal 24:24c91a6ae6ba 492 if (e2 == false)
mjhaugsdal 24:24c91a6ae6ba 493 {
mjhaugsdal 24:24c91a6ae6ba 494 Thread::wait(1000);
mjhaugsdal 24:24c91a6ae6ba 495 int stepsToCenter = 242;
mjhaugsdal 24:24c91a6ae6ba 496
mjhaugsdal 24:24c91a6ae6ba 497 while (stepsToCenter >= 0)
mjhaugsdal 24:24c91a6ae6ba 498 {
mjhaugsdal 24:24c91a6ae6ba 499 stepEngine2Left();
mjhaugsdal 24:24c91a6ae6ba 500 stepsToCenter --;
mjhaugsdal 24:24c91a6ae6ba 501 }
mjhaugsdal 24:24c91a6ae6ba 502 break;
mjhaugsdal 24:24c91a6ae6ba 503 }//end if
mjhaugsdal 24:24c91a6ae6ba 504 }
mjhaugsdal 24:24c91a6ae6ba 505 while (true)
mjhaugsdal 24:24c91a6ae6ba 506 {
mjhaugsdal 24:24c91a6ae6ba 507 stepEngine2Left();
mjhaugsdal 24:24c91a6ae6ba 508 if (e3 == false)
mjhaugsdal 24:24c91a6ae6ba 509 {
mjhaugsdal 24:24c91a6ae6ba 510 Thread::wait(1000);
mjhaugsdal 24:24c91a6ae6ba 511 int stepsToCenter = 242;
mjhaugsdal 24:24c91a6ae6ba 512
mjhaugsdal 24:24c91a6ae6ba 513 while (stepsToCenter >= 0)
mjhaugsdal 24:24c91a6ae6ba 514 {
mjhaugsdal 24:24c91a6ae6ba 515 stepEngine3Right();
mjhaugsdal 24:24c91a6ae6ba 516 stepsToCenter --;
mjhaugsdal 24:24c91a6ae6ba 517 }
mjhaugsdal 24:24c91a6ae6ba 518 break;
mjhaugsdal 24:24c91a6ae6ba 519 }//end if
mjhaugsdal 24:24c91a6ae6ba 520 }
mjhaugsdal 24:24c91a6ae6ba 521
mjhaugsdal 24:24c91a6ae6ba 522 */
mjhaugsdal 24:24c91a6ae6ba 523
mjhaugsdal 1:a3287196a9b3 524 while (true)
mjhaugsdal 22:f6e328f7bd28 525 {
mjhaugsdal 24:24c91a6ae6ba 526 speed = 1500;
mjhaugsdal 24:24c91a6ae6ba 527
mjhaugsdal 24:24c91a6ae6ba 528
mjhaugsdal 22:f6e328f7bd28 529 if(m_cmd == 'a')
mjhaugsdal 22:f6e328f7bd28 530 {
mjhaugsdal 22:f6e328f7bd28 531 stepAllRight();
mjhaugsdal 22:f6e328f7bd28 532 }
mjhaugsdal 22:f6e328f7bd28 533
mjhaugsdal 22:f6e328f7bd28 534 else if (m_cmd == 'b')
mjhaugsdal 22:f6e328f7bd28 535 {
mjhaugsdal 22:f6e328f7bd28 536 stepAllLeft();
mjhaugsdal 22:f6e328f7bd28 537
mjhaugsdal 22:f6e328f7bd28 538 }
mjhaugsdal 3:15be78948724 539
mjhaugsdal 19:79600d3509d7 540 /*
mjhaugsdal 5:fdc7a779d525 541 else if (m_cmd == 'k')
mjhaugsdal 1:a3287196a9b3 542 {
mjhaugsdal 6:ac5d277bd497 543 int steps = 242;
mjhaugsdal 5:fdc7a779d525 544
mjhaugsdal 5:fdc7a779d525 545
mjhaugsdal 6:ac5d277bd497 546 //Steps = 242 gives a 60' rotation to the left.
mjhaugsdal 5:fdc7a779d525 547 while (steps >= 0)
mjhaugsdal 5:fdc7a779d525 548 {
mjhaugsdal 17:ff0021c4dcdc 549 stepAllLeft();
mjhaugsdal 5:fdc7a779d525 550 steps--;
mjhaugsdal 5:fdc7a779d525 551 }
mjhaugsdal 5:fdc7a779d525 552 m_cmd = 'x';
mjhaugsdal 24:24c91a6ae6ba 553 } */
mjhaugsdal 24:24c91a6ae6ba 554
mjhaugsdal 21:9f3ae352ee63 555
mjhaugsdal 21:9f3ae352ee63 556 //Release all motors / Set all pins to 0
mjhaugsdal 18:ef02a9014491 557 if (m_cmd == 'z')
mjhaugsdal 18:ef02a9014491 558 {
mjhaugsdal 18:ef02a9014491 559 stopAll();
mjhaugsdal 19:79600d3509d7 560 }
mjhaugsdal 21:9f3ae352ee63 561 //Make all engines halt and hold still.
mjhaugsdal 19:79600d3509d7 562 if (m_cmd == 'x')
mjhaugsdal 19:79600d3509d7 563 {
mjhaugsdal 18:ef02a9014491 564 }
mjhaugsdal 19:79600d3509d7 565
mjhaugsdal 21:9f3ae352ee63 566 //Controlling each motor seperately.
mjhaugsdal 19:79600d3509d7 567 //ENGINE 1
mjhaugsdal 24:24c91a6ae6ba 568 if(m_cmd == '7' && steps3 > -242)
mjhaugsdal 19:79600d3509d7 569 {
mjhaugsdal 22:f6e328f7bd28 570
mjhaugsdal 24:24c91a6ae6ba 571 steps3 --;
mjhaugsdal 24:24c91a6ae6ba 572 stepEngine3Left();
mjhaugsdal 24:24c91a6ae6ba 573 }
mjhaugsdal 24:24c91a6ae6ba 574 else if (m_cmd == '9' && steps3 < 242)
mjhaugsdal 24:24c91a6ae6ba 575 {
mjhaugsdal 23:ad08a8eabc24 576 stepEngine3Right();
mjhaugsdal 24:24c91a6ae6ba 577 steps3 ++;
mjhaugsdal 19:79600d3509d7 578 }
mjhaugsdal 19:79600d3509d7 579 //ENGINE 2
mjhaugsdal 24:24c91a6ae6ba 580 else if (m_cmd == '4' && steps2 > -242)
mjhaugsdal 19:79600d3509d7 581 {
mjhaugsdal 24:24c91a6ae6ba 582 steps2 --;
mjhaugsdal 19:79600d3509d7 583 stepEngine2Left();
mjhaugsdal 19:79600d3509d7 584 }
mjhaugsdal 24:24c91a6ae6ba 585 else if (m_cmd == '6' && steps2 < 242)
mjhaugsdal 19:79600d3509d7 586 {
mjhaugsdal 24:24c91a6ae6ba 587 steps2 ++;
mjhaugsdal 19:79600d3509d7 588 stepEngine2Right();
mjhaugsdal 19:79600d3509d7 589 }
mjhaugsdal 19:79600d3509d7 590 //ENGINE 3
mjhaugsdal 24:24c91a6ae6ba 591 else if (m_cmd == '1' && steps1 > -242)
mjhaugsdal 19:79600d3509d7 592 {
mjhaugsdal 24:24c91a6ae6ba 593 steps1--;
mjhaugsdal 23:ad08a8eabc24 594 stepEngine1Left();
mjhaugsdal 19:79600d3509d7 595 }
mjhaugsdal 24:24c91a6ae6ba 596 else if (m_cmd == '3' && steps1 < 242)
mjhaugsdal 19:79600d3509d7 597 {
mjhaugsdal 24:24c91a6ae6ba 598 steps1++;
mjhaugsdal 23:ad08a8eabc24 599 stepEngine1Right();
mjhaugsdal 19:79600d3509d7 600 }
mjhaugsdal 23:ad08a8eabc24 601
mjhaugsdal 23:ad08a8eabc24 602 else if (m_cmd == 'y')
mjhaugsdal 23:ad08a8eabc24 603 {
mjhaugsdal 23:ad08a8eabc24 604 int a = 121;
mjhaugsdal 23:ad08a8eabc24 605 int b = 60;
mjhaugsdal 23:ad08a8eabc24 606 int c = 60;
mjhaugsdal 23:ad08a8eabc24 607
mjhaugsdal 23:ad08a8eabc24 608 while (a >= 0)
mjhaugsdal 23:ad08a8eabc24 609 {
mjhaugsdal 23:ad08a8eabc24 610 stepEngine1Left();
mjhaugsdal 23:ad08a8eabc24 611 a--;
mjhaugsdal 23:ad08a8eabc24 612 }
mjhaugsdal 23:ad08a8eabc24 613 while (b >= 0)
mjhaugsdal 23:ad08a8eabc24 614 {
mjhaugsdal 23:ad08a8eabc24 615 stepEngine2Right();
mjhaugsdal 23:ad08a8eabc24 616 b--;
mjhaugsdal 23:ad08a8eabc24 617 }
mjhaugsdal 23:ad08a8eabc24 618 while (c >= 0)
mjhaugsdal 23:ad08a8eabc24 619 {
mjhaugsdal 23:ad08a8eabc24 620 stepEngine3Left();
mjhaugsdal 23:ad08a8eabc24 621 c--;
mjhaugsdal 23:ad08a8eabc24 622 }
mjhaugsdal 23:ad08a8eabc24 623
mjhaugsdal 23:ad08a8eabc24 624 m_cmd = 'x';
mjhaugsdal 23:ad08a8eabc24 625
mjhaugsdal 23:ad08a8eabc24 626 }
mjhaugsdal 23:ad08a8eabc24 627 /*
mjhaugsdal 23:ad08a8eabc24 628 new_pb = pb;
mjhaugsdal 23:ad08a8eabc24 629 if (new_pb == '1')
mjhaugsdal 23:ad08a8eabc24 630 {
mjhaugsdal 23:ad08a8eabc24 631 m_cmd ='x';
mjhaugsdal 23:ad08a8eabc24 632 }
mjhaugsdal 23:ad08a8eabc24 633 */
mjhaugsdal 24:24c91a6ae6ba 634 //Thread::wait(100);
mjhaugsdal 24:24c91a6ae6ba 635 //pc.printf("%d", steps3);
mjhaugsdal 5:fdc7a779d525 636
mjhaugsdal 4:4d655fdae399 637
mjhaugsdal 24:24c91a6ae6ba 638 } //END WHILE TRUE
mjhaugsdal 1:a3287196a9b3 639
mjhaugsdal 3:15be78948724 640 } //END Main