Code | robotics

#include <tistdtypes.h>
#include <coecsl.h>
#include "user_includes.h"
#include "math.h"

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

int myvar = 0;

// Your global variables.
long mycount = 0;
long tcount = 0;
double tcounts = 0; //to get tcount in seconds for cubic trajectory calculations

float tempttotal = 0;
float temptcount = 0;
float tdiff = 0;

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

#pragma DATA_SECTION(theta1array, ".my_arrs")
float theta1array[100];
#pragma DATA_SECTION(theta2array, ".my_arrs")
float theta2array[100];

long arrayindex = 0;

// initialize variables for calculation
float theta1dh=0;
float theta2dh=0;
float theta3dh=0;
double x=0;
double dx = 0;
double x_old = 0;
double dx_old1 = 0;
double dx_old2 = 0;
double y=0;
double dy = 0;
double y_old = 0;
double dy_old1 = 0;
double dy_old2 = 0;
double z=0;
double dz = 0;
double z_old = 0;
double dz_old1 = 0;
double dz_old2 = 0;
float cPhi;
float theta1c=0;
float theta2c=0;
float theta3c=0;
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;

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

// Task Space Control
float Kp[] = {0.8,0.8,1.3};
float Kd[] = {0.025,0.025,0.025};

// Force for task space
double f[3] = {};

// desired coordinate variables
double xd = 10;
double dxd = 0;
double xd_old = 0;
double dxd_old1 = 0;
double dxd_old2 = 0;
double yd = 0;
double dyd = 0;
double yd_old = 0;
double dyd_old1 = 0;
double dyd_old2 = 0;
double zd = 20;
double dzd = 0;
double zd_old = 0;
double dzd_old1 = 0;
double dzd_old2 = 0;

// Initial and final variables for trajectory
float xa = 10;
float ya = 0;
float za = 10;
float xc = 0;
float yc = 0;
float zc = 0;
float xb = 0;
float yb = 0;
float zb = 0;
float xe = 0;
float ye = 0;
float ze = 0;
float xf = 0;
float yf = 0;
float zf = 0;
float xg = 0;
float yg = 0;
float zg = 0;
float xh = 0;
float yh = 0;
float zh = 0;
float xi = 0;
float yi = 0;
float zi = 0;
float xj = 0;
float yj = 0;
float zj = 0;
float xk = 0;
float yk = 0;
float zk = 0;
float xl = 0;
float yl = 0;
float zl = 0;
float xn = 0;
float yn = 0;
float zn = 0;
float xo = 0;
float yo = 0;
float zo = 0;

float speedmax = 20;
float speed = 9;
float speed2 = 5;

float tstart = 1.0;
float ttotal1 = 0;
float ttotal2 = 0;
float ttotal3 = 0;
float ttotal4 = 0;
float ttotal5 = 0;
float ttotal6 = 0;
float ttotal7 = 0;
float ttotal8 = 0;
float ttotal9 = 0;
float ttotal10 = 0;
float ttotal11 = 0;
float ttotal12 = 0;
float ttotal13 = 0;
float twait1s = 1;
float tempcount = 0;
float t_elapsed = 0;
float t_hole = 1.0;

float deltax = 0;
float deltay = 0;
float deltaz = 0;

//friction compensation parameters
float u_fric1 = 0;
float range1 = 0.1;
float Viscous_positive1 = 0.1958;
float Viscous_negative1 = 0.23202;
float Coulomb_positive1 = 0.37904;
float Coulomb_negative1 = -0.316;
float min_slope1 = 3.6;
float u_fric2 = 0;
float range2 = 0.05;
float Viscous_positive2 = 0.2500;
float Viscous_negative2 = 0.2919;
float Coulomb_positive2 = 0.4059;
float Coulomb_negative2 = -0.4631;
float min_slope2 = 3.6;
float u_fric3 = 0;
float range3 = 0.05;
float Viscous_positive3 = 0.1922;
float Viscous_negative3 = 0.2132;
float Coulomb_positive3 = 0.5339;
float Coulomb_negative3 = -0.5190;
float min_slope3 = 3.6;

