Diff: StateMachinePTR.cpp

Revision:

2:c2ae5044ec82

Child:

3:97cac1d5ba8a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachinePTR.cpp	Mon Oct 29 14:50:50 2018 +0000
@@ -0,0 +1,251 @@
+#include "mbed.h"
+#include "math.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "BiQuad.h"
+
+//states
+enum States {Waiting, Homing, Emergency,EMGCal,MotorCal,Operation,Demo};
+
+// Global variables
+States CurrentState;
+Ticker TickerLoop;
+Timer TimerLoop;
+
+//Communication
+MODSERIAL pc(USBTX,USBRX);
+QEI Encoder(D10,D11,NC,32);
+
+//Global pin variables Motor 1
+PwmOut PwmPin(D5);
+DigitalOut DirectionPin(D4);
+AnalogIn Potmeter1(A1);
+AnalogIn Potmeter2(A0);
+
+//Output LED
+DigitalOut LedRed (LED_RED);
+DigitalOut LedGreen (LED_GREEN);
+DigitalOut LedBlue (LED_BLUE);
+
+
+// Buttons
+DigitalIn Button1(SW2);
+DigitalIn Button2(SW3);
+DigitalIn ButtonHome(D8);
+
+
+//Global variables for printing on screen
+volatile bool PrintFlag = false;
+volatile float PosRefPrint; // for printing value on screen
+volatile float PosMotorPrint; // for printing value on screen
+volatile float ErrorPrint;
+volatile int PrintCount = 0;
+volatile float MotorValuePrint;
+
+//------------------------------------------------------------------------------
+
+//Functions
+void StateMachine ()
+{
+    switch (CurrentState)
+    {
+        case Waiting:
+            LedRed = 0;
+            LedGreen = 0;
+            LedBlue = 1; //Yellow
+        break;
+        
+        case Homing:
+            LedRed = 0;
+            LedGreen = 0;
+            LedBlue = 0; //White
+            if (TimerLoop.read() >= 5.0f)
+            {CurrentState = Waiting;}
+            
+        break;  
+        
+        case Emergency:
+            LedRed = 0;
+            LedGreen = 1;
+            LedBlue = 1; //Red
+        break;
+              
+        case EMGCal:
+            LedRed = 0;
+            LedGreen = 1;
+            LedBlue = 0; //Pink
+        break;
+        
+        case MotorCal:
+            LedRed = 1;
+            LedGreen = 0;
+            LedBlue = 0; //Turqoise
+        break;
+              
+        case Operation:
+            LedRed = 1;
+            LedGreen = 0;
+            LedBlue = 1; //Green
+        break;
+        
+        case Demo:
+            LedRed = 1;
+            LedGreen = 1;
+            LedBlue = 0; //Blue       
+        break;
+        
+
+    }
+}
+
+//-----------------------------------------------------------------------------
+//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
+    
+
+    
+    double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
+    
+        
+        double PositionRef = 3*6.2830*ValuePot - 3*3.1415; // position reference ranging from -1.5 rotations till 1.5 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)
+{
+
+   double Ts = 0.005; //Sampling time 100 Hz
+   double Kp = 5.34; // proportional gain
+   double Ki = 2.0;//integral gain
+   double Kd = 6.9;  //derivative 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;
+   
+   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)>3) // if error more than 1 radian, full duty cycle
+    {
+        PwmPin = 1; 
+    }
+    else
+    {
+        PwmPin = 0; //fabs(MotorValue);
+    }
+}
+
+void SetMotorOff()
+{
+    PwmPin = 0; // Turn motor off
+}
+
+//-----------------------------------------------------------------------------
+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;
+    MotorValuePrint = MotorValue;
+}
+
+
+
+void PrintToScreen()
+{
+    PrintCount++;
+    if (PrintCount == 100) // printing with 2 Hz
+    {PrintFlag = true; PrintCount = 0;}
+}
+
+
+// Execution function
+void LoopFunction()
+{
+    // buttons
+    if (Button1.read() == false) // if pressed
+    {CurrentState = Operation; TimerLoop.reset();}
+    
+    if (Button2.read() == false) // if pressed 
+    {CurrentState = Demo; TimerLoop.reset();}
+    
+    if (ButtonHome.read() == false) // if pressed
+    {CurrentState = Homing; TimerLoop.reset();}
+    //functions
+    StateMachine(); //determine states
+    if (CurrentState >= 4)
+    {MeasureAndControl(); PrintToScreen();}
+    else 
+    {SetMotorOff();}
+}
+
+int main()
+{
+    pc.baud(115200);
+    pc.printf("Hi I'm PTR, you can call me Peter!\r\n");
+    TimerLoop.start();
+    CurrentState = Waiting; // set initial state as Waiting
+    TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz
+    
+    while (true)
+    {
+        if(PrintFlag) // if-statement for printing every second four times to screen
+        {
+
+            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, motorvalue = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,MotorValuePrint);
+            PrintFlag = false;
+        }        
+    }
+    
+}
\ No newline at end of file