Vasil IakimovitchCarlos Villanueva
Published

ME 446 Robotic Arm Project

We programmed a CRS robot arm to perform a demo using impedance control!

AdvancedShowcase (no instructions)130
ME 446 Robotic Arm Project

Things used in this project

Hardware components

CRS Robot Arm
×1

Software apps and online services

Code Composer Studio
Texas Instruments Code Composer Studio
MATLAB
MATLAB
Used for recording and plotting robot states.

Story

Read more

Code

LabFinal_vars.h

C Header File
/*
 * LabFinal_vars.h
 *
 * Authors: Carlos Villanueva and Vasil Iakimovitch
 * 
 * Abstract:
 * Variable declarations for LabFinal_VI.c
*/

#ifndef LAB_FINAL_VARS_H_
#define LAB_FINAL_VARS_H_

// Standard constants
#define PI          3.1415926535897932384626433832795
#define TWOPI       6.283185307179586476925286766559
#define HALFPI      1.5707963267948966192313216916398
#define GRAV        9.81

// Motor encoder offsets (determined experimentally)
const float offset_Enc2_rad = -25.6*PI/180;
const float offset_Enc3_rad = 11.40*PI/180;

// Print function variable
int UARTprint = 0;

// Timer counter
long mycount = 0;

// Simulink plotting variables
float Simulink_PlotVar1 = 0;
float Simulink_PlotVar2 = 0;
float Simulink_PlotVar3 = 0;
float Simulink_PlotVar4 = 0;

// Values to print
float printtheta1motor = 0;
float printtheta2motor = 0;
float printtheta3motor = 0;

float printX = 0;
float printY = 0;
float printZ = 0;

float printInvTheta1Motor = 0;
float printInvTheta2Motor = 0;
float printInvTheta3Motor = 0;

float printInvDHtheta1 = 0;
float printInvDHtheta2 = 0;
float printInvDHtheta3 = 0;

// Low pass filter variables
float Theta1_old = 0;
float Omega1_old1 = 0;
float Omega1_old2 = 0;
float Omega1 = 0;

float Theta2_old = -25.6*PI/180;
float Omega2_old1 = 0;
float Omega2_old2 = 0;
float Omega2 = 0;

float Theta3_old = 11.40*PI/180;
float Omega3_old1 = 0;
float Omega3_old2 = 0;
float Omega3 = 0;


// Friction compensation variables
float u_fricfac1 = .9; //0.5 to 0.9
float u_fric1 = 0;
float Vpos1 = 0.17;
float Cpos1 = 0.303;
float Vneg1 = 0.19;
float Cneg1 = -0.303;
float slope1 = 3;
float vmin1 = .1;

float u_fricfac2 = 0.9;
float u_fric2 = 0;
float Vpos2 = 0.13;
float Cpos2 = 0.330;
float Vneg2 = 0.18;
float Cneg2 = -0.330;
float slope2 = 2.5;
float vmin2 = .05;

float u_fricfac3 = 0.7;
float u_fric3 = 0;
float Vpos3 = 0.2;
float Cpos3 = 0.370;
float Vneg3 = 0.2;
float Cneg3 = -0.370;
float slope3 = 2.5;
float vmin3 = .05;

// Transpose and rotation matrix 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 R_11 = 0;
float R_12 = 0;
float R_13 = 0;
float R_21 = 0;
float R_22 = 0;
float R_23 = 0;
float R_31 = 0;
float R_32 = 0;
float R_33 = 0;
float RT_11 = 0;
float RT_12 = 0;
float RT_13 = 0;
float RT_21 = 0;
float RT_22 = 0;
float RT_23 = 0;
float RT_31 = 0;
float RT_32 = 0;
float RT_33 = 0;

// Task space PD controller gains
float KPx = 190;
float KPy = 100;
float KPz = 100;

float KDx = 15;
float KDy = 12;
float KDz = 10;

float xdesired=0.2540;
float ydesired=0;
float zdesired=0.5080;

float xdotdesired=0;
float ydotdesired=0;
float zdotdesired=0;

float xdot = 0;
float ydot = 0;
float zdot = 0;

float xcurr = 0;
float xold = 0;
float xdotold1 = 0;
float xdotold2 = 0;

float ycurr = 0;
float yold = 0;
float ydotold1 = 0;
float ydotold2 = 0;

float zcurr = 0;
float zold = 0;
float zdotold1 = 0;
float zdotold2 = 0;

float errorx = 0;
float errory = 0;
float errorz = 0;

