Diff: main.cpp

Revision:

8:e8734a254818

Parent:

7:439940ae1197

Child:

9:355dd95199c3

diff -r 439940ae1197 -r e8734a254818 main.cpp
--- a/main.cpp	Fri Oct 26 07:58:07 2018 +0000
+++ b/main.cpp	Mon Oct 29 14:02:21 2018 +0000
@@ -10,18 +10,18 @@
 PwmOut PwmPin2(D6);
 DigitalIn Knop1(D2);
 DigitalIn Knop2(D3);
+DigitalIn Knop3(PTA4);
 AnalogIn pot1 (A5);
 AnalogIn pot2 (A4);
 AnalogIn emg0( A0 );
 AnalogIn emg1( A1 );
 AnalogIn emg2( A2 );
 AnalogIn emg3( A3 );
+DigitalOut led(LED_GREEN);
 
-QEI Encoder1(D12,D13,NC,64);
+QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
 
-//DigitalOut LED(LED_RED);
-
 Ticker      StateTicker;
 Ticker      printTicker;
 
@@ -31,15 +31,31 @@
 volatile float Bicep_Left           = 0.0;
 volatile float Tricep_Right         = 0.0;
 volatile float Tricep_Left          = 0.0;
+
 volatile const float maxVelocity    = 8.4; // in rad/s
 volatile const float pi             = 3.1415926; 
 volatile const float rad_count      = 0.0007479; // 2pi/8400;
+
 volatile float referenceVelocity1   = 0.5; //dit is de gecentreerde waarde en dus de nulstand
 volatile float referenceVelocity2   = 0.5;
 
-enum states{Calibration, Homing, Function};
+volatile float q_1;
+volatile float q_2;
+volatile float r_1;
+volatile float r_2;
+volatile const float r_3 = 0.035;
+volatile float w_1;
+volatile float w_2;
+
+volatile float Flex_Bi_R;
+volatile float Flex_Bi_L;
+volatile float Flex_Tri_R;
+volatile float Flex_Tri_L;
+
+
+enum states{Starting, Calibration, Homing, Function};
     
-volatile states Active_State = Calibration;
+volatile states Active_State = Starting;
 
 volatile int counts1 ;
 volatile int counts2 ;
@@ -75,6 +91,36 @@
     scope.send();
 }
 
+void Inverse()
+{
+    q_1= rad_m1;                                          // uit Encoder
+    q_2= rad_m2+(pi/2.0f);                                          // uit Encoder
+    r_1= -0.2f;
+    r_2= -0.2f;
+    
+        float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
+        float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3;
+        float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3;
+        float x = (-2.0f)*r_2*sin(q_1)*cos(q_2);
+        float D = 1.0f/(u*z-x*y);                         // Determinant
+        printf("Determinant is %f\n", D);
+        
+        float a = D*z;                                  // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
+        float b = -D*x;                                 // Inverse jacobian
+        float c = -D*y;                                 // Inverse jacobian
+        float d = D*u;                                  // Inverse jacobian
+        
+        float vx = 0.01f;                                // uit emg data
+        float vy = 0.0f;                                 // uit emg data
+        
+        w_1 = vx*a+vy*b;
+        w_2 = vx*c+vy*d;
+        
+        /*
+        printf("%f\n", w_1);
+        printf("%f\n", w_2);
+        */     
+}
    
 void velocity1()
     {
@@ -117,37 +163,119 @@
     }    
     
 void motor1()
