Final Project
Successful trial of whole Robot Motion
Read moreThe final project consists of multiple tasks. The CRS robot arm goes through multiple points to complete the whole task. Task for final project can be break down to three big independent tasks. Peg in a hole, zig-zagging, and force on an egg.
Throughout the whole control process, Task space PD control and Feedforward force control was implemented. Then various points were added to the trajectory points, so that the CRS robot arm could follow. In total, there are 21 points from the beginning and the end for the whole task. At the beginning and the end, the robot arm is at the zero-position. The (x, y, z) coordinate of the zero-position is (0.25, 0, 0.5).
Task 1: Peg in a hole
From the zero position, the robot arm moves towards the hole in a wood. The end-effector stops right above the hole, and moves down vertically. Peg on the end-effector goes into the hole and rests there for two seconds. The end-effector then moves upwards vertically to take the peg out of the hole. Then the robot arm goes back to the zero position.
Task 2: Zigzag
After completing Task 1, starting from the zero position, the end-effector shifts to the entrance of the 3D-printed zigzag route shown below. To make the end-effector smoothly go through the curved route, there are total 8 points added to the trajectory of the robot arm to follow. There are more points assigned at the curve portion of the pathway, and only two start and end point for the linear portion of the pathway.
Task 3: Force control on egg
After the robot completes Task 2, starting again from the zero-position, the peg on the end-effector moves right above the egg. Then, for the force control, the gains Kpz and Kdz are set to 0. Then, the z-direction force FzCmd is modified to not crack the egg, but to apply the force onto egg, so that the scale below the egg could show the force between 500 ~ 1000 grams. The sweet spot of the FzCmd to meet the conditions was -12.65.
Challenging point
One of the most challenging point was implementing the force control on Task 3, and finding the appropriate FzCmd value. Once implementing the Feedforward force control from Lab 4, trial and error was the best option to fine-tune the FzCmd value. Finally, it was able to get the optimal value for the z-direction force control, and the window of 500~ 1000 grams of force was achieved.
The whole task of robot motion
Contributors of the project
Ethan Boris - B.S. Mechanical Engineering UIUC
Ethan, graduating Spring 2023
Jaewan Park - PhD student, Mechanical Engineering UIUC
Jaewan, Enjoying Fancy Italian dish
#include "math.h"
#include "F28335Serial.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
#define GRAV 9.81
#define NUMPOINTS 21
// These two offsets are only used in the main file user_CRSRobot.c You just need to create them here and find the correct offset and then these offset will adjust the encoder readings
float offset_Enc2_rad = -0.435; //0; //-0.42; //-0.37
float offset_Enc3_rad = 0.222; //0; //0.23; //0.27
// Your global varialbes.
long mycount = 0;
#pragma DATA_SECTION(whattoprint, ".my_vars")
float whattoprint = 0.0;
#pragma DATA_SECTION(printtowhat, ".my_vars")
float printtowhat = 3.0;
#pragma DATA_SECTION(theta1array, ".my_arrs")
float theta1array[100];
#pragma DATA_SECTION(theta2array, ".my_arrs")
float theta2array[100];
long arrayindex = 0;
int UARTprint = 0;
float printtheta1motor = 0;
float printtheta2motor = 0;
float printtheta3motor = 0;
// Assign these float to the values you would like to plot in Simulink
float Simulink_PlotVar1 = 0;
float Simulink_PlotVar2 = 0;
float Simulink_PlotVar3 = 0;
float Simulink_PlotVar4 = 0;
float x = 0;
float y = 0;
float z = 0;
float theta1M = 0;
float theta2M = 0;
float theta3M = 0;
/*
float IVtheta1DH = 0;
float IVtheta2DH = 0;
float IVtheta3DH = 0;
float IVtheta1 = 0;
float IVtheta2 = 0;
float IVtheta3 = 0;
*/
float theta1dotraw = 0, theta2dotraw = 0, theta3dotraw = 0;
float theta1old = 0, theta2old = 0, theta3old = 0;
float theta1dotold = 0, theta2dotold = 0, theta3dotold = 0;
float theta1dotold2 = 0, theta2dotold2 = 0, theta3dotold2 = 0;
float theta1des = 0, etheta1 = 0, etheta1dot = 0;
float theta1dotfilt = 0, Ik1 = 0, Ik1old = 0;
float etheta1old = 0;
float theta1dotdes = 0, theta1ddotdes = 0;
float theta2des = 0, etheta2 = 0, etheta2dot = 0;
float theta2dotfilt = 0, Ik2 = 0, Ik2old = 0;
float etheta2old = 0;
float theta2dotdes = 0, theta2ddotdes = 0;
float theta3des = 0, etheta3 = 0, etheta3dot = 0;
float theta3dotfilt = 0, Ik3 = 0, Ik3old = 0;
float etheta3old = 0;
float theta3dotdes = 0, theta3ddotdes = 0;
/*
float Kp1 = 50.0;
float Kd1 = 1.8;
float Ki1 = 0.0;
float Kp2 = 5000;
float Kd2 = 180;
float Ki2 = 0;
float Kp3 = 5000;
float Kd3 = 180;
float Ki3 = 0; // or 7.0
float Kp2PD = 250;
float Kd2PD = 10;
float Kp3PD = 250;
float Kd3PD = 8;
*/
float thres1 = 0.01;
//float thres2 = 0.01;
//float thres3 = 0.02;
//float J1 = 0.0167;
//float J2 = 0.03;
//float J3 = 0.0128;
typedef struct point_tag {
float xb;
float yb;
float zb;
float thz;
int mode;
}Point;
Point myarraypoints[NUMPOINTS] =
{{0.25, 0, 0.5, 0, 0},
{0.04, 0.35, 0.25, 0, 0},
{0.04, 0.35, 0.125, 0, 1},
{0.04, 0.35, 0.125, 0, 1},
{0.04, 0.35, 0.25, 0, 0},
{0.25, 0, 0.5, 0, 0},
// Zig zagging start
/*
{0.36, 0.12, 0.22, 0, 0},
{0.38, 0.07, 0.22, 0, 3},
{0.38, 0.05, 0.22, 0, 3},
{0.37, 0.04, 0.22, 0, 3},
{0.31, 0.06, 0.22, 0, 3},
{0.30, 0.05, 0.22, 0, 3},
{0.29, 0.04, 0.22, 0, 3},
{0.29, 0.03, 0.22, 0, 3},
{0.36, -0.05, 0.22, 0, 3},
*/
{0.37, 0.11, 0.21, 0, 0},
{0.40, 0.05, 0.21, 0, 3},
{0.40, 0.03, 0.21, 0, 3},
{0.38, 0.03, 0.21, 0, 3},
{0.33, 0.04, 0.21, 0, 3},
{0.31, 0.03, 0.21, 0, 3},
{0.31, 0.02, 0.21, 0, 3},
{0.38, -0.05, 0.20, 0, 3},
// Egg motion
/*
{0.25, 0, 0.5, 0, 0},
{0.17, 0.14, 0.31, 0, 0},
{0.17, 0.14, 0.30, 0, 4},
{0.17, 0.14, 0.30, 0, 4},
//{0.17, 0.14, 0.29, 0, 1},
//{0.17, 0.14, 0.29, 0, 1},
{0.17, 0.14, 0.31, 0, 0},
*/
{0.25, 0, 0.5, 0, 0},
{0.20, 0.18, 0.31, 0, 0},
{0.20, 0.18, 0.30, 0, 4},
{0.20, 0.18, 0.30, 0, 4},
{0.20, 0.18, 0.31, 0, 0},
// Zero position and stay forever
{0.25, 0, 0.5, 0, 0},
{0.25, 0, 0.5, 0, 2}
};
float xdes = 0;
float ydes = 0;
float zdes = 0;
//xdes = myarraypoints[0].xb;
//ydes = myarraypoints[0].yb;
//zdes = myarraypoints[0].zb;
float xdotdes = 0, ydotdes = 0, zdotdes = 0;
float Kpx = 1000.0, Kdx = 8.0;
float Kpy = 1000.0, Kdy = 8.0;
float Kpz = 1000.0, Kdz = 8.0;
float ff = 0;
float Vp1 = 0.2513;
float Vn1 = 0.2477;
float Cp1 = 0.3637;
float Cn1 = -0.2948;
float Vp2 = 0.2500;
float Vn2 = 0.287;
float Cp2 = 0.4759;
float Cn2 = -0.5031;
float Vp3 = 0.1922;
float Vn3 = 0.2132;
float Cp3 = 0.5339;
float Cn3 = -0.519;
float u_fric1 = 0;
float u_fric2 = 0;
float u_fric3 = 0;
/*
float p1 = 0.0300;
float p2 = 0.0128;
float p3 = 0.0076;
float p4 = 0.0753;
float p5 = 0.0298;
float a_theta2 = 0.0;
float a_theta3 = 0.0;
float a_vec[2];
float D[2][2];
float C[2][2];
float G[2];
float mystep = 0.0;
float mystep2 = 0.0;
int controller = 0;
*/
float Fx = 0, Fy = 0, Fz = 0;
float FzCmd = 0; // 14 for gravity compensation
float Kt = 6.0;
float KPxn = 700, KPyn = 700, KPzn = 700;
float KDxn = 8, KDyn = 8, KDzn = 8;
float Fxn = 0, Fyn = 0, Fzn = 0;
float xdotraw = 0, xold = 0;
float xdotfilt = 0, xdotold = 0, xdotold2 = 0;
float errorx = 0, errorxdot = 0;
float ydotraw = 0, yold = 0;
float ydotfilt = 0, ydotold = 0, ydotold2 = 0;
float errory = 0, errorydot = 0;
float zdotraw = 0, zold = 0;
float zdotfilt = 0, zdotold = 0, zdotold2 = 0;
float errorz = 0, errorzdot = 0;
float cosq1 = 0;
float sinq1 = 0;
float cosq2 = 0;
float sinq2 = 0;
float cosq3 = 0;
float sinq3 = 0;
float JT_11 = 0;
float JT_12 = 0;
float JT_13 = 0;
float JT_21 = 0;
float JT_22 = 0;
float JT_23 = 0;
float JT_31 = 0;
float JT_32 = 0;
float JT_33 = 0;
float cosz = 0;
float sinz = 0;
float cosx = 0;
float sinx = 0;
float cosy = 0;
float siny = 0;
float thetaz = 0;
float thetax = 0;
float thetay = 0;
float R11 = 0;
float R12 = 0;
float R13 = 0;
float R21 = 0;
float R22 = 0;
float R23 = 0;
float R31 = 0;
float R32 = 0;
float R33 = 0;
float RT11 = 0;
float RT12 = 0;
float RT13 = 0;
float RT21 = 0;
float RT22 = 0;
float RT23 = 0;
float RT31 = 0;
float RT32 = 0;
float RT33 = 0;
int state = 1;
int currentpoint = 0;
//float xp1 = 0.254, yp1 = 0.254, zp1 = 0.254;
//float xp2 = 0.4, yp2 = 0.0, zp2 = 0.254;
float xa , ya, za;
//xa = 1.9;
//xa = myarraypoints[0].xb;
//ya = myarraypoints[0].yb;
//za = myarraypoints[0].zb;
float xb , yb , zb;
//xb = myarraypoints[1].xb;
//yb = myarraypoints[1].yb;
//zb = myarraypoints[1].zb;
float deltax = 0, deltay = 0, deltaz = 0;
float t_total = 0, t = 0, t_start = 0;
float velocity = 0.15;
// this function must be called every 1ms.
// This function is called every 1 ms
void lab(float theta1motor,float theta2motor,float theta3motor,float *tau1,float *tau2,float *tau3, int error) {
//Motor torque limitation(Max: 5 Min: -5)
// save past states
if ((mycount%50)==0) {
theta1array[arrayindex] = theta1motor;
theta2array[arrayindex] = theta2motor;
if (arrayindex >= 100) {
arrayindex = 0;
} else {
arrayindex++;
}
}
if ((mycount%500)==0) {
UARTprint = 1;
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1; // Blink LED on Control Card
GpioDataRegs.GPBTOGGLE.bit.GPIO60 = 1; // Blink LED on Emergency Stop Box
}
printtheta1motor = theta1motor;
printtheta2motor = theta2motor;
printtheta3motor = theta3motor;
theta1M = theta1motor;
theta2M = theta2motor;
theta3M = theta3motor;
x = (127.0*cos(theta1M)*(cos(theta3M) + sin(theta2M)))/500.0;
y = (127.0*sin(theta1M)*(cos(theta3M) + sin(theta2M)))/500.0;
z = (127.0*cos(theta2M))/500.0 - (127.0*sin(theta3M))/500.0 + 127.0/500.0;
t = mycount * 0.001;
if(t == 0)
{
xa = x;
ya = y;
za = z;
xb = myarraypoints[0].xb;
yb = myarraypoints[0].yb;
zb = myarraypoints[0].zb;
}
if(t - t_start > t_total)
{
if(myarraypoints[currentpoint + 1].mode == 0)
{
state = 1;
//xa = myarraypoints[0].xb;
//ya = myarraypoints[0].yb;
//za = myarraypoints[0].zb;
xa = x;
ya = y;
za = z;
xb = myarraypoints[currentpoint + 1].xb;
yb = myarraypoints[currentpoint + 1].yb;
zb = myarraypoints[currentpoint + 1].zb;
Kpx = 1000;
Kpy = 1000;
Kpz = 1000;
Kdx = 50;
Kdy = 50;
Kdz = 50;
FzCmd = 0;
currentpoint++;
}else if(myarraypoints[currentpoint + 1].mode == 1)
{
state = 0;
xa = x;
ya = y;
za = z;
xb = myarraypoints[currentpoint + 1].xb;
yb = myarraypoints[currentpoint + 1].yb;
zb = myarraypoints[currentpoint + 1].zb;
Kpx = 500;
Kpy = 500;
Kdx = 2;
Kdy = 2;
FzCmd = 0;
currentpoint++;
}
else if(myarraypoints[currentpoint + 1].mode == 2)
{
//xa = x;
//ya = y;
//za = z;
xa = 0.25;
ya = 0;
za = 0.5;
xb = 0.25;
yb = 0;
zb = 0.5;
FzCmd = 0;
}
else if(myarraypoints[currentpoint + 1].mode == 3)
{
state = 1;
//xa = myarraypoints[0].xb;
//ya = myarraypoints[0].yb;
//za = myarraypoints[0].zb;
xa = x;
ya = y;
za = z;
xb = myarraypoints[currentpoint + 1].xb;
yb = myarraypoints[currentpoint + 1].yb;
zb = myarraypoints[currentpoint + 1].zb;
Kpx = 1000;
Kpy = 1000;
Kpz = 1000;
Kdx = 2;
Kdy = 2;
Kdz = 50;
FzCmd = 0;
currentpoint++;
}
else if(myarraypoints[currentpoint + 1].mode == 4)
{
state = 0;
xa = myarraypoints[currentpoint].xb;
ya = myarraypoints[currentpoint].yb;
za = myarraypoints[currentpoint].zb;
xb = myarraypoints[currentpoint + 1].xb;
yb = myarraypoints[currentpoint + 1].yb;
zb = myarraypoints[currentpoint + 1].zb;
Kpx = 1000;
Kpy = 1000;
Kpz = 0;
Kdx = 8;
Kdy = 8;
Kdz = 0;
FzCmd = -12.65;
currentpoint++;
}
t_start = t;
}
deltax = xb - xa;
deltay = yb - ya;
deltaz = zb - za;
if(myarraypoints[currentpoint].mode == 0 || myarraypoints[currentpoint].mode == 3)
{
t_total = sqrt(deltax * deltax + deltay * deltay + deltaz * deltaz) / velocity;
}
else if(myarraypoints[currentpoint].mode == 1 || myarraypoints[currentpoint].mode == 4){
t_total = 1.5;
}
else if(myarraypoints[currentpoint].mode == 2){
t_total=1000;
}
xdes = deltax * ((t-t_start) / t_total) + xa;
ydes = deltay * ((t-t_start) / t_total) + ya;
zdes = deltaz * ((t-t_start) / t_total) + za;
xdotdes = deltax / t_total;
ydotdes = deltay / t_total;
zdotdes = deltaz / t_total;
theta1dotraw = (theta1motor - theta1old)/0.001;
theta1dotfilt = (theta1dotraw + theta1dotold + theta1dotold2)/3.0;
theta1dotold2 = theta1dotold;
theta1dotold = theta1dotfilt;
etheta1 = theta1des - theta1motor;
etheta1dot = theta1dotdes-theta1dotfilt;
theta2dotraw = (theta2motor - theta2old)/0.001;
theta2dotfilt = (theta2dotraw + theta2dotold + theta2dotold2)/3.0;
theta2dotold2 = theta2dotold;
theta2dotold = theta2dotfilt;
etheta2 = theta2des - theta2motor;
etheta2dot = theta2dotdes-theta2dotfilt;
theta3dotraw = (theta3motor - theta3old)/0.001;
theta3dotfilt = (theta3dotraw + theta3dotold + theta3dotold2)/3.0;
theta3dotold2 = theta3dotold;
theta3dotold = theta3dotfilt;
etheta3 = theta3des - theta3motor;
etheta3dot = theta3dotdes-theta3dotfilt;
//This part for Part 1: Implement Friction Compensation
if(theta1dotfilt > 0.1)
{
u_fric1 = Vp1*theta1dotfilt + Cp1;
} else if (theta1dotfilt < -0.1) {
u_fric1 = Vn1*theta1dotfilt + Cn1;
} else {
u_fric1 = 3.6 * theta1dotfilt;
}
if(theta2dotfilt > 0.05)
{
u_fric2 = Vp2*theta2dotfilt + Cp2;
} else if (theta2dotfilt < -0.05) {
u_fric2 = Vn2*theta2dotfilt + Cn2;
} else {
u_fric2 = 3.6 * theta2dotfilt;
}
if(theta3dotfilt > 0.05)
{
u_fric3 = Vp3*theta3dotfilt + Cp3;
} else if (theta3dotfilt < -0.05) {
u_fric3 = Vn3*theta3dotfilt + Cn3;
} else {
u_fric3 = 3.6 * theta3dotfilt;
}
// Rotation zxy and its Transpose
cosz = cos(thetaz);
sinz = sin(thetaz);
cosx = cos(thetax);
sinx = sin(thetax);
cosy = cos(thetay);
siny = sin(thetay);
RT11 = R11 = cosz*cosy-sinz*sinx*siny;
RT21 = R12 = -sinz*cosx;
RT31 = R13 = cosz*siny+sinz*sinx*cosy;
RT12 = R21 = sinz*cosy+cosz*sinx*siny;
RT22 = R22 = cosz*cosx;
RT32 = R23 = sinz*siny-cosz*sinx*cosy;
RT13 = R31 = -cosx*siny;
RT23 = R32 = sinx;
RT33 = R33 = cosx*cosy;
// Jacobian Transpose
cosq1 = cos(theta1motor);
sinq1 = sin(theta1motor);
cosq2 = cos(theta2motor);
sinq2 = sin(theta2motor);
cosq3 = cos(theta3motor);
sinq3 = sin(theta3motor);
JT_11 = -0.254*sinq1*(cosq3 + sinq2);
JT_12 = 0.254*cosq1*(cosq3 + sinq2);
JT_13 = 0;
JT_21 = 0.254*cosq1*(cosq2 - sinq3);
JT_22 = 0.254*sinq1*(cosq2 - sinq3);
JT_23 = -0.254*(cosq3 + sinq2);
JT_31 = -0.254*cosq1*sinq3;
JT_32 = -0.254*sinq1*sinq3;
JT_33 = -0.254*cosq3;
xdotraw = (x - xold)/0.001;
xdotfilt = (xdotraw + xdotold + xdotold2)/3.0;
xdotold2 = xdotold;
xdotold = xdotfilt;
errorx = xdes - x;
errorxdot = xdotdes-xdotfilt;
ydotraw = (y - yold)/0.001;
ydotfilt = (ydotraw + ydotold + ydotold2)/3.0;
ydotold2 = ydotold;
ydotold = ydotfilt;
errory = ydes - y;
errorydot = ydotdes-ydotfilt;
zdotraw = (z - zold)/0.001;
zdotfilt = (zdotraw + zdotold + zdotold2)/3.0;
zdotold2 = zdotold;
zdotold = zdotfilt;
errorz = zdes - z;
errorzdot = zdotdes-zdotfilt;
Fx = Kpx * (xdes - x) + Kdx * (xdotdes - xdotfilt);
Fy = Kpy * (ydes - y) + Kdy * (ydotdes - ydotfilt);
Fz = Kpz * (zdes - z) + Kdz * (zdotdes - zdotfilt);
// Part 2 of Lab 4, adding 14 should be the gravity compensation
*tau1 = JT_11 * Fx + JT_12 * Fy + JT_13 * Fz + JT_13 * (14 + FzCmd) / Kt;
*tau2 = JT_21 * Fx + JT_22 * Fy + JT_23 * Fz + JT_23 * (14 + FzCmd) / Kt;
*tau3 = JT_31 * Fx + JT_32 * Fy + JT_33 * Fz + JT_33 * (14 + FzCmd) / Kt;
*tau1 = *tau1 + ff*u_fric1;
*tau2 = *tau2 + ff*u_fric2;
*tau3 = *tau3 + ff*u_fric3;
// Saturate torques
if(*tau1 > 5.0)
{
*tau1 = 5.0;
} else if(*tau1 < -5.0)
{
*tau1 = -5.0;
}
if(*tau2 > 5.0)
{
*tau2 = 5.0;
}else if(*tau2 < -5.0)
{
*tau2 = -5.0;
}
if(*tau3 > 5.0)
{
*tau3 = 5.0;
}else if(*tau3 < -5.0)
{
*tau3 = -5.0;
}
//Save States
theta1old = theta1motor;
theta2old = theta2motor;
theta3old = theta3motor;
etheta1old = etheta1;
etheta2old = etheta2;
etheta3old = etheta3;
Ik1old = Ik1;
Ik2old = Ik2;
Ik3old = Ik3;
xold = x;
yold = y;
zold = z;
Simulink_PlotVar1 = theta2motor;
Simulink_PlotVar2 = theta3des - theta3motor;
Simulink_PlotVar3 = theta2des;
Simulink_PlotVar4 = theta2des - theta2motor;
mycount++;
}
void printing(void){
serial_printf(&SerialA, "%.2f, %.2f, %.2f, %.2f, %.2f, %.2f \n\r", printtheta1motor*180/PI, printtheta2motor*180/PI, printtheta3motor*180/PI, x, y, z);
}
Comments
Please log in or sign up to comment.