•

Neil Wagner

Created April 28, 2024

ME 446 Final Project: Robot navigating an obstacle course

A project to control the lab CRS arm through a small obstacle course.

IntermediateShowcase (no instructions)18

ME 446 Final Project: Robot navigating an obstacle course

Things used in this project

Software apps and online services

Texas Instruments Code Composer Studio

Story

Objectives

The goal of this project is to create a CRS arm program that calculates the forward and inverse kinematics, implements task space impedance control, and uses accurately tuned gains to control the robot arm as it moves through an obstacle course. Within the obstacle course, it is desired to move the end effector in and out of a hole, through a curved path, and press down on an egg with 500–1000 g of force. These objectives are to show task space impedance control in which we can control the robot in the x, y, and z coordinates as well as actively control the impedance values in any desired direction.

Implementation

The program can be broken up into four major parts: the waypoint array & getNextPose function, IIR velocity filtering & torque limitation, friction compensation, and the Task Space Impedance Controller (TSIC).

Waypoint Navigation

The waypoint array is an array containing the location in cartesian space, the time taken between waypoints, impedance gains for the spring and damping constants, and the angle at which to rotate the impedance controller’s frame.

When getNextPose is called, the function takes the current (A) and the next (B) waypoint's coordinates and the current time step to produce linear steps between the waypoints, as desired coordinates for the TSIC. The impedance gains, as well as their rotation, are also applied in the controller. Once the desired time has passed—corresponding to reaching waypoint B—the waypoint count is incremented, setting B to A and pulling the next B.

Velocity Filtering and Torque Limiting

The IIR filter requires knowledge of the most recent encoder joint readings. A finite difference is used to obtain the raw velocity. Then, since this can be noisy, is smoothed by averaging the three most recent velocities.

Sometimes the robot may try to command high-frequency torques, which can break the arm or injure the user. Thus, a torque limiter is placed on the commanded torques, saturating at ±5 Nm.

Friction Compensation

Friction compensation is applied to the controller to compensate for the effects of the robot’s weight and joint friction. This is done by modeling the friction as a combination of coulombic and viscous friction and tuning such values until the system is easily moved by hand with no resistance or acceleration. This allows the TSIC to more accurately represent the spring and damper system which is a notable feature of impedance control.

Task Space Impedance Control

The TSIC is the system that calculates the output torque to send the arm’s motors. This is done with an impedance controller which turns the system into a spring damper system in each cartesian direction. This allows the system to reach the desired x, y, and z coordinates with desired stiffnesses, providing varying soft contact and compliance in each direction.

Within the controller there also is the option to rotate the end effector frame so that the impedance in the world x, y, and z directions will be rotated, allowing for impedance to be controlled relative to any desired direction other than just the world frame. It is important to note that the controller applies the spring and damping forces proportional to the coordinate errors and coordinate velocity errors, which require inputs from the sensors on the robot, therefore requiring the filtering of the velocity signal.

Bringing it All Together

Using the four components discussed above, the CRS arm can smoothly navigate the obstacle course quickly, satisfying all of the objectives necessary for completion of the obstacle course.

Here is a video of the robot arm running through the obstacle course:

/*
Authors: Marshall Tenzer (mtenzer 2) and Neil Wagner (neilrw2)
TA: Nagesh Eranki
Course: ME 446

Final Project - May 7th, 2024
 */

#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 = -27.15*PI/180.0; // -0.37;
float offset_Enc3_rad = 13.05*PI/180.0; //  0.27;

// 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];

long arrayindex = 0;
int UARTprint = 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;

// Lab 1: Robot motor thetas from encoders, TeraTerm print variables
float printtheta1motor = 0;
float printtheta2motor = 0;
float printtheta3motor = 0;

// Lab 1: Intialize variables for DH
float DHx = 0;
float DHy = 0;
float DHz = 0;

// Lab 1: Init TS print vars
float printx = 0;
float printy = 0;
float printz = 0;

// Lab 1: Link constants
float l1 = .254;
float l2 = .254;
float l3 = .254;
float l = .254;

// Lab 1: Init IK variables
float DHtheta1out = 0;
float DHtheta2out = 0;
float DHtheta3out = 0;

