Position Control of a 3-DOF Manipulator
Last Updated on Saturday, 07 August 2010 04:01 Tuesday, 22 December 2009 07:15
Articulated Manipulator Diagram:
SolidWorks 3D Model :
Position Control on DSP Microcontroller:
// output signal - duty, adc_a_data[0], range : 0 (-100%) - 2047 (0%) - 4096 (100%)
// command signal - speed(RPM)
// feedback signal - speed(RPM)
// direction of motor rotation
/*if(motor_dir == -1) current_motor_rpm = (long)motor_rpm; // CCW
else current_motor_rpm = motor_rpm * (-1); // CW */
//current_motor_rpm = (long)motor_rpm; // Get current RPM
if (iteration==40)
{
iteration=0;
ref_position=command_position;
command_position=0;
}
else
{iteration+=1;
command_position+=Voltage;
}
current_position = (long)EQep1Regs.QPOSCNT; // Get the current strut position
error_position = ref_position - current_position; // Get position error
error_delta = pre_position-current_position;
//Position Control
command_rpm = (-1) * (error_position * kp_position + error_delta * kd_position+ ki_position*error_integral) ;
command_rpm = command_rpm / N_GAIN;
// Limit output value
if(command_rpm > 2000) command_rpm = 2000;
if(command_rpm < (-2000)) command_rpm = (-2000);
command = command_rpm+2047;
adc_a_data[0] = command;
// Save current error
error_integral += error_position;
error_before = error_position;
pre_position = current_position;
float deg1, deg2, deg3, X, Y, Z, error, mult;
int8 possible;
if (Xc<0) offset = pi;
//else if (Xc>=0) if (Yc<0) offset = pi;
else offset = 0;
if (Xc==0) Xc=0.00001; // to avoid division by zero
Theta1 = atan(abs(Yc/Xc)) - pi/2 + offset;
D = (Xc*Xc + Yc* Yc + (Zc - d1)* (Zc - d1) - a2*a2 - a3*a3)/(2*a2*a3);
if (D<0) offset = pi;
else offset =0;
Theta3 = -atan(sqrt(abs(1-D*D))/D)+offset;
if (abs(Theta3) > limit3) Theta3 = atan(sqrt(abs(1-D*D))/D)+offset;
//if ((Xc/Yc) < 0) Theta3 = -1*Theta3;
//Theta31 = -atan(sqrt(abs(1-D*D))/D)+offset; Theta31 = Theta31/pi*180;
//Theta3 = Theta3+offset;
if ((a2+a3*cos(Theta3))<0) offset = pi;
else offset =0;
if (Yc < 0) mult = -1;
else mult = 1;
Theta2 = mult*atan((Zc-d1)/sqrt(Xc*Xc+Yc*Yc)) -atan(a3*sin(Theta3)/(a2+a3*cos(Theta3)))-pi/2*mult+offset;
// round angles
if (abs(Theta1)<1E-5) Theta1 = 0;
if (abs(Theta2)<1E-5) Theta2 = 0;
if (abs(Theta3)<1E-5) Theta3 = 0;
// restrict Theta1
limit1 = 165/180*pi;
if (Theta1 > limit1) Theta1 = limit1;
if (Theta1 < -limit1) Theta1 = -limit1;
// restrict Theta2
limit2 = 120/180*pi;
if (Theta2 > limit2) Theta2 = limit2;
if (Theta2 < -limit2) Theta2 = -limit2;
// restrict Theta3
limit3 = 160/180*pi;
if (Theta3 > limit3) Theta3 = limit3;
if (Theta3 < -limit3) Theta3 = -limit3;
deg1 = Theta1/pi*180;
deg2 = Theta2/pi*180;
deg3 = Theta3/pi*180;
// solve forward kinematics in order to check reachability
Theta1 = Theta1 - pi/2; Theta3 = -1*Theta3;
X = (-cos(Theta1)*cos(Theta2)*sin(Theta3)+cos(Theta1)*sin(Theta2)*cos(Theta3))*a3 + cos(Theta1)*sin(Theta2)*a2;
Y = (-sin(Theta1)*cos(Theta2)*sin(Theta3)+sin(Theta1)*sin(Theta2)*cos(Theta3))*a3 + sin(Theta1)*sin(Theta2)*a2;
//if ((Xc/Yc) < 0 ) Y = -Y;
//else Y = Y;
Z = (sin(Theta2)*sin(Theta3)+cos(Theta2)*cos(Theta3))*a3 + cos(Theta2)*a2+ d1;
Theta1 = Theta1 + pi/2; Theta3 = -1*Theta3;
// find positioning error
//error = abs((Xc-X)) + abs((Yc-Y)/Yc*100) + abs((Zc-Z)/Zc*100);
error = abs(Xc-X) + abs(Yc-Y) + abs(Zc-Z);
if (error>0.001) possible = 0;
else possible = 1;
if (possible ==1)
if (Zc<5) possible = 0;
LabVIEW Block Diagram:
Dim lights Embed Embed this video on your site
.
.
Add comment