Diff: main.cpp

Revision:

35:a21c1cab8086

Parent:

34:96bcbddc7d2d

--- a/main.cpp	Tue Nov 01 13:45:31 2016 +0000
+++ b/main.cpp	Tue Nov 01 15:47:40 2016 +0000
@@ -57,7 +57,7 @@
 const int cw = 0;       //values for cw and ccw are inverted!! cw=0 and ccw=1
 const int ccw = 1;
 
-//set lengths (VALUES HAVE TO BE CHANGED)
+//set lengths and constants (VALUES HAVE TO BE CHANGED)
 float x0 = 1.0;
 float L0 = 1.0;
 float L1 = 1.0;
@@ -67,7 +67,6 @@
 float y_stampup = 0.1;      //height stamp while not stamping: 10cm above table surface
 float y_stampdown = 0.0;    //height stamp while stamping: at table surface
 
-
 //set initial conditions
 float biceps_l = 0;
 float biceps_r = 0;
@@ -107,6 +106,10 @@
 float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
 float t_sample = 0.1; //seconds
 const float maxStampDistance = 2.0;
+float Kp = 3; //potMeter2.read();
+float Ki = 0.2*Kp;  //0.01*Kp; //potMeter2.read();
+float Kd = 0.05;     //0.1;(unstable!) //0.05; //0.02;        //0.04*Kp; //potMeter2.read();
+float N = 100; //N=1/Tf, Higher N is faster derivative action but more sensitive to noise.
 
 float q1_refOutNew = 0;
 float q1_refOutMin = -100;         //WRONG values
@@ -122,6 +125,7 @@
 BiQuadChain bqc;
 BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
 BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);
+BiQuad pidf;        //for lowpass filtering of PID controller
 
 //set go-Ticker settings
 volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
@@ -211,7 +215,7 @@
         //led1 = 0;
         //led2 = 0;
         ReferencePosition_xnew = ReferencePosition_x;
-        ReferencePosition_ynew = ReferencePosition_y;   //y_stampup; 
+        ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
         }
       //  }
     
@@ -286,17 +290,17 @@
     // linear feedback control
     float q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
     float q2_error = q2_ref - q2; //referencePosition1 - Position1;             // proportional angular error in radians
-    float Kp = 3; //potMeter2.read();
-
+    
+    float TotalError1 = pidf.step(q1_error);
+    float TotalError2 = pidf.step(q2_error);
+    
     float q1IntError = q1IntError + q1_error*t_sample;             // integrated error in radians
     float q2IntError = q2IntError + q2_error*t_sample;             // integrated error in radians
     //float maxKi = 0.2;
-    float Ki = 0.04*Kp;  //0.01*Kp; //potMeter2.read();
     
     float q1DerivativeError = (q1_error - q1_error_prev)/t_sample;  // derivative of error in radians
     float q2DerivativeError = (q2_error_prev + q2_error)/t_sample;  // derivative of error in radians
     //float maxKd = 0.2;
-    float Kd = 0.0;  //0.04*Kp; //potMeter2.read();
     
     //scope.set(0,referencePosition1);
     //scope.set(1,Position1);
@@ -307,20 +311,6 @@
     TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output in radians = motor input
     
     /*
-    if (fabs(TotalError1) < TotalErrorMin) {
-        TotalError1=0;
-        }
-    else {
-        TotalError1=TotalError1;
-        }
-    if (fabs(TotalError2) < TotalErrorMin) {
-        TotalError2=0;
-        }
-    else {
-        TotalError2=TotalError2;
-        }
-    */
-    /*
     DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
     DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
     motorValue1Out = DerTotalError1/MotorGain;       
@@ -441,8 +431,8 @@
  pc.baud(115200);
  pc.printf("Test putty IK");
  MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
- bqc.add(&bq1).add(&bq2);
- 
+ bqc.add(&bq1).add(&bq2);           //set BiQuad chain
+ pidf.PIDF( Kp, Ki, Kd, N, t_sample );    //set PID filter
  while(1)
     {
         if (MeasureTicker_go){