Aleksandr Daeyoung KimYuanzhe Wang
Published © GPL3+

[UIUC] ME446 - Robot Dynamics and Controls Final Project

ME446 Final Project: Completing 3 main tasks with the Thermo Fisher CRS Robot using impedance controls.

IntermediateProtip5 hours29
[UIUC] ME446 - Robot Dynamics and Controls Final Project

Things used in this project

Hardware components

Thermo Fisher CRS Robot
×1
Fresh Egg
×1
Kitchen Scale
×1
Customized Wood Block with a Hole
×1
Customized Zig-Zag Slot
×1
Cardboard Obstacle
×1

Software apps and online services

Code Composer Studio
Texas Instruments Code Composer Studio

Story

Read more

Schematics

Physical Dimensions of Thermo Fisher CRS Robot

Code

ME446 Final Project Code

C/C++
// #################################################################################
//  FILE:   FinalProject_ADK_YZ_main.c
//  TITLE:  ME446 Final Project
//  NAME: Aleksandr Kim
//        - Industrial & Systems Engineering - Autonomous Systems and Robotics
//  NAME: Yuanzhe Wang
//        - Mechanical Engineering & Science
// #################################################################################

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

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

// 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.42; //-0.37;
float offset_Enc3_rad = 0.24;  // 0.27;
long mycount = 0;

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

// ###################
// ### MATLAB VARS ###
// ###################

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

#pragma DATA_SECTION(ADK_YZ, ".my_vars")
float ADK_YZ = 3.22;

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

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

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

// ####################
// ### GLOABAL VARS ###
// ####################

long arrayindex = 0;
int UARTprint = 0;

// omega (Joint Velocities)
float omega1 = 0.0;
float omega2 = 0.0;
float omega3 = 0.0;

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

// Friction Curve - Friction Compensation
float u_fric1 = 0;
float u_fric2 = 0;
float u_fric3 = 0;
float min_slope = 3.6;

// Joint 1 control effort properties
float min_vel1 = 0.1;
float visc_pos1 = 0.2;
float visc_neg1 = 0.2;
float coul_pos1 = 0.34;
float coul_neg1 = -0.34;
float fric1 = 0.75;

// Joint 2 control effort properties
float min_vel2 = 0.05;
float visc_pos2 = 0.2500;
float visc_neg2 = 0.2870;
float coul_pos2 = 0.4759;
float coul_neg2 = -0.5031;
float fric2 = 0.70;

// Joint 3 control effort properties
float min_vel3 = 0.05;
float visc_pos3 = 0.1922;
float visc_neg3 = 0.2132;
float coul_pos3 = 0.5339;
float coul_neg3 = -0.5190;
float fric3 = 0.575;

// Task Space Forces for Each Axis
float Fx = 0;
float Fy = 0;
float Fz = 0;

// End Effector Position
float x = 0;
float y = 0;
float z = 0;
// End Effector Velocity
float xdot = 0;
float ydot = 0;
float zdot = 0;

//  Desired End Effector Position
float xd = 0.30;
float yd = 0;
float zd = 0.45;
//  Desired End Effector Velocity
float xd_dot = 0;
float yd_dot = 0;
float zd_dot = 0;

// Task Space Control - Torque Friction Compensation
float tauM1fric = 0;
float tauM2fric = 0;
float tauM3fric = 0;

// Task Space PD Controller Gains
// Proportional Gains Kp
float KPx = 100;
float KPy = 100;
float KPz = 90;
// Derivative Gains Kd
float KDx = 6.0;
float KDy = 7.0;
float KDz = 7.0;

// Negative Force (Downward Force) and Robot's Torque Constant
float Fzcmd = 0.0;
float Kt = 6.0;

// Impedance Control Errors
// Position Errors
float ex = 0;
float ey = 0;
float ez = 0;
// Velocity Errors
float exdot = 0;
float eydot = 0;
float ezdot = 0;

// Proportional Gains Kp
float KPxn = 1.2 * 800; // 200
float KPyn = 1.2 * 500; // 120
float KPzn = 1.2 * 600; // 90
// Derivative Gains Kd
float KDxn = 1.2 * 40.0; // 6
float KDyn = 1.2 * 40.0; // 5
float KDzn = 1.2 * 50.0; // 7

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

