[ME446] Final Project

#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.39;
float offset_Enc3_rad = 0.259;


// Your global varialbes.

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(theta2array, ".my_arrs")
float theta2array[100];

#pragma DATA_SECTION(endefarray, ".my_arrs")
float endefarray[3];

long arrayindex = 0;
int UARTprint = 0;

float printtheta1motor = 0;
float printtheta2motor = 0;
float printtheta3motor = 0;

/*
    global variables for previous lab (we don't use these in this lab)
*/
float theta1dh = 0;
float theta2dh = 0;
float theta3dh = 0;
float theta1m = 0;
float theta2m = 0;
float theta3m = 0;
float kp1 = 35;
float kd1 = 2;
float kp2 = 60;
float kd2 = 2.9;
float kp3 = 60;
float kd3 = 2;
float theta_desired = 0;
float theta_desired_dot = 0;
float theta_desired_dotdot = 0;
float x1 = 0.0;
float y1 = 0.0;
float z1 = 0.0;
float x2 = 0.0;
float y2 = 0.0;
float z2 = 0.0;
float tstart = 0;
float theta_desired1 = 0;
float theta_desired2 = 0;
float theta_desired3 = 0;
float theta_desired_dot1 = 0;
float theta_desired_dot2 = 0;
float theta_desired_dot3 = 0;
float theta_desired_dotdot1 = 0;
float theta_desired_dotdot2 = 0;
float theta_desired_dotdot3 = 0;
float ki1 = 250;
float ki2 = 600;
float ki3 = 800;
float ek1 = 0;
float ek2 = 0;
float ek3 = 0;
float ek1_1 = 0;
float ek2_1 = 0;
float ek3_1 = 0;
float Ik1_1 = 0;
float Ik2_1 = 0;
float Ik3_1 = 0;
float Ik1 = 0;
float Ik2 = 0;
float Ik3 = 0;
float threshold = 0.01;
float J1 = 0.0167;
float J2= 0.03;
float J3 = 0.0128;
float c1 = 1;
float c2 = -PI/2;
float c3 = -1;
float c4 = 1;
float c5 = PI/2;
float xa = 0.15;
float xb = 0.27;
float ya = 0.0;
float yb = 0.0;
float za = 0.42;
float zb = 0.5;
float FzCmd = 0.0;
float Kt = 6.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;


/*
    global variables for averaging filter
*/
float Theta1_old = 0;
float Omega1_raw = 0;
float Omega1_old1 = 0;
float Omega1_old2 = 0;
float Omega1 = 0;
float Theta2_old = 0;
float Omega2_raw = 0;
float Omega2_old1 = 0;
float Omega2_old2 = 0;
float Omega2 = 0;
float Theta3_old = 0;
float Omega3_raw = 0;
float Omega3_old1 = 0;
float Omega3_old2 = 0;
float Omega3 = 0;

float xdot = 0.0;
float ydot = 0.0;
float zdot = 0.0;
float x_prev = 0.0;
float y_prev = 0.0;
float z_prev = 0.0;

float xdot_raw = 0.0;
float xdot_old1 = 0.0;
float xdot_old2 = 0.0;
float ydot_raw = 0.0;
float ydot_old1 = 0.0;
float ydot_old2 = 0.0;
float zdot_raw = 0.0;
float zdot_old1 = 0.0;
float zdot_old2 = 0.0;


/*
    global variables for friction compensation 
*/
float ufric1 = 0;
float Pviscous1 = 0.2513;
float Nviscous1 = 0.2477;
float Pcoulomb1 = 0.18;
float Ncoulomb1 = -0.14;
float minimum_velocity1 = 0.1;
float slope1 = 2.0;
float ufric2 = 0;
float Pviscous2 = 0.2500;
float Nviscous2 = 0.2870;
float Pcoulomb2 = 0.5000;
float Ncoulomb2 = -0.6000;
float minimum_velocity2 = 0.05;
float slope2 = 2.5;
float ufric3 = 0;
float Pviscous3 = 0.1922;
float Nviscous3 = 0.2132;
float Pcoulomb3 = 0.2800;
float Ncoulomb3 = -0.6999;
float minimum_velocity3 = 0.05;
float slope3 = 3.0;
float ff1 = 0.55;
float ff2 = 0.55;
float ff3 = 0.9;

