main_sebi.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 //
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 start_period, traj_period, end_period;
00064 //
00065 //// Hardware kinematic parameters -- NEED PAN PARAMETERS
00066 //const float l_c1; //upper arm center of mass
00067 //const float l_B; //upper arm length
00068 //const float r_c2; //lower arm center of mass
00069 //const float l_C; //lower arm length
00070 //
00071 //const float m1; //mass of upper arm
00072 //const float m2; //mass of lower arm
00073 //
00074 //const float I1; //upper arm interia
00075 //const float I2;  //lower arm inertia
00076 //
00077 //const float N; //gear ratio
00078 //const float Ir; //motor inertia
00079 //
00080 //// Control parameters
00081 //float current_Kp = 4.0f;         
00082 //float current_Ki = 0.4f;           
00083 //float current_int_max = 3.0f;       
00084 //float duty_max;      
00085 //float K_xx;
00086 //float K_yy;
00087 //float K_xy;
00088 //float D_xx;
00089 //float D_xy;
00090 //float D_yy;
00091 //
00092 //// Model parameters
00093 //float supply_voltage = 12;     // motor supply voltage
00094 //float R = 2.0f;                // motor resistance
00095 //float k_t = 0.18f;             // motor torque constant
00096 //float nu = 0.0005;             // motor viscous friction
00097 //
00098 //// Current control interrupt function
00099 //void CurrentLoop()
00100 //{
00101 //    // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
00102 //    
00103 //    //use the motor shield as follows:
00104 //    //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
00105 //        
00106 //    current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
00107 //    velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
00108 //    float err_c1 = current_des1 - current1;                                             // current errror
00109 //    current_int1 += err_c1;                                                             // integrate error
00110 //    current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max);      // anti-windup
00111 //    float ff1 = R*current_des1 + k_t*velocity1;                                         // feedforward terms
00112 //    duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage;   // PI current controller
00113 //    
00114 //    float absDuty1 = abs(duty_cycle1);
00115 //    if (absDuty1 > duty_max) {
00116 //        duty_cycle1 *= duty_max / absDuty1;
00117 //        absDuty1 = duty_max;
00118 //    }    
00119 //    if (duty_cycle1 < 0) { // backwards
00120 //        motorShield.motorAWrite(absDuty1, 1);
00121 //    } else { // forwards
00122 //        motorShield.motorAWrite(absDuty1, 0);
00123 //    }             
00124 //    prev_current_des1 = current_des1; 
00125 //    
00126 //    current2     = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f);       // measure current
00127 //    velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;                                  // measure velocity  
00128 //    float err_c2 = current_des2 - current2;                                             // current error
00129 //    current_int2 += err_c2;                                                             // integrate error
00130 //    current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max);      // anti-windup   
00131 //    float ff2 = R*current_des2 + k_t*velocity2;                                         // feedforward terms
00132 //    duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage;   // PI current controller
00133 //    
00134 //    float absDuty2 = abs(duty_cycle2);
00135 //    if (absDuty2 > duty_max) {
00136 //        duty_cycle2 *= duty_max / absDuty2;
00137 //        absDuty2 = duty_max;
00138 //    }    
00139 //    if (duty_cycle2 < 0) { // backwards
00140 //        motorShield.motorBWrite(absDuty2, 1);
00141 //    } else { // forwards
00142 //        motorShield.motorBWrite(absDuty2, 0);
00143 //    }             
00144 //    prev_current_des2 = current_des2; 
00145 //}
00146 //
00147 //int main (void)
00148 //{
00149 //    // Object for 7th order Cartesian foot trajectory
00150 //    BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
00151 //    
00152 //    // Link the terminal with our server and start it up
00153 //    server.attachTerminal(pc);
00154 //    server.init();
00155 //    
00156 //    // Continually get input from MATLAB and run experiments
00157 //    float input_params[NUM_INPUTS];
00158 //    pc.printf("%f",input_params[0]);
00159 //    
00160 //    while(1) {
00161 //        // If there are new inputs, this code will run
00162 //        if (server.getParams(input_params,NUM_INPUTS)) {
00163 //            // Get inputs from MATLAB          
00164 //            start_period = input_params[0]; // First buffer time, before trajectory
00165 //            traj_period = input_params[1]; // Trajectory time/length
00166 //            end_period = input_params[2]; // Second buffer time, after trajectory
00167 //            
00168 //            l_c1 = input_params[3]; //upper arm center of mass
00169 //            l_B = input_params[4]; //upper arm length
00170 //            l_c2 = input_params[5]; //lower arm center of mass
00171 //            l_C = input_params[6]; //lower arm length
00172 //            
00173 //            m1 = input_params[7]; //mass of upper arm
00174 //            m2 = input_params[8]; //mass of lower arm
00175 //            
00176 //            I1 = input_params[9]; //upper arm interia
00177 //            I2 = input_params[10];  //lower arm inertia
00178 //            
00179 //            N = input_params[11]; //gear ratio
00180 //            Ir = input_params[12]; //motor inertia
00181 //    
00182 //            angle1_init = input_params[13]; // Initial angle for q1 (rad)
00183 //            angle2_init = input_params[14]; // Initial angle for q2 (rad)
00184 //
00185 //            K_xx = input_params[15]; // Foot stiffness N/m
00186 //            K_yy = input_params[16]; // Foot stiffness N/m
00187 //            K_xy = input_params[17]; // Foot stiffness N/m
00188 //            D_xx = input_params[18]; // Foot damping N/(m/s)
00189 //            D_yy = input_params[19]; // Foot damping N/(m/s)
00190 //            D_xy = input_params[20]; // Foot damping N/(m/s)
00191 //            duty_max = input_params[21]; // Maximum duty factor
00192 //          
00193 //            // Get foot trajectory points
00194 //            float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
00195 //            for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
00196 //              foot_pts[i] = input_params[12+i];    
00197 //            }
00198 //            rDesFoot_bez.setPoints(foot_pts);
00199 //            
00200 //            // Attach current loop interrupt
00201 //            currentLoop.attach_us(CurrentLoop,current_control_period_us);
00202 //                        
00203 //            // Setup experiment
00204 //            t.reset();
00205 //            t.start();
00206 //            encoderA.reset();
00207 //            encoderB.reset();
00208 //            encoderC.reset();
00209 //            encoderD.reset();
00210 //
00211 //            motorShield.motorAWrite(0, 0); //turn motor A off
00212 //            motorShield.motorBWrite(0, 0); //turn motor B off
00213 //                         
00214 //            // Run experiment
00215 //            while( t.read() < start_period + traj_period + end_period) { 
00216 //                 
00217 //                // Read encoders to get motor states
00218 //                angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;       
00219 //                velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
00220 //                 
00221 //                angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;       
00222 //                velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;           
00223 //                
00224 //                const float th1 = angle1;
00225 //                const float th2 = angle2;
00226 //                const float dth1= velocity1;
00227 //                const float dth2= velocity2;
00228 // 
00229 //                // Calculate the Jacobian
00230 //                float Jx_th1 = l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1);
00231 //                float Jx_th2 = l_AC*cos(th1+th2);
00232 //                float Jy_th1 = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
00233 //                float Jy_th2 = l_AC*sin(th1+th2);
00234 //                                
00235 //                // Calculate the forward kinematics (position and velocity)
00236 //                float xFoot = l_DE*sin(th1)+l_OB*sin(th1)+l_AC*sin(th1+th2);
00237 //                float yFoot = -l_DE*cos(th1)-l_OB*cos(th1)-l_AC*cos(th1+th2);
00238 //                float dxFoot = dth1*(l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1))+dth2*l_AC*cos(th1+th2);
00239 //                float dyFoot = dth1*(l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1))+dth2*l_AC*sin(th1+th2); 
00240 // 
00241 //
00242 //                // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
00243 //                float teff  = 0;
00244 //                float vMult = 0;
00245 //                if( t < start_period) {
00246 //                    if (K_xx > 0 || K_yy > 0) {
00247 //                        K_xx = 100; 
00248 //                        K_yy = 100; 
00249 //                        D_xx = 5;  
00250 //                        D_yy = 5;  
00251 //                        K_xy = 0;
00252 //                        D_xy = 0;
00253 //                    }
00254 //                    teff = 0;
00255 //                }
00256 //                else if (t < start_period + traj_period)
00257 //                {
00258 //                    K_xx = input_params[15];  // Foot stiffness N/m
00259 //                    K_yy = input_params[16];  // Foot stiffness N/m
00260 //                    K_xy = input_params[17];  // Foot stiffness N/m
00261 //                    D_xx = input_params[18];  // Foot damping N/(m/s)
00262 //                    D_yy = input_params[19];  // Foot damping N/(m/s)
00263 //                    D_xy = input_params[20]; // Foot damping N/(m/s)
00264 //                    teff = (t-start_period);
00265 //                    vMult = 1;
00266 //                }
00267 //                else
00268 //                {
00269 //                    teff = traj_period;
00270 //                    vMult = 0;
00271 //                }
00272 //                
00273 //                // Get desired foot positions and velocities
00274 //                float rDesFoot[2] , vDesFoot[2];
00275 //                rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
00276 //                rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
00277 //                vDesFoot[0]/=traj_period;
00278 //                vDesFoot[1]/=traj_period;
00279 //                vDesFoot[0]*=vMult;
00280 //                vDesFoot[1]*=vMult;
00281 //                
00282 //                // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
00283 //                float xFootd = -rDesFoot[0];
00284 //                float yFootd = rDesFoot[1];                
00285 //                float l_OE = sqrt( (pow(xFootd,2) + pow(yFootd,2)) );
00286 //                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)) ));
00287 //                float th2_des = -(3.14159f - alpha); 
00288 //                float th1_des = -((3.14159f/2.0f) + atan2(yFootd,xFootd) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
00289 //                
00290 //                float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
00291 //                float dth1_des = (1.0f/dd) * (  Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
00292 //                float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
00293 //        
00294 //                // Calculate error variables
00295 //                float e_x = 0;
00296 //                float e_y = 0;
00297 //                float de_x = 0;
00298 //                float de_y = 0;
00299 //        
00300 //                // Calculate virtual force on foot
00301 //                float xdelta = -xFootd - xFoot;
00302 //                float ydelta = yFootd - yFoot;
00303 //                float dydelta = vDesFoot[1] - dyFoot;
00304 //                float dxdelta = vDesFoot[0] - dxFoot;
00305 //                float fx = K_xx*xdelta+K_xy*ydelta+D_xx*dxdelta+D_xy*dydelta;
00306 //                float fy = K_yy*ydelta+K_xy*xdelta+D_yy*dydelta+D_xy*dxdelta;
00307 //                
00308 //                float t1 = Jx_th1*fx + Jy_th1*fy;
00309 //                float t2 = Jx_th2*fx + Jy_th2*fy;
00310 //                
00311 //                // Calculate mass matrix elements
00312 //                float M11 = I1 + I2 + I3 + I4 + Ir + Ir*N*N + l_AC*l_AC*m4 + l_A_m3*l_A_m3*m3 + l_B_m2*l_B_m2*m2 + l_C_m4*l_C_m4*m4 + l_OA*l_OA*m3 + l_OB*l_OB*m2 + l_OA*l_OA*m4 + l_O_m1*l_O_m1*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); 
00313 //                float M12 = I2 + I3 + l_AC*l_AC*m4 + l_A_m3*l_A_m3*m3 + l_B_m2*l_B_m2*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); 
00314 //                float M22 = Ir*N*N + m4*l_AC*l_AC + m3*l_A_m3*l_A_m3 + m2*l_B_m2*l_B_m2 + I2 + I3;
00315 // 
00316 //                // Populate mass matrix
00317 //                MassMatrix.Clear();
00318 //                MassMatrix << M11 << M12
00319 //                           << M12 << M22;
00320 //                
00321 //                // Populate Jacobian matrix
00322 //                Jacobian.Clear();
00323 //                Jacobian << Jx_th1 << Jx_th2
00324 //                         << Jy_th1 << Jy_th2;
00325 //                
00326 //                // Calculate Lambda matrix
00327 //                JacobianT = MatrixMath::Transpose(Jacobian);
00328 //                InverseMassMatrix = MatrixMath::Inv(MassMatrix);
00329 //                temp_product = Jacobian*InverseMassMatrix*JacobianT;
00330 //                Lambda = MatrixMath::Inv(temp_product); 
00331 //                
00332 //                // Pull elements of Lambda matrix
00333 //                float L11 = Lambda.getNumber(1,1);
00334 //                float L12 = Lambda.getNumber(1,2);
00335 //                float L21 = Lambda.getNumber(2,1);
00336 //                float L22 = Lambda.getNumber(2,2);      
00337 //                
00338 //                // Calculate desired motor torque
00339 //                float t1_op = (Jx_th1*L11+Jy_th1*L21)*fx + (Jx_th1*L12+Jy_th1*L22)*fy;
00340 //                float t2_op = (Jx_th2*L11+Jy_th2*L21)*fx + (Jx_th2*L12+Jy_th2*L22)*fy;      
00341 //                                
00342 //                // Set desired currents             
00343 //                current_des1 = t1_op/k_t;          
00344 //                current_des2 = t2_op/k_t;   
00345 //
00346 //                // Form output to send to MATLAB     
00347 //                float output_data[NUM_OUTPUTS];
00348 //                // current time
00349 //                output_data[0] = t.read();
00350 //                // motor 1 state
00351 //                output_data[1] = angle1;
00352 //                output_data[2] = velocity1;  
00353 //                output_data[3] = current1;
00354 //                output_data[4] = current_des1;
00355 //                output_data[5] = duty_cycle1;
00356 //                // motor 2 state
00357 //                output_data[6] = angle2;
00358 //                output_data[7] = velocity2;
00359 //                output_data[8] = current2;
00360 //                output_data[9] = current_des2;
00361 //                output_data[10]= duty_cycle2;
00362 //                // foot state
00363 //                output_data[11] = xFoot;
00364 //                output_data[12] = yFoot;
00365 //                output_data[13] = dxFoot;
00366 //                output_data[14] = dyFoot;
00367 //                output_data[15] = rDesFoot[0];
00368 //                output_data[16] = rDesFoot[1];
00369 //                output_data[17] = vDesFoot[0];
00370 //                output_data[18] = vDesFoot[1];
00371 //                
00372 //                // Send data to MATLAB
00373 //                server.sendData(output_data,NUM_OUTPUTS);
00374 //
00375 //                wait_us(impedance_control_period_us);   
00376 //            }
00377 //            
00378 //            // Cleanup after experiment
00379 //            server.setExperimentComplete();
00380 //            currentLoop.detach();
00381 //            motorShield.motorAWrite(0, 0); //turn motor A off
00382 //            motorShield.motorBWrite(0, 0); //turn motor B off
00383 //        } // end if
00384 //    } // end while
00385 //} // end main