Published May 12, 2023 © MIT

ME446 Final Project

The website is about the final project for UIUC ME446 Robotics and Control course.

AdvancedFull instructions provided182

Things used in this project

Hardware components

CRS Robot Arm

Software apps and online services

Texas Instruments Code Composer Studio

MATLAB

Story

Introduction

This website describes the final project of the course ME446 Robot Dynamics and Control. The course emphasizes the fundamentals of kinematics, dynamics, and motion and force control of robots. We will take advantage of all the knowledge learned in the lecture to experiment with the precise control of the robot arm.

The final project task is to make our CRS Robot Arm perform the sequence of tasks using appropriate control and follows the desired path as marked in the following figure. Task 1 is to move the end effector vertically down into the hole and then out. Task 2 is to get the robotic arm around the obstacle without bumping into it and then move the end effector along a zigzag path. Task 3 is to push the end effector down on the egg so that the scale reads in the range 500 to 1000 grams and stayed there for at least 2 seconds. When the robot arm returns to its original position, the task is marked as complete.

Desired points of path

Methodology

Trajectory planning

For our project, we mainly used Straight Line Trajectory to move the robot arm from points to points in line segments.

The straight line between each points are calculated by:

where critical points are defined by:

#define num_points 23 //number of total points
point array_points[num_points] = {                   //x, y, z, thetaz, mode
{   0.138, 0.000, 0.432, 0.0, 0},               //state  1 - point 1: home
{0.254, 0.000, 0.508, 0.0, 0},                  //state  2 - point 2: rest position
// Task 1: Insert into a hole
{task1_x, task1_y, task1_z + 0.3, 0.0, 0},      //state  3 - point 2.1: above the hole
{task1_x, task1_y, task1_z, 0.0, 1},            //state  4 - point 3: surface of the hole
{task1_x, task1_y, task1_z - 0.065, 0.0, 2},    //state  5 - point 4: insert 2 inches into the hole
{task1_x, task1_y, task1_z - 0.065, 0.0, 1},    //state  6 - point 4: wait for 1 second at point 4
{task1_x, task1_y, task1_z, 0.0, 0},            //state  7 - point 3: go back to surface of the hole
{task1_x, task1_y, task1_z + 0.3, 0.0, 0},      //state  8 - point 2.1: go back above the hole
{0.030, 0.104, 0.494, 0.0, 0},                  //state  9 - point 4.1: reduce y from point point 2.1
{0.363, 0.104, 0.494, 0.0, 1},                  //state 10 - point 4.2: increase x from point 4.1 and above the zigzag
// Task 2: Zig Zag
{task2_x, task2_y, task2_z, -(90 - 36.87)/180*PI, 3},                       //state 11 - point 5: entry of zig-zag
{task2_x + 0.022, task2_y - 0.050, task2_z, -(90 + 34.065)/180*PI, 3},      //state 12 - point 5.1: first line segment of zigzag
{task2_x + 0.01, task2_y - 0.067, task2_z, (180 - 15)/180*PI, 3},           //state 13 - point 5.2: first curve segment of zigzag
{task2_x - 0.06, task2_y - 0.055, task2_z, -(90 + 34.065)/180*PI, 3},       //state 14 - point 6: second line segment of zigzag
{task2_x - 0.069, task2_y - 0.073, task2_z,  -(90 - 36.87)/180*PI, 3},      //state 15 - point 6.1: second curve segment of zigzag
{task2_x - 0.012, task2_y - 0.149, task2_z, 0.0, 0},                        //state 16 - point 7 :exit of zig zag
{task2_x - 0.012, task2_y - 0.149, task2_z + 0.3, 0.0, 0},                  //state 17 - point 7.1: above point 7
// Task 3: Desired force
{task3_x, task3_y, task3_z + 0.2, 0.0, 4},      //state 18 - point 8: above the egg
{task3_x, task3_y, task3_z, 0.0, 4},            //state 19 - point 8.1: move down to touch the egg
{task3_x, task3_y, task3_z - 0.019, 0.0, 5},    //state 20 - point 9: press on egg
{task3_x, task3_y, task3_z - 0.019, 0.0, 4},    //state 21 - point 9: press on egg for 2s
{task3_x, task3_y, task3_z + 0.2, 0.0, 0},      //state 22 - point 8: back above the egg
{0.254, 0.000, 0.508, 0.0, 5},                  //state 23 - point 10: thetamotor = 0
};

