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@9:1636083fce43, 2016-04-26 (annotated)

Committer:
mjhaugsdal
Date:
Tue Apr 26 12:05:57 2016 +0000
Revision:
9:1636083fce43
Parent:
8:5703dadaed07
Child:
10:17a16ceb376b
Removed engine 2 and 3

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