Diff: main.cpp

Revision:

4:6dec99ee29e6

Parent:

3:659998b2bee1

--- a/main.cpp	Thu Oct 18 14:16:49 2018 +0000
+++ b/main.cpp	Thu Nov 01 13:16:41 2018 +0000
@@ -2,12 +2,11 @@
 #include "math.h"
 #include "QEI.h"
 #include "HIDScope.h"
+#include "BiQuad.h"
 
 HIDScope scope(3);
 
-AnalogIn pot1(A2); // Controls potentiometer 1, which controls motor 1
-AnalogIn pot2(A3); // Controls potentiometer 2, which is used to control Kp (position gain)
-InterruptIn button1(D0);
+AnalogIn pot1(A2); // Controls potentiometer 1, which controls the reference position
 
 InterruptIn motor1A(D13);   // Encoder 1a
 InterruptIn motor1B(D12);   // Encoder 1b
@@ -21,22 +20,40 @@
 
 double GetReferencePosition(){
     // Returns the wanted reference position
-    const int CpR = 4200; //Counts Per Revolution; when the counts i 4200, 1 revolution has been made. (Using X2 encoding)
+    const int CpR = 4200; //Counts Per Revolution; when the counts 4200, 1 revolution has been made. (Using X2 encoding)
     double ReferencePosition = CpR*pot1; // Returns a value between 0 and 4200, the wanted rotation. 
     return ReferencePosition;
 }
     
 double GetActualPosition(){
-    double ActualPosition = -Encoder1.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
+    double ActualPosition = Encoder1.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
     return ActualPosition;
 }
+
+double PID_controller(double error){
+    static double error_integral = 0;
+    static double error_prev = error; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    double Ts = 0.01;
     
-double P_controller(double error){
-    double Kp = pot2*17+1;
-    double u_k = Kp*error;  // Proportional part
+    // Proportional part
+        
+        double Kp = 1;
+        double u_k = Kp*error;
+
+    // Integral part
+        double Ki = 0.06;
+        error_integral = error_integral + error * Ts;
+        double u_i = Ki * error_integral;
     
+    // Derivative part
+        double Kd = 0.2;
+        double error_derivative = (error - error_prev)/Ts;
+        double filtered_error_derivative = LowPassFilter.step(error_derivative);
+        double u_d = Kd * filtered_error_derivative;
+        error_prev = error;
     // Sum all parts and return it
-    return u_k;
+    return u_k + u_i + u_d;
 }
 
 void SetMotor1(double motorValue){
@@ -50,10 +67,10 @@
         else pwmpin1 = fabs(motorValue);
 }
 
-void ScopeData(){
-        scope.set(0,pot1*4200);              // Wanted amount of counts
+void ScopeData(double motorValue){
+        scope.set(0,GetReferencePosition());  // Wanted amount of counts
         scope.set(1,-Encoder1.getPulses());  // Amount of counts of motor 1
-        scope.set(2,pwmpin1);                // Current pwm-value send to motor 1
+        scope.set(2,motorValue);
         scope.send(); // send what's in scope memory to PC
 }
 
@@ -63,20 +80,20 @@
     // a simple Feedback controller. Call this from a Ticker.
     float ReferencePosition = GetReferencePosition();
     float ActualPosition = GetActualPosition();
-    float motorValue = P_controller(ReferencePosition - ActualPosition);
+    float motorValue = PID_controller(ReferencePosition - ActualPosition);
     SetMotor1(motorValue);
-    ScopeData();
+    ScopeData(motorValue);
 }
 
 int main(){
     pwmpin1.period_us(60);
-  //  Motor1.attach(&MeasureAndControl, 0.01); 
+   // Motor1.attach(&MeasureAndControl, 0.01); 
     while(1){
             float ReferencePosition = GetReferencePosition();
             float ActualPosition = GetActualPosition();
-            float motorValue = P_controller(ReferencePosition - ActualPosition);
+            float motorValue = PID_controller(ReferencePosition - ActualPosition);
             SetMotor1(motorValue);
-            ScopeData();
+            ScopeData(motorValue);
             wait(0.01f);
         }
     }
\ No newline at end of file