The robot arm start from state 1 and end at state 23. For each state, the first three elements are start points (x_a, y_a, z_a ) and the first three elements of the next state are end points (x_b, y_b, z_b).

The fourth element of each state stands for the rotation angle of the end effecter frame with respect to the world frame, which is used in the task 2.

The last element of each state stands for the mode, which are defined as:

// mode = 0: speed = 0.5
// mode = 1: speed = 0.1, weak x y force
// mode = 2: wait for 1 second, weak x y force
// mode = 3: speed = 0.05, weak y, slightly weak x
// mode = 4: speed = 0.05
// mode = 5: wait for 2 seconds

Controller

We implemented Task Space PD control together with the feedforward force or impedance control as the position/force controller, learned in Lab 4.

In task 1 and 2, we used PD controller added with impedance control, where theta z were set into different values when performing task 2. We will illustrate more details of task 2 solution in the Challenge part.

In task 3, we used PD controller added with feedforward force, where we applied a force on the z direction to guarantee the egg is weighted between 500 and 1000 grams.

The final result exhibition is as follows：

Final Demo

Challenges

There are two big challenges. The first is from the ZigZag path. To let the end effector smoothly move in the narrow slot, we carefully designed the line trajectory in the zigzag as shown in the following figure:

The x axis of the world frame points down in the figure, and the y axis points right. The yellow arrows indicate directions and line segments that the robot followed. In addition, by using impedance control, we set different theta z to rotate the end effector frame and align the rotated x axis with the yellow arrows. Theta z are green degree values in the figure. We weak the rotated y axis force (vertical to the yellow arrows) and slightly weak rotated x axis force. In this way, the peg followed the slot smoothly even if the force control was not perfectly applied only in the yellow arrow directions (the weakened y axis force did not push the slot so hard).

The second challenge is that the scale reads after pushing the egg is very sensitive to the initial home position, we tuned the gains, the position to touch the egg and the desired forces for many times to attain the perfect read.

Group member:

1. Zhao, Yang

UIUC ME MS Graduate

E-mail: [email protected]

2. Tang, Meng

UIUC ME MENG Graduate

E-mail: [email protected]

Team of Zhao, Yang(left) and Tang, Mengjie(right)

ME446 Final Project Code

#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.448; //-0.37;zero 0.59 start -25.06
float offset_Enc3_rad = 0.203; //0.27; zero 0.52 start 12.14


// 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(newvariable, ".my_vars")
float newvariable = 0.0;

long arrayindex = 0;
int UARTprint = 0;

float printtheta1motor = 0;
float printtheta2motor = 0;
float printtheta3motor = 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;

// Velocity filter
float Theta1_old = 0;
float Omega1_old1 = 0;
float Omega1_old2 = 0;
float Omega1 = 0;
float Omega1raw = 0;

float Theta2_old = 0;
float Omega2_old1 = 0;
float Omega2_old2 = 0;
float Omega2 = 0;
float Omega2raw = 0;

float Theta3_old = 0;
float Omega3_old1 = 0;
float Omega3_old2 = 0;
float Omega3 = 0;
float Omega3raw = 0;

// Desired theta thetad thetadd
float theta1_desired = 0.0;
float theta1_dot_desired = 0.0;
float theta1_ddot_desired = 0.0;

float theta2_desired = 0.0;
float theta2_dot_desired = 0.0;
float theta2_ddot_desired = 0.0;

float theta3_desired = 0.0;
float theta3_dot_desired = 0.0;
float theta3_ddot_desired = 0.0;

//time
float time = 0.0;

//lab3 parameters
// friction coefficients
float pos_Viscous1 = 0.2513;
float pos_Coulomb1 = 0.3637;
float neg_Viscous1 = 0.2477;
float neg_Coulomb1 = -0.2948;
float pos_Viscous2 = 0.2500;
float pos_Coulomb2 = 0.4500;
float neg_Viscous2 = 0.2870;
float neg_Coulomb2 = -0.4900;
float pos_Viscous3 = 0.1922;
float pos_Coulomb3 = 0.5339;
float neg_Viscous3 = 0.2132;
float neg_Coulomb3 = -0.5190;

