Published May 12, 2023

ME 446 Final Project

This project details the movement of an elbow manipulator through a number of specified tasks with certain conditions that must be met.

IntermediateFull instructions provided5 hours37

Things used in this project

Software apps and online services

Texas Instruments Code Composer Studio

Story

Introduction

This is the ME446 Final Project where we have to successfully complete the following points. By Leotis Davenport and Jonathan Chang

Visual Representation

Controller

We used the impedance controller from lab 4, shown below, for the entirety of the project, with some altered Kp and Kd gain values depending on which part of the process we are on.

Controller

Altering the gains provided less rigidness of the robot and more leeway for the peg to go through tight spaces. We generated an array of coordinates by measuring with Tera Term that we would set as points A and B for the straight-line trajectory we created in Lab 4.

Straight Line Trajectory

As the lab function loops, we had an iterator that would increment when each trajectory is finished and would then move on to the next point in the array to follow.

Hole

For putting the peg in the hole, we simply created two waypoints, one above the z-coordinates of the hole, and one inside the hole. To make the peg stay in one position for a predefined amount of time, we looked at how our line trajectory code is made. The code looks at the euclidean distance and the speed of the arm to calculate how long the trajectory from point A to B is going to take (time = distance/speed). This amount of time is checked by a counter in the lab function. By setting the time to our preferred amount of time instead of the equation above, and making point A and B the same point, we can trick the code to think that it takes our preferred amount of time to stay in the same spot. Regarding the Kp and Kd gains, the gains in both the x and y directions are weakened to allow the peg to slide in the hole if the placement of the peg was inaccurate (likely due to the initial position of the elbow manipulator not being set properly).

Egg

The egg part was straightforward like the peg in the hole part. We found the coordinates at the egg, and we decreased the z-coordinates until we were within specifications on the scale. We also slowed the speed of it moving down towards the egg in case the egg is more fragile than we expect. To hold it for 2 seconds, we implemented the same trick as mentioned above with the hole.

ZigZag

The most challenging part of the project was getting the end effector to move through the zig zag pattern. We first attempted the zig zag with a 3-waypoint trajectory. We thought with a low enough Kp and Kd gain, there would be enough flexibility in the robot to go through. Most of the time, the end effector got stuck on the bends in the zig zag, stopping the elbow manipulator. This is because it was difficult to determine the correct rotated gains to apply to the impedance controller to have the end effector weak in the necessary directions. As an alternative, we opted to define multiple waypoints with a shorter distance in-between within the zig zag. The shorter distance in-between waypoints made it easier for the end effector to navigate through the zig zag without the issue of getting caught around the bends of the zig zag.

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
#define NUMPOINTS   30
#define zigzagZ      0.199

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

// Your global varialbes.

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

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

#pragma DATA_SECTION(foo, ".my_vars")
float foo;

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

#pragma DATA_SECTION(printer, ".my_vars")
float printer = 6.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;
float Simulink_PlotVar5 = 0;
float Simulink_PlotVar6 = 0;

long mycount = 0;

//VARIABLES ABOVE ARE GIVEN TO US

//a multiplier to use to alter the amount of friction compensation we want to use
float multiplier = 0;

//End effector coordinates.
float x=0;
float y=0;
float z=0;

//desired theta positions
float th1des = 0;
float th2des = 0;
float th3des = 0;

//difference between theta motor positions and desired theta positions
float errorth1m = 0;
float errorth2m = 0;
float errorth3m = 0;

//theta values from the previous iteration (mycount - 1)
float th1mOld = 0;
float th2mOld = 0;
float th3mOld = 0;

//the velocity values, calculated by distance/(time in seconds) aka (theta motor value - theta motor Old)/0.001
float th1mdotraw = 0;
float th2mdotraw = 0;
float th3mdotraw = 0;

//velocity values from the previous iteration (mycount - 1)
float th1mOld1dot = 0;
float th2mOld1dot = 0;
float th3mOld1dot = 0;

//velocity values from the second previous iteration (mycount - 2)
float th1mOld2dot = 0;
float th2mOld2dot = 0;
float th3mOld2dot = 0;

