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 BEZIER_ORDER_TORQUE    2 /// if x -> plots x+1 points, so should have x+1 Bezier points for T1 and T2 in MATLAB
00012 //#define NUM_INPUTS (14 + 2*(BEZIER_ORDER_FOOT+1))
00013 #define NUM_INPUTS (16 + 2*(BEZIER_ORDER_TORQUE+1))
00014 #define NUM_OUTPUTS 21
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 // Variables for q1
00032 float current1;
00033 float current_des1 = 0;
00034 float prev_current_des1 = 0;
00035 float current_int1 = 0;
00036 float angle1;
00037 float angle_des1;
00038 float velocity1;
00039 float velocity_des1;
00040 float duty_cycle1;
00041 float angle1_init;
00042 float shoulder;
00043 
00044 // Variables for q2
00045 float current2;
00046 float current_des2 = 0;
00047 float prev_current_des2 = 0;
00048 float current_int2 = 0;
00049 float angle2;
00050 float angle_des2;
00051 float velocity2;
00052 float velocity_des2;
00053 float duty_cycle2;
00054 float angle2_init;
00055 float elbow;
00056 
00057 float l1;
00058 float l2;
00059 
00060 // Timing parameters
00061 float current_control_period_us = 200.0f;     // 5kHz current control loop
00062 float impedance_control_period_us = 1000.0f;  // 1kHz impedance control loop
00063 float pre_buffer_time, traj_period, post_buffer_time;
00064 
00065 // Control parameters
00066 float current_Kp = 4.0f;         
00067 float current_Ki = 0.4f;           
00068 float current_int_max = 3.0f;       
00069 float duty_max;      
00070 float K_xx;
00071 float K_yy;
00072 float K_xy;
00073 float D_xx;
00074 float D_xy;
00075 float D_yy;
00076 
00077 // Model parameters
00078 float supply_voltage = 12;     // motor supply voltage
00079 float R = 2.0f;                // motor resistance
00080 float k_t = 0.18f;             // motor torque constant
00081 float nu = 0.0005;             // motor viscous friction
00082 
00083 // Current control interrupt function
00084 void CurrentLoop()
00085 {
00086     // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
00087     
00088     //use the motor shield as follows:
00089     //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
00090         
00091     current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current DO WE NEED TO ADJUST GEAR RATIO HERE?
00092     velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
00093     float err_c1 = current_des1 - current1;                                             // current errror
00094     current_int1 += err_c1;                                                             // integrate error
00095     current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max);      // anti-windup
00096     float ff1 = R*current_des1 + k_t*velocity1;                                         // feedforward terms
00097     duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage;   // PI current controller
00098     
00099     float absDuty1 = abs(duty_cycle1);
00100     if (absDuty1 > duty_max) {
00101         duty_cycle1 *= duty_max / absDuty1;
00102         absDuty1 = duty_max;
00103     }    
00104     if (duty_cycle1 < 0) { // backwards
00105         motorShield.motorAWrite(absDuty1, 1);
00106     } else { // forwards
00107         motorShield.motorAWrite(absDuty1, 0);
00108     }             
00109     prev_current_des1 = current_des1; 
00110     
00111     current2     = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f);       // measure current 
00112     velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;                                  // measure velocity  
00113     float err_c2 = current_des2 - current2;                                             // current error
00114     current_int2 += err_c2;                                                             // integrate error
00115     current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max);      // anti-windup   
00116     float ff2 = R*current_des2 + k_t*velocity2;                                         // feedforward terms
00117     duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage;   // PI current controller
00118     
00119     float absDuty2 = abs(duty_cycle2);
00120     if (absDuty2 > duty_max) {
00121         duty_cycle2 *= duty_max / absDuty2;
00122         absDuty2 = duty_max;
00123     }    
00124     if (duty_cycle2 < 0) { // backwards
00125         motorShield.motorBWrite(absDuty2, 1);
00126     } else { // forwards
00127         motorShield.motorBWrite(absDuty2, 0);
00128     }             
00129     prev_current_des2 = current_des2; 
00130     
00131 }
00132 
00133 int main (void)
00134 {
00135     
00136     // Object for 7th order Cartesian foot trajectory
00137     BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
00138     BezierCurve torque_bez(2,BEZIER_ORDER_TORQUE);
00139     
00140     // Link the terminal with our server and start it up
00141     server.attachTerminal(pc);
00142     server.init();
00143     
00144     // Continually get input from MATLAB and run experiments
00145     float input_params[NUM_INPUTS];
00146     pc.printf("%f",input_params[0]);
00147     
00148     while(1) {
00149         
00150         // If there are new inputs, this code will run
00151         if (server.getParams(input_params,NUM_INPUTS)) {
00152             
00153                         
00154             // Get inputs from MATLAB          
00155             pre_buffer_time             = input_params[0];    // First buffer time, before trajectory
00156             traj_period                 = input_params[1];    // Trajectory time/length
00157             post_buffer_time            = input_params[2];    // Second buffer time, after trajectory
00158     
00159             angle1_init                 = input_params[3];    // Initial angle for q1 (rad)
00160             angle2_init                 = input_params[4];    // Initial angle for q2 (rad)
00161             shoulder                    = input_params[5];
00162             elbow                       = input_params[6];
00163 
00164             K_xx                        = input_params[7];    // Foot stiffness N/m
00165             K_yy                        = input_params[8];    // Foot stiffness N/m
00166             K_xy                        = input_params[9];    // Foot stiffness N/m
00167             D_xx                        = input_params[10];    // Foot damping N/(m/s)
00168             D_yy                        = input_params[11];    // Foot damping N/(m/s)
00169             D_xy                        = input_params[12];   // Foot damping N/(m/s)
00170             duty_max                    = input_params[13];   // Maximum duty factor
00171             l1                          = input_params[14];   // Length of 1st link
00172             l2                          = input_params[15];   // Length of 2nd link
00173           
00174             // Get foot trajectory points
00175             float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
00176             for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
00177               foot_pts[i]               = input_params[16+i]; // Bezier curve points
00178             }
00179             rDesFoot_bez.setPoints(foot_pts);
00180             
00181             // Get torque traj points
00182             float torque_pts[2*(BEZIER_ORDER_FOOT+1)];
00183             for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
00184               torque_pts[i] = input_params[16+i];    
00185             }
00186             torque_bez.setPoints(torque_pts);
00187             float torque_des[2];
00188             
00189             // Attach current loop interrupt
00190             currentLoop.attach_us(CurrentLoop,current_control_period_us);
00191                         
00192             // Setup experiment
00193             t.reset();
00194             t.start();
00195             encoderA.reset();
00196             encoderB.reset();
00197             encoderC.reset();
00198             encoderD.reset();
00199 
00200             motorShield.motorAWrite(0, 0); //turn motor A off
00201             motorShield.motorBWrite(0, 0); //turn motor B off
00202                          
00203             // Run experiment
00204             while( t.read() < pre_buffer_time + traj_period + post_buffer_time) { 
00205                  
00206                 // Read encoders to get motor states
00207                 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;       
00208                 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
00209                  
00210                 angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;       
00211                 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;           
00212                 
00213                 const float th1 = angle1;
00214                 const float th2 = angle2;
00215                 const float dth1= velocity1;
00216                 const float dth2= velocity2;
00217  
00218                 // Calculate the Jacobian
00219                 float Jx_th1 = l1*cos(th1)+l2*cos(th1+th2);
00220                 float Jx_th2 = l2*cos(th1+th2);
00221                 float Jy_th1 = l1*sin(th1)+l2*sin(th1+th2);
00222                 float Jy_th2 = l2*sin(th1+th2);
00223                 
00224 
00225                 // Calculate the forward kinematics (position and velocity) 
00226                 float xFoot = l2*sin(th1+th2) + l1*sin(th1);
00227                 float yFoot = -l2*cos(th1+th2) - l1*cos(th1);
00228                 float dxFoot = l1*cos(th1)*dth1 + l2*cos(th1+th2)*(dth1+dth2);
00229                 float dyFoot = l1*sin(th1)*dth1 + l2*sin(th1+th2)*(dth1+dth2);
00230                 
00231                 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
00232                 float teff  = 0;
00233                 float vMult = 0;
00234                 if( t < pre_buffer_time) { // impedance operational space to start point
00235                 
00236                     teff = 0.f;
00237                     float th1_d= shoulder;
00238                     float th2_d= -elbow;
00239                     float e_th1= th1_d-th1;
00240                     float e_th2= th2_d-th2;
00241                     
00242                     float e_dth1= -dth1;
00243                     float e_dth2= -dth2;
00244                     
00245                     torque_des[0] = K_xx*e_th1 + D_xx*e_dth1;
00246                     torque_des[1] = K_yy*e_th2 + D_yy*e_dth2;
00247            
00248                     current_des1 = torque_des[0]/k_t;          
00249                     current_des2 = torque_des[1]/k_t;
00250                 }
00251                 else if (t < pre_buffer_time + traj_period) // torque control to follow Bezier
00252                 {
00253                     K_xx = input_params[7];  // Foot stiffness N/m
00254                     K_yy = input_params[8];  // Foot stiffness N/m
00255                     K_xy = input_params[9];  // Foot stiffness N/m
00256                     D_xx = input_params[10];  // Foot damping N/(m/s)
00257                     D_yy = input_params[11];  // Foot damping N/(m/s)
00258                     D_xy = input_params[12]; // Foot damping N/(m/s)
00259                     
00260                     teff = (t-pre_buffer_time);
00261                     vMult = 1;
00262                     torque_bez.evaluate(teff/traj_period,torque_des);
00263                     current_des1 = torque_des[0]/k_t;          
00264                     current_des2 = torque_des[1]/k_t;
00265                 }
00266                 else // impedance operational space to end point
00267                 {
00268                     teff = 0.f;
00269                     float th1_d= shoulder;
00270                     float th2_d= -elbow;
00271                     float e_th1= th1_d-th1;
00272                     float e_th2= th2_d-th2;
00273                     
00274                     float e_dth1= -dth1;
00275                     float e_dth2= -dth2;
00276                     
00277                     torque_des[0] = (K_xx*e_th1 + D_xx*e_dth1)/2;
00278                     torque_des[1] = (K_yy*e_th2 + D_yy*e_dth2)/2;
00279            
00280                     current_des1 = torque_des[0]/k_t;          
00281                     current_des2 = torque_des[1]/k_t;
00282                     
00283                 }
00284                   
00285                 // Form output to send to MATLAB     
00286                 float output_data[NUM_OUTPUTS];
00287                 // current time
00288                 output_data[0] = t.read();
00289                 // motor 1 state
00290                 output_data[1] = angle1;
00291                 output_data[2] = velocity1;  
00292                 output_data[3] = current1;
00293                 output_data[4] = current_des1;
00294                 output_data[5] = duty_cycle1;
00295                 // motor 2 state
00296                 output_data[6] = angle2;
00297                 output_data[7] = velocity2;
00298                 output_data[8] = current2;
00299                 output_data[9] = current_des2;
00300                 output_data[10]= duty_cycle2;
00301                 // foot state
00302                 output_data[11] = xFoot;
00303                 output_data[12] = yFoot;
00304                 output_data[13] = dxFoot;
00305                 output_data[14] = dyFoot;
00306                 output_data[15] = current1*k_t;
00307                 output_data[16] = current2*k_t;
00308                 output_data[17] = torque_des[0];
00309                 output_data[18] = torque_des[1];
00310                 
00311                 // Send data to MATLAB
00312                 server.sendData(output_data,NUM_OUTPUTS);
00313 
00314                 wait_us(impedance_control_period_us);   
00315             }
00316             
00317             // Cleanup after experiment
00318             server.setExperimentComplete();
00319             currentLoop.detach();
00320             motorShield.motorAWrite(0, 0); //turn motor A off
00321             motorShield.motorBWrite(0, 0); //turn motor B off
00322         
00323         } // end if
00324         
00325     } // end while
00326     
00327 } // end main