float u_fric1 = 0.0;
float u_fric2 = 0.0;
float u_fric3 = 0.0;

float g = 9.81;

float fric_coef = 0.2; //friction coef

//Lab4 starter code
float cosq1 = 0.0;
float sinq1 = 0.0;
float cosq2 = 0.0;
float sinq2 = 0.0;
float cosq3 = 0.0;
float sinq3 = 0.0;
float JT_11 = 0.0;
float JT_12 = 0.0;
float JT_13 = 0.0;
float JT_21 = 0.0;
float JT_22 = 0.0;
float JT_23 = 0.0;
float JT_31 = 0.0;
float JT_32 = 0.0;
float JT_33 = 0.0;
float cosz = 0.0;
float sinz = 0.0;
float cosx = 0.0;
float sinx = 0.0;
float cosy = 0.0;
float siny = 0.0;
float thetaz = 0.0; //rotate around z axis by 45 degree will make Y axis parallel to the trajectory
float thetax = 0.0;
float thetay = 0.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 RT11 = 0.0;
float RT12 = 0.0;
float RT13 = 0.0;
float RT21 = 0.0;
float RT22 = 0.0;
float RT23 = 0.0;
float RT31 = 0.0;
float RT32 = 0.0;
float RT33 = 0.0;

//Define Kp and Kd for egge press controller
float Kpx = 500.0;
float Kpy = 500.0;
float Kpz = 1050.0;

float Kdx = 25.0;
float Kdy = 25.0;
float Kdz = 25.0;

//Define KP and Kd for controller
float Kpxn = 500.0;
float Kpyn = 500.0;
float Kpzn = 500.0;

float Kdxn = 25.0;
float Kdyn = 25.0;
float Kdzn = 25.0;

// Set task space x y z related parameters
float x_desired = 0.0;
float y_desired = 0.0;
float z_desired = 0.0;
float x_dot_desired = 0.0;
float y_dot_desired = 0.0;
float z_dot_desired = 0.0;
float x_real = 0.0;
float y_real = 0.0;
float z_real = 0.0;
float x_real_past1 = 0.0;
float y_real_past1 = 0.0;
float z_real_past1 = 0.0;
float x_dot_real = 0.0;
float y_dot_real = 0.0;
float z_dot_real = 0.0;
float x_dot_real_past1 = 0.0;
float y_dot_real_past1 = 0.0;
float z_dot_real_past1 = 0.0;
float x_dot_real_past2 = 0.0;
float y_dot_real_past2 = 0.0;
float z_dot_real_past2 = 0.0;

float x_start = 0.0;
float y_start = 0.0;
float z_start = 0.0;
float x_destination = 0.0;
float y_destination = 0.0;
float z_destination = 0.0;

float t_total = 0.0;
float t_start = 0.0;
float t = 0.0;

// Define start and end points
float xa = 0.0;
float ya = 0.0;
float za = 0.0;
float xb = 0.0;
float yb = 0.0;
float zb = 0.0;


//Define error and error_dot
float e_x = 0.0;
float e_y = 0.0;
float e_z = 0.0;
float e_dot_x = 0.0;
float e_dot_y = 0.0;
float e_dot_z = 0.0;

// Desired force on z axis to press egg
float Fzcmd = -7.5;
float Kt = 6.0;


// Critical points
// Task 1: surface of the hole:  0.039 0.351 0.199
float task1_x = 0.037;
float task1_y = 0.348;
float task1_z = 0.199;
// Task 2: entry of zig zag: 0.383 0.094 0.231
float task2_x = 0.383;
float task2_y = 0.094;
float task2_z = 0.220;
// Task 3: top of the egg: 0.191 0.167 0.311
float task3_x = 0.203;
float task3_y = 0.181;
float task3_z = 0.3125;

int mode = 0;
float speed = 0.1;