float errorxdot = 0;
float errorydot = 0;
float errorzdot = 0;

// Trajectory forward and reverse direction tracker
int tictoc = 1;

// Impedance control axis angles
float thetax = 0;
float thetay = 0;
float thetaz = 0;

// Straight line trajectory parameters
long starttime = 0;
long endtime = 0;
float duration = 0;

float xstart = 0;
float ystart = 0;
float zstart = 0;

float xend = 0;
float yend = 0;
float zend = 0;

float deltax = 0;
float deltay = 0;
float deltaz = 0;

// Struct for storing straight line trajectory waypoints
struct point_tag {
    float nextX;
    float nextY;
    float nextZ;
    float nextThetaX;
    float nextThetaY;
    float nextThetaZ;
    float duration; // [milliseconds]
    float KPx;
    float KPy;
    float KPz;
    float KDx;
    float KDy;
    float KDz;
};

// Variables for managing execution of straight line trajectory loops
int point_count = 0;
bool first_time = true;
bool first_loop = true;

// Torque enable
bool torqueEnable = true;


#endif /* LAB_FINAL_VARS_H_ */

LabFinal_CV_VI.c

C/C++
/*
 * LabFinal_VI.c
 *
 * Authors: Carlos Villanueva and Vasil Iakimovitch
 * 
 * Abstract:
 * This script commands a CRS robot arm to perform a motion demonstration as described in the ME 446 final project manual.
 * In order to complete this demonstration, the robot arm is commanded using straight line trajectories.
 * These trajectories are executed by an impedance controller that incorporates friction compensation and torque limiting.
 * Impedance control is implemented such that impedance can be adjusted along any arbitrary axis or set of arbitrarily rotated orthoginal axes.
 * Friction compensation is implemented to account for both Coloumb and viscous friction.
 * Please see LabFinal_vars.h for variable declarations.
*/

// Includes
#include "math.h"
#include "F28335Serial.h"
#include "LabFinal_vars.h"

// Allocate space for global variables
#pragma DATA_SECTION(whattoprint, ".my_vars")
float whattoprint = 0.0;

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

// Number of waypoints to be traversed
int numPoints = 27;

// Struct containing all motion waypoints using format described in LabFinals_vars.h
struct point_tag points[27] = {{0.1392, 0, 0.4329, 0, 0, 0, 3000, 20, 10, 10, 15, 12, 10}, // Starting position
                              {0.1230, 0.2079, 0.3172, 0, 0, 0, 500, 200, 100, 100, 15, 12, 10}, // Move to arbitrary point
                              {0.1230, 0.2079, 0.3172, 0, 0, 0, 500, 200, 100, 100, 15, 12, 10}, // Rest at arbitrary point
                              {0.0344, 0.3473, 0.1873+.02, 0, 0, 0, 1000, 200, 100, 100, 15, 12, 10}, // Above hole
                              {0.0344, 0.3473, 0.1873+.02, 0, 0, 0, 250, 200, 100, 100, 15, 12, 10}, // Settle above hole
                              {0.0342, 0.3489, 0.1339, 0, 0, 0, 500, 200/2, 100/2, 100, 15, 12, 10}, // Inside hole
                              {0.0342, 0.3489, 0.1339, 0, 0, 0, 500, 200/2, 100/2, 100, 15, 12, 10}, // Dwell inside hole
                              {0.0344, 0.3473, 0.1873+.1, 0, 0, 0, 500, 200/2, 100/2, 100, 15, 12, 10}, // Above hole
                              {0.3791-0.2, 0.1209, 0.1995+.02, 0, 0, 0, 500, 200, 100, 100, 15, 12, 10}, // Intermediate point
                              {0.3791, 0.1209, 0.1995+.025, 0, 0, 0, 500, 200, 100, 100, 15, 12, 10}, // Entrance to zigzag
                              {0.4096, 0.0699, 0.1995+.025, 0, 0, -36.87*180/PI, 250, 200, 100/2, 100, 15, 12, 10}, // Zigzag line 1
                              {0.4127, 0.0301, 0.1995+.025, 0, 0, -90*180/PI, 150, 200, 100/2, 100, 15, 12, 10}, // Zigzag curve 1 point 1
                              {0.3899, 0.0238, 0.1995+.023, 0, 0, -90*180/PI, 150, 200, 100/2, 100, 15, 12, 10}, // Zigzag curve 1 point 2
                              {0.3350, 0.0501, 0.1995+.023, 0, 0, 0*180/PI, 250, 200, 100/2, 100, 15, 12, 10}, // Zigzag line 2
                              {0.3178, 0.0591, 0.1995+.023, 0, 0, 0*180/PI, 150, 200, 100/2, 100, 15, 12, 10}, // Zigzag curve 2 point 1
                              {0.3033, 0.0326, 0.1995+.023, 0, 0, 0*180/PI, 150, 200, 100/2, 100, 15, 12, 10}, // Zigzag curve 2 point 2
                              {0.3844, -0.0475, 0.1995+.023, 0, 0, -36.87*180/PI, 400, 200, 100/2, 100, 15, 12, 10}, // Zigzag line 3
                              {0.3988, -0.0682, 0.1995+.023, 0, 0, 0*180/PI, 150, 200, 100, 100, 15, 12, 10}, // Zigzag exit point
                              {0.2185, -0.0537, 0.3996, 0, 0, 0*180/PI, 500, 200, 100, 100, 15, 12, 10}, // Intermediate point
                              {0.2695, 0.1865, 0.3743+0.02, 0, 0, 0*180/PI, 500, 200, 100, 100, 15, 12, 10}, // Above egg
                              {0.2695, 0.1865, 0.3065+0.047, 0, 0, 0*180/PI, 250, 200, 100, 20, 15, 12, 10}, // Closer above egg
                              {0.2695, 0.1865, 0.3065+0.047, 0, 0, 0*180/PI, 500, 200, 100, 20, 15, 12, 10}, // Apply force to egg
                              {0.2695, 0.1865, 0.3065+0.047, 0, 0, 0*180/PI, 4000, 200, 100, 20, 15, 12, 10}, // Dwell when applying force to egg
                              {0.2695, 0.1865, 0.3065+0.057, 0, 0, 0*180/PI, 1500, 200, 100, 20, 15, 12, 10}, // Intermediate above egg 1
                              {0.2695, 0.1865, 0.3065+0.067, 0, 0, 0*180/PI, 1500, 200, 100, 20, 15, 12, 10}, // Intermediate above egg 2
                              {0.2695, 0.1865, 0.3065+0.077, 0, 0, 0*180/PI, 1500, 200, 100, 50, 15, 12, 10}, // Above egg
                              {0.254, 0, 0.254*2, 0, 0, 0*180/PI, 1000, 200, 100, 100, 15, 12, 10}, // Home position

};

