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:39:14 2016 +0000
Revision:
12:de8381ca371d
Parent:
11:ef54754100ea
Child:
13:9aa772ae6057
Added void methods to each thread

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