typedef struct point_tags{
    float x;
    float y;
    float z;
    float thz;
    int mode;
}point;
int state = 1; //totally 23 states, refer to different path details. point 1-2 is state 1, point 2-3 is state 2 ...

// This function is called every 1 ms
void lab(float theta1motor,float theta2motor,float theta3motor,float *tau1,float *tau2,float *tau3, int error) {
#define num_points 23 //number of total points
point array_points[num_points] = {                   //x, y, z, thetaz, mode
    {   0.138, 0.000, 0.432, 0.0, 0},               //state  1 - point 1: home
    {0.254, 0.000, 0.508, 0.0, 0},                  //state  2 - point 2: rest position
    // Task 1: Insert into a hole
    {task1_x, task1_y, task1_z + 0.3, 0.0, 0},      //state  3 - point 2.1: above the hole
    {task1_x, task1_y, task1_z, 0.0, 1},            //state  4 - point 3: surface of the hole
    {task1_x, task1_y, task1_z - 0.065, 0.0, 2},    //state  5 - point 4: insert 2 inches into the hole
    {task1_x, task1_y, task1_z - 0.065, 0.0, 1},    //state  6 - point 4: wait for 1 second at point 4
    {task1_x, task1_y, task1_z, 0.0, 0},            //state  7 - point 3: go back to surface of the hole
    {task1_x, task1_y, task1_z + 0.3, 0.0, 0},      //state  8 - point 2.1: go back above the hole
    {0.030, 0.104, 0.494, 0.0, 0},                  //state  9 - point 4.1: reduce y from point point 2.1
    {0.363, 0.104, 0.494, 0.0, 1},                  //state 10 - point 4.2: increase x from point 4.1 and above the zigzag

    // Task 2: Zig Zag
    {task2_x, task2_y, task2_z, -(90 - 36.87)/180*PI, 3},                       //state 11 - point 5: entry of zig-zag
    {task2_x + 0.022, task2_y - 0.050, task2_z, -(90 + 34.065)/180*PI, 3},      //state 12 - point 5.1: first line segment of zigzag
    {task2_x + 0.01, task2_y - 0.067, task2_z, (180 - 15)/180*PI, 3},           //state 13 - point 5.2: first curve segment of zigzag
    {task2_x - 0.06, task2_y - 0.055, task2_z, -(90 + 34.065)/180*PI, 3},       //state 14 - point 6: second line segment of zigzag
    {task2_x - 0.069, task2_y - 0.073, task2_z,  -(90 - 36.87)/180*PI, 3},      //state 15 - point 6.1: second curve segment of zigzag
    {task2_x - 0.012, task2_y - 0.149, task2_z, 0.0, 0},                        //state 16 - point 7 :exit of zig zag
    {task2_x - 0.012, task2_y - 0.149, task2_z + 0.3, 0.0, 0},                  //state 17 - point 7.1: above point 7

    // Task 3: Desired force
    {task3_x, task3_y, task3_z + 0.2, 0.0, 4},      //state 18 - point 8: above the egg
    {task3_x, task3_y, task3_z, 0.0, 4},            //state 19 - point 8.1: move down to touch the egg
    {task3_x, task3_y, task3_z - 0.019, 0.0, 5},    //state 20 - point 9: press on egg
    {task3_x, task3_y, task3_z - 0.019, 0.0, 4},    //state 21 - point 9: press on egg for 2s
    {task3_x, task3_y, task3_z + 0.2, 0.0, 0},      //state 22 - point 8: back above the egg
    {0.254, 0.000, 0.508, 0.0, 5},                  //state 23 - point 10: thetamotor = 0

};
// mode = 0: speed = 0.5
// mode = 1: speed = 0.1, weak x y
// mode = 2: wait for 1 second, weak x y
// mode = 3: speed = 0.05, weak y, slightly weak x
// mode = 4: speed = 0.05
// mode = 5: wait for 2 seconds

    // Total time
    t = mycount * 0.001;

    //Final project calculate trajectory ##########
    if (state < num_points){
        // Set start points
        x_start = array_points[state-1].x;
        y_start = array_points[state-1].y;
        z_start = array_points[state-1].z;
        // Set destination points
        x_destination = array_points[state].x;
        y_destination = array_points[state].y;
        z_destination = array_points[state].z;
    } else { // Set start and end point
        state = num_points;
        // Set start points
        x_start = array_points[state-1].x;
        y_start = array_points[state-1].y;
        z_start = array_points[state-1].z;
        // Set destination points
        x_destination = array_points[state-1].x;
        y_destination = array_points[state-1].y;
        z_destination = array_points[state-1].z;
    }
    // Set theta z and mode
    thetaz = array_points[state-1].thz;
    mode = array_points[state-1].mode;
    // mode = 0: speed = 0.5
    // mode = 1: speed = 0.1, weak x y
    // mode = 2: wait for 1 second, weak x y
    // mode = 3: speed = 0.05, weak y, slightly weak x
    // mode = 4: speed = 0.05
    // mode = 5: wait for 2 seconds
    if(mode == 0){
        speed = 0.2;
        t_total = sqrt((x_destination - x_start)*(x_destination - x_start) + (y_destination - y_start)*(y_destination - y_start)+(z_destination - z_start)*(z_destination - z_start))/speed;
    } else if(mode == 1){
        speed = 0.2;
        t_total = sqrt((x_destination - x_start)*(x_destination - x_start) + (y_destination - y_start)*(y_destination - y_start)+(z_destination - z_start)*(z_destination - z_start))/speed;
    } else if (mode == 2){
        speed = 0;
        t_total = 1;
    } else if (mode == 3){
        speed = 0.05;
        t_total = sqrt((x_destination - x_start)*(x_destination - x_start) + (y_destination - y_start)*(y_destination - y_start)+(z_destination - z_start)*(z_destination - z_start))/speed;
    } else if (mode == 4){
        speed = 0.05;
        t_total = sqrt((x_destination - x_start)*(x_destination - x_start) + (y_destination - y_start)*(y_destination - y_start)+(z_destination - z_start)*(z_destination - z_start))/speed;
    } else {
        speed = 0;
        t_total = 2;
    }

    //Set t_start and state
    if (t - t_start >= t_total){
        t_start = t;
        state = state + 1;  //state will add 1 if the robot finish the current path
    }

    x_desired = (x_destination-x_start)/t_total*(t-t_start) + x_start;
    y_desired = (y_destination-y_start)/t_total*(t-t_start) + y_start;
    z_desired = (z_destination-z_start)/t_total*(t-t_start) + z_start;
    x_dot_desired = (x_destination-x_start)/t_total;
    y_dot_desired = (y_destination-y_start)/t_total;
    z_dot_desired = (z_destination-z_start)/t_total;

    // End of trajectory setting ##########


    // save 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
    }


    printtheta1motor = theta1motor;
    printtheta2motor = theta2motor;
    printtheta3motor = theta3motor;

    //Velocity filter #########
    Omega1raw = (theta1motor - Theta1_old)/0.001;
    Omega1 = (theta1motor - Theta1_old)/0.001;
    Omega1 = (Omega1 + Omega1_old1 + Omega1_old2)/3.0;
    Theta1_old = theta1motor;
    //order matters here. Why??
    Omega1_old2 = Omega1_old1;
    Omega1_old1 = Omega1;
    //Omega1_old1 = Omega1raw;

    Omega2raw = (theta2motor - Theta2_old)/0.001;
    Omega2 = (theta2motor - Theta2_old)/0.001;
    Omega2 = (Omega2 + Omega2_old1 + Omega2_old2)/3.0;
    Theta2_old = theta2motor;
    //order matters here. Why??
    Omega2_old2 = Omega2_old1;
    Omega2_old1 = Omega2;
    //Omega2_old1 = Omega2raw;

    Omega3raw = (theta3motor - Theta3_old)/0.001;
    Omega3 = (theta3motor - Theta3_old)/0.001;
    Omega3 = (Omega3 + Omega3_old1 + Omega3_old2)/3.0;
    Theta3_old = theta3motor;
    //order matters here. Why??
    Omega3_old2 = Omega3_old1;
    Omega3_old1 = Omega3;
    //Omega3_old1 = Omega3raw;
    // End of velocity filter #########


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

    //Rotation zxy and its Transpose
    cosz = cos(thetaz);
    sinz = sin(thetaz);
    cosx = cos(thetax);
    sinx = sin(thetax);
    cosy = cos(thetay);
    siny = sin(thetay);

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

    // Calculate real x y z
    // Forward kinematic
    x_real = 0.254*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor));
    y_real = 0.254*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor));
    z_real = 0.254*(1 + cos(theta2motor) - sin(theta3motor));

    x_dot_real = (x_real - x_real_past1)/0.001;
    x_dot_real = (x_dot_real + x_dot_real_past1 + x_dot_real_past2)/3.0;
    y_dot_real = (y_real - y_real_past1)/0.001;
    y_dot_real = (y_dot_real + y_dot_real_past1 + y_dot_real_past2)/3.0;
    z_dot_real = (z_real - z_real_past1)/0.001;
    z_dot_real = (z_dot_real + z_dot_real_past1 + z_dot_real_past2)/3.0;

    // Calculate error and error_dot
    e_x = x_desired - x_real;
    e_y = y_desired - y_real;
    e_z = z_desired - z_real;

    e_dot_x = (x_dot_desired - x_dot_real);
    e_dot_y = (y_dot_desired - y_dot_real);
    e_dot_z = (z_dot_desired - z_dot_real);

    //Set KP and Kd
    if (mode == 0 || mode == 4 || mode == 5) {
        float Kpxn = 500.0;
        float Kpyn = 500.0;
        float Kpzn = 500.0;
        float Kdxn = 25.0;
        float Kdyn = 25.0;
        float Kdzn = 25.0;
    } else if (mode == 1 || mode == 2) {
        float Kpxn = 10.0;
        float Kpyn = 10.0;
        float Kpzn = 500.0;
        float Kdxn = 1.0;
        float Kdyn = 1.0;
        float Kdzn = 25.0;
    } else if (mode == 3) {
        float Kpxn = 150.0;
        float Kpyn = 50.0;
        float Kpzn = 100.0;
        float Kdxn = 5.0;
        float Kdyn = 1.0;
        float Kdzn = 5.0;
    }

    // Set torque
    //if ((x_real >= task3_x-0.01 && x_real <= task3_x+0.01) && (y_real >= task3_y-0.01 && y_real <= task3_y+0.01) && (z_real-task3_z <=0)){
    if (state == 20){ //press on egg, z-axis force = Fzcmd
        *tau1 = JT_11 * (Kpx * e_x + Kdx * e_dot_x) + JT_12 * (Kpy*e_y + Kdy*e_dot_y) + JT_13 * (Kpz*e_z + Kdz*e_dot_z) + JT_13 * Fzcmd/Kt;
        *tau2 = JT_21 * (Kpx * e_x + Kdx * e_dot_x) + JT_22 * (Kpy*e_y + Kdy*e_dot_y) + JT_23 * (Kpz*e_z + Kdz*e_dot_z) + JT_23 * Fzcmd/Kt;
        *tau3 = JT_31 * (Kpx * e_x + Kdx * e_dot_x) + JT_32 * (Kpy*e_y + Kdy*e_dot_y) + JT_33 * (Kpz*e_z + Kdz*e_dot_z) + JT_33 * Fzcmd/Kt;
    } else { // Common controller
        *tau1 = (JT_11*R11 + JT_12*R21 + JT_13*R31)*(Kdxn*RT11*e_dot_x + Kdxn*RT12*e_dot_y + Kdxn*RT13*e_dot_z + Kpxn*RT11*e_x + Kpxn*RT12*e_y + Kpxn*RT13*e_z) + (JT_11*R12 + JT_12*R22 + JT_13*R32)*(Kdyn*RT21*e_dot_x + Kdyn*RT22*e_dot_y + Kdyn*RT23*e_dot_z + Kpyn*RT21*e_x + Kpyn*RT22*e_y + Kpyn*RT23*e_z) + (JT_11*R13 + JT_12*R23 + JT_13*R33)*(Kdzn*RT31*e_dot_x + Kdzn*RT32*e_dot_y + Kdzn*RT33*e_dot_z + Kpzn*RT31*e_x + Kpzn*RT32*e_y + Kpzn*RT33*e_z);
        *tau2 = (JT_21*R11 + JT_22*R21 + JT_23*R31)*(Kdxn*RT11*e_dot_x + Kdxn*RT12*e_dot_y + Kdxn*RT13*e_dot_z + Kpxn*RT11*e_x + Kpxn*RT12*e_y + Kpxn*RT13*e_z) + (JT_21*R12 + JT_22*R22 + JT_23*R32)*(Kdyn*RT21*e_dot_x + Kdyn*RT22*e_dot_y + Kdyn*RT23*e_dot_z + Kpyn*RT21*e_x + Kpyn*RT22*e_y + Kpyn*RT23*e_z) + (JT_21*R13 + JT_22*R23 + JT_23*R33)*(Kdzn*RT31*e_dot_x + Kdzn*RT32*e_dot_y + Kdzn*RT33*e_dot_z + Kpzn*RT31*e_x + Kpzn*RT32*e_y + Kpzn*RT33*e_z);
        *tau3 = (JT_31*R11 + JT_32*R21 + JT_33*R31)*(Kdxn*RT11*e_dot_x + Kdxn*RT12*e_dot_y + Kdxn*RT13*e_dot_z + Kpxn*RT11*e_x + Kpxn*RT12*e_y + Kpxn*RT13*e_z) + (JT_31*R12 + JT_32*R22 + JT_33*R32)*(Kdyn*RT21*e_dot_x + Kdyn*RT22*e_dot_y + Kdyn*RT23*e_dot_z + Kpyn*RT21*e_x + Kpyn*RT22*e_y + Kpyn*RT23*e_z) + (JT_31*R13 + JT_32*R23 + JT_33*R33)*(Kdzn*RT31*e_dot_x + Kdzn*RT32*e_dot_y + Kdzn*RT33*e_dot_z + Kpzn*RT31*e_x + Kpzn*RT32*e_y + Kpzn*RT33*e_z);

    }
    //set ufric
    if(Omega1 > 0.1){
        u_fric1 = pos_Viscous1 * Omega1 + pos_Coulomb1;
    }else if(Omega1 < -0.1){
        u_fric1 = neg_Viscous1 * Omega1 + neg_Coulomb1;
    }else{
        u_fric1 = 3.6*Omega1;
    }
    if(Omega2 > 0.05){
        u_fric2 = pos_Viscous2 * Omega2 + pos_Coulomb2;
    }else if(Omega2 < -0.05){
        u_fric2 = neg_Viscous2 * Omega2 + neg_Coulomb2;
    }else{
        u_fric2 = 3.6*Omega2;
    }
    if(Omega3 > 0.05){
        u_fric3 = pos_Viscous3 * Omega3 + pos_Coulomb3;
    }else if(Omega3 < -0.05){
        u_fric3 = neg_Viscous3 * Omega3 + neg_Coulomb3;
    }else{
        u_fric3 = 3.6*Omega3;
    }

    *tau1 = *tau1 + u_fric1*fric_coef;
    *tau2 = *tau2 + u_fric2*fric_coef;
    *tau3 = *tau3 + u_fric3*fric_coef;

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

    Simulink_PlotVar1 = theta2_desired;
    Simulink_PlotVar2 = theta2motor;
    Simulink_PlotVar3 = theta3_desired;
    Simulink_PlotVar4 = theta3motor;

    mycount++;

    //Assign current value to past
    x_dot_real_past2 = x_dot_real_past1;
    y_dot_real_past2 = y_dot_real_past1;
    z_dot_real_past2 = z_dot_real_past1;

    x_dot_real_past1 = x_dot_real;
    y_dot_real_past1 = y_dot_real;
    z_dot_real_past1 = z_dot_real;

    x_real_past1 = x_real;
    y_real_past1 = y_real;
    z_real_past1 = z_real;
}

void printing(void){
    serial_printf(&SerialA, "Theta: %.2f %.2f %.2f Position: %.3f %.3f %.3f Desired: %.3f %.3f %.3f \n\r",printtheta1motor*180/PI,printtheta2motor*180/PI,printtheta3motor*180/PI, x_real, y_real, z_real, x_desired, y_desired, z_desired);
}