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