Diff: main.cpp

Revision:

18:e522dfbab4c6

Parent:

17:4a0912c93771

--- a/main.cpp	Fri Oct 19 12:08:31 2018 +0000
+++ b/main.cpp	Fri Oct 19 14:18:38 2018 +0000
@@ -3,6 +3,8 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include <math.h>
+
+// Defining input and output pins
 MODSERIAL pc(USBTX, USBRX);
 DigitalOut motor1DirectionPin(D7);
 FastPWM motor1MagnitudePin(D6); 
@@ -16,13 +18,15 @@
 Ticker print;
 
 //Global variables
+    // Measure
 volatile double measuredPosition = 0.0;
 volatile double referencePosition = 0.0; 
-volatile double motorValue= 0.01;
-volatile double Kp = 5.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
-volatile double Ki = 1.0; //dit moeten we bepalen met een plot bijvoorbeeld
-volatile double Kd = 0.0;
-volatile double Ts = 0.01;
+volatile double motorValue= 0.00;
+    // Control
+volatile double Kp = 0.0; // Proportional gain. Dit maken we variabel, dit zorgt voor een grote of kleine overshoot
+volatile double Ki = 0.0; // Integral gain. Dit moeten we bepalen met een plot bijvoorbeeld
+volatile double Kd = 0.0; // Diverential gain.
+volatile double Ts = 0.01; // Sample time in FeedbackControl
 
 //------------------------------------------------------------------------------
 // Functions
@@ -43,20 +47,29 @@
 
 double FeedbackControl(double Error)
 {  
-    static double Error_integral = 0;
+    static double Error_integral = 0.0;
     static double Error_prev = Error;
-    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
+    /*static BiQuad LowPassFilter(..., ..., ..., ..., ...); */
+    
+    // Read potmeter
+    Kd = 20*potMeter2.read();
+    
     // Proportional part: 
-    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
+    // van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder 
+    // trilling motor beste combinatie hiervan)
     double u_k = Kp * Error;
+    
     // Integral part:
     Error_integral = Error_integral + Error * Ts; 
     double u_i = Ki * Error_integral;
-    // Derivative part
+    
+    // Derivative part:
     double Error_derivative = (Error - Error_prev)/Ts;
-    Kd = 20*potMeter2.read();
     double u_d = Kd * Error_derivative;
+    /* Filtered_Error_derivative = LowPassFilter(Error_derivative);
+    double u_d = Kd * Filtered_Error_derivative;   */
     Error_prev = Error;
+    
     // Sum all parts and return it 
     return u_k + u_i + u_d; //motorValue
  }
@@ -118,7 +131,5 @@
     
     //Other initializations
           
-    while(true)
-    {
-    }
+    while(true) {     }
 }