// Indicating Variables
int waypoint = 0; // State/Waypoint
int i = 0;        // Integer

// Struct containing desired x,y,z variables and time required (dt) to complete waypoint
struct Point
{
    float x;
    float y;
    float z;
    float dt;
};

// Time t
float t = 0.0;
float t_start = 0.0;
float t_total = 0.0;

// Trajectory Speed Variable
float trajspeed = 0.0;

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

// Past States Initialization
float theta1_old = 0.0;
float omega1_old1 = 0.0;
float omega1_old2 = 0.0;
float theta2_old = 0.0;
float omega2_old1 = 0.0;
float omega2_old2 = 0.0;
float theta3_old = 0.0;
float omega3_old1 = 0.0;
float omega3_old2 = 0.0;
float x_old = 0;
float y_old = 0;
float z_old = 0;
float xdot_old1 = 0;
float xdot_old2 = 0;
float ydot_old1 = 0;
float ydot_old2 = 0;
float zdot_old1 = 0;
float zdot_old2 = 0;

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

// #####################
// ### SIMULINK VARS ###
// #####################

// Assign these float to the values you would like to plot in Simulink
float Simulink_PlotVar1 = 0.0;
float Simulink_PlotVar2 = 0.0;
float Simulink_PlotVar3 = 0.0;
float Simulink_PlotVar4 = 0.0;

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

