The objective of this project is to program a robot manipulator to carry out various tasks.
The first task involves starting from a resting position and moving the end effector above the hole. The next step is to position the peg just above the hole and insert it to a depth of 2 inches. After a half-second pause, the robot should move out of the hole and make its way to the entrance of the zig-zag while avoiding obstacles.
For the second task, the robot must navigate through a zig-zag maze.
The final task involves moving the end effector of the robot to the top of the egg and gently pushing it until the scale displays a weight between 500-1000g. The robot manipulator then returns to the start position and rest.
In order for our robot to complete certain tasks, such as positioning itself at a specific location, it must have a planned path. We used straight line trajectory for this project. The start and end points for each line trajectory was determined by moving the end effector by hand and recording the end effector position. A C struct was created to store key information about the trajectory.
Next, a state machine was used to move through each point. A state variable is used to identify which line segment we are currently tracking. As the state variable increases, we move between the points described in the trajectory structure.
The next step is to take in all the values we've calculated, along with the program's current time, to compute the desired position and velocity. The desired position is determined through simple linear interpolation. The desired velocity is also calculated simply by subtracting the current position with previous position.
Team members
In this project, we implemented two different controllers.
A task space PD controller and a feed forward impedance controller were implemented in this project. The task space PD controller was used to drive the robot manipulator to the desired position. And the feed forward impedance controller was used to control the force in certain directions. The impedance controller makes the end effector behave like it's in a mass spring damper system, where Kp controls the spring and Kd controls the damper. By reducing the gains on some axes experimentally, we were able to make the end effector easy to move in a specific direction or plane while maintaining stiffness in other directions. The impedance controller was used to insert the peg into the hole, navigating through the zig-zag maze and push on the egg.
#include "math.h"
#include "F28335Serial.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
#define GRAV 9.81
#define NUMJOINT 3
#define NUMTRAJ 16
#define INITTIME 1500
// 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.43; //-0.37; Keep this so you remember the sign
float offset_Enc3_rad = 0.22; //0.27; Keep this so you remember the sign
// Trajectory structure
typedef struct traj_s{
float x_start; // X axis start location
float y_start; // Y axis start location
float z_start; // Z axis start location
float x_end; // X axis end location
float y_end; // Y axis end location
float z_end; // Z axis end location
float kpx; // Kpx if force control is used
float kpy; // Kpy if force control is used
float kpz; // Kpz if force control is used
float kdx; // Kdx if force control is used
float kdy; // Kdy if force control is used
float kdz; // Kdz if force control is used
float theta_z; // thetaZ if force control is used
float force_z; // Force in Z axis if force control is used
float time_start; // Start time for the current trajactory
float time_end; // End time for the current trajactory
uint16_t mode; // 0 for PID control, 1 for task space control
}traj_t;
traj_t traj[NUMTRAJ] = {
//x_s y_s z_s x_e y_e z_e kpx kpy kpz kdx kdy kdz theta_z force_z time_s time_e mode
{0.14, 0, 0.43, 0.25, 0, 0.50, 1000, 1000, 1000, 50, 50, 50, 0, 0, 0, 500, 0 }, // 1-2
{0.25, 0, 0.50, 0.028, 0.35, 0.23, 1000, 1000, 1000, 50, 50, 50, 0, 0, 500, 1500, 0 }, // 2-3
{0.028, 0.35, 0.23, 0.028, 0.35, 0.13, 200, 200, 1000, 10, 10, 50, 0, 0, 1500, 2500, 1 }, // 3-4
{0.028, 0.35, 0.13, 0.028, 0.35, 0.23, 200, 200, 1000, 10, 10, 50, 0, 0, 2500, 3500, 1 }, // 4-3
{0.028, 0.35, 0.23, 0.20, 0.10, 0.35, 1000, 1000, 1000, 50, 50, 50, 0, 0, 3500, 4000, 0 }, // 3-5
{0.20, 0.10, 0.35, 0.38, 0.12, 0.21, 1000, 1000, 1000, 50, 50, 50, 0, 0, 4000, 5000, 0 }, // 5-6
{0.38, 0.12, 0.21, 0.42, 0.05, 0.21, 1000, 300, 1000, 50, 15, 50, -1.166, 0, 5000, 6000, 1 }, // 6-7
{0.42, 0.05, 0.21, 0.31, 0.06, 0.21, 1000, 300, 1000, 50, 15, 50, -0.100, 0, 6000, 7000, 1 }, // 7-8
{0.31, 0.06, 0.21, 0.40, -0.04, 0.21, 1000, 300, 1000, 50, 15, 50, -0.896, 0, 7000, 8000, 1 }, // 8-9
{0.40, -0.04, 0.21, 0.40, -0.04, 0.35, 1000, 1000, 1000, 50, 50, 50, 0, 0, 8000, 8500, 0 }, // 9-10
{0.40, -0.04, 0.35, 0.24, 0.18, 0.32, 1000, 1000, 1000, 50, 50, 50, 0, 0, 8500, 9000, 0 }, // 10-12
{0.24, 0.18, 0.31, 0.24, 0.18, 0.285, 1000, 1000, 1000, 50, 50, 50, 0, 0, 9000, 9500, 0 }, // 12-11
{0.24, 0.18, 0.285, 0.24, 0.18, 0.285, 1000, 1000, 1000, 50, 50, 50, 0, 0, 9500, 11500, 1 }, // 11-11
{0.24, 0.18, 0.285, 0.24, 0.18, 0.32, 1000, 1000, 1000, 50, 50, 50, 0, 0, 11500, 12000, 0 }, // 11-12
{0.24, 0.18, 0.35, 0.25, 0.00, 0.50, 1000, 1000, 1000, 50, 50, 50, 0, 0, 12000, 12500, 0 }, // 12-2
{0.25, 0.00, 0.50, 0.25, 0.00, 0.50, 1000, 1000, 1000, 50, 50, 50, 0, 0, 12500, 100000, 0 }, // 2-
};
// Current step
uint16_t cur_step = 0;
// Current state structure
typedef struct cur_state_s{
float x_desired;
float y_desired;
float z_desired;
float x_cur;
float y_cur;
float z_cur;
float theta_M[NUMJOINT];
float theta_desired[NUMJOINT];
float theta_d_desired[NUMJOINT];
float theta_dd_desired[NUMJOINT];
float e_theta[NUMJOINT];
float e_theta_d[NUMJOINT];
float e_theta_old[NUMJOINT];
float theta_d[NUMJOINT];
float theta_d_old1[NUMJOINT];
float theta_d_old2[NUMJOINT];
float theta_d_filtered[NUMJOINT];
float theta_M_old[NUMJOINT];
float I_k[NUMJOINT];
float x_cur_dot;
float y_cur_dot;
float z_cur_dot;
float x_cur_dot_filtered;
float y_cur_dot_filtered;
float z_cur_dot_filtered;
float x_desired_dot;
float y_desired_dot;
float z_desired_dot;
float x_cur_old;
float y_cur_old;
float z_cur_old;
float x_desired_old;
float y_desired_old;
float z_desired_old;
float x_cur_dot_old1, x_cur_dot_old2;
float y_cur_dot_old1, y_cur_dot_old2;
float z_cur_dot_old1, z_cur_dot_old2;
}cur_state_t;
cur_state_t cur_state;
// Task space PD control variables
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 x_cur = 0;
float y_cur = 0;
float z_cur = 0;
float Fx = 0;
float Fy = 0;
float Fz = 0;
float x_desired = 0;
float y_desired = 0;
float z_desired = 0;
float x_cur_dot = 0;
float y_cur_dot = 0;
float z_cur_dot = 0;
float x_cur_dot_filtered = 0;
float y_cur_dot_filtered = 0;
float z_cur_dot_filtered = 0;
float x_desired_dot = 0;
float y_desired_dot = 0;
float z_desired_dot = 0;
float x_cur_old = 0;
float y_cur_old = 0;
float z_cur_old = 0;
float x_desired_old = 0;
float y_desired_old = 0;
float z_desired_old = 0;
float x_cur_dot_old1, x_cur_dot_old2;
float y_cur_dot_old1, y_cur_dot_old2;
float z_cur_dot_old1, z_cur_dot_old2;
float cosz = 0;
float sinz = 0;
float cosx = 0;
float sinx = 0;
float cosy = 0;
float siny = 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;
float Fxcmd = 0;
float Fycmd = 0;
const float Kt = 1;
// Global variables.
long mycount = 0;
#pragma DATA_SECTION(whattoprint, ".my_vars")
float whattoprint = 0.0;
#pragma DATA_SECTION(theta1array, ".my_arrs")
float theta1array[100];
#pragma DATA_SECTION(print_theta2, ".my_vars")
float print_theta2 = 0.0;
#pragma DATA_SECTION(theta2array, ".my_arrs")
float theta2array[100];
long arrayindex = 0;
int UARTprint = 0;
float printtheta1motor = 0;
float printtheta2motor = 0;
float printtheta3motor = 0;
// Plot to Simulink
float Simulink_PlotVar1 = 0;
float Simulink_PlotVar2 = 0;
float Simulink_PlotVar3 = 0;
float Simulink_PlotVar4 = 0;
float px, py, pz;
float thetaM_1, thetaM_2, thetaM_3;
float IK_theta1, IK_theta2, IK_theta3;
float IK_M_theta1, IK_M_theta2, IK_M_theta3;
// PID control
float tau[NUMJOINT];
float kp[NUMJOINT] = {30, 200, 100}; // No FF {15, 250, 50} // FF {15, 100, 25}
float kd[NUMJOINT] = {2.5, 5.0, 4.0}; // No FF {1, 10.0, 2.0} // FF {1, 5.0, 1.0}
// Integration control
float threshold = 0.1;
float ki[NUMJOINT] = {0, 0, 0}; // No FF {0, 200.0, 10.0} // FF {10, 200.0, 10.0}
// Friction compensation
float viscous_positive[NUMJOINT] = {0.1800, 0.2000, 0.1400};
float viscous_negative[NUMJOINT] = {0.2200, 0.2800, 0.2000};
float coulomb_positive[NUMJOINT] = {0.3600, 0.4000, 0.3500};
float coulomb_negative[NUMJOINT] = {-0.3000, -0.4500, -0.5000};
float slope_between_minimums[NUMJOINT] = {3.6, 3.6, 3.6};
float u_fric[NUMJOINT];
float minimum_velocity[NUMJOINT] = {0.1, 0.1, 0.1};
float ff = 1;
// Calculate the current desired Cartisian coordinates
void calculate_step(traj_t cur_traj, uint16_t t, float *x_desired, float *y_desired, float *z_desired){
float dis_x = cur_traj.x_end - cur_traj.x_start;
float dis_y = cur_traj.y_end - cur_traj.y_start;
float dis_z = cur_traj.z_end - cur_traj.z_start;
float x_step = dis_x / (cur_traj.time_end - cur_traj.time_start);
float y_step = dis_y / (cur_traj.time_end - cur_traj.time_start);
float z_step = dis_z / (cur_traj.time_end - cur_traj.time_start);
*x_desired = cur_traj.x_start + x_step * (t - cur_traj.time_start);
*y_desired = cur_traj.y_start + y_step * (t - cur_traj.time_start);
*z_desired = cur_traj.z_start + z_step * (t - cur_traj.time_start);
}
// Calculate the current actual Cartisian coordinates from motor angles
void motor_angle_to_cartesian(float theta1, float theta2, float theta3, float *x, float *y, float *z){
float cos_q1 = cos(theta1);
float sin_q1 = sin(theta1);
float cos_q2 = cos(theta2);
float sin_q2 = sin(theta2);
float cos_q3 = cos(theta3);
float sin_q3 = sin(theta3);
*x = 0.254 * cos_q1 * (cos_q3 + sin_q2);
*y = 0.254 * sin_q1 * (cos_q3 + sin_q2);
*z = 0.254 * (1 + cos_q2 - sin_q3);
}
// Calculate current state variables for PID control and force control
void calc_cur_state(traj_t cur_traj, uint16_t t){
// Calculate desired Cartesian position
calculate_step(cur_traj, t, &cur_state.x_desired, &cur_state.y_desired, &cur_state.z_desired);
// Inverse kinematics
float theta1_DH = atan2(cur_state.y_desired, cur_state.x_desired);
float theta2_DH = -atan2((cur_state.z_desired - 0.254),sqrt((cur_state.x_desired * cur_state.x_desired)+(cur_state.y_desired * cur_state.y_desired))) - acos(sqrt((cur_state.z_desired - 0.254) * (cur_state.z_desired - 0.254) + cur_state.x_desired * cur_state.x_desired + cur_state.y_desired * cur_state.y_desired) / (2 * 0.254));
float theta3_DH = 2 * acos(sqrt((cur_state.z_desired - 0.254) * (cur_state.z_desired - 0.254) + cur_state.x_desired * cur_state.x_desired + cur_state.y_desired * cur_state.y_desired) / (2 * 0.254));
cur_state.theta_desired[0] = theta1_DH;
cur_state.theta_desired[1] = theta2_DH + HALFPI;
cur_state.theta_desired[2] = theta3_DH + cur_state.theta_desired[1] - HALFPI;
// Calculate PID data
for (int i=0; i<=2; i++){
// calculate states
cur_state.e_theta[i] = cur_state.theta_desired[i] - cur_state.theta_M[i];
cur_state.theta_d[i] = (cur_state.theta_M[i] - cur_state.theta_M_old[i])*1000;
cur_state.theta_d_filtered[i] = (cur_state.theta_d[i] + cur_state.theta_d_old1[i] + cur_state.theta_d_old2[i]) / 3.0;
// Integration control
// if (abs(tau[i]) < 5) I_k[i] += (e_theta_old[i] + e_theta[i]) / 2.0 * 0.001;
// if (abs(e_theta[i]) > threshold) I_k[i] = 0;
// save old states
cur_state.theta_M_old[i] = cur_state.theta_M[i];
cur_state.theta_d_old2[i] = cur_state.theta_d_old1[i];
cur_state.theta_d_old1[i] = cur_state.theta_d_filtered[i];
cur_state.e_theta_old[i] = cur_state.e_theta[i];
}
// Calculate task space data
// Rotation zxy and its Transpose
cosz = cos(cur_traj.theta_z);
sinz = sin(cur_traj.theta_z);
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(cur_state.theta_M[0]);
sinq1 = sin(cur_state.theta_M[0]);
cosq2 = cos(cur_state.theta_M[1]);
sinq2 = sin(cur_state.theta_M[1]);
cosq3 = cos(cur_state.theta_M[2]);
sinq3 = sin(cur_state.theta_M[2]);
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;
motor_angle_to_cartesian(cur_state.theta_M[0], cur_state.theta_M[1], cur_state.theta_M[2], &cur_state.x_cur, &cur_state.y_cur, &cur_state.z_cur);
// Calculate states
cur_state.x_desired_dot = (cur_state.x_desired - cur_state.x_desired_old) * 1000;
cur_state.y_desired_dot = (cur_state.y_desired - cur_state.y_desired_old) * 1000;
cur_state.z_desired_dot = (cur_state.z_desired - cur_state.z_desired_old) * 1000;
cur_state.x_cur_dot = (cur_state.x_cur - cur_state.x_cur_old) * 1000;
cur_state.y_cur_dot = (cur_state.y_cur - cur_state.y_cur_old) * 1000;
cur_state.z_cur_dot = (cur_state.z_cur - cur_state.z_cur_old) * 1000;
cur_state.x_cur_dot_filtered = (cur_state.x_cur_dot + cur_state.x_cur_dot_old1 + cur_state.x_cur_dot_old2) / 3.0;
cur_state.y_cur_dot_filtered = (cur_state.y_cur_dot + cur_state.y_cur_dot_old1 + cur_state.y_cur_dot_old2) / 3.0;
cur_state.z_cur_dot_filtered = (cur_state.z_cur_dot + cur_state.z_cur_dot_old1 + cur_state.z_cur_dot_old2) / 3.0;
// Save old states
cur_state.x_desired_old = cur_state.x_desired;
cur_state.x_cur_old = cur_state.x_cur;
cur_state.x_cur_dot_old2 = cur_state.x_cur_dot_old1;
cur_state.x_cur_dot_old1 = cur_state.x_cur_dot_filtered;
cur_state.y_desired_old = cur_state.y_desired;
cur_state.y_cur_old = cur_state.y_cur;
cur_state.y_cur_dot_old2 = cur_state.y_cur_dot_old1;
cur_state.y_cur_dot_old1 = cur_state.y_cur_dot_filtered;
cur_state.z_desired_old = cur_state.z_desired;
cur_state.z_cur_old = cur_state.z_cur;
cur_state.z_cur_dot_old2 = cur_state.z_cur_dot_old1;
cur_state.z_cur_dot_old1 = cur_state.z_cur_dot_filtered;
}
// This function is called every 1 ms
void lab(float theta1motor,float theta2motor,float theta3motor,float *tau1,float *tau2,float *tau3, int error) {
// save past states
if ((mycount%50)==0) {
theta1array[arrayindex] = theta1motor;
theta2array[arrayindex] = theta2motor;
if (arrayindex >= 99) {
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
}
// Sending data to print
printtheta1motor = theta1motor;
printtheta2motor = theta2motor;
printtheta3motor = theta3motor;
cur_state.theta_M[0] = theta1motor;
cur_state.theta_M[1] = theta2motor;
cur_state.theta_M[2] = theta3motor;
if (mycount > traj[cur_step].time_end){
cur_step++;
}
// Pre-control calculation
calc_cur_state(traj[cur_step], mycount);
x_desired = cur_state.x_desired;
y_desired = cur_state.y_desired;
z_desired = cur_state.z_desired;
// Switch between PID control and force control
if (traj[cur_step].mode == 0){
// PID control
for (int i=0; i<=2; i++){
// calculate tau
tau[i] = kp[i] * cur_state.e_theta[i] - kd[i] * cur_state.theta_d_filtered[i]; // + ki[i] * I_k[i];
}
}
else if (traj[cur_step].mode == 1){
//Force for non world frame
tau[0] = (JT_11*R11 + JT_12*R21 + JT_13*R31)*(RT11*traj[cur_step].kdx*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT11*traj[cur_step].kpx*(cur_state.x_cur - cur_state.x_desired) + RT12*traj[cur_step].kdx*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT12*traj[cur_step].kpx*(cur_state.y_cur - cur_state.y_desired) + RT13*traj[cur_step].kdx*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT13*traj[cur_step].kpx*(cur_state.z_cur - cur_state.z_desired)) + (JT_11*R12 + JT_12*R22 + JT_13*R32)*(RT21*traj[cur_step].kdy*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT21*traj[cur_step].kpy*(cur_state.x_cur - cur_state.x_desired) + RT22*traj[cur_step].kdy*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT22*traj[cur_step].kpy*(cur_state.y_cur - cur_state.y_desired) + RT23*traj[cur_step].kdy*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT23*traj[cur_step].kpy*(cur_state.z_cur - cur_state.z_desired)) + (JT_11*R13 + JT_12*R23 + JT_13*R33)*(RT31*traj[cur_step].kdz*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT31*traj[cur_step].kpz*(cur_state.x_cur - cur_state.x_desired) + RT32*traj[cur_step].kdz*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT32*traj[cur_step].kpz*(cur_state.y_cur - cur_state.y_desired) + RT33*traj[cur_step].kdz*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT33*traj[cur_step].kpz*(cur_state.z_cur - cur_state.z_desired));
tau[1] = (JT_21*R11 + JT_22*R21 + JT_23*R31)*(RT11*traj[cur_step].kdx*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT11*traj[cur_step].kpx*(cur_state.x_cur - cur_state.x_desired) + RT12*traj[cur_step].kdx*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT12*traj[cur_step].kpx*(cur_state.y_cur - cur_state.y_desired) + RT13*traj[cur_step].kdx*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT13*traj[cur_step].kpx*(cur_state.z_cur - cur_state.z_desired)) + (JT_21*R12 + JT_22*R22 + JT_23*R32)*(RT21*traj[cur_step].kdy*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT21*traj[cur_step].kpy*(cur_state.x_cur - cur_state.x_desired) + RT22*traj[cur_step].kdy*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT22*traj[cur_step].kpy*(cur_state.y_cur - cur_state.y_desired) + RT23*traj[cur_step].kdy*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT23*traj[cur_step].kpy*(cur_state.z_cur - cur_state.z_desired)) + (JT_21*R13 + JT_22*R23 + JT_23*R33)*(RT31*traj[cur_step].kdz*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT31*traj[cur_step].kpz*(cur_state.x_cur - cur_state.x_desired) + RT32*traj[cur_step].kdz*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT32*traj[cur_step].kpz*(cur_state.y_cur - cur_state.y_desired) + RT33*traj[cur_step].kdz*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT33*traj[cur_step].kpz*(cur_state.z_cur - cur_state.z_desired));
tau[2] = (JT_31*R11 + JT_32*R21 + JT_33*R31)*(RT11*traj[cur_step].kdx*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT11*traj[cur_step].kpx*(cur_state.x_cur - cur_state.x_desired) + RT12*traj[cur_step].kdx*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT12*traj[cur_step].kpx*(cur_state.y_cur - cur_state.y_desired) + RT13*traj[cur_step].kdx*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT13*traj[cur_step].kpx*(cur_state.z_cur - cur_state.z_desired)) + (JT_31*R12 + JT_32*R22 + JT_33*R32)*(RT21*traj[cur_step].kdy*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT21*traj[cur_step].kpy*(cur_state.x_cur - cur_state.x_desired) + RT22*traj[cur_step].kdy*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT22*traj[cur_step].kpy*(cur_state.y_cur - cur_state.y_desired) + RT23*traj[cur_step].kdy*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT23*traj[cur_step].kpy*(cur_state.z_cur - cur_state.z_desired)) + (JT_31*R13 + JT_32*R23 + JT_33*R33)*(RT31*traj[cur_step].kdz*(cur_state.x_desired_dot - cur_state.x_cur_dot_filtered) - RT31*traj[cur_step].kpz*(cur_state.x_cur - cur_state.x_desired) + RT32*traj[cur_step].kdz*(cur_state.y_desired_dot - cur_state.y_cur_dot_filtered) - RT32*traj[cur_step].kpz*(cur_state.y_cur - cur_state.y_desired) + RT33*traj[cur_step].kdz*(cur_state.z_desired_dot - cur_state.z_cur_dot_filtered) - RT33*traj[cur_step].kpz*(cur_state.z_cur - cur_state.z_desired));
tau[0] += JT_11 * Fxcmd / Kt + JT_12 * Fycmd / Kt + JT_13 * traj[cur_step].force_z / Kt;
tau[1] += JT_21 * Fxcmd / Kt + JT_22 * Fycmd / Kt + JT_23 * traj[cur_step].force_z / Kt;
tau[2] += JT_31 * Fxcmd / Kt + JT_32 * Fycmd / Kt + JT_33 * traj[cur_step].force_z / Kt;
}
// Friction compensation
for (int i=0; i<=2; i++){
if (cur_state.theta_d_filtered[i] > minimum_velocity[i]) {
u_fric[i] = viscous_positive[i] * cur_state.theta_d_filtered[i] + coulomb_positive[i];
}
else if (cur_state.theta_d_filtered[i] < -minimum_velocity[i]) {
u_fric[i] = viscous_negative[i] * cur_state.theta_d_filtered[i] + coulomb_negative[i];
}
else {
u_fric[i] = slope_between_minimums[i] * cur_state.theta_d_filtered[i];
}
}
tau[0] += u_fric[0] * ff;
tau[1] += u_fric[1] * ff;
tau[2] += u_fric[2] * ff;
*tau1 = fminf(fmaxf(tau[0], -5), 5);
*tau2 = fminf(fmaxf(tau[1], -5), 5);
*tau3 = fminf(fmaxf(tau[2], -5), 5);
Simulink_PlotVar1 = theta1motor;
Simulink_PlotVar2 = theta2motor;
Simulink_PlotVar3 = theta3motor;
Simulink_PlotVar4 = cur_state.theta_desired[0];
mycount++;
}
void printing(void){
// serial_printf(&SerialA, "Motor angle: %.2f %.2f,%.2f \n\r", printtheta1motor*180/PI,printtheta2motor*180/PI,printtheta3motor*180/PI);
thetaM_1 = printtheta1motor;
thetaM_2 = printtheta2motor;
thetaM_3 = printtheta3motor;
px = (127*cos(thetaM_1)*(cos(thetaM_3) + sin(thetaM_2)))/500;
py = (127*sin(thetaM_1)*(cos(thetaM_3) + sin(thetaM_2)))/500;
pz = (127*cos(thetaM_2))/500 - (127*sin(thetaM_3))/500 + 127.0/500;
serial_printf(&SerialA, "FK End effector position: %.2f %.2f,%.2f \n\r", px, py, pz);
IK_theta1 = atan2(py,px);
IK_theta2 = -atan2((pz - 0.254),sqrt((px * px)+(py * py))) - acos(sqrt((pz - 0.254) * (pz - 0.254) + px * px + py * py) / (2 * 0.254));
IK_theta3 = 2 * acos(sqrt((pz - 0.254) * (pz - 0.254) + px * px + py * py) / (2 * 0.254));
IK_M_theta1 = IK_theta1;
IK_M_theta2 = IK_theta2 + HALFPI;
IK_M_theta3 = IK_theta3 + IK_M_theta2 - HALFPI;
// serial_printf(&SerialA, "IK Motor Angle: %.2f %.2f,%.2f \n\r", IK_M_theta1*180/PI, IK_M_theta2*180/PI, IK_M_theta3*180/PI);
// serial_printf(&SerialA, "\n\r");
}
Comments