CODE IMPLEMENTATION

#include "math.h"
#include "F28335Serial.h"
#include <stdio.h>
#include <string.h>

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

// Declare positions as comments for reference
// position1: 0.14 0.00,0.42
// position2: .15 0.27,0.46
// position3: 0.03 0.34,0.23
// position4: 0.03 0.34,0.12
// position5: 0.39 0.08,0.19
// position5.1: 0.37 0.03,0.19
// position6: 0.34 0.05,0.20
// position7: [not specified]

float v = 1;

// Define a struct for positions
typedef struct position {
    float x_Des;
    float y_Des;
    float z_Des;
    float velocity; // Velocity from the previous point to this point
} p;

// Initialize positions and their respective velocities
p points[] = {
    {0.14, 0, 0.42, 0.2},
    {0.15, 0.27, 0.46, 1},
    {0.03, 0.34, 0.23, 1},
    {0.03, 0.34, 0.13, 0.5},
    {0.03, 0.34, 0.13, 0.5},
    {0.03, 0.34, 0.23, 1},
    {0.16, 0.16, 0.4, 1},
    {0.388, 0.078, 0.22, 1},
    {0.41, 0.04, 0.205, 0.65},
    {0.412, 0.037, 0.207, 0.6},
    {0.415, 0.034, 0.209, 0.64},
    {0.4105, 0.0215, 0.21, 0.7},
    {0.403, 0.009, 0.215, 0.8},
    {0.33, 0.02, 0.21, 0.8},
    {0.32, 0.01, 0.21, 0.8},
    {0.33, 0, 0.21, 0.8},
    {0.4, -0.07, 0.21, 0.8},
    {0.4, -0.07, 0.3, 0.9},
    {0.24, 0.125, 0.31, 0.8},
    {0.24, 0.125, 0.281, 0.075},
    {0.24, 0.125, 0.281, 0.075},
    {0.24, 0.125, 0.31, 0.8},
    {0.254, 0, 0.508, 1}
};

float time_counts[sizeof(points) - 1]; // Array to store time taken to travel between consecutive points
float time_total[sizeof(points)] = {0};  // Cumulative time totals for reaching each point from the start
int time_counts_length = sizeof(time_counts) / sizeof(time_counts[0]);
int time_total_length = sizeof(time_total) / sizeof(time_total[0]);
int points_length = sizeof(points) / sizeof(points[0]);

// Function to calculate the time to travel between points
int get_time(void){
    for (int i = 0; i < time_counts_length; i++) {
        // Calculate Euclidean distance between points and divide by velocity to get time
        time_counts[i] = sqrt((points[i+1].x_Des - points[i].x_Des) * (points[i+1].x_Des - points[i].x_Des) +
                              (points[i+1].y_Des - points[i].y_Des) * (points[i+1].y_Des - points[i].y_Des) +
                              (points[i+1].z_Des - points[i].z_Des) * (points[i+1].z_Des - points[i].z_Des)) /
                              points[i+1].velocity;
    }
    // Manually adjust certain times due to specific conditions
    time_counts[3] = 0.5;  // Adjust time for 4th segment
    time_counts[19] = 2.0; // Adjust time for 20th segment

    for (int i = 1; i < time_total_length; i++) {
        time_total[i] = 0;
        for (int j = 0; j < i; j++) {
            time_total[i] += time_counts[j];
        }
    }
    return 0;
}

// Placeholder variables for positions and velocities
float x_d = 0;
float y_d = 0;
float z_d = 0;
float x_ddotw = 0;
float y_ddotw = 0;
float z_ddotw = 0;

// Offsets for encoder readings
float offset_Enc2_rad = -0.44; // Initial offset for encoder 2
float offset_Enc3_rad = 0.21; // Initial offset for encoder 3

// Global variables for kinematics and dynamics
long mycount = 0;
float x_w, y_w, z_w, theta1, theta2, theta3, D, theta1m, theta2m, theta3m;
#pragma DATA_SECTION(whattoprint, ".my_vars")
float whattoprint = 0.0;
#pragma DATA_SECTION(secondvariable, ".my_vars")
float secondvariable = 0.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;

float times[4];

float tau1_print = 0;
float tau2_print = 0;
float tau3_print = 0;

// Variables for control law gains and motor torque calculations
float Simulink_PlotVar1 = 0;
float Simulink_PlotVar2 = 0;
float Simulink_PlotVar3 = 0;
float Simulink_PlotVar4 = 0;

float theta1_desired = 0;
float theta2_desired = 0;
float theta3_desired = 0;

