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@15:d9c9ae9e5fc2, 2016-04-26 (annotated)

Committer:
mjhaugsdal
Date:
Tue Apr 26 14:05:10 2016 +0000
Revision:
15:d9c9ae9e5fc2
Parent:
14:c3d1689dccf8 
Changed speed to 500;

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 15:d9c9ae9e5fc2 26 static int fart = 500;
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 13:9aa772ae6057 58 void step4Right2()
mjhaugsdal 13:9aa772ae6057 59 {
mjhaugsdal 7:ba0caeb296bb 60
mjhaugsdal 13:9aa772ae6057 61 //engine 2
mjhaugsdal 13:9aa772ae6057 62 IN5=0;
mjhaugsdal 13:9aa772ae6057 63 IN6=1;
mjhaugsdal 13:9aa772ae6057 64 IN7=0;
mjhaugsdal 13:9aa772ae6057 65 IN8=1;
mjhaugsdal 13:9aa772ae6057 66 wait_us(fart);
mjhaugsdal 13:9aa772ae6057 67 //engine 2
mjhaugsdal 13:9aa772ae6057 68 IN5=0;
mjhaugsdal 13:9aa772ae6057 69 IN6=1;
mjhaugsdal 13:9aa772ae6057 70 IN7=1;
mjhaugsdal 13:9aa772ae6057 71 IN8=0;
mjhaugsdal 13:9aa772ae6057 72 wait_us(fart);
mjhaugsdal 13:9aa772ae6057 73 //engine 2
mjhaugsdal 13:9aa772ae6057 74 IN5=1;
mjhaugsdal 13:9aa772ae6057 75 IN6=0;
mjhaugsdal 13:9aa772ae6057 76 IN7=1;
mjhaugsdal 13:9aa772ae6057 77 IN8=0;
mjhaugsdal 13:9aa772ae6057 78 wait_us(fart);
mjhaugsdal 13:9aa772ae6057 79 //engine 2
mjhaugsdal 13:9aa772ae6057 80 IN5=1;
mjhaugsdal 13:9aa772ae6057 81 IN6=0;
mjhaugsdal 13:9aa772ae6057 82 IN7=0;
mjhaugsdal 13:9aa772ae6057 83 IN8=1;
mjhaugsdal 13:9aa772ae6057 84 wait_us(fart);
mjhaugsdal 13:9aa772ae6057 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 13:9aa772ae6057 150 void step4Left2()
mjhaugsdal 13:9aa772ae6057 151 {
mjhaugsdal 13:9aa772ae6057 152 //engine 2
mjhaugsdal 13:9aa772ae6057 153 IN5=1;
mjhaugsdal 13:9aa772ae6057 154 IN6=0;
mjhaugsdal 13:9aa772ae6057 155 IN7=0;
mjhaugsdal 13:9aa772ae6057 156 IN8=1;
mjhaugsdal 13:9aa772ae6057 157 wait_us(fart);
mjhaugsdal 7:ba0caeb296bb 158
mjhaugsdal 13:9aa772ae6057 159
mjhaugsdal 13:9aa772ae6057 160 //engine 2
mjhaugsdal 13:9aa772ae6057 161 IN5=1;
mjhaugsdal 13:9aa772ae6057 162 IN6=0;
mjhaugsdal 13:9aa772ae6057 163 IN7=1;
mjhaugsdal 13:9aa772ae6057 164 IN8=0;
mjhaugsdal 13:9aa772ae6057 165 wait_us(fart);
mjhaugsdal 13:9aa772ae6057 166
mjhaugsdal 13:9aa772ae6057 167 //engine 2
mjhaugsdal 13:9aa772ae6057 168 IN5=0;
mjhaugsdal 13:9aa772ae6057 169 IN6=1;
mjhaugsdal 13:9aa772ae6057 170 IN7=1;
mjhaugsdal 13:9aa772ae6057 171 IN8=0;
mjhaugsdal 13:9aa772ae6057 172 wait_us(fart);
mjhaugsdal 13:9aa772ae6057 173
mjhaugsdal 13:9aa772ae6057 174 //engine 2
mjhaugsdal 13:9aa772ae6057 175 IN5=0;
mjhaugsdal 13:9aa772ae6057 176 IN6=1;
mjhaugsdal 13:9aa772ae6057 177 IN7=0;
mjhaugsdal 13:9aa772ae6057 178 IN8=1;
mjhaugsdal 13:9aa772ae6057 179 wait_us(fart);
mjhaugsdal 13:9aa772ae6057 180
mjhaugsdal 13:9aa772ae6057 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
mjhaugsdal 1:a3287196a9b3 216
mjhaugsdal 1:a3287196a9b3 217 void input(void const *args)
mjhaugsdal 1:a3287196a9b3 218 {
mjhaugsdal 1:a3287196a9b3 219 while(true)
mjhaugsdal 1:a3287196a9b3 220 {
mjhaugsdal 3:15be78948724 221
rlanghbv 2:25bcc26f7a5b 222 if(pc.readable())
mjhaugsdal 3:15be78948724 223 { m_cmd=pc.getc();
mjhaugsdal 3:15be78948724 224
mjhaugsdal 3:15be78948724 225 }
rlanghbv 2:25bcc26f7a5b 226
rlanghbv 2:25bcc26f7a5b 227 Thread::wait(10);
mjhaugsdal 5:fdc7a779d525 228 //pc.printf("%d", steps);
mjhaugsdal 1:a3287196a9b3 229 }
mjhaugsdal 1:a3287196a9b3 230 }
rlanghbv 0:bd78e433db61 231
rlanghbv 0:bd78e433db61 232 int main()
rlanghbv 0:bd78e433db61 233 {
mjhaugsdal 1:a3287196a9b3 234 //Thread 1 has constant feed from usb
rlanghbv 2:25bcc26f7a5b 235 Thread t1(input);
mjhaugsdal 14:c3d1689dccf8 236
mjhaugsdal 3:15be78948724 237 static char global_direction;
mjhaugsdal 3:15be78948724 238 printf("Started");
mjhaugsdal 3:15be78948724 239
mjhaugsdal 5:fdc7a779d525 240
mjhaugsdal 1:a3287196a9b3 241 while (true)
mjhaugsdal 1:a3287196a9b3 242 {
mjhaugsdal 4:4d655fdae399 243 //Thread::wait(5);
mjhaugsdal 3:15be78948724 244
mjhaugsdal 7:ba0caeb296bb 245 if (m_cmd == 't')
mjhaugsdal 7:ba0caeb296bb 246
mjhaugsdal 7:ba0caeb296bb 247 {
mjhaugsdal 7:ba0caeb296bb 248 step4Left1();
mjhaugsdal 14:c3d1689dccf8 249 step4Left2();
mjhaugsdal 7:ba0caeb296bb 250 }
mjhaugsdal 7:ba0caeb296bb 251
mjhaugsdal 4:4d655fdae399 252 //0 STEPS LEFT AT MAX SPEED
mjhaugsdal 3:15be78948724 253 if (m_cmd == '0')
mjhaugsdal 3:15be78948724 254 {
mjhaugsdal 4:4d655fdae399 255 fart = 1000;
mjhaugsdal 5:fdc7a779d525 256 //global_direction = '0';
mjhaugsdal 7:ba0caeb296bb 257 step4Left1();
mjhaugsdal 14:c3d1689dccf8 258 step4Left2();
mjhaugsdal 8:5703dadaed07 259 //step4Left2();
mjhaugsdal 8:5703dadaed07 260 //step4Left3();
mjhaugsdal 5:fdc7a779d525 261 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 262
mjhaugsdal 5:fdc7a779d525 263
mjhaugsdal 4:4d655fdae399 264 //printf("LEFT \n");
mjhaugsdal 3:15be78948724 265 //printf("%c \n" + global_direction);
mjhaugsdal 3:15be78948724 266 }
mjhaugsdal 4:4d655fdae399 267 //1 STEPS RIGHT AT MAX SPEED
mjhaugsdal 3:15be78948724 268 else if (m_cmd == '1')
mjhaugsdal 3:15be78948724 269 {
mjhaugsdal 4:4d655fdae399 270 fart = 1000;
mjhaugsdal 5:fdc7a779d525 271 //global_direction = '1';
mjhaugsdal 7:ba0caeb296bb 272 step4Right1();
mjhaugsdal 8:5703dadaed07 273 //step4Right2();
mjhaugsdal 8:5703dadaed07 274 // step4Right3();
mjhaugsdal 3:15be78948724 275 }
mjhaugsdal 4:4d655fdae399 276 else
mjhaugsdal 4:4d655fdae399 277 {
mjhaugsdal 5:fdc7a779d525 278 //global_direction = 'x';
mjhaugsdal 4:4d655fdae399 279 }
mjhaugsdal 6:ac5d277bd497 280
mjhaugsdal 3:15be78948724 281 if (m_cmd == 'a')
mjhaugsdal 3:15be78948724 282 {
mjhaugsdal 4:4d655fdae399 283 fart = 2000;
mjhaugsdal 5:fdc7a779d525 284 //global_direction = '1';
mjhaugsdal 7:ba0caeb296bb 285 step4Right1();
mjhaugsdal 8:5703dadaed07 286 //step4Right2();
mjhaugsdal 8:5703dadaed07 287 //step4Right3();
mjhaugsdal 5:fdc7a779d525 288 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 289 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 290
mjhaugsdal 3:15be78948724 291 }
mjhaugsdal 4:4d655fdae399 292
mjhaugsdal 4:4d655fdae399 293 else if (m_cmd == 'c')
mjhaugsdal 3:15be78948724 294 {
mjhaugsdal 4:4d655fdae399 295 fart = 2000;
mjhaugsdal 5:fdc7a779d525 296 //global_direction = '0';
mjhaugsdal 7:ba0caeb296bb 297 step4Left1();
mjhaugsdal 8:5703dadaed07 298 //step4Left2();
mjhaugsdal 8:5703dadaed07 299 //step4Left3();
mjhaugsdal 5:fdc7a779d525 300
mjhaugsdal 5:fdc7a779d525 301 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 302 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 303 }
mjhaugsdal 5:fdc7a779d525 304 else if (m_cmd == 'k')
mjhaugsdal 1:a3287196a9b3 305 {
mjhaugsdal 10:17a16ceb376b 306 int steps1 = 50;
mjhaugsdal 10:17a16ceb376b 307 int steps2 = 50;
mjhaugsdal 5:fdc7a779d525 308
mjhaugsdal 5:fdc7a779d525 309
mjhaugsdal 6:ac5d277bd497 310 //Steps = 242 gives a 60' rotation to the left.
mjhaugsdal 10:17a16ceb376b 311 while (steps1 >= 0)
mjhaugsdal 5:fdc7a779d525 312 {
mjhaugsdal 7:ba0caeb296bb 313 step4Left1();
mjhaugsdal 13:9aa772ae6057 314 step4Left2();
mjhaugsdal 8:5703dadaed07 315 //step4Left3();
mjhaugsdal 10:17a16ceb376b 316 steps1--;
mjhaugsdal 10:17a16ceb376b 317 }
mjhaugsdal 13:9aa772ae6057 318 /*
mjhaugsdal 10:17a16ceb376b 319 while (steps2 >= 0)
mjhaugsdal 10:17a16ceb376b 320 {
mjhaugsdal 10:17a16ceb376b 321 step4Left2();
mjhaugsdal 10:17a16ceb376b 322
mjhaugsdal 10:17a16ceb376b 323 steps2--;
mjhaugsdal 13:9aa772ae6057 324 } */
mjhaugsdal 5:fdc7a779d525 325 m_cmd = 'x';
mjhaugsdal 3:15be78948724 326 }
mjhaugsdal 5:fdc7a779d525 327 else if (m_cmd == 'l')
mjhaugsdal 5:fdc7a779d525 328 {
mjhaugsdal 6:ac5d277bd497 329 int steps = 242;
mjhaugsdal 6:ac5d277bd497 330 //Steps = 242 gives a 60' rotation to the right.
mjhaugsdal 5:fdc7a779d525 331 while (steps >= 0)
mjhaugsdal 5:fdc7a779d525 332 {
mjhaugsdal 7:ba0caeb296bb 333 step4Right1();
mjhaugsdal 13:9aa772ae6057 334 step4Right2();
mjhaugsdal 8:5703dadaed07 335 //step4Right3();
mjhaugsdal 5:fdc7a779d525 336 steps--;
mjhaugsdal 5:fdc7a779d525 337 }
mjhaugsdal 5:fdc7a779d525 338 m_cmd = 'x';
mjhaugsdal 5:fdc7a779d525 339
mjhaugsdal 5:fdc7a779d525 340 }
mjhaugsdal 5:fdc7a779d525 341
mjhaugsdal 5:fdc7a779d525 342
mjhaugsdal 5:fdc7a779d525 343
mjhaugsdal 4:4d655fdae399 344
mjhaugsdal 1:a3287196a9b3 345 }
mjhaugsdal 1:a3287196a9b3 346
mjhaugsdal 3:15be78948724 347 } //END Main