/*
    global variables for forward kinematics 
*/
float x = 0.0;
float y = 0.0;
float z = 0.0;


/*
    global variables for line trajectory
*/
float t = 0;

float x_desired = 0.0;
float y_desired = 0.0;
float z_desired = 0.0;

float distance = 0.0;
float t_total = 0.0;
float t_holdstart = 0.0;
float t_start = 0.0;
int index = 0;


/*
    global variables for impedance control 
*/
float J11 = 0.0;
float J12 = 0.0;
float J13 = 0.0;
float J21 = 0.0;
float J22 = 0.0;
float J23 = 0.0;
float J31 = 0.0;
float J32 = 0.0;
float J33 = 0.0;

float thetaNx = 0; 
float thetaNy = 0; 
float thetaNz = 0;
float R11 = 0.0;
float R12 = 0.0;
float R13 = 0.0;
float R21 = 0.0;
float R22 = 0.0;
float R23 = 0.0;
float R31 = 0.0;
float R32 = 0.0;
float R33 = 0.0;

float A11 = 0.0;
float A12 = 0.0;
float A13 = 0.0;
float A21 = 0.0;
float A22 = 0.0;
float A23 = 0.0;
float A31 = 0.0;
float A32 = 0.0;
float A33 = 0.0;

float xdot_desired = 0.0;
float ydot_desired = 0.0;
float zdot_desired = 0.0;

float Kpx = 175.0;
float Kpy = 200.0;
float Kpz = 180.0;
float Kdx = 20.0;
float Kdy = 20.0;
float Kdz = 12.0;


/*
    Creating the struct for the waypoints
*/
typedef struct{
    float xp; //x position
    float yp; //y position
    float zp; //z position
    float velocity; //desired velocity
    float t_hold;  //desired holding time at position
    float rotation; //desired rotation of robot frame for impedance control
    float Kpx;  //All Kp/Kd are the gains optimized for each point for the controller
    float Kpy;
    float Kpz;
    float Kdx;
    float Kdy;
    float Kdz;

}points ;


void mains_code(void);
void main(void)
{
    mains_code();
}