//velocity values filtered by averaging the current raw velocity, previous velocity, and second previous velocity (thmdotraw + thmOlddot + thmOld2dot)/3.0
float th1mdotfilt = 0;
float th2mdotfilt = 0;
float th3mdotfilt = 0;

//temporary value to assign the calculate taus to, to prevent tau values from exceeding 5 at all times
float tau1_temp, tau2_temp, tau3_temp = 0.0;

// GIVEN JACOBIAN CODE FROM LAB 4 MANUAL
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;

//x averaged position, x values from previous iterations, etc
float xdot = 0;
float xfilter = 0;
float xold = 0;
float xdotold2 = 0;
float xdotold1 = 0;

//y averaged position, y values from previous iterations, etc
float ydot = 0;
float yfilter = 0;
float yold = 0;
float ydotold2 = 0;
float ydotold1 = 0;

//z averaged position, z values from previous iterations, etc
float zdot = 0;
float zfilter = 0;
float zold = 0;
float zdotold2 = 0;
float zdotold1 = 0;

//Proportional gain
float kpx = 500;
float kpy = 500;
float kpz = 500;

//derivaive control gain
float kdx = 25;
float kdy = 25;
float kdz = 25;

//Force values in world frame
float Fxw = 0;
float Fyw = 0;
float Fzw = 0;

// Force values in N frame
float FNx = 0;
float FNy = 0;
float FNz = 0;

//friction compensation coefficients
float u_fric1 = 0;
float u_fric2 = 0;
float u_fric3 = 0;

float visP1 = 0.26;
float visP2 = 0.23;
float visP3 = 0.1922;

float visN1 = 0.27;
float visN2 = 0.237;
float visN3 = 0.2132;

float CouP1 = 0.37;
float CouP2 = 0.4759;
float CouP3 = 0.5339;

float CouN1 = -0.31;
float CouN2 = -0.5031;
float CouN3 = -0.519;

float slopeBetweenMin1 = 3.6;
float slopeBetweenMin2 = 2;
float slopeBetweenMin3 = 2;

float min1, min2, min3 = 0;

//speed of trajectory
float speed = 0.15;

//difference in x,y,z from point a to b
float deltax = 0;
float deltay = 0;
float deltaz = 0;

//total time to go through path
float ttotal = 0;

//start time
float tstart = 0;

//desired x,y,z positions
float xdes = 0;
float ydes = 0;
float zdes = 0;

//desired x,y,z dot positions
float xdotdes = 0;
float ydotdes = 0;
float zdotdes = 0;

float Fx = 0;
float Fy = 0;
float Fz = 0;

float FzCmd = 0;

coordinates expressed in (x,y,z)
//above the hole is (0.03 , 0.35, 0.25)
//the hole is (0.03, 0.35, 0.13)

//stores time in ms
float t = 0;

//initializing coordinates a and b, values here are irrelevant for the final lab
float xa,ya,za = 0;
float xb,yb,zb = 0;

//a struct that contains relevant information for the waypoint coordinates we will be constructing
//floats x, y, z represent the coordinates at each waypoint. Mode is an int that triggers certain if
//statements in the lab function for us to instruct the ARM to move at a certain speed or change
//the axis orientation or change the gains for the controller.
typedef struct point_array{
    float x;
    float y;
    float z;
    int mode;
}points;

//mode = 0 --> default controller gains (slightly rigid, 0.15 speed, 0 degree axis orientation
//mode = 1,2,3,4 --> rotate about z axis by differing degrees for the zigzag, and make it more wiggly
//mode = 5,6,9 --> changes the amount of time the trajectory takes, more explained below
//mode = 7,8,10,11,12 --> changes the speed by differing amounts, 
//applied in a gradient so the robot doesnt jump when it goes from speed = 0 to speed > 0

