File content as of revision 2:4e581e5b39e8:

#include "mbed.h"
#include <stdio.h>      /* printf */
#include <math.h>       /* cos */
#include "rtos.h"
#include "EthernetInterface.h"
#include "ExperimentServer.h"
#include "QEI.h"
#include "BezierCurve.h"
#include "MotorShield.h" 
#include "HardwareSetup.h"

#define BEZIER_ORDER_FOOT    7
#define NUM_INPUTS (21 + 2*(BEZIER_ORDER_FOOT+1))
#define NUM_OUTPUTS 29

#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)

// Initializations
Serial pc(USBTX, USBRX);    // USB Serial Terminal
ExperimentServer server;    // Object that lets us communicate with MATLAB
Timer t;                    // Timer to measure elapsed time of experiment

QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING);  // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING);  // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING);  // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)

float directionChange=-1;
MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
Ticker currentLoop;

// Variables for q1
float current1;
float current_des1 = 0;
float prev_current_des1 = 0;
float current_int1 = 0;
float angle1;
float velocity1;
float duty_cycle1;
float angle1_init;

// Variables for q2
float current2;
float current_des2 = 0;
float prev_current_des2 = 0;
float current_int2 = 0;
float angle2;
float velocity2;
float duty_cycle2;
float angle2_init;

//Variables for q3 (leg 2 q1)
float current3;
float current_des3 = 0;
float prev_current_des3 = 0;
float current_int3 = 0;
float angle3;
float velocity3;
float duty_cycle3;
float angle3_init;

//variables for q4 (leg 2 q2)
float current4;
float current_des4 = 0;
float prev_current_des4 = 0;
float current_int4 = 0;
float angle4;
float velocity4;
float duty_cycle4;
float angle4_init;

// Fixed kinematic parameters
const float l_OA=.011; 
const float l_OB=.042; 
const float l_AC=.096; 
const float l_DE=.091;

// Timing parameters
float current_control_period_us = 200.0f;     // 5kHz current control loop
float impedance_control_period_us = 1000.0f;  // 1kHz impedance control loop
float start_period, traj_period, end_period;

// Control parameters
float current_Kp = 4.0f;         
float current_Ki = 0.4f;           
float current_int_max = 3.0f;       
float duty_max;      
float K_xx;
float K_yy;
float K_xy;
float D_xx;
float D_xy;
float D_yy;


//Second foot:
float current_Kp2 = 4.0f;         
float current_Ki2 = 0.4f;           
float current_int_max2 = 3.0f;       
float duty_max2;      
float K_xx2;
float K_yy2;
float K_xy2;
float D_xx2;
float D_xy2;
float D_yy2;


// Model parameters
float supply_voltage = 12;     // motor supply voltage
float R = 2.0f;                // motor resistance
float k_t = 0.18f;             // motor torque constant
float nu = 0.0005;             // motor viscous friction

// Current control interrupt function
void CurrentLoop()
{
    // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
    
    //use the motor shield as follows:
    //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
        
    current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
    velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
    float err_c1 = current_des1 - current1;                                             // current errror
    current_int1 += err_c1;                                                             // integrate error
    current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max);      // anti-windup
    float ff1 = R*current_des1 + k_t*velocity1;                                         // feedforward terms
    duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage;   // PI current controller
    
    float absDuty1 = abs(duty_cycle1);
    if (absDuty1 > duty_max) {
        duty_cycle1 *= duty_max / absDuty1;
        absDuty1 = duty_max;
    }    
    if (duty_cycle1 < 0) { // backwards
        motorShield.motorAWrite(absDuty1, 1);
    } else { // forwards
        motorShield.motorAWrite(absDuty1, 0);
    }             
    prev_current_des1 = current_des1; 
    
    current2     = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f);       // measure current
    velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;                                  // measure velocity  
    float err_c2 = current_des2 - current2;                                             // current error
    current_int2 += err_c2;                                                             // integrate error
    current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max);      // anti-windup   
    float ff2 = R*current_des2 + k_t*velocity2;                                         // feedforward terms
    duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage;   // PI current controller
    
    float absDuty2 = abs(duty_cycle2);
    if (absDuty2 > duty_max) {
        duty_cycle2 *= duty_max / absDuty2;
        absDuty2 = duty_max;
    }    
    if (duty_cycle2 < 0) { // backwards
        motorShield.motorBWrite(absDuty2, 1);
    } else { // forwards
        motorShield.motorBWrite(absDuty2, 0);
    }             
    prev_current_des2 = current_des2; 
    
    
    current3 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
    velocity3 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
    float err_c3 = current_des3 - current3;                                             // current errror
    current_int3 += err_c3;                                                             // integrate error
    current_int3 = fmaxf( fminf(current_int3, current_int_max), -current_int_max);      // anti-windup
    float ff3 = R*current_des3 + k_t*velocity3;                                         // feedforward terms
    duty_cycle3 = (ff3 + current_Kp*err_c3 + current_Ki*current_int3)/supply_voltage;   // PI current controller
    
    float absDuty3 = abs(duty_cycle3);
    if (absDuty3 > duty_max) {
        duty_cycle3 *= duty_max / absDuty3;
        absDuty3 = duty_max;
    }    
    if (duty_cycle3 < 0) { // backwards
        motorShield.motorCWrite(absDuty3, 1);
    } else { // forwards
        motorShield.motorCWrite(absDuty3, 0);
    }             
    prev_current_des3 = current_des3; 
    
    
    current4 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
    velocity4 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
    float err_c4 = current_des4 - current4;                                             // current errror
    current_int4 += err_c4;                                                             // integrate error
    current_int4 = fmaxf( fminf(current_int4, current_int_max), -current_int_max);      // anti-windup
    float ff4 = R*current_des4 + k_t*velocity4;                                         // feedforward terms
    duty_cycle4 = (ff4 + current_Kp*err_c4 + current_Ki*current_int4)/supply_voltage;   // PI current controller
    
    float absDuty4 = abs(duty_cycle4);
    if (absDuty4 > duty_max) {
        duty_cycle4 *= duty_max / absDuty4;
        absDuty4 = duty_max;
    }    
    if (duty_cycle4 < 0) { // backwards
        motorShield.motorCWrite(absDuty4, 1);
    } else { // forwards
        motorShield.motorCWrite(absDuty4, 0);
    }             
    prev_current_des4 = current_des4;     
    
    
}