void mains_code(void);
    /*
    * FUNCTION: mains_code
    *
    * Dummy function that is run by the main function
    * 
    */ 

void main(void)
{ 
    /*
    * FUNCTION: main
    *
    * Main function that continuously calls a dummy function until code is externally terminated by user
    * 
    */

	mains_code();
}

void lab(float theta1motor,float theta2motor,float theta3motor,float *tau1,float *tau2,float *tau3, int error)
{
    /*
    * FUNCTION: lab
    * 
    * This function is called every 1 millisecond to command the robot arm to follow a series of straight line trajectories.
    * Straight line trajectories are followed using impedance control with added friction compensation and torque limiting behaviour.
    * PD-control is used to implement impedance control about an arbitrarily rotatible set of orthoginal axes.
    * 
    * INPUTS:
    * theta1motor: Measured angle of motor 1 (rad)
    * theta2motor: Measured angle of motor 2 (rad)
    * theta3motor: Measured angle of motor 3 (rad)
    * *tau1: Pointer used to set commanded torque of motor 1 (Nm)
    * *tau2: Pointer used to set commanded torque of motor 2 (Nm)
    * *tau3: Pointer used to set commanded torque of motor 3 (Nm)
    */ 

    /*
     * Forward Kinematics
     *
     * Forward kinematics is used to calculate the current position of the end effector in the X, Y, and Z directions
     * Pre-computated trigonometric terms allow for the reuse of common terms and saves on the overall computation time.
     *
     */

    // Precompute motor cosine and sine terms
    cosq1 = cos(theta1motor);
    sinq1 = sin(theta1motor);
    cosq2 = cos(theta2motor);
    sinq2 = sin(theta2motor);
    cosq3 = cos(theta3motor);
    sinq3 = sin(theta3motor);

    // Forward kinematics
    xcurr = 0.254*cosq1*(cosq3 + sinq2);
    ycurr = 0.254*sinq1*(cosq3 + sinq2);
    zcurr = 0.254*cosq2 - 0.254*sinq3 + 0.254;

    /*
     * Task-Space Velocity
     *
     * The velocity is found using the finite difference method: the difference between two points for a given direction
     * is divided by the time step to get the velocity in that direction. An infinite-impulse response filter is applied
     * to reduce noise by filtering high frequencies from the computed velocity.
     * 
     */

    // Low pass filter for X linear velocity
    xdot = (xcurr - xold)/0.001;
    xdot = (xdot + xdotold1 + xdotold2)/3.0;
    xold = xcurr;
    xdotold2 = xdotold1;
    xdotold1 = xdot;

    // Low pass filter for Y linear velocity
    ydot = (ycurr - yold)/0.001;
    ydot = (ydot + ydotold1 + ydotold2)/3.0;
    yold = ycurr;
    ydotold2 = ydotold1;
    ydotold1 = ydot;

    // Low pass filter for Z linear velocity
    zdot = (zcurr - zold)/0.001;
    zdot = (zdot + zdotold1 + zdotold2)/3.0;
    zold = zcurr;
    zdotold2 = zdotold1;
    zdotold1 = zdot;

    /*
     * Straight Line Trajectory Execution
     *
     * This section of the code commands the arm to move between the waypoint positions specified in the points struct.
     * For the first waypoint, we command the arm to move to a starting position that is close to its physical starting position.
     * This first move is done using purely PD control to smoothly initialize the initial state of the arm.
     * For all remaining points, we use straight line trajectories to move the arm to the desired waypoint.
     * Straight line trajectories are computed by multipling the gradient (deltax, deltay, deltaz) with the fraction of time elapsed
     * for the specified trajectory ((mycount-starttime)/duration) and adding the trajectory starting position (xstart, ystart, zstart).
     * When the time duration of a given trajectory elapses, we advance to the next trajectory by setting the new start position to be equal to the
     * previous trajectory end position and the new start time to be equal to the previous end time.
     * When all trajectories have been traversed, the arm remains at its final commanded location.
     * 
     */

    // Command the arm to move to its starting position
    if(first_time){
        starttime = mycount;
        endtime = starttime + points[0].duration;

        xend = points[0].nextX;
        yend = points[0].nextY;
        zend = points[0].nextZ;

        KPx = points[0].KPx;
        KPy = points[0].KPy;
        KPz = points[0].KPz;

        KDx = points[0].KDx;
        KDy = points[0].KDy;
        KDz = points[0].KDz;

        first_time = false;
    }

    // Force arm to move to the starting position using PD control 
    if(first_loop && mycount >= endtime){
        first_loop = false;
    }

    // Move end effector to each waypoint using consecutive straight line trajectories
    if(!first_loop && point_count < numPoints){
        if(mycount < endtime){ // Move the end effector in a straight line
            xdesired = deltax*(mycount-starttime)/duration + xstart;
            ydesired = deltay*(mycount-starttime)/duration + ystart;
            zdesired = deltaz*(mycount-starttime)/duration + zstart;
        } else if(point_count < numPoints-1){ // Switch from one straight line trajectory to the next
            // New starttime picks up at previous endtime
            starttime = endtime;

            // New starting point picks up at previous ending point
            xstart = xend;
            ystart = yend;
            zstart = zend;

            // Advance to next waypoint index
            point_count++;

            // Calculate duration and endtime of next waypoint trajectory
            duration = points[point_count].duration;
            endtime += duration;

            // Extract new end position from struct
            xend = points[point_count].nextX;
            yend = points[point_count].nextY;
            zend = points[point_count].nextZ;

            // Calculate gradient of the new straight line trajectory
            deltax = xend - xstart;
            deltay = yend - ystart;
            deltaz = zend - zstart;

            // Extract impedance control parameters from struct
            thetax = points[point_count].nextThetaX;
            thetay = points[point_count].nextThetaY;
            thetaz = points[point_count].nextThetaZ;

            KPx = points[point_count].KPx;
            KPy = points[point_count].KPy;
            KPz = points[point_count].KPz;

            KDx = points[point_count].KDx;
            KDy = points[point_count].KDy;
            KDz = points[point_count].KDz;
        }
    } else { // Remain at final waypoint once it has been reached
        xdesired = xend;
        ydesired = yend;
        zdesired = zend;
    }

    /*
     * Task-Space Error
     * 
     * Task-space error is computed by simply taking the difference between the desired and measured position and velocity terms.
     * 
    */

    // Task space position error
    errorx = xdesired - xcurr;
    errory = ydesired - ycurr;
    errorz = zdesired - zcurr;

    // Task space velocity error
    errorxdot = xdotdesired - xdot;
    errorydot = ydotdesired - ydot;
    errorzdot = zdotdesired - zdot;

    /*
     * Impedance Control
     * 
     * Impedance control is implemented with PD-feedback and in such way as to enable control of impedance about an arbitrarily 
     * rotated set of orthoginal axes. The terms of the Jacobian tranpose and the rotation matrix that specifies the orientation of these
     * arbitrarily rotated axes are precomputed to enhance computational efficiency.
     * 
    */

    // Precompute Jacobian transpose terms
    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;

    // Precompute rotation matrix terms
    cosz = cos(thetaz);
    sinz = sin(thetaz);
    cosx = cos(thetax);
    sinx = sin(thetax);
    cosy = cos(thetay);
    siny = sin(thetay);
    RT_11 = R_11 = cosz*cosy-sinz*sinx*siny;
    RT_21 = R_12 = -sinz*cosx;
    RT_31 = R_13 = cosz*siny+sinz*sinx*cosy;
    RT_12 = R_21 = sinz*cosy+cosz*sinx*siny;
    RT_22 = R_22 = cosz*cosx;
    RT_32 = R_23 = sinz*siny-cosz*sinx*cosy;
    RT_13 = R_31 = -cosx*siny;
    RT_23 = R_32 = sinx;
    RT_33 = R_33 = cosx*cosy;

    // Impedance control (copied from MATLAB script)
    *tau1 = (JT_11*R_11 + JT_12*R_21 + JT_13*R_31)*(KDx*RT_11*errorxdot + KDx*RT_12*errorydot + KDx*RT_13*errorzdot + KPx*RT_11*errorx + KPx*RT_12*errory + KPx*RT_13*errorz) + (JT_11*R_12 + JT_12*R_22 + JT_13*R_32)*(KDy*RT_21*errorxdot + KDy*RT_22*errorydot + KDy*RT_23*errorzdot + KPy*RT_21*errorx + KPy*RT_22*errory + KPy*RT_23*errorz) + (JT_11*R_13 + JT_12*R_23 + JT_13*R_33)*(KDz*RT_31*errorxdot + KDz*RT_32*errorydot + KDz*RT_33*errorzdot + KPz*RT_31*errorx + KPz*RT_32*errory + KPz*RT_33*errorz);
    *tau2 = (JT_21*R_11 + JT_22*R_21 + JT_23*R_31)*(KDx*RT_11*errorxdot + KDx*RT_12*errorydot + KDx*RT_13*errorzdot + KPx*RT_11*errorx + KPx*RT_12*errory + KPx*RT_13*errorz) + (JT_21*R_12 + JT_22*R_22 + JT_23*R_32)*(KDy*RT_21*errorxdot + KDy*RT_22*errorydot + KDy*RT_23*errorzdot + KPy*RT_21*errorx + KPy*RT_22*errory + KPy*RT_23*errorz) + (JT_21*R_13 + JT_22*R_23 + JT_23*R_33)*(KDz*RT_31*errorxdot + KDz*RT_32*errorydot + KDz*RT_33*errorzdot + KPz*RT_31*errorx + KPz*RT_32*errory + KPz*RT_33*errorz);
    *tau3 = (JT_31*R_11 + JT_32*R_21 + JT_33*R_31)*(KDx*RT_11*errorxdot + KDx*RT_12*errorydot + KDx*RT_13*errorzdot + KPx*RT_11*errorx + KPx*RT_12*errory + KPx*RT_13*errorz) + (JT_31*R_12 + JT_32*R_22 + JT_33*R_32)*(KDy*RT_21*errorxdot + KDy*RT_22*errorydot + KDy*RT_23*errorzdot + KPy*RT_21*errorx + KPy*RT_22*errory + KPy*RT_23*errorz) + (JT_31*R_13 + JT_32*R_23 + JT_33*R_33)*(KDz*RT_31*errorxdot + KDz*RT_32*errorydot + KDz*RT_33*errorzdot + KPz*RT_31*errorx + KPz*RT_32*errory + KPz*RT_33*errorz);

    /*
     * Angular Velocity
     * 
     * Much like the velocity calculation for linear velocity, the same approach was taken for angular velocity.
     * The velocity is found using the finite difference method: the difference between two points for a given direction
     * is divided by the time step to get the velocity in that direction. An infinite-impulse response filter is applied
     * to reduce noise by filtering high frequencies from the computed velocity.
     * 
     */

    // Low pass filter for velocity for theta 1
    Omega1 = (theta1motor - Theta1_old)/0.001;
    Omega1 = (Omega1 + Omega1_old1 + Omega1_old2)/3.0;
    Theta1_old = theta1motor;
    Omega1_old2 = Omega1_old1;
    Omega1_old1 = Omega1;

    // Low pass filter for velocity for theta 2
    Omega2 = (theta2motor - Theta2_old)/0.001;
    Omega2 = (Omega2 + Omega2_old1 + Omega2_old2)/3.0;
    Theta2_old = theta2motor;
    Omega2_old2 = Omega2_old1;
    Omega2_old1 = Omega2;

    // Low pass filter for velocity for theta 3
    Omega3 = (theta3motor - Theta3_old)/0.001;
    Omega3 = (Omega3 + Omega3_old1 + Omega3_old2)/3.0;
    Theta3_old = theta3motor;
    Omega3_old2 = Omega3_old1;
    Omega3_old1 = Omega3;

    /*
     * Friction Compensation
     * 
     * Friction is calculated using a piecewise model consisting of 3 linear equations for each motor. Initial
     * values for the parameters were given but were finely tuned in order to visually appear to "glide on ice" in the
     * X and Y directions and appear to free fall in the Z direction.
     * 
    */

    if (Omega1 > vmin1) {
        u_fric1 = Vpos1*Omega1 + Cpos1 ;
    } else if (Omega1 < -vmin1) {
        u_fric1 = Vneg1*Omega1 + Cneg1;
    } else {
        u_fric1 = slope1*Omega1;
    }

    if (Omega2 > vmin2) {
        u_fric2 = Vpos2*Omega2 + Cpos2 ;
    } else if (Omega2 < -vmin2) {
        u_fric2 = Vneg2*Omega2 + Cneg2;
    } else {
        u_fric2 = slope2*Omega2;
    }

    if (Omega3 > vmin3) {
        u_fric3 = Vpos3*Omega3 + Cpos3 ;
    } else if (Omega3 < -vmin3) {
        u_fric3 = Vneg3*Omega3 + Cneg3;
    } else {
        u_fric3 = slope3*Omega3;
    }

    *tau1 = *tau1 + u_fric1*u_fricfac1;
    *tau2 = *tau2 + u_fric2*u_fricfac2;
    *tau3 = *tau3 + u_fric3*u_fricfac3;

    /*
     * Torque Saturation
     * 
     * Clamps torque at an amplitude of 5 Nm
     * 
    */

    if (*tau1 > 5) {
        *tau1 = 5;
    } else if (*tau1 < -5){
        *tau1 = -5;
    }
    if (*tau2 > 5) {
        *tau2 = 5;
    } else if (*tau2 < -5){
        *tau2 = -5;
    }
    if (*tau3 > 5) {
        *tau3 = 5;
    } else if (*tau3 < -5){
        *tau3 = -5;
    }

    /*
     * Utilities
     *
     * Blink LEDs, increment counter for timers, update Simulink variables
     *
    */

    // Blink LEDs
        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
        }

    // Increment count for timers
    mycount++;

    /*
     * Update Simulink Variables
     *
     * The simulink PlotVar variables were used when rapid testing to gauge trajectory tracking performance of the arm. They were
     * used less in this lab assignment since the performance metrics were more qualitative and less quantitative. But they
     * were still used to track trajectories in Simulink to ensure the robot moved between the points we specified.
     * 
    */
  
    Simulink_PlotVar1 = xcurr;
    Simulink_PlotVar2 = ycurr;
    Simulink_PlotVar3 = zcurr;
    Simulink_PlotVar4 = zdesired;

    /*
     * Torque Enable
     *
     * Disable torques if specified by torqueEnable in LabFinal_vars.h
     * Used to observe behaviour of controller code during debugging
     *
    */

    if(!torqueEnable){
        *tau1 = 0;
        *tau2 = 0;
        *tau3 = 0;
    }

}

void printing(void)
{
    /*
    * FUNCTION: printing
    * 
    * Print current position of the end effector to the terminal
    * 
    */ 

    if (whattoprint == 0) {
        serial_printf(&SerialA, "Pos FK: %.4f %.4f,%.4f   \n\r",xcurr, ycurr, zcurr);
    } else {
        serial_printf(&SerialA, "%.2f   \n\r",printval);
    }
}

Credits

Vasil Iakimovitch

Vasil Iakimovitch

1 project • 0 followers
Carlos Villanueva

Carlos Villanueva

1 project • 0 followers

Comments

Related channels and tags
  • MATLAB
  • Robotics
  • Texas Instruments