// Variables for calculating R and Jacobian
float cosq1 = 0;
float sinq1 = 0;
float cosq2 = 0;
float sinq2 = 0;
float cosq3 = 0;
float sinq3 = 0;
double JT_11 = 0;
double JT_12 = 0;
double JT_13 = 0;
double JT_21 = 0;
double JT_22 = 0;
double JT_23 = 0;
double JT_31 = 0;
double JT_32 = 0;
double JT_33 = 0;
float cosz = 0;
float sinz = 0;
float cosx = 0;
float sinx = 0;
float cosy = 0;
float siny = 0;
double thetaz = 0;
double thetax = 0;
double thetay = 0;
double R11 = 0;
double R12 = 0;
double R13 = 0;
double R21 = 0;
double R22 = 0;
double R23 = 0;
double R31 = 0;
double R32 = 0;
double R33 = 0;
double RT11 = 0;
double RT12 = 0;
double RT13 = 0;
double RT21 = 0;
double RT22 = 0;
double RT23 = 0;
double RT31 = 0;
double RT32 = 0;
double RT33 = 0;
double JTR_11 = 0;
double JTR_12 = 0;
double JTR_13 = 0;
double JTR_21 = 0;
double JTR_22 = 0;
double JTR_23 = 0;
double JTR_31 = 0;
double JTR_32 = 0;
double JTR_33 = 0;

// variables for calculating R.xerror
float Rxerr = 0;
float Rxerrdot = 0;
float Ryerr = 0;
float Ryerrdot = 0;
float Rzerr = 0;
float Rzerrdot = 0;

float temp;

