Revision 6:b7f6433cc765, committed 2015-11-30

Comitter:

laskowsk

Date:

Mon Nov 30 18:29:56 2015 +0000

Parent:

5:06d11758cc1e

Commit message:

commit

Changed in this revision

--- a/main.cpp	Wed Nov 25 20:25:20 2015 +0000
+++ b/main.cpp	Mon Nov 30 18:29:56 2015 +0000
@@ -5,8 +5,8 @@
 #include "QEI.h"
 #include "HX711.h"
 
-#define NUM_INPUTS 21
-#define NUM_OUTPUTS 17
+#define NUM_INPUTS 22
+#define NUM_OUTPUTS 16
 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
 
 Serial pc(USBTX, USBRX);    // USB Serial Terminal
@@ -36,12 +36,6 @@
 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;
@@ -59,6 +53,7 @@
 float D_2;
 
 float current_gain;
+float calibration_factor;
 
 float xDesFoot;
 float yDesFoot;
@@ -84,9 +79,11 @@
 
 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);
+HX711 scale(A0,A1);
+
 Ticker currentLoop;
 
+
 float prev_current_des1 = 0;
 float prev_current_des2 = 0;
 void CurrentLoop()
@@ -130,7 +127,7 @@
 }
 
 int main (void) {
-      
+  scale.tare(); // tare scale    
   encoder1.reset();
   encoder2.reset();
 
@@ -167,26 +164,12 @@
       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
-      
+      calibration_factor      = input_params[21]; // calibration factor for load cell
+            
       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;
+      scale.setScale(calibration_factor); //Adjust to this calibration factor
+         
 
       // Attach current loop!
       currentLoop.attach_us(CurrentLoop,current_control_period_us);
@@ -211,18 +194,21 @@
         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
+        angle2 = encoder2.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;
 
         // 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);
+        float tau_des2 = (-K_2*dif_ang2-D_2*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;               
+        current_des2 = tau_des2/k_emf;  //(-K_yy*angle2-D_yy*velocity2+nu2*velocity2)/k_emf;   
+        
+        // take measurement from load cell
+        float F = scale.getGram();     // value from load cell that uses calibration factor       
                       
         // Form output to send to MATLAB     
         float output_data[NUM_OUTPUTS];
@@ -243,7 +229,7 @@
         output_data[12] = tau_des2;
         output_data[13] = dif_ang1;
         output_data[14] = dif_ang2;
-        //output_data[15] = fx;
+        output_data[15] = F;
         //output_data[16] = fy;
                           
         // Send data to MATLAB