Feike van Veen / Mbed 2 deprecated Minor_test_serial

Robot tryout

Dependencies:   mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@19:9c8ab7922191, 2019-10-25 (annotated)

Committer:
Feike
Date:
Fri Oct 25 07:18:00 2019 +0000
Revision:
19:9c8ab7922191
Parent:
18:ab0fe311e7f3
Child:
21:c826abca79c3
QEI 1

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