working PID + Kinematics + Motorcontrol
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 7:83a69ca630bc
- Parent:
- 6:6545e197858a
- Child:
- 8:cfe7ad86837c
--- a/main.cpp Wed Oct 31 19:52:34 2018 +0000 +++ b/main.cpp Wed Oct 31 20:03:47 2018 +0000 @@ -167,7 +167,7 @@ float alpha1 = atan2((c1y-a1y),(c1x-a1x)); // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt float psi1 = acos(( pow(la,2)-pow(lp,2)+pow((c1x-a1x),2)+pow((c1y-a1y),2))/(2*la*sqrt(pow ((c1x-a1x),2)+pow((c1y-a1y),2) ))); //Hoek tussen lijn van motor naar bijbehorende end=effector punt en actieve arm float a1 = alpha1 + psi1 - da1; //hoek tussen horizontaal en actieve arm - printf("arm 1 = %f \n\r",a1); + //printf("arm 1 = %f \n\r",a1); return a1; } @@ -179,7 +179,7 @@ float alpha2 = atan2((c2y-a2y),(c2x-a2x)); float psi2 = acos(( pow(la,2)-pow(lp,2)+pow((c2x-a2x),2)+pow((c2y-a2y),2))/(2*la*sqrt(pow ((c2x-a2x),2)+pow((c2y-a2y),2) ))); float a2 = alpha2 + psi2 - da2; - printf("arm 2 = %f \n\r",a2); + //printf("arm 2 = %f \n\r",a2); return a2; } @@ -191,7 +191,7 @@ float alpha3 = atan2((c3y-a3y),(c3x-a3x)); float psi3 = acos(( pow(la,2)-pow(lp,2)+pow((c3x-a3x),2)+pow((c3y-a3y),2))/(2*la*sqrt(pow ((c3x-a3x),2)+pow((c3y-a3y),2) ))); float a3 = alpha3 + psi3 - da3; - printf("arm 3 = %f \n\r",a3); + //printf("arm 3 = %f \n\r",a3); return a3; } @@ -322,7 +322,7 @@ float a2 = hoek2(px, py); float a3 = hoek3(px, py); -printf("hoek(%f,%f,%f)\n\r",a1,a2,a3); +//printf("hoek(%f,%f,%f)\n\r",a1,a2,a3); return 0; */