Diff: StateMachinePTR.cpp

Revision:

4:4728763bbb44

Parent:

3:97cac1d5ba8a

Child:

5:cbcff21e74a0

--- a/StateMachinePTR.cpp	Tue Oct 30 13:48:54 2018 +0000
+++ b/StateMachinePTR.cpp	Tue Oct 30 14:53:14 2018 +0000
@@ -6,14 +6,6 @@
 #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};
 
@@ -67,6 +59,7 @@
 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]
+const double Ts = 0.002; //Sampling time 500 Hz
 
  
 // Volatile variables
@@ -168,6 +161,30 @@
             LedRed = 0;
             LedGreen = 1;
             LedBlue = 0; //Pink
+            static double MaxLeft = 0;
+            static double MaxRight = 0;
+            
+                if(LeftBicepsOut > MaxLeft) 
+                {
+                    MaxLeft = LeftBicepsOut;
+                }
+    
+                if(RightBicepsOut > MaxRight)
+                {
+                    MaxRight = RightBicepsOut;
+                }
+                
+            if (BUT1 == false)
+            {
+                ThresholdLeft = abs(0.15000*MaxLeft);
+                ThresholdRight = abs(0.15000*MaxRight);
+                TimerLoop.reset();
+            }
+            if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 2))
+            {
+                CurrentState = MotorCal;
+                TimerLoop.reset();
+            }
         break;
         
         case MotorCal:
@@ -193,10 +210,32 @@
 }
 
 //------------------------------------------------------------------------------
+// Function to set desired cartesian velocities of end-effector
+void Vdesired()
+{
+    double Vconst = 0.1; // m/s (10 cm per second)
+    VdesX = EMGBoolLeft*Vconst; // Left biceps is X-direction
+    VdesY = EMGBoolRight*Vconst; // Right biceps is Y-direction
+}
+
 // Inverse Kinematics
-void InverKinematics()
+void InverseKinematics()
 {
-
+    // matrix inverse Jacobian
+    double InvJac11 = (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)));
+    double InvJac12 = -(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)));  
+    double InvJac21 = -(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)));
+    double InvJac22 = (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)));
+    
+    //Gear Ratio's
+    double RatioGears = 134.0/30.0; //Gear Ratio for motor 1
+    double RatioPulleys = 87.4/27.5; // Gear Ratio for motor 2
+    
+    double q1DotRef = (InvJac11*VdesX + InvJac12*VdesY)*RatioGears; //reference angular velocity motor 1
+    double q2DotRef = (InvJac21*VdesX + InvJac22*VdesY)*RatioPulleys; //reference angular velocity motor 2
+    
+    Error1 = q1DotRef*Ts; // difference between qReference and current q1
+    Error2 = q2DotRef*Ts; // difference between qReference and current q2
 }
 
 
@@ -204,10 +243,9 @@
 // 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
+   double Kp = 19.8; // proportional gain
+   double Ki = 40.9;//integral gain
+   double Kd = 3;  //derivative gain
    static double ErrorIntegral = 0;
    static double ErrorPrevious = Error1;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
@@ -231,10 +269,9 @@
 // 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
+   double Kp = 11.1; // proportional gain
+   double Ki = 22.81;//integral gain
+   double Kd = 1.7;  //derivative gain
    static double ErrorIntegral = 0;
    static double ErrorPrevious = Error2;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);