// Lab 1: Intermediate values for IK equations
float a = 0;
float b = 0;

// Lab 1: Init variables for changing DH to motor thetas
float Calcmt1 = 0;
float Calcmt2 = 0;
float Calcmt3 = 0;

// Lab 2: Angular velocity IIR filter vars
float theta1_old = -.05;
float omega1_old1 = 0;
float omega1_old2 = 0;
float omega1 = 0;
float theta2_old = -27.03;
float omega2_old1 = 0;
float omega2_old2 = 0;
float omega2 = 0;
float theta3_old = 13.16;
float omega3_old1 = 0;
float omega3_old2 = 0;
float omega3 = 0;

/*
Lab 3: Friction comp variables (Part 1) Tuned values herein initializations, 1 denote joint 1, 2 denotes joint 2, and 3 denotes joint 3
    Viscous friction is the gain of the friction force while system is actively rotating around that joint.
    Coulombic friction is the gain of the friction force while system is stationary, i.e. sticky-ness of joints
    ff is used for overall scaling of the friction compensation system
*/
float minvelo1 = 0.1;
float Viscous_positive1 = 0.0687;
float Viscous_negative1 = 0.0005;
float Coulomb_positive1 = 0.5809;
float Coulomb_negative1 = -0.4128;
float slopemin1 = 1;
float ufric1 = 0;
float ff1 = .7;

float minvelo2 = 0.05;
float Viscous_positive2 = 0.05;
float Viscous_negative2 = .5;
float Coulomb_positive2 = .7;
float Coulomb_negative2 = -.025;
float slopemin2 = 2.2;
float ufric2 = 0;
float ff2 = .7;

float minvelo3 = 0.05;
float Viscous_positive3 = .15;
float Viscous_negative3 = .1;
float Coulomb_positive3 = .5;
float Coulomb_negative3 = -.5;
float slopemin3 = 2;
float ufric3 = 0;
float ff3 = .7;

// Lab 3: Init desired x, y, z positions and velocities in task space
float xDes = 0;
float yDes = 0;
float zDes = 0;
float xdesdot = 0;
float ydesdot = 0;
float zdesdot = 0;

// Lab 3: Init impedance control vars (Part 4)
float icx = 0;
float icy = 0;
float icz = 0;

// Lab 3: Init vars for rotation about world frame axes for impedance control in any direction
float tx = 0;
float ty = 0;
float tz = 0;

// Lab 3: Init straight line following vars (Part 5)
float deltax = 0;
float deltay = 0;
float deltaz = 0;
float t = 0;
long tcount = 0;
float tstart = 0;

// Lab 3: Init waypoint variables for straight line following (Part 5)
float xa = 0;
float xb = 0;
float ya = 0;
float yb = 0;
float za = 0;
float zb = 0;

// Lab 3: Init rotated frame impedance control gains
float kpxn;
float kpyn;
float kpzn;
float kdxn;
float kdyn;
float kdzn;

// Final: Init straight line trajectory duration and counter variables
float ttotal = 0;
float tcheck = 0;
int wayCount = 0;

/*
Final:  Waypoint struct for desired waypoints. This struct contains information on waypoint positions and the desired time to reach each waypoint, as well as the strength and direction of impedance control.

Note:   It is an unknown bug with the robot for why ttotal needs to be half the actual desired time for movement. This was a bug that was identified in Lab 3. 
        Other attempts have included the use of flag variables and modulos, neither of which worked either. There is a count variable used to control the arm through its straight line trajectory. 
        Watch expressions show that the counter was incrementing half as fast as it should have, causing the error. The lab TA tried to help debug as well, and was also unable to find a 
        solution either. As such, since the robot moves twice as slow as it should, ttotal needs to be halved from the actual desired trajectory time to achieve the proper waypoints in time.
*/   
typedef struct waypoint
{
    float x;        // Desired x position
    float y;        // Desired y position
    float z;        // Desired z position
    float ttotal;   // Half of the time it takes to move from the current waypoint to the next waypoint in a straight line. Ex: Move from A to B in 2s, set ttotal to 1. See the below note.
    float kpx;      // Proportional gain for task space/impedence control in the x direction
    float kpy;      // Proportional gain for task space/impedence control in the y direction
    float kpz;      // Proportional gain for task space/impedence control in the z direction
    float kdx;      // Derivative gain for task space/impedence control in the x direction
    float kdy;      // Derivative gain for task space/impedence control in the y direction
    float kdz;      // Derivative gain for task space/impedence control in the z direction
    float tx;       // Angle to rotate about the world x-axis to change the direction of impedence control
    float ty;       // Angle to rotate about the world y-axis to change the direction of impedence control
    float tz;       // Angle to rotate about the world z-axis to change the direction of impedence control

} waypoint;