// This function is called every 1 ms
void lab(float theta1motor,float theta2motor,float theta3motor,float *tau1,float *tau2,float *tau3, int error) {

   // Swapping initial and final values of the trajectory at the end of the trajectory
   tcounts = tcount/1000.0;
//   if(tcounts>=ttotal){
//       tcount = 0;
//       tcounts=0;
//       temp = xa;
//       xa=xb;
//       xb = temp;
//       temp = ya;
//       ya=yb;
//       yb = temp;
//       temp = za;
//       za=zb;
//       zb= temp;
//   }

   //Coordinates of Objects
   xa = 10;
   xb = 1.23;
   xc = 1.23;
   xn = 11.94;
   xe = 15.24;
   xf = 15.43;
   xg = 16.02;
   xh = 12.68;
   xi = 14.91;
   xj = 14.35;
   xk = 14.35;
   xl = 10;
   ya = 0;
   yb = 13.79;
   yc = 13.79;
   yn = 4.42;
   ye = 4.42;
   yf = 3.69;
   yg = 2.80;
   yh = 1.87;
   yi = -1.16;
   yj = -5.27;
   yk = -5.27;
   yl = 0;
   za = 20;
   zb = 8.80;
   zc = 5.19;
   zn = 8.21;
   ze = zn;
   zf = zn;
   zg = zn;
   zh = zn;
   zi = 8.18;
   zj = 16;
   zk = 13.30;
   zl = 20;
   xo = 15.81;
   yo = 1.96;
   zo = zn;

   deltax = xb - xa;
   deltay = yb - ya;
   deltaz = zb - za;
   ttotal1 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speedmax;
   deltax = xc - xb;
   deltay = yc - yb;
   deltaz = zc - zb;
   ttotal2 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speed;
   deltax = xb - xc;
   deltay = yb - yc;
   deltaz = zb - zc;
   ttotal3 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speed;
   deltax = xn - xb;
   deltay = yn - yb;
   deltaz = zn - zb;
   ttotal4 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speedmax;
   deltax = xe - xn;
   deltay = ye - yn;
   deltaz = ze - zn;
   ttotal5 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speedmax;
   deltax = xf - xe;
   deltay = yf - ye;
   deltaz = zf - ze;
   ttotal6 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speed;
   deltax = xg - xf;
   deltay = yg - yf;
   deltaz = zg - zf;
   ttotal7 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speed;
   deltax = xo - xg;
   deltay = yo - yg;
   deltaz = zo - zg;
   ttotal8 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speed;
   deltax = xi - xh;
   deltay = yi - yh;
   deltaz = zi - zh;
   ttotal9 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speed;
   deltax = xj - xi;
   deltay = yj - yi;
   deltaz = zj - zi;
   ttotal10 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speedmax;
   deltax = xk - xj;
   deltay = yk - yj;
   deltaz = zk - zj;
   ttotal11 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speed2;
   deltax = xl - xk;
   deltay = yl - yk;
   deltaz = zl - zk;
   ttotal12 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speedmax;
   deltax = xh - xo;
   deltay = yh - yo;
   deltaz = zh - zo;
   ttotal13 = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/speed;

   if((tcounts) < tstart) {
       xd = 10;
       yd = 0;
       zd = 20;
   }

   if((tcounts)>=tstart && (tcounts) < (tstart + ttotal1)) {
       //Path to hole
//       tempcount = tcounts - tstart;
//       deltax = xb - xa;
//       deltay = yb - ya;
//       deltaz = zb - za;
//       xd = deltax*(tempcount/ttotal1) + xa;
//       yd = deltay*(tempcount/ttotal1) + ya;
//       zd = deltaz*(tempcount/ttotal1) + za;
       pathCalculations(xa,ya,za,xb,yb,zb,tstart,ttotal1);
   }
   else if((tcounts) >= (tstart + ttotal1) && (tcounts) < (tstart + ttotal1 + ttotal2)) {
//       // peg in hole
//       tempcount = tcounts - (tstart + ttotal1);
//       deltax = xc - xb;
//       deltay = yc - yb;
//       deltaz = zc - zb;
       Kp[0] = 0.1;
       Kp[1] = 0.1;
       Kd[0] = 0.000;
       Kd[1] = 0.000;
//       xd = deltax*(tempcount/ttotal2) + xb;
//       yd = deltay*(tempcount/ttotal2) + yb;
//       zd = deltaz*(tempcount/ttotal2) + zb;
       pathCalculations(xb,yb,zb,xc,yc,zc,tstart+ttotal1,ttotal2);
   }
//
   else if((tcounts) >= (tstart + ttotal1 + ttotal2) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole)) {
//
   }
//
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole + ttotal3)) {
//       //peg out of hole
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2+ t_hole);
//       deltax = xb - xc;
//       deltay = yb - yc;
//       deltaz = zb - zc;
       Kp[0] = 0.1;
       Kp[1] = 0.1;
       Kd[0] = 0.000;
       Kd[1] = 0.000;
//       xd = deltax*(tempcount/ttotal3) + xc;
//       yd = deltay*(tempcount/ttotal3) + yc;
//       zd = deltaz*(tempcount/ttotal3) + zc;
       pathCalculations(xc,yc,zc,xb,yb,zb, tstart + ttotal1 + ttotal2 + t_hole, ttotal3);
   }
//
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole + ttotal3) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4)) {
//       //path to box
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal3);
//       deltax = xn - xb;
//       deltay = yn - yb;
//       deltaz = zn - zb;
       Kp[0] = 0.8;
       Kp[1] = 0.8;
       Kd[0] = 0.025;
       Kd[1] = 0.025;
//       xd = deltax*(tempcount/ttotal4) + xb;
//       yd = deltay*(tempcount/ttotal4) + yb;
//       zd = deltaz*(tempcount/ttotal4) + zb;
       pathCalculations(xb,yb,zb,xn,yn,zn, tstart + ttotal1 + ttotal2 + t_hole + ttotal3, ttotal4);
   }
