Diff: Test_cal.cpp

Revision:

0:84e285dc97bd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Test_cal.cpp	Tue Oct 30 12:27:19 2018 +0000
@@ -0,0 +1,187 @@
+#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); //Button for + arm rotation
+DigitalIn BUT2(D9); //Button for - arm rotation 
+DigitalIn button_set(SW3); //Button to set the 0 config
+DigitalIn button(SW2); //Button to return to 0-reference config
+//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
+
+//Calibration for arm drive
+//double GetReferencePosition()
+//{
+  //  static double PositionRef=0;
+
+    //if (BUT1==false) {
+      //  PositionRef += 0.0005*(6.2830);
+    //}
+    //if (button_set == false) {
+      //  PositionRef = 0;
+       // Encoder.reset();
+    //}
+    //if BUT2 == false){
+        //PositionRef -= 0.0005*(6.2830);
+    //if (button == false) {
+      //  if (PositionRef>=-0.52*(6.2830)) {
+        //    PositionRef -=0.0005*(6.2830);
+        //}
+    //}
+    //return PositionRef;
+//}
+
+//Calibration for belt drive
+double GetReferencePosition()
+{
+    static double PositionRef=0;
+
+    if (BUT1==false) {
+        PositionRef += 0.0005*(6.2830);
+    }
+    if (button_set == false) {
+        PositionRef = 0;
+        Encoder.reset();
+    }
+    if (BUT2 == false){
+        PositionRef -=0.0005*(6.2830);
+    }
+    if (button == false) {
+        if (PositionRef<=0.733*(6.2830)) {
+            PositionRef +=0.0005*(6.2830);
+        }
+    }
+      return PositionRef;
+}
+
+// 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