![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Motor_tryout.cpp
- Committer:
- viviien
- Date:
- 2019-10-31
- Revision:
- 31:3c13f1c35ee5
- Parent:
- 30:390cab7cd6e6
- Child:
- 32:60a71dcfdb7a
File content as of revision 31:3c13f1c35ee5:
#include "mbed.h" #include "MODSERIAL.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); AnalogIn potmeter1(A1); QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING); float steps1 = 0; float steps2 = 0; float steps3 = 0; 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; const float cospi = -0.5; const float sinpi = 0.8660254; 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 x00; float y00; float z00; float theta1; float theta2; float theta3; float oldtheta1=0; float oldtheta2=0; float oldtheta3=0; // Constant values for PI float Kp; float Kd; float Ts = 0.0025; float error1 = 0; float error2 = 0; float error3 = 0; float newmotor1 = 1; float newmotor2 = 1; float newmotor3 = 1; float u_k1 = 0; float u_k2 = 0; float u_k3 = 0; float u_d1 = 0; float u_d2 = 0; float u_d3 = 0; float lasterror1 = 0; float lasterror2 = 0; float lasterror3 = 0; float derivative1 = 0; float derivative2 = 0; float derivative3 = 0; //bool timer = false; Ticker motor_timer; void motor() { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); counts3 = Encoder3.getPulses(); // angle1 = counts1/(8400.0)*2.0*pie; // angle2 = counts2/(8400.0)*2.0*pie; // angle3 = counts3/(8400.0)*2.0*pie; // sinewave = sin(time_sin); lasterror1 = error1; lasterror2 = error2; lasterror3 = error3; error1 = counts1 - steps1; error2 = counts2 - steps2; error3 = counts3 - steps3; derivative1 = error1 - lasterror1; derivative2 = error2 - lasterror2; derivative3 = error3 - lasterror3; Kp = 50; Kd = potmeter1 * 1000; // Proportional part u_k1 = Kp * error1; u_k2 = Kp * error2; u_k3 = Kp * error3; u_d1 = Kd * derivative1; u_d2 = Kd * derivative2; u_d3 = Kd * derivative3; newmotor1 = u_k1 + u_d1; newmotor2 = u_k2 + u_d2; newmotor3 = u_k3 + u_d3; if (newmotor1>1.0f){ newmotor1 =1.0f; } if (newmotor1<-1.0f){ newmotor1 = -1.0f; } if (newmotor2>1.0f){ newmotor2 =1.0f; } if (newmotor2<-1.0f){ newmotor2 = -1.0f; } if (newmotor3>1.0f){ newmotor3 =1.0f; } if (newmotor3<-1.0f){ newmotor3 = -1.0f; } if (timer==true) { motor1_pwm.write(fabs(newmotor1)); motor1_dir.write(newmotor1>0); motor2_pwm.write(fabs(newmotor2)); motor2_dir.write(newmotor2<0); motor3_pwm.write(fabs(newmotor3)); motor3_dir.write(newmotor3>0); } // else { // motor1_pwm.write(0); // motor2_pwm.write(0); // motor3_pwm.write(0); // } } float delta_calcangleyz(float x00, float y00, float z00) { float y2 = y00 + le; float y1 = f; float z1 = 0.0; float z2 = z00; float rje2 = re*re - x00*x00; float rje = sqrt(rje2); float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00); 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 x00, float y00, float z00) { theta1 = theta2 = theta3 = 0; x00=x0; y00=y0; z00=z0; theta1 = delta_calcangleyz(x00, y00, z00); if (check == 0) { x00=x0*cospi+y0*sinpi; y00=y0*cospi-x0*sinpi; z00=z0; theta2 = delta_calcangleyz(x00, y00, z00); x00=x0*cospi-y0*sinpi; y00=y0*cospi+y0*sinpi; z00=z0; theta3 = delta_calcangleyz(x00, y00, z00); } return theta1, theta2, theta3; } //double error; // Calculate the steps from angle float anglestep(float angle) { float steps; steps = angle / 360 * 8400; return steps; } // Omschrijving float movefunctioninit () { theta1 = delta_calcinverse(x00,y00,z00); // theta1 = theta1 - oldtheta1; // theta2 = theta2 - oldtheta2; // theta3 = theta3 - oldtheta3; // // steps1 = steps1 + anglestep(theta1); // steps2 = steps2 + anglestep(theta2); // steps3 = steps3 + anglestep(theta3); steps1 = anglestep(theta1); steps2 = anglestep(theta2); steps3 = anglestep(theta3); pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3); pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3); // pc.printf("\n\r error1: %f", error1); // Set the direction of the motors. // if (theta1 < 0) { // motor1_dir.write(1); // } // else { // motor1_dir.write(0); // } // // if (theta2 < 0) { // motor2_dir.write(0); // } // else { // motor2_dir.write(1); // } // // if (theta3 < 0) { // motor3_dir.write(0); // } // else { // motor3_dir.write(1); // } // // int frequency_pwm = 10000; //10 kHz PWM // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor1_pwm.write(0.57); // write Duty Cycle // // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor2_pwm.write(0.57); // write Duty Cycle // // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor3_pwm.write(0.57); // write Duty Cycle // // check1 = true; // check2 = true; // check3 = true; // // Encoder1.reset(); // Encoder2.reset(); // Encoder3.reset(); // } // Omschrijving //float movestop() { //wait(0.6); //timer = false; //} // // while (check1 || check2 || check3) { // // int frequency_pwm = 10000; //10 kHz PWM // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor1_pwm.write(newmotor1); // write Duty Cycle // // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor2_pwm.write(newmotor2); // write Duty Cycle // // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor3_pwm.write(newmotor3); // write Duty Cycle // // if (error1==0) { // check1=false; // } // if (error2==0) { // check2=false; // } // if (error3==0) { // check3=false; // } // //// if(abs(counts1)>=abs(steps1)) { //// pc.printf("\n\r 1 is false"); //// motor1_pwm.write(0); //// check1=false; //// pc.printf("\n\rcounts1 %i", counts1); //// } //// //// if (abs(counts2)>=abs(steps2)) { //// pc.printf("\n\r 2 is false"); //// motor2_pwm.write(0); //// check2=false; //// pc.printf("\n\rcounts2 %i", counts2); //// } //// //// if (abs(counts3)>=abs(steps3)) { //// pc.printf("\n\r 3 is false"); //// motor3_pwm.write(0); //// check3=false; //// pc.printf("\n\rcounts3 %i", counts3); //// } // } // wait(1.0); // pc.printf("\n\rdone moving"); //} /////////////////// // MAIN FUNCTION // /////////////////// int main(void) { motor_timer.attach(&motor, 0.002); pc.baud(115200); char cc = pc.getc(); int frequency_pwm = 10000; //10 kHz PWM // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor1_pwm.write(newmotor1); // write Duty Cycle // // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor2_pwm.write(newmotor2); // write Duty Cycle // // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f // motor3_pwm.write(newmotor3); // write Duty Cycle while(true){ pc.printf("\n\r----------------------------------------------------------"); pc.printf("\n\r----------------------------------------------------------"); delta_calcinverse(x0,y0,z0); oldtheta1 = theta1; oldtheta2 = theta2; oldtheta3 = theta3; char cc = pc.getc(); // timer = true; if (cc=='d') { x0=x0+5.0f; if (x0>=-75 && x0<=75){ movefunctioninit (); // movestop(); } else { // timer = false; x0=x0-5.0f; } } if (cc=='a') { x0=x0-5.0f; if (x0>=-75 && x0<=75){ movefunctioninit (); // movestop(); } else { // timer = false; x0=x0+5.0f; } } if (cc=='w'){ y0=y0+5.0f; if (y0>=-75 && y0<=75){ movefunctioninit (); // movestop(); } else { // timer = false; y0=y0-5.0f; } } if (cc=='s'){ y0=y0-5.0f; if (y0>=-75 && y0<=75) { movefunctioninit (); // movestop(); } else { // timer = false; y0=y0+5.0f; } } if (cc=='u') { z0=z0+1.0f; if (z0>=-210 && z0<=-130) { movefunctioninit (); pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3); pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3); // movestop(); // for (float i=0.0; i<5; i++) { // pc.printf("\n\r dit is error1 %f, error2 %f, error3 %f", error1, error2, error3); // wait(1.0/10.0); // } } else{ pc.printf("else statement"); // timer = false; z0=z0-5.0f; } } if (cc=='j') { z0=z0-1.0f; if (z0>=-210 && z0<=-130){ movefunctioninit (); // movestop(); } else { // timer = false; z0=z0+5.0f; } } // end while } // end main }