//
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5)) {
//       //path to zigzag
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal3+ttotal4);
//       deltax = xe - xn;
//       deltay = ye - yn;
//       deltaz = ze - zn;
       Kp[0] = 0.8;
       Kp[1] = 0.8;
       Kd[0] = 0.025;
       Kd[1] = 0.025;
//       xd = deltax*(tempcount/ttotal5) + xn;
//       yd = deltay*(tempcount/ttotal5) + yn;
//       zd = deltaz*(tempcount/ttotal5) + zn;
       pathCalculations(xn,yn,zn,xe,ye,ze,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4,ttotal5);
//
   }
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6)) {
//       //zigzag start
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal3+ttotal4+ttotal5);
//       deltax = xf - xe;
//       deltay = yf - ye;
//       deltaz = zf - ze;
       Kp[0] = 1.3;
       Kp[1] = 0.8;
       Kd[0] = 0.025;
       Kd[1] = 0.025;
//       xd = deltax*(tempcount/ttotal6) + xe;
//       yd = deltay*(tempcount/ttotal6) + ye;
//       zd = deltaz*(tempcount/ttotal6) + ze;
       pathCalculations(xe,ye,ze,xf,yf,zf,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5,ttotal6);
//
   }
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7)) {
//       //zigzag1
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3+ttotal4+ttotal5+ttotal6);
//       deltax = xg - xf;
//       deltay = yg - yf;
//       deltaz = zg - zf;
       Kp[0] = 1.3;
       Kp[1] = 0.3;
       Kd[0] = 0.025;
       Kd[1] = 0.005;
       thetaz = -53.13*PI/180;
//       xd = deltax*(tempcount/ttotal7) + xf;
//       yd = deltay*(tempcount/ttotal7) + yf;
//       zd = deltaz*(tempcount/ttotal7) + zf;
       pathCalculations(xf,yf,zf,xg,yg,zg,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6,ttotal7);
//
   }
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8)) {
//       //zigzag1 curve
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal3+ttotal4+ttotal5+ttotal6+ttotal7);
//       deltax = xo - xg;
//       deltay = yo - yg;
//       deltaz = zo - zg;
       Kp[0] = 1.3;
       Kp[1] = 0.3;
       Kd[0] = 0.025;
       Kd[1] = 0.005;
       thetaz = -53.13*PI/180;
//       xd = deltax*(tempcount/ttotal8) + xg;
//       yd = deltay*(tempcount/ttotal8) + yg;
//       zd = deltaz*(tempcount/ttotal8) + zg;
       pathCalculations(xg,yg,zg,xo,yo,zo,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7,ttotal8);
   }
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13)) {
       //curve1 to curve2
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal3+ttotal4+ttotal5+ttotal6+ttotal7+ttotal8);
//       deltax = xh - xo;
//       deltay = yh - yo;
//       deltaz = zh - zo;
       Kp[0] = 1.3;
       Kp[1] = 0.3;
       Kd[0] = 0.025;
       Kd[1] = 0.005;
       thetaz = -15*PI/180;
//       xd = deltax*(tempcount/ttotal13) + xo;
//       yd = deltay*(tempcount/ttotal13) + yo;
//       zd = deltaz*(tempcount/ttotal13) + zo;
       pathCalculations(xo,yo,zo,xh,yh,zh,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8,ttotal13);
   }
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9)) {
//       //zigzag out
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal1+ttotal3+ttotal4+ttotal5+ttotal6+ttotal7+ttotal8+ttotal13);
//       deltax = xi - xh;
//       deltay = yi - yh;
//       deltaz = zi - zh;
       Kp[0] = 0.8;
       Kp[1] = 0.3;
       Kd[0] = 0.025;
       Kd[1] = 0.005;
       thetaz = -53.13*PI/180;