points mypoints[NUMPOINTS] = {{0.15 , 0 , 0.43, 0} , //start hole
                              {0.03, 0.35, 0.25, 0} ,
                              {0.03, 0.35, 0.15, 12},
                              {0.03, 0.35, 0.14, 11},
                              {0.03, 0.35, 0.13, 10},
                              {0.03, 0.35, 0.12, 6},
                              {0.03, 0.35, 0.15, 0},
                              {0.03, 0.35, 0.18, 0},
                              {0.03 , 0.35, 0.37 , 0},
                              {0.29, 0.13, 0.37,0},  //hole ends
                              {0.38,0.1,0.20, 0},  //front of zigzag

                              {0.39, 0.09, zigzagZ, 0},
                              {0.4, 0.08, zigzagZ, 0},
                              {0.41, 0.07, zigzagZ, 0},
                              {0.41,0.06,zigzagZ,0},
                              {0.41, 0.05, zigzagZ, 0},
                              {0.4, 0.05, zigzagZ, 0},
                              {0.38,0.05, zigzagZ, 0},
                              {0.34,0.06,zigzagZ, 0},
                              {0.33, 0.05, zigzagZ,0},
                              {0.32,0.04,zigzagZ,0},
                              {0.32,0.03,zigzagZ,0},
                              {0.38, -0.04, zigzagZ, 0},
                              {0.38, -0.04, zigzagZ, 0},
                               {0.38, -0.04, 0.31, 0},

                               {0.24, 0.19, 0.31,0}, //above egg
                                {0.24, 0.18, 0.3, 7}, //push
                                {0.24,0.18 ,0.285, 7}, //stay
                                {0.24,0.18 ,0.281, 5}, //stay
                                {0.24,0.18 ,0.3, 8}, //stay
                                {0.24, 0.19, 0.31,0}, //above again

                                {0.254, 0, 0.508, 9} //back home
                                };

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

    if(i < NUMPOINTS){
    if(mypoints[i].mode == 0){
        thetaz = 0;
        thetax = 0;
        thetay = 0;
        kpx = 400;
        kpy = 400;
        kpz = 1000;
        kdx = 25;
        kdy = 25;
        kdz = 25;
        speed = 0.15;
    }
    else if(mypoints[i].mode == 1){
       thetaz = 36.9*PI/180;
        kpx = 150;
        kdx = 5;
        kpy = 450;
        kpy = 25;
    }
    else if(mypoints[i].mode == 2){
        thetaz = 0.643; //36 degrees
        kpx = 250;
        kdx = 1;
        kpy = 250;
        kdy = 1;
    }
    else if(mypoints[i].mode == 3){
            thetaz = -0.262; //-15 degrees
            kpy = 0;
            kpx = 250;
            kdx = 25;
            kpy = 250;
            kdy = 1;
        }
    else if(mypoints[i].mode == 4){
               thetaz = -0.927;
               kpx = 250;
               kdx = 25;
               kpy = 250;
               kdy = 1;
           }
    else if(mypoints[i].mode == 7){
            speed = 0.15;
           }
    else if(mypoints[i].mode == 8){
            speed = 0.1;
           }
    else if(mypoints[i].mode == 10){
               speed = 0.1;
              }
    else if(mypoints[i].mode == 11){
               speed = 0.3;
              }
    else if(mypoints[i].mode == 12){
               speed = 0.5;
              }

    }
    
    /*
    this segment of code calculates and sets the desired x,y,z coordinates for the 
    straight line following trajectory, it also chooses the direction of which it is moving in
    */
    t = mycount*0.001; // saves time in seconds 

    /*determines whether the path is going from a to b, or b to a
    once the trajectory reaches to one end point, the if statement is triggered as 
    the value in t - tstart will exceed the total amount of time it takes for the path to happen.
    In the if statement, xa is set to xb and xb is set to xa. This means that xa does not neccessarily 
    correspond to point a, and same for xb and point b. xa and xb are just variables to signify 
    the start point from one end going to the other point. This is the same for the y and z cooresponding variables.
    */

    if(i != 29){
        xa = mypoints[i].x;
        ya = mypoints[i].y;
        za = mypoints[i].z;

        xb = mypoints[i+1].x;
        yb = mypoints[i+1].y;
        zb = mypoints[i+1].z;
    }
    else{
        xa = mypoints[NUMPOINTS-1].x;
        ya = mypoints[NUMPOINTS-1].y;
        za = mypoints[NUMPOINTS-1].z;

        xb = mypoints[NUMPOINTS-1].x;
        yb = mypoints[NUMPOINTS-1].y;
        zb = mypoints[NUMPOINTS-1].z;
    }

    //find the difference between the points
    deltax = (xb - xa);
    deltay = (yb - ya);
    deltaz = (zb - za);

    //finds the total time using distance/speed formula
    ttotal = sqrt(deltax * deltax + deltay * deltay + deltaz * deltaz)/speed;

    
    //depending on the mode the ttotal time is set to a predefined amount of seconds.
    //This is done so that we can halt the robot arm where we need to by setting points a
    // and b to the same coordinates, and have it stay for ttotal seconds.
    if(mypoints[i].mode == 5){
        ttotal = 2;
    }
    else if(mypoints[i].mode == 6){
        ttotal = 0.5;
    }
    else if(mypoints[i].mode == 9){
        ttotal = 100;
    }
    else{
    //given formula from lab manual to set the desired x,y,z positions
    xdes = deltax*(t-tstart)/ttotal + xa;
    ydes = deltay*(t-tstart)/ttotal + ya;
    zdes = deltaz*(t-tstart)/ttotal + za;

    xdotdes = deltax/ttotal;
    ydotdes = deltay/ttotal;
    zdotdes = deltaz/ttotal;
    }

    if(t - tstart >= ttotal){
        if(i < NUMPOINTS){
            i++;
            tstart = t;
        }
    }

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

    //motor thetas found using forward kinematics with matlab
    
    x = (127.0*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor)))/500.0;

    y = (127.0*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor)))/500.0;

    z = (127.0*cos(theta2motor))/500.0 - (127.0*sin(theta3motor))/500.0 + 127/500.0;

    /*
    This segment of code calculates values that are used to find the friction compensation coefficents
    */
    
    //finds the error for each theta motor
