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@10:17a16ceb376b, 2016-04-26 (annotated)

Committer:
mjhaugsdal
Date:
Tue Apr 26 13:10:22 2016 +0000
Revision:
10:17a16ceb376b
Parent:
9:1636083fce43
Child:
11:ef54754100ea
Testing sequence controlled engines

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 10:17a16ceb376b 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 1:a3287196a9b3 215
mjhaugsdal 1:a3287196a9b3 216 void input(void const *args)
mjhaugsdal 1:a3287196a9b3 217 {
mjhaugsdal 1:a3287196a9b3 218 while(true)
mjhaugsdal 1:a3287196a9b3 219 {
mjhaugsdal 3:15be78948724 220
rlanghbv 2:25bcc26f7a5b 221 if(pc.readable())
mjhaugsdal 3:15be78948724 222 { m_cmd=pc.getc();
mjhaugsdal 3:15be78948724 223
mjhaugsdal 3:15be78948724 224 }
rlanghbv 2:25bcc26f7a5b 225
rlanghbv 2:25bcc26f7a5b 226 Thread::wait(10);
mjhaugsdal 5:fdc7a779d525 227 //pc.printf("%d", steps);
mjhaugsdal 1:a3287196a9b3 228 }
mjhaugsdal 1:a3287196a9b3 229 }
rlanghbv 0:bd78e433db61 230
rlanghbv 0:bd78e433db61 231 int main()
rlanghbv 0:bd78e433db61 232 {
mjhaugsdal 1:a3287196a9b3 233 //Thread 1 has constant feed from usb
rlanghbv 2:25bcc26f7a5b 234 Thread t1(input);
mjhaugsdal 3:15be78948724 235 static char global_direction;
mjhaugsdal 3:15be78948724 236 printf("Started");
mjhaugsdal 3:15be78948724 237
mjhaugsdal 5:fdc7a779d525 238
mjhaugsdal 1:a3287196a9b3 239 while (true)
mjhaugsdal 1:a3287196a9b3 240 {
mjhaugsdal 4:4d655fdae399 241 //Thread::wait(5);
mjhaugsdal 3:15be78948724 242
mjhaugsdal 7:ba0caeb296bb 243 if (m_cmd == 't')
mjhaugsdal 7:ba0caeb296bb 244
mjhaugsdal 7:ba0caeb296bb 245 {
mjhaugsdal 7:ba0caeb296bb 246 step4Left1();
mjhaugsdal 7:ba0caeb296bb 247 }
mjhaugsdal 7:ba0caeb296bb 248
mjhaugsdal 4:4d655fdae399 249 //0 STEPS LEFT AT MAX SPEED
mjhaugsdal 3:15be78948724 250 if (m_cmd == '0')
mjhaugsdal 3:15be78948724 251 {
mjhaugsdal 4:4d655fdae399 252 fart = 1000;
mjhaugsdal 5:fdc7a779d525 253 //global_direction = '0';
mjhaugsdal 7:ba0caeb296bb 254 step4Left1();
mjhaugsdal 8:5703dadaed07 255 //step4Left2();
mjhaugsdal 8:5703dadaed07 256 //step4Left3();
mjhaugsdal 5:fdc7a779d525 257 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 258
mjhaugsdal 5:fdc7a779d525 259
mjhaugsdal 4:4d655fdae399 260 //printf("LEFT \n");
mjhaugsdal 3:15be78948724 261 //printf("%c \n" + global_direction);
mjhaugsdal 3:15be78948724 262 }
mjhaugsdal 4:4d655fdae399 263 //1 STEPS RIGHT AT MAX SPEED
mjhaugsdal 3:15be78948724 264 else if (m_cmd == '1')
mjhaugsdal 3:15be78948724 265 {
mjhaugsdal 4:4d655fdae399 266 fart = 1000;
mjhaugsdal 5:fdc7a779d525 267 //global_direction = '1';
mjhaugsdal 7:ba0caeb296bb 268 step4Right1();
mjhaugsdal 8:5703dadaed07 269 //step4Right2();
mjhaugsdal 8:5703dadaed07 270 // step4Right3();
mjhaugsdal 3:15be78948724 271 }
mjhaugsdal 4:4d655fdae399 272 else
mjhaugsdal 4:4d655fdae399 273 {
mjhaugsdal 5:fdc7a779d525 274 //global_direction = 'x';
mjhaugsdal 4:4d655fdae399 275 }
mjhaugsdal 6:ac5d277bd497 276
mjhaugsdal 3:15be78948724 277 if (m_cmd == 'a')
mjhaugsdal 3:15be78948724 278 {
mjhaugsdal 4:4d655fdae399 279 fart = 2000;
mjhaugsdal 5:fdc7a779d525 280 //global_direction = '1';
mjhaugsdal 7:ba0caeb296bb 281 step4Right1();
mjhaugsdal 8:5703dadaed07 282 //step4Right2();
mjhaugsdal 8:5703dadaed07 283 //step4Right3();
mjhaugsdal 5:fdc7a779d525 284 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 285 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 286
mjhaugsdal 3:15be78948724 287 }
mjhaugsdal 4:4d655fdae399 288
mjhaugsdal 4:4d655fdae399 289 else if (m_cmd == 'c')
mjhaugsdal 3:15be78948724 290 {
mjhaugsdal 4:4d655fdae399 291 fart = 2000;
mjhaugsdal 5:fdc7a779d525 292 //global_direction = '0';
mjhaugsdal 7:ba0caeb296bb 293 step4Left1();
mjhaugsdal 8:5703dadaed07 294 //step4Left2();
mjhaugsdal 8:5703dadaed07 295 //step4Left3();
mjhaugsdal 5:fdc7a779d525 296
mjhaugsdal 5:fdc7a779d525 297 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 298 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 299 }
mjhaugsdal 5:fdc7a779d525 300 else if (m_cmd == 'k')
mjhaugsdal 1:a3287196a9b3 301 {
mjhaugsdal 10:17a16ceb376b 302 int steps1 = 50;
mjhaugsdal 10:17a16ceb376b 303 int steps2 = 50;
mjhaugsdal 5:fdc7a779d525 304
mjhaugsdal 5:fdc7a779d525 305
mjhaugsdal 6:ac5d277bd497 306 //Steps = 242 gives a 60' rotation to the left.
mjhaugsdal 10:17a16ceb376b 307 while (steps1 >= 0)
mjhaugsdal 5:fdc7a779d525 308 {
mjhaugsdal 7:ba0caeb296bb 309 step4Left1();
mjhaugsdal 8:5703dadaed07 310 //step4Left2();
mjhaugsdal 8:5703dadaed07 311 //step4Left3();
mjhaugsdal 10:17a16ceb376b 312 steps1--;
mjhaugsdal 10:17a16ceb376b 313 }
mjhaugsdal 10:17a16ceb376b 314 while (steps2 >= 0)
mjhaugsdal 10:17a16ceb376b 315 {
mjhaugsdal 10:17a16ceb376b 316 step4Left2();
mjhaugsdal 10:17a16ceb376b 317
mjhaugsdal 10:17a16ceb376b 318 steps2--;
mjhaugsdal 5:fdc7a779d525 319 }
mjhaugsdal 5:fdc7a779d525 320 m_cmd = 'x';
mjhaugsdal 3:15be78948724 321 }
mjhaugsdal 5:fdc7a779d525 322 else if (m_cmd == 'l')
mjhaugsdal 5:fdc7a779d525 323 {
mjhaugsdal 6:ac5d277bd497 324 int steps = 242;
mjhaugsdal 6:ac5d277bd497 325 //Steps = 242 gives a 60' rotation to the right.
mjhaugsdal 5:fdc7a779d525 326 while (steps >= 0)
mjhaugsdal 5:fdc7a779d525 327 {
mjhaugsdal 7:ba0caeb296bb 328 step4Right1();
mjhaugsdal 8:5703dadaed07 329 //step4Right2();
mjhaugsdal 8:5703dadaed07 330 //step4Right3();
mjhaugsdal 5:fdc7a779d525 331 steps--;
mjhaugsdal 5:fdc7a779d525 332 }
mjhaugsdal 5:fdc7a779d525 333 m_cmd = 'x';
mjhaugsdal 5:fdc7a779d525 334
mjhaugsdal 5:fdc7a779d525 335 }
mjhaugsdal 5:fdc7a779d525 336
mjhaugsdal 5:fdc7a779d525 337
mjhaugsdal 5:fdc7a779d525 338
mjhaugsdal 4:4d655fdae399 339
mjhaugsdal 1:a3287196a9b3 340 }
mjhaugsdal 1:a3287196a9b3 341
mjhaugsdal 3:15be78948724 342 } //END Main