![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 35:4cb2ed6dd2d2
- Parent:
- 33:88fbf14d8aaf
- Child:
- 36:22d1bcb82061
--- a/Motor_tryout.cpp Thu Oct 31 15:02:17 2019 +0000 +++ b/Motor_tryout.cpp Fri Nov 01 08:24:42 2019 +0000 @@ -22,22 +22,21 @@ 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; +float savedX = 0; +float savedY = 0; +float savedZ = 0; +int sign = 0; + const float le = 15.0; const float f = 37.5; @@ -55,20 +54,17 @@ float r2; float r; -float z0=-172; +float z0=-158; float y0=0; float x0=0; 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; @@ -83,23 +79,10 @@ 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; - float angle1 = 0; float angle2 = 0; float angle3 = 0; -bool timer = false; float time_sin = 0; @@ -119,11 +102,25 @@ } else { float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r)); - float beta = atan((z1-z2)/(y1-y2)); - if(beta<0) { + float beta = atan((z1+z2)/(y1-y2)); + if(beta<=0) { beta = beta + pi; + if (beta>=alpha) { + theta1 = beta-alpha; + } + else { + theta1 = -alpha+beta; + } + } - theta1 = (beta - alpha); + if(beta>0) { + if (beta<=alpha) { + theta1 = -alpha+beta; + } + else { + theta1 = beta-alpha; + } + } } } @@ -144,19 +141,33 @@ 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; + else { + float alpha2 = acos((r2 + rf*rf -rje2)/(2*rf*r)); + float beta2 = atan((z1+z2)/(y1-y2)); + if(beta2<=0) { + beta2 = beta2 + pi; + if (beta2>=alpha2) { + theta2 = beta2-alpha2; + } + else { + theta2 = -alpha2+beta2; + } + } - theta2 = (beta - alpha); + if(beta2>0) { + if (beta2<=alpha2) { + theta2 = -alpha2+beta2; + } + else { + theta2 = beta2-alpha2; + } + } } } void delta_calctheta3 () { float xreff = x0*cospi-y0*sinpi; - float yreff = y0*cospi+y0*sinpi; + float yreff = y0*cospi+x0*sinpi; float zreff = z0; float y2 = yreff + le; float y1 = f; @@ -171,13 +182,27 @@ 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; + else { + float alpha3 = acos((r2 + rf*rf -rje2)/(2*rf*r)); + float beta3 = atan((z1+z2)/(y1-y2)); + if(beta3<=0) { + beta3 = beta3 + pi; + if (beta3>=alpha3) { + theta3 = beta3-alpha3; + } + else { + theta3 = -alpha3+beta3; + } + } - theta3 = (beta - alpha); + if(beta3>0) { + if (beta3<=alpha3) { + theta3 = -alpha3+beta3; + } + else { + theta3 = beta3-alpha3; + } + } } } @@ -191,11 +216,13 @@ counts2 = Encoder2.getPulses(); counts3 = Encoder3.getPulses(); -// z0 = -172.0 + 10* sin(time_sin); +//z0 = 158 - 60* sin(3*time_sin); -float r = 20*(1-1/(1+(time_sin))); - x0 = r*sin(6.28 * time_sin); - y0 = r*cos(6.28 * time_sin); +if (sign == 1) { + float r = 20*(1-1/(1+(time_sin))); + x0 = savedX0 - r*sin(3*time_sin); + y0 = savedY0 - r*cos(3*time_sin); +} delta_calctheta1 (); @@ -210,18 +237,18 @@ error2 = theta2 - angle2; error3 = theta3 - angle3; - Kp = potmeter1 * 25; + Kp = potmeter1 * 10; - u_k1 = Kp * error1; - u_k2 = Kp * error2; - u_k3 = Kp * error3; + u_k1 = 10 * error1; + u_k2 = 10 * error2; + u_k3 = 10 * error3; newmotor1 = u_k1; newmotor2 = u_k2; newmotor3 = u_k3; if (newmotor1>1.0f){ - newmotor1 =1.0f; + newmotor1 = 1.0f; } if (newmotor1<-1.0f){ newmotor1 = -1.0f; @@ -241,6 +268,7 @@ newmotor3 = -1.0f; } + motor1_pwm.write(fabs(newmotor1)); motor1_dir.write(newmotor1<0); @@ -259,20 +287,157 @@ // MAIN FUNCTION // /////////////////// -int main(void) { - motor_timer.attach(&motor, 0.002); +int main(void) { + + delta_calctheta1 (); + delta_calctheta2 (); + delta_calctheta3 (); + + pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); - pc.baud(115200); char cc = pc.getc(); + motor_timer.attach(&motor, 0.002); + + int frequency_pwm = 10000; //10 kHz PWM motor1_pwm.period_us(6); motor2_pwm.period_us(6); motor3_pwm.period_us(6); + + pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); + pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); + pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); + + + + pc.baud(115200); while(true){ + while (boxcheckC == 0) { + if (boxcheckA == 1 && x0>=-76 && x0<=75) { + x0=x0+1.0f ; + wait(1.0/20); + } + if (boxcheckB== 1 && x0>=-75 && x0<=76) { + x0=x0-1.0f; + wait(1.0/20); + } + } + wait(1.5); + savedX = x0; + + while (boxcheckC == 0) { + if (boxcheckA==1 && y0>=-76 && y0<=75){ + y0=y0+1.0f; + wait(1.0/20); + } + if (boxcheckB==1 && y0>=-75 && y0<=76){ + y0=y0-1.0f; + wait(1.0/20); + } + } + + wait(1.5); + savedY = y0; + + while (boxcheckC == 0) { + if (boxcheckA == 1 && z0>=-179 && z0<=-158){ + z0=z0+1.0f; + wait(1.0/20); + } + if (boxcheckB == 1 && z0>=-178 && z0<=-157){ + z0=z0-1.0f; + wait(1.0/20); + } + } + + wait(1.5); + savedZ = z0; + + if (boxcheckC == 1) { + sign = 1; + } + + wait(5.0); + + sign = 0; + + float diffZ = -158 - z0; + float diffX = 0 - x0; + float diffY = 0 - y0; + + for (int i=0; i<=diffZ; i++) { + z0 = z0+1; + } + + for (int i=0; i<=diffY; i++) { + if (diffY>0) { + y0 = y0+1; + } + if (diffY<0) { + y0 = y0-1; + } + } + + for (int i=0; i<=diffX; i++) { + if (diffX>0) { + x0 = x0+1; + } + if (diffX<0) { + x0 = x0-1; + } + } + +break; + + + +// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); +// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); +// wait(0.1); + + +// char cc = pc.getc(); +// if (cc=='d' && x0>=-76 && x0<=75) { +// x0=x0+1.0f ; +// } +// +// if (cc=='a' && x0>=-75 && x0<=76) { +// +// x0=x0-1.0f; +// } +// +// if (cc=='w' && y0>=-76 && y0<=75){ +// y0=y0+1.0f; +// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); +// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); +// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); +// +// } +// +// if (cc=='s' && y0>=-75 && y0<=76){ +// y0=y0-1.0f; +// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); +// pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); +// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); +// } +// +// if (cc=='u' && z0>=-211 && z0<=-130){ +// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); +// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); +// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); +// z0=z0+1.0f; +// } +// +// if (cc=='j' && z0>=-210 && z0<=-129){ +// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); +// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); +// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); +// z0=z0-1.0f; +// } } //END MAIN } \ No newline at end of file