//    errorth1m = th1des - theta1motor;
//    errorth2m = th2des - theta2motor;
//    errorth3m = th3des - theta3motor;

    //to find the velocity, we do distance/time 
    //the raw value is the (current theta value minus the previous theta value) / 1ms
    th1mdotraw = (theta1motor - th1mOld)/0.001;
    th2mdotraw = (theta2motor - th2mOld)/0.001;
    th3mdotraw = (theta3motor - th3mOld)/0.001;

    //to find the filtered velocity, we averaged the raw velocity, and filtered velocities from the previous 2 iterations
    th1mdotfilt = (th1mdotraw + th1mOld1dot + th1mOld2dot)/3.0;
    th2mdotfilt = (th2mdotraw + th2mOld1dot + th2mOld2dot)/3.0;
    th3mdotfilt = (th3mdotraw + th3mOld1dot + th3mOld2dot)/3.0;

    //update the values to accordingly for the next iteration
    //the current theta motor value becomes the previous one, and so on
    //order matters here because an incorrect order will overwrite the actual values we want to represent 
    th1mOld = theta1motor;
    th1mOld2dot = th1mOld1dot;
    th1mOld1dot = th1mdotfilt;

    th2mOld = theta2motor;
    th2mOld2dot = th2mOld1dot;
    th2mOld1dot = th2mdotfilt;

    th3mOld = theta3motor;
    th3mOld2dot = th3mOld1dot;
    th3mOld1dot = th3mdotfilt;

    //lab 3 fric part 1

    /* this segment of code follows the formula given in the lab manual to calculate the friction compensation coefficents
    for each theta value. The viscous and coulomb values are manually tuned to better suit our CRS robot arm after we are given generalized
    coefficients in the lab 3 manual 
    */
    
    //These follow the pseudocode given from lab 3
    // when the filtered velocity is greater than the threshold it uses the positive viscous and
    // coulomb coefficents, when it is less than the threshold it follows the negative values. The threshold
    // value was given to us after they recorded the friction values from an actual CRS robot arm, and a line of best fit
    //implemented.
    //when the velocity is between the threshold value we have a 3rd equation used

    //theta1
    if(th1mdotfilt > min1){
        u_fric1 = visP1 * th1mdotfilt + CouP1;
    }
    else if(th1mdotfilt < -min1){
        u_fric1 = visN1 * th1mdotfilt + CouN1;
    }
    else{
        u_fric1 = slopeBetweenMin1 * th1mdotfilt;
    }

    //theta2
    if(th2mdotfilt > min2){
        u_fric2 = visP2 * th2mdotfilt + CouP2;
    }
    else if(th2mdotfilt < -min2){
        u_fric2 = visN2 * th2mdotfilt + CouN2;
    }
    else{
        u_fric2 = slopeBetweenMin2 * th2mdotfilt;
    }

    //theta3
    if(th3mdotfilt > min3){
        u_fric3 = visP3 * th3mdotfilt + CouP3;
    }
    else if(th3mdotfilt < -min3){
        u_fric3 = visN3 * th3mdotfilt + CouN3;
    }
    else{
        u_fric3 = slopeBetweenMin3 * th3mdotfilt;
    }
    
    //Lab 4

    /* this segment of code is similar to what we saw earlier above. This time instead
    of focusing on the angular velocity based off the theta angles, we are looking at the velocity 
    based off each axis coordinate difference from the a-b path
    */
      //x
      xdot = (x - xold)/0.001;
      xfilter = (xdot + xdotold1 + xdotold2)/3.0;
      xold = x;
      xdotold2 = xdotold1;
      xdotold1 = xdot;

      //y
      ydot = (y - yold)/0.001;
      yfilter = (ydot + ydotold1 + ydotold2)/3.0;
      yold = y;
      ydotold2 = ydotold1;
      ydotold1 = ydot;

      //z
      zdot = (z - zold)/0.001;
      zfilter = (zdot + zdotold1 + zdotold2)/3.0;
      zold = z;
      zdotold2 = zdotold1;
      zdotold1 = zdot;
    
    /* The force formulas in the x, y, z direction
    in part 1 of the lab we implemented task space control laws with friction compensation. These values allowed the arm to stay rigid 
    at a point of our choosing. Any external forces applied, by our hands for example,
    did not budge the end effector. these force values are used to calculate tau after being multiplied with the jacobian transpose, given from the manual
    In part 2 of the lab, depending on which kp and kd values we change, we could make the CRS robot arm rigid
    in some directions but malleable in others. We specifically made the Z axis weak, and X and Y strong. This way gravity was heavily affecting
    the CRS robot arm. From there we tinkered a force value called FzCmd to counteract the gravity component.
    */
      Fx = kpx*(xdes - x) + kdx*(xdotdes - xdot);
      Fy = kpy*(ydes - y) + kdy*(ydotdes - ydot);
      Fz = kpz*(zdes - z) + kdz*(zdotdes - zdot);

      //part 2 tau
     //tau1_temp = JT_11*Fx + JT_12*Fy + JT_13*Fz + multiplier* u_fric1 + JT_13 * FzCmd / 6.0;
     //tau2_temp = JT_21*Fx + JT_22*Fy + JT_23*Fz + multiplier* u_fric2 + JT_23 * FzCmd / 6.0;
     //tau3_temp = JT_31*Fx + JT_32*Fy + JT_33*Fz + multiplier* u_fric3 + JT_33 * FzCmd / 6.0;
    
      //part 3 tau
    
    /* the manual provided a formula for calculating tau similar to what we had in part 2. The major difference is that we wanted
    the tau values to be in another frame of reference. We wanted to change from the world 
    frame to the frame where the axis is "weak". To do so, we applied rotation matrices in Matlab to the
    formula from part 2, simplified them, and pasted them here
    */
      tau1_temp = - (JT_11*R11 + JT_12*R21 + JT_13*R31)*(RT11*kpx*(x - xdes) + RT11*kdx*(xdot - xdotdes) + RT12*kpx*(y - ydes) + RT12*kdx*(ydot - ydotdes) + RT13*kpx*(z - zdes) + RT13*kdx*(zdot - zdotdes)) - (JT_11*R12 + JT_12*R22 + JT_13*R32)*(RT21*kpy*(x - xdes) + RT21*kdy*(xdot - xdotdes) + RT22*kpy*(y - ydes) + RT22*kdy*(ydot - ydotdes) + RT23*kpy*(z - zdes) + RT23*kdy*(zdot - zdotdes)) - (JT_11*R13 + JT_12*R23 + JT_13*R33)*(RT31*kpz*(x - xdes) + RT31*kdz*(xdot - xdotdes) + RT32*kpz*(y - ydes) + RT32*kdz*(ydot - ydotdes) + RT33*kpz*(z - zdes) + RT33*kdz*(zdot - zdotdes)) + JT_13 * FzCmd / 6.0;

      tau2_temp = - (JT_21*R11 + JT_22*R21 + JT_23*R31)*(RT11*kpx*(x - xdes) + RT11*kdx*(xdot - xdotdes) + RT12*kpx*(y - ydes) + RT12*kdx*(ydot - ydotdes) + RT13*kpx*(z - zdes) + RT13*kdx*(zdot - zdotdes)) - (JT_21*R12 + JT_22*R22 + JT_23*R32)*(RT21*kpy*(x - xdes) + RT21*kdy*(xdot - xdotdes) + RT22*kpy*(y - ydes) + RT22*kdy*(ydot - ydotdes) + RT23*kpy*(z - zdes) + RT23*kdy*(zdot - zdotdes)) - (JT_21*R13 + JT_22*R23 + JT_23*R33)*(RT31*kpz*(x - xdes) + RT31*kdz*(xdot - xdotdes) + RT32*kpz*(y - ydes) + RT32*kdz*(ydot - ydotdes) + RT33*kpz*(z - zdes) + RT33*kdz*(zdot - zdotdes)) + JT_13 * FzCmd / 6.0;

      tau3_temp = - (JT_31*R11 + JT_32*R21 + JT_33*R31)*(RT11*kpx*(x - xdes) + RT11*kdx*(xdot - xdotdes) + RT12*kpx*(y - ydes) + RT12*kdx*(ydot - ydotdes) + RT13*kpx*(z - zdes) + RT13*kdx*(zdot - zdotdes)) - (JT_31*R12 + JT_32*R22 + JT_33*R32)*(RT21*kpy*(x - xdes) + RT21*kdy*(xdot - xdotdes) + RT22*kpy*(y - ydes) + RT22*kdy*(ydot - ydotdes) + RT23*kpy*(z - zdes) + RT23*kdy*(zdot - zdotdes)) - (JT_31*R13 + JT_32*R23 + JT_33*R33)*(RT31*kpz*(x - xdes) + RT31*kdz*(xdot - xdotdes) + RT32*kpz*(y - ydes) + RT32*kdz*(ydot - ydotdes) + RT33*kpz*(z - zdes) + RT33*kdz*(zdot - zdotdes)) + JT_13 * FzCmd / 6.0;

    //Motor torque limitation(Max: 5 Min: -5)
    if(tau1_temp > 5){
        tau1_temp = 4.9;
    }
    if(tau2_temp > 5){
        tau2_temp = 4.9;
    }
    if(tau3_temp > 5){
        tau3_temp = 4.9;
    }
    if(tau1_temp < -5){
        tau1_temp = -4.9;
    }
    if(tau2_temp < -5){
        tau2_temp = -4.9;
    }
    if(tau3_temp < -5){
        tau3_temp = -4.9;
    }

    *tau1 = tau1_temp;
    *tau2 = tau2_temp;
    *tau3 = tau3_temp;

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

        theta1array[arrayindex] = theta1motor;
        theta2array[arrayindex] = theta2motor;
        if (arrayindex >= 100) {
            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;

    Simulink_PlotVar1 = th2des;
    Simulink_PlotVar2 = theta2motor;
    Simulink_PlotVar3 = th3des;
    Simulink_PlotVar4 = theta3motor;

    mycount++;
}

void printing(void){
    serial_printf(&SerialA, "%.2f %.2f,%.2f    %.4f %.4f,%.4f , %d  \n\r",printtheta1motor*180/PI,printtheta2motor*180/PI,printtheta3motor*180/PI,x,y,z , i);
    //serial_printf(&SerialA, "%.2f \n\r",printer);
}

Credits

Leotis Davenport

1 project • 0 followers

Embed the widget on your own site

ME 446 Final Project

ME 446 Final Project

Things used in this project

Software apps and online services

Story

Introduction

Controller

Hole

Egg

Code

Final Project Code

Credits

Leotis Davenport

Comments

Related channels and tags