Revision 10:fb52590deb29, committed 2018-10-31

Comitter:

Sven_van_Wincoop

Date:

Wed Oct 31 12:09:46 2018 +0000

Parent:

9:34e3dd8548c0

Commit message:

Controller with values adjusted for Ts=0.002

Changed in this revision

diff -r 34e3dd8548c0 -r fb52590deb29 Controller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Wed Oct 31 12:09:46 2018 +0000
@@ -0,0 +1,193 @@
+#include "mbed.h"
+#include "math.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "BiQuad.h"
+
+//Tickers
+Ticker TickerMeasureAndControl;
+Ticker TickerPrintToScreen;
+
+//Communication
+MODSERIAL pc(USBTX,USBRX);
+QEI Encoder(D10,D11,NC,32);
+
+//Global pin variables 
+PwmOut PwmPin(D5);
+DigitalOut DirectionPin(D4);
+AnalogIn Potmeter1(A1);
+AnalogIn Potmeter2(A0);
+DigitalIn BUT1(D8);
+DigitalIn BUT2(D9);
+DigitalIn button(SW2);
+//Global variables
+volatile bool PrintFlag = false;
+
+//Global variables for printing on screen
+volatile float PosRefPrint; // for printing value on screen
+volatile float PosMotorPrint; // for printing value on screen
+volatile float ErrorPrint;
+
+//-----------------------------------------------------------------------------
+//The double-functions
+
+//Get reference position
+double GetReferencePosition()
+{
+// This function set the reference position to determine the position of the signal.
+// a positive position is defined as a counterclockwise (CCW) rotation
+    static double PositionRef = 0;
+    if (button)
+    {
+    if (BUT2 == false)
+    {
+        if (PositionRef <= 0.5*(6.2830))
+        {                                                                                                                                                                                                                                                                                                                                                                       
+            PositionRef += 0.001*(6.2830);
+        }
+    }
+    
+        if (BUT1 == false)
+    {
+        if (PositionRef >= -0.5*(6.2830))
+        {
+            PositionRef -= 0.001*(6.2830);
+        }
+    }
+    }
+    else
+    {
+            if (PositionRef <= 0)
+            {
+                PositionRef += 0.001*(6.2830);
+            }
+        
+            if (PositionRef >= 0)
+            {
+                PositionRef -= 0.001*(6.2830);
+            }
+    }
+   // double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
+    
+        
+        //double PositionRef = 0.8*6.2830*ValuePot - 0.8*3.1415; // position reference ranging from -0.4 rotations till 0.4 rotations
+        
+        return PositionRef; //rad
+}
+
+// actual position of the motor
+double GetActualPosition()
+{
+    //This function determines the actual position of the motor
+    //The count:radians relation is 8400:2pi
+    double EncoderCounts = Encoder.getPulses();    //number of counts
+    double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
+    
+    return PositionMotor;
+}
+
+
+
+///The controller
+double PID_Controller(double Error)
+{
+    //Belt drive parameters
+   double Ts = 0.002; //Sampling time 100 Hz
+   double Kp = 11.1; // proportional gain. Model value: 11.1
+   double Ki = 22.81; //Integral gain. Model value: 11.2
+   double Kd = 1.7; //Differential gain. Model value: 5.5
+   
+   //Arm drive parameters
+   //double Ts = 0.002; //Sampling time 100 Hz
+   //double Kp = 19.8; // proportional gain
+   //double Ki = 40.9; //Intergral gain. Model value: 19.9
+   //double Kd = 3; //Differential gain. Model value: 9.8
+   
+   static double ErrorIntegral = 0;
+   static double ErrorPrevious = Error;
+   static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+   
+   //Proportional part:
+   double u_k = Kp * Error;
+   
+   //Integral part:
+   ErrorIntegral = ErrorIntegral + Error*Ts;
+   double u_i = Ki * ErrorIntegral; //
+   
+   //Derivative part:
+   double ErrorDerivative = (Error - ErrorPrevious)/Ts;//Kd must be made smaller to compensate for the higher Ts
+   double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
+   double u_d = Kd * FilteredErrorDerivative;
+   ErrorPrevious = Error;
+   
+   // sum of parts and return it
+   return u_k + u_i + u_d; //This will become the MotorValue
+}
+
+//Ticker function set motorvalues
+void SetMotor(double MotorValue)
+{
+    if (MotorValue >=0)
+    {
+        DirectionPin = 1;
+    }
+    else
+    {
+        DirectionPin = 0;
+    }
+    
+    if (fabs(MotorValue)>1) // if error more than 1 radian, full duty cycle
+    {
+        PwmPin = 1; 
+    }
+    else
+    {
+        PwmPin = fabs(MotorValue);
+    }
+}
+
+// ----------------------------------------------------------------------------
+//Ticker function
+void MeasureAndControl(void)
+{
+    double PositionRef = GetReferencePosition();
+    double PositionMotor = GetActualPosition();
+    double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
+    SetMotor(MotorValue);
+    
+    //for printing on screen
+    PosRefPrint = PositionRef;
+    PosMotorPrint = PositionMotor;
+    ErrorPrint = PositionRef - PositionMotor;
+    
+}
+
+
+
+void PrintToScreen()
+{
+    PrintFlag = true;
+}
+
+
+//-----------------------------------------------------------------------------
+int main()
+{
+    pc.baud(115200);
+    pc.printf("Hello World\n\r");
+    PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
+    TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz
+    TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen
+    
+    while (true) 
+    {
+        if(PrintFlag) // if-statement for printing every second four times to screen
+        {
+            double KpPrint = 3.52;
+            double KiPrint = 0.88;
+            double KdPrint = 6.9;
+            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f, Ki = %f and Kd = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint,KdPrint);
+            PrintFlag = false;
+        }
+    }
+}
\ No newline at end of file