/*
Final:  Define each waypoint for the entire trajectory of the robot throughout the obstacle course.
        When moving between the current and next waypoint, the robot relies on the current waypoints data for it's trajectory control. As such, when a wait is required, an additional waypoint
        has been added to illustrate the delay. All waypoints are labled.
*/
waypoint nextPose[] = {
                       {.13, 0, .42, .5, 500, 700, 200, 10, 10, 10, 0, 0, 0},               // Initialization
                       {.25, 0, .5, 1, 500, 700, 200, 10, 10, 10, 0, 0, 0},                 // Start
                       {.04, .235, .30, 1, 100, 100, 700, 10, 10, 10, 0, 0, 0},             // Above hole
                       {.04, .235, .135, .5, 100, 100, 700, 10, 10, 10, 0, 0, 0},           // In hole
                       {.04, .235, .135, 1, 100, 100, 700, 10, 10, 10, 0, 0, 0},            // Wait in hole
                       {.04, .235, .30, 1, 500, 500, 700, 10, 10, 10, 0, 0, 0},             // Above hole
                       {.25, .07, .36, 1, 700, 700, 500, 10, 10, 10, 0, 0, 0},              // Avoid obstacle
                       {.41, .105, .22, 1, 100, 500, 500, 5, 10, 10, 0, 0, 40*PI/180.0},    // Path entrance
                       {.44, .03, .22, 1, 300, 700, 500, 5, 10, 10, 0, 0, -120*PI/180.0},   // First bend
                       {.345, .035, .22, 1, 250, 700, 500, 5, 10, 10, 0, 0, 40*PI/180.0},   // Second bend
                       {.42, -.06, .22, .5, 50, 700, 500, 5, 10, 10, 0, 0, 0},              // Path exit
                       {.42, -.06, .27, 1, 700, 700, 500, 10, 10, 10, 0, 0, 0},             // Above path exit
                       {.26, .17, .36, 1, 700, 700, 500, 10, 10, 10, 0, 0, 0},              // Above egg
                       {.26, .17, .289, 2, 700, 700, 500, 10, 10, 10, 0, 0, 0},             // Egg wait
                       {.26, .17, .289, 1, 700, 700, 500, 10, 10, 10, 0, 0, 0},             // In egg
                       {.26, .17, .36 , 1, 700, 700, 700, 10, 10, 10, 0, 0, 0},             // Above egg
                       {.25, 0, .5, 20, 700, 700, 200, 10, 10, 10, 0, 0, 0},                // Start
                       {.25, 0, .5, 20, 700, 700, 200, 10, 10, 10, 0, 0, 0},                // Wait at the end
                       {.25, 0, .5, 20, 700, 700, 200, 10, 10, 10, 0, 0, 0}                 // Pseudo-end (since the pose function needs a next desired waypoint)
};

