Diff: main.cpp

Revision:

4:300ced917633

Parent:

3:c8e53b5762bd

Child:

6:b7f6433cc765

--- a/main.cpp	Sat Oct 03 19:42:23 2015 +0000
+++ b/main.cpp	Wed Nov 25 20:24:31 2015 +0000
@@ -3,134 +3,260 @@
 #include "EthernetInterface.h"
 #include "ExperimentServer.h"
 #include "QEI.h"
+#include "HX711.h"
 
-#define NUM_INPUTS 7
-#define NUM_OUTPUTS 6
+#define NUM_INPUTS 21
+#define NUM_OUTPUTS 17
+#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
 
 Serial pc(USBTX, USBRX);    // USB Serial Terminal
 ExperimentServer server;    // Object that lets us communicate with MATLAB
-PwmOut motorPWM(D7);        // Motor PWM output
-DigitalOut motorFwd(D5);    // Motor forward enable
-DigitalOut motorRev(D6);    // Motor backward enable
 Timer t;                    // Timer to measure elapsed time of experiment
-Ticker k;                   // Time interval
-AnalogIn Anal(A5);          // AnalogIn port for current sensing
-QEI encoder(D3,D4, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
-float Volt = 0;
-float emf  = 0;
-float Current =0;
-float i = 0;
-float  P = 0;
-float vel=0;
-float pot=0;
-float i_d=0;
+
+// Variables for q1
+float current1;
+float current_des1;
+float angle1;
+float angle_des1;
+float velocity1;
+float velocity_des1;
+float duty_factor1;
+float angle1_init;
+float max_angle1;
+float dif_ang1;
+
+// Variables for q2
+float current2;
+float current_des2;
+float angle2;
+float angle_des2;
+float velocity2;
+float velocity_des2;
+float duty_factor2;
+float angle2_init;
+float max_angle2;
+float dif_ang2;
+
+// Fixed kinematic parameters
+const float l_OA=.010; 
+const float l_OB=.040; 
+const float l_AC=.095; 
+const float l_DE=.095;
+              
+// Timing parameters
+float pwm_period_us;
+float current_control_period_us;
+float impedance_control_period_us;
+float exp_period;
+float omega;
+float phase;
+
+// Control parameters
+float K_1;
+float K_2;
+
+float D_1;
+float D_2;
+
+float current_gain;
+
+float xDesFoot;
+float yDesFoot;
+
 
+// Model parameters
+float R;
+float k_emf;
+float nu1, nu2;
+float supply_voltage;
+float duty_max;
+
+
+DigitalOut motorFwd1(D7);
+DigitalOut motorRev1(D6);  
+AnalogIn currentSense1(A0);
+PwmOut   pwmOut1(D5);
+
+DigitalOut motorFwd2(D13);
+DigitalOut motorRev2(D12);  
+AnalogIn currentSense2(A1);
+PwmOut   pwmOut2(D11);
+
+QEI encoder1(D3,D4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
+QEI encoder2(D9,D10, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
+//HX711 load_cell(D0,D1,uint8_t 120);
+Ticker currentLoop;
+
+float prev_current_des1 = 0;
+float prev_current_des2 = 0;
+void CurrentLoop()
+{
+    motorFwd1 = current_des1 >= 0;
+    motorRev1 = current_des1 <  0;
+     
+    current1  = currentSense1.read()*3.3f / 0.14f;   //measure current                
+    if (prev_current_des1 < 0)
+        current1*=-1;    
+        
+    duty_factor1=(current_des1*R + current_gain*(current_des1-current1) + k_emf*velocity1)/supply_voltage;   
+    motorRev1 = duty_factor1< 0;
+    motorFwd1 = duty_factor1>=0;
+    float absDuty1 = abs(duty_factor1);
+    if (absDuty1 > duty_max) {
+        duty_factor1 *= duty_max / absDuty1;
+        absDuty1= duty_max;
+    }
+    pwmOut1.write(absDuty1);
+    prev_current_des1 = current_des1;
+    
+    motorFwd2 = current_des2 >= 0;
+    motorRev2 = current_des2 <  0;
+    
+    current2  = currentSense2.read()*3.3f / 0.14f;   //measure current                
+    if (prev_current_des2 < 0)
+        current2*=-1;    
+        
+    duty_factor2=(current_des2*R + current_gain*(current_des2-current2) + k_emf*velocity2)/supply_voltage;
+        
+    motorRev2 = duty_factor2< 0;
+    motorFwd2 = duty_factor2>=0;
+    float absDuty2 = abs(duty_factor2);
+    if (absDuty2 > duty_max) {
+        duty_factor2 *= duty_max / absDuty2;
+        absDuty2= duty_max;
+    }
+    pwmOut2.write(absDuty2);
+    prev_current_des2 = current_des2;
+}
 
 int main (void) {
-    // Link the terminal with our server and start it up
-    server.attachTerminal(pc);
-    server.init();
+      
+  encoder1.reset();
+  encoder2.reset();
+
+  // Link the terminal with our server and start it up
+  server.attachTerminal(pc);
+  server.init();
+
+  // Continually get input from MATLAB and run experiments
+  float input_params[NUM_INPUTS];
+  while(1) {
+    if (server.getParams(input_params,NUM_INPUTS)) {
+      pwm_period_us               = input_params[0]; // PWM_Period in mirco seconds
+      current_control_period_us   = input_params[1]; // Current control period in micro seconds
+      impedance_control_period_us = input_params[2]; // Impedance control period in microseconds seconds
+      exp_period                  = input_params[3]; // Experiment time in seconds 
+
+      R                        = input_params[4]; // Terminal resistance (Ohms)
+      k_emf                    = input_params[5]; // Back EMF Constant (V / (rad/s))
+      nu1                      = input_params[6]; // Friction coefficienct 1 (Nm / (rad/s))
+      nu2                      = input_params[7]; // Friction coefficienct 1 (Nm / (rad/s))
+      supply_voltage           = input_params[8]; // Power Supply Voltage (V)
+
+      angle1_init              = input_params[9]; // Initial angle for q1 (rad)
+      angle2_init              = input_params[10];// Initial angle for q2 (rad)
+
+      current_gain             = input_params[11]; // Proportional current gain (V/A)
+      K_1                     = input_params[12]; // Foot stiffness N/m
+      K_2                     = input_params[13]; // Foot stiffness N/m
+      max_angle1              = input_params[14]; // Foot stiffness N/m
 
-    // PWM period should nominally be a multiple of our control loop
-    motorPWM.period_us(200);
-    
-    // Continually get input from MATLAB and run experiments
-    float input_params[NUM_INPUTS];
-    
-    while(1) {
-        if (server.getParams(input_params,NUM_INPUTS)) {
-           float tf   = input_params[0]; // Voltage for first second
-           float i_d  = input_params[1]; // Voltage for second second
-           float Kp   = input_params[2];
-           float R   = input_params[3];
-           float Kb   = input_params[4];
-           float Kth   = input_params[5];
-           float b   = input_params[6];
-            //float .0thd  = input_params[3];
-            Volt = 0.0;
-            emf  = 0.0;
-            Current =0.0;
-             i = 0.0;
-              P = 0.0;
-             vel=0.0;
-             pot=0.0;
-             i_d=0.0;
-            // Setup experiment
-            t.reset();
-            t.start();
-            encoder.reset();
-            //float thj = 0;
-            motorFwd = 1;
-            motorRev = 0;
-            motorPWM.write(0);
+      D_1                     = input_params[15]; // Foot damping N/(m/s)
+      D_2                     = input_params[16]; // Foot damping N/(m/s)
+      max_angle2              = input_params[17]; // Foot damping N/(m/s)
+      duty_max                = input_params[18]; // Maximum duty factor
+      omega                   = input_params[19]; // Oscillation freq of the tail         
+      phase                   = input_params[20]; // phase difference between two motors
+      
+      pwmOut1.period_us(pwm_period_us);
+      pwmOut2.period_us(pwm_period_us);
+           float m1 =.02 + .210;         
+           float m2 =.0225; 
+           float m3 = .004;              
+           float m4 = .017;
+            //float I1 = 45.389 * 10^-6;    
+            //float I2 = 22.918 * 10^-6;
+            //float I3 = 3.2570 * 10^-6;    
+            //float I4 = 22.176 * 10^-6;
+            float l_OA=.011;              
+            float l_OB=.042; 
+            float l_AC=.096;              
+            float l_DE=.096;
+            float l_O_m1=0.0364;          
+            float l_B_m2=0.040; 
+            float l_A_m3=1/2 * l_AC;      
+            float l_C_m4=1/2 * (l_DE+ l_OB-l_OA);
+            float g = 9.81;
+
+      // Attach current loop!
+      currentLoop.attach_us(CurrentLoop,current_control_period_us);
+
+      // Setup experiment
+      t.reset();
+      t.start();
 
-            // Run experiment
-            while( t.read() < tf ) { 
-                // Perform control loop logic
-        //void attime(){ 
-            //motorPWM.write(v1);
-                         
-            
-            //k.attach_us(0, 2000);
-            // This is for position based PID
-               //thj = thj + (thd - encoder.getPulses());
-                //float P= Kp*(thd-encoder.getPulses()); 
-                //float I= Ki*(0-encoder.getVelocity());
-                //float D= Kd*thj;
-                // float Volt = P+I+D;
-                // float pulse = encoder.getPulses();
-              
-                
-                //below is for Current Based PID
-                 Current = Anal.read();
-                 vel=encoder.getVelocity()/1200.0*6.28;
-                 pot=encoder.getPulses()/1200.0*6.28;
-                 i_d=(Kth*pot+b*vel)/Kb;
-                 //i_d = 1.5;
-                 i = 3.3f*Current/0.140f;
-                 if (i_d<0){
-                  i=i*-1;
-                  }
-                  P = Kp*(i_d-i);
-                emf = Kb*vel;
-                Volt = (R*i_d+P+emf)/12.0;
-               //float Kb=(Volt-R*i_d)/vel;
-            
-                //float Volt = 0;
-                //pc.printf("  %f  /n",P);
-                if (Volt>0){
-                motorRev.write(1);
-                motorFwd.write(0);
-                motorPWM.write(Volt);
-                }
-                else{
-                motorFwd.write(1); 
-                motorRev.write(0); 
-                motorPWM.write(-1*Volt); 
-                } 
-                
-                //if (t.read() < 1) 
-                   //motorPWM.write(v1);
-                //else 
-                  //  motorPWM.write(v2);
-                    
-                // Form output to send to MATLAB    
-                float output_data[NUM_OUTPUTS];
-                output_data[0] = t.read();
-                output_data[1] = encoder.getPulses()/1200.0*6.28;
-                output_data[2] = vel;
-                output_data[3] = Volt;
-                output_data[4] = i;
-                output_data[5] = i_d;
-             
-                // Send data to MATLAB
-                server.sendData(output_data,NUM_OUTPUTS);
-                wait(.001); 
-            }     
-            // Cleanup after experiment
-            server.setExperimentComplete();
-            motorPWM.write(0);
-        } // end if
-    } // end while
+      motorFwd1 = 1;
+      motorRev1 = 0;
+      pwmOut1.write(0);
+
+      motorFwd2 = 1;
+      motorRev2 = 0;
+      pwmOut2.write(0);
+
+      // Run experiment
+      while( t.read() < exp_period ) { 
+        // Perform control loop logic
+        
+        angle1 = encoder1.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor
+        dif_ang1 =  angle1 - max_angle1*sin(omega*t.read()); // calc difference of actual and desired angle                 
+        velocity1 = encoder1.getVelocity() * PULSE_TO_RAD;
+        
+        angle2 = encoder1.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor
+        dif_ang2 = angle2 - max_angle2*sin(omega*t.read()+phase); // calc difference of actual and desired angle       
+        velocity2 = encoder2.getVelocity() * PULSE_TO_RAD;
 
-}// end main
\ No newline at end of file
+        // Forward Kinematics
+        
+        float tau_des1 = (-K_1*dif_ang1-D_1*velocity1+nu1*velocity1); 
+        float tau_des2 = (-K_1*dif_ang2-D_1*velocity2+nu2*velocity2);
+        
+        // Set desired currents
+        current_des1 = tau_des1/k_emf;//(-K_xx*angle1-D_xx*velocity1+nu1*velocity1)/k_emf;
+        current_des2 = tau_des2/k_emf;  //(-K_yy*angle2-D_yy*velocity2+nu2*velocity2)/k_emf;               
+                      
+        // Form output to send to MATLAB     
+        float output_data[NUM_OUTPUTS];
+        output_data[0] = t.read();
+        output_data[1] = angle1;
+        output_data[2] = velocity1;  
+        output_data[3] = current1;
+        output_data[4] = current_des1;
+        output_data[5] = duty_factor1;
+          
+        output_data[6] = angle2;
+        output_data[7] = velocity2;
+        output_data[8] = current2;
+        output_data[9] = current_des2;
+        output_data[10]= duty_factor2;
+
+        output_data[11] = tau_des1;
+        output_data[12] = tau_des2;
+        output_data[13] = dif_ang1;
+        output_data[14] = dif_ang2;
+        //output_data[15] = fx;
+        //output_data[16] = fy;
+                          
+        // Send data to MATLAB
+        server.sendData(output_data,NUM_OUTPUTS);
+        wait_us(impedance_control_period_us);   
+      }     
+      // Cleanup after experiment
+      server.setExperimentComplete();
+      // control.detach();
+      currentLoop.detach();
+      pwmOut1.write(0);
+      pwmOut2.write(0);
+
+    } // end if
+  } // end while
+} // end main
\ No newline at end of file