float Kp1 = 200;
float Kd1 = 4.5;
float Ki1 = 1000;
float Kp2 = 200;
float Kd2 = 4.5;
float Ki2 = 1000;
float Kp3 = 100;
float Kd3 = 5.2;
float Ki3 = 1000;

float J1 = 0.0167;
float J2 = 0.03;
float J3 = 0.0128;

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;

float theta1_IK = 0;
float theta2_IK = 0;
float theta3_IK = 0;

float theta1_d = 0;
float theta2_d = 0;
float theta3_d = 0;

float theta1_IKm = 0;
float theta2_IKm = 0;
float theta3_IKm = 0;
float e1 = 0;
float e2 = 0;
float e3 = 0;
float e1_old = 0;
float e2_old = 0;
float e3_old = 0;

float Ik1 = 0;
float Ik2 = 0;
float Ik3 = 0;

float Ik1_old = 0;
float Ik2_old = 0;
float Ik3_old = 0;

float threshold1 = 0.005;
float threshold2 = 0.005;
float threshold3 = 0.005;

float theta_d = 0;
float theta_ddot = 0;
float theta_dddot = 0;

float u_fric1 = 0;
float min_v1 = 0.100000001;
float v_p1 = 0.2513;
float v_n1 = 0.2477;
float col_p1 = 0.25;
float col_n1 = -0.248;
float slope1 = 3.6;
float f1 = 0.7;

float min_v2 = 0.05;
float v_p2 = 0.25;
float v_n2 = 0.287;
float col_p2 = 0.4759;
float col_n2 = -0.5031;
float slope2 = 3.6;
float f2 = 0.7;

float min_v3 = 0.05;
float v_p3 = 0.1922;
float v_n3 = 0.25499;
float col_p3 = 0.2849999;
float col_n3 = -0.5;
float slope3 = 3.29;
float f3 = 1.0;

float u_fric2 = 0;
float u_fric3 = 0;

float a_x = 0.3;
float a_y = 0;
float a_z = 0.2;
float b_x = 0.2;
float b_y = 0.2;
float b_z = 0.2;

float t_total = 0;

float t12 = 0;
float t23 = 0;
float line_k = 0;

float pre_k = 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;
float Kpx = 50;
float Kdx = 2;
float Kpy = 50;
float Kdy = 2;
float Kpz = 80;
float Kdz = 7;
float x_dw = 0;
float y_dw = 0;
float z_dw = 0;

float x_dotw = 0;
float y_dotw = 0;
float z_dotw = 0;
float x_old = 0;
float y_old = 0;
float z_old = 0;
float x_dotold1 = 0;
float x_dotold2 = 0;
float y_dotold1 = 0;
float y_dotold2 = 0;
float z_dotold1 = 0;
float z_dotold2 = 0;

float Kt = 6.0;
float F_zcmd = 0;
float F_ycmd = 0;
float F_xcmd = 0;

float KP_XN = 550;
float KP_YN = 550;
float KP_ZN = 600;
float KD_XN = 40;
float KD_YN = 20;
float KD_ZN = 20;

int time_c = 1;

void mains_code(void);