/* Final: Waypoint computation function - getNextPose
This function computes various values needed for the controller to move the arm from the current waypoint A to the next waypoint B. It will update the controller values globally.

Inputs:
t - The current time in the arm's entire operation
tstart - The time that the next straight line trajectory begins at
A - The current waypoint's data
B - The next waypoint's data
theta1motor - The measured encoder theta of joint 1
theta2motor - The measured encoder theta of joint 2
theta3motor - The measured encoder theta of joint 3

Output:
None
*/ 
void getNextPose(float t, float tstart, waypoint A, waypoint B, float theta1motor,float theta2motor,float theta3motor)
{
    // Get current (A) and next (B) waypoints
    xa = A.x;
    xb = B.x;
    ya = A.y;
    yb = B.y;
    za = A.z;
    zb = B.z;

    // Get the time it will take to move from A to B
    ttotal = A.ttotal;

    // Get the impedance control gains and directions when moving from A to B
    kpxn = A.kpx;
    kpyn = A.kpy;
    kpzn = A.kpz;
    kdxn = A.kdx;
    kdyn = A.kdy;
    kdzn = A.kdz;
    tx = A.tx;
    ty = A.ty;
    tz = A.tz;

    // Compute the change in the x, y, z coordinates of waypoints A and B
    deltax = xb - xa;
    deltay = yb - ya;
    deltaz = zb - za;

    // Compute the next incremental waypoint step for straight line trajectory.
    // The first term can be thought of as a percent completion of the end effector going from A to B (in distance), which is added to the intitial starting point of A.
    xDes = deltax * (t - tstart)/ttotal + xa;
    yDes = deltay * (t - tstart)/ttotal + ya;
    zDes = deltaz * (t - tstart)/ttotal + za;

    /*
    World (W) to new frame (N) task space and impedance control (derived in MATLAB: taskspace.m).
    
    The new frame is dictated by applying a rotation matrix to world frame impedance control, so that the new frame is some rotation(s) of the world frame about each world axis. Impedance 
    control provides a way to control how stiff the robot arm will feel in a certain direction. Then, using the Kp and Kd gains, the stiffness in the respective direction can easily be 
    controlled (higher gains being more stiff). This control is beneficial, as it can make the arm stiff in one direction, while weak in another, providing a more robust system. In particular,
    impedance control was significantly important in placing the end-effector in the hole, as well as moving through the zig-zag. The zig-zag required rotated frames to be strong in the forward
    direction, while weak perpendicular to the direction of motion.  

    Note:   There is no implementation of end effector velocity filtering. The end effector's velocity is obtained by directly differentiating the task space position FK
            as functions of the motor joint coordinates, and put directly into the below task space and impedence control. This is seen in the MATLAB file.
    */ 
    icx = (l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx)) - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz)))*(kdzn*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz))*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdzn*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx))*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz))*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx))*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*cos(tx)*cos(ty)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) + kdzn*cos(tx)*cos(ty)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor)))) - (l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty)) - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz)))*(kdxn*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty))*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdxn*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz))*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpxn*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz))*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpxn*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty))*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpxn*cos(tx)*sin(ty)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) + kdxn*cos(tx)*sin(ty)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor)))) + (l*cos(theta1motor)*cos(tx)*cos(tz)*(cos(theta3motor) + sin(theta2motor)) + l*cos(tx)*sin(theta1motor)*sin(tz)*(cos(theta3motor) + sin(theta2motor)))*(kdyn*sin(tx)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor))) + kpyn*sin(tx)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) - kdyn*cos(tx)*sin(tz)*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpyn*cos(tx)*sin(tz)*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpyn*cos(tx)*cos(tz)*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdyn*cos(tx)*cos(tz)*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))));
    icy = (l*cos(theta1motor)*(cos(theta2motor) - sin(theta3motor))*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz)) - l*cos(tx)*cos(ty)*(cos(theta3motor) + sin(theta2motor)) + l*sin(theta1motor)*(cos(theta2motor) - sin(theta3motor))*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx)))*(kdzn*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz))*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdzn*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx))*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz))*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx))*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*cos(tx)*cos(ty)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) + kdzn*cos(tx)*cos(ty)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor)))) - (l*sin(tx)*(cos(theta3motor) + sin(theta2motor)) + l*cos(theta1motor)*cos(tx)*sin(tz)*(cos(theta2motor) - sin(theta3motor)) - l*cos(tx)*cos(tz)*sin(theta1motor)*(cos(theta2motor) - sin(theta3motor)))*(kdyn*sin(tx)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor))) + kpyn*sin(tx)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) - kdyn*cos(tx)*sin(tz)*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpyn*cos(tx)*sin(tz)*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpyn*cos(tx)*cos(tz)*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdyn*cos(tx)*cos(tz)*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor)))) - (l*cos(theta1motor)*(cos(theta2motor) - sin(theta3motor))*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz)) + l*cos(tx)*sin(ty)*(cos(theta3motor) + sin(theta2motor)) + l*sin(theta1motor)*(cos(theta2motor) - sin(theta3motor))*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty)))*(kdxn*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty))*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdxn*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz))*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpxn*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz))*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpxn*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty))*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpxn*cos(tx)*sin(ty)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) + kdxn*cos(tx)*sin(ty)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor))));
    icz = (l*cos(theta1motor)*sin(theta3motor)*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz)) + l*sin(theta1motor)*sin(theta3motor)*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty)) - l*cos(theta3motor)*cos(tx)*sin(ty))*(kdxn*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty))*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdxn*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz))*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpxn*(cos(ty)*cos(tz) - sin(tx)*sin(ty)*sin(tz))*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpxn*(cos(ty)*sin(tz) + cos(tz)*sin(tx)*sin(ty))*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpxn*cos(tx)*sin(ty)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) + kdxn*cos(tx)*sin(ty)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor)))) - (l*cos(theta1motor)*sin(theta3motor)*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz)) + l*sin(theta1motor)*sin(theta3motor)*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx)) + l*cos(theta3motor)*cos(tx)*cos(ty))*(kdzn*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz))*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdzn*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx))*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*(cos(tz)*sin(ty) + cos(ty)*sin(tx)*sin(tz))*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*(sin(ty)*sin(tz) - cos(ty)*cos(tz)*sin(tx))*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpzn*cos(tx)*cos(ty)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) + kdzn*cos(tx)*cos(ty)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor)))) - (l*cos(theta3motor)*sin(tx) - l*cos(theta1motor)*cos(tx)*sin(theta3motor)*sin(tz) + l*cos(tx)*cos(tz)*sin(theta1motor)*sin(theta3motor))*(kdyn*sin(tx)*(zdesdot + l*(omega3*cos(theta3motor) + omega2*sin(theta2motor))) + kpyn*sin(tx)*(zDes - l*(cos(theta2motor) - sin(theta3motor) + 1)) - kdyn*cos(tx)*sin(tz)*(xdesdot - l*cos(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) + l*omega1*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kpyn*cos(tx)*sin(tz)*(xDes - l*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))) + kpyn*cos(tx)*cos(tz)*(yDes - l*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor))) - kdyn*cos(tx)*cos(tz)*(l*sin(theta1motor)*(omega2*cos(theta2motor) - omega3*sin(theta3motor)) - ydesdot + l*omega1*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor))));
}

