![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Motor_tryout.cpp
- Committer:
- Feike
- Date:
- 2019-10-25
- Revision:
- 19:9c8ab7922191
- Parent:
- 18:ab0fe311e7f3
- Child:
- 21:c826abca79c3
File content as of revision 19:9c8ab7922191:
#include "mbed.h" #include "MODSERIAL.h" #include "QEI1.h" #include "QEI2.h" #include "QEI.h" #include "Math.h" #include "ttmath.h" MODSERIAL pc(USBTX, USBRX); //Serial term (USBTX, USBRX); PwmOut motor1_pwm(PTC2); DigitalOut motor1_dir(PTC3); PwmOut motor2_pwm(PTA2); DigitalOut motor2_dir(PTB23); PwmOut motor3_pwm(PTC4); DigitalOut motor3_dir(PTC12); QEI1 Encoder1(D12,D13,NC,64,QEI1::X4_ENCODING); QEI2 Encoder2(D10,D11,NC,64,QEI2::X4_ENCODING); QEI Encoder(D2,D3,NC,64,QEI::X4_ENCODING); int check; int quit; int limit_pos = 8400; float steps; int g = 0; bool check1; bool check2; bool check3; int counts1 = 0; int counts2 = 0; int counts3 = 0; const float le = 15.0; const float f = 37.5; const float re = 174.0; const float rf = 50.0; const float pi = 3.14159265358979323846; float y2; float y1; float z1; float z2; float rje2; float rje; float r2; float r; float z0=-172; float y0=0; float x0=0; float theta1; float theta2; float theta3; float oldtheta1=0; float oldtheta2=0; float oldtheta3=0; float delta_calcangleyz(float x0, float y0, float z0) { float y2 = y0 + le; float y1 = f; float z1 = 0.0; float z2 = z0; float rje2 = re*re - x0*x0; float rje = sqrt(rje2); float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0); float r = sqrt(r2); if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) { int check = 1; pc.printf("\n\rPunt bestaat niet"); } else { float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r)); float beta = atan((z1-z2)/(y1-y2)); if(beta<0) { beta = beta + pi; } float theta1 = (beta - alpha)*180.0/pi; return theta1; } } float delta_calcinverse(float x0, float y0, float z0) { theta1 = theta2 = theta3 = 0; theta1 = delta_calcangleyz(x0, y0, z0); if (check == 0) { theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0); theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0); } return theta1, theta2, theta3; } Ticker pulse; void pulseget() { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); counts3 = Encoder.getPulses(); } float anglestep(float angle) { float steps; steps = angle / 360 * 8400; return steps; } // DE MAIN FUNCTIE int main(void) { pc.baud(115200); char cc = pc.getc(); while(true) { counts1 = 0; counts2 = 0; counts3 = 0; delta_calcinverse(x0,y0,z0); oldtheta1 = theta1; oldtheta2 = theta2; oldtheta3 = theta3; char cc = pc.getc(); if (cc=='a') { check1=true; check2=true; check3=true; z0=z0+2.0f; theta1 = delta_calcinverse(x0,y0,z0); theta1 = theta1 - oldtheta1; theta2 = theta2 - oldtheta2; theta3 = theta3 - oldtheta3; float steps1 = anglestep(theta1); float steps2 = anglestep(theta2); float steps3 = anglestep(theta3); motor1_dir.write(1); // positief motor2_dir.write(0); // positief motor3_dir.write(1); // positief if (steps1>0) { motor1_dir.write(0); // positief } if (steps1>0) { motor2_dir.write(1); // positief } if (steps1>0) { motor3_dir.write(0); // positief } int frequency_pwm = 10000; //10 kHz PWM motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f motor1_pwm.write(0.7); // write Duty Cycle motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f motor2_pwm.write(0.7); // write Duty Cycle motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f motor3_pwm.write(0.7); // write Duty Cycle wait(1.0/100); while (check1 || check2 || check3) { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); counts3 = Encoder.getPulses(); wait(1.0/1000); //pc.printf("while"); if(abs(counts1)>=abs(steps1)) { motor1_pwm.write(0); check1=false; counts1=0; } if (abs(counts2)>=abs(steps2)) { motor2_pwm.write(0); check2=false; counts2=0; } if (abs(counts3)>=abs(steps3)) { motor3_pwm.write(0); check3=false; counts3=0; } } } if (cc=='d') { check1=true; check2=true; check3=true; x0=x0-2.0f; theta1 = delta_calcinverse(x0,y0,z0); pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); theta1 = theta1 - oldtheta1; theta2 = theta2 - oldtheta2; theta3 = theta3 - oldtheta3; float steps1 = anglestep(theta1); float steps2 = anglestep(theta2); float steps3 = anglestep(theta3); //pc.printf("\n\r (%f, %f, %f)", steps1, steps2, steps3); motor1_dir.write(0); // positief motor2_dir.write(1); // positief motor3_dir.write(0); // positief if (steps1<0) { motor1_dir.write(1); // positief } if (steps1<0) { motor2_dir.write(0); // positief } if (steps1<0) { motor3_dir.write(1); // positief } int frequency_pwm = 10000; //10 kHz PWM motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f motor1_pwm.write(0.7); // write Duty Cycle motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f motor2_pwm.write(0.7); // write Duty Cycle motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f motor3_pwm.write(0.7); // write Duty Cycle while (check1 || check2 || check3) { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); counts3 = Encoder.getPulses(); wait(1.0/500); //pc.printf("while"); if(abs(counts1)>=abs(steps1)) { motor1_pwm.write(0); check1=false; counts1=0; wait(1.0/100); } if (abs(counts2)>=abs(steps2)) { motor2_pwm.write(0); check2=false; counts2=0; wait(1.0/100); } if (abs(counts3)>=abs(steps3)) { motor3_pwm.write(0); check3=false; //pc.printf("\n\rsteps %f, counts %f", steps3, counts3); counts3=0; wait(1.0/100); } } } wait(1.0/100); } }