//       xd = deltax*(tempcount/ttotal9) + xh;
//       yd = deltay*(tempcount/ttotal9) + yh;
//       zd = deltaz*(tempcount/ttotal9) + zh;
       pathCalculations(xh,yh,zh,xi,yi,zi,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13,ttotal9);
   }
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10)) {
//       //zigzag to above egg
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal3+ttotal4+ttotal5+ttotal6+ttotal7+ttotal8+ttotal13+ttotal9);
//       deltax = xj - xi;
//       deltay = yj - yi;
//       deltaz = zj - zi;
       Kp[0] = 0.8;
       Kp[1] = 0.3;
       Kd[0] = 0.025;
       Kd[1] = 0.005;
       thetaz = 0;
//       xd = deltax*(tempcount/ttotal10) + xi;
//       yd = deltay*(tempcount/ttotal10) + yi;
//       zd = deltaz*(tempcount/ttotal10) + zi;
       pathCalculations(xi,yi,zi,xj,yj,zj,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9,ttotal10);
   }
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole+ ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10 + ttotal11)) {
//       //touch egg
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal3+ttotal4+ttotal5+ttotal6+ttotal7+ttotal8+ttotal13+ttotal9+ttotal10);
//       deltax = xk - xj;
//       deltay = yk - yj;
//       deltaz = zk - zj;
       Kp[0] = 0.8;
       Kp[1] = 0.8;
       Kd[0] = 0.025;
       Kd[1] = 0.025;
       thetaz = 0;
//       xd = deltax*(tempcount/ttotal11) + xj;
//       yd = deltay*(tempcount/ttotal11) + yj;
//       zd = deltaz*(tempcount/ttotal11) + zj;
       pathCalculations(xj,yj,zj,xk,yk,zk,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10,ttotal11);
   }

   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10 + ttotal11) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10 + ttotal11 + t_hole)) {
       // wait
   }
   else if((tcounts) >= (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10 + ttotal11 + t_hole) && (tcounts) < (tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10 + ttotal11 + t_hole + ttotal12)) {
//       //back home
//       tempcount = tcounts - (tstart + ttotal1 + ttotal2 + t_hole+ttotal3+ttotal4+ttotal5+ttotal6+ttotal7+ttotal8+ttotal13+ttotal9+ttotal10+ttotal11);
//       deltax = xl - xk;
//       deltay = yl - yk;
//       deltaz = zl - zk;
       Kp[0] = 0.8;
       Kp[1] = 0.8;
       Kd[0] = 0.025;
       Kd[1] = 0.025;
       thetaz = 0;
//       xd = deltax*(tempcount/ttotal12) + xk;
//       yd = deltay*(tempcount/ttotal12) + yk;
//       zd = deltaz*(tempcount/ttotal12) + zk;
       pathCalculations(xk,yk,zk,xl,yl,zl,tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10 + ttotal11 + t_hole,ttotal12);
   }

//
   if(tcounts > tstart + ttotal1 + ttotal2 + t_hole + ttotal3 + ttotal4 + ttotal5 + ttotal6 + ttotal7 + ttotal8 + ttotal13 + ttotal9 + ttotal10 + ttotal11 + t_hole + ttotal12) {
       tcount = tstart*1000;
   }