void mains_code(void);

// Main
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) {
    // Lab 2: IIR filter for angular velocities ********************************************************************************************************************************
    /*
    The first step in implementing an accurate control scheme for the CRS arm is obtaining the velocity for each joint. The raw velocity, omega, can be obtained by discretely
    differentiating the motor thetas. This raw velocity is a good approximation; however, it is still prone to noise. To reduce the noise, it is necessary to implement some form 
    of velocity filtering. Thus, an IIR averaging filter is used. The IIR filter was chosen due to its ability to produce a smoother averaged velocity, as it is less prone to 
    impulses through noisy data by setting the old values to the averaged velocity.
    */

    // Joint 1
    omega1 = (theta1motor - theta1_old)/0.001;          // Differentiate for raw velocity
    omega1 = (omega1 + omega1_old1 + omega1_old2)/3.0;  // Filter via average
    theta1_old = theta1motor;

    // Joint 2
    omega2 = (theta2motor - theta2_old)/0.001;          // Differentiate for raw velocity
    omega2 = (omega2 + omega2_old1 + omega2_old2)/3.0;  // Filter via average
    theta2_old = theta2motor;

    // Joint 3
    omega3 = (theta3motor - theta3_old)/0.001;          // Differentiate for raw velocity
    omega3 = (omega3 + omega3_old1 + omega3_old2)/3.0;  // Filter via average
    theta3_old = theta3motor;

    // Lab 2: Update old states for angular velocities. Order matters here because both states would be the same otherwise.
    // Joint 1
    omega1_old2 = omega1_old1;
    omega1_old1 = omega1;

    // Joint 2
    omega2_old2 = omega2_old1;
    omega2_old1 = omega2;

    // Joint 3
    omega3_old2 = omega3_old1;
    omega3_old1 = omega3;
    // *************************************************************************************************************************************************************************

    // Lab 3: Part 1 - Compute friction compensation control for each joint ****************************************************************************************************
    /*
    This seciton of code is taking in the rotatioinal velocity of each of the joints, determining if the friction that should be applied is viscous or coulombic, and then outputting the resulting friction
    compensation torques for the joint. If the angular velocity is above the threshold value then the viscous component is added to the friciton compensation torque. Each joint and direction has its own tuned
    value, which is unique to the system and can depend on many different attributes of the physical system, so it is important to tune each joint seperately, in both positive and negative directions. 

    This is in order to remove the inherent forces that the system has between joints and its movement. The goal is to remove all of the friction so that the robot moves freely and will move by the 
    touch of your hand and allowing it to move with little to no effort. This is done by calculating the friction force of the system which is done below. By using a friction model of coulombic and 
    viscous frictions we can produce a system that moves freely and with little effort.
    
    To tune the gains used within the friction compensation controller, we isolated each joint and proceeded to move the joint back and forth and tested each coefficient needed. The coulombic friction 
    constant was tuned by reducing the stickiness of the system by feel, and the viscous constant was tuned by seeing how little of a force we could give the system in order for the joint to "glide" 
    wiithout any additional force. If the joint speeds up, then the coefficient is too high and the friction is being overcompensated. The other variables need to be tuned so there is no jump between 
    the areas of the friciton, with or without viscous friction.
    
    The friction compensation is implemented in the controller, in joint 1 for example, as the ff1*ufric1 term.
    */

    // Friction comp for joint 1
    if (omega1 > minvelo1) {
        ufric1 = Viscous_positive1*omega1 + Coulomb_positive1 ;
    } else if (omega1 < -minvelo1) {
        ufric1 = Viscous_negative1*omega1 + Coulomb_negative1;
    } else {
        ufric1 = slopemin1*omega1;
    }

    // Friction comp for joint 2
    if (omega2 > minvelo2) {
        ufric2 = Viscous_positive2*omega2 + Coulomb_positive2 ;
    } else if (omega2 < -minvelo2) {
        ufric2 = Viscous_negative2*omega2 + Coulomb_negative2;
    } else {
        ufric2 = slopemin2*omega2;
    }

    // Friction comp for joint 3
    if (omega3 > minvelo3) {
        ufric3 = Viscous_positive3*omega3 + Coulomb_positive3 ;
    } else if (omega3 < -minvelo3) {
        ufric3 = Viscous_negative3*omega3 + Coulomb_negative3;
    } else {
        ufric3 = slopemin3*omega3;
    }
    // *************************************************************************************************************************************************************************

    // Lab 2: Motor torque limitation (Max: 5 Min: -5) *************************************************************************************************************************
    // Sometimes the robot may try to command high-frequency torques, which can break the arm or injure the user. Thus, a torque limiter is placed on the commanded torques, saturating at 5 Nm.

    // Torque limiting for Joint 1
    if (*tau1 > 5)
    {
        *tau1 = 5;
    }
    else if (*tau1 < -5)
    {
        *tau1 = -5;
    }

    // Torque limiting for Joint 2
    if (*tau2 > 5)
    {
        *tau2 = 5;
    }
    else if (*tau2 < -5)
    {
        *tau2 = -5;
    }

    // Torque limiting for Joint 3
    if (*tau3 > 5)
    {
        *tau3 = 5;
    }
    else if (*tau3 < -5)
    {
        *tau3 = -5;
    }
    // *************************************************************************************************************************************************************************

    // Final: State machine for waypoint navigation ****************************************************************************************************************************
    /*
    This block of code implements the state machine for waypoint navigation, telling the arm when to update its next desired waypoint. When the current time is less than the
    time (tcheck) at which the arm should be at the next waypoint, waypoint B, the state machine continues to update the TS impedance controller to achieve the waypoint B position.
    Once the arm's end effector reaches waypoint B, the time will be equal to the time at which it should be at waypoint B. When this occurs, the code increments the waypoint
    counter. Once the waypoint counter updates, the next end time is computed, and the arm updates its waypoint coordinates, such that waypoint B now becomes waypoint A, and the
    next desired position is loaded as waypoint B.
    */

    t = tcount / 1000.0;                // Get the current time step, in seconds
    ttotal = nextPose[wayCount].ttotal; // The time it will take to move from waypoint A to B, in seconds
    tcheck = tstart + ttotal;           // The end time when the arm reaches waypoint B, in seconds

    if(t < tcheck)
    {
        // Compute TS impedance controller terms and update them globally, using the current data
        getNextPose(t, tstart, nextPose[wayCount], nextPose[wayCount+1], theta1motor, theta2motor,theta3motor);
    }

    if(t == tcheck)
    {
        tstart += ttotal;   // Update tstart to include the time that passed to move from A to B
        wayCount += 1;      // Increment the waypoint counter
    }

    tcount += 1; // Increment the time step
    // *************************************************************************************************************************************************************************

    // Lab 3: Task space impedance control with friciton compensation **********************************************************************************************************
    *tau1 = icx + ff1*ufric1;
    *tau2 = icy + ff2*ufric2;
    *tau3 = icz + ff3*ufric3;
    // *************************************************************************************************************************************************************************

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

        theta1array[arrayindex] = theta1motor;
        theta2array[arrayindex] = theta2motor; // adding values for theta 2 array NRWMT
        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
    }

    // Lab 1: FK and IK computations of the end effector's world frame position ************************************************************************************************
    // These are in the code as they are used to print to the Tera Term console for position refinement.

    // FK equations of the CRS arm, in terms of motor thetas, using DH parameters
    DHx = (.254*cos(theta1motor)*(cos(theta3motor) + sin(theta2motor)));
    DHy = (.254*sin(theta1motor)*(cos(theta3motor) + sin(theta2motor)));
    DHz = (.254*cos(theta2motor)) - (.254*sin(theta3motor)) + .254;

    // Lab 1: IK **************************************************************************
    a = DHz - l1; // EE pose without the l1 length
    b = sqrt(DHx*DHx + DHy*DHy); // Projection of arm onto x-z plane

    // IK equations from geometry
    DHtheta1out  = atan2(DHy,DHx);
    DHtheta2out = atan2(a,b) + acos((l2*l2 - l3*l3 + (b*b + a*a))/(2*l2*sqrt(b*b+a*a)));
    DHtheta3out = PI - acos((l2*l2 + l3*l3 - b*b - a*a)/(2*l2*l3));

    // Converting IK thetas back to motor thetas
    Calcmt1 = DHtheta1out;
    Calcmt2 = -DHtheta2out + HALFPI; // -DHtheta2 since the angle was defined as positive in derivation
    Calcmt3 = DHtheta3out + Calcmt2 - HALFPI;
    // *************************************************************************************
    // *************************************************************************************************************************************************************************

    // Lab 1: Defining print values
    printtheta1motor = theta1motor;
    printtheta2motor = theta2motor;
    printtheta3motor = theta3motor;
    printx = DHx;
    printy = DHy;
    printz = DHz;

    Simulink_PlotVar1 = printy;
    Simulink_PlotVar2 = yDes;
    Simulink_PlotVar3 = printz;
    Simulink_PlotVar4 = zDes;

    mycount++;
}

