Diff: StateMachinePTR.cpp

Revision:

3:97cac1d5ba8a

Parent:

2:c2ae5044ec82

Child:

4:4728763bbb44

--- a/StateMachinePTR.cpp	Mon Oct 29 14:50:50 2018 +0000
+++ b/StateMachinePTR.cpp	Tue Oct 30 13:48:54 2018 +0000
@@ -3,9 +3,19 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "BiQuad.h"
+#include "MovingAverage.h"
+#define NSAMPLE 200
+
+// Inverse dingen
+//InverseJacobian11 = (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
+//InverseJacobian12 = -(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
+//InverseJacobian21 = -(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
+//InverseJacobian22 = (L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
+
+
 
 //states
-enum States {Waiting, Homing, Emergency,EMGCal,MotorCal,Operation,Demo};
+enum States {Waiting, Homing, Emergency, EMGCal, MotorCal, Operation, Demo};
 
 // Global variables
 States CurrentState;
@@ -14,38 +24,125 @@
 
 //Communication
 MODSERIAL pc(USBTX,USBRX);
-QEI Encoder(D10,D11,NC,32);
+QEI Encoder1(D10,D11,NC,32);    // Encoder motor q1 (arm)
+QEI Encoder2(D12,D13,NC,32);    // Encoder motor q2 (end-effector)
 
-//Global pin variables Motor 1
-PwmOut PwmPin(D5);
-DigitalOut DirectionPin(D4);
-AnalogIn Potmeter1(A1);
-AnalogIn Potmeter2(A0);
+//EMG settings
+AnalogIn emg0(A0); //Biceps Left
+AnalogIn emg1(A1); //Biceps Right
+MovingAverage <double>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above 
+MovingAverage <double>Movag_RB(NSAMPLE,0.0);
+    
+    // filters
+//Make Biquad filters Left(a0, a1, a2, b1, b2) 
+BiQuadChain bqc1; //chain voor High Pass en Notch
+BiQuad bq1(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
+BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
+//Make Biquad filters Right
+BiQuadChain bqc2; //chain voor High Pass en Notch
+BiQuad bq3(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
+BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
+
+
+//Global pin variables Motors 1
+PwmOut PwmPin1(D5);
+DigitalOut DirectionPin1(D4);
+
+//Global pin variables Motors 2
+PwmOut PwmPin2(D6);
+DigitalOut DirectionPin2(D7);
 
 //Output LED
 DigitalOut LedRed (LED_RED);
 DigitalOut LedGreen (LED_GREEN);
 DigitalOut LedBlue (LED_BLUE);
 
+// Buttons
+DigitalIn ButtonHoming(SW2); // Button to go to Homing state
+DigitalIn BUTSW3(SW3);
+DigitalIn BUT1(D8);
+DigitalIn BUT2(D9);
 
-// Buttons
-DigitalIn Button1(SW2);
-DigitalIn Button2(SW3);
-DigitalIn ButtonHome(D8);
+//Constant variables
+const float L0 = 0.1; // Distance between origin frame and joint 1 [m[
+const float L1 = 0.326; // Length link 1 (shaft to shaft) [m] 
+const float L2 = 0.209; // Length link 2 (end-effector length) [m]
+
+ 
+// Volatile variables
+//EMG
+volatile bool EMGBoolLeft; // Boolean EMG 1 (left)
+volatile bool EMGBoolRight; // Boolean EMG 2 (right)
+volatile double LeftBicepsOut; // Final value left of processed EMG
+volatile double RightBicepsOut; // Final value right of processed EMG
+volatile double ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1)
+volatile double ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1)
+
+
+//Motors
+volatile float q1; // Current angle of motor 1 (arm)
+volatile float q2; // Current angle of motor 2 (end-effector)
+volatile float MotorValue1; // Inputvalue to set motor 1 
+volatile float MotorValue2; // Inputvalue to set motor 2
+
+//Inverse kinematics
+volatile float VdesX; // Desired velocity in x direction
+volatile float VdesY; // Desired velocity in y direction
+volatile float Error1; // Difference in reference angle and current angle motor 1
+volatile float Error2; // Difference in reference angle and current angle motor 2
+
 
 
-//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 ()
+// Function for processing EMG
+    void ProcessingEMG()
+    {
+    //Filter Left Biceps
+    double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
+    double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal 
+    Movag_LB.Insert(LB_Rectify); //Moving Average
+    LeftBicepsOut = Movag_LB.GetAverage(); //Get final value
+    
+    //Filter Right Biceps
+    double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
+    double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
+    Movag_RB.Insert(RB_Rectify); //Moving Average 
+    RightBicepsOut = Movag_RB.GetAverage();  //Get final value
+    
+    if (LeftBicepsOut > ThresholdLeft) 
+        {
+            EMGBoolLeft = true;
+        }
+    else 
+        {
+            EMGBoolLeft = false;
+        }
+    if (RightBicepsOut > ThresholdRight) 
+        {
+            EMGBoolRight = true;
+        }
+    else 
+        {
+            EMGBoolRight = false;
+        }
+    
+    } 
+
+void MeasureAll()
+{
+    //Processing and reading EMG
+    ProcessingEMG();
+
+    //Measure current motor angles
+    q1 = Encoder1.getPulses()/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2)
+    q2 = Encoder2.getPulses()/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2)
+    
+}
+
+//State machine
+void StateMachine()
 {
     switch (CurrentState)
     {
@@ -59,9 +156,6 @@
             LedRed = 0;
             LedGreen = 0;
             LedBlue = 0; //White
-            if (TimerLoop.read() >= 5.0f)
-            {CurrentState = Waiting;}
-            
         break;  
         
         case Emergency:
@@ -98,154 +192,164 @@
     }
 }
 
