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

Committer:
mjhaugsdal
Date:
Tue Apr 26 14:13:34 2016 +0000
Revision:
16:a3555918c025
Parent:
6:ac5d277bd497
Child:
17:ff0021c4dcdc
Went back to revision 6

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 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 16:a3555918c025 17 //Third Engine
mjhaugsdal 16:a3555918c025 18 DigitalOut IN9(D8);
mjhaugsdal 16:a3555918c025 19 DigitalOut IN10(D9);
mjhaugsdal 16:a3555918c025 20 DigitalOut IN11(D10);
mjhaugsdal 16:a3555918c025 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
rlanghbv 0:bd78e433db61 27
mjhaugsdal 5:fdc7a779d525 28
rlanghbv 0:bd78e433db61 29 void step4Right()
rlanghbv 0:bd78e433db61 30 {
mjhaugsdal 6:ac5d277bd497 31 //engine 1
rlanghbv 0:bd78e433db61 32 IN1=0;
rlanghbv 0:bd78e433db61 33 IN2=1;
rlanghbv 0:bd78e433db61 34 IN3=0;
rlanghbv 0:bd78e433db61 35 IN4=1;
mjhaugsdal 6:ac5d277bd497 36 //engine 2
mjhaugsdal 6:ac5d277bd497 37 IN5=0;
mjhaugsdal 6:ac5d277bd497 38 IN6=1;
mjhaugsdal 6:ac5d277bd497 39 IN7=0;
mjhaugsdal 6:ac5d277bd497 40 IN8=1;
mjhaugsdal 1:a3287196a9b3 41 wait_us(fart); //legg som global variabel "fart"
mjhaugsdal 6:ac5d277bd497 42 //engine 1
rlanghbv 0:bd78e433db61 43 IN1=0;
rlanghbv 0:bd78e433db61 44 IN2=1;
rlanghbv 0:bd78e433db61 45 IN3=1;
rlanghbv 0:bd78e433db61 46 IN4=0;
mjhaugsdal 6:ac5d277bd497 47 //engine 2
mjhaugsdal 6:ac5d277bd497 48 IN5=0;
mjhaugsdal 6:ac5d277bd497 49 IN6=1;
mjhaugsdal 6:ac5d277bd497 50 IN7=1;
mjhaugsdal 6:ac5d277bd497 51 IN8=0;
mjhaugsdal 1:a3287196a9b3 52 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 53 //engine 1
rlanghbv 0:bd78e433db61 54 IN1=1;
rlanghbv 0:bd78e433db61 55 IN2=0;
rlanghbv 0:bd78e433db61 56 IN3=1;
rlanghbv 0:bd78e433db61 57 IN4=0;
mjhaugsdal 6:ac5d277bd497 58 //engine 2
mjhaugsdal 6:ac5d277bd497 59 IN5=1;
mjhaugsdal 6:ac5d277bd497 60 IN6=0;
mjhaugsdal 6:ac5d277bd497 61 IN7=1;
mjhaugsdal 6:ac5d277bd497 62 IN8=0;
mjhaugsdal 1:a3287196a9b3 63 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 64 //engine 1
rlanghbv 0:bd78e433db61 65 IN1=1;
rlanghbv 0:bd78e433db61 66 IN2=0;
rlanghbv 0:bd78e433db61 67 IN3=0;
rlanghbv 0:bd78e433db61 68 IN4=1;
mjhaugsdal 6:ac5d277bd497 69 //engine 2
mjhaugsdal 6:ac5d277bd497 70 IN5=1;
mjhaugsdal 6:ac5d277bd497 71 IN6=0;
mjhaugsdal 6:ac5d277bd497 72 IN7=0;
mjhaugsdal 6:ac5d277bd497 73 IN8=1;
mjhaugsdal 1:a3287196a9b3 74 wait_us(fart);
rlanghbv 0:bd78e433db61 75 }
rlanghbv 0:bd78e433db61 76 void step4Left()
rlanghbv 0:bd78e433db61 77 {
mjhaugsdal 6:ac5d277bd497 78 //engine 1
rlanghbv 0:bd78e433db61 79 IN1=1;
rlanghbv 0:bd78e433db61 80 IN2=0;
rlanghbv 0:bd78e433db61 81 IN3=0;
rlanghbv 0:bd78e433db61 82 IN4=1;
mjhaugsdal 6:ac5d277bd497 83 //engine 2
mjhaugsdal 6:ac5d277bd497 84 IN5=1;
mjhaugsdal 6:ac5d277bd497 85 IN6=0;
mjhaugsdal 6:ac5d277bd497 86 IN7=0;
mjhaugsdal 6:ac5d277bd497 87 IN8=1;
mjhaugsdal 1:a3287196a9b3 88 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 89 //engine 1
rlanghbv 0:bd78e433db61 90 IN1=1;
rlanghbv 0:bd78e433db61 91 IN2=0;
rlanghbv 0:bd78e433db61 92 IN3=1;
rlanghbv 0:bd78e433db61 93 IN4=0;
mjhaugsdal 6:ac5d277bd497 94 //engine 2
mjhaugsdal 6:ac5d277bd497 95 IN5=1;
mjhaugsdal 6:ac5d277bd497 96 IN6=0;
mjhaugsdal 6:ac5d277bd497 97 IN7=1;
mjhaugsdal 6:ac5d277bd497 98 IN8=0;
mjhaugsdal 1:a3287196a9b3 99 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 100 //engine 1
rlanghbv 0:bd78e433db61 101 IN1=0;
rlanghbv 0:bd78e433db61 102 IN2=1;
rlanghbv 0:bd78e433db61 103 IN3=1;
rlanghbv 0:bd78e433db61 104 IN4=0;
mjhaugsdal 6:ac5d277bd497 105 //engine 2
mjhaugsdal 6:ac5d277bd497 106 IN5=0;
mjhaugsdal 6:ac5d277bd497 107 IN6=1;
mjhaugsdal 6:ac5d277bd497 108 IN7=1;
mjhaugsdal 6:ac5d277bd497 109 IN8=0;
mjhaugsdal 1:a3287196a9b3 110 wait_us(fart);
mjhaugsdal 6:ac5d277bd497 111 //engine 1
rlanghbv 0:bd78e433db61 112 IN1=0;
rlanghbv 0:bd78e433db61 113 IN2=1;
rlanghbv 0:bd78e433db61 114 IN3=0;
rlanghbv 0:bd78e433db61 115 IN4=1;
mjhaugsdal 6:ac5d277bd497 116 //engine 2
mjhaugsdal 6:ac5d277bd497 117 IN5=0;
mjhaugsdal 6:ac5d277bd497 118 IN6=1;
mjhaugsdal 6:ac5d277bd497 119 IN7=0;
mjhaugsdal 6:ac5d277bd497 120 IN8=1;
mjhaugsdal 1:a3287196a9b3 121 wait_us(fart);
rlanghbv 0:bd78e433db61 122 }
rlanghbv 0:bd78e433db61 123
mjhaugsdal 1:a3287196a9b3 124
mjhaugsdal 1:a3287196a9b3 125 void input(void const *args)
mjhaugsdal 1:a3287196a9b3 126 {
mjhaugsdal 1:a3287196a9b3 127 while(true)
mjhaugsdal 1:a3287196a9b3 128 {
mjhaugsdal 3:15be78948724 129
rlanghbv 2:25bcc26f7a5b 130 if(pc.readable())
mjhaugsdal 3:15be78948724 131 { m_cmd=pc.getc();
mjhaugsdal 3:15be78948724 132
mjhaugsdal 3:15be78948724 133 }
rlanghbv 2:25bcc26f7a5b 134
rlanghbv 2:25bcc26f7a5b 135 Thread::wait(10);
mjhaugsdal 5:fdc7a779d525 136 //pc.printf("%d", steps);
mjhaugsdal 1:a3287196a9b3 137 }
mjhaugsdal 1:a3287196a9b3 138 }
rlanghbv 0:bd78e433db61 139
rlanghbv 0:bd78e433db61 140 int main()
rlanghbv 0:bd78e433db61 141 {
mjhaugsdal 1:a3287196a9b3 142 //Thread 1 has constant feed from usb
rlanghbv 2:25bcc26f7a5b 143 Thread t1(input);
mjhaugsdal 3:15be78948724 144 static char global_direction;
mjhaugsdal 3:15be78948724 145 printf("Started");
mjhaugsdal 3:15be78948724 146
mjhaugsdal 5:fdc7a779d525 147
mjhaugsdal 1:a3287196a9b3 148 while (true)
mjhaugsdal 1:a3287196a9b3 149 {
mjhaugsdal 4:4d655fdae399 150 //Thread::wait(5);
mjhaugsdal 3:15be78948724 151
mjhaugsdal 4:4d655fdae399 152 //0 STEPS LEFT AT MAX SPEED
mjhaugsdal 3:15be78948724 153 if (m_cmd == '0')
mjhaugsdal 3:15be78948724 154 {
mjhaugsdal 4:4d655fdae399 155 fart = 1000;
mjhaugsdal 5:fdc7a779d525 156 //global_direction = '0';
mjhaugsdal 3:15be78948724 157 step4Left();
mjhaugsdal 5:fdc7a779d525 158 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 159
mjhaugsdal 5:fdc7a779d525 160
mjhaugsdal 4:4d655fdae399 161 //printf("LEFT \n");
mjhaugsdal 3:15be78948724 162 //printf("%c \n" + global_direction);
mjhaugsdal 3:15be78948724 163 }
mjhaugsdal 4:4d655fdae399 164 //1 STEPS RIGHT AT MAX SPEED
mjhaugsdal 3:15be78948724 165 else if (m_cmd == '1')
mjhaugsdal 3:15be78948724 166 {
mjhaugsdal 4:4d655fdae399 167 fart = 1000;
mjhaugsdal 5:fdc7a779d525 168 //global_direction = '1';
mjhaugsdal 6:ac5d277bd497 169 step4Right();
mjhaugsdal 3:15be78948724 170 }
mjhaugsdal 4:4d655fdae399 171 else
mjhaugsdal 4:4d655fdae399 172 {
mjhaugsdal 5:fdc7a779d525 173 //global_direction = 'x';
mjhaugsdal 4:4d655fdae399 174 }
mjhaugsdal 6:ac5d277bd497 175
mjhaugsdal 3:15be78948724 176 if (m_cmd == 'a')
mjhaugsdal 3:15be78948724 177 {
mjhaugsdal 4:4d655fdae399 178 fart = 2000;
mjhaugsdal 5:fdc7a779d525 179 //global_direction = '1';
mjhaugsdal 4:4d655fdae399 180 step4Right();
mjhaugsdal 5:fdc7a779d525 181 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 182 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 183
mjhaugsdal 3:15be78948724 184 }
mjhaugsdal 4:4d655fdae399 185
mjhaugsdal 4:4d655fdae399 186 else if (m_cmd == 'c')
mjhaugsdal 3:15be78948724 187 {
mjhaugsdal 4:4d655fdae399 188 fart = 2000;
mjhaugsdal 5:fdc7a779d525 189 //global_direction = '0';
mjhaugsdal 1:a3287196a9b3 190 step4Left();
mjhaugsdal 5:fdc7a779d525 191
mjhaugsdal 5:fdc7a779d525 192 //steps = steps + step;
mjhaugsdal 5:fdc7a779d525 193 // printf("%i", &steps);
mjhaugsdal 3:15be78948724 194 }
mjhaugsdal 5:fdc7a779d525 195 else if (m_cmd == 'k')
mjhaugsdal 1:a3287196a9b3 196 {
mjhaugsdal 6:ac5d277bd497 197 int steps = 242;
mjhaugsdal 5:fdc7a779d525 198
mjhaugsdal 5:fdc7a779d525 199
mjhaugsdal 6:ac5d277bd497 200 //Steps = 242 gives a 60' rotation to the left.
mjhaugsdal 5:fdc7a779d525 201 while (steps >= 0)
mjhaugsdal 5:fdc7a779d525 202 {
mjhaugsdal 5:fdc7a779d525 203 step4Left();
mjhaugsdal 5:fdc7a779d525 204 steps--;
mjhaugsdal 5:fdc7a779d525 205 }
mjhaugsdal 5:fdc7a779d525 206 m_cmd = 'x';
mjhaugsdal 3:15be78948724 207 }
mjhaugsdal 5:fdc7a779d525 208 else if (m_cmd == 'l')
mjhaugsdal 5:fdc7a779d525 209 {
mjhaugsdal 6:ac5d277bd497 210 int steps = 242;
mjhaugsdal 6:ac5d277bd497 211 //Steps = 242 gives a 60' rotation to the right.
mjhaugsdal 5:fdc7a779d525 212 while (steps >= 0)
mjhaugsdal 5:fdc7a779d525 213 {
mjhaugsdal 5:fdc7a779d525 214 step4Right();
mjhaugsdal 5:fdc7a779d525 215 steps--;
mjhaugsdal 5:fdc7a779d525 216 }
mjhaugsdal 5:fdc7a779d525 217 m_cmd = 'x';
mjhaugsdal 5:fdc7a779d525 218
mjhaugsdal 5:fdc7a779d525 219 }
mjhaugsdal 5:fdc7a779d525 220
mjhaugsdal 5:fdc7a779d525 221
mjhaugsdal 5:fdc7a779d525 222
mjhaugsdal 4:4d655fdae399 223
mjhaugsdal 1:a3287196a9b3 224 }
mjhaugsdal 1:a3287196a9b3 225
mjhaugsdal 3:15be78948724 226 } //END Main