ME446 Final Project (LL)

#include "math.h"
#include "F28335Serial.h"

#define PI      	3.1415926535897932384626433832795
#define TWOPI   	6.283185307179586476925286766559
#define HALFPI  	1.5707963267948966192313216916398
#define GRAV    	9.81

// 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.42324; //-0.37
float offset_Enc3_rad = 0.225147; //0.27


// Your global variables.

long mycount = 0;

#pragma DATA_SECTION(whattoprint, ".my_vars")
float whattoprint = 0.0;

#pragma DATA_SECTION(randoms, ".my_vars")
float randoms = 5.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;

//--------------------- below this line is code that we modified ---------------------------------

// Define the number of destinations (xb,tb,zb) in struct we create
#define NUMPOINTS 18

//the following variables store the previous motor theta values for each call of the lab function (theta[n-1]). It is important to store these values because they are necessary to calculate the velocity of the motors
float t1old = 0;
float t2old = 0;
float t3old = 0;

/*
The following variables have to do with calculating the filtered velocity of each motor which will be used for the joint space derivative control and friction compensation. Each motor velocity needs four variables:
    
    w1raw: which stores the raw motor velocity (theta[n] - theta[n-1])/(change in time (1ms)) for each call of the lab function
    w1filt: which stores the value of the calculated filtered angular velocity
    w1old: which stores the previously calculated filtered angular velocity value (w1filt[n-1]) from the last lab function call
    w1older: which stores the calculated filtered angular velocity value before the previous one (w1filt[n-2])
    
These values are stored for each motor in separate variables shown below:
*/

//velocity variables for motor 1 to store and generate a moving average
float w1raw = 0;
float w1old = 0;
float w1filt = 0;
float w1older = 0;

//velocity variables for motor 2 to store and generate a moving average
float w2raw = 0;
float w2old = 0;
float w2filt = 0;
float w2older = 0;

//velocity variables for motor 3 to store and generate a moving average
float w3raw = 0;
float w3old = 0;
float w3filt = 0;
float w3older = 0;

/*
The following variables have to do with the friction compensation for each motor which we use to try to minimize friction disturbances in the system to have a smoother controller. Each motor needs 7 friction compensation variables:

    vmin1: which stores the velocity value used as the boundary of the coulomb and viscous friction models
    visc_neg1: which stores the negative viscous friction curve slope
    visc_pos1: which stores the positive viscous friction curve slope
    coul_neg1: which stores the negative viscous friction y-intercept
    coul_pos1: which stores the positive viscous friction y-intercept
    slope_between_mins1: which stores the slope of the coulomb friction
    u_fric1: which initializes the friction compensation control effort
    
These values are stored for each motor in separate variables shown below:
*/

//friction compensation variables for motor 1
float vmin1 = 0.1;
float visc_neg1 = 0.20;
float visc_pos1 = 0.15;
float coul_neg1 = -0.2948;
float coul_pos1 = 0.3637;
float slope_between_mins1 = 3.6;
float u_fric1 = 0;

//friction compensation variables for motor 2
float vmin2 = 0.05;
float visc_neg2 = 0.21;
float visc_pos2 = 0.22;
float coul_neg2 = -0.5031;
float coul_pos2 = 0.25;
float slope_between_mins2 = 3.6;
float u_fric2 = 0;

//friction compensation variables for motor 3
float vmin3 = 0.05;
float visc_neg3 = 0.2;
float visc_pos3 = 0.15;
float coul_neg3 = -0.5;
float coul_pos3 = 0.4;
float slope_between_mins3 = 3.6;
float u_fric3 = 0;

//this variable is used to change the amount of friction compensation used in the control effort (1 - full friction compensation, 0.5 - half friction compensation, 0 - no friction compensation). This is useful for if the friction compensation is causes steady state oscillations, we can turn down its power until it stops
float frictionfactor = 0.85;

//we found new p4 and p5 from gravity compensation testing that seemed to work better with our robot than the given ones so we define those here for our gravity compensation calculations
float p4=0.085;
float p5=0.04;

//these variables initialize the gravity compensation control effort for each motor and are unused in our controller
float G2 = 0;
float G3 = 0;

//the following varibles initialize the task space error for both position and velocity for each coordinate axis
float ex = 0;
float ey = 0;
float ez = 0;
float exdot = 0;
float eydot = 0;
float ezdot = 0;

//the following variables define the initial task space Kp and Kd gains for each coordinate axis. These variables control the response of the robot to input signals. Kp makes the robot have a faster stronger response but it can cause the robot to be unstable. Kd damps the robot response to make it more controlled but can also introduce steady state errors.
float Kpx = 200;
float Kdx = 30;
float Kpy = 200;
float Kdy = 30;
float Kpz = 200;
float Kdz = 30;

