Being tasked with the obstacle course shown below, we took a strategic but slow approach to getting the robot to traverse it. We set up a structure to move the robot from point to point, adjusting different characteristics at each point.
To begin, we used multiple points to get a clear path to the hole where the peg needed to be inserted. Once we were directly above the hole, we used simple impedance control and loosened the x and y directions so the peg could be inserted smoothly. We then moved the z level until the peg was inserted to the correct depth. We repeated this step multiple times in order to get the 0.5 second required wait. This was the most difficult challenge for our team, as our wait variable was not working as we had anticipated. Adding the multiple steps alleviated this issue.
We then moved the robot over a series of points, avoiding the box, to get to the beginning of the track. The coordinate axes were rotated about the z to line up the x axis with the track. The y axis was loosened to allow the robot to glide against the walls. At each turn, a similar strategy was used to move in the new direction.
The next step was moving directly above the egg. Once there, we moved down to the correct z coordinate and applied a force using feedforward control. The force was the biggest part of our tuning in order to keep the weight in between 500 and 1000 grams. After holding there for 2 seconds, we returned to the start.
A picture of our group (Kamden, Madison, and Alex) with our robot on demo day!
#include "math.h"
#include "F28335Serial.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
#define GRAV 9.81
// to call this function create a variable that steps to the new positions you want to go to, pass this var to step
// pass a reference to your qd variable your qd_dot variable and your qd_double_dot variable
// for example
// implement_discrete_tf(&trajectory, mystep, &qd, &dot, &ddot);
float offset_Enc2_rad = -0.42; //-0.37 original value changed to measured
float offset_Enc3_rad = 0.24; // 0.27 original value changed to measured
// Your global varialbes.
long mycount = 1;
#pragma DATA_SECTION(whattoprint, ".my_vars")
float whattoprint = 0.0;
#pragma DATA_SECTION(theta1array, ".my_arrs")
float theta1array[100];
#pragma DATA_SECTION(noway, ".my_vars") //new float
float noway = 6.0;
//#pragma DATA_SECTION(x, ".my_vars") //x position
float x = 0.0;
//#pragma DATA_SECTION(y, ".my_vars") //y position
float y = 0.0;
//#pragma DATA_SECTION(z, ".my_vars") //z position
float z = 0.0;
#pragma DATA_SECTION(printtheta1motor, ".my_vars") //set up to send motor values
#pragma DATA_SECTION(printtheta2motor, ".my_vars")
#pragma DATA_SECTION(printtheta3motor, ".my_vars")
#pragma DATA_SECTION(theta2array, ".my_arrs") //new array
float theta2array[100];
#pragma DATA_SECTION(theta3array, ".my_arrs") //new array
float theta3array[100];
long arrayindex = 0;
int UARTprint = 0;
// We created a structure to define each point in the system which includes the cartesian coordinate in space in meters, the rotation angle about the z-axis so that we can loosen
// the controls in varying directions, a mode indicator, and a wait time.
#define NUM_POINT 22
typedef struct point_tag{
float x;
float y;
float z;
float thetaZ;
int mode;
float wait_time;
} point;
point point_array[NUM_POINT] = {{.254, 0, .508, 0, 1, 0}, //This is the starting position
{.2, .2, .4, 0, 1, 0}, //This point makes a smooth transition from the start to the hole
{ 0.03, 0.35, 0.21, 0, 1, 0}, //This point lines the peg up above the hole
{ 0.03, 0.35, 0.11, 0, 2, 0}, //The following points hold the peg into the hole
{ 0.03, 0.35, 0.11, 0, 2, 0},
{ 0.03, 0.35, 0.11, 0, 2, 0},
{ 0.03, 0.35, 0.11, 0, 2, 0},
{ 0.03, 0.35, 0.11, 0, 2, 0},
{ 0.03, 0.35, 0.11, 0, 2, 0},
{ 0.03, 0.35, 0.21, 0, 2, 0}, //This point brings the peg out of the hole
{ 0.21, 0.09, 0.21, 0, 1, 0}, //This point helps to traverse from the hole to the track
{ 0.39, 0.09, 0.21, 0, 1, 0}, //This point lines up at the entrance of the track
{ 0.42, 0.03, 0.21, -0.9273, 3, 0}, //This point is at the first curve of the track
{ 0.31, 0.02, 0.21, -0.2618, 3, 0}, //This point is at the second curve in the track
{ 0.38, -0.06, 0.21, -0.9273, 3, 0},//This point is the last point inside of the track
{ 0.39, -0.10, 0.21, 0, 1, 0}, //This point is clear of the track
{ 0.25, -0.05, 0.45, 0, 1, 0}, //This point is used to smoothly move closer to the egg away from the track
{ 0.24, 0.19, 0.45, 0, 1, 0}, //This point lines us up exactly above the egg
{ 0.24, 0.19, 0.30, 0, 1, 0}, //This gets us to the correct z-value to compress the egg
{ 0.24, 0.19, 0.45, 0, 4, 2.0}}; //This point was the bottom point of the egg pressure with mode 4 (the force mode)
int point_index = 0;
float printtheta1motor = 0;
float printtheta2motor = 0;
float printtheta3motor = 0;
float inverse_theta1 = 0;
float inverse_theta2 = 0;
float inverse_theta3 = 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 d = 0;
float h = 0;
float alpha = 0;
float beta = 0;
float gamma = 0;
float inverse_theta1_DH = 0;
float inverse_theta2_DH = 0;
float inverse_theta3_DH = 0;
//theta1 initialize variables
float theta1_des = 0;
float theta1_dot = 0;
float theta1_dot_filt = 0;
float theta1_dot_k1 = 0;
float theta1_dot_k2 = 0;
float theta1_prev = 0;
float err1 = 0;
float err1_prev = 0;
float I1 = 0;
float I1_prev = 0;
//theta2 initialize variables
float theta2_des = 0;
float theta2_dot = 0;
float theta2_dot_filt = 0;
float theta2_dot_k1 = 0;
float theta2_dot_k2 = 0;
float theta2_prev = 0;
float err2 = 0;
float err2_prev = 0;
float I2 = 0;
float I2_prev = 0;
//theta3 initialize variables
float theta3_des = 0;
float theta3_dot = 0;
float theta3_dot_filt = 0;
float theta3_dot_k1 = 0;
float theta3_dot_k2 = 0;
float theta3_prev = 0;
float err3 = 0;
float err3_prev = 0;
float I3 = 0;
float I3_prev = 0;
//control law parameters
float J1 = 0.0167;
float J2 = 0.03;
float J3 = 0.0128;
float theta1_des_dot = 0;
float theta2_des_dot = 0;
float theta3_des_dot = 0;
float theta1_des_dot_dot = 0;
float theta2_des_dot_dot = 0;
float theta3_des_dot_dot = 0;
float t = 0;
//friction variables!!!!
float min_velo1 = 0.1;
float u_fric1 = 0;
float V_pos1 = 0.135;
float V_neg1 = 0.15;
float C_pos1 = 0.3637;
float C_neg1 = -0.2948;
float slope1 = 4.99;
float min_velo2 = 0.05;
float u_fric2 = 0;
float V_pos2 = 0.18;
float V_neg2 = 0.287;
float C_pos2 = 0.4759;
float C_neg2 = -0.5031;
float slope2 = 3.6;
float min_velo3 = 0.05;
float u_fric3 = 0;
float V_pos3 = 0.1;
float V_neg3 = 0.1;
float C_pos3 = 0.5339;
float C_neg3 = -0.5190;
float slope3 = 3.6;
float p1 = .0300;
float p2 = .0128;
float p3 = .0076;
float p4 = .0753;
float p5 = .0298;
float a_theta2 = 0;
float a_theta3 = 0;
float D[2][2];
float C[2][2];
float G[2][1];
//float mystep = 0;
float mystep = .25;
float mystep3 = .3;
//lab 4 intialization
//variables to make trig calculations not as taxing
float cosq1 = 0;
float sinq1 = 0;
float cosq2 = 0;
float sinq2 = 0;
float cosq3 = 0;
float sinq3 = 0;
//Jacobian place holders
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;
//cartesian variable holders
float x_dot = 0;
float y_dot = 0;
float z_dot = 0;
float x_dot_prev = 0;
float y_dot_prev = 0;
float z_dot_prev = 0;
float x_dot_prev_prev = 0;
float y_dot_prev_prev = 0;
float z_dot_prev_prev = 0;
float x_dot_filt = 0;
float y_dot_filt = 0;
float z_dot_filt = 0;
float x_prev = 0;
float y_prev = 0;
float z_prev = 0;
//cartesian forces
float Fx = 0;
float Fy = 0;
float Fz = 0;
//cartesian desired positions and velocities
float xd = .25;
float yd = .25;
float zd = .25;
float xd_dot = 0;
float yd_dot = 0;
float zd_dot = 0;
//cartesian rotation gain values mode 1
float KPx1 = 2000;
float KDx1 = 35;
float KPy1 = 2000;
float KDy1 = 35;
float KPz1 = 2000;
float KDz1 = 35;
//cartesian rotation gain values mode 2
float KPx2 = 500;
float KDx2 = 15;
float KPy2 = 500;
float KDy2 = 15;
float KPz2 = 2000;
float KDz2 = 35;
//cartesian rotation gain values mode 1
float KPx3 = 2000;
float KDx3 = 35;
float KPy3 = 500;
float KDy3 = 15;
float KPz3 = 2000;
float KDz3 = 35;
//friction factor, torque constant and gravity compensation
float ff = .6;
float Kt = 6.0;
float F_Zcmd = -2.75;
//lab 4 part 3
//trig substitutions
float cosz = 0;
float sinz = 0;
float cosx = 0;
float sinx = 0;
float cosy = 0;
float siny = 0;
//rotation angles
float thetaz = 0;
float thetax = 0;
float thetay = 0;
//rotation matrix placeholders
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;
//transpose rotation matrix placeholders
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;
//desired start and end position
float x1 = 0;
float x2 = 0;
float y1 = 0;
float y2 = 0;
float z1 = 0;
float z2 = 0;
float deltax = 0;
float deltay = 0;
float deltaz = 0;
int mode = 0;
int wait_time = 0;
//time and speed variables for desired trajectory
float t_start = 0;
float t_tot = 0;
float speed = .125;
int wait = 0;
// This function is called every 1 ms
void lab(float theta1motor,float theta2motor,float theta3motor,float *tau1,float *tau2,float *tau3, int error) {
// create temporary float to switch position 1 and 2
// save past states
if ((mycount%50)==0) {
theta1array[arrayindex] = theta1motor;
if (arrayindex >= 100) {
arrayindex = 0;
} else {
arrayindex++;
}
}
if ((mycount%50)==0) { //for theta2
theta2array[arrayindex] = theta2motor;
if (arrayindex >= 100) {
arrayindex = 0;
} else {
arrayindex++;
}
}
if ((mycount%50)==0) { //for theta3
theta3array[arrayindex] = theta3motor;
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;
Simulink_PlotVar1 = theta2motor;
Simulink_PlotVar2 = theta3motor;
Simulink_PlotVar3 = theta2_des;
Simulink_PlotVar4 = theta3_des;
mycount++;
/* Set desired trajectory for lab 4 part 4
* We first determine the total distance between the first and second points, and using the speed which we set, calculate the total amount of time it takes to travel from point 1 to point 2.
* When the time exceeds this total time for the path, we switch position one and two so that the desired path is the other way.
* Based on our chosen speed and start/end positions, we determine the desired position of the robot at any given time using the following equation:
* x = deltaX / speed * time + xinitial (and same for y and z)
* To determine the desired velocity in each direction, we normalize the position vector in each direction and multiply by speed.
*/
t = mycount / 1000.0; //calculate time passed
x = .254*cos(theta1motor) * (cos(theta3motor)+sin(theta2motor));
y = .254*sin(theta1motor) * (cos(theta3motor)+sin(theta2motor));
z = .254 *(1 + cos(theta2motor) - sin(theta3motor));
//These following statements enact the wait time if the wait has not yet been completed
if (wait == 0) {
if ((t-t_start) >= wait_time){
t_start = t;
wait = 1;
}
else {
wait = 0;
}
}
//When the wait is 1, the wait has been completed and it runs the normal controller, moving on to the next point in the struc
if (wait == 1) {
if ((t-t_start) >= t_tot) { //when the end of the path is reached, switch position 1 and 2
t_start = t;
x1 = x;
x2 = point_array[point_index].x;
y1 = y;
y2 = point_array[point_index].y;
z1 = z;
z2 = point_array[point_index].z;
thetaz = point_array[point_index].thetaZ;
mode = point_array[point_index].mode;
wait_time = point_array[point_index].wait_time;
wait = 0;
t_tot = sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1)) / speed; //total distance between the points over the speed
if (t_tot < 0.005) {
t_tot = 4;
}
deltax = x2 - x1;
deltay = y2 - y1;
deltaz = z2 - z1;
point_index += 1;
if (point_index > NUM_POINT -1) {
point_index = 0;
}
}
//calculate the desired position values
xd = (deltax)*(t-t_start)/t_tot + x1;
yd = (deltay)*(t-t_start)/t_tot + y1;
zd = (deltaz)*(t-t_start)/t_tot + z1;
//calculate the desired velocity values (normalize position vector and multiply by speed)
xd_dot = deltax/t_tot;
yd_dot = deltay/t_tot;
zd_dot = deltaz/t_tot;
/* Simplify calculations to get Jacobian and Rotation Matrix
* In order to decrease computation time, we set variables equation to the trigonometric functions. We then calculate the entries of the Jacobian matrix.
* We do the same thing for the rotation matrix, but with our rotation values instead of motor angles.
* For the rotation transpose, we just switch the entry values.
*/
// Jacobian Transpose (code from lab manual)
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;
cosz = cos(thetaz);
sinz = sin(thetaz);
cosx = cos(thetax);
sinx = sin(thetax);
cosy = cos(thetay);
siny = sin(thetay);
R11 = cosz*cosy-sinz*sinx*siny;
R12 = -sinz*cosx;
R13 = cosz*siny+sinz*sinx*cosy;
R21 = sinz*cosy+cosz*sinx*siny;
R22 = cosz*cosx;
R23 = sinz*siny-cosz*sinx*cosy;
R31 = -cosx*siny;
R32 = sinx;
R33 = cosx*cosy;
RT11 = R11;
RT21 = R12;
RT31 = R13;
RT12 = R21;
RT22 = R22;
RT32 = R23;
RT13 = R31;
RT23 = R32;
RT33 = R33;
/* In this section we calculate the cartesian position from the motor angles, the motor velocities, and the cartesian velocities
* To calculate cartesian position from motor angles we use equations from previous labs (which used FK)
* To calculate the motor velocities we take the current position value - the previous over the time difference (1ms) and then filter this value with the previous three values in order to get more consistent values
* We do the same thing for the cartesian velocities, by taking current cartesian position - previous cartesian position and then filtering.
*/
//calculate x, y, z position from motor angles
//calculate motor velocities and filter values
theta1_dot = (theta1motor - theta1_prev)/.001;
theta1_dot_filt = (theta1_dot + theta1_dot_k1 + theta1_dot_k2)/3;
theta2_dot = (theta2motor - theta2_prev)/.001;
theta2_dot_filt = (theta2_dot + theta2_dot_k1 + theta2_dot_k2)/3;
theta3_dot = (theta3motor - theta3_prev)/.001;
theta3_dot_filt = (theta3_dot + theta3_dot_k1 + theta3_dot_k2)/3;
//calculate cartesian velocities and filter values
x_dot = (x - x_prev)/.001;
x_dot_filt = (x_dot + x_dot_prev + x_dot_prev_prev)/3;
y_dot = (y - y_prev)/.001;
y_dot_filt = (y_dot + y_dot_prev + y_dot_prev_prev)/3;
z_dot = (z - z_prev)/.001;
z_dot_filt = (z_dot + z_dot_prev + z_dot_prev_prev)/3;
/* Calculate Control Laws
* In part one of the lab, we used a Taks Space PD Control, where we first calculated forces in cartesian directions using PD control, then added friction compensation (from Lab 3), and finally,
* combined forces with Jacobian and added friction factor (reduces the affect of the friction compensation) to get the final control law.
* In part two, we added feed forward force, so that the robot would exert a force in one direction (we choose z). To compensate for gravity we hard coded a positive force as well.
* In part three (and also used in part four) we changed the control law to impedance control. This allows us to rotate the cartesian coordinates so we can "weaken" the controls in more directions
* than the world x,y,z.
*/
//calculate friction compensation
if (theta1_dot_filt > min_velo1) {
u_fric1 = V_pos1*theta1_dot_filt + C_pos1 ;
} else if (theta1_dot_filt < -min_velo1) {
u_fric1 = V_neg1*theta1_dot_filt + C_neg1;
} else {
u_fric1 = slope1*theta1_dot_filt;
}
if (theta2_dot_filt > min_velo2) {
u_fric2 = V_pos2*theta2_dot_filt + C_pos2 ;
} else if (theta2_dot_filt < -min_velo2) {
u_fric2 = V_neg2*theta2_dot_filt + C_neg2;
} else {
u_fric2 = slope2*theta2_dot_filt;
}
if (theta3_dot_filt > min_velo3) {
u_fric3 = V_pos3*theta3_dot_filt + C_pos3 ;
} else if (theta3_dot_filt < -min_velo3) {
u_fric3 = V_neg3*theta3_dot_filt + C_neg3;
} else {
u_fric3 = slope3*theta3_dot_filt;
}
//Modes 1-3 set their own coefficients to loosen things in certain directions depending on the movement
if (mode == 1) {
//The stiffest movement, when the robot is moving from point to point with no obstacles (smooth motion)
*tau1 = (JT_11*R11 + JT_12*R21 + JT_13*R31) * ((KPx1*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx1*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_11*R12 + JT_12*R22 + JT_13*R32) * ((KPy1*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy1*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_11*R13 + JT_12*R23 + JT_13*R33) * ((KPz1*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz1*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric1;
*tau2 = (JT_21*R11 + JT_22*R21 + JT_23*R31) * ((KPx1*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx1*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_21*R12 + JT_22*R22 + JT_23*R32) * ((KPy1*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy1*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_21*R13 + JT_22*R23 + JT_23*R33) * ((KPz1*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz1*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric2;
*tau3 = (JT_31*R11 + JT_32*R21 + JT_33*R31) * ((KPx1*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx1*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_31*R12 + JT_32*R22 + JT_33*R32) * ((KPy1*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy1*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_31*R13 + JT_32*R23 + JT_33*R33) * ((KPz1*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz1*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric3;
}
if (mode == 2) {
//Movement loosened in the x and y directions, most helful when inserting the peg into the hole
*tau1 = (JT_11*R11 + JT_12*R21 + JT_13*R31) * ((KPx2*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx2*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_11*R12 + JT_12*R22 + JT_13*R32) * ((KPy2*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy2*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_11*R13 + JT_12*R23 + JT_13*R33) * ((KPz2*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz2*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric1;
*tau2 = (JT_21*R11 + JT_22*R21 + JT_23*R31) * ((KPx2*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx2*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_21*R12 + JT_22*R22 + JT_23*R32) * ((KPy2*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy2*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_21*R13 + JT_22*R23 + JT_23*R33) * ((KPz2*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz2*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric2;
*tau3 = (JT_31*R11 + JT_32*R21 + JT_33*R31) * ((KPx2*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx2*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_31*R12 + JT_32*R22 + JT_33*R32) * ((KPy2*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy2*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_31*R13 + JT_32*R23 + JT_33*R33) * ((KPz2*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz2*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric3;
}
if (mode == 3) {
//Movement loosened in the y direction, helpful during the track when the movement is aligned in the x direction
*tau1 = (JT_11*R11 + JT_12*R21 + JT_13*R31) * ((KPx3*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx3*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_11*R12 + JT_12*R22 + JT_13*R32) * ((KPy3*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy3*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_11*R13 + JT_12*R23 + JT_13*R33) * ((KPz3*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz3*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric1;
*tau2 = (JT_21*R11 + JT_22*R21 + JT_23*R31) * ((KPx3*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx3*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_21*R12 + JT_22*R22 + JT_23*R32) * ((KPy3*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy3*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_21*R13 + JT_22*R23 + JT_23*R33) * ((KPz3*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz3*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric2;
*tau3 = (JT_31*R11 + JT_32*R21 + JT_33*R31) * ((KPx3*(RT11*(xd-x)+RT12*(yd-y)+RT13*(zd-z))+KDx3*(RT11*(xd_dot-x_dot_filt)+RT12*(yd_dot-y_dot_filt)+RT13*(zd_dot-z_dot_filt)))) + (JT_31*R12 + JT_32*R22 + JT_33*R32) * ((KPy3*(RT21*(xd-x)+RT22*(yd-y)+RT23*(zd-z))+KDy3*(RT21*(xd_dot-x_dot_filt)+RT22*(yd_dot-y_dot_filt)+RT23*(zd_dot-z_dot_filt)))) + (JT_31*R13 + JT_32*R23 + JT_33*R33) * ((KPz3*(RT31*(xd-x)+RT32*(yd-y)+RT33*(zd-z))+KDz3*(RT31*(xd_dot-x_dot_filt)+RT32*(yd_dot-y_dot_filt)+RT33*(zd_dot-z_dot_filt)))) + ff*u_fric3;
}
if (mode == 4) {
//This mode compensates gravity and adds the additional necessary force to get the right movement on the egg.
*tau1 = JT_11*Fx + JT_12*Fy + JT_13*Fz + ff*u_fric1 + JT_13*(12+F_Zcmd) / Kt; //12 is for gravity
*tau2 = JT_21*Fx + JT_22*Fy + JT_23*Fz + ff*u_fric2 + JT_23*(12+F_Zcmd) / Kt;
*tau3 = JT_31*Fx + JT_32*Fy + JT_33*Fz + ff*u_fric3 + JT_33*(12+F_Zcmd) / Kt;
}
}
//calculate cartesian forces based on desired and current positions and velocities using PD controller for part 1
//Fx = (KPx*(xd - x) + KDx*(xd_dot - x_dot_filt));
//Fy = (KPy*(yd - y) + KDy*(yd_dot - y_dot_filt));
//Fz = (KPz*(zd - z) + KDz*(zd_dot - z_dot_filt));
//part 2- task space PD control with feedforward control
//*tau1 = JT_11*Fx + JT_12*Fy + JT_13*Fz + ff*u_fric1 + JT_13*(12+F_Zcmd) / Kt; //12 is for gravity
//*tau2 = JT_21*Fx + JT_22*Fy + JT_23*Fz + ff*u_fric2 + JT_23*(12+F_Zcmd) / Kt;
//*tau3 = JT_31*Fx + JT_32*Fy + JT_33*Fz + ff*u_fric3 + JT_33*(12+F_Zcmd) / Kt;
/* Update previous values
* In order to calculate velocities as described above, we need to save previous values after reading the current ones. To do so, we save current values as previous values and allow the current values to be
* rewritten when the function is run again. We do this for cartesian positions, motor positions, cartesian velocities, and motor velocities.
*/
x_dot_prev_prev = x_dot_prev;
x_dot_prev = x_dot_filt;
x_prev = x;
y_dot_prev_prev = y_dot_prev;
y_dot_prev = y_dot_filt;
y_prev = y;
z_dot_prev_prev = z_dot_prev;
z_dot_prev = z_dot_filt;
z_prev = z;
theta1_dot_k2 = theta1_dot_k1;
theta1_dot_k1 = theta1_dot_filt;
theta1_prev = theta1motor;
theta2_dot_k2 = theta2_dot_k1;
theta2_dot_k1 = theta2_dot_filt;
theta2_prev = theta2motor;
theta3_dot_k2 = theta3_dot_k1;
theta3_dot_k1 = theta3_dot_filt;
theta3_prev = theta3motor;
}
void printing(void){
/*
* This function prints to TeraTerm.
* */
//serial_printf(&SerialA, "%.2f %.2f,%.2f \r",printtheta1motor*180/PI,printtheta2motor*180/PI,printtheta3motor*180/PI);
serial_printf(&SerialA, "motor values: %.2f %.2f,%.2f ",printtheta1motor*180/PI, printtheta2motor*180/PI, printtheta3motor*180/PI); // print in degrees instead of radians
serial_printf(&SerialA, "position: %.2f %.2f,%.2f ", x, y, z); // print position from FK
serial_printf(&SerialA, "inverse theta: %.2f %.2f,%.2f \n\r", inverse_theta1*180/PI, inverse_theta2*180/PI, inverse_theta3*180/PI); // print thetas from IK
//serial_printf(&SerialA, "%.2f\n\r",noway); // print new float value
}
Comments