int main (void)
{
    
    // Object for 7th order Cartesian foot trajectory. 
    
    //CREATE A TRAJECTORY 
    BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
    
    // Link the terminal with our server and start it up
    server.attachTerminal(pc);
    server.init();
    
    // Continually get input from MATLAB and run experiments
    float input_params[NUM_INPUTS];
    pc.printf("%f",input_params[0]);
    
    while(1) {
        
        // If there are new inputs, this code will run
        if (server.getParams(input_params,NUM_INPUTS)) {
            
                        
            // Get inputs from MATLAB          
            start_period                = input_params[0];    // First buffer time, before trajectory
            traj_period                 = input_params[1];    // Trajectory time/length
            end_period                  = input_params[2];    // Second buffer time, after trajectory
    
            angle1_init                 = input_params[3];    // Initial angle for q1 (rad)
            angle2_init                 = input_params[4];    // Initial angle for q2 (rad)

            K_xx                        = input_params[5];    // Foot stiffness N/m
            K_yy                        = input_params[6];    // Foot stiffness N/m
            K_xy                        = input_params[7];    // Foot stiffness N/m
            D_xx                        = input_params[8];    // Foot damping N/(m/s)
            D_yy                        = input_params[9];    // Foot damping N/(m/s)
            D_xy                        = input_params[10];   // Foot damping N/(m/s)
            duty_max                    = input_params[11];   // Maximum duty factor
            
            angle3_init                 = input_params[12];
            angle4_init                 = input_params[13];
            
            K_xx2                       = input_params[14];
            K_yy2                       = input_params[15];
            K_xy2                       = input_params[16];
            D_xx2                       = input_params[17];
            D_yy2                       = input_params[18];
            D_xy2                       = input_params[19];
            duty_max2                   = input_params[20]
          
            // Get foot trajectory points
            float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
            for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
              foot_pts[i] = input_params[21+i];    
            }
            rDesFoot_bez.setPoints(foot_pts);
            
            // Attach current loop interrupt
            currentLoop.attach_us(CurrentLoop,current_control_period_us);
                        
            // Setup experiment
            t.reset();
            t.start();
            encoderA.reset();
            encoderB.reset();
            encoderC.reset();
            encoderD.reset();

            motorShield.motorAWrite(0, 0); //turn motor A off
            motorShield.motorBWrite(0, 0); //turn motor B off
            motorShield.motorCWrite(0, 0);
            motorShield.motorDWrite(0, 0);
                         
            // Run experiment
            while( t.read() < start_period + traj_period + end_period) { 
                 
                // Read encoders to get motor states
                angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;       
                velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
                 
                angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;       
                velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;    
                
                
                angle3 = encoderC.getPulses() *PULSE_TO_RAD + angle3_init;       
                velocity3 = encoderC.getVelocity() * PULSE_TO_RAD;
                 
                angle4 = encoderD.getPulses() * PULSE_TO_RAD + angle4_init;       
                velocity4 = encoderD.getVelocity() * PULSE_TO_RAD;   
            
                
                const float th1 = angle1;
                const float th2 = angle2;
                const float dth1= velocity1;
                const float dth2= velocity2;
 
 
                const float th3 = angle3;
                const float th4 = angle4;
                const float dth3= velocity3;
                const float dth4= velocity4;
                
                
                
                // Calculate the Jacobian
                float Jx_th1 = l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1);
                float Jx_th2 = l_AC*cos(th1+th2);
                float Jy_th1 = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
                float Jy_th2 = l_AC*sin(th1+th2);
                

                float Jx_th3 = l_AC*cos(th3+th4)+l_DE*cos(th3)+l_OB*cos(th3);
                float Jx_th4 = l_AC*cos(th3+th4);
                float Jy_th3 = l_AC*sin(th3+th4)+l_DE*sin(th3)+l_OB*sin(th3);
                float Jy_th4 = l_AC*sin(th3+th4);

                                
                // Calculate the forward kinematics (position and velocity)
                float xFoot = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
                float yFoot = -l_AC*cos(th1+th2)-l_DE*cos(th1)-l_OB*cos(th1);
                float dxFoot = Jx_th1*dth1+Jx_th2*dth2;
                float dyFoot = Jy_th1*dth1+Jy_th2*dth2;    
                
                
                float xFoot2 = l_AC*sin(th3+th4)+l_DE*sin(th3)+l_OB*sin(th3);
                float yFoot2 = -l_AC*cos(th3+th4)-l_DE*cos(th3)-l_OB*cos(th3);
                float dxFoot2 = Jx_th3*dth3+Jx_th4*dth4;
                float dyFoot2 = Jy_th3*dth3+Jy_th4*dth4;  
                   

                // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
                float teff  = 0;
                float vMult = 0;
                if( t < start_period) {
                    if (K_xx > 0 || K_yy > 0 || K_xx2 > 0 || K_yy2 >0) {
                        K_xx = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
                        K_yy = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
                        D_xx = 0.1;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
                        D_yy = 0.1;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
                        K_xy = 0;
                        D_xy = 0;
                        
                        K_xx2=1;
                        K_yy2=1;
                        D_xx2=0.1;
                        D_yy2=0.1;
                        D_xy2=0;
                        K_xy2=0;
                        
                        
                    }
                    teff = 0;
                }
                else if (t < start_period + traj_period)
                {
                    K_xx = input_params[5];  // Foot stiffness N/m
                    K_yy = input_params[6];  // Foot stiffness N/m
                    K_xy = input_params[7];  // Foot stiffness N/m
                    D_xx = input_params[8];  // Foot damping N/(m/s)
                    D_yy = input_params[9];  // Foot damping N/(m/s)
                    D_xy = input_params[10]; // Foot damping N/(m/s)
                    
                    K_xx2 = input_params[14];
                    K_yy2 = input_params[15];
                    K_xy2 = input_params[16];
                    D_xx2 = input_params[17];
                    D_yy2 = input_params[18];
                    D_xy2 = input_params[19];
            
            
                    teff = (t-start_period);
                    vMult = 1;
                }
                else
                {
                    teff = traj_period;
                    vMult = 0;
                }
                
                // Get desired foot positions and velocities
                float rDesFoot[2] , vDesFoot[2];
                rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
                rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
                vDesFoot[0]/=traj_period;
                vDesFoot[1]/=traj_period;
                vDesFoot[0]*=vMult;
                vDesFoot[1]*=vMult;
                
                float rDesFoot2[2] , vDesFoot2[2];
                float evalPoint = teff/traj_period+traj_period/2;
                if(evalPoint>traj_period) evalPoint=evalPoint-traj_period;
                rDesFoot_bez.evaluate(evalPoint,rDesFoo2);
                rDesFoot_bez.evaluateDerivative(evalPoint,vDesFoot2);
                vDesFoot2[0]/=traj_period;
                vDesFoot2[1]/=traj_period;
                vDesFoot2[0]*=vMult;
                vDesFoot2[1]*=vMult;
                
                // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
                float xFoot_inv = -rDesFoot[0];
                float yFoot_inv = rDesFoot[1];                
                float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
                float alpha = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
                float th2_des = -(3.14159f - alpha); 
                float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
                
                
                float xFoot_inv2 = -rDesFoot2[0];
                float yFoot_inv2 = rDesFoot2[1];                
                float l_OE = sqrt( (pow(xFoot_inv2,2) + pow(yFoot_inv2,2)) );
                float alpha2 = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
                float th3_des = -(3.14159f - alpha2); 
                float th4_des = -((3.14159f/2.0f) + atan2(yFoot_inv2,xFoot_inv2) - abs(asin( (l_AC/l_OE)*sin(alpha2) )));
                
                
                
                float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
                float dth1_des = (1.0f/dd) * (  Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
                float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
                
                float dd2 = (Jx_th3*Jy_th4 - Jx_th4*Jy_th3);
                float dth3_des = (1.0f/dd2) * (  Jy_th4*vDesFoot[0] - Jx_th4*vDesFoot[1] );
                float dth4_des = (1.0f/dd2) * ( -Jy_th3*vDesFoot[0] + Jx_th3*vDesFoot[1] );
        
                // Calculate error variables
                float e_x = 0;
                float e_y = 0;
                float de_x = 0;
                float de_y = 0;
                
                float e_x2 = 0;
                float e_y2 = 0;
                float de_x2 = 0;
                float de_y2 = 0;
        
                // Calculate virtual force on foot
                float fx = K_xx*(rDesFoot[0]-xFoot) +K_xy*(rDesFoot[1]-yFoot)+D_xx*(vDesFoot[0]-dxFoot)+D_xy*(vDesFoot[1]-dyFoot);
                float fy = K_xy*(rDesFoot[0]-xFoot) + K_yy*(rDesFoot[1]-yFoot) + D_xy*(vDesFoot[0]-xFoot)+D_yy*(vDesFoot[1]-dyFoot);
                
                float fx2 = K_xx2*(rDesFoot2[0]-xFoot2) +K_xy*(rDesFoot2[1]-yFoot2)+D_xx2*(vDesFoot2[0]-dxFoot2)+D_xy2*(vDesFoot2[1]-dyFoot2);
                float fy2 = K_xy2*(rDesFoot2[0]-xFoot2) + K_yy*(rDesFoot2[1]-yFoot2) + D_xy2*(vDesFoot2[0]-xFoot2)+D_yy2*(vDesFoot2[1]-dyFoot2);
                
                                                
                // Set desired currents             
                current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;          
                current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;   
                current_des3 = (Jx_th1*fx+Jy_th1*fy)/k_t;   
                current_des4 = (Jx_th2*fx+Jy_th2*fy)/k_t;          
       

        
                // Joint impedance
                // sub Kxx for K1, Dxx for D1, Kyy for K2, Dyy for D2
                // Note: Be careful with signs now that you have non-zero desired angles!
                // Your equations should be of the form i_d = K1*(q1_d - q1) + D1*(dq1_d - dq1)
                
                //PART 0A FIRST
//                float q1_d=0;
//                float dq1_d=0;
//                current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;    
//                current_des2 = 0;      

//              PART 2   
//                float q1_d=th1_des;
//                float dq1_d=dth1_des;                
//                float q2_d=th2_des;
//                float dq2_d=dth2_des;
//                current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;
//                current_des2 = (K_yy*(q2_d-th2) + D_yy*(dq2_d-dth2))/k_t;    



    /*  PART 3!!!!!!!!!!!!!!!!*/     
                
                // Cartesian impedance  
                // Note: As with the joint space laws, be careful with signs!              
//                current_des1 = 0;          
//                current_des2 = 0;   
                
                
                // Form output to send to MATLAB     
                float output_data[NUM_OUTPUTS];
                // current time
                output_data[0] = t.read();
                // motor 1 state
                output_data[1] = angle1;
                output_data[2] = velocity1;  
                output_data[3] = current1;
                output_data[4] = current_des1;
                output_data[5] = duty_cycle1;
                // motor 2 state
                output_data[6] = angle2;
                output_data[7] = velocity2;
                output_data[8] = current2;
                output_data[9] = current_des2;
                output_data[10]= duty_cycle2;
                // motor 3 state
                output_data[11] = angle2;
                output_data[12 = velocity2;
                output_data[13] = current2;
                output_data[14] = current_des2;
                output_data[15]= duty_cycle2;
                // motor 4 state
                output_data[16] = angle2;
                output_data[17 = velocity2;
                output_data[18] = current2;
                output_data[19] = current_des2;
                output_data[20]= duty_cycle2;
                // foot state
                output_data[21] = xFoot;
                output_data[22] = yFoot;
                output_data[23] = dxFoot;
                output_data[24] = dyFoot;
                output_data[25] = rDesFoot[0];
                output_data[26] = rDesFoot[1];
                output_data[27] = vDesFoot[0];
                output_data[28] = vDesFoot[1];
                
                // Send data to MATLAB
                server.sendData(output_data,NUM_OUTPUTS);

                wait_us(impedance_control_period_us);   
            }
            
            // Cleanup after experiment
            server.setExperimentComplete();
            currentLoop.detach();
            motorShield.motorAWrite(0, 0); //turn motor A off
            motorShield.motorBWrite(0, 0); //turn motor B off
            motorShield.motorCWrite(0,0);
            motorShield.motorDWrite(0,0);
        
        } // end if
        
    } // end while
    
} // end main