void printing(void){
    if (whattoprint == 0) {
        // Print motor thetas from encoders, EE pose (x, y, z) from FK, DH thetas from IK, and motor thetas from conversion
        serial_printf(&SerialA, "ET1:%.2f, ET2:%.2f,ET3:%.2f, X:%.2f Y:%.2f Z:%.2f DH1:%.2f DH2:%.2f DH3:%.2f MT1:%.2f MT2:%.2f MT3:%.2f \r",printtheta1motor*180/PI,printtheta2motor*180/PI,printtheta3motor*180/PI, printx,printy,printz, DHtheta1out*180/PI,DHtheta2out*180/PI,DHtheta3out*180/PI,  Calcmt1*180/PI, Calcmt2*180/PI, Calcmt3*180/PI);
    } else {
        serial_printf(&SerialA, "Print test   \n\r");
    }
}

% This file is used to derive task space and rotated task space control for
% Lab 3. Some variables are changed from the MATLAB code when implemented
% into the C code. 

% Define all variables symbolically
syms t1 t2 t3 td1 td2 td3 kpx kpy kpz kdx kdy kdz xdes ydes zdes xdesdot ydesdot zdesdot l kt Zfcmd
syms tx ty tz xn yn zn kpxn kpyn kpzn kdxn kdyn kdzn xndes yndes zndes xndot yndot zndot xndotdes yndotdes zndotdes