diff -r 34e3dd8548c0 -r fb52590deb29 main.cpp
--- a/main.cpp	Mon Oct 29 12:12:18 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,193 +0,0 @@
-#include "mbed.h"
-#include "math.h"
-#include "MODSERIAL.h"
-#include "QEI.h"
-#include "BiQuad.h"
-
-//Tickers
-Ticker TickerMeasureAndControl;
-Ticker TickerPrintToScreen;
-
-//Communication
-MODSERIAL pc(USBTX,USBRX);
-QEI Encoder(D10,D11,NC,32);
-
-//Global pin variables 
-PwmOut PwmPin(D5);
-DigitalOut DirectionPin(D4);
-AnalogIn Potmeter1(A1);
-AnalogIn Potmeter2(A0);
-DigitalIn BUT1(D8);
-DigitalIn BUT2(D9);
-DigitalIn button(SW2);
-//Global variables
-volatile bool PrintFlag = false;
-
-//Global variables for printing on screen
-volatile float PosRefPrint; // for printing value on screen
-volatile float PosMotorPrint; // for printing value on screen
-volatile float ErrorPrint;
-
-//-----------------------------------------------------------------------------
-//The double-functions
-
-//Get reference position
-double GetReferencePosition()
-{
-// This function set the reference position to determine the position of the signal.
-// a positive position is defined as a counterclockwise (CCW) rotation
-    static double PositionRef = 0;
-    if (button)
-    {
-    if (BUT2 == false)
-    {
-        if (PositionRef <= 0.4*(6.2830))
-        {
-            PositionRef += 0.001*(6.2830);
-        }
-    }
-    
-        if (BUT1 == false)
-    {
-        if (PositionRef >= -0.4*(6.2830))
-        {
-            PositionRef -= 0.001*(6.2830);
-        }
-    }
-    }
-    else
-    {
-            if (PositionRef <= 0)
-            {
-                PositionRef += 0.001*(6.2830);
-            }
-        
-            if (PositionRef >= 0)
-            {
-                PositionRef -= 0.001*(6.2830);
-            }
-    }
-   // double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
-    
-        
-        //double PositionRef = 0.8*6.2830*ValuePot - 0.8*3.1415; // position reference ranging from -0.4 rotations till 0.4 rotations
-        
-        return PositionRef; //rad
-}
-
-// actual position of the motor
-double GetActualPosition()
-{
-    //This function determines the actual position of the motor
-    //The count:radians relation is 8400:2pi
-    double EncoderCounts = Encoder.getPulses();    //number of counts
-    double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
-    
-    return PositionMotor;
-}
-
-
-
-///The controller
-double PID_Controller(double Error)
-{
-    //Belt drive parameters
-   double Ts = 0.01; //Sampling time 100 Hz
-   double Kp = 11.1; // proportional gain
-   double Ki = 11.2; //Integral gain
-   double Kd = 5.5; //Differential gain
-   
-   //Arm drive parameters
-   //double Ts = 0.01; //Sampling time 100 Hz
-   //double Kp = 19.8; // proportional gain
-   //double Ki = 19.9; //Intergral gain
-   //double Kd = 9.8; //Differential gain.
-   
-   static double ErrorIntegral = 0;
-   static double ErrorPrevious = Error;
-   static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-   
-   //Proportional part:
-   double u_k = Kp * Error;
-   
-   //Integral part:
-   ErrorIntegral = ErrorIntegral + Error*Ts;
-   double u_i = Ki * ErrorIntegral;
-   
-   //Derivative part:
-   double ErrorDerivative = (Error - ErrorPrevious)/Ts;
-   double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
-   double u_d = Kd * FilteredErrorDerivative;
-   ErrorPrevious = Error;
-   
-   // sum of parts and return it
-   return u_k + u_i + u_d; //This will become the MotorValue
-}
-
-//Ticker function set motorvalues
-void SetMotor(double MotorValue)
-{
-    if (MotorValue >=0)
-    {
-        DirectionPin = 1;
-    }
-    else
-    {
-        DirectionPin = 0;
-    }
-    
-    if (fabs(MotorValue)>1) // if error more than 1 radian, full duty cycle
-    {
-        PwmPin = 1; 
-    }
-    else
-    {
-        PwmPin = fabs(MotorValue);
-    }
-}
-
-// ----------------------------------------------------------------------------
-//Ticker function
-void MeasureAndControl(void)
-{
-    double PositionRef = GetReferencePosition();
-    double PositionMotor = GetActualPosition();
-    double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
-    SetMotor(MotorValue);
-    
-    //for printing on screen
-    PosRefPrint = PositionRef;
-    PosMotorPrint = PositionMotor;
-    ErrorPrint = PositionRef - PositionMotor;
-    
-}
-
-
-
-void PrintToScreen()
-{
-    PrintFlag = true;
-}
-
-
-//-----------------------------------------------------------------------------
-int main()
-{
-    pc.baud(115200);
-    pc.printf("Hello World\n\r");
-    PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
-    TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz
-    TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen
-    
-    while (true) 
-    {
-        if(PrintFlag) // if-statement for printing every second four times to screen
-        {
-            double KpPrint = 3.52;
-            double KiPrint = 0.88;
-            double KdPrint = 6.9;
-            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f, Ki = %f and Kd = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint,KdPrint);
-            PrintFlag = false;
-        }
-    }
-}
\ No newline at end of file