-//-----------------------------------------------------------------------------
-//The double-functions
-
-//Get reference position
-double GetReferencePosition()
+//------------------------------------------------------------------------------
+// Inverse Kinematics
+void InverKinematics()
 {
-// 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
+//------------------------------------------------------------------------------
+// controller motor 1
+ void PID_Controller1()
+ {
+   double Ts = 0.002; //Sampling time 500 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 = Error1;
+   static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+   
+   //Proportional part:
+   double u_k = Kp * Error1;
+   
+   //Integral part:
+   ErrorIntegral = ErrorIntegral + Error1*Ts;
+   double u_i = Ki * ErrorIntegral;
+   
+   //Derivative part:
+   double ErrorDerivative = (Error1 - ErrorPrevious)/Ts;
+   double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
+   double u_d = Kd * FilteredErrorDerivative;
+   ErrorPrevious = Error1;
+   
+   MotorValue1 =  u_k + u_i + u_d; //This will become the MotorValue to set motor 1
+ }
+   
+// controller motor 2
+ void PID_Controller2()
+ {
+   double Ts = 0.002; //Sampling time 500 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 double ErrorPrevious = Error2;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    //Proportional part:
-   double u_k = Kp * Error;
+   double u_k = Kp * Error2;
    
    //Integral part:
-   ErrorIntegral = ErrorIntegral + Error*Ts;
+   ErrorIntegral = ErrorIntegral + Error2*Ts;
    double u_i = Ki * ErrorIntegral;
    
    //Derivative part:
-   double ErrorDerivative = (Error - ErrorPrevious)/Ts;
+   double ErrorDerivative = (Error2 - ErrorPrevious)/Ts;
    double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
    double u_d = Kd * FilteredErrorDerivative;
-   ErrorPrevious = Error;
+   ErrorPrevious = Error2;
    
-   return u_k + u_i + u_d; //This will become the MotorValue
+   MotorValue2 =  u_k + u_i + u_d; //This will become the MotorValue to set motor 2
+ }
+
+// Main function for motorcontrol
+void MotorControllers()
+{   
+    PID_Controller1();
+    PID_Controller2();
+    
 }
+//------------------------------------------------------------------------------
 
 //Ticker function set motorvalues
-void SetMotor(double MotorValue)
+void SetMotors()
 {
-    if (MotorValue >=0)
+// Motor 1
+    if (MotorValue1 >=0) // set direction
     {
-        DirectionPin = 1;
+        DirectionPin1 = 1;
     }
     else
     {
-        DirectionPin = 0;
+        DirectionPin1 = 0;
     }
     
-    if (fabs(MotorValue)>3) // if error more than 1 radian, full duty cycle
+    if (fabs(MotorValue1)>1) // set duty cycle
+    {
+        PwmPin1 = 1; 
+    }
+    else
     {
-        PwmPin = 1; 
+        PwmPin1 = fabs(MotorValue1);
+    }
+    
+// Motor 2
+    if (MotorValue2 >=0) // set direction
+    {
+        DirectionPin2 = 1;
     }
     else
     {
-        PwmPin = 0; //fabs(MotorValue);
+        DirectionPin2 = 0;
     }
+    
+    if (fabs(MotorValue2)>1) // set duty cycle
+    {
+        PwmPin2 = 1; 
+    }
+    else
+    {
+        PwmPin2 = fabs(MotorValue2);
+    }
+
 }
 
-void SetMotorOff()
+void SetMotorsOff()
 {
-    PwmPin = 0; // Turn motor off
+    // Turn motors off
+    PwmPin1 = 0; 
+    PwmPin2 = 0;
 }
 
 //-----------------------------------------------------------------------------
-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 (Button1.read() == false) // if pressed
+  //  {CurrentState = Operation; TimerLoop.reset();}
     
-    if (Button2.read() == false) // if pressed 
-    {CurrentState = Demo; TimerLoop.reset();}
+   // if (Button2.read() == false) // if pressed 
+   // {CurrentState = Demo; TimerLoop.reset();}
     
-    if (ButtonHome.read() == false) // if pressed
-    {CurrentState = Homing; TimerLoop.reset();}
+   // if (ButtonHome.read() == false) // if pressed
+  //  {CurrentState = Homing; TimerLoop.reset();}
     //functions
-    StateMachine(); //determine states
-    if (CurrentState >= 4)
-    {MeasureAndControl(); PrintToScreen();}
-    else 
-    {SetMotorOff();}
+  //  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();
+    TimerLoop.start(); // start Timer
     CurrentState = Waiting; // set initial state as Waiting
+    bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain EMG left
+    bqc2.add( &bq3 ).add( &bq4 ); //make BiQuadChain EMG right
     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