% J^T as given in Lab 3, Part 2
Jt = [-l*sin(t1)*(cos(t3)+sin(t2)), l*cos(t1)*(cos(t3)+sin(t2)), 0;
    l*cos(t1)*(cos(t2)-sin(t3)), l*sin(t1)*(cos(t2)-sin(t3)), -l*(cos(t3)+sin(t2));
    -l*cos(t1)*sin(t3), -l*sin(t1)*sin(t3), -l*cos(t3)];

% FK equations of the CRS arm
x = l*cos(t1)*(cos(t3)+sin(t2));
y = l*sin(t1)*(cos(t3)+sin(t2));
z = l*(1+cos(t2)-sin(t3));

% Time derivatives of the FK equation to get xyz velocities
xdot = -l*sin(t1)*td1*(cos(t3)+sin(t2)) + l*cos(t1)*(-td3*sin(t3) + td2*cos(t2)); 
ydot = l*cos(t1)*td1*(cos(t3)+sin(t2)) + l*sin(t1)*(-td3*sin(t3) + td2*cos(t2)); 
zdot = l*(-td2*sin(t2)- td3*cos(t3));

% Task space errors with gains
F = [kpx*(xdes - x) + kdx*(xdesdot - xdot);
    kpy*(ydes - y) + kdy*(ydesdot - ydot);
    kpz*(zdes - z) + kdz*(zdesdot - zdot)];