//the following variables initialize the task space desired position and velocity for each coordinate axis. These are the values that the robot controller will try to match.
float xdes = 0.254;
float ydes = 0.254;
float zdes = 0.254;
float xdes_dot = 0;
float ydes_dot = 0;
float zdes_dot = 0;

//the following initializes the variables that will be used to store the current forward kinematic calculated end effector position in task space
float x = 0;
float y = 0;
float z = 0;

//the following initializes the variables that will be used to store the previously calculated forward kinematic calculated end effector position in the task space (x[n-1]). This will be useful for calculating task space velocity
float x_old = 0;
float y_old = 0;
float z_old = 0;

/*
The following variables are similar to the previous motor filtered velocity variables. The difference here is that the following velocities are for the task space and will be used for the task space derivative control. Once again each coordinate velocity needs four variables:

    x_dot_raw: which stores the raw task space velocity (x[n] - x[n-1])/(change in time (1ms)) for each call of the lab function
    x_dot_filt: which stores the value of the calculated filtered task space velocity
    x_dot_old: which stores the previously calculated filtered task space velocity value (x_dot_filt[n-1]) from the last lab function call
    x_dot_older: which stores the calculated filtered task space velocity value before the previous one (x_dot_filt[n-2])
    
These values are stored for each coordinate (x,y,z) in the variables shown below:
*/

//velocity variables for x direction
float x_dot_raw = 0;
float x_dot_filt = 0;
float x_dot_old = 0;
float x_dot_older = 0;

//velocity variables for y direction
float y_dot_raw = 0;
float y_dot_filt = 0;
float y_dot_old = 0;
float y_dot_older = 0;

//velocity variables for z direction
float z_dot_raw = 0;
float z_dot_filt = 0;
float z_dot_old = 0;
float z_dot_older = 0;

//The following variables initialize the task space force values that we can apply to the robot end effector
float Fx = 0;
float Fy = 0;
float Fz = 0;

//These variables can be used to apply a force in the z direction. This will be useful for the final project when we have to apply a force to an egg in the z direction
float Fz_cmd = 0; // to offset gravity 9.5 works well
float Kt = 6; //this value is the robot torque constant used for turning this force into a torque

//---------------sample code---------------------------
//the following variables were given in the Lab 4 sample code to initialize the rotation matrix, rotation matrix transpose, the end effector jacobian transpose, the rotation values for the rotation matrix, trig functions for each task space variable, and some angle values on each coordinate axis
float rotx = 0;
float roty = 0;
float rotz = 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;

//-----------------------------------------------

/*
The following variables have to do with setting the points for a line trajectory and defining variables that will make the controller follow the line. These are the following purposes of the variables explained for the x task space direction:

    xa: stores the start point of the line for each task space coordinate
    xb: stores the end point of the line for each task space coordinate
    delx: stores the value of the change in x that must occur to go from point A to point B
    t_total: stores the total amount of time that we want the robot to take to move from point A to point B
    t: stores the total amount of time that has passed since the program began running in seconds (current time value)
    t_start: stores the start time for when the robot began moving from point A to point B
    state: stores a number which defines which line the robot is currently following
    speed: stores the speed, in meters per second, that we want the end effector to travel at
    
The following variables initialize these values to be modified later in the Lab function:
*/

//xa and xb are initialized with a value in order to make the robot start at its initial position

//x start and end points
float xa = 0.1473;
float xb = 0.1473;

//y start and end points
float ya = 0;
float yb = 0;

//z start and end points
float za = 0.42546;
float zb = 0.42546;

//change in x, y, and z needed to go from point A to point B
float delx = 0;
float dely = 0;
float delz = 0;

//total time for the line movement
float t_total = 0;

//start time of the line movement
float t_start = 0;

//current time
float t = 0;4

//which line are we following
int state = 0;

//speed of the end effector
float speed = 0.15;

// 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;

// We use this integer as a boolean to know when we have inserted the peg into a hole. If it is 0 we have not yet, if it is 1 we have.
int insertion = 0;

// Relax mode : 1 = relax x, 2 = relax y, 3 = relax z, 0  =  all constrained
//This is a variable that we use to control our robot dynamics. Different numbers here will weaken different axis which is important for our controller to complete the task. Mode 0 makes the controller stiff on all axis, 1 relaxes x, 2 relaxes y, 3 relaxes z. Using a combination of these such as "12" will relax multiple axis at once. This is useful for many of the tasks that we need to complete in our demo. Specifics of which axis is relaxed will be described in our code when it appears.
int relaxmode = 0;

