Diff: main.cpp

Revision:

13:b5868fd8ffe9

Parent:

12:02eba9a294d2

Child:

14:54cbd8f0efe4

--- a/main.cpp	Thu Nov 02 11:59:11 2017 +0000
+++ b/main.cpp	Thu Nov 02 14:47:48 2017 +0000
@@ -4,6 +4,7 @@
 #include "math.h"
 #include "FastPWM.h"
 #include "encoder.h"
+#include "BiQuad.h"
 
 
 
@@ -37,12 +38,12 @@
 
 // Constants EMG ----------------------- Start
 double X;
-double X_maxTreshold = 560;
+double X_maxTreshold = 480;
 double X_minTreshold = 20;
 const double X_Callibrate  = 300;
 
 double Y;
-double Y_maxTreshold = 560;
+double Y_maxTreshold = 480;
 double Y_minTreshold = 0;
 const double Y_Callibrate  = 300;
 
@@ -52,8 +53,8 @@
 float   Pwmperiod = 0.001f;
 int     potmultiplier = 600;    // Multiplier for the pot meter reference which is normally between 0 and 1
 const float gearM1 = 6.2;
-const double   gainM1 = 1/35.17;       // encoder pulses per degree theta
-const double   gainM2 = 1/(2*gearM1*pi);       // gain for radius r
+const double   gainM1 = 1/38.17;       // encoder pulses per degree theta
+const double   gainM2 = 1/107.8;       // pulses per mm
 
 double motor1;
 double motor2;
@@ -73,13 +74,17 @@
 double          m1_err_int = 0;
 double          m1_prev_err = 0;
 
-const float     M2_KP = 5; 
+const float     M2_KP = 30; 
 const float     M2_KI = 0.5;
 const float     M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
 double          m2_err_int = 0;
 double          m2_prev_err = 0;
 
 // Constants Biquad
+BiQuad bq1(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01);
+BiQuad bq2(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01);
+BiQuadChain bqc;
+
 const double    M1_F_A1 = 1.0;
 const double    M1_F_A2  = 2.0; 
 const double    M1_F_B0  = 1.0; 
@@ -96,6 +101,8 @@
 double PID1(double e1, const double Kp1, const double Ki1, const double Kd1, double Ts, double &e_int1, double &e_prev1, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
 
     double e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep                 // Derivative
+    
+    e_der1 = bqc.step(e_der1);
     // biquad part, see slide
     //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
     
@@ -118,6 +125,8 @@
     // biquad part, see slide
     //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
     
+    e_der2 = bqc.step(e_der2);
+    
     e_prev2 = e2;
     e_int2 += Ts*e2;
     
@@ -171,10 +180,10 @@
             }
     */
     
-    if (X > X_maxTreshold){
+    if (X >= X_maxTreshold){
         X = X_maxTreshold;
         }
-        else if (X < X_minTreshold){
+        else if (X <= X_minTreshold){
             X = X_minTreshold;
             }
         else{
@@ -234,10 +243,10 @@
                 Y = Y+0.1;
             }
     */        
-    if (Y > Y_maxTreshold){
+    if (Y >= Y_maxTreshold){
         Y = Y_maxTreshold;
         }
-        else if (Y < Y_minTreshold){
+        else if (Y <= Y_minTreshold){
             Y = Y_minTreshold;
             }
         else{
@@ -317,7 +326,7 @@
     double dRadius  = reference_motor2 - pos_M2;
 
     pc.baud(115200);
-    //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (delta1,delta2):(%f,%f), posM(M1,M2):(%f,%f) \n",x,y, dTheta ,dRadius,delta1, delta2, pos_M1,pos_M2);
+    //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (delta1,delta2):(%f,%f)\n",x,y, dTheta ,dRadius,delta1, delta2);
     pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
     
     //motor1PWM = motor1;
@@ -347,8 +356,8 @@
     }
     
     motor1 = abs(delta1)/1000.0;
-        if(motor1 >= 0.35) {
-        motor1 = 0.35;
+        if(motor1 >= 0.20) {
+        motor1 = 0.20;
       //pc.baud(115200);
       //pc.printf("\r val motor1: %f\n", motor1);
     }
@@ -381,7 +390,7 @@
         motor2 = 0.50;
     }
     
-    motor1PWM = motor1 + 0.65;
+    motor1PWM = motor1 + 0.80;
     motor2PWM = motor2 + 0.50;
     
     //pc.printf("\r delta(1,2):(%f,%f)\n", delta1,delta2);
@@ -395,6 +404,8 @@
     motor1PWM.period(Pwmperiod);
     motor2PWM.period(Pwmperiod);
     
+    bqc.add(&bq1).add(&bq2);
+    
     while(1){
     /*
     double x = Get_X_Position();