Diff: main.cpp

Revision:

6:ea3b3f50c893

Parent:

5:2ae500da8fe1

Child:

7:08fd3bc7a3cf

--- a/main.cpp	Mon Oct 07 14:22:05 2019 +0000
+++ b/main.cpp	Fri Oct 11 12:42:36 2019 +0000
@@ -3,55 +3,80 @@
 #include "FastPWM.h"
 #include "QEI.h"
 #include <math.h>
+#include "BiQuad.h"
 
 DigitalIn button1(D12);
 AnalogIn pot2(A0);
-Ticker john;
-DigitalOut motor1Direction(D7);
+Ticker motor; //ticker to call motor values
+DigitalOut motor1Direction(D7); // is a boolean
 FastPWM motor1Velocity(D6);
-Serial pc(USBTX, USBRX);
+MODSERIAL pc(USBTX, USBRX);
 QEI Encoder(D8,D9,NC,8400);
 volatile double frequency = 17000.0;
 volatile double period_signal = 1.0/frequency;
 float vel = 0.0f;
 float referencevelocity;
 float motorvalue2;
-double Kp = 17.5;
 int counts;
 float position1 = 0;
 float position2;
-float timeinterval = 0.001;
+float position11;
+float position22;
+float timeinterval = 0.001f;
 float measuredvelocity;
-
-// README
-// positive referencevelocity corresponds to a counterclockwise motion
+double Kp = 5;
+double Ki = 0.1;
+double Kd = 1;
+float motorvalue;
+float desiredposition; 
+float measuredposition;
+float referenceposition;
 
 
-//P control implementation (behaves like a spring)
-double P_controller(double error)
+
+// --------------------   README ------------------------------------
+// positive referenceposition corresponds to a counterclockwise motion
+// negative referenceposition corresponds to a clockwise motion 
+
+//PID control implementation (BiQuead)
+double PID_controller(double error)
 {
+    static double error_integral = 0;
+    static double error_prev = error;
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    
     //Proportional part:
     double u_k = Kp * error;
     
+    // Integreal part
+    error_integral = error_integral + error * timeinterval;
+    double u_i = Ki*error_integral;
+    
+    // Derivate part
+    double error_derivative = (error - error_prev)/timeinterval;
+    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;
 }
 
-//get the measured velocity
-double getmeasuredvelocity()
+//get the measured position
+double getmeasuredposition()
 {   
     counts = Encoder.getPulses();
     counts = counts % 8400;
-    position2 = counts/8400*2*3.141592;
-    measuredvelocity = (position2-position1)/timeinterval; 
-    position1 = position2;
+    measuredposition = ((float)counts) / 8400.0f * 2.0f;
+    
+    return measuredposition;
 }
 
-//get the reference of the velocity: positive or negative
-double getreferencevelocity()
+//get the reference of the velocity
+double getreferenceposition()
 {
-    referencevelocity = -1.0 + 2.0*pot2.read();
-    return referencevelocity;
+    referenceposition = 1; //this determines the amount of spins
+    return referenceposition;
 }   
 
 //send value to motor
@@ -59,25 +84,26 @@
 {   
     motorvalue2 = motorvalue;
     vel = fabs(motorvalue);
-    vel = vel > 1.0f ? 1.0f : vel;   
+    vel = vel > 1.0f ? 1.0f : vel;   //if velocity is greater than 1, reduce to 1, otherwise remain vel
     motor1Velocity = vel;
 
-    motor1Direction = (motorvalue > 0.0f);
+    motor1Direction = (motorvalue > 0.0f); //boolean output: true gives counterclockwise direction, false gives clockwise direction
 }
 
 // function to call reference velocity, measured velocity and controls motor with feedback
 void measureandcontrol(void)
 {
-    float referencevelocity = getreferencevelocity();
-    float measuredvelocity = getmeasuredvelocity();
-    sendtomotor(referencevelocity);
+    float referenceposition = getreferenceposition();
+    measuredposition = getmeasuredposition();
+    motorvalue = PID_controller(referenceposition - measuredposition);
+    sendtomotor(motorvalue);
 }
 int main()
 {
     pc.baud(115200);
     pc.printf("Starting...\r\n");
     motor1Velocity.period(period_signal);
-    john.attach(measureandcontrol, timeinterval);
+    motor.attach(measureandcontrol, timeinterval); 
     while(true)
     {
         wait(0.5);
@@ -85,8 +111,19 @@
         //pc.printf("motor1Direction = %i\r\n", (int)motor1Direction);
         //pc.printf("motorvalue2 = %f\r\n", motorvalue2);
         pc.printf("number of counts %i\r\n", counts);
-        pc.printf("measured velocity is %f\r\n", measuredvelocity);
-        pc.printf("position1 = %f\r\n",position1);
-        pc.printf("position2 = %f\r\n",position2);
+        pc.printf("measured position is %f\r\n", measuredposition);
+        //pc.printf("position1 = %f\r\n",position11);
+        //pc.printf("position2 = %f\r\n",position22);
+        pc.printf("motorvalue is %f\r\n", motorvalue);
+        
+        char c = pc.getcNb();
+        
+        if (c == 'c')
+        {
+            pc.printf("Programm stops running");
+            motorvalue = 0;
+            sendtomotor(motorvalue);
+            return -1;
+        }
     }
 }
\ No newline at end of file