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