Vivien van Veldhuizen / Mbed 2 deprecated Signa-bot

Signa-bot code for project BioRobotics, at University of Twente.

Dependencies:   mbed QEI MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@27:3eb181cbe183, 2019-10-29 (annotated)

Committer:
viviien
Date:
Tue Oct 29 11:10:23 2019 +0000
Revision:
27:3eb181cbe183
Parent:
26:432d3519ba86
Child:
28:43a1d67ff8ea
Het werkt, maar met lichte overshoot voor 1 motor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Feike 4:bd21569250c7 1 #include "mbed.h"
Feike 8:017b813c72bb 2 #include "MODSERIAL.h"
Feike 12:2382468d36a4 3 #include "QEI.h"
Feike 18:ab0fe311e7f3 4 #include "Math.h"
Feike 18:ab0fe311e7f3 5 #include "ttmath.h"
Feike 18:ab0fe311e7f3 6
Feike 17:6da57acb7bea 7 MODSERIAL pc(USBTX, USBRX);
Feike 18:ab0fe311e7f3 8 //Serial term (USBTX, USBRX);
Feike 17:6da57acb7bea 9 PwmOut motor1_pwm(PTC2);
Feike 17:6da57acb7bea 10 DigitalOut motor1_dir(PTC3);
Feike 18:ab0fe311e7f3 11 PwmOut motor2_pwm(PTA2);
Feike 18:ab0fe311e7f3 12 DigitalOut motor2_dir(PTB23);
Feike 18:ab0fe311e7f3 13 PwmOut motor3_pwm(PTC4);
Feike 18:ab0fe311e7f3 14 DigitalOut motor3_dir(PTC12);
Feike 4:bd21569250c7 15
viviien 23:18b0be02187f 16 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 17 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 18 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
Feike 7:e119b12e5e7f 19
viviien 25:eb3204e45d33 20 float steps1 = 0;
viviien 25:eb3204e45d33 21 float steps2 = 0;
viviien 25:eb3204e45d33 22 float steps3 = 0;
Feike 19:9c8ab7922191 23 int check;
Feike 13:18dd7a15603f 24 int quit;
Feike 19:9c8ab7922191 25 int limit_pos = 8400;
Feike 19:9c8ab7922191 26 float steps;
Feike 19:9c8ab7922191 27 int g = 0;
Feike 19:9c8ab7922191 28
Feike 19:9c8ab7922191 29 bool check1;
Feike 19:9c8ab7922191 30 bool check2;
Feike 19:9c8ab7922191 31 bool check3;
Feike 18:ab0fe311e7f3 32 int counts1 = 0;
Feike 18:ab0fe311e7f3 33 int counts2 = 0;
Feike 18:ab0fe311e7f3 34 int counts3 = 0;
viviien 27:3eb181cbe183 35
Feike 13:18dd7a15603f 36
Feike 18:ab0fe311e7f3 37 const float le = 15.0;
Feike 18:ab0fe311e7f3 38 const float f = 37.5;
Feike 18:ab0fe311e7f3 39 const float re = 174.0;
Feike 18:ab0fe311e7f3 40 const float rf = 50.0;
Feike 18:ab0fe311e7f3 41 const float pi = 3.14159265358979323846;
viviien 22:9f911405e096 42 const float cospi = -0.5;
viviien 22:9f911405e096 43 const float sinpi = 0.8660254;
Feike 18:ab0fe311e7f3 44 float y2;
Feike 18:ab0fe311e7f3 45 float y1;
Feike 18:ab0fe311e7f3 46 float z1;
Feike 18:ab0fe311e7f3 47 float z2;
Feike 18:ab0fe311e7f3 48 float rje2;
Feike 18:ab0fe311e7f3 49 float rje;
Feike 18:ab0fe311e7f3 50 float r2;
Feike 18:ab0fe311e7f3 51 float r;
Feike 19:9c8ab7922191 52
Feike 18:ab0fe311e7f3 53 float z0=-172;
Feike 18:ab0fe311e7f3 54 float y0=0;
Feike 18:ab0fe311e7f3 55 float x0=0;
viviien 21:c826abca79c3 56 float x00;
viviien 21:c826abca79c3 57 float y00;
viviien 21:c826abca79c3 58 float z00;
Feike 19:9c8ab7922191 59
Feike 18:ab0fe311e7f3 60 float theta1;
Feike 18:ab0fe311e7f3 61 float theta2;
Feike 18:ab0fe311e7f3 62 float theta3;
Feike 18:ab0fe311e7f3 63 float oldtheta1=0;
Feike 18:ab0fe311e7f3 64 float oldtheta2=0;
Feike 18:ab0fe311e7f3 65 float oldtheta3=0;
Feike 19:9c8ab7922191 66
viviien 27:3eb181cbe183 67 Ticker pulses;
viviien 27:3eb181cbe183 68 void getpulses() {
viviien 23:18b0be02187f 69 counts1 = Encoder1.getPulses();
viviien 23:18b0be02187f 70 counts2 = Encoder2.getPulses();
viviien 23:18b0be02187f 71 counts3 = Encoder3.getPulses();
viviien 23:18b0be02187f 72 }
viviien 25:eb3204e45d33 73
viviien 23:18b0be02187f 74
viviien 21:c826abca79c3 75 float delta_calcangleyz(float x00, float y00, float z00) {
viviien 21:c826abca79c3 76 float y2 = y00 + le;
Feike 18:ab0fe311e7f3 77 float y1 = f;
Feike 18:ab0fe311e7f3 78 float z1 = 0.0;
viviien 21:c826abca79c3 79 float z2 = z00;
viviien 21:c826abca79c3 80 float rje2 = re*re - x00*x00;
Feike 18:ab0fe311e7f3 81 float rje = sqrt(rje2);
viviien 22:9f911405e096 82 float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00);
Feike 18:ab0fe311e7f3 83 float r = sqrt(r2);
Feike 18:ab0fe311e7f3 84
Feike 19:9c8ab7922191 85 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
Feike 19:9c8ab7922191 86 int check = 1;
Feike 19:9c8ab7922191 87 pc.printf("\n\rPunt bestaat niet");
Feike 19:9c8ab7922191 88 }
Feike 19:9c8ab7922191 89 else {
Feike 19:9c8ab7922191 90 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
Feike 19:9c8ab7922191 91 float beta = atan((z1-z2)/(y1-y2));
Feike 19:9c8ab7922191 92 if(beta<0) {
Feike 18:ab0fe311e7f3 93 beta = beta + pi;
Feike 18:ab0fe311e7f3 94 }
Feike 19:9c8ab7922191 95 float theta1 = (beta - alpha)*180.0/pi;
Feike 19:9c8ab7922191 96 return theta1;
viviien 22:9f911405e096 97 }
Feike 18:ab0fe311e7f3 98 }
Feike 18:ab0fe311e7f3 99
viviien 21:c826abca79c3 100 float delta_calcinverse(float x00, float y00, float z00) {
viviien 21:c826abca79c3 101 theta1 = theta2 = theta3 = 0;
viviien 22:9f911405e096 102 x00=x0;
viviien 22:9f911405e096 103 y00=y0;
viviien 22:9f911405e096 104 z00=z0;
viviien 21:c826abca79c3 105 theta1 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 106
viviien 21:c826abca79c3 107 if (check == 0) {
viviien 21:c826abca79c3 108 x00=x0*cospi+y0*sinpi;
viviien 21:c826abca79c3 109 y00=y0*cospi-x0*sinpi;
viviien 21:c826abca79c3 110 z00=z0;
viviien 22:9f911405e096 111 theta2 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 112 x00=x0*cospi-y0*sinpi;
viviien 21:c826abca79c3 113 y00=y0*cospi+y0*sinpi;
viviien 21:c826abca79c3 114 z00=z0;
viviien 22:9f911405e096 115 theta3 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 116 }
viviien 21:c826abca79c3 117
viviien 21:c826abca79c3 118 return theta1, theta2, theta3;
Feike 18:ab0fe311e7f3 119 }
Feike 18:ab0fe311e7f3 120
viviien 23:18b0be02187f 121
Feike 12:2382468d36a4 122
Feike 19:9c8ab7922191 123 float anglestep(float angle) {
Feike 18:ab0fe311e7f3 124 float steps;
Feike 18:ab0fe311e7f3 125 steps = angle / 360 * 8400;
Feike 18:ab0fe311e7f3 126 return steps;
Feike 17:6da57acb7bea 127 }
Feike 17:6da57acb7bea 128
viviien 27:3eb181cbe183 129 float movefunctioninit () {
viviien 27:3eb181cbe183 130
viviien 21:c826abca79c3 131
viviien 22:9f911405e096 132 theta1 = delta_calcinverse(x00,y00,z00);
viviien 21:c826abca79c3 133 pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);
viviien 21:c826abca79c3 134 pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);
viviien 21:c826abca79c3 135
viviien 21:c826abca79c3 136 theta1 = theta1 - oldtheta1;
viviien 21:c826abca79c3 137 theta2 = theta2 - oldtheta2;
viviien 21:c826abca79c3 138 theta3 = theta3 - oldtheta3;
viviien 21:c826abca79c3 139
viviien 25:eb3204e45d33 140 steps1 = anglestep(theta1);
viviien 25:eb3204e45d33 141 steps2 = anglestep(theta2);
viviien 25:eb3204e45d33 142 steps3 = anglestep(theta3);
viviien 27:3eb181cbe183 143 pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
viviien 27:3eb181cbe183 144
viviien 21:c826abca79c3 145 // Set the direction of the motors.
viviien 21:c826abca79c3 146 if (theta1 < 0) {
viviien 21:c826abca79c3 147 motor1_dir.write(1);
viviien 21:c826abca79c3 148 }
viviien 21:c826abca79c3 149 else {
viviien 21:c826abca79c3 150 motor1_dir.write(0);
viviien 21:c826abca79c3 151 }
viviien 21:c826abca79c3 152
viviien 21:c826abca79c3 153 if (theta2 < 0) {
viviien 21:c826abca79c3 154 motor2_dir.write(0);
viviien 21:c826abca79c3 155 }
viviien 21:c826abca79c3 156 else {
viviien 21:c826abca79c3 157 motor2_dir.write(1);
viviien 21:c826abca79c3 158 }
viviien 21:c826abca79c3 159
viviien 21:c826abca79c3 160 if (theta3 < 0) {
viviien 21:c826abca79c3 161 motor3_dir.write(0);
viviien 21:c826abca79c3 162 }
viviien 21:c826abca79c3 163 else {
viviien 21:c826abca79c3 164 motor3_dir.write(1);
viviien 21:c826abca79c3 165 }
viviien 21:c826abca79c3 166
viviien 21:c826abca79c3 167 int frequency_pwm = 10000; //10 kHz PWM
viviien 21:c826abca79c3 168 motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 27:3eb181cbe183 169 motor1_pwm.write(0.57); // write Duty Cycle
viviien 21:c826abca79c3 170
viviien 21:c826abca79c3 171 motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 27:3eb181cbe183 172 motor2_pwm.write(0.57); // write Duty Cycle
viviien 21:c826abca79c3 173
viviien 21:c826abca79c3 174 motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 27:3eb181cbe183 175 motor3_pwm.write(0.57); // write Duty Cycle
viviien 22:9f911405e096 176
viviien 22:9f911405e096 177 check1 = true;
viviien 22:9f911405e096 178 check2 = true;
viviien 22:9f911405e096 179 check3 = true;
viviien 25:eb3204e45d33 180
viviien 27:3eb181cbe183 181 Encoder1.reset();
viviien 27:3eb181cbe183 182 Encoder2.reset();
viviien 27:3eb181cbe183 183 Encoder3.reset();
viviien 27:3eb181cbe183 184 }
viviien 25:eb3204e45d33 185
viviien 27:3eb181cbe183 186 float movefunction() {
viviien 27:3eb181cbe183 187
viviien 21:c826abca79c3 188 while (check1 || check2 || check3) {
viviien 25:eb3204e45d33 189 pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
viviien 25:eb3204e45d33 190
viviien 27:3eb181cbe183 191 if(abs(counts1)>=abs(steps1)) {
viviien 25:eb3204e45d33 192 pc.printf("\n 1 is false");
viviien 21:c826abca79c3 193 motor1_pwm.write(0);
viviien 21:c826abca79c3 194 check1=false;
viviien 27:3eb181cbe183 195 pc.printf("\n\rcounts1 %i", counts1);
viviien 21:c826abca79c3 196 }
viviien 27:3eb181cbe183 197 if (abs(counts2)>=abs(steps2)) {
viviien 24:61d03692f92e 198 pc.printf("\n 2 is false");
viviien 21:c826abca79c3 199 motor2_pwm.write(0);
viviien 21:c826abca79c3 200 check2=false;
viviien 27:3eb181cbe183 201 pc.printf("\n\rcounts2 %i", counts2);
viviien 27:3eb181cbe183 202
viviien 21:c826abca79c3 203 }
viviien 27:3eb181cbe183 204 if (abs(counts3)>=abs(steps3)) {
viviien 24:61d03692f92e 205 pc.printf("\n 3 is false");
viviien 21:c826abca79c3 206 motor3_pwm.write(0);
viviien 21:c826abca79c3 207 check3=false;
viviien 27:3eb181cbe183 208 pc.printf("\n\rcounts3 %i", counts3);
viviien 21:c826abca79c3 209 }
viviien 27:3eb181cbe183 210 }
viviien 27:3eb181cbe183 211 pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
viviien 25:eb3204e45d33 212
viviien 27:3eb181cbe183 213 wait(3.0);
viviien 25:eb3204e45d33 214 pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
viviien 22:9f911405e096 215 }
viviien 22:9f911405e096 216
viviien 22:9f911405e096 217 int main(void) {
viviien 26:432d3519ba86 218
Feike 18:ab0fe311e7f3 219 pc.baud(115200);
Feike 18:ab0fe311e7f3 220
Feike 17:6da57acb7bea 221 char cc = pc.getc();
Feike 19:9c8ab7922191 222
Feike 19:9c8ab7922191 223 while(true) {
viviien 27:3eb181cbe183 224 pulses.attach(&getpulses, 1.0/10000);
viviien 26:432d3519ba86 225
viviien 26:432d3519ba86 226 Encoder1.reset();
viviien 26:432d3519ba86 227 Encoder2.reset();
viviien 26:432d3519ba86 228 Encoder3.reset();
viviien 26:432d3519ba86 229 counts1 = Encoder1.getPulses();
viviien 26:432d3519ba86 230 counts2 = Encoder2.getPulses();
viviien 26:432d3519ba86 231 counts3 = Encoder3.getPulses();
viviien 26:432d3519ba86 232
Feike 18:ab0fe311e7f3 233
Feike 19:9c8ab7922191 234 delta_calcinverse(x0,y0,z0);
Feike 19:9c8ab7922191 235
Feike 19:9c8ab7922191 236 oldtheta1 = theta1;
Feike 19:9c8ab7922191 237 oldtheta2 = theta2;
viviien 21:c826abca79c3 238 oldtheta3 = theta3;
Feike 18:ab0fe311e7f3 239 char cc = pc.getc();
Feike 18:ab0fe311e7f3 240
viviien 21:c826abca79c3 241 if (cc=='d') {
viviien 24:61d03692f92e 242 x0=x0+5.0f;
viviien 22:9f911405e096 243 if (x0>=-75 && x0<=75) {
viviien 27:3eb181cbe183 244 movefunctioninit ();
viviien 22:9f911405e096 245 movefunction ();
viviien 22:9f911405e096 246 }
viviien 22:9f911405e096 247 else {
viviien 24:61d03692f92e 248 x0=x0-5.0f;
viviien 22:9f911405e096 249 }
viviien 21:c826abca79c3 250 }
Feike 19:9c8ab7922191 251
viviien 21:c826abca79c3 252 if (cc=='a') {
viviien 24:61d03692f92e 253 x0=x0-5.0f;
viviien 21:c826abca79c3 254 if (x0>=-75 && x0<=75) {
viviien 27:3eb181cbe183 255 movefunctioninit ();
viviien 21:c826abca79c3 256 movefunction ();
viviien 21:c826abca79c3 257 }
viviien 21:c826abca79c3 258 else {
viviien 24:61d03692f92e 259 x0=x0+5.0f;
viviien 21:c826abca79c3 260 }
viviien 21:c826abca79c3 261 }
viviien 21:c826abca79c3 262
viviien 21:c826abca79c3 263 if (cc=='w') {
viviien 24:61d03692f92e 264 y0=y0+5.0f;
viviien 21:c826abca79c3 265 if (y0>=-75 && y0<=75) {
viviien 27:3eb181cbe183 266 movefunctioninit ();
viviien 21:c826abca79c3 267 movefunction ();
viviien 21:c826abca79c3 268 }
viviien 21:c826abca79c3 269 else {
viviien 24:61d03692f92e 270 y0=y0-5.0f;
viviien 21:c826abca79c3 271 }
Feike 19:9c8ab7922191 272 }
Feike 19:9c8ab7922191 273
viviien 21:c826abca79c3 274 if (cc=='s') {
viviien 24:61d03692f92e 275 y0=y0-5.0f;
viviien 21:c826abca79c3 276 if (y0>=-75 && y0<=75) {
viviien 27:3eb181cbe183 277 movefunctioninit ();
viviien 21:c826abca79c3 278 movefunction ();
viviien 21:c826abca79c3 279 }
viviien 21:c826abca79c3 280 else {
viviien 24:61d03692f92e 281 y0=y0+5.0f;
Feike 19:9c8ab7922191 282 }
viviien 21:c826abca79c3 283 }
viviien 21:c826abca79c3 284
viviien 21:c826abca79c3 285 if (cc=='u') {
viviien 24:61d03692f92e 286 z0=z0+5.0f;
viviien 21:c826abca79c3 287 if (z0>=-210 && z0<=-130) {
viviien 27:3eb181cbe183 288 movefunctioninit ();
viviien 21:c826abca79c3 289 movefunction ();
viviien 21:c826abca79c3 290 }
viviien 21:c826abca79c3 291 else {
viviien 24:61d03692f92e 292 z0=z0-5.0f;
viviien 21:c826abca79c3 293 }
viviien 21:c826abca79c3 294 }
Feike 18:ab0fe311e7f3 295
viviien 21:c826abca79c3 296 if (cc=='j') {
viviien 24:61d03692f92e 297 z0=z0-5.0f;
viviien 21:c826abca79c3 298 if (z0>=-210 && z0<=-130) {
viviien 27:3eb181cbe183 299 movefunctioninit ();
viviien 21:c826abca79c3 300 movefunction ();
viviien 21:c826abca79c3 301 }
viviien 21:c826abca79c3 302 else {
viviien 24:61d03692f92e 303 z0=z0+5.0f;
Feike 18:ab0fe311e7f3 304 }
viviien 21:c826abca79c3 305 }
viviien 22:9f911405e096 306 }
viviien 22:9f911405e096 307 }