Diff: OperationMode.cpp

Revision:

10:93957c339972

Child:

11:dcc416dbe3ea

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OperationMode.cpp	Wed Oct 31 19:39:57 2018 +0000
@@ -0,0 +1,247 @@
+#include "mbed.h"
+#include "math.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "BiQuad.h"
+
+//Tickers
+Ticker TickerMeasureAndControl;
+Timer   TimerLoop;
+//Communication
+MODSERIAL pc(USBTX,USBRX);
+QEI Encoder(D10,D11,NC,32);
+QEI Encoder2(D12,D13,NC,32);
+//Global pin variables
+PwmOut PwmPin(D5);
+PwmOut PwmPin2(D6);
+DigitalOut DirectionPin(D4);
+DigitalOut DirectionPin2(D7);
+DigitalIn BUT1(D8);
+DigitalIn BUT2(D9);
+DigitalIn button_sw2(SW2);
+DigitalIn button_sw3(SW3);
+
+DigitalOut LedGreen(LED_GREEN);
+DigitalOut LedRed(LED_RED);
+DigitalOut LedBlue(LED_BLUE);
+
+double PositionRef=0;
+double PositionRef2=0;
+
+
+//Define substates Operation Mode
+
+enum States {OPWait, OPState1, OPState2, OPState3, OPState4, OPHoming};
+States CurrentOperationState;
+
+void OperationStateMachine()
+{
+
+    
+    switch (CurrentOperationState) {
+        case OPWait:
+            LedRed = 0; //red
+            if (button_sw2 == false) {
+                CurrentOperationState = OPState1;
+                TimerLoop.reset();
+                }
+            break;
+        case OPState1:
+            LedGreen = 0; //yellow
+            if (PositionRef < 0.24*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
+                PositionRef += 0.0005*(6.380);
+            }
+            if ((PositionRef > 0.24*(6.380)) && (TimerLoop >= 4.0)) {
+                CurrentOperationState = OPState2;
+                TimerLoop.reset();
+            }
+            break;
+        case OPState2:
+            LedBlue = 0; //white
+            if (PositionRef2 > -0.23*(6.380)) {
+                PositionRef2 -= 0.0005*(6.380);
+            }
+            if ((PositionRef2 < -0.23*(6.380)) && (TimerLoop >= 4.0)) {
+                CurrentOperationState = OPState3;
+                TimerLoop.reset();
+            }
+            break;
+        case OPState3:
+            LedGreen = 1; //Turqoise
+            if (PositionRef > -0.005*(6.380)) {
+                PositionRef -= 0.0005*(6.380);
+            }
+            if (PositionRef2 > -0.345*(6.380)) {
+                PositionRef2 -= 0.0005*(6.380);
+            }
+            if ((PositionRef < -0.005*(6.380)) && (PositionRef2 < -0.345*(6.380)) && (TimerLoop >= 4.0)) {
+                
+                CurrentOperationState = OPState4;
+                TimerLoop.reset();
+            }
+            break;
+        case OPState4:
+            LedGreen = 1; //Blue
+            if (PositionRef2 < 0.595*(6.380)) {
+                PositionRef2 += 0.0005*(6.380);
+            }
+            if (PositionRef2 > 0.595*(6.380)) {
+                CurrentOperationState = OPHoming;
+            }
+            break;
+        case OPHoming:
+            LedGreen = 0; //Green
+            LedBlue = 1;
+            LedRed = 1;
+            break;
+    }
+}
+
+
+
+
+//-----------------------------------------------------------------------------
+//The double-functions
+
+
+
+
+// actual position of the motor 1
+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;
+}
+
+double GetActualPosition2() //motor 2
+{
+    double Encoder2Counts = Encoder2.getPulses();    //number of counts
+    double PositionMotor2 = Encoder2Counts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
+
+    return PositionMotor2;
+}
+
+///The controller
+double PID_Controller(double Error)
+{
+    //Arm drive parameters
+    double Ts = 0.002; //Sampling time 100 Hz
+    double Kp = 19.8; // proportional gain
+    double Ki = 3.98; //Intergral gain
+    double Kd = 1.96; //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
+}
+
+double PID_Controller2(double Error)
+{
+    //Belt drive parameters
+    double Ts = 0.002; //Sampling time 100 Hz
+    double Kp = 11.1; // proportional gain
+    double Ki = 2.24; //Integral gain
+    double Kd = 1.1; //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, double MotorValue2)
+{
+    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);
+    }
+
+    if (MotorValue2 >=0) {
+        DirectionPin2 = 1;
+    } else {
+        DirectionPin2 = 0;
+    }
+
+    if (fabs(MotorValue2)>1) { // if error more than 1 radian, full duty cycle
+        PwmPin2 = 1;
+    } else {
+        PwmPin2 = fabs(MotorValue2);
+    }
+}
+
+// ----------------------------------------------------------------------------
+//Ticker function
+void MeasureAndControl(void)
+{
+    
+    //double PositionRef = GetReferencePosition();
+    //double PositionRef2 = GetReferencePosition2();
+    
+    OperationStateMachine();
+    double PositionMotor = GetActualPosition();
+    double PositionMotor2 = GetActualPosition2();
+    double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
+    double MotorValue2 = PID_Controller2(PositionRef2 - PositionMotor2);
+    SetMotor(MotorValue, MotorValue2);
+}
+
+
+
+//-----------------------------------------------------------------------------
+int main()
+{
+    LedGreen = 1;
+    LedRed = 1;
+    LedBlue = 1;
+    pc.baud(115200);
+    pc.printf("Hello World\n\r");
+    TimerLoop.start();
+    CurrentOperationState = OPWait;
+    PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
+    TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz
+    while (true) {
+    }
+}
\ No newline at end of file