// #################
// ### MAIN CODE ###
// #################

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)
{

    *tau1 = 0.0;
    *tau2 = 0.0;
    *tau3 = 0.0;

    // Motor torque limitation(Max: 5 Min: -5)

    // Saving past states
    if ((mycount % 50) == 0)
    {

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

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

    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
    }

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

    // #####################
    // ### SIMULINK VARS ###
    // #####################

    Simulink_PlotVar1 = xd;
    Simulink_PlotVar2 = yd;
    Simulink_PlotVar3 = zd;
    Simulink_PlotVar4 = z;

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

    // ##################
    // ### IIR FILTER ###
    // ##################

    //
    // theta1dot
    omega1 = (theta1motor - theta1_old) / 0.001;
    omega1 = (omega1 + omega1_old1 + omega1_old2) / 3.0;

    // theta2dot
    omega2 = (theta2motor - theta2_old) / 0.001;
    omega2 = (omega2 + omega2_old1 + omega2_old2) / 3.0;

    // theta3dot
    omega3 = (theta3motor - theta3_old) / 0.001;
    omega3 = (omega3 + omega3_old1 + omega3_old2) / 3.0;

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

    // ####################
    // ### Trajectories ###
    // ####################

    struct Point wp[] = {
        {0.254, 0.0, 0.508, 0.0},                // Point 0 (HOME POSITION)
        {0.038054874, 0.249578774, 0.22, 1.0},   // Point 1 (Above the Hole)
        {0.038054874, 0.249578774, 0.122, 1.0},  // Point 2 (Inside the Hole)
        {0.038054874, 0.249578774, 0.122, 0.5},  // Point 2 STAY for 0.5 seconds
        {0.038054874, 0.249578774, 0.22, 1.0},   // Point 3 (Above the Hole)
        {0.278772503, 0.104385808, 0.22, 1.0},   // Point 4 (Midpoint to the Curve)
        {0.428007482, 0.0979673564, 0.20, 0.75}, // Point 5 (Curve Entrance)
        {0.43283993, 0.0715207011, 0.20, 0.2},   // Point 6 (Inside the Curve)
        {0.432613403, 0.0393368863, 0.20, 0.5},  // Point 7 (First Curve)
        {0.357446969, 0.0185978226, 0.20, 0.5},  // Point 8 (Second Curve)
        {0.421108186, -0.0472436917, 0.20, 0.5}, // Point 9 (Curve Exit)
        {0.421108186, -0.0452436917, 0.31, 0.5}, // Point 10 (Above Curve Exit)
        {0.270858706, 0.170081974, 0.31, 1.0},   // Point 11 (Above the Egg)
        {0.270858706, 0.170081974, 0.273, 3.0},  // Point 12 (Press Down)
        {0.270858706, 0.170081974, 0.273, 2.0},  // Point 12 STAY for 2 seconds
        {0.270858706, 0.170081974, 0.30, 1.0},   // Point 13 (Above the Egg)
        {0.254, 0.0, 0.508, 2.0}                 // Point 14(0) (HOME POSITION)
    };

    float t0 = 1.0;
    float t1 = t0 + wp[1].dt;
    float t2 = t1 + wp[2].dt;
    float t3 = t2 + wp[3].dt;
    float t4 = t3 + wp[4].dt;
    float t5 = t4 + wp[5].dt;
    float t6 = t5 + wp[6].dt;
    float t7 = t6 + wp[7].dt;
    float t8 = t7 + wp[8].dt;
    float t9 = t8 + wp[9].dt;
    float t10 = t9 + wp[10].dt;
    float t11 = t10 + wp[11].dt;
    float t12 = t11 + wp[12].dt;
    float t13 = t12 + wp[13].dt;
    float t14 = t13 + wp[14].dt;
    float t15 = t14 + wp[15].dt;
    float t16 = t15 + wp[16].dt;
    float ts[] = {t0, t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16};

    // Distances from the point to the next point by axis (x,y,z)
    float dx0 = 0;
    float dy0 = 0;
    float dz0 = 0;
    float dx1 = wp[1].x - wp[0].x;
    float dy1 = wp[1].y - wp[0].y;
    float dz1 = wp[1].z - wp[0].z;
    float dx2 = wp[2].x - wp[1].x;
    float dy2 = wp[2].y - wp[1].y;
    float dz2 = wp[2].z - wp[1].z;
    float dx3 = wp[3].x - wp[2].x;
    float dy3 = wp[3].y - wp[2].y;
    float dz3 = wp[3].z - wp[2].z;
    float dx4 = wp[4].x - wp[3].x;
    float dy4 = wp[4].y - wp[3].y;
    float dz4 = wp[4].z - wp[3].z;
    float dx5 = wp[5].x - wp[4].x;
    float dy5 = wp[5].y - wp[4].y;
    float dz5 = wp[5].z - wp[4].z;
    float dx6 = wp[6].x - wp[5].x;
    float dy6 = wp[6].y - wp[5].y;
    float dz6 = wp[6].z - wp[5].z;
    float dx7 = wp[7].x - wp[6].x;
    float dy7 = wp[7].y - wp[6].y;
    float dz7 = wp[7].z - wp[6].z;
    float dx8 = wp[8].x - wp[7].x;
    float dy8 = wp[8].y - wp[7].y;
    float dz8 = wp[8].z - wp[7].z;
    float dx9 = wp[9].x - wp[8].x;
    float dy9 = wp[9].y - wp[8].y;
    float dz9 = wp[9].z - wp[8].z;
    float dx10 = wp[10].x - wp[9].x;
    float dy10 = wp[10].y - wp[9].y;
    float dz10 = wp[10].z - wp[9].z;
    float dx11 = wp[11].x - wp[10].x;
    float dy11 = wp[11].y - wp[10].y;
    float dz11 = wp[11].z - wp[10].z;
    float dx12 = wp[12].x - wp[11].x;
    float dy12 = wp[12].y - wp[11].y;
    float dz12 = wp[12].z - wp[11].z;
    float dx13 = wp[13].x - wp[12].x;
    float dy13 = wp[13].y - wp[12].y;
    float dz13 = wp[13].z - wp[12].z;
    float dx14 = wp[14].x - wp[13].x;
    float dy14 = wp[14].y - wp[13].y;
    float dz14 = wp[14].z - wp[13].z;
    float dx15 = wp[15].x - wp[14].x;
    float dy15 = wp[15].y - wp[14].y;
    float dz15 = wp[15].z - wp[14].z;
    float dx16 = wp[16].x - wp[15].x;
    float dy16 = wp[16].y - wp[15].y;
    float dz16 = wp[16].z - wp[15].z;

    float dx[] = {dx0, dx1, dx2, dx3, dx4, dx5, dx6, dx7, dx8, dx9, dx10, dx11, dx12, dx13, dx14, dx15, dx16};
    float dy[] = {dy0, dy1, dy2, dy3, dy4, dy5, dy6, dy7, dy8, dy9, dy10, dy11, dy12, dy13, dy14, dy15, dy16};
    float dz[] = {dz0, dz1, dz2, dz3, dz4, dz5, dz6, dz7, dz8, dz9, dz10, dz11, dz12, dz13, dz14, dz15, dz16};

    // Straight Line Trajectory
    /*  The straight-line trajectory from A(x_a, y_a, z_a) to B(x_b, y_b, z_b) is generated by the
    equations given. the time duration is determined for each trajectory trying to reach the optimal
    precision & smoothness.
    For certain tasks like the going through the curve, the Impedance controller gains were adjusted,
    enabling free movement along certain axis. This was done by adjusting theta z and x axis as reference.
    For EX.
    theta z = 15 degrees, KPxn = 0, KDxn = 0, will allow free movement in  \ (rotated x axis) direction
    theta z = 0 degrees, KPxn = 0, KDxn = 0, will allow free movement in  | (original x axis) direction
    */

    float t = (mycount) * 0.001; //

    // Straight line trajectory and Waypoints

    // Setting to Home Position (For 1 seconds)
    if (0 <= t && t < t0)
    {
        KPxn = 0.7 * 800; // 200
        KPyn = 0.7 * 500; // 120
        KPzn = 0.7 * 600; // 90
        // Derivative Gains Kd
        KDxn = 0.7 * 40.0; // 6
        KDyn = 0.7 * 40.0; // 5
        KDzn = 0.7 * 50.0; // 7
        xd = 0.143034786 + (wp[0].x - 0.143034786) * (t) / (t0);
        yd = 0.000302692351 + (wp[0].y - 0.000302692351) * (t) / (t0);
        zd = 0.425765932 + (wp[0].z - 0.425765932) * (t) / (t0);
        xd_dot = (wp[0].x - 0.143034786) / t0;
        yd_dot = (wp[0].y - 0.000302692351) / t0;
        zd_dot = (wp[0].z - 0.425765932) / t0;
        waypoint = 0;
    }

    // Waypoint 0 (Home Position) -> 1 (Above the Hole)
    if (ts[0] <= t && t < ts[1])
    {
        KPxn = 1.2 * 800; // 200
        KPyn = 1.2 * 500; // 120
        KPzn = 1.2 * 600; // 90
        // Derivative Gains Kd
        KDxn = 1.2 * 40.0; // 6
        KDyn = 1.2 * 40.0; // 5
        KDzn = 1.2 * 50.0; // 7
        xd = wp[0].x + dx[1] * (t - ts[0]) / wp[1].dt;
        yd = wp[0].y + dy[1] * (t - ts[0]) / wp[1].dt;
        zd = wp[0].z + dz[1] * (t - ts[0]) / wp[1].dt;
        xd_dot = dx[1] / wp[1].dt;
        yd_dot = dy[1] / wp[1].dt;
        zd_dot = dz[1] / wp[1].dt;
        waypoint = 1;
    }

    // Waypoint 1 (Above the Hole) -> 2 (Inside the Hole)
    if (ts[1] <= t && t < ts[2])
    {
        xd = wp[1].x + dx[2] * (t - ts[1]) / wp[2].dt;
        yd = wp[1].y + dy[2] * (t - ts[1]) / wp[2].dt;
        zd = wp[1].z + dz[2] * (t - ts[1]) / wp[2].dt;
        xd_dot = dx[2] / wp[2].dt;
        yd_dot = dy[2] / wp[2].dt;
        zd_dot = dz[2] / wp[2].dt;
        waypoint = 2;
    }

    // Waypoint 2 (Stay Inside the Hole for 0.5 seconds)
    if (ts[2] <= t && t < ts[3])
    {
        xd = wp[2].x + dx[3] * (t - ts[2]) / wp[3].dt;
        yd = wp[2].y + dy[3] * (t - ts[2]) / wp[3].dt;
        zd = wp[2].z + dz[3] * (t - ts[2]) / wp[3].dt;
        xd_dot = dx[3] / wp[3].dt;
        yd_dot = dy[3] / wp[3].dt;
        zd_dot = dz[3] / wp[3].dt;
        waypoint = 2;
    }

    // Waypoint 2 (Inside the Hole) -> 3 (Above the Hole)
    if (ts[3] <= t && t < ts[4])
    {
        xd = wp[3].x + dx[4] * (t - ts[3]) / wp[4].dt;
        yd = wp[3].y + dy[4] * (t - ts[3]) / wp[4].dt;
        zd = wp[3].z + dz[4] * (t - ts[3]) / wp[4].dt;
        xd_dot = dx[4] / wp[4].dt;
        yd_dot = dy[4] / wp[4].dt;
        zd_dot = dz[4] / wp[4].dt;
        waypoint = 3;
    }

    // Waypoint 3 (Above the Hole) -> 4 (Midpoint to the Curve)
    if (ts[4] <= t && t < ts[5])
    {
        xd = wp[4].x + dx[5] * (t - ts[4]) / wp[5].dt;
        yd = wp[4].y + dy[5] * (t - ts[4]) / wp[5].dt;
        zd = wp[4].z + dz[5] * (t - ts[4]) / wp[5].dt;
        xd_dot = dx[5] / wp[5].dt;
        yd_dot = dy[5] / wp[5].dt;
        zd_dot = dz[5] / wp[5].dt;
        waypoint = 4;
    }

    // Waypoint 4 (Midpoint to the Curve) -> 5 (Inside the Curve)
    if (ts[5] <= t && t < ts[6])
    {

        xd = wp[5].x + dx[6] * (t - ts[5]) / wp[6].dt;
        yd = wp[5].y + dy[6] * (t - ts[5]) / wp[6].dt;
        zd = wp[5].z + dz[6] * (t - ts[5]) / wp[6].dt;
        xd_dot = dx[6] / wp[6].dt;
        yd_dot = dy[6] / wp[6].dt;
        zd_dot = dz[6] / wp[6].dt;
        waypoint = 5;
    }

    // Waypoint 5 (Curve Entrance) -> 6 (Inside the Curve)
    if (ts[6] <= t && t < ts[7])
    {
        KPxn = 800; // 200
        KPyn = 500; // 120
        KPzn = 600; // 90
        // Derivative Gains Kd
        KDxn = 40.0; // 6
        KDyn = 40.0; // 5
        KDzn = 50.0; // 7
        xd = wp[6].x + dx[7] * (t - ts[6]) / wp[7].dt;
        yd = wp[6].y + dy[7] * (t - ts[6]) / wp[7].dt;
        zd = wp[6].z + dz[7] * (t - ts[6]) / wp[7].dt;
        xd_dot = dx[7] / wp[7].dt;
        yd_dot = dy[7] / wp[7].dt;
        zd_dot = dz[7] / wp[7].dt;
        waypoint = 6;
    }

    // Waypoint 6 (Inside the Curve) -> 7 (First Curve)
    if (ts[7] <= t && t < ts[8])
    {
        // Allowing Free Movement in original x direction
        thetaz = 0;
        KPxn = 0;
        KDxn = 0;
        xd = wp[7].x + dx[8] * (t - ts[7]) / wp[8].dt;
        yd = wp[7].y + dy[8] * (t - ts[7]) / wp[8].dt;
        zd = wp[7].z + dz[8] * (t - ts[7]) / wp[8].dt;
        xd_dot = dx[8] / wp[8].dt;
        yd_dot = dy[8] / wp[8].dt;
        zd_dot = dz[8] / wp[8].dt;
        waypoint = 7;
    }

    // Waypoint 7 (First Curve) -> 8 (Second Curve)
    if (ts[8] <= t && t < ts[9])
    {
        // Allowing Free Movement in x axis rotated counter clock wise 105 degrees
        thetaz = 7 * PI / 12;
        KPxn = 0;
        KDxn = 0;
        xd = wp[8].x + dx[9] * (t - ts[8]) / wp[9].dt;
        yd = wp[8].y + dy[9] * (t - ts[8]) / wp[9].dt;
        zd = wp[8].z + dz[9] * (t - ts[8]) / wp[9].dt;
        xd_dot = dx[9] / wp[9].dt;
        yd_dot = dy[9] / wp[9].dt;
        zd_dot = dz[9] / wp[9].dt;
        waypoint = 8;
    }

    // Waypoint 8 (Second Curve) -> 9 (Curve Exit)
    if (ts[9] <= t && t < ts[10])
    {
        // Allowing Free Movement in original x direction
        thetaz = 0;
        KPxn = 0;
        KDxn = 0;
        xd = wp[9].x + dx[10] * (t - ts[9]) / wp[10].dt;
        yd = wp[9].y + dy[10] * (t - ts[9]) / wp[10].dt;
        zd = wp[9].z + dz[10] * (t - ts[9]) / wp[10].dt;
        xd_dot = dx[10] / wp[10].dt;
        yd_dot = dy[10] / wp[10].dt;
        zd_dot = dz[10] / wp[10].dt;
        waypoint = 9;
    }

    // Waypoint 9 (Curve Exit) -> 10 (Above Curve Exit)
    if (ts[10] <= t && t < ts[11])
    {
        KPxn = 1.2 * 800; // 200
        KPyn = 1.2 * 500; // 120
        KPzn = 1.2 * 600; // 90
        // Derivative Gains Kd
        KDxn = 1.2 * 40.0; // 6
        KDyn = 1.2 * 40.0; // 5
        KDzn = 1.2 * 50.0; // 7
        xd = wp[10].x + dx[11] * (t - ts[10]) / wp[11].dt;
        yd = wp[10].y + dy[11] * (t - ts[10]) / wp[11].dt;
        zd = wp[10].z + dz[11] * (t - ts[10]) / wp[11].dt;
        xd_dot = dx[11] / wp[11].dt;
        yd_dot = dy[11] / wp[11].dt;
        zd_dot = dz[11] / wp[11].dt;
        waypoint = 10;
    }
    // Waypoint 10 (Above Curve Exit) -> 11 (Above the Egg)
    if (ts[11] <= t && t < ts[12])
    {
        xd = wp[11].x + dx[12] * (t - ts[11]) / wp[12].dt;
        yd = wp[11].y + dy[12] * (t - ts[11]) / wp[12].dt;
        zd = wp[11].z + dz[12] * (t - ts[11]) / wp[12].dt;
        xd_dot = dx[12] / wp[12].dt;
        yd_dot = dy[12] / wp[12].dt;
        zd_dot = dz[12] / wp[12].dt;
        waypoint = 11;
    }

    // Waypoint 11 (Above the Egg) -> 12 (Press down)
    if (ts[12] <= t && t < ts[13])
    {
        xd = wp[12].x + dx[13] * (t - ts[12]) / wp[13].dt;
        yd = wp[12].y + dy[13] * (t - ts[12]) / wp[13].dt;
        zd = wp[12].z + dz[13] * (t - ts[12]) / wp[13].dt;
        xd_dot = dx[13] / wp[13].dt;
        yd_dot = dy[13] / wp[13].dt;
        zd_dot = dz[13] / wp[13].dt;
        waypoint = 12;
    }

    // Waypoint 12 (Press Down & STAY for 2 seconds)
    if (ts[13] <= t && t < ts[14])
    {
        xd = wp[13].x + dx[14] * (t - ts[13]) / wp[14].dt;
        yd = wp[13].y + dy[14] * (t - ts[13]) / wp[14].dt;
        zd = wp[13].z + dz[14] * (t - ts[13]) / wp[14].dt;
        xd_dot = dx[14] / wp[14].dt;
        yd_dot = dy[14] / wp[14].dt;
        zd_dot = dz[14] / wp[14].dt;
        waypoint = 13;
    }

    // Waypoint 12 (Press Down) -> 13 (Above the Egg)
    if (ts[14] <= t && t < ts[15])
    {
        xd = wp[14].x + dx[15] * (t - ts[14]) / wp[15].dt;
        yd = wp[14].y + dy[15] * (t - ts[14]) / wp[15].dt;
        zd = wp[14].z + dz[15] * (t - ts[14]) / wp[15].dt;
        xd_dot = dx[15] / wp[15].dt;
        yd_dot = dy[15] / wp[15].dt;
        zd_dot = dz[15] / wp[15].dt;
        waypoint = 13;
    }

    // Waypoint 13 (Above the Egg) -> 14 (Return to Home Position)
    if (ts[15] <= t && t < ts[16])
    {
        xd = wp[15].x + dx[16] * (t - ts[15]) / wp[16].dt;
        yd = wp[15].y + dy[16] * (t - ts[15]) / wp[16].dt;
        zd = wp[15].z + dz[16] * (t - ts[15]) / wp[16].dt;
        xd_dot = dx[16] / wp[16].dt;
        yd_dot = dy[16] / wp[16].dt;
        zd_dot = dz[16] / wp[16].dt;
        waypoint = 14;
    }

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

    // ##################
    // ### PARAMETERS ###
    // ##################

    // Jacobian Transpose Variables
    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;

    // Rotational Matrix Variables
    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;

    // Friction Compensation
    /*
    The friction compensation is implemented by applying a small torque approximately equal to the
    frictional torque to each joint to cancel the friction. The behavior of friction is characterized
    by a static friction zone, where joint velocity is small, and two kinetic friction zones, where
    joint velocity exceeds the static zone in positive and negative direction. The boundary point,
    viscous, and coulomb component of friction is taken from the pre-determined friction plot.
    Some viscous and coulomb constants were modified for our specific robot arm. Upon successful
    implementation, the joints should move smoothly without apparent resistance.
    */

    if (omega1 > min_vel1)
    {
        u_fric1 = visc_pos1 * omega1 + coul_pos1;
    }
    else if (omega1 < -min_vel1)
    {
        u_fric1 = visc_neg1 * omega1 + coul_neg1;
    }
    else
    {
        u_fric1 = min_slope * omega1;
    }

    if (omega2 > min_vel2)
    {
        u_fric2 = visc_pos2 * omega2 + coul_pos2;
    }
    else if (omega2 < -min_vel2)
    {
        u_fric2 = visc_neg2 * omega2 + coul_neg2;
    }
    else
    {
        u_fric2 = min_slope * omega2;
    }

    if (omega3 > min_vel3)
    {
        u_fric3 = visc_pos3 * omega3 + coul_pos3;
    }
    else if (omega3 < -min_vel3)
    {
        u_fric3 = visc_neg3 * omega3 + coul_neg3;
    }
    else
    {
        u_fric3 = min_slope * omega3;
    }

    // Defining Motor Friciton
    tauM1fric = fric1 * u_fric1;
    tauM2fric = fric2 * u_fric2;
    tauM3fric = fric3 * u_fric3;

    // Forward Kinematics Equations for Mapping End Effector Coordinates
    x = 0.254 * cosq1 * (cosq3 + sinq2);
    y = 0.254 * sinq1 * (cosq3 + sinq2);
    z = 0.254 * cosq2 - 0.254 * sinq3 + 0.254;

    // Using Trapezoid Rule for Task Space Velocity
    xdot = (x - x_old) / 0.001;
    xdot = (xdot + xdot_old1 + xdot_old2) / 3.0;
    ydot = (y - y_old) / 0.001;
    ydot = (ydot + ydot_old1 + ydot_old2) / 3.0;
    zdot = (z - z_old) / 0.001;
    zdot = (zdot + zdot_old1 + zdot_old2) / 3.0;

    // Task Space Forces
    Fx = KPx * (xd - x) + KDx * (xd_dot - xdot);
    Fy = KPy * (yd - y) + KDy * (yd_dot - ydot);
    Fz = KPz * (zd - z) + KDz * (zd_dot - zdot);

    // Defining Position and Velocity Errors
    ex = xd - x;
    ey = yd - y;
    ez = zd - z;
    exdot = xd_dot - xdot;
    eydot = yd_dot - ydot;
    ezdot = zd_dot - zdot;

    //------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
    // ###################
    // ### CONTROLLERS ###
    // ###################

    // Impedance Controller
    /*
    The impedance control is implemented in a similar fashion as task-space PD control, with a rotation matrix
    R^W_N,transformation between end-effector’s frame and world frame, multiplied to the force matrix. By adjusting
    the angles in this rotation matrix, the end-effector should be able to resist motion in the according direction
    at stability or move freely in that direction if gains for this direction are zeroed.
    */
    *tau1 = (JT_11 * R11 + JT_12 * R21 + JT_13 * R31) * (KDxn * R11 * exdot + KDxn * R21 * eydot + KDxn * R31 * ezdot + KPxn * R11 * ex + KPxn * R21 * ey + KPxn * R31 * ez) + (JT_11 * R12 + JT_12 * R22 + JT_13 * R32) * (KDyn * R12 * exdot + KDyn * R22 * eydot + KDyn * R32 * ezdot + KPyn * R12 * ex + KPyn * R22 * ey + KPyn * R32 * ez) + (JT_11 * R13 + JT_12 * R23 + JT_13 * R33) * (KDzn * R13 * exdot + KDzn * R23 * eydot + KDzn * R33 * ezdot + KPzn * R13 * ex + KPzn * R23 * ey + KPzn * R33 * ez) + tauM1fric;
    *tau2 = (JT_21 * R11 + JT_22 * R21 + JT_23 * R31) * (KDxn * R11 * exdot + KDxn * R21 * eydot + KDxn * R31 * ezdot + KPxn * R11 * ex + KPxn * R21 * ey + KPxn * R31 * ez) + (JT_21 * R12 + JT_22 * R22 + JT_23 * R32) * (KDyn * R12 * exdot + KDyn * R22 * eydot + KDyn * R32 * ezdot + KPyn * R12 * ex + KPyn * R22 * ey + KPyn * R32 * ez) + (JT_21 * R13 + JT_22 * R23 + JT_23 * R33) * (KDzn * R13 * exdot + KDzn * R23 * eydot + KDzn * R33 * ezdot + KPzn * R13 * ex + KPzn * R23 * ey + KPzn * R33 * ez) + tauM2fric;
    *tau3 = (JT_31 * R11 + JT_32 * R21 + JT_33 * R31) * (KDxn * R11 * exdot + KDxn * R21 * eydot + KDxn * R31 * ezdot + KPxn * R11 * ex + KPxn * R21 * ey + KPxn * R31 * ez) + (JT_31 * R12 + JT_32 * R22 + JT_33 * R32) * (KDyn * R12 * exdot + KDyn * R22 * eydot + KDyn * R32 * ezdot + KPyn * R12 * ex + KPyn * R22 * ey + KPyn * R32 * ez) + (JT_31 * R13 + JT_32 * R23 + JT_33 * R33) * (KDzn * R13 * exdot + KDzn * R23 * eydot + KDzn * R33 * ezdot + KPzn * R13 * ex + KPzn * R23 * ey + KPzn * R33 * ez) + tauM3fric;

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

    // ###################
    // ### PAST 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;
    x_old = x;
    y_old = y;
    z_old = z;
    xdot_old1 = xdot;
    xdot_old2 = xdot_old1;
    ydot_old1 = ydot;
    ydot_old2 = ydot_old1;
    zdot_old1 = zdot;
    zdot_old2 = zdot_old1;

    mycount++;
} // END OF MAIN Function

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

// ##################
// ### UART PRINT ###
// ##################
void printing(void)
{
    if (whattoprint == 0)
    {
    }
    else
    {
        serial_printf(&SerialA, "Print test   \n\r");
    }
} // END of UART PRINT

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

ME446 - Final Project Repository

Credits

Aleksandr Daeyoung Kim

Aleksandr Daeyoung Kim

2 projects • 1 follower
Yuanzhe Wang

Yuanzhe Wang

1 project • 0 followers
Thanks to Professor Justin Yim, Nagesh Eranki, Negin Musavi.

Comments

Related channels and tags
  • Robotics
  • Texas Instruments