Diff: main.cpp

Revision:

40:9ecaab27acde

Parent:

39:cc7754330c26

Child:

41:68b170829965

--- a/main.cpp	Thu Nov 03 15:09:14 2016 +0000
+++ b/main.cpp	Thu Nov 03 16:12:23 2016 +0000
@@ -113,6 +113,10 @@
 float t_sample = 0.01; //seconds
 const float maxStampDistance = 0.65;       //0.66;
 const float minStampDistance = 0.39;
+float Kp = 3.0;//potMeter2.read();
+float Ki = 0.1;  //0.01*Kp; //potMeter2.read();
+float Kd = 0.04;     //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 = 0;             //Physical min angle 0.00 radians + 0.1 rad
@@ -129,6 +133,8 @@
 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;
+
 //set go-Ticker settings
 volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
@@ -316,8 +322,11 @@
     // 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;
@@ -335,7 +344,7 @@
     
     TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;         //total controller output in radians = motor input
     TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output in radians = motor input
-    
+    */
     /*
     if (fabs(TotalError1) < TotalErrorMin) {
         TotalError1=0;
@@ -475,7 +484,7 @@
  wait(10.0);
  MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
  bqc.add(&bq1).add(&bq2);
- 
+ pidf.PIDF( Kp, Ki, Kd, N, t_sample );    //set PID filter
  while(1)
     {
         if (MeasureTicker_go){