USN Robotics / Mbed 2 deprecated Robot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   PinDetect Ulrasonic mbed-rtos mbed WTV020SD_Sound_Breakout_Library

Fork of MC by Robot Bachelor

main.cpp@32:5163db06f6cc, 2016-09-01 (annotated)

Committer:
azadmehr
Date:
Thu Sep 01 08:55:14 2016 +0000
Revision:
32:5163db06f6cc
Parent:
31:358d4c032d76
Child:
33:6f500188e36d
ultralyd sensor added

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