//   goToPoint(xa,ya,za,xa,ya,za,1,0);
//   goToPoint(xa,ya,za,xb,yb,zb,0,0);
//   goToPoint(xb,yb,zb,xc,yc,zc,0,0);

   // Calculating Straight Line Trajectory
   // Calculating length along x y z directions
   // Total time = Total distance / speed
   // Calculating derivative
   dxd = (xd - xd_old) / 0.001;
   dyd = (yd - yd_old) / 0.001;
   dzd = (zd - zd_old) / 0.001;
   xd_old = xd;
   yd_old = yd;
   zd_old = zd;
   dxd = (dxd + dxd_old1 + dxd_old2)/3.0;
   dyd = (dyd + dyd_old1 + dyd_old2)/3.0;
   dxd = (dzd + dzd_old1 + dzd_old2)/3.0;
   dxd_old2 = dxd_old1;
   dxd_old1 = dxd;
   dyd_old2 = dyd_old1;
   dyd_old1 = dyd;
   dzd_old2 = dzd_old1;
   dzd_old1 = dzd;

   // Calculate end effector coordinates from motor angles based on forward kinematics
   x=10*cos(theta1motor)*(cos(theta3motor)+sin(theta2motor));
   y=10*sin(theta1motor)*(cos(theta3motor)+sin(theta2motor));
   z=10*(1+cos(theta2motor)-sin(theta3motor));

   // Calculating derivatives
   dx = (x - x_old) / 0.001;
   dy = (y - y_old) / 0.001;
   dz = (z - z_old) / 0.001;
   x_old = x;
   y_old = y;
   z_old = z;
   dx = (dx + dx_old1 + dx_old2)/3.0;
   dy = (dy + dy_old1 + dy_old2)/3.0;
   dx = (dz + dz_old1 + dz_old2)/3.0;
   dx_old2 = dx_old1;
   dx_old1 = dx;
   dy_old2 = dy_old1;
   dy_old1 = dy;
   dz_old2 = dz_old1;
   dz_old1 = dz;

   // Jacobian Transpose
   cosq1 = cos(theta1motor);
   sinq1 = sin(theta1motor);
   cosq2 = cos(theta2motor);
   sinq2 = sin(theta2motor);
   cosq3 = cos(theta3motor);
   sinq3 = sin(theta3motor);
   JT_11 = -10*sinq1*(cosq3 + sinq2);
   JT_12 = 10*cosq1*(cosq3 + sinq2);
   JT_13 = 0;
   JT_21 = 10*cosq1*(cosq2 - sinq3);
   JT_22 = 10*sinq1*(cosq2 - sinq3);
   JT_23 = -10*(cosq3 + sinq2);
   JT_31 = -10*cosq1*sinq3;
   JT_32 = -10*sinq1*sinq3;
   JT_33 = -10*cosq3;

   // Rotation zxy and its Transpose
   cosz = cos(thetaz);
   sinz = sin(thetaz);
   cosx = cos(thetax);
   sinx = sin(thetax);
   cosy = cos(thetay);
   siny = sin(thetay);
   RT11 = R11 = cosz*cosy-sinz*sinx*siny;
   RT21 = R12 = -sinz*cosx;
   RT31 = R13 = cosz*siny+sinz*sinx*cosy;
   RT12 = R21 = sinz*cosy+cosz*sinx*siny;
   RT22 = R22 = cosz*cosx;
   RT32 = R23 = sinz*siny-cosz*sinx*cosy;
   RT13 = R31 = -cosx*siny;
   RT23 = R32 = sinx;
   RT33 = R33 = cosx*cosy;

   // JT * R
   JTR_11 = JT_11 * R11 + JT_12 * R21 + JT_13 * R31;
   JTR_12 = JT_11 * R12 + JT_12 * R22 + JT_13 * R32;
   JTR_13 = JT_11 * R13 + JT_12 * R23 + JT_13 * R33;
   JTR_21 = JT_21 * R11 + JT_22 * R21 + JT_23 * R31;
   JTR_22 = JT_21 * R12 + JT_22 * R22 + JT_23 * R32;
   JTR_23 = JT_21 * R13 + JT_22 * R23 + JT_23 * R33;
   JTR_31 = JT_31 * R11 + JT_32 * R21 + JT_33 * R31;
   JTR_32 = JT_31 * R12 + JT_32 * R22 + JT_33 * R32;
   JTR_33 = JT_31 * R13 + JT_32 * R23 + JT_33 * R33;

   // Friction Compensation
   // joint 1
   u_fric1= 0;
   if (Omega1 > range1) {
       u_fric1 = Viscous_positive1*Omega1 + Coulomb_positive1 ;
   } else if (Omega1 < -range1) {
       u_fric1 = Viscous_negative1*Omega1 + Coulomb_negative1;
   } else {
       u_fric1 = min_slope1*Omega1;
   }

   // joint 2
   u_fric2= 0;
   if (Omega2 > range2) {
       u_fric2 = Viscous_positive2*Omega2 + Coulomb_positive2 ;
   } else if (Omega2 < -range2) {
       u_fric2 = Viscous_negative2*Omega2 + Coulomb_negative2;
   } else {
       u_fric2 = min_slope2*Omega2;
   }

   // joint 3
   u_fric3= 0;
   if (Omega3 > range3) {
       u_fric3 = Viscous_positive3*Omega3 + Coulomb_positive3 ;
   } else if (Omega3 < -range3) {
       u_fric3 = Viscous_negative3*Omega3 + Coulomb_negative3;
   } else {
       u_fric3 = min_slope3*Omega3;
   }

   // Rotating error
   Rxerr = RT11*(xd-x) + RT12*(yd-y) + RT13*(zd-z);
   Rxerrdot = RT11*(dxd-dx) + RT12*(dyd-dy) + RT13*(dzd-dz);

   Ryerr = RT21*(xd-x) + RT22*(yd-y) + RT23*(zd-z);
   Ryerrdot = RT21*(dxd-dx) + RT22*(dyd-dy) + RT23*(dzd-dz);

   Rzerr = RT31*(xd-x) + RT32*(yd-y) + RT33*(zd-z);
   Rzerrdot = RT31*(dxd-dx) + RT32*(dyd-dy) + RT33*(dzd-dz);

   // Calculating Force
   // Calculating Force wrt Robot Arm Coordinate Frame