% Get task space control equation vector (Parts 2 and 3)
ts = simplify(Jt*F)

% Z direction feedforward control vector
Fz = [0;0;Zfcmd/kt];

% Get feedforward force control vector (Part 3)
zout = simplify(Jt * Fz)

% Create rotation matrices for world to new and new to world rotations
Rz = [cos(tz), -sin(tz), 0;
    sin(tz), cos(tz), 0;
    0,0,1];
Rx = [1,0,0;
    0, cos(tx), -sin(tx) ;
    0, sin(tx), cos(tx)];

Ry = [cos(ty),0,sin(ty);
    0,1,0;
    -sin(ty),0,cos(ty)];

Rwn = Rz*Rx*Ry;
Rnw = transpose(Rwn);

% Create the impedance control vectors and gains
KP = diag([kpxn, kpyn,kpzn]);
KD = diag([kdxn,kdyn,kdzn]);
state = [xdes - x;
    ydes -  y;
    zdes - z];
statesdot = [xdesdot - xdot;
    ydesdot - ydot;
    zdesdot - zdot];

% Get the impedance control vector (Part 4)
impOut = simplify(Jt*Rwn*(KP*Rnw*state+KD*Rnw*statesdot))

Credits

Marshall Tenzer

2 projects • 0 followers

Neil Wagner

2 projects • 0 followers

ME 446 Final Project: Robot navigating an obstacle course

Things used in this project

Software apps and online services

Story

Objectives

Implementation

Code

Final.c

taskspace.m

CRS Robot Arm Repo

Credits

Marshall Tenzer

Neil Wagner

Comments

Embed the widget on your own site

ME 446 Final Project: Robot navigating an obstacle course

ME 446 Final Project: Robot navigating an obstacle course

Things used in this project

Software apps and online services

Story

Objectives

Implementation

Code

Final.c

taskspace.m

CRS Robot Arm Repo

Credits

Marshall Tenzer

Neil Wagner

Comments