//We created a C struct to store the information important to our trajectory. The struct has 5 parts that define each part of the trajectory: an end point for each coordinate, a rotation value on the z axis, the relax mode needed, and the time taken to move for this step. Each member of the struct is defined as an array with the following form: [x position, y position, z position, z rotation, relax mode, time]
typedef struct point_tag{ // desired x,y,z, rotation of N frame, which axis is weakened
	float nextX;
	float nextY;
	float nextZ;
	float rotation_z;
	int mode;
    float total_time;
    
}points;

points destination[NUMPOINTS] = {
     //the following are all of the points of our trajectory along with a description of where each one is
     {0.1473,0,0.42546,0,0,0.8}, // 0, start position
     {0.254,0,0.508,0,0,0.2},	// 1, zero position
     {0.035,0.346,0.25, 0,0,0.5}, // 2, hole (up)
     {0.035,0.346,0.25, 0,0,0.1}, // 3, stay at hole (up)
     {0.035,0.346,0.13,0,12,0.5},  // 4, hole (down)
     {0.035,0.346,0.13,0,12,0.5},  // 5, stay at hole (down)
     {0.035,0.346,0.25, 0,0,0.3}, // 6, hole (up)
     {0.3,0.11,0.25,0,0,0.5},  // 7, point 1 to maze
     {0.36,0.11,0.2,0,0,0.2},  // 8, point 2 to maze
     {0.397,0.0411,0.2,0.644,1,0.2},  // 9, point 1 in maze
     {0.326,0.0496,0.2,1.309,1,0.2},  // 10, point 2 in maze
     {0.315,0.0324,0.2,0,1,0.08},  // 11, point 2 in maze
     {0.394,-0.0524,0.2,0.644,1,0.2},  // 12, point 3 in maze
     {0.394,-0.0524,0.3,0.644,1,0.1},  // 13, above point 3
     {0.246,0.225,0.3,0,0,0.5},  // 14, above egg
     {0.246,0.225,0.3,0,3,2},  // 15, push egg
     {0.246,0.225,0.3,0,0,0.25},  // 16, lift egg
     {0.254,0,0.508,0,0,0.5}	// 17, go back to zero position slower
};


/*
This is the main function of our robot controller. It is called automatically every 1 ms without fail. We do all of our calculations in it. The function assigns three variables that we use in our code as feedback:

    theta1motor: which is the current motor 1 angle
    theta2motor: which is the current motor 2 angle
    theta3motor: which is the current motor 3 angle
    
The function also allows us to assign three motor torque values to control the robot:

    *tau1: a pointer to the first motor torque
    *tau2: a pointer to the second motor torque
    *tau3: a pointer to the third motor torque
*/