//   f[0] = Kp[0]*(xd-x) + Kd[0]*(dxd - dx);
//   f[1] = Kp[1]*(yd-y) + Kd[1]*(dyd - dy);
//   f[2] = Kp[2]*(zd-z) + Kd[2]*(dzd - dz);

   // Calculating Force wrt arbitrary set coordinate frame
   f[0] = Kp[0]*Rxerr + Kd[0]*Rxerrdot;
   f[1] = Kp[1]*Ryerr + Kd[1]*Ryerrdot;
   f[2] = Kp[2]*Rzerr + Kd[2]*Rzerrdot;

   // Task Space Control Law
   // Torque Calculation wrt Robot Arm Coordinate Frame
//   *tau1 = JT_11 * f[0] + JT_12 * f[1] + JT_13 * f[2] + 0.6 * u_fric1;
//   *tau2 = JT_21 * f[0] + JT_22 * f[1] + JT_23 * f[2] + 0.6 * u_fric2;
//   *tau3 = JT_31 * f[0] + JT_32 * f[1] + JT_33 * f[2] + 0.6 * u_fric3;

//   Torque Calculation wrt arbitrary set coordinate frame
   *tau1 = JTR_11 * f[0] + JTR_12 * f[1] + JTR_13 * f[2] + 0.6 * u_fric1;
   *tau2 = JTR_21 * f[0] + JTR_22 * f[1] + JTR_23 * f[2] + 0.6 * u_fric2;
   *tau3 = JTR_31 * f[0] + JTR_32 * f[1] + JTR_33 * f[2] + 0.6 * u_fric3;

   // Zero Torque for Debugging