// Main function
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) {
    *tau1 = 0;
    *tau2 = 0;
    *tau3 = 0;
    line_k = (b_y - a_y) / (b_x - a_x);
    pre_k = -1 / line_k;

    // Calculate angles from slope
    thetaz = atan2(pre_k, 1);

    // Compute cosines and sines for transformation matrix
    cosz = cos(thetaz);
    sinz = sin(thetaz);
    cosx = cos(thetax);
    sinx = sin(thetax);
    cosy = cos(thetay);
    siny = sin(thetay);

    // Compute rotation matrix
    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;

    // Compute Jacobian transpose for 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;

    float t = mycount * 0.001; // Calculate time based on the counter, assuming 1ms per count

    // Use precomputed total times and durations to calculate positions and velocities
    if (time_c == 1) {
        int a = get_time(); // Calculate times if not already done
        time_c = 0;
    }

    // Iterate through all time segments and update position and velocity
    for (int i = 0; i < time_counts_length; i++) {
        if (t >= time_total[i] && t < time_total[i + 1]) {
            get_trajectory(t, time_total[i], time_total[i + 1], points[i], points[i + 1]);
        }
    }

    // Calculate friction forces based on velocity thresholds
    if (Omega1 > min_v1) {
        u_fric1 = v_p1 * Omega1 + col_p1;
    } else if (Omega1 < -min_v1) {
        u_fric1 = v_n1 * Omega1 + col_n1;
    } else {
        u_fric1 = slope1 * Omega1;
    }

    if (Omega2 > min_v2) {
        u_fric2 = v_p2 * Omega2 + col_p2;
    } else if (Omega2 < -min_v2) {
        u_fric2 = v_n2 * Omega2 + col_n2;
    } else {
        u_fric2 = slope2 * Omega2;
    }

    if (Omega3 > min_v3) {
        u_fric3 = v_p3 * Omega3 + col_p3;
    } else if (Omega3 < -min_v3) {
        u_fric3 = v_n3 * Omega3 + col_n3;
    } else {
        u_fric3 = slope3 * Omega3;
    }

    // Compute derivatives of position for kinematic equations
    x_dotw = (x_w - x_old) / 0.001; // Calculate first derivative
    x_dotw = (x_dotw + x_dotold1 + x_dotold2) / 3.0; // Smoothing by averaging
    x_old = x_w;
    x_dotold2 = x_dotold1;
    x_dotold1 = x_dotw;

    y_dotw = (y_w - y_old) / 0.001;
    y_dotw = (y_dotw + y_dotold1 + y_dotold2) / 3.0;
    y_old = y_w;
    y_dotold2 = y_dotold1;
    y_dotold1 = y_dotw;

    z_dotw = (z_w - z_old) / 0.001;
    z_dotw = (z_dotw + z_dotold1 + z_dotold2) / 3.0;
    z_old = z_w;
    z_dotold2 = z_dotold1;
    z_dotold1 = z_dotw;

    x_dw = x_d; // Desired positions are updated
    y_dw = y_d;
    z_dw = z_d;

    // Calculate torques based on dynamics equations
    *tau1 = - (JT_11 * R11 + JT_12 * R21 + JT_13 * R31) * (KD_XN * RT11 * (x_dotw - x_ddotw) - KP_XN * RT11 * (x_dw - x_w) + KD_XN * RT12 * (y_dotw - y_ddotw) - KP_XN * RT12 * (y_dw - y_w) + KD_XN * RT13 * (z_dotw - z_ddotw) - KP_XN * RT13 * (z_dw - z_w)) - (JT_11 * R12 + JT_12 * R22 + JT_13 * R32) * (KD_YN * RT21 * (x_dotw - x_ddotw) - KP_YN * RT21 * (x_dw - x_w) + KD_YN * RT22 * (y_dotw - y_ddotw) - KP_YN * RT22 * (y_dw - y_w) + KD_YN * RT23 * (z_dotw - z_ddotw) - KP_YN * RT23 * (z_dw - z_w)) - (JT_11 * R13 + JT_12 * R23 + JT_13 * R33) * (KD_ZN * RT31 * (x_dotw - x_ddotw) - KP_ZN * RT31 * (x_dw - x_w) + KD_ZN * RT32 * (y_dotw - y_ddotw) - KP_ZN * RT32 * (y_dw - y_w) + KD_ZN * RT33 * (z_dotw - z_ddotw) - KP_ZN * RT33 * (z_dw - z_w)) + u_fric1 * f1;
    *tau2 = - (JT_21 * R11 + JT_22 * R21 + JT_23 * R31) * (KD_XN * RT11 * (x_dotw - x_ddotw) - KP_XN * RT11 * (x_dw - x_w) + KD_XN * RT12 * (y_dotw - y_ddotw) - KP_XN * RT12 * (y_dw - y_w) + KD_XN * RT13 * (z_dotw - z_ddotw) - KP_XN * RT13 * (z_dw - z_w)) - (JT_21 * R12 + JT_22 * R22 + JT_23 * R32) * (KD_YN * RT21 * (x_dotw - x_ddotw) - KP_YN * RT21 * (x_dw - x_w) + KD_YN * RT22 * (y_dotw - y_ddotw) - KP_YN * RT22 * (y_dw - y_w) + KD_YN * RT23 * (z_dotw - z_ddotw) - KP_YN * RT23 * (z_dw - z_w)) - (JT_21 * R13 + JT_22 * R23 + JT_23 * R33) * (KD_ZN * RT31 * (x_dotw - x_ddotw) - KP_ZN * RT31 * (x_dw - x_w) + KD_ZN * RT32 * (y_dotw - y_ddotw) - KP_ZN * RT32 * (y_dw - y_w) + KD_ZN * RT33 * (z_dotw - z_ddotw) - KP_ZN * RT33 * (z_dw - z_w)) + u_fric2 * f2;
    *tau3 = - (JT_31 * R11 + JT_32 * R21 + JT_33 * R31) * (KD_XN * RT11 * (x_dotw - x_ddotw) - KP_XN * RT11 * (x_dw - x_w) + KD_XN * RT12 * (y_dotw - y_ddotw) - KP_XN * RT12 * (y_dw - y_w) + KD_XN * RT13 * (z_dotw - z_ddotw) - KP_XN * RT13 * (z_dw - z_w)) - (JT_31 * R12 + JT_32 * R22 + JT_33 * R32) * (KD_YN * RT21 * (x_dotw - x_ddotw) - KP_YN * RT21 * (x_dw - x_w) + KD_YN * RT22 * (y_dotw - y_ddotw) - KP_YN * RT22 * (y_dw - y_w) + KD_YN * RT23 * (z_dotw - z_ddotw) - KP_YN * RT23 * (z_dw - z_w)) - (JT_31 * R13 + JT_32 * R23 + JT_33 * R33) * (KD_ZN * RT31 * (x_dotw - x_ddotw) - KP_ZN * RT31 * (x_dw - x_w) + KD_ZN * RT32 * (y_dotw - y_ddotw) - KP_ZN * RT32 * (y_dw - y_w) + KD_ZN * RT33 * (z_dotw - z_ddotw) - KP_ZN * RT33 * (z_dw - z_w)) + u_fric3 * f3;

    // Update motor velocities based on current and previous states
    Omega1 = (theta1motor - Theta1_old) / 0.001;
    Omega1 = (Omega1 + Omega1_old1 + Omega1_old2) / 3.0;

    Omega2 = (theta2motor - Theta2_old) / 0.001;
    Omega2 = (Omega2 + Omega2_old1 + Omega2_old2) / 3.0;

    Omega3 = (theta3motor - Theta3_old) / 0.001;
    Omega3 = (Omega3 + Omega3_old1 + Omega3_old2) / 3.0;

    // Print and log motor positions periodically
    if ((mycount % 50) == 0) {
        theta1array[arrayindex] = theta1motor;
        theta2array[arrayindex] = theta2motor;

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

    e1 = theta1_d - theta1motor;
    e2 = theta2_d - theta2motor;
    e3 = theta3_d - theta3motor;

    // Update error integrals for PID control
    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 = x_d;
    printtheta2motor = y_d;
    printtheta3motor = z_d;

    // Limit torques to within specified bounds
    if (fabs(*tau1) < 5) {
        Ik1 = Ik1_old;
    } else if (*tau1 > 5) {
        *tau1 = 5;
    } else {
        *tau1 = -5;
    }

    if (fabs(*tau2) < 5) {
        Ik2 = Ik2_old;
    } else if (*tau2 > 5) {
        *tau2 = 5;
    } else {
        *tau2 = -5;
    }

    if (fabs(*tau3) < 5) {
        Ik3 = Ik3_old;
    } else if (*tau3 > 5) {
        *tau3 = 5;
    } else {
        *tau3 = -5;
    }

    tau1_print = *tau1;
    tau2_print = *tau2;
    tau3_print = *tau3;

    // Update previous states
    Theta1_old = theta1motor;
    Omega1_old2 = Omega1_old1;
    Omega1_old1 = Omega1;

    Theta2_old = theta2motor;
    Omega2_old2 = Omega2_old1;
    Omega2_old1 = Omega2;

    Theta3_old = theta3motor;
    Omega3_old2 = Omega3_old1;
    Omega3_old1 = Omega3;

    Ik1_old = Ik1;
    e1_old = e1;
    Ik2_old = Ik2;
    e2_old = e2;
    Ik3_old = Ik3;
    e3_old = e3;

    x_w = 0.254 * cos(theta1motor) * (cos(theta3motor) + sin(theta2motor));
    y_w = 0.254 * sin(theta1motor) * (cos(theta3motor) + sin(theta2motor));
    z_w = 0.254 * cos(theta2motor) - 0.254 * sin(theta3motor) + 0.254;

    Simulink_PlotVar1 = x_d - x_w;
    Simulink_PlotVar2 = y_d - y_w;
    Simulink_PlotVar3 = z_d - z_w;
    Simulink_PlotVar4 = x_d;

    mycount++;
}

void printing(void) {
    if (whattoprint == 0) {
        serial_printf(&SerialA, "time_total: %.2f %.2f,%.2f, %.2f, %.2f, %.2f    \n\r", time_total[2], time_total[3], time_total[4], time_total[16], time_total[17], time_total[18]);
        serial_printf(&SerialA, "desired position %.2f %.2f %.2f    \n\r", x_d, y_d, z_d);
        serial_printf(&SerialA, "actual position: %.2f %.2f %.2f    \n\r", x_w, y_w, z_w);
    } else {
        serial_printf(&SerialA, "Print test   \n\r");
    }
}