void lab(float theta1motor,float theta2motor,float theta3motor,float *tau1,float *tau2,float *tau3, int error) {
    
    //--------------This code was provided in the initial Lab 1 code--------------------
	// save past states
	if ((mycount%50)==0) {

    	theta1array[arrayindex] = theta1motor;
    	theta2array[arrayindex] = theta2motor;

    	if (arrayindex >= 100) {
        	arrayindex = 0;
    	} else {
        	arrayindex++;
    	}

	}

	if ((mycount%500)==0) { // count every 0.5 second
    	UARTprint = 1;
    	GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1; // Blink LED on Control Card
    	GpioDataRegs.GPBTOGGLE.bit.GPIO60 = 1; // Blink LED on Emergency Stop Box
	}
	//--------------End initial Lab 1 code ---------------------------------------------
    
    //---------------------sample code provided in Lab 4 ---------------------------------
	// This is the code given to us in Lab 4 that calculates the end effector Jacobian transpose depending on the current motor angles. This is important for converting our task space controller into the joint space command torques
	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;

	// This is the code given to us in Lab 4 that calculates a rotation matrix depending on what rotations we define (rotx, roty, rotz). This is important for being able to turn our end effector task space reference frame so that we can weaken the controller in a direction that is not one of the main x, y, or z coordinate axis
	cosz = cos(rotz);
	sinz = sin(rotz);
	cosx = cos(rotx);
	sinx = sin(rotx);
	cosy = cos(roty);
	siny = sin(roty);
	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;
	//-------------------end sample code provided in Lab 4 ---------------------------------------------

	//this line of code uses mycount to determine the current time since the code began running. The variable mycount increments once every time the lab function is called. Since the lab function is called reliably every 1 millisecond, the current time (t) in seconds can be calculated by simply multiplying mycount by 0.001. This is a simple and effective process we use to time the line trajectory in this Lab.
	t=mycount*0.001;

    /*
    This is the code that generates the trajectory that our robot will follow. The trajectory is divided into different line segments, the details of which are stored in the destination struct. This if statement is called every time the previous line segment has finished. This is determined when the elapsed time since the current line segment started is greater than the time we determined that the robot will take the line segment to complete. We assume the trajectory is 100% complete once this time has elapsed. After that, we calculate new values to use for the next line segment in the following steps:
    
   	 1. We set the beginning of the new line segment to the end of the previous line segment. This should be the case since we are assuming that the trajectory is 100% complete once the elapsed time has passed.
   	 2. We gather all of the information in the struct that we need to define this line segment. This includes the end point, the relax mode, the rotation along the z axis, and the total time this line segment will take.
   	 3. Next, we calculate the change in x, y, and z that will need to occur to get from the start of the line to the end of the line. This is important for calculating the current position the robot must be at.
   	 4. Then, we reset the start time to the current time.
   	 5. Finally, we increment the state variable.
   	 
    The state variable is used to determine which line segment we are currently tracking. This represents directly which entry of the destination struct that we are reading. As the state variable gets incremented, we will go between the points described in the struct.
    */
    if (t-t_start >= t_total)
    {
   	 xa = xb;
   	 ya = yb;
   	 za = zb;
   	 xb = destination[state].nextX;
   	 yb = destination[state].nextY;
   	 zb = destination[state].nextZ;
   	 relaxmode = destination[state].mode;
   	 rotz = destination[state].rotation_z;
   	 t_total = destination[state].total_time;
   	 
   	 delx = xb-xa;
   	 dely = yb-ya;
   	 delz = zb-za;
   	 
   	 t_start = t;
   	 
   	 state++;
    }

    //once the state variable has gone higher than the amount of points defined in the struct, we reset the state variable to 1 in order to repeat the trajectory again!
    if (state >=17)
    {
   	 state = 1;
    }
    
    /*
    The following code is what controls the relax mode, or which axis are stiff and which are loose for the robot. In the destination struct, we hard coded only 4 different modes which are shown below. The relax mode variable is updated in the above trajectory function, and these if statements are called every millisecond to determine which axis to weaken. The following description is what each mode does:
    
   	 Mode 0: in this mode all axis are set to be stiff. This is the default mode of our robot and is essential for being able to move around to exact positions.
   	 Mode 1: in this mode the x axis is made weak while the other axis remain strong. This mode is used when going through the zig zag maze. In the zig zag maze we change the rotation of the z axis, which in turn changes the orientation of the end effector x axis in the world frame. This is useful because, in order to make the robot get through the maze smoother, we weaken the axis perpendicular to the direction of travel in the maze, for us the x axis. This weak axis helps the robot to not get stuck in the maze so we can make it go through it much faster.
   	 Mode 12: in this mode both the x and y axis are made weak while the z axis remains strong. This mode is very helpful for getting the peg into the hole. This is because for this action, the robot could easily get stuck on the edge of the hole when trying to push into it. By weakening the x and y axis, we allow the end effector peg to slide into the hole without much resistance. We keep the z axis strong so we can still determine the depth that we are going into the hole.
   	 Mode 3: in this mode we change the dynamics on the z axis completely, setting the Kp gain to 0 and adding a command force. We do this in order to apply a force to the egg. We keep the x and y axis stiff in this mode because we do not want the end effector peg to slide off the top of the egg. We leave Kd active in this mode because we want some damping in this system as the peg applies force to the egg. If we did not keep this damping, we are afraid that the end effector would hit the egg with too much force and break it. We determined the z force to apply to the egg by trial and error. The force is positive because without gravity compensation, this force must counteract the force of gravity acting in the z direction in order to apply the correct force to the egg. Another benefit of turning off Kp in this mode, is that the controller no longer depends on the desired position of the end effector. This is important because we do not want to apply a force by moving the end effector to a specific spot in the egg, rather we simply want to apply a force.
   	 
    */
	if (relaxmode == 0){ // all axis stiff
    	Kpx = 300;
    	Kdx = 30;
    	Kpy = 300;
    	Kdy = 30;
    	Kpz = 300;
    	Kdz = 30;
    	Fz_cmd = 0;
	}
	if (relaxmode == 1){ // x axis relaxed
    	Kpx = 100;
    	Kdx = 10;
    	Kpy = 300;
    	Kdy = 30;
    	Kpz = 300;
    	Kdz = 30;
    	Fz_cmd = 0;
	}
	if (relaxmode == 12){ // x,y axis relaxed
    	Kpx = 100;
    	Kdx = 10;
    	Kpy = 100;
    	Kdy = 10;
    	Kpz = 300;
    	Kdz = 30;
    	Fz_cmd = 0;
	}
	if (relaxmode == 3){ // force applied on z axis
    	Kpx = 300;
    	Kdx = 30;
    	Kpy = 300;
    	Kdy = 30;
    	Kpz = 0;
    	Kdz = 8;
    	Fz_cmd = 8.5;
	}

    /*
    The next part of the code is essential to trajectory generation as it takes in all of the values we just calculated along with the current time of the program, and calculates the desired position and desired velocity. The desired position is calculated by simple linear interpolation. The desired position starts at the point xa, ya, za. Then as the value of t-t_start increases from 0 to t_total, a fraction of the previously calculated delx, dely, delz are added until the desired variables reach the end point. This works simply by dividing (t-t_start) by t_total.
    
    Then the desired velocity is calculated. This is also a very easy calculation. We know the total change of each coordinate, and we know the total time. Since the velocity of a linear interpolation is constant, the velocity for the whole trajectory stays the same at delx/t_total (change in position divided by change in time).
    */

    //code that calculates desired position
	xdes = delx*(t-t_start)/t_total+xa;
	ydes = dely*(t-t_start)/t_total+ya;
	zdes = delz*(t-t_start)/t_total+za;

    //code that calculates the desired velocity
	xdes_dot = delx/t_total;
	ydes_dot = dely/t_total;
	zdes_dot = delz/t_total;

    /*
    The next part of the code calculates all of the current robot state values. The current task space position of the end effector, the filtered velocity, and the filtered task space velocity are all calculated here. The task space position of the end effector is straightforward as it is simply the forward kinematics equations found in Lab 1. The velocity is more complicated. From the physical robot we only receive angle data from the motor encoders. That means that we need to find a way to calculate the velocity based off of this data. The easiest way to calculate velocity is to take the change in motor angle and divide it by the time it took for that to happen. This method is seen in the raw velocity calculations below. The previous motor angle is subtracted from the current motor angle and then this value is divided by 0.001 seconds. This value of time is used because the lab function runs consistently every 1 ms so we can assume the time change for this velocity calculation is also 1 ms. This raw velocity, however, is not ideal. Calculating velocity in this way can generate a very noisy velocity signal. For this reason we filter the velocity. We do this by averaging current raw velocity by the two previous filtered velocities. This filter action smooths out noise, but comes with a trade off that it adds delay to our velocity reading. The fact that this filter averages the previous filtered velocities instead of raw ones is also important. This causes the velocity filter to be somewhat of an infinite average filter with a weight that makes it more sensitive to current velocity. All of that being said, once this calculation occurs, we are able to use the filtered velocity as an approximation for the actual robot velocity. This is useful for our control algorithms. Also important to note, when calculating the filtered task space velocity, the same approach is used but with forward kinematic task space positions instead of motor encoder angle values.
    */

	//this code calculates the current raw velocity by dividing the change of angle by change in time
	w1raw = (theta1motor - t1old)/0.001;
	w2raw = (theta2motor - t2old)/0.001;
	w3raw = (theta3motor - t3old)/0.001;

	//calculate a moving average velocity to smooth out noise from quantization. This is done by averaging the current raw velocity with the two previous filtered velocities.
	w1filt = (w1raw+w1old+w1older)/3.0;
	w2filt = (w2raw+w2old+w2older)/3.0;
	w3filt = (w3raw+w3old+w3older)/3.0;
    
    //Forward kinematics calculates the current task space end effector position
	x = 0.254*cosq1*(cosq3+sinq2);
	y = 0.254* sinq1*(cosq3+sinq2);
	z = 0.254*(1+cosq2-sinq3);

	//The raw end effector task space velocity is calculated in the same way as the raw motor angular velocity.
	x_dot_raw = (x - x_old)/0.001;
	y_dot_raw = (y - y_old)/0.001;
	z_dot_raw = (z - z_old)/0.001;

    //the filter for end effectory task space velocity is also calculated in the same way as the motor angular velocity
	x_dot_filt = (x_dot_raw + x_dot_old + x_dot_older)/3.0;
	y_dot_filt = (y_dot_raw + y_dot_old + y_dot_older)/3.0;
	z_dot_filt = (z_dot_raw + z_dot_old + z_dot_older)/3.0;

	/*
    The following code is our friction compensation code. The way this works is touched on above, when the variables are defined, but it will be explained in more detail here. The friction compensation of each motor is calculated in the same way, but with coefficients of friction that are different for each. We tuned the friction compensation coefficients in Lab 3 by feel. We tuned motor 1 by pushing on the joint and watching how it moved once the force was removed. With perfect friction compensation, the motor would continue moving at the same velocity after the force was removed. For the other two motors, we did the tuning in a different way. We tuned these motors by tossing the robot arm up and watching how it fell. Our goal was the make the robot arm feel like tossing a ball up and down. When a feeling like this was achieved, we tested all of our tuning by simply moving the end effector around the task space. When the end effector had little resistance in all directions, we knew we were done tuning.
    
    The friction compensation equation is divided into two parts. The first part is the coulomb friction. This occurs when there is a low motor angular velocity and it represents static friction. This friction model is active when the magnitude of the motor velocity is less than a certain value. The model of coulomb friction is simply a line that passes through the origin. To tune this friction we adjusted the slope of this line. The second part is the viscous friction. This occurs when there is a relatively high motor angular velocity and it represents all of the dynamic friction values. This friction model is active when the magnitude of the motor velocity is greater than a certain value. This model of viscous friction itself is divided into two parts. One part for if the angular velocity is positive and one for if it is negative. The model of viscous friction is also linear. This time we tune both the slope and the y-intercept of the equation to find the best possible model.
    
    Once calculated, each friction value is stored in a variable and will be used later in the control law to offset the friction force of the motors. This makes the control law more effective since by compensating for friction, this removes a disturbance that could make the motor controller ineffective.
    */
    
	// motor 1 friction compensation
	if (w1filt > vmin1){ //positive viscous friction
    	u_fric1 = visc_pos1*w1filt +coul_pos1;
	} else if(w1filt < -vmin1){ //negative viscous friction
    	u_fric1 = visc_neg1*w1filt + coul_neg1;
	}else{ //coulomb friction
    	u_fric1 = slope_between_mins1*w1filt;
	}
	// motor 2 friction compensation
	if (w2filt > vmin2){ //positive viscous friction
    	u_fric2 = visc_pos2*w2filt +coul_pos2;
	} else if(w2filt < -vmin2){ //negative viscous friction
    	u_fric2 = visc_neg2 * w2filt + coul_neg2;
	}else{ //coulomb friction
    	u_fric2 = slope_between_mins2 * w2filt;
	}
	// motor 3 friction compensation
	if (w3filt > vmin3){ //positive viscous friction
    	u_fric3 = visc_pos3 * w3filt + coul_pos3;
	} else if(w3filt < -vmin3){ //negative viscous friction
    	u_fric3 = visc_neg3 * w3filt + coul_neg3;
	}else{ //coulomb friction
    	u_fric3 = slope_between_mins3 * w3filt;
	}
    
    /*
    For the controller in this final project, we decided to copy part three of lab 4 and implement a task space impedance controller. It was important for our controller to be in the task space because it allowed us to visualize our trajectory much easier than if we did it with joint angles. A task space impedance controller is a controller that makes the end effector act as if it is in a mass spring damper system where Kp controls the spring and Kd controls the damper. There is a separate Kp and Kd for each axis (x,y,z) meaning that there are separate "mass spring damper" systems controlling the behavior of the end effector on each axis. This is very important because some of the obstacles we need to overcome were simplified by using this controller. As stated before, most of the time we had our controller operate with strong gains in all directions which allowed our robot to track trajectories accurately. This was important for moving around obstacles. Other times, it was beneficial for us to weaken the gains in some of the directions on our controller, making the robot arm have a weak spring force (lower Kp) in some directions. This allowed us to not need to create the perfect trajectory for some operations such as putting a peg in the hole and navigating through the maze. By making our robot have weak x-y impedance when entering the hole, it allowed our end effector to slip into the hole since there was no strong controller action forcing the end effector to a certain x-y position. In the maze, we weakened the direction perpendicular to the direction of movement. This helped our robot to not get stuck if it ran into the wall. It also allowed us to set the target location a little bit around the corner. With the low impedance, the robot would not struggle trying to move into the wall, but once there was no more wall, the end effector would spring toward the point we chose. While that direction had low impedance, the direction of movement was kept on high impedance to make sure that the end effector would move through the maze correctly. Finally, with the egg we tried something different. Since we are attempting to push on the egg with a force, and therefore define a force control effort in the z direction, we did not need or want to specify a z location that we desired the end effector to be at. For this reason we set Kp to 0, removing the "spring" from our impedance controller, which allowed the end effector to now move freely in the z direction. We left the Kd in place, however, so that there would still be a damping value present in the z direction impedance. We did this because we believe that this damping will cause the end effector to impact the egg softer, minimizing the risk of breaking it.
    
    Another thing that this controller has the ability to do is it can use a rotation matrix (as calculated above in the example code) to rotate the coordinate axis so we could make a direction or plane weak that was not in a direction of the original world axis. This control law works by first multiplying the end effector task space position and velocity errors by the rotation matrix. Only then is the PD controller applied to these rotated errors. After that the inverse of the rotation matrix is applied to bring the force control efforts back into the world frame. This is needed so that the inverse end effector velocity jacobian can work correctly to convert these forces to torques. However, because the errors were rotated before the control effort calculation, the axis of this control effort is different. In our trajectory, we only ever rotated about the z axis. This was useful for the action described above: making the direction perpendicular to the direction of travel weak as we went through the maze.
    */

    //we calculate the task space position and velocity errors outside the torque calculation to save on calculations and simplify the code.
	ex=xdes-x;
	ey=ydes-y;
	ez=zdes-z;
	exdot=xdes_dot-x_dot_filt;
	eydot=ydes_dot-y_dot_filt;
	ezdot=zdes_dot-z_dot_filt;

   	 /*
    These torque calculations are very long so instead of writing it by hand and making mistakes, we created a MATLAB script that would write the commands for us. The script works by using the MATLAB symbolic toolbox. We generate a bunch of arrays with the same names as the variables in our code. We then do the control law calculation as seen in the lab manual. The code spits out the correct C code which we are then able to copy and paste into code composer to easily implement a task space impedance controller. The matlab code is as follows:
    
    clear all;
    syms ex ey ez exdot eydot ezdot
    syms JT_11 JT_12 JT_13 JT_21 JT_22 JT_23 JT_31 JT_32 JT_33
    syms R11 R12 R13 R21 R22 R23 R31 R32 R33
    syms RT11 RT12 RT13 RT21 RT22 RT23 RT31 RT32 RT33
    syms Kpx Kpy Kpz Kdx Kdy Kdz
    JT=[JT_11 JT_12 JT_13; JT_21 JT_22 JT_23; JT_31 JT_32 JT_33];
    R=[R11 R12 R13; R21 R22 R23; R31 R32 R33];
    RT = [RT11 RT12 RT13; RT21 RT22 RT23; RT31 RT32 RT33];
    Kp=diag([Kpx Kpy Kpz]);
    Kd=diag([Kdx Kdy Kdz]);
    e=[ex;ey;ez];
    edot = [exdot;eydot;ezdot];
    Tau=simplify(JT*R*(Kp*RT*e+Kd*RT*edot))
    */
    
	*tau1 = u_fric1 + JT_13*Fz_cmd/Kt + (JT_11*R11 + JT_12*R21 + JT_13*R31)*(Kdx*RT11*exdot + Kdx*RT12*eydot + Kdx*RT13*ezdot + Kpx*RT11*ex + Kpx*RT12*ey + Kpx*RT13*ez) + (JT_11*R12 + JT_12*R22 + JT_13*R32)*(Kdy*RT21*exdot + Kdy*RT22*eydot + Kdy*RT23*ezdot + Kpy*RT21*ex + Kpy*RT22*ey + Kpy*RT23*ez) + (JT_11*R13 + JT_12*R23 + JT_13*R33)*(Kdz*RT31*exdot + Kdz*RT32*eydot + Kdz*RT33*ezdot + Kpz*RT31*ex + Kpz*RT32*ey + Kpz*RT33*ez);
	*tau2 = u_fric2 + JT_23*Fz_cmd/Kt + (JT_21*R11 + JT_22*R21 + JT_23*R31)*(Kdx*RT11*exdot + Kdx*RT12*eydot + Kdx*RT13*ezdot + Kpx*RT11*ex + Kpx*RT12*ey + Kpx*RT13*ez) + (JT_21*R12 + JT_22*R22 + JT_23*R32)*(Kdy*RT21*exdot + Kdy*RT22*eydot + Kdy*RT23*ezdot + Kpy*RT21*ex + Kpy*RT22*ey + Kpy*RT23*ez) + (JT_21*R13 + JT_22*R23 + JT_23*R33)*(Kdz*RT31*exdot + Kdz*RT32*eydot + Kdz*RT33*ezdot + Kpz*RT31*ex + Kpz*RT32*ey + Kpz*RT33*ez);
	*tau3 = u_fric3 + JT_33*Fz_cmd/Kt + (JT_31*R11 + JT_32*R21 + JT_33*R31)*(Kdx*RT11*exdot + Kdx*RT12*eydot + Kdx*RT13*ezdot + Kpx*RT11*ex + Kpx*RT12*ey + Kpx*RT13*ez) + (JT_31*R12 + JT_32*R22 + JT_33*R32)*(Kdy*RT21*exdot + Kdy*RT22*eydot + Kdy*RT23*ezdot + Kpy*RT21*ex + Kpy*RT22*ey + Kpy*RT23*ez) + (JT_31*R13 + JT_32*R23 + JT_33*R33)*(Kdz*RT31*exdot + Kdz*RT32*eydot + Kdz*RT33*ezdot + Kpz*RT31*ex + Kpz*RT32*ey + Kpz*RT33*ez);

	/*
    In order to make our controller more safe, we use the following code to limit the torque commands that we request from the robot. In certain conditions, such as the motor running into an immovable obstacle, incorrect trajectory calculation, and other code errors, the torque that we request from the robot controller may be too high for the physical controller to handle and we may damage it. By using the following function, we assure that we will never request more than 5 Nm from the motor controller. If we do detect that we are asking for too much torque, we set the torque back to +/-5. This ensures that we can still get the desired movement we want if the robot is not constrained, but we will do it without damaging the physical robot controller.
    */
	if (*tau1 > 5) {//if the torque requested from motor 1 is too high, set it to 5 Nm
    	*tau1 = 5;
	}
	if (*tau1 < -5) {//if the torque requested from motor 1 is too high in the opposite direction, set it to -5 Nm
    	*tau1 = -5;
	}

	if (*tau2 > 5) {//if the torque requested from motor 2 is too high, set it to 5 Nm
    	*tau2 = 5;
	}
	if (*tau2 < -5) {//if the torque requested from motor 2 is too high in the opposite direction, set it to -5 Nm
    	*tau2 = -5;
	}

	if (*tau3 > 5) {//if the torque requested from motor 3 is too high, set it to 5 Nm
    	*tau3 = 5;
	}
	if (*tau3 < -5) {//if the torque requested from motor 3 is too high in the opposite direction, set it to -5 Nm
    	*tau3 = -5;
	}

	//The following code allows us to plot 4 variables in the simulink data viewer by assigning whichever variables we want to watch to these 4 simulink plot variables. This is useful for trying to tune a PD controller since we can watch the current and desired robot position and adjust our PD gains based on the controller response that we see.
	Simulink_PlotVar1 = x;
	Simulink_PlotVar2 = y;
	Simulink_PlotVar3 = z;
	Simulink_PlotVar4 = xdes;

    /*
    The following code stores all of the previous states of our lab function for use in the next time it is called. Each variable has a different use as described below for the motor 1 variables and the x direction variables:
    
   	 t1old: stores the old theta1motor value. This is used to calculate the raw velocity value for motor 1 which is then used for calculating the filtered velocity value, which is used for calculating the friction compensation.
   	 x_old: stores the old end effector x position value. This is used to calculate the raw task space velocity, which is then used for calculating the filtered task space velocity, which is used for our task space controller.
   	 w1old: stores the old value of the filtered angular velocity. This is used for our velocity filter which averages the last 2 filtered velocities with the current raw velocity.
   	 x_dot_old: stores the old value of the filtered task space velocity. Similarly to the angular velocity this is used in our velocity filter algorithm which averages the last 2 filtered task space velocities with the current raw task space velocity to find the current filtered task space velocity.
   	 w1older: stores the old value of the filtered angular velocity. Again, this is used for our velocity filter.
   	 x_dot_old: stores the old old value of the filtered task space velocity. This is used for the velocity filter.

    All of these values are very important as they are essential to calculating velocity of our robot. For the w1old, w1older and x_dot_old, x_dot_older pairs the order that we assign the values is important. We need to assign the older velocity value before we assign the old velocity value. This is because once we assign the old velocity value to the current velocity value, the data it once held is lost. If we do not assign the older velocity value before doing this, we will not be able to store the correct data (we will have stored the current velocity value twice) and our velocity filter will not work correctly.
    */
    
	//storing old motor angles
	t1old = theta1motor;
	t2old = theta2motor;
	t3old = theta3motor;

	//storing old old filtered angular velocity values before the old filtered angular velocity value is overwritten with the current one
	w1older=w1old;
	w2older=w2old;
	w3older=w3old;
    
	//once the old filtered angular velocity has been stored we store the current filtered angular velocity in its place
	w1old = w1filt;
	w2old = w2filt;
	w3old = w3filt;

    //storing old end effector task space position
	x_old = x;
	y_old = y;
	z_old = z;

    //storing old old filtered task space velocity values before the old filtered task space velocity value is overwritten with the current one
	x_dot_older = x_dot_old;
	y_dot_older = y_dot_old;
	z_dot_older = z_dot_old;

    //once the old filtered task space velocity has been stored we store the current filtered task space velocity in its place
	x_dot_old = x_dot_filt;
	y_dot_old = y_dot_filt;
	z_dot_old = z_dot_filt;

    //This line of code increments the variable which stores the number of times the lab function has been called since the controller was turned on. Because the lab function is called every 1 ms without fail, this variable doubles as the amount of milliseconds since the robot code started running. As seen earlier, by storing start and end time variables, this mycount variable can be used as the basis for all of the different timing techniques that we use in our code.
	mycount++;
}

//For this project, we used the printing function to print out the end effector forward kinematic task space position. This was essential for generating our trajectory. When we wanted to add an additional point, we simply moved the end effector to that position, read out the forward kinematic position and set the point to that.
void printing(void){
	//printing the values to teraterm for inspection
	serial_printf(&SerialA, "end_x %3f end_y %3f end_z %3f \n\r" , x,y,z);
}