// This function is called every 1 ms.
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++;
        }

    }
    if ((mycount%50)==0) {

        theta2array[arrayindex] = theta2motor;

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

    }

    
    //waypoints are defined with following structure: {x_position, y_position, z_position, velocity, time for holding, rotation angle, Kp_x, Kp_y, Kp_z, Kd_x, Kd_y, Kd_z}
    points waypoint[] = {{0.15,     0,  0.42,  0.15,  0,      0,  700.0,  700.0, 600.0, 40.0, 40.0, 40.0},
                         {0.27,     0,   0.5,  0.15,  0,      0,  700.0,  700.0, 600.0, 45.0, 45.0, 40.0},
                         {0.02,  0.25,   0.5,  0.15,  0,      0,  700.0,  700.0, 700.0, 45.0, 45.0, 45.0},
                         {0.05, 0.355,  0.17,  0.08,  0,      0,  700.0,  700.0, 600.0, 45.0, 45.0, 45.0}, 
                         {0.05, 0.355,  0.15,  0.04,  0,      0,  300.0,  300.0, 350.0, 25.0, 25.0, 20.0}, 
                         {0.05, 0.355,  0.12,  0.08,  2,      0,  300.0,  300.0, 350.0, 25.0, 20.0, 20.0}, //hole
                         {0.04,  0.35,   0.3,  0.08,  0,      0,  300.0,  300.0, 300.0, 25.0, 25.0, 12.0},
                         {0.04,  0.34,   0.5, 0.075,  0,      0,  400.0,  400.0, 350.0, 28.0, 27.0, 12.0},
                         {0.27,  0.11,  0.39, 0.075,  0,      0,  700.0,  700.0, 600.0, 35.0, 35.0, 28.0},
                         {0.40,  0.10,  0.40,  0.17,  0,      0,  500.0,  500.0, 500.0, 25.0, 25.0, 25.0}, //zigzag start 
                         {0.40,  0.10,  0.20,  0.17,  0,  0.644,  500.0,  500.0, 400.0, 20.0, 25.0, 25.0},
                         {0.42,  0.04,  0.20,  0.17,  0,  0.644,  200.0,  500.0, 400.0, 20.0, 25.0, 25.0},
                         {0.41, 0.02,   0.20,  0.17,  0,  0.644,  200.0,  500.0, 400.0, 20.0, 25.0, 25.0},
                         {0.40,  0.03,  0.20,  0.17,  0,  6.018,  200.0,  500.0, 400.0, 20.0, 25.0, 25.0},
                         {0.39,  0.03,  0.20,  0.17,  0,  6.018,  200.0,  200.0, 400.0, 25.0, 25.0, 25.0},
                         {0.35,  0.05,  0.20,  0.17,  0,      0,  350.0,  200.0, 250.0, 18.0, 25.0, 12.0},
                         {0.33,  0.04,  0.20,  0.17,  0,  0.644,  200.0,  350.0, 250.0, 18.0, 25.0, 12.0},
                         {0.33,  0.03,  0.20,  0.17,  0,  0.644,  200.0,  350.0, 250.0, 18.0, 25.0, 12.0},
                         {0.32,  0.02,  0.20,  0.17,  0,  0.644,  200.0,  350.0, 250.0, 18.0, 25.0, 12.0},
                         {0.34,  0.00,  0.20,  0.17,  0,  0.644,  200.0,  350.0, 250.0, 18.0, 25.0, 12.0},
                         {0.365,-0.03,  0.20,  0.17,  0,  0.644,  200.0,  350.0, 250.0, 18.0, 25.0, 12.0},
                         {0.38, -0.05,  0.20,  0.17,  0,  0.644,  200.0,  350.0, 250.0, 18.0, 25.0, 12.0},
                         {0.39, -0.08,  0.20,  0.17,  0,  0.644,  200.0,  350.0, 250.0, 18.0, 25.0, 12.0},
                         {0.40, -0.08,  0.20,  0.17,  0,  0.644,  400.0,  350.0, 350.0, 25.0, 25.0, 12.0},
                         {0.40, -0.08,  0.4,   0.17,  0,  0.644,  400.0,  350.0, 350.0, 25.0, 25.0, 12.0}, //zigzag finish
                         {0.26,  0.17,  0.4,    0.1,  0,      0,  400.0,  350.0, 350.0, 25.0, 25.0, 12.0}, 
                         {0.26,  0.17,  0.35,  0.05,  0,      0,  400.0,  350.0, 350.0, 25.0, 25.0, 12.0},
                         {0.26,  0.17,  0.27,  0.05,  2,      0,  400.0,  350.0, 350.0, 25.0, 25.0, 12.0}, //egg
                         {0.26,  0.17,   0.4,  0.17,  0,      0,  400.0,  350.0, 350.0, 25.0, 25.0, 12.0},
                         {0.15,     0,  0.42,  0.15,  0,      0,  700.0,  700.0, 600.0, 30.0, 30.0, 30.0},
                         {0.15,     0,  0.42,     0,  0,      0,  700.0,  700.0, 600.0, 30.0, 30.0, 30.0}
    }; 
    

    t = (mycount)/1000.0; //calculate seconds 
    

    
    /*
        Straight line trajectory. 
            distance: distance between the starting point and the end point. 
            t_total = desired elapsed time (distance / desired velocity)
    */
    
    distance = sqrt((waypoint[index].xp - waypoint[index+1].xp)*(waypoint[index].xp - waypoint[index+1].xp) + (waypoint[index].yp - waypoint[index+1].yp)*(waypoint[index].yp - waypoint[index+1].yp) + (waypoint[index].zp - waypoint[index+1].zp)*(waypoint[index].zp - waypoint[index+1].zp));
    t_total = distance / waypoint[index].velocity;
    /*
       This is where we actually designed how the robot goes to each of the points/through the straight-line trajectories. Since we run through each point with an index which we add one to everytime the code is run. And then we only want this code to run for our 31 points so we set it to only run when this index is under 30. Then from here we have the code dependent on time which we keep track of with t_start and t_total as well as implement our hold of t_hold if there exists one for our different points as defined in the waypoint structure. We did this holding by running the code for the amount of time that is the total run time of each trajectory with the addition of the hold. Then from there once it runs the trajectory for the total time it then goes to hold that point for the rest of the time duration if there is the additional hold time.
    */ 
    if(index < 30){ 
        if ((t-(t_start)) <= (waypoint[index+1].t_hold + t_total)){
            if (t - t_holdstart <= t_total){ //move to the desired point
                x_desired = (waypoint[index+1].xp -waypoint[index].xp)*((t-t_start)/ t_total) + waypoint[index].xp;
                y_desired = (waypoint[index+1].yp -waypoint[index].yp)*((t-t_start)/ t_total) + waypoint[index].yp;
                z_desired = (waypoint[index+1].zp -waypoint[index].zp)*((t-t_start)/ t_total) + waypoint[index].zp;
                thetaNz = waypoint[index].rotation;
            }
            else{ //hold on the desired position during t_hold
                x_desired = waypoint[index+1].xp;
                y_desired = waypoint[index+1].yp;
                z_desired = waypoint[index+1].zp;
            }
        }

        else{
            
            //update
            index = index + 1;
            t_start = t;
            t_holdstart = t;
        }
    }


    
    /*
        Average filter: processing three angular velocity signals using an average filter. At each time step, the average of previous values is calculated to derive the current value, and the previous values are updated accordingly. This method reduces noise and yields a smoother signal.
    */
    
    Omega1_raw = (theta1motor - Theta1_old)/0.001;
    Omega1 = (Omega1_raw + Omega1_old1 + Omega1_old2)/3.0;
    Theta1_old = theta1motor;
    Omega2_raw = (theta2motor - Theta2_old)/0.001;
    Omega2 = (Omega2_raw + Omega2_old1 + Omega2_old2)/3.0;
    Theta2_old = theta2motor;
    Omega3_raw = (theta3motor - Theta3_old)/0.001;
    Omega3 = (Omega3_raw + Omega3_old1 + Omega3_old2)/3.0;
    Theta3_old = theta3motor;

    
    
    /*
        Calculate the friction compensation (u_fric) for each joint. 
        
    */
    
    if(Omega1 > minimum_velocity1){
        ufric1 = Pviscous1*Omega1 + Pcoulomb1;
    } else if(Omega1 < minimum_velocity1*-1){
        ufric1 = Nviscous1*Omega1 + Ncoulomb1;
    } else{
        ufric1 = slope1*Omega1;
    } //joint1 

    
    if(Omega2 > minimum_velocity2){
        ufric2 = Pviscous2*Omega2 + Pcoulomb2;
    } else if(Omega2 < minimum_velocity2*-1){
        ufric2 = Nviscous2*Omega2 + Ncoulomb2;
    } else{
        ufric2 = slope2*Omega2;
    } //joint2

    
    if(Omega3 > minimum_velocity3){
        ufric3 = Pviscous3*Omega3 + Pcoulomb3;
    } else if(Omega3 < minimum_velocity3*-1){
        ufric3 = Nviscous3*Omega3 + Ncoulomb3;
    } else{
        ufric3 = slope3*Omega3;
    } //joint3

    

    /*
        Forward Kinematics to calculate the current position of the end-effector.
    */
    x = 0.254*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor));
    y = 0.254*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor));
    z = 0.254*(1+cos(theta2motor)-sin(theta3motor));
    
    

    /* 
        Transpose of the Jacobian for the CRS robot 
    */
    J11 = -0.254*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor));
    J12 = 0.254*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor));
    J13 = 0;
    J21 = 0.254*cos(theta1motor)*(cos(theta2motor) - sin(theta3motor));
    J22 = 0.254*sin(theta1motor)*(cos(theta2motor) - sin(theta3motor));
    J23 = -0.254*(cos(theta3motor) + sin(theta2motor));
    J31 = -0.254*cos(theta1motor)*sin(theta3motor);
    J32 = -0.254*sin(theta1motor)*sin(theta3motor);
    J33 = -0.254*cos(theta3motor);

    
    
    /*
        Average filter: processing three velocity (along x, y, z axis) using an average filter. At each time step, the average of previous values is calculated to derive the current value, and the previous values are updated accordingly. This method reduces noise and yields a smoother signal.
    */
    xdot_raw = (x - x_prev)/0.001;
    xdot = (xdot_raw + xdot_old1 + xdot_old2)/3.0;
    xdot_old1 = x;
    ydot_raw = (y - y_prev)/0.001;
    ydot = (ydot_raw + ydot_old1 + ydot_old2)/3.0;
    ydot_old1 = y;
    zdot_raw = (z - z_prev)/0.001;
    zdot = (zdot_raw + zdot_old1 + zdot_old2)/3.0;
    zdot_old1 = z;

    
    
    /*
        Rotation matrix from the world coordinate frame to the N frame. 
    */
    R11 = cos(thetaNz)*cos(thetaNy) - sin(thetaNz)*sin(thetaNx)*sin(thetaNy);
    R12 = -sin(thetaNz)*cos(thetaNx);
    R13 = cos(thetaNz)*sin(thetaNy) + sin(thetaNz)*sin(thetaNx)*cos(thetaNy);
    R21 = sin(thetaNz)*cos(thetaNy)+cos(thetaNz)*sin(thetaNx)*sin(thetaNy);
    R22 = cos(thetaNz)*cos(thetaNx) ;
    R23 = sin(thetaNz)*sin(thetaNy)-cos(thetaNz)*sin(thetaNx)*cos(thetaNy);
    R31 = -cos(thetaNx)*sin(thetaNy);
    R32 = sin(thetaNx);
    R33 = cos(thetaNx)*cos(thetaNy);
    
    
    
    /*
        Matrix calculation for Impedance control: Jacobian transpose * Rotation matrix. 
    */
    A11 = J11 * R11 + J12 * R21 + J13 * R31;
    A12 = J11 * R12 + J12 * R22 + J13 * R32;
    A13 = J11 * R13 + J12 * R23 + J13 * R33;
    A21 = J21 * R11 + J22 * R22 + J23 * R31;
    A22 = J21 * R12 + J22 * R22 + J23 * R32;
    A23 = J21 * R13 + J22 * R23 + J23 * R33;
    A31 = J31 * R11 + J32 * R21 + J33 * R31;
    A32 = J31 * R12 + J32 * R22 + J33 * R32;
    A33 = J31 * R13 + J32 * R23 + J33 * R33;


    
    /*
        Impedance control controller equation. 
    */
    *tau1 = A11*(waypoint[index].Kpx*R11*(x_desired-x)+ waypoint[index].Kpx*R21*(y_desired-y) +waypoint[index].Kpx*R31*(z_desired-z) + waypoint[index].Kdx*R11*(xdot_desired-xdot)+ waypoint[index].Kdx*R21*(ydot_desired-ydot)+waypoint[index].Kdx*R31*(zdot_desired-zdot)) + A12*(waypoint[index].Kpy*R12*(x_desired-x)+ waypoint[index].Kpy*R22*(y_desired-y)+waypoint[index].Kpy*R32*(z_desired-z) + waypoint[index].Kdy*R12*(xdot_desired-xdot)+waypoint[index].Kdy*R22*(ydot_desired-ydot)+waypoint[index].Kdy*R32*(zdot_desired-zdot)) + A13*(waypoint[index].Kpz*R13*(x_desired-x)+ waypoint[index].Kpz*R23*(y_desired-y)+waypoint[index].Kpz*R33*(z_desired-z) + waypoint[index].Kdz*R13*(xdot_desired-xdot)+waypoint[index].Kdz*R23*(ydot_desired-ydot)+waypoint[index].Kdz*R33*(zdot_desired-zdot))+ff1*ufric1 ;
    *tau2 = A21*(waypoint[index].Kpx*R11*(x_desired-x)+ waypoint[index].Kpx*R21*(y_desired-y) +waypoint[index].Kpx*R31*(z_desired-z) + waypoint[index].Kdx*R11*(xdot_desired-xdot)+ waypoint[index].Kdx*R21*(ydot_desired-ydot)+waypoint[index].Kdx*R31*(zdot_desired-zdot)) + A22*(waypoint[index].Kpy*R12*(x_desired-x)+ waypoint[index].Kpy*R22*(y_desired-y)+waypoint[index].Kpy*R32*(z_desired-z) + waypoint[index].Kdy*R12*(xdot_desired-xdot)+waypoint[index].Kdy*R22*(ydot_desired-ydot)+waypoint[index].Kdy*R32*(zdot_desired-zdot)) + A23*(waypoint[index].Kpz*R13*(x_desired-x)+ waypoint[index].Kpz*R23*(y_desired-y)+waypoint[index].Kpz*R33*(z_desired-z) + waypoint[index].Kdz*R13*(xdot_desired-xdot)+waypoint[index].Kdz*R23*(ydot_desired-ydot)+waypoint[index].Kdz*R33*(zdot_desired-zdot))+ff2*ufric2 ;
    *tau3 = A31*(waypoint[index].Kpx*R11*(x_desired-x)+ waypoint[index].Kpx*R21*(y_desired-y) +waypoint[index].Kpx*R31*(z_desired-z) + waypoint[index].Kdx*R11*(xdot_desired-xdot)+ waypoint[index].Kdx*R21*(ydot_desired-ydot)+waypoint[index].Kdx*R31*(zdot_desired-zdot)) + A32*(waypoint[index].Kpy*R12*(x_desired-x)+ waypoint[index].Kpy*R22*(y_desired-y)+waypoint[index].Kpy*R32*(z_desired-z) + waypoint[index].Kdy*R12*(xdot_desired-xdot)+waypoint[index].Kdy*R22*(ydot_desired-ydot)+waypoint[index].Kdy*R32*(zdot_desired-zdot)) + A33*(waypoint[index].Kpz*R13*(x_desired-x)+ waypoint[index].Kpz*R23*(y_desired-y)+waypoint[index].Kpz*R33*(z_desired-z) + waypoint[index].Kdz*R13*(xdot_desired-xdot)+waypoint[index].Kdz*R23*(ydot_desired-ydot)+waypoint[index].Kdz*R33*(zdot_desired-zdot))+ff3*ufric3;

    //updates
    Omega1_old2 = Omega1_old1;
    Omega1_old1 = Omega1_raw;
    Omega2_old2 = Omega2_old1;
    Omega2_old1 = Omega2_raw;
    Omega3_old2 = Omega3_old1;
    Omega3_old1 = Omega3_raw;

    xdot_old2 = xdot_old1;
    xdot_old1 = xdot_raw;
    ydot_old2 = ydot_old1;
    ydot_old1 = ydot_raw;
    zdot_old2 = zdot_old1;
    zdot_old1 = zdot_raw;

    x_prev = x;
    y_prev = y;
    z_prev = z;



    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;

//printing to simulink to ensure the code is running as desired before implementing on robot arm
    Simulink_PlotVar1 = theta1motor;
    Simulink_PlotVar2 =   x_desired;
    Simulink_PlotVar3 =  y_desired;
    Simulink_PlotVar4 = z_desired;

    mycount++;


}
// Printing position in TeraTerm to find waypoints
void printing(void){
    if (whattoprint == 0) {
        serial_printf(&SerialA, " position %.2f %.2f,%.2f   \n\r", x,y,z); 

    } else {
        serial_printf(&SerialA, "Print test   \n\r");
    }
}