//   *tau1 = 0;
//   *tau2 = 0;
//   *tau3 = 0;

   //Motor torque limitation(Max: 5 Min: -5)
   if(*tau1>5) {
       *tau1=5;
   }
   if(*tau1<-5) {
       *tau1=-5;
   }
   if(*tau2>5) {
       *tau2=5;
   }
   if(*tau2<-5) {
       *tau2=-5;
   }
   if(*tau3>5) {
       *tau3 = 5;
   }
   if(*tau3<-5) {
       *tau3 = -5;
   }

   // Convert motor angles to DH angles
   theta1dh=theta1motor;
   theta2dh=theta2motor-PI/2;
   theta3dh=-theta2motor+theta3motor+PI/2;

   // Calculate joint angles from end effector coordinates based on inverse kinematics
   theta1c=atan2(y,x);
   cPhi=-(x*x+y*y+(z-10)*(z-10)-200)/200;
   theta3c=PI-atan(sqrt(1-cPhi*cPhi)/cPhi);
   theta2c=-atan((z-10)/(sqrt(x*x+y*y)))-atan(10*sin(theta3c)/(10+10*cos(theta3c)));

   // save past states
   if ((mycount%50)==0) {
       theta1array[arrayindex] = theta1motor;
       theta2array[arrayindex] = theta2motor;

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

   }

   //Function used to toggle LED lights on Emergency Button
   if ((mycount%500)==0) {
       if (whattoprint > 0.5) {
           serial_printf(&SerialA, "I love robotics\n\r");
       } else {
           SWI_post(&SWI_printf); //Using a SWI to fix SPI issue from sending too many floats.
       }
       GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1; // Blink LED on Control Card
       GpioDataRegs.GPBSET.bit.GPIO60 = 1; // Blink LED on Emergency Stop Box
   }

   // send data to Simulink for plotting
   Simulink_PlotVar1 = xd;
   Simulink_PlotVar2 = x;
   Simulink_PlotVar3 = yd;
   Simulink_PlotVar4 = y;

   //   for plotting motor angles
   //       Simulink_PlotVar1 = theta1motor;
   //       Simulink_PlotVar2 = theta2motor;
   //       Simulink_PlotVar3 = theta3motor;
   //       Simulink_PlotVar4 = theta2d;

   mycount++;
   tcount++;

}

void printing(void){
   // print DH angles in degrees, end-effector coordinates from forward kinematics, and joint angles from inverse kinematics in degrees
   serial_printf(&SerialA, "%.2f, %.2f, %.2f,    %.2f, %.2f, %.2f,      %.2f, %.2f, %.2f, %d   \n\r",theta1dh*180/PI,theta2dh*180/PI,theta3dh*180/PI,x,y,z,xd,yd,zd, myvar);
}

void pathCalculations(float x_init, float y_init, float z_init, float x_end, float y_end, float z_end, float t_elapsed, float t_path) {
   // Straight Line Trajectory Calculations for Desired Coordinates
   float tempcounts = tcounts - (t_elapsed);
   deltax = x_end - x_init;
   deltay = y_end - y_init;
   deltaz = z_end - z_init;
   xd = deltax*(tempcounts/t_path) + x_init;
   yd = deltay*(tempcounts/t_path) + y_init;
   zd = deltaz*(tempcounts/t_path) + z_init;
}

//Experimental Straight Path Planning Function (DOES NOT WORK YET) -Nagarjun
//void goToPoint(float x_init, float y_init, float z_init, float x_end, float y_end, float z_end, float time, float v) {
//   if(tcounts>=t_elapsed) {
//       tempttotal = 0;
//       temptcount = 0;
//       tdiff = 0;
//       if(v==0) {
//           v = speed;
//       }
//
//
//       if(time==0) {
//           deltax = x_end-x_init;
//           deltay = y_end-y_init;
//           deltaz = z_end-z_init;
//
//           tempttotal = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)/v;
//           temptcount = tcounts - t_elapsed;
//
//           if(tcounts<(t_elapsed+tempttotal)) {
//               xd = deltax*(temptcount/tempttotal) + x_init;
//               yd = deltay*(temptcount/tempttotal) + y_init;
//               zd = deltaz*(temptcount/tempttotal) + z_init;
//           }
//       }
//       else {
//           tempttotal = time;
//           if(tcounts<(t_elapsed+tempttotal)) {
//               xd = x_init;
//               yd = y_init;
//               zd = z_init;
//           }
//       }
//       tdiff = ((t_elapsed+tempttotal)*1000-tcount);
//       if((tdiff<=1) && (tdiff>0)) {
//           t_elapsed += tempttotal;
//       }
//   }
//}

C File: https://www.dropbox.com/s/quqwm5nopuomgkm/project.c?dl=0