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@5:fdc7a779d525, 2016-04-18 (annotated)

Committer:
mjhaugsdal
Date:
Mon Apr 18 12:18:06 2016 +0000
Revision:
5:fdc7a779d525
Parent:
4:4d655fdae399
Child:
6:ac5d277bd497
Commit before making room for more than one engine

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 5:fdc7a779d525 7 //First engine
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 5:fdc7a779d525 12 //Second engine
mjhaugsdal 5:fdc7a779d525 13 DigitalOut IN5(D2);
mjhaugsdal 5:fdc7a779d525 14 DigitalOut IN6(D3);
mjhaugsdal 5:fdc7a779d525 15 DigitalOut IN7(D4);
mjhaugsdal 5:fdc7a779d525 16 DigitalOut IN8(D5);
mjhaugsdal 5:fdc7a779d525 17
rlanghbv 0:bd78e433db61 18
mjhaugsdal 1:a3287196a9b3 19 static int fart = 1000;
mjhaugsdal 3:15be78948724 20 static char m_cmd = 'x';
rlanghbv 0:bd78e433db61 21
rlanghbv 0:bd78e433db61 22
mjhaugsdal 5:fdc7a779d525 23
rlanghbv 0:bd78e433db61 24 void step4Right()
rlanghbv 0:bd78e433db61 25 {
rlanghbv 0:bd78e433db61 26 IN1=0;
rlanghbv 0:bd78e433db61 27 IN2=1;
rlanghbv 0:bd78e433db61 28 IN3=0;
rlanghbv 0:bd78e433db61 29 IN4=1;
mjhaugsdal 1:a3287196a9b3 30 wait_us(fart); //legg som global variabel "fart"
rlanghbv 0:bd78e433db61 31 IN1=0;
rlanghbv 0:bd78e433db61 32 IN2=1;
rlanghbv 0:bd78e433db61 33 IN3=1;
rlanghbv 0:bd78e433db61 34 IN4=0;
mjhaugsdal 1:a3287196a9b3 35 wait_us(fart);
rlanghbv 0:bd78e433db61 36 IN1=1;
rlanghbv 0:bd78e433db61 37 IN2=0;
rlanghbv 0:bd78e433db61 38 IN3=1;
rlanghbv 0:bd78e433db61 39 IN4=0;
mjhaugsdal 1:a3287196a9b3 40 wait_us(fart);
rlanghbv 0:bd78e433db61 41 IN1=1;
rlanghbv 0:bd78e433db61 42 IN2=0;
rlanghbv 0:bd78e433db61 43 IN3=0;
rlanghbv 0:bd78e433db61 44 IN4=1;
mjhaugsdal 1:a3287196a9b3 45 wait_us(fart);
rlanghbv 0:bd78e433db61 46 }
rlanghbv 0:bd78e433db61 47 void step4Left()
rlanghbv 0:bd78e433db61 48 {
rlanghbv 0:bd78e433db61 49 IN1=1;
rlanghbv 0:bd78e433db61 50 IN2=0;
rlanghbv 0:bd78e433db61 51 IN3=0;
rlanghbv 0:bd78e433db61 52 IN4=1;
mjhaugsdal 1:a3287196a9b3 53 wait_us(fart);
rlanghbv 0:bd78e433db61 54
rlanghbv 0:bd78e433db61 55 IN1=1;
rlanghbv 0:bd78e433db61 56 IN2=0;
rlanghbv 0:bd78e433db61 57 IN3=1;
rlanghbv 0:bd78e433db61 58 IN4=0;
mjhaugsdal 1:a3287196a9b3 59 wait_us(fart);
rlanghbv 0:bd78e433db61 60
rlanghbv 0:bd78e433db61 61 IN1=0;
rlanghbv 0:bd78e433db61 62 IN2=1;
rlanghbv 0:bd78e433db61 63 IN3=1;
rlanghbv 0:bd78e433db61 64 IN4=0;
mjhaugsdal 1:a3287196a9b3 65 wait_us(fart);
rlanghbv 0:bd78e433db61 66
rlanghbv 0:bd78e433db61 67 IN1=0;
rlanghbv 0:bd78e433db61 68 IN2=1;
rlanghbv 0:bd78e433db61 69 IN3=0;
rlanghbv 0:bd78e433db61 70 IN4=1;
mjhaugsdal 1:a3287196a9b3 71 wait_us(fart);
rlanghbv 0:bd78e433db61 72 }
rlanghbv 0:bd78e433db61 73
mjhaugsdal 1:a3287196a9b3 74
mjhaugsdal 1:a3287196a9b3 75 void input(void const *args)
mjhaugsdal 1:a3287196a9b3 76 {
mjhaugsdal 1:a3287196a9b3 77 while(true)
mjhaugsdal 1:a3287196a9b3 78 {
mjhaugsdal 3:15be78948724 79
rlanghbv 2:25bcc26f7a5b 80 if(pc.readable())
mjhaugsdal 3:15be78948724 81 { m_cmd=pc.getc();
mjhaugsdal 3:15be78948724 82
mjhaugsdal 3:15be78948724 83 }
rlanghbv 2:25bcc26f7a5b 84
rlanghbv 2:25bcc26f7a5b 85 Thread::wait(10);
mjhaugsdal 5:fdc7a779d525 86 //pc.printf("%d", steps);
mjhaugsdal 1:a3287196a9b3 87 }
mjhaugsdal 1:a3287196a9b3 88 }
rlanghbv 0:bd78e433db61 89
rlanghbv 0:bd78e433db61 90 int main()
rlanghbv 0:bd78e433db61 91 {
mjhaugsdal 1:a3287196a9b3 92 //Thread 1 has constant feed from usb
rlanghbv 2:25bcc26f7a5b 93 Thread t1(input);
mjhaugsdal 3:15be78948724 94 static char global_direction;
mjhaugsdal 3:15be78948724 95 printf("Started");
mjhaugsdal 3:15be78948724 96
mjhaugsdal 5:fdc7a779d525 97
mjhaugsdal 1:a3287196a9b3 98 while (true)
mjhaugsdal 1:a3287196a9b3 99 {
mjhaugsdal 4:4d655fdae399 100 //Thread::wait(5);
mjhaugsdal 3:15be78948724 101
mjhaugsdal 4:4d655fdae399 102 //0 STEPS LEFT AT MAX SPEED
mjhaugsdal 3:15be78948724 103 if (m_cmd == '0')
mjhaugsdal 3:15be78948724 104 {
mjhaugsdal 4:4d655fdae399 105 fart = 1000;
mjhaugsdal 5:fdc7a779d525 106 //global_direction = '0';
mjhaugsdal 3:15be78948724 107 step4Left();
mjhaugsdal 5:fdc7a779d525 108 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 109
mjhaugsdal 5:fdc7a779d525 110
mjhaugsdal 4:4d655fdae399 111 //printf("LEFT \n");
mjhaugsdal 3:15be78948724 112 //printf("%c \n" + global_direction);
mjhaugsdal 3:15be78948724 113 }
mjhaugsdal 4:4d655fdae399 114 //1 STEPS RIGHT AT MAX SPEED
mjhaugsdal 3:15be78948724 115 else if (m_cmd == '1')
mjhaugsdal 3:15be78948724 116 {
mjhaugsdal 4:4d655fdae399 117 fart = 1000;
mjhaugsdal 5:fdc7a779d525 118 //global_direction = '1';
mjhaugsdal 3:15be78948724 119 step4Right();
mjhaugsdal 5:fdc7a779d525 120 // steps = steps + step;
mjhaugsdal 5:fdc7a779d525 121 //printf("%i", &steps);
mjhaugsdal 3:15be78948724 122 //printf("RIGHT \n");
mjhaugsdal 4:4d655fdae399 123
mjhaugsdal 3:15be78948724 124 }
mjhaugsdal 4:4d655fdae399 125 else
mjhaugsdal 4:4d655fdae399 126 {
mjhaugsdal 5:fdc7a779d525 127 //global_direction = 'x';
mjhaugsdal 4:4d655fdae399 128 }
mjhaugsdal 3:15be78948724 129 if (m_cmd == 'a')
mjhaugsdal 3:15be78948724 130 {
mjhaugsdal 4:4d655fdae399 131 fart = 2000;
mjhaugsdal 5:fdc7a779d525 132 //global_direction = '1';
mjhaugsdal 4:4d655fdae399 133 step4Right();
mjhaugsdal 5:fdc7a779d525 134 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 135 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 136
mjhaugsdal 3:15be78948724 137 }
mjhaugsdal 4:4d655fdae399 138
mjhaugsdal 4:4d655fdae399 139 else if (m_cmd == 'c')
mjhaugsdal 3:15be78948724 140 {
mjhaugsdal 4:4d655fdae399 141 fart = 2000;
mjhaugsdal 5:fdc7a779d525 142 //global_direction = '0';
mjhaugsdal 1:a3287196a9b3 143 step4Left();
mjhaugsdal 5:fdc7a779d525 144
mjhaugsdal 5:fdc7a779d525 145 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 146 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 147 }
mjhaugsdal 5:fdc7a779d525 148 else if (m_cmd == 'k')
mjhaugsdal 1:a3287196a9b3 149 {
mjhaugsdal 5:fdc7a779d525 150 int steps = 485;
mjhaugsdal 5:fdc7a779d525 151
mjhaugsdal 5:fdc7a779d525 152
mjhaugsdal 5:fdc7a779d525 153 //Steps = 485 gives a 120' rotation to the left.
mjhaugsdal 5:fdc7a779d525 154 while (steps >= 0)
mjhaugsdal 5:fdc7a779d525 155 {
mjhaugsdal 5:fdc7a779d525 156 step4Left();
mjhaugsdal 5:fdc7a779d525 157 steps--;
mjhaugsdal 5:fdc7a779d525 158 }
mjhaugsdal 5:fdc7a779d525 159 m_cmd = 'x';
mjhaugsdal 3:15be78948724 160 }
mjhaugsdal 5:fdc7a779d525 161 else if (m_cmd == 'l')
mjhaugsdal 5:fdc7a779d525 162 {
mjhaugsdal 5:fdc7a779d525 163 int steps = 485;
mjhaugsdal 5:fdc7a779d525 164 //Steps = 485 gives a 120' rotation to the right.
mjhaugsdal 5:fdc7a779d525 165 while (steps >= 0)
mjhaugsdal 5:fdc7a779d525 166 {
mjhaugsdal 5:fdc7a779d525 167 step4Right();
mjhaugsdal 5:fdc7a779d525 168 steps--;
mjhaugsdal 5:fdc7a779d525 169 }
mjhaugsdal 5:fdc7a779d525 170 m_cmd = 'x';
mjhaugsdal 5:fdc7a779d525 171
mjhaugsdal 5:fdc7a779d525 172 }
mjhaugsdal 5:fdc7a779d525 173
mjhaugsdal 5:fdc7a779d525 174
mjhaugsdal 5:fdc7a779d525 175
mjhaugsdal 4:4d655fdae399 176
mjhaugsdal 1:a3287196a9b3 177 }
mjhaugsdal 1:a3287196a9b3 178
mjhaugsdal 3:15be78948724 179 } //END Main