//ProjectME446.c
//Written by Animesh Singh and Ethan Cho
/*
* The code is written for the completion of the final project demo for ME446 - Robot Dynamics
and Control
* The initial part of the code initializes the global variables used for states,
* filtering of states, control gains, friction compensation parameters and the waypoints
* structure and actual waypoints
* The code has been programmed to repeat the trajectory in a circular loop to make it easier for
* evaluation and calibration of smoothness(time interval and distance)
*/
#include "math.h"
#include "F28335Serial.h"
#include <stdio.h>
// These constants are defined in advance to improve efficiency.
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
#define GRAV 9.81
#define LENGTH 0.254
// 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 offsets will adjust the encoder readings
float offset_Enc2_rad = -0.45; //-0.37;
float offset_Enc3_rad = 0.24; //0.27;
// Your global variables.
// Important variable as we are using this to get the actual time
long mycount = 1;
// These variables are used to control the robot from Matlab to make it easier to debug and
// integrate Matlab into
// the control loop
#pragma DATA_SECTION(whattoprint, ".my_vars")
float whattoprint = 0.0;
#pragma DATA_SECTION(theta1array, ".my_arrs")
float theta1array[100];
#pragma DATA_SECTION(theta2array, ".my_arrs")
float theta2array[100];
#pragma DATA_SECTION(Lab1_trial, ".my_vars")
float Lab1_trial = 0.0;
long arrayindex = 0;
int UARTprint = 0;
// Variables to be used for printing the motor theta values to TeraTerm
float printtheta1motor = 0;
float printtheta2motor = 0;
float printtheta3motor = 0;
// Assign these floats 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;
// Joint and Motor Angles - used for calculation of DH angles
float dh_theta1 = 0;
float dh_theta2 = 0;
float dh_theta3 = 0;
float dh_theta1_inv = 0;
float dh_theta2_inv = 0;
float dh_theta3_inv = 0;
float motor_theta1_inv = 0;
float motor_theta2_inv = 0;
float motor_theta3_inv = 0;
// Joint states : multiple joint states are defined to filter the values of the joint states
float Theta1_old = 0;
float Omega1_old1 = 0;
float Omega1_old2 = 0;
float Omega1 = 0;
float Theta2_old = 0;
float Omega2_old1 = 0;
float Omega2_old2 = 0;
float Omega2 = 0;
float Theta3_old = 0;
float Omega3_old1 = 0;
float Omega3_old2 = 0;
float Omega3 = 0;
// Jacobian terms
float J1 = 0.0167;
float J2 = 0.03;
float J3 = 0.0128;
// Friction parameters for the three joints : used to eliminate the friction of the joints to
// make it easier
// to move around. This will also helps in removing any trajectory variations as friction may
// lead to non-uniform forces
float u_fric1 = 0;
float u_fric2 = 0;
float u_fric3 = 0;
float viscous_pos_1 = 0.2513;
float viscous_neg_1 = 0.2477;
float coulumb_pos_1 = 0.3637;
float coulumb_neg_1 = -0.2948;
float fslope1 = 3.6;
float velocity_threshold_1 = 0.1;
float viscous_pos_2 = 0.2500;
float viscous_neg_2 = 0.2870;
float coulumb_pos_2 = 0.4759;
float coulumb_neg_2 = -0.5031;
float fslope2 = 3.6;
float velocity_threshold_2 = -0.05;
float viscous_pos_3 = 0.1922;
float viscous_neg_3 = 0.38;
float coulumb_pos_3 = 0.2132;
float coulumb_neg_3 = -0.3190;
float fslope3 = 3.6;
float velocity_threshold_3 = 0.05;
// Friction coefficient : the friction compensation calculated above may over-compensate
// or under-compensate
// These coeeficients helps us to recalibrate them by just adding a scaling parameter
float ff1 = 0.75;
float ff2 = 0.7;
float ff3 = 0.85;
// Task Space Control : task space positions and velocities along with additional
// variables for filtering
float x = 0.1358;
float x_old1 = 0.1358;
float x_old2 = 0.1358;
float vx = 0;
float vx_old1 = 0;
float vx_old2 = 0;
float y = 0;
float y_old1 = 0;
float y_old2 = 0;
float vy = 0.12;
float vy_old1 = 0.12;
float vy_old2 = 0.12;
float z = 0.4229;
float z_old1 = 0.4229;
float z_old2 = 0.4229;
float vz = 0.05;
float vz_old1 = 0.05;
float vz_old2 = 0.05;
// Control Gains : Note that we are only using PD gains
float Kpx = 300;
float Kdx = 12;
float Kpy = 300;
float Kdy = 15;
float Kpz = 600;
float Kdz = 18;
// Forces : These variables help in transforming task space forces into joint torques
float Fx = 0;
float Fy = 0;
float Fz = 0;
float FxN = 0;
float FyN = 0;
float FzN = 0;
float Jx = 0;
float Jy = 0;
float Jz = 0;
// Desired values : desired values of task space positions and velocities
float x_d = 0;
float y_d = 0;
float z_d = 0;
float vx_d = 0;
float vy_d = 0;
float vz_d = 0;
// Impedance Forces : The desired force value that we want the robot to exert on contact
float Fz_impedance = 0;
float Fy_impedance = 0;
float Fx_impedance = 0;
float gravity_offset = 12;
float Kt = 6.0;
// Rotation : The rotation helps us to define the direction of movement and helps us to reduce the
// control gains perpendicular to it so that the robot moves in a straight line
float Rx = 0;
float Ry = 0;
float Rz = 0;
// Straight Line trajectory : defining variables for straight line, will help us to control
// the trajectory
uint16_t flag = 1;
float x_initial = 0.19;
float y_initial = 0;
float z_initial = 0.44;
float x_final = 0.19;
float y_final = 0.0;
float z_final = 0.44;
float xy_angle = 0;
// Way Point Structure : We have also kept the control gains as inputs to allow us to change them
struct waypoint {
float x;
float y;
float z;
float Rz;
float time;
float Kpx;
float Kpy;
float Kpz;
float Kdz;
float Fz_impedance;
};
// Specifying the waypoints through an array.
struct waypoint p[] = {// x, y, z, Rz, time, Kpx, Kpy, Kpz, Kdz, Fz
{0.21, 0.13, 0.48, 0 , 1.0, 300, 300, 600, 18, 0},
{0.04, 0.34, 0.29, 0 , 1.0, 300, 300, 600, 18, 0},
{0.04, 0.34, 0.13, 0 , 1.5, 200, 200, 600, 18, 0},
// this is for going inside the hole - less Kpx and Kpy
{0.04, 0.34, 0.13, 0 , 0.5, 200, 200, 600, 18, 0},
{0.04, 0.34, 0.50, 0 , 1.0, 300, 300, 600, 18, 0}, // Part 1 complete
{0.26, 0.07, 0.46, 0 , 0.5, 300, 300, 600, 18, 0},
{0.38, 0.09, 0.24, 0 , 0.5, 300, 300, 600, 18, 0},
{0.38, 0.09, 0.21, 0 , 0.5, 300, 300, 600, 18, 0},
// Part 2 starts here
{0.41, 0.04, 0.21, -0.93, 1.0, 300, 200, 600, 18, 0},
{0.40, 0.03, 0.21, 0 , 0.5, 300, 300, 600, 18, 0},
{0.34, 0.044, 0.21, -0.26, 1.0, 300, 300, 600, 18, 0},
{0.33, 0.04, 0.21, 0 , 0.2, 200, 200, 600, 18, 0},
{0.3292, 0.0448, 0.21, 0 , 0.2, 200, 200, 600, 18, 0},
{0.3211, 0.0422, 0.21, 0 , 0.2, 200, 200, 600, 18, 0},
{0.3152, 0.0351, 0.21, 0 , 0.2, 200, 200, 600, 18, 0},
{0.3162, 0.0217, 0.21, 0 , 0.2, 200, 200, 600, 18, 0},
{0.37, -0.04, 0.21, -0.93, 1.0, 300, 200, 600, 18, 0},
{0.37, -0.04, 0.51, 0 , 1.0, 300, 300, 600, 18, 0},
// zigzag complete
{0.28, 0.06, 0.33, 0 , 1.0, 300, 300, 600, 18, 0},
{0.2565, 0.1387, 0.32, 0 , 0.5, 300, 300, 600, 18, 0},
{0.2565, 0.1387, 0.2915,0 , 0.5, 500, 500, 400, 9, 0},
// reaching above the egg
{0.2565, 0.1387, 0.2915,0 , 3.0, 500, 500, 400, 9,10},
// pressing the egg using impedance control(decrease Kpz and increase Fz)
{0.2565, 0.1387, 0.50, 0 , 1.0, 300, 300, 600, 18, 0},
{0.25, 0.0 , 0.51, 0 , 0.8, 300, 300, 600, 18, 0},
};
float time_end = 0;
float time_current = 0;
float time_interval = 0;
float time_old = 0;
uint16_t index = 0;
uint16_t n_waypoints = 24;
void mains_code(void);
//
// Main
//
void main(void)
{
mains_code();
}
//lab() calculates torque values required for the robot's joints to follow a desired trajectory.
// It incorporates filtering techniques
//to smooth velocity measurements and account for frictional effects to ensure accurate control
// of the robotic system.
void lab(float theta1motor,float theta2motor,float theta3motor,float *tau1,float *tau2,float *tau3, int error) {
if ((mycount%50)==0) {
theta1array[arrayindex] = theta1motor;
if (arrayindex >= 99) {
arrayindex = 0;
} else {
arrayindex++;
}
}
time_current = mycount/1000.0;
// This section calculates a straight line task-space trajectory from one point to the next.
if((time_current >= time_end) && (index != n_waypoints))
{
time_end = p[index].time + time_end;
x_initial = x_final;
y_initial = y_final;
z_initial = z_final;
x_final = p[index].x;
y_final = p[index].y;
z_final = p[index].z;
time_interval = time_end - time_current;
vx_d = (x_final-x_initial)/time_interval;
vy_d = (y_final-y_initial)/time_interval;
vz_d = (z_final-z_initial)/time_interval;
Rz = p[index].Rz;
//Kpx = p[index].Kpx;
//Kpy = p[index].Kpy;
Kpz = p[index].Kpz;
Kdz = p[index].Kdz;
Fz_impedance = p[index].Fz_impedance;
time_old = time_current;
index = index + 1;
}
x_d = (x_final - x_initial)*(time_current - time_old)/time_interval + x_initial;
y_d = (y_final - y_initial)*(time_current - time_old)/time_interval + y_initial;
z_d = (z_final - z_initial)*(time_current - time_old)/time_interval + z_initial;
if ((index == n_waypoints) && (time_current >= time_end))
{
// If we want the robot to stop after reaching the last waypoint, uncomment the block below
//vx_d = 0;
//vy_d = 0;
//vz_d = 0;
//x_d = x_final;
//y_d = y_final;
//z_d = z_final;
// Repeating the whole trajectory after reaching the endpoint
index = 0;
time_end = 0;
time_current = 0;
time_interval = 0;
time_old = 0;
mycount=0;
}
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;
// Plotting the variables in Simulink
Simulink_PlotVar1 = x_d;
Simulink_PlotVar2 = y_d;
Simulink_PlotVar3 = z_d;
Simulink_PlotVar4 = time_interval;
// Filtering to find the velocity of the motors
// The filter works by averaging the previous three velocity values to remove the noise
// from our calculations.
Omega1 = (theta1motor - Theta1_old)/0.001;
Omega1 = (Omega1 + Omega1_old1 + Omega1_old2)/3.0;
Theta1_old = theta1motor;
Omega1_old2 = Omega1_old1;
Omega1_old1 = Omega1;
Omega2 = (theta2motor - Theta2_old)/0.001;
Omega2 = (Omega2 + Omega2_old1 + Omega2_old2)/3.0;
Theta2_old = theta2motor;
Omega2_old2 = Omega2_old1;
Omega2_old1 = Omega2;
Omega3 = (theta3motor - Theta3_old)/0.001;
Omega3 = (Omega3 + Omega3_old1 + Omega3_old2)/3.0;
Theta3_old = theta3motor;
Omega3_old2 = Omega3_old1;
Omega3_old1 = Omega3;
// Friction component calculation
// There are three friction ranges - negative velocity, positive velocity and zero velocity
// The zero velocity range applies forces proportional to the velocity as seen in the else
// statement(else condition)
// The positive velocity friction applies a friction force that is also dependent on the velocity
// but with an additional bias term(if condition). Similarly for the negative velocity
// friction(else if condition)
// We need to follow the same calculations for all the three joints and tune the coefficients
// Joint 1
if (Omega1 > velocity_threshold_1)
u_fric1 = viscous_pos_1*Omega1 + coulumb_pos_1;
else if (Omega1 < -velocity_threshold_1)
u_fric1 = viscous_neg_1*Omega1 + coulumb_neg_1;
else
u_fric1 = fslope1*Omega1;
// Joint 2
if (Omega2 > velocity_threshold_2)
u_fric2 = viscous_pos_2*Omega2 + coulumb_pos_2;
else if (Omega2 < -velocity_threshold_2)
u_fric2 = viscous_neg_2*Omega2 + coulumb_neg_2;
else
u_fric2 = fslope2*Omega2;
// Joint 3
if (Omega3 > velocity_threshold_3)
u_fric3 = viscous_pos_3*Omega3 + coulumb_pos_3;
else if (Omega3 < -velocity_threshold_3)
u_fric3 = viscous_neg_3*Omega3 + coulumb_neg_3;
else
u_fric3 = fslope3*Omega3;
// x, y, and z coordinate axis
// This expression just uses the forward dynamics calculation
x = LENGTH * cos(theta1motor)*(sin(theta2motor) + cos(theta3motor));
y = LENGTH * sin(theta1motor)*(sin(theta2motor) + cos(theta3motor));
z = LENGTH - LENGTH*sin(theta3motor) + LENGTH*cos(theta2motor);
// Force calculation
// Just like we filtered for velocity in joint space, for the task space controller we need to
// filter the velocities in x, y, and z directions to remove the noise
// vx Calculation
vx = (x - x_old1)/0.001;
vx = (vx + vx_old1 + vx_old2)/3.0;
vx_old2 = vx_old1;
vx_old1 = vx;
x_old1 = x;
// vy calculation
vy = (y - y_old1)/0.001;
vy = (vy + vy_old1 + vy_old2)/3.0;
vy_old2 = vy_old1;
vy_old1 = vy;
y_old1 = y;
// vz calculation
vz = (z - z_old1)/0.001;
vz = (vz + vz_old1 + vz_old2)/3.0;
vz_old2 = vz_old1;
vz_old1 = vz;
z_old1 = z;
// C does not contain a linear algebra package and hence we need to do the matrix calculations
// by hand
// Calculation of angles to avoid repetitions
// This will make our program more efficient
float sinM1 = sin(theta1motor);
float cosM1 = cos(theta1motor);
float sinM2 = sin(theta2motor);
float cosM2 = cos(theta2motor);
float sinM3 = sin(theta3motor);
float cosM3 = cos(theta3motor);
// Rotational components along z,x,y
// This section will rotate the world axis to the direction that we want
// Useful to make the robot compliant along the axis we want
float sinX = sin(Rx);
float cosX = cos(Rx);
float sinY = sin(Ry);
float cosY = cos(Ry);
float sinZ = sin(Rz);
float cosZ = cos(Rz);
// Rotation Matrix components
// Efficient coding practice to avoid errors and repeated calculations
float R11 = cosZ*cosY-sinZ*sinX*sinY;
float R12 = -sinZ*cosX;
float R13 = cosZ*sinY+sinZ*sinX*cosY;
float R21 = sinZ*cosY+cosZ*sinX*sinY;
float R22 = cosZ*cosX;
float R23 = sinZ*sinY-cosZ*sinX*cosY;
float R31 = -cosX*sinY;
float R32 = sinX;
float R33 = cosX*cosY;
// Force calculation in World Frame
Fx = Kpx * (R11 * (x_d - x) + R21 * (y_d - y) + R31 * (z_d - z));
Fy = Kpy * (R12 * (x_d - x) + R22 * (y_d - y) + R32 * (z_d - z));
Fz = Kpz * (R13 * (x_d - x) + R23 * (y_d - y) + R33 * (z_d - z));
Fx = Fx + Kdx * (R11 * (vx_d - vx) + R21 * (vy_d - vy) + R31 * (vz_d - vz));
Fy = Fy + Kdy * (R12 * (vx_d - vx) + R22 * (vy_d - vy) + R32 * (vz_d - vz));
Fz = Fz + Kdz * (R13 * (vx_d - vx) + R23 * (vy_d - vy) + R33 * (vz_d - vz));
// Force calculation in Rotated Frame
FxN = R11 * Fx + R12 * Fy + R13 * Fz;
FyN = R21 * Fx + R22 * Fy + R23 * Fz;
FzN = R31 * Fx + R32 * Fy + R33 * Fz;
// Transforming forces in Task frame to Joint frame
Jx = -LENGTH*sinM1*(cosM3+sinM2) * FxN + LENGTH*cosM1*(cosM3+sinM2)*FyN;
Jy = LENGTH*cosM1*(cosM2 - sinM3)*FxN + LENGTH*sinM1*(cosM2-sinM3)*FyN - LENGTH*(cosM3+sinM2)*FzN;
Jz = -LENGTH*cosM1*sinM3*FxN - LENGTH*sinM1*sinM3*FyN - LENGTH*cosM3*FzN;
// Torque calculation
*tau1 = Jx + ff1*u_fric1;
*tau2 = Jy + ff2*u_fric2;
*tau3 = Jz + ff3*u_fric3;
// Saturation Joint 1
if (*tau1 > 5)
*tau1 = 5;
else if (*tau1 < -5)
*tau1 = -5;
// Saturation Joint 2
if (*tau2 > 5)
*tau2 = 5;
else if (*tau2 < -5)
*tau2 = -5;
// Saturation Joint 3
if (*tau3 > 5)
*tau3 = 5;
else if (*tau3 < -5)
*tau3 = -5;
// changing the direction every 1 second
if ((mycount%1000)==0) {
flag = flag*(-1);
}
mycount++;
}
void printing(void){
if (whattoprint == 0) {
// The print statement outputs to the Teraterm and hence we can use them for debugging
serial_printf(&SerialA, "Pos : %.4f %.4f,%.4f \n\r",x,y,z);
serial_printf(&SerialA, "Forces : %.2f %.2f,%.2f \n\r",Fx,Fy,Fz);
} else {
serial_printf(&SerialA, "Print test : %.2f \n\r", Lab1_trial);
}
}
Comments