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@8:5703dadaed07, 2016-04-26 (annotated)

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