-    {  
-        float u = referenceVelocity1;
+    {          
+        float u_v1 = w_1; //referenceVelocity1       
+        float u = u_v1/ (2.0f * pi);
         DirectionPin1 = u < 0.0f;
         PwmPin1 = fabs(u);
     }
 
 void motor2()
     {  
-        float u = referenceVelocity2;
+        float u_v2 = w_2; //referenceVelocity2
+        float u = u_v2/ (2.0f * pi);
         DirectionPin2 = u > 0.0f;
         PwmPin2 = fabs(u);
     }
+    
+void Calibrating()
+{
+    static float n = 0.0;
+    static float m = 0.0;
+    static float l = 0.0;
+    static float k = 0.0;
+ 
+    for(int ii=0; ii<=20000; ii++)
+    {
+            if (ii <2500)
+            {
+                n = n + emg0.read();
+            }
+            else if (ii == 2500)
+            {
+                Flex_Bi_R = n / 2500.0f;
+            }
+            else if (ii>2500 && ii<=3500)
+            {
+                //chillen
+            }
+            else if(ii>3500 && ii<6000)
+            {
+                m = m + emg1.read();
+            }
+            else if (ii == 6000)
+            {
+                Flex_Bi_L = m / 2500.0f;
+            }
+            else if (ii>6000 && ii<=7000)
+            {
+                //chillen
+            }
+            else if (ii>7000 && ii<9500)
+            {
+                l = l + emg2.read();
+            }
+            else if (ii == 9500)
+            {
+                Flex_Tri_R = l / 2500.0f;
+            }
+            else if (ii>9500 && ii <=10500)
+            {
+                //chillen
+            }
+            else if (ii>10500 && ii<13000)
+            {
+                k = k + emg3.read();
+            }
+            else if (ii == 13000)
+            {
+                Flex_Tri_L = k / 2500.0f;
+            }          
+    }
+}
+
+void OFF()
+{
+    PwmPin1 = PwmPin2 = 0;
+}
 
 void Printing()
 {
-    float v1 = fabs(referenceVelocity1) * maxVelocity;
-    float v2 = fabs(referenceVelocity2) * maxVelocity;
-    
-    //eventueel nog counts -> rad/s 
-    
-    //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
-    pc.printf("%f    %f \n",rad_m1,rad_m2);
+    float v1 = PwmPin1 * maxVelocity;
+    float v2 = PwmPin2 * maxVelocity;
+
+    pc.printf("q1 = %f [rad] \n q2 = %f [rad] \n q1dot = %f [rad/s] \n q2dot = %f [rad/s] \n\n\n\n\n", rad_m1, rad_m2, v1, v2);
 }
 
 void StateMachine() 
 {
     switch (Active_State)
     {
+        case Starting:   
+        OFF();            
+            for (int i = 0; i<=20; i++)
+            {
+                led = !led;
+                wait(0.05);
+                
+                if (i == 0)
+                {
+                    pc.printf("Starting up..\n");
+                }    
+                else if (i == 20)
+                {    
+                    Active_State = Calibration;
+                    pc.printf("Entering Calibration state \n");
+                }
+            }                           
+        break;
+        
         case Calibration:
                 //calibration actions
                 //pc.printf("Calibration State");
+                
+                Calibrating();
+                OFF();
                 if (Knop1==false)
                 {
                     pc.printf("Entering Homing state \n");
@@ -157,7 +285,6 @@
                 sample();
                 EMG_Read();
                 Encoding();    
-                
         break;
         
         case Homing:
@@ -176,27 +303,20 @@
         
         case Function:
             //pc.printf("Funtioning State");
-                /*
-                if (Knop2==false)
-                {
-                    pc.printf("Re-entering Homing State \n");
-                    Active_State = Homing;
-                }
-                else if (Knop1==false)
-                {
+                
+                if (Knop3==false)
+                { 
                     pc.printf("Re-entering Calibration State \n");
                     Active_State = Calibration;
                 }
-                */
-                
+               
                 sample();
                 EMG_Read();
                 Encoding();
                 velocity1();
                 velocity2();
                 motor1();
-                motor2();
-                
+                motor2();                
         break;    
             
         default:
@@ -209,9 +329,9 @@
     pc.baud(115200);    
     PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz 
     
-    StateTicker.attach(StateMachine, 0.002);
+    StateTicker.attach(&StateMachine, 0.002);
    
-    printTicker.attach(&Printing, 2.0);
+    printTicker.attach(&Printing, 4.0);
     
     while(true)
     {