Feike van Veen / Mbed 2 deprecated Minor_test_serial

Robot tryout

Dependencies:   mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@29:7eb028b359a1, 2019-10-29 (annotated)

Committer:
viviien
Date:
Tue Oct 29 12:45:05 2019 +0000
Revision:
29:7eb028b359a1
Parent:
28:43a1d67ff8ea
Child:
30:390cab7cd6e6
meer PI

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 28:43a1d67ff8ea 67 // Constant values for PI
viviien 28:43a1d67ff8ea 68 double Kp = 17.5;
viviien 28:43a1d67ff8ea 69 double Ki = 1.02;
viviien 28:43a1d67ff8ea 70 double Ts = 0.0025;
viviien 28:43a1d67ff8ea 71 float new_steps1 = 0;
viviien 28:43a1d67ff8ea 72 float error1 = 0;
viviien 28:43a1d67ff8ea 73
viviien 29:7eb028b359a1 74 float u_k = 0;
viviien 29:7eb028b359a1 75 float u_i = 0;
viviien 29:7eb028b359a1 76
viviien 27:3eb181cbe183 77 Ticker pulses;
viviien 27:3eb181cbe183 78 void getpulses() {
viviien 23:18b0be02187f 79 counts1 = Encoder1.getPulses();
viviien 23:18b0be02187f 80 counts2 = Encoder2.getPulses();
viviien 23:18b0be02187f 81 counts3 = Encoder3.getPulses();
viviien 23:18b0be02187f 82 }
viviien 25:eb3204e45d33 83
viviien 28:43a1d67ff8ea 84
viviien 28:43a1d67ff8ea 85
viviien 21:c826abca79c3 86 float delta_calcangleyz(float x00, float y00, float z00) {
viviien 21:c826abca79c3 87 float y2 = y00 + le;
Feike 18:ab0fe311e7f3 88 float y1 = f;
Feike 18:ab0fe311e7f3 89 float z1 = 0.0;
viviien 21:c826abca79c3 90 float z2 = z00;
viviien 21:c826abca79c3 91 float rje2 = re*re - x00*x00;
Feike 18:ab0fe311e7f3 92 float rje = sqrt(rje2);
viviien 22:9f911405e096 93 float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00);
Feike 18:ab0fe311e7f3 94 float r = sqrt(r2);
Feike 18:ab0fe311e7f3 95
Feike 19:9c8ab7922191 96 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
Feike 19:9c8ab7922191 97 int check = 1;
Feike 19:9c8ab7922191 98 pc.printf("\n\rPunt bestaat niet");
Feike 19:9c8ab7922191 99 }
Feike 19:9c8ab7922191 100 else {
Feike 19:9c8ab7922191 101 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
Feike 19:9c8ab7922191 102 float beta = atan((z1-z2)/(y1-y2));
Feike 19:9c8ab7922191 103 if(beta<0) {
Feike 18:ab0fe311e7f3 104 beta = beta + pi;
Feike 18:ab0fe311e7f3 105 }
Feike 19:9c8ab7922191 106 float theta1 = (beta - alpha)*180.0/pi;
Feike 19:9c8ab7922191 107 return theta1;
viviien 28:43a1d67ff8ea 108 }
viviien 22:9f911405e096 109 }
viviien 28:43a1d67ff8ea 110
viviien 28:43a1d67ff8ea 111
Feike 18:ab0fe311e7f3 112
viviien 21:c826abca79c3 113 float delta_calcinverse(float x00, float y00, float z00) {
viviien 21:c826abca79c3 114 theta1 = theta2 = theta3 = 0;
viviien 22:9f911405e096 115 x00=x0;
viviien 22:9f911405e096 116 y00=y0;
viviien 22:9f911405e096 117 z00=z0;
viviien 21:c826abca79c3 118 theta1 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 119
viviien 21:c826abca79c3 120 if (check == 0) {
viviien 21:c826abca79c3 121 x00=x0*cospi+y0*sinpi;
viviien 21:c826abca79c3 122 y00=y0*cospi-x0*sinpi;
viviien 21:c826abca79c3 123 z00=z0;
viviien 22:9f911405e096 124 theta2 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 125 x00=x0*cospi-y0*sinpi;
viviien 21:c826abca79c3 126 y00=y0*cospi+y0*sinpi;
viviien 21:c826abca79c3 127 z00=z0;
viviien 22:9f911405e096 128 theta3 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 129 }
viviien 21:c826abca79c3 130
viviien 21:c826abca79c3 131 return theta1, theta2, theta3;
Feike 18:ab0fe311e7f3 132 }
Feike 18:ab0fe311e7f3 133
viviien 28:43a1d67ff8ea 134 //double error;
viviien 29:7eb028b359a1 135 Ticker piticker;
viviien 29:7eb028b359a1 136 void PIcontroller()
viviien 28:43a1d67ff8ea 137 {
viviien 29:7eb028b359a1 138 error1 = steps1-counts1;
viviien 29:7eb028b359a1 139 static float error1_integral = 0;
viviien 28:43a1d67ff8ea 140
viviien 28:43a1d67ff8ea 141 // Proportional part
viviien 29:7eb028b359a1 142 u_k = Kp * error1;
viviien 28:43a1d67ff8ea 143
viviien 28:43a1d67ff8ea 144 // Integral part
viviien 28:43a1d67ff8ea 145 error1_integral = error1_integral + error1 * Ts;
viviien 29:7eb028b359a1 146 u_i = Ki * error1_integral;
viviien 29:7eb028b359a1 147 new_steps1 = u_k + u_i;
viviien 28:43a1d67ff8ea 148 }
Feike 12:2382468d36a4 149
viviien 28:43a1d67ff8ea 150 // Calculate the steps from angle
Feike 19:9c8ab7922191 151 float anglestep(float angle) {
Feike 18:ab0fe311e7f3 152 float steps;
Feike 18:ab0fe311e7f3 153 steps = angle / 360 * 8400;
Feike 18:ab0fe311e7f3 154 return steps;
Feike 17:6da57acb7bea 155 }
Feike 17:6da57acb7bea 156
viviien 28:43a1d67ff8ea 157 // Omschrijving
viviien 28:43a1d67ff8ea 158 float movefunctioninit () {
viviien 21:c826abca79c3 159
viviien 22:9f911405e096 160 theta1 = delta_calcinverse(x00,y00,z00);
viviien 21:c826abca79c3 161 pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);
viviien 21:c826abca79c3 162 pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);
viviien 21:c826abca79c3 163
viviien 21:c826abca79c3 164 theta1 = theta1 - oldtheta1;
viviien 21:c826abca79c3 165 theta2 = theta2 - oldtheta2;
viviien 21:c826abca79c3 166 theta3 = theta3 - oldtheta3;
viviien 21:c826abca79c3 167
viviien 25:eb3204e45d33 168 steps1 = anglestep(theta1);
viviien 25:eb3204e45d33 169 steps2 = anglestep(theta2);
viviien 25:eb3204e45d33 170 steps3 = anglestep(theta3);
viviien 27:3eb181cbe183 171 pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
viviien 28:43a1d67ff8ea 172
viviien 28:43a1d67ff8ea 173
viviien 28:43a1d67ff8ea 174 pc.printf("\n\r error1: %f", error1);
viviien 28:43a1d67ff8ea 175 pc.printf("\n\rnew steps1: %f", new_steps1);
viviien 27:3eb181cbe183 176
viviien 21:c826abca79c3 177 // Set the direction of the motors.
viviien 21:c826abca79c3 178 if (theta1 < 0) {
viviien 21:c826abca79c3 179 motor1_dir.write(1);
viviien 21:c826abca79c3 180 }
viviien 28:43a1d67ff8ea 181 else {
viviien 28:43a1d67ff8ea 182 motor1_dir.write(0);
viviien 28:43a1d67ff8ea 183 }
viviien 21:c826abca79c3 184
viviien 21:c826abca79c3 185 if (theta2 < 0) {
viviien 21:c826abca79c3 186 motor2_dir.write(0);
viviien 21:c826abca79c3 187 }
viviien 28:43a1d67ff8ea 188 else {
viviien 28:43a1d67ff8ea 189 motor2_dir.write(1);
viviien 28:43a1d67ff8ea 190 }
viviien 21:c826abca79c3 191
viviien 21:c826abca79c3 192 if (theta3 < 0) {
viviien 21:c826abca79c3 193 motor3_dir.write(0);
viviien 21:c826abca79c3 194 }
viviien 28:43a1d67ff8ea 195 else {
viviien 28:43a1d67ff8ea 196 motor3_dir.write(1);
viviien 28:43a1d67ff8ea 197 }
viviien 21:c826abca79c3 198
viviien 21:c826abca79c3 199 int frequency_pwm = 10000; //10 kHz PWM
viviien 21:c826abca79c3 200 motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 27:3eb181cbe183 201 motor1_pwm.write(0.57); // write Duty Cycle
viviien 21:c826abca79c3 202
viviien 21:c826abca79c3 203 motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 27:3eb181cbe183 204 motor2_pwm.write(0.57); // write Duty Cycle
viviien 21:c826abca79c3 205
viviien 21:c826abca79c3 206 motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 27:3eb181cbe183 207 motor3_pwm.write(0.57); // write Duty Cycle
viviien 22:9f911405e096 208
viviien 22:9f911405e096 209 check1 = true;
viviien 22:9f911405e096 210 check2 = true;
viviien 22:9f911405e096 211 check3 = true;
viviien 25:eb3204e45d33 212
viviien 27:3eb181cbe183 213 Encoder1.reset();
viviien 27:3eb181cbe183 214 Encoder2.reset();
viviien 27:3eb181cbe183 215 Encoder3.reset();
viviien 28:43a1d67ff8ea 216 }
viviien 25:eb3204e45d33 217
viviien 28:43a1d67ff8ea 218 // Omschrijving
viviien 27:3eb181cbe183 219 float movefunction() {
viviien 27:3eb181cbe183 220
viviien 21:c826abca79c3 221 while (check1 || check2 || check3) {
viviien 25:eb3204e45d33 222 pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
viviien 29:7eb028b359a1 223
viviien 29:7eb028b359a1 224 pc.printf("\n\rerror1: %f", error1);
viviien 29:7eb028b359a1 225 pc.printf("\n\ru_k: %f", u_k);
viviien 29:7eb028b359a1 226 pc.printf("\n\ru_i: %f", u_i);
viviien 29:7eb028b359a1 227 pc.printf("\n\ru_k + u_i: %f", (u_k+u_i));
viviien 29:7eb028b359a1 228 pc.printf("\n\r");
viviien 25:eb3204e45d33 229
viviien 27:3eb181cbe183 230 if(abs(counts1)>=abs(steps1)) {
viviien 29:7eb028b359a1 231 pc.printf("\n\r 1 is false");
viviien 21:c826abca79c3 232 motor1_pwm.write(0);
viviien 28:43a1d67ff8ea 233 check1=false;
viviien 27:3eb181cbe183 234 pc.printf("\n\rcounts1 %i", counts1);
viviien 21:c826abca79c3 235 }
viviien 28:43a1d67ff8ea 236
viviien 27:3eb181cbe183 237 if (abs(counts2)>=abs(steps2)) {
viviien 29:7eb028b359a1 238 pc.printf("\n\r 2 is false");
viviien 21:c826abca79c3 239 motor2_pwm.write(0);
viviien 21:c826abca79c3 240 check2=false;
viviien 27:3eb181cbe183 241 pc.printf("\n\rcounts2 %i", counts2);
viviien 21:c826abca79c3 242 }
viviien 28:43a1d67ff8ea 243
viviien 27:3eb181cbe183 244 if (abs(counts3)>=abs(steps3)) {
viviien 29:7eb028b359a1 245 pc.printf("\n\r 3 is false");
viviien 21:c826abca79c3 246 motor3_pwm.write(0);
viviien 21:c826abca79c3 247 check3=false;
viviien 27:3eb181cbe183 248 pc.printf("\n\rcounts3 %i", counts3);
viviien 21:c826abca79c3 249 }
viviien 27:3eb181cbe183 250 }
viviien 27:3eb181cbe183 251 pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
viviien 25:eb3204e45d33 252
viviien 27:3eb181cbe183 253 wait(3.0);
viviien 25:eb3204e45d33 254 pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
viviien 22:9f911405e096 255 }
viviien 22:9f911405e096 256
viviien 28:43a1d67ff8ea 257
viviien 28:43a1d67ff8ea 258 ///////////////////
viviien 28:43a1d67ff8ea 259 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 260 ///////////////////
viviien 28:43a1d67ff8ea 261
viviien 22:9f911405e096 262 int main(void) {
viviien 26:432d3519ba86 263
viviien 28:43a1d67ff8ea 264 pc.baud(115200);
Feike 17:6da57acb7bea 265 char cc = pc.getc();
Feike 19:9c8ab7922191 266
viviien 28:43a1d67ff8ea 267 while(true){
viviien 29:7eb028b359a1 268 pc.printf("\n\r----------------------------------------------------------");
viviien 29:7eb028b359a1 269 pc.printf("\n\r----------------------------------------------------------");
viviien 28:43a1d67ff8ea 270 pulses.attach(&getpulses, 1.0/10000);
viviien 29:7eb028b359a1 271 piticker.attach(&PIcontroller, 1.0/1000);
viviien 28:43a1d67ff8ea 272
viviien 28:43a1d67ff8ea 273 Encoder1.reset();
viviien 28:43a1d67ff8ea 274 Encoder2.reset();
viviien 28:43a1d67ff8ea 275 Encoder3.reset();
viviien 28:43a1d67ff8ea 276 counts1 = Encoder1.getPulses();
viviien 28:43a1d67ff8ea 277 counts2 = Encoder2.getPulses();
viviien 28:43a1d67ff8ea 278 counts3 = Encoder3.getPulses();
viviien 28:43a1d67ff8ea 279
viviien 28:43a1d67ff8ea 280 delta_calcinverse(x0,y0,z0);
viviien 26:432d3519ba86 281
viviien 28:43a1d67ff8ea 282 oldtheta1 = theta1;
viviien 28:43a1d67ff8ea 283 oldtheta2 = theta2;
viviien 28:43a1d67ff8ea 284 oldtheta3 = theta3;
viviien 28:43a1d67ff8ea 285
viviien 28:43a1d67ff8ea 286 char cc = pc.getc();
viviien 28:43a1d67ff8ea 287
viviien 28:43a1d67ff8ea 288 if (cc=='d') {
viviien 28:43a1d67ff8ea 289 x0=x0+5.0f;
viviien 28:43a1d67ff8ea 290 if (x0>=-75 && x0<=75){
viviien 28:43a1d67ff8ea 291 movefunctioninit ();
viviien 28:43a1d67ff8ea 292 movefunction ();
viviien 28:43a1d67ff8ea 293 }
viviien 28:43a1d67ff8ea 294 else {
viviien 28:43a1d67ff8ea 295 x0=x0-5.0f;
viviien 28:43a1d67ff8ea 296 }
viviien 22:9f911405e096 297 }
viviien 28:43a1d67ff8ea 298
viviien 28:43a1d67ff8ea 299 if (cc=='a') {
viviien 28:43a1d67ff8ea 300 x0=x0-5.0f;
viviien 28:43a1d67ff8ea 301 if (x0>=-75 && x0<=75){
viviien 28:43a1d67ff8ea 302 movefunctioninit ();
viviien 28:43a1d67ff8ea 303 movefunction ();
viviien 28:43a1d67ff8ea 304 }
viviien 28:43a1d67ff8ea 305 else {
viviien 28:43a1d67ff8ea 306 x0=x0+5.0f;
viviien 21:c826abca79c3 307 }
viviien 28:43a1d67ff8ea 308 }
viviien 28:43a1d67ff8ea 309
viviien 28:43a1d67ff8ea 310 if (cc=='w'){
viviien 28:43a1d67ff8ea 311 y0=y0+5.0f;
viviien 28:43a1d67ff8ea 312 if (y0>=-75 && y0<=75){
viviien 28:43a1d67ff8ea 313 movefunctioninit ();
viviien 28:43a1d67ff8ea 314 movefunction ();
viviien 28:43a1d67ff8ea 315 }
viviien 28:43a1d67ff8ea 316 else {
viviien 28:43a1d67ff8ea 317 y0=y0-5.0f;
viviien 21:c826abca79c3 318 }
viviien 28:43a1d67ff8ea 319 }
viviien 28:43a1d67ff8ea 320
viviien 28:43a1d67ff8ea 321 if (cc=='s'){
viviien 28:43a1d67ff8ea 322 y0=y0-5.0f;
viviien 28:43a1d67ff8ea 323 if (y0>=-75 && y0<=75) {
viviien 28:43a1d67ff8ea 324 movefunctioninit ();
viviien 28:43a1d67ff8ea 325 movefunction ();
viviien 28:43a1d67ff8ea 326 }
viviien 28:43a1d67ff8ea 327 else {
viviien 28:43a1d67ff8ea 328 y0=y0+5.0f;
Feike 19:9c8ab7922191 329 }
viviien 28:43a1d67ff8ea 330 }
viviien 28:43a1d67ff8ea 331
viviien 28:43a1d67ff8ea 332 if (cc=='u')
viviien 28:43a1d67ff8ea 333 {
viviien 28:43a1d67ff8ea 334 z0=z0+5.0f;
viviien 28:43a1d67ff8ea 335 if (z0>=-210 && z0<=-130) {
viviien 28:43a1d67ff8ea 336 movefunctioninit ();
viviien 28:43a1d67ff8ea 337 movefunction ();
viviien 28:43a1d67ff8ea 338 }
viviien 28:43a1d67ff8ea 339 else{
viviien 28:43a1d67ff8ea 340 z0=z0-5.0f;
viviien 21:c826abca79c3 341 }
viviien 28:43a1d67ff8ea 342 }
viviien 28:43a1d67ff8ea 343
viviien 28:43a1d67ff8ea 344 if (cc=='j')
viviien 28:43a1d67ff8ea 345 {
viviien 28:43a1d67ff8ea 346 z0=z0-5.0f;
viviien 28:43a1d67ff8ea 347 if (z0>=-210 && z0<=-130){
viviien 28:43a1d67ff8ea 348 movefunctioninit ();
viviien 28:43a1d67ff8ea 349 movefunction ();
viviien 28:43a1d67ff8ea 350 }
viviien 28:43a1d67ff8ea 351 else {
viviien 28:43a1d67ff8ea 352 z0=z0+5.0f;
viviien 28:43a1d67ff8ea 353 }
viviien 28:43a1d67ff8ea 354 }
viviien 28:43a1d67ff8ea 355 // end while
viviien 21:c826abca79c3 356 }
viviien 29:7eb028b359a1 357
viviien 28:43a1d67ff8ea 358 // end main
viviien 22:9f911405e096 359 }