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@6:ac5d277bd497, 2016-04-20 (annotated)

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