main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "EthernetInterface.h"
00004 #include "ExperimentServer.h"
00005 #include "QEI.h"
00006 #include "BezierCurve.h"
00007 #include "MotorShield.h" 
00008 #include "HardwareSetup.h"
00009 #include "Matrix.h"
00010 #include "MatrixMath.h"
00011 
00012 #define BEZIER_ORDER_FOOT    7
00013 #define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
00014 #define NUM_OUTPUTS 19
00015 
00016 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
00017 
00018 // Initializations
00019 Serial pc(USBTX, USBRX);    // USB Serial Terminal
00020 ExperimentServer server;    // Object that lets us communicate with MATLAB
00021 Timer t;                    // Timer to measure elapsed time of experiment
00022 
00023 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING);  // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
00024 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING);  // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
00025 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING);  // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
00026 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
00027 
00028 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
00029 Ticker currentLoop;
00030 
00031 Matrix MassMatrix(2,2);
00032 Matrix Jacobian(2,2);
00033 Matrix JacobianT(2,2);
00034 Matrix InverseMassMatrix(2,2);
00035 Matrix temp_product(2,2);
00036 Matrix Lambda(2,2);
00037 
00038 // Variables for q1
00039 float current1;
00040 float current_des1 = 0;
00041 float prev_current_des1 = 0;
00042 float current_int1 = 0;
00043 float angle1;
00044 float velocity1;
00045 float duty_cycle1;
00046 float angle1_init;
00047 
00048 // Variables for q2
00049 float current2;
00050 float current_des2 = 0;
00051 float prev_current_des2 = 0;
00052 float current_int2 = 0;
00053 float angle2;
00054 float velocity2;
00055 float duty_cycle2;
00056 float angle2_init;
00057 
00058 // Fixed kinematic parameters
00059 const float l_OA=.011; 
00060 const float l_OB=.042; 
00061 const float l_AC=.096; 
00062 const float l_DE=.091;
00063 const float m1 =.0393 + .2;
00064 const float m2 =.0368; 
00065 const float m3 = .00783;
00066 const float m4 = .0155;
00067 const float I1 = 0.0000251;  //25.1 * 10^-6;
00068 const float I2 = 0.0000535;  //53.5 * 10^-6;
00069 const float I3 = 0.00000925; //9.25 * 10^-6;
00070 const float I4 = 0.0000222;  //22.176 * 10^-6;
00071 const float l_O_m1=0.032;
00072 const float l_B_m2=0.0344; 
00073 const float l_A_m3=0.0622;
00074 const float l_C_m4=0.0610;
00075 const float N = 18.75;
00076 const float Ir = 0.0035/pow(N,2);
00077 
00078 // Timing parameters
00079 float current_control_period_us = 200.0f;     // 5kHz current control loop
00080 float impedance_control_period_us = 1000.0f;  // 1kHz impedance control loop
00081 float start_period, traj_period, end_period;
00082 
00083 // Control parameters
00084 float current_Kp = 4.0f;         
00085 float current_Ki = 0.4f;           
00086 float current_int_max = 3.0f;       
00087 float duty_max;      
00088 float K_xx;
00089 float K_yy;
00090 float K_xy;
00091 float D_xx;
00092 float D_xy;
00093 float D_yy;
00094 
00095 // Model parameters
00096 float supply_voltage = 12;     // motor supply voltage
00097 float R = 2.0f;                // motor resistance
00098 float k_t = 0.18f;             // motor torque constant
00099 float nu = 0.0005;             // motor viscous friction
00100 
00101 // Current control interrupt function
00102 void CurrentLoop()
00103 {
00104     // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
00105     
00106     //use the motor shield as follows:
00107     //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
00108         
00109     current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
00110     velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
00111     float err_c1 = current_des1 - current1;                                             // current errror
00112     current_int1 += err_c1;                                                             // integrate error
00113     current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max);      // anti-windup
00114     float ff1 = R*current_des1 + k_t*velocity1;                                         // feedforward terms
00115     duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage;   // PI current controller
00116     
00117     float absDuty1 = abs(duty_cycle1);
00118     if (absDuty1 > duty_max) {
00119         duty_cycle1 *= duty_max / absDuty1;
00120         absDuty1 = duty_max;
00121     }    
00122     if (duty_cycle1 < 0) { // backwards
00123         motorShield.motorAWrite(absDuty1, 1);
00124     } else { // forwards
00125         motorShield.motorAWrite(absDuty1, 0);
00126     }             
00127     prev_current_des1 = current_des1; 
00128     
00129     current2     = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f);       // measure current
00130     velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;                                  // measure velocity  
00131     float err_c2 = current_des2 - current2;                                             // current error
00132     current_int2 += err_c2;                                                             // integrate error
00133     current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max);      // anti-windup   
00134     float ff2 = R*current_des2 + k_t*velocity2;                                         // feedforward terms
00135     duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage;   // PI current controller
00136     
00137     float absDuty2 = abs(duty_cycle2);
00138     if (absDuty2 > duty_max) {
00139         duty_cycle2 *= duty_max / absDuty2;
00140         absDuty2 = duty_max;
00141     }    
00142     if (duty_cycle2 < 0) { // backwards
00143         motorShield.motorBWrite(absDuty2, 1);
00144     } else { // forwards
00145         motorShield.motorBWrite(absDuty2, 0);
00146     }             
00147     prev_current_des2 = current_des2; 
00148     
00149 }
00150 
00151 int main (void)
00152 {
00153     
00154     // Object for 7th order Cartesian foot trajectory
00155     BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
00156     
00157     // Link the terminal with our server and start it up
00158     server.attachTerminal(pc);
00159     server.init();
00160     
00161     // Continually get input from MATLAB and run experiments
00162     float input_params[NUM_INPUTS];
00163     pc.printf("%f",input_params[0]);
00164     
00165     while(1) {
00166         
00167         // If there are new inputs, this code will run
00168         if (server.getParams(input_params,NUM_INPUTS)) {
00169             
00170                         
00171             // Get inputs from MATLAB          
00172             start_period                = input_params[0];    // First buffer time, before trajectory
00173             traj_period                 = input_params[1];    // Trajectory time/length
00174             end_period                  = input_params[2];    // Second buffer time, after trajectory
00175     
00176             angle1_init                 = input_params[3];    // Initial angle for q1 (rad)
00177             angle2_init                 = input_params[4];    // Initial angle for q2 (rad)
00178 
00179             K_xx                        = input_params[5];    // Foot stiffness N/m
00180             K_yy                        = input_params[6];    // Foot stiffness N/m
00181             K_xy                        = input_params[7];    // Foot stiffness N/m
00182             D_xx                        = input_params[8];    // Foot damping N/(m/s)
00183             D_yy                        = input_params[9];    // Foot damping N/(m/s)
00184             D_xy                        = input_params[10];   // Foot damping N/(m/s)
00185             duty_max                    = input_params[11];   // Maximum duty factor
00186           
00187             // Get foot trajectory points
00188             float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
00189             for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
00190               foot_pts[i] = input_params[12+i];    
00191             }
00192             rDesFoot_bez.setPoints(foot_pts);
00193             
00194             // Attach current loop interrupt
00195             currentLoop.attach_us(CurrentLoop,current_control_period_us);
00196                         
00197             // Setup experiment
00198             t.reset();
00199             t.start();
00200             encoderA.reset();
00201             encoderB.reset();
00202             encoderC.reset();
00203             encoderD.reset();
00204 
00205             motorShield.motorAWrite(0, 0); //turn motor A off
00206             motorShield.motorBWrite(0, 0); //turn motor B off
00207             
00208             float M11=0;
00209             float M12=0;
00210             float M22=0;
00211             float Jx_th1=0;
00212             float Jx_th2=0;
00213             float Jy_th1=0;
00214             float Jy_th2=0;
00215             float L11=0;
00216             float L12=0;
00217             float L21=0;
00218             float L22=0;   
00219             // Run experiment
00220             while( t.read() < start_period + traj_period + end_period) { 
00221                  
00222                 // Read encoders to get motor states
00223                 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;       
00224                 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
00225                  
00226                 angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;       
00227                 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;           
00228                 
00229                 const float th1 = angle1;
00230                 const float th2 = angle2;
00231                 const float dth1= velocity1;
00232                 const float dth2= velocity2;
00233  
00234                 // Calculate the Jacobian
00235                 float Jx_th1 = l_AC*cos(th1 + th2) + l_DE*cos(th1) + l_OB*cos(th1);
00236                 float Jx_th2 = l_AC*cos(th1 + th2);
00237                 float Jy_th1 = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
00238                 float Jy_th2 = l_AC*sin(th1 + th2);
00239                                 
00240                 // Calculate the forward kinematics (position and velocity)
00241                 float xFoot = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
00242                 float yFoot = - l_AC*cos(th1 + th2) - l_DE*cos(th1) - l_OB*cos(th1);
00243                 float dxFoot = dth1*(l_AC*cos(th1 + th2) + l_DE*cos(th1) + l_OB*cos(th1)) + dth2*l_AC*cos(th1 + th2);
00244                 float dyFoot = dth1*(l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1)) + dth2*l_AC*sin(th1 + th2);          
00245 
00246                 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
00247                 float teff  = 0;
00248                 float vMult = 0;
00249                 if( t < start_period) {
00250                     if (K_xx > 0 || K_yy > 0) {
00251                         K_xx = 100; 
00252                         K_yy = 100; 
00253                         D_xx = 5;  
00254                         D_yy = 5;  
00255                         K_xy = 0;
00256                         D_xy = 0;
00257                     }
00258                     teff = 0;
00259                 }
00260                 else if (t < start_period + traj_period)
00261                 {
00262                     K_xx = input_params[5];  // Foot stiffness N/m
00263                     K_yy = input_params[6];  // Foot stiffness N/m
00264                     K_xy = input_params[7];  // Foot stiffness N/m
00265                     D_xx = input_params[8];  // Foot damping N/(m/s)
00266                     D_yy = input_params[9];  // Foot damping N/(m/s)
00267                     D_xy = input_params[10]; // Foot damping N/(m/s)
00268                     teff = (t-start_period);
00269                     vMult = 1;
00270                 }
00271                 else
00272                 {
00273                     teff = traj_period;
00274                     vMult = 0;
00275                 }
00276                 
00277                 // Get desired foot positions and velocities
00278                 float rDesFoot[2] , vDesFoot[2];
00279                 rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
00280                 rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
00281                 vDesFoot[0]/=traj_period;
00282                 vDesFoot[1]/=traj_period;
00283                 vDesFoot[0]*=vMult;
00284                 vDesFoot[1]*=vMult;
00285                 
00286                 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
00287                 float xFoot_inv = -rDesFoot[0];
00288                 float yFoot_inv = rDesFoot[1];                
00289                 float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
00290                 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)) ));
00291                 float th2_des = -(3.14159f - alpha); 
00292                 float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
00293                 
00294                 float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
00295                 float dth1_des = (1.0f/dd) * (  Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
00296                 float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
00297         
00298                 // Calculate error variables
00299                 float e_x = -xFoot_inv-xFoot;
00300                 float e_y = yFoot_inv-yFoot;
00301 //                float de_x = vDesFoot[0]-dxFoot;
00302 //                float de_y = vDesFoot[1]-dyFoot;
00303                 float de_x = -dxFoot;
00304                 float de_y = -dyFoot;
00305         
00306                 // Calculate virtual force on foot
00307                 float fx = K_xx*(e_x) + K_xy*(e_y) + D_xx*(de_x) + D_xy*(de_y);
00308                 float fy = K_xy*(e_x) + K_yy*(e_y) + D_xy*(de_x) + D_yy*(de_y);
00309                                 
00310                 //float Torque_q1= (Jx_th1*fx+Jy_th1*fy);
00311 //                float Torque_q2= (Jx_th2*fx+Jy_th2*fy);
00312                 
00313                 // Calculate mass matrix elements
00314                 M11 = I1 + I2 + I3 + I4 + Ir + Ir*pow(N,2) + pow(l_AC,2)*m4 + pow(l_A_m3,2)*m3 + pow(l_B_m2,2)*m2 + pow(l_C_m4,2)*m4 + pow(l_OA,2)*m3 + pow(l_OB,2)*m2 + pow(l_OA,2)*m4 + pow(l_O_m1,2)*m1 + 2*l_C_m4*l_OA*m4 + 2*l_AC*l_C_m4*m4*cos(th2) + 2*l_AC*l_OA*m4*cos(th2) + 2*l_A_m3*l_OA*m3*cos(th2) + 2*l_B_m2*l_OB*m2*cos(th2); 
00315                 //    I1 + I2 + I3 + I4 + Ir + Ir*N^2 +      l_AC^2*m4 +      l_A_m3^2*m3 +      l_B_m2^2*m2 +      l_C_m4^2*m4 +      l_OA^2*m3 +      l_OB^2*m2 +      l_OA^2*m4 +      l_O_m1^2*m1 +      2*l_C_m4*l_OA*m4 + 2*l_AC*l_C_m4*m4*cos(th2) + 2*l_AC*l_OA*m4*cos(th2) + 2*l_A_m3*l_OA*m3*cos(th2) + 2*l_B_m2*l_OB*m2*cos(th2)
00316                 M12 = I2 + I3 + pow(l_AC,2)*m4 + pow(l_A_m3,2)*m3 + pow(l_B_m2,2)*m2 + Ir*N + l_AC*l_C_m4*m4*cos(th2) + l_AC*l_OA*m4*cos(th2) + l_A_m3*l_OA*m3*cos(th2) + l_B_m2*l_OB*m2*cos(th2); 
00317                 //    I2 + I3 + l_AC^2*m4 +      l_A_m3^2*m3 +      l_B_m2^2*m2 +      Ir*N + l_AC*l_C_m4*m4*cos(th2) + l_AC*l_OA*m4*cos(th2) + l_A_m3*l_OA*m3*cos(th2) + l_B_m2*l_OB*m2*cos(th2)
00318                 M22 = Ir*pow(N,2) + m4*pow(l_AC,2) + m3*pow(l_A_m3,2) + m2*pow(l_B_m2,2) + I2 + I3;
00319                 //    Ir*N^2 + m4*l_AC^2 + m3*l_A_m3^2 + m2*l_B_m2^2 + I2 + I3
00320                 
00321                 
00322                 // Populate mass matrix
00323                 MassMatrix.Clear();
00324                 MassMatrix << M11 << M12
00325                            << M12 << M22;
00326                 
00327                 // Populate Jacobian matrix
00328                 Jacobian.Clear();
00329                 Jacobian << Jx_th1 << Jx_th2
00330                          << Jy_th1 << Jy_th2;
00331                 
00332                 // Once you have copied the elements of the mass matrix, uncomment the following section
00333                 
00334                 // Calculate Lambda matrix
00335                 JacobianT = MatrixMath::Transpose(Jacobian);
00336                 InverseMassMatrix = MatrixMath::Inv(MassMatrix);
00337                 temp_product = Jacobian*InverseMassMatrix*JacobianT;
00338                 Lambda = MatrixMath::Inv(temp_product); 
00339                 
00340                 // Pull elements of Lambda matrix
00341                 L11 = Lambda.getNumber(1,1);
00342                 L12 = Lambda.getNumber(1,2);
00343                 L21 = Lambda.getNumber(2,1);
00344                 L22 = Lambda.getNumber(2,2);               
00345                             
00346                                 
00347                 float Torque_q1= (Jx_th1*L11+Jy_th1*L12)*fx+(Jx_th1*L12+Jy_th1*L22)*fy;
00348                 float Torque_q2= (Jx_th2*L11+Jy_th2*L12)*fx+(Jx_th2*L12+Jy_th2*L22)*fy;
00349                         
00350                 // Set desired currents             
00351                 current_des1 = Torque_q1/k_t;           
00352                 current_des2 = Torque_q2/k_t; 
00353 
00354 
00355 
00356                 // Form output to send to MATLAB     
00357                 float output_data[NUM_OUTPUTS];
00358                 // current time
00359                 output_data[0] = t.read();
00360                 // motor 1 state
00361                 output_data[1] = angle1;
00362                 output_data[2] = velocity1;  
00363                 output_data[3] = current1;
00364                 output_data[4] = current_des1;
00365                 output_data[5] = duty_cycle1;
00366                 // motor 2 state
00367                 output_data[6] = angle2;
00368                 output_data[7] = velocity2;
00369                 output_data[8] = current2;
00370                 output_data[9] = current_des2;
00371                 output_data[10]= duty_cycle2;
00372                 // foot state
00373                 output_data[11] = xFoot;
00374                 output_data[12] = yFoot;
00375                 output_data[13] = dxFoot;
00376                 output_data[14] = dyFoot;
00377                 output_data[15] = rDesFoot[0];
00378                 output_data[16] = rDesFoot[1];
00379                 output_data[17] = vDesFoot[0];
00380                 output_data[18] = vDesFoot[1];
00381                 
00382                 // Send data to MATLAB
00383                 server.sendData(output_data,NUM_OUTPUTS);
00384 
00385                 wait_us(impedance_control_period_us);   
00386             }
00387             
00388             // Cleanup after experiment
00389             server.setExperimentComplete();
00390             currentLoop.detach();
00391             motorShield.motorAWrite(0, 0); //turn motor A off
00392             motorShield.motorBWrite(0, 0); //turn motor B off
00393             pc.printf("\n M11: %f", M11);
00394             pc.printf("\n M12: %f", M12);
00395             pc.printf("\n M22: %f", M22);
00396             pc.printf("\n Jx_th1: %f", Jx_th1);
00397             pc.printf("\n Jx_th2: %f", Jx_th2);
00398             pc.printf("\n Jy_th1: %f", Jy_th1);
00399             pc.printf("\n Jy_th2: %f", Jy_th2);
00400             pc.printf("\n L11: %f", L11);
00401             pc.printf("\n L12: %f", L12);
00402             pc.printf("\n L21: %f", L21);
00403             pc.printf("\n L22: %f", L22);
00404         
00405         } // end if
00406         
00407     } // end while
00408 //    using namespace std;
00409 } // end main
00410