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