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

Committer:
mjhaugsdal
Date:
Tue Apr 26 13:28:52 2016 +0000
Revision:
11:ef54754100ea
Parent:
10:17a16ceb376b
Child:
12:de8381ca371d
Put each engine in threads

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"
rlanghbv 0:bd78e433db61 3
rlanghbv 2:25bcc26f7a5b 4 Serial pc(USBTX, USBRX); // tx, rx
rlanghbv 0:bd78e433db61 5
mjhaugsdal 1:a3287196a9b3 6 //Analog Pins
mjhaugsdal 7:ba0caeb296bb 7 //First engine 1-4
mjhaugsdal 5:fdc7a779d525 8 DigitalOut IN1(A0);
mjhaugsdal 5:fdc7a779d525 9 DigitalOut IN2(A1);
mjhaugsdal 5:fdc7a779d525 10 DigitalOut IN3(A2);
mjhaugsdal 5:fdc7a779d525 11 DigitalOut IN4(A3);
mjhaugsdal 7:ba0caeb296bb 12 //Second engine 5-8
mjhaugsdal 10:17a16ceb376b 13
mjhaugsdal 6:ac5d277bd497 14 DigitalOut IN5(D3);
mjhaugsdal 6:ac5d277bd497 15 DigitalOut IN6(D4);
mjhaugsdal 6:ac5d277bd497 16 DigitalOut IN7(D5);
mjhaugsdal 11:ef54754100ea 17 DigitalOut IN8(D6);
mjhaugsdal 10:17a16ceb376b 18 /*
mjhaugsdal 7:ba0caeb296bb 19 //Third engine 9-12
mjhaugsdal 7:ba0caeb296bb 20 DigitalOut IN9(D8);
mjhaugsdal 7:ba0caeb296bb 21 DigitalOut IN10(D9);
mjhaugsdal 7:ba0caeb296bb 22 DigitalOut IN11(D10);
mjhaugsdal 9:1636083fce43 23 DigitalOut IN12(D11); */
mjhaugsdal 5:fdc7a779d525 24
rlanghbv 0:bd78e433db61 25
mjhaugsdal 1:a3287196a9b3 26 static int fart = 1000;
mjhaugsdal 3:15be78948724 27 static char m_cmd = 'x';
mjhaugsdal 9:1636083fce43 28 /*
mjhaugsdal 7:ba0caeb296bb 29 void step4Right3()
rlanghbv 0:bd78e433db61 30 {
mjhaugsdal 7:ba0caeb296bb 31
mjhaugsdal 7:ba0caeb296bb 32 //engine 2
mjhaugsdal 7:ba0caeb296bb 33 IN9=0;
mjhaugsdal 7:ba0caeb296bb 34 IN10=1;
mjhaugsdal 7:ba0caeb296bb 35 IN11=0;
mjhaugsdal 7:ba0caeb296bb 36 IN12=1;
mjhaugsdal 7:ba0caeb296bb 37 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 38 //engine 2
mjhaugsdal 7:ba0caeb296bb 39 IN9=0;
mjhaugsdal 7:ba0caeb296bb 40 IN10=1;
mjhaugsdal 7:ba0caeb296bb 41 IN11=1;
mjhaugsdal 7:ba0caeb296bb 42 IN12=0;
mjhaugsdal 7:ba0caeb296bb 43 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 44 //engine 2
mjhaugsdal 7:ba0caeb296bb 45 IN9=1;
mjhaugsdal 7:ba0caeb296bb 46 IN10=0;
mjhaugsdal 7:ba0caeb296bb 47 IN11=1;
mjhaugsdal 7:ba0caeb296bb 48 IN12=0;
mjhaugsdal 7:ba0caeb296bb 49 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 50 //engine 2
mjhaugsdal 7:ba0caeb296bb 51 IN9=1;
mjhaugsdal 7:ba0caeb296bb 52 IN10=0;
mjhaugsdal 7:ba0caeb296bb 53 IN11=0;
mjhaugsdal 7:ba0caeb296bb 54 IN12=1;
mjhaugsdal 7:ba0caeb296bb 55 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 56 }
mjhaugsdal 10:17a16ceb376b 57 */
mjhaugsdal 7:ba0caeb296bb 58 void step4Right2()
mjhaugsdal 7:ba0caeb296bb 59 {
mjhaugsdal 7:ba0caeb296bb 60
mjhaugsdal 7:ba0caeb296bb 61 //engine 2
mjhaugsdal 6:ac5d277bd497 62 IN5=0;
mjhaugsdal 6:ac5d277bd497 63 IN6=1;
mjhaugsdal 6:ac5d277bd497 64 IN7=0;
mjhaugsdal 6:ac5d277bd497 65 IN8=1;
mjhaugsdal 7:ba0caeb296bb 66 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 67 //engine 2
mjhaugsdal 6:ac5d277bd497 68 IN5=0;
mjhaugsdal 6:ac5d277bd497 69 IN6=1;
mjhaugsdal 6:ac5d277bd497 70 IN7=1;
mjhaugsdal 6:ac5d277bd497 71 IN8=0;
mjhaugsdal 1:a3287196a9b3 72 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 73 //engine 2
mjhaugsdal 6:ac5d277bd497 74 IN5=1;
mjhaugsdal 6:ac5d277bd497 75 IN6=0;
mjhaugsdal 6:ac5d277bd497 76 IN7=1;
mjhaugsdal 6:ac5d277bd497 77 IN8=0;
mjhaugsdal 1:a3287196a9b3 78 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 79 //engine 2
mjhaugsdal 6:ac5d277bd497 80 IN5=1;
mjhaugsdal 6:ac5d277bd497 81 IN6=0;
mjhaugsdal 6:ac5d277bd497 82 IN7=0;
mjhaugsdal 6:ac5d277bd497 83 IN8=1;
mjhaugsdal 1:a3287196a9b3 84 wait_us(fart);
rlanghbv 0:bd78e433db61 85 }
mjhaugsdal 10:17a16ceb376b 86
mjhaugsdal 7:ba0caeb296bb 87 void step4Right1()
rlanghbv 0:bd78e433db61 88 {
mjhaugsdal 6:ac5d277bd497 89 //engine 1
mjhaugsdal 7:ba0caeb296bb 90 IN1=0;
mjhaugsdal 7:ba0caeb296bb 91 IN2=1;
rlanghbv 0:bd78e433db61 92 IN3=0;
rlanghbv 0:bd78e433db61 93 IN4=1;
mjhaugsdal 7:ba0caeb296bb 94 wait_us(fart); //legg som global variabel "fart"
mjhaugsdal 7:ba0caeb296bb 95 //engine 1
mjhaugsdal 7:ba0caeb296bb 96 IN1=0;
mjhaugsdal 7:ba0caeb296bb 97 IN2=1;
mjhaugsdal 7:ba0caeb296bb 98 IN3=1;
mjhaugsdal 7:ba0caeb296bb 99 IN4=0;
mjhaugsdal 1:a3287196a9b3 100 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 101 //engine 1
rlanghbv 0:bd78e433db61 102 IN1=1;
rlanghbv 0:bd78e433db61 103 IN2=0;
rlanghbv 0:bd78e433db61 104 IN3=1;
rlanghbv 0:bd78e433db61 105 IN4=0;
mjhaugsdal 7:ba0caeb296bb 106 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 107 //engine 1
mjhaugsdal 7:ba0caeb296bb 108 IN1=1;
mjhaugsdal 7:ba0caeb296bb 109 IN2=0;
mjhaugsdal 7:ba0caeb296bb 110 IN3=0;
mjhaugsdal 7:ba0caeb296bb 111 IN4=1;
mjhaugsdal 7:ba0caeb296bb 112 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 113 }
mjhaugsdal 7:ba0caeb296bb 114
mjhaugsdal 9:1636083fce43 115 /*
mjhaugsdal 7:ba0caeb296bb 116 void step4Left3()
mjhaugsdal 7:ba0caeb296bb 117 {
mjhaugsdal 7:ba0caeb296bb 118 //engine 2
mjhaugsdal 7:ba0caeb296bb 119 IN9=1;
mjhaugsdal 7:ba0caeb296bb 120 IN10=0;
mjhaugsdal 7:ba0caeb296bb 121 IN11=0;
mjhaugsdal 7:ba0caeb296bb 122 IN12=1;
mjhaugsdal 7:ba0caeb296bb 123 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 124
mjhaugsdal 7:ba0caeb296bb 125
mjhaugsdal 7:ba0caeb296bb 126 //engine 2
mjhaugsdal 7:ba0caeb296bb 127 IN9=1;
mjhaugsdal 7:ba0caeb296bb 128 IN10=0;
mjhaugsdal 7:ba0caeb296bb 129 IN11=1;
mjhaugsdal 7:ba0caeb296bb 130 IN12=0;
mjhaugsdal 7:ba0caeb296bb 131 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 132
mjhaugsdal 7:ba0caeb296bb 133 //engine 2
mjhaugsdal 7:ba0caeb296bb 134 IN9=0;
mjhaugsdal 7:ba0caeb296bb 135 IN10=1;
mjhaugsdal 7:ba0caeb296bb 136 IN11=1;
mjhaugsdal 7:ba0caeb296bb 137 IN12=0;
mjhaugsdal 7:ba0caeb296bb 138 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 139
mjhaugsdal 7:ba0caeb296bb 140 //engine 2
mjhaugsdal 7:ba0caeb296bb 141 IN9=0;
mjhaugsdal 7:ba0caeb296bb 142 IN10=1;
mjhaugsdal 7:ba0caeb296bb 143 IN11=0;
mjhaugsdal 7:ba0caeb296bb 144 IN12=1;
mjhaugsdal 7:ba0caeb296bb 145 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 146
mjhaugsdal 7:ba0caeb296bb 147 }
mjhaugsdal 7:ba0caeb296bb 148
mjhaugsdal 10:17a16ceb376b 149 */
mjhaugsdal 7:ba0caeb296bb 150 void step4Left2()
mjhaugsdal 7:ba0caeb296bb 151 {
mjhaugsdal 7:ba0caeb296bb 152 //engine 2
mjhaugsdal 7:ba0caeb296bb 153 IN5=1;
mjhaugsdal 7:ba0caeb296bb 154 IN6=0;
mjhaugsdal 7:ba0caeb296bb 155 IN7=0;
mjhaugsdal 7:ba0caeb296bb 156 IN8=1;
mjhaugsdal 7:ba0caeb296bb 157 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 158
mjhaugsdal 7:ba0caeb296bb 159
mjhaugsdal 6:ac5d277bd497 160 //engine 2
mjhaugsdal 6:ac5d277bd497 161 IN5=1;
mjhaugsdal 6:ac5d277bd497 162 IN6=0;
mjhaugsdal 6:ac5d277bd497 163 IN7=1;
mjhaugsdal 6:ac5d277bd497 164 IN8=0;
mjhaugsdal 1:a3287196a9b3 165 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 166
mjhaugsdal 7:ba0caeb296bb 167 //engine 2
mjhaugsdal 7:ba0caeb296bb 168 IN5=0;
mjhaugsdal 7:ba0caeb296bb 169 IN6=1;
mjhaugsdal 7:ba0caeb296bb 170 IN7=1;
mjhaugsdal 7:ba0caeb296bb 171 IN8=0;
mjhaugsdal 7:ba0caeb296bb 172 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 173
mjhaugsdal 7:ba0caeb296bb 174 //engine 2
mjhaugsdal 7:ba0caeb296bb 175 IN5=0;
mjhaugsdal 7:ba0caeb296bb 176 IN6=1;
mjhaugsdal 7:ba0caeb296bb 177 IN7=0;
mjhaugsdal 7:ba0caeb296bb 178 IN8=1;
mjhaugsdal 7:ba0caeb296bb 179 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 180
mjhaugsdal 7:ba0caeb296bb 181 }
mjhaugsdal 10:17a16ceb376b 182
mjhaugsdal 7:ba0caeb296bb 183 void step4Left1()
mjhaugsdal 7:ba0caeb296bb 184 {
mjhaugsdal 7:ba0caeb296bb 185 //engine 1
mjhaugsdal 7:ba0caeb296bb 186 IN1=1;
mjhaugsdal 7:ba0caeb296bb 187 IN2=0;
mjhaugsdal 7:ba0caeb296bb 188 IN3=0;
mjhaugsdal 7:ba0caeb296bb 189 IN4=1;
mjhaugsdal 7:ba0caeb296bb 190
mjhaugsdal 7:ba0caeb296bb 191 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 192 //engine 1
mjhaugsdal 7:ba0caeb296bb 193 IN1=1;
mjhaugsdal 7:ba0caeb296bb 194 IN2=0;
mjhaugsdal 7:ba0caeb296bb 195 IN3=1;
mjhaugsdal 7:ba0caeb296bb 196 IN4=0;
mjhaugsdal 7:ba0caeb296bb 197
mjhaugsdal 7:ba0caeb296bb 198 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 199 //engine 1
rlanghbv 0:bd78e433db61 200 IN1=0;
rlanghbv 0:bd78e433db61 201 IN2=1;
rlanghbv 0:bd78e433db61 202 IN3=1;
rlanghbv 0:bd78e433db61 203 IN4=0;
mjhaugsdal 7:ba0caeb296bb 204
mjhaugsdal 1:a3287196a9b3 205 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 206 //engine 1
rlanghbv 0:bd78e433db61 207 IN1=0;
rlanghbv 0:bd78e433db61 208 IN2=1;
rlanghbv 0:bd78e433db61 209 IN3=0;
rlanghbv 0:bd78e433db61 210 IN4=1;
mjhaugsdal 7:ba0caeb296bb 211
mjhaugsdal 1:a3287196a9b3 212 wait_us(fart);
rlanghbv 0:bd78e433db61 213 }
rlanghbv 0:bd78e433db61 214
mjhaugsdal 11:ef54754100ea 215 void motor2(void const *args)
mjhaugsdal 11:ef54754100ea 216 {
mjhaugsdal 11:ef54754100ea 217 while(true)
mjhaugsdal 11:ef54754100ea 218 {
mjhaugsdal 11:ef54754100ea 219 if(m_cmd == 't')
mjhaugsdal 11:ef54754100ea 220 {
mjhaugsdal 11:ef54754100ea 221 step4Left2();
mjhaugsdal 11:ef54754100ea 222 }
mjhaugsdal 11:ef54754100ea 223 }
mjhaugsdal 11:ef54754100ea 224 }
mjhaugsdal 11:ef54754100ea 225
mjhaugsdal 1:a3287196a9b3 226
mjhaugsdal 1:a3287196a9b3 227 void input(void const *args)
mjhaugsdal 1:a3287196a9b3 228 {
mjhaugsdal 1:a3287196a9b3 229 while(true)
mjhaugsdal 1:a3287196a9b3 230 {
mjhaugsdal 3:15be78948724 231
rlanghbv 2:25bcc26f7a5b 232 if(pc.readable())
mjhaugsdal 3:15be78948724 233 { m_cmd=pc.getc();
mjhaugsdal 3:15be78948724 234
mjhaugsdal 3:15be78948724 235 }
rlanghbv 2:25bcc26f7a5b 236
rlanghbv 2:25bcc26f7a5b 237 Thread::wait(10);
mjhaugsdal 5:fdc7a779d525 238 //pc.printf("%d", steps);
mjhaugsdal 1:a3287196a9b3 239 }
mjhaugsdal 1:a3287196a9b3 240 }
rlanghbv 0:bd78e433db61 241
rlanghbv 0:bd78e433db61 242 int main()
rlanghbv 0:bd78e433db61 243 {
mjhaugsdal 1:a3287196a9b3 244 //Thread 1 has constant feed from usb
rlanghbv 2:25bcc26f7a5b 245 Thread t1(input);
mjhaugsdal 11:ef54754100ea 246 Thread t2(motor2);
mjhaugsdal 3:15be78948724 247 static char global_direction;
mjhaugsdal 3:15be78948724 248 printf("Started");
mjhaugsdal 3:15be78948724 249
mjhaugsdal 5:fdc7a779d525 250
mjhaugsdal 1:a3287196a9b3 251 while (true)
mjhaugsdal 1:a3287196a9b3 252 {
mjhaugsdal 4:4d655fdae399 253 //Thread::wait(5);
mjhaugsdal 3:15be78948724 254
mjhaugsdal 7:ba0caeb296bb 255 if (m_cmd == 't')
mjhaugsdal 7:ba0caeb296bb 256
mjhaugsdal 7:ba0caeb296bb 257 {
mjhaugsdal 7:ba0caeb296bb 258 step4Left1();
mjhaugsdal 7:ba0caeb296bb 259 }
mjhaugsdal 7:ba0caeb296bb 260
mjhaugsdal 4:4d655fdae399 261 //0 STEPS LEFT AT MAX SPEED
mjhaugsdal 3:15be78948724 262 if (m_cmd == '0')
mjhaugsdal 3:15be78948724 263 {
mjhaugsdal 4:4d655fdae399 264 fart = 1000;
mjhaugsdal 5:fdc7a779d525 265 //global_direction = '0';
mjhaugsdal 7:ba0caeb296bb 266 step4Left1();
mjhaugsdal 8:5703dadaed07 267 //step4Left2();
mjhaugsdal 8:5703dadaed07 268 //step4Left3();
mjhaugsdal 5:fdc7a779d525 269 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 270
mjhaugsdal 5:fdc7a779d525 271
mjhaugsdal 4:4d655fdae399 272 //printf("LEFT \n");
mjhaugsdal 3:15be78948724 273 //printf("%c \n" + global_direction);
mjhaugsdal 3:15be78948724 274 }
mjhaugsdal 4:4d655fdae399 275 //1 STEPS RIGHT AT MAX SPEED
mjhaugsdal 3:15be78948724 276 else if (m_cmd == '1')
mjhaugsdal 3:15be78948724 277 {
mjhaugsdal 4:4d655fdae399 278 fart = 1000;
mjhaugsdal 5:fdc7a779d525 279 //global_direction = '1';
mjhaugsdal 7:ba0caeb296bb 280 step4Right1();
mjhaugsdal 8:5703dadaed07 281 //step4Right2();
mjhaugsdal 8:5703dadaed07 282 // step4Right3();
mjhaugsdal 3:15be78948724 283 }
mjhaugsdal 4:4d655fdae399 284 else
mjhaugsdal 4:4d655fdae399 285 {
mjhaugsdal 5:fdc7a779d525 286 //global_direction = 'x';
mjhaugsdal 4:4d655fdae399 287 }
mjhaugsdal 6:ac5d277bd497 288
mjhaugsdal 3:15be78948724 289 if (m_cmd == 'a')
mjhaugsdal 3:15be78948724 290 {
mjhaugsdal 4:4d655fdae399 291 fart = 2000;
mjhaugsdal 5:fdc7a779d525 292 //global_direction = '1';
mjhaugsdal 7:ba0caeb296bb 293 step4Right1();
mjhaugsdal 8:5703dadaed07 294 //step4Right2();
mjhaugsdal 8:5703dadaed07 295 //step4Right3();
mjhaugsdal 5:fdc7a779d525 296 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 297 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 298
mjhaugsdal 3:15be78948724 299 }
mjhaugsdal 4:4d655fdae399 300
mjhaugsdal 4:4d655fdae399 301 else if (m_cmd == 'c')
mjhaugsdal 3:15be78948724 302 {
mjhaugsdal 4:4d655fdae399 303 fart = 2000;
mjhaugsdal 5:fdc7a779d525 304 //global_direction = '0';
mjhaugsdal 7:ba0caeb296bb 305 step4Left1();
mjhaugsdal 8:5703dadaed07 306 //step4Left2();
mjhaugsdal 8:5703dadaed07 307 //step4Left3();
mjhaugsdal 5:fdc7a779d525 308
mjhaugsdal 5:fdc7a779d525 309 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 310 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 311 }
mjhaugsdal 5:fdc7a779d525 312 else if (m_cmd == 'k')
mjhaugsdal 1:a3287196a9b3 313 {
mjhaugsdal 10:17a16ceb376b 314 int steps1 = 50;
mjhaugsdal 10:17a16ceb376b 315 int steps2 = 50;
mjhaugsdal 5:fdc7a779d525 316
mjhaugsdal 5:fdc7a779d525 317
mjhaugsdal 6:ac5d277bd497 318 //Steps = 242 gives a 60' rotation to the left.
mjhaugsdal 10:17a16ceb376b 319 while (steps1 >= 0)
mjhaugsdal 5:fdc7a779d525 320 {
mjhaugsdal 7:ba0caeb296bb 321 step4Left1();
mjhaugsdal 8:5703dadaed07 322 //step4Left2();
mjhaugsdal 8:5703dadaed07 323 //step4Left3();
mjhaugsdal 10:17a16ceb376b 324 steps1--;
mjhaugsdal 10:17a16ceb376b 325 }
mjhaugsdal 10:17a16ceb376b 326 while (steps2 >= 0)
mjhaugsdal 10:17a16ceb376b 327 {
mjhaugsdal 10:17a16ceb376b 328 step4Left2();
mjhaugsdal 10:17a16ceb376b 329
mjhaugsdal 10:17a16ceb376b 330 steps2--;
mjhaugsdal 5:fdc7a779d525 331 }
mjhaugsdal 5:fdc7a779d525 332 m_cmd = 'x';
mjhaugsdal 3:15be78948724 333 }
mjhaugsdal 5:fdc7a779d525 334 else if (m_cmd == 'l')
mjhaugsdal 5:fdc7a779d525 335 {
mjhaugsdal 6:ac5d277bd497 336 int steps = 242;
mjhaugsdal 6:ac5d277bd497 337 //Steps = 242 gives a 60' rotation to the right.
mjhaugsdal 5:fdc7a779d525 338 while (steps >= 0)
mjhaugsdal 5:fdc7a779d525 339 {
mjhaugsdal 7:ba0caeb296bb 340 step4Right1();
mjhaugsdal 8:5703dadaed07 341 //step4Right2();
mjhaugsdal 8:5703dadaed07 342 //step4Right3();
mjhaugsdal 5:fdc7a779d525 343 steps--;
mjhaugsdal 5:fdc7a779d525 344 }
mjhaugsdal 5:fdc7a779d525 345 m_cmd = 'x';
mjhaugsdal 5:fdc7a779d525 346
mjhaugsdal 5:fdc7a779d525 347 }
mjhaugsdal 5:fdc7a779d525 348
mjhaugsdal 5:fdc7a779d525 349
mjhaugsdal 5:fdc7a779d525 350
mjhaugsdal 4:4d655fdae399 351
mjhaugsdal 1:a3287196a9b3 352 }
mjhaugsdal 1:a3287196a9b3 353
mjhaugsdal 3:15be78948724 354 } //END Main