Diff: main.cpp

Revision:

9:355dd95199c3

Parent:

8:e8734a254818

Child:

10:05ad15c48388

--- a/main.cpp	Mon Oct 29 14:02:21 2018 +0000
+++ b/main.cpp	Tue Oct 30 13:57:00 2018 +0000
@@ -11,13 +11,16 @@
 DigitalIn Knop1(D2);
 DigitalIn Knop2(D3);
 DigitalIn Knop3(PTA4);
+DigitalIn Knop4(PTC6);
 AnalogIn pot1 (A5);
 AnalogIn pot2 (A4);
 AnalogIn emg0( A0 );
 AnalogIn emg1( A1 );
 AnalogIn emg2( A2 );
 AnalogIn emg3( A3 );
-DigitalOut led(LED_GREEN);
+DigitalOut led_G(LED_GREEN);
+DigitalOut led_R(LED_RED);
+DigitalOut led_B(LED_BLUE);
 
 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
@@ -32,13 +35,17 @@
 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 const float rad_count      = 0.0007479;                                // 2pi/8400;
+volatile const float maxVelocity    = 2.0f * pi;                                // in rad/s
 
-volatile float referenceVelocity1   = 0.5; //dit is de gecentreerde waarde en dus de nulstand
+volatile float referenceVelocity1   = 0.5;                                      // dit is de gecentreerde waarde en dus de nulstand
 volatile float referenceVelocity2   = 0.5;
 
+volatile int i   = 0;                                                           // Led blink status
+volatile int ii  = 0;                                                           // Calibratie timer
+volatile int iii = 0;                                                           // Start up timer
+
 volatile float q_1;
 volatile float q_2;
 volatile float r_1;
@@ -51,12 +58,17 @@
 volatile float Flex_Bi_L;
 volatile float Flex_Tri_R;
 volatile float Flex_Tri_L;
-
+volatile float Threshold_Bi_R;
+volatile float Threshold_Bi_L;
+volatile float Threshold_Tri_R;
+volatile float Threshold_Tri_L;
 
 enum states{Starting, Calibration, Homing, Function};
     
 volatile states Active_State = Starting;
 
+volatile float vx;
+volatile float vy;
 volatile int counts1 ;
 volatile int counts2 ;
 volatile float rad_m1;
@@ -72,6 +84,53 @@
     rad_m2 = rad_count * (float)counts2;
 }
 
+void BlinkLed()
+{
+    if(i==1)
+    {
+            static int rr = 0;
+            rr++;
+            if (rr == 1)
+            {
+                led_R = !led_R;
+            }
+            else if (rr == 100)
+            {
+                rr = 0;
+            }
+    }     
+    else if(i==2)
+    {        
+            static int gg = 0;
+            gg++;
+            if (gg == 1)
+            {
+                led_G = !led_G;
+            }
+            else if (gg == 100)
+            { 
+                gg = 0;
+            }
+    }
+    else if (i==3)
+    {            
+            static int bb = 0;
+            bb++;
+            if (bb == 1)
+            {
+                led_B = !led_B;
+            }
+            else if (bb == 100)
+            {
+                bb = 0;
+            }
+    }
+    else 
+    {
+        led_R=led_G=led_B=1;
+    }  
+}
+
 void EMG_Read()
 {
     Bicep_Right     =   emg0.read();
@@ -93,8 +152,8 @@
 
 void Inverse()
 {
-    q_1= rad_m1;                                          // uit Encoder
-    q_2= rad_m2+(pi/2.0f);                                          // uit Encoder
+    q_1= rad_m1;                                                                // uit Encoder
+    q_2= rad_m2+(pi/2.0f);                                                      // uit Encoder
     r_1= -0.2f;
     r_2= -0.2f;
     
@@ -102,26 +161,26 @@
         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 D = 1.0f/(u*z-x*y);                                               // Determinant
+        printf("Determinant is %f\r\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 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
+        float vx = pot1.read();//0.01f;                                                       // uit emg data
+        float vy = pot2.read();//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);
+        printf("%f\r\n", w_1);
+        printf("%f\r\n", w_2);
         */     
 }
-   
+/*   
 void velocity1()
     {
             if (pot1.read()>0.5f)
@@ -161,7 +220,7 @@
                 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
                 }
     }    
-    
+*/    
 void motor1()
     {          
         float u_v1 = w_1; //referenceVelocity1       
@@ -177,60 +236,128 @@
         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)
+    
+    //static int ii; 
+    ii++;
+    
+            if (ii<=2500)
             {
-                n = n + emg0.read();
+                if (ii == 0)
+                {
+                    pc.printf("Relax your muscles please. \r\n");
+                }
+                else if (ii == 2250)
+                {
+                    pc.printf("Flex your right bicep now please.\r\n");
+                }
+                //chillen
             }
-            else if (ii == 2500)
+            else if (ii>2500 && ii<5000) //
+            {
+                n = n + emg0.read();// dit wordt de variable naam na het filter
+            }
+            else if (ii == 5000)
             {
                 Flex_Bi_R = n / 2500.0f;
+                pc.printf("You can relax your right bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_R);
             }
-            else if (ii>2500 && ii<=3500)
+            else if (ii>5000 && ii<=6000)
             {
+                if (ii == 5750)
+                {
+                    pc.printf("Flex your left bicep now please. \r\n");
+                }
                 //chillen
             }
-            else if(ii>3500 && ii<6000)
+            else if(ii>6000 && ii<8500)
             {
                 m = m + emg1.read();
             }
-            else if (ii == 6000)
+            else if (ii == 8500)
             {
                 Flex_Bi_L = m / 2500.0f;
+                pc.printf("You can relax your left bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_L);
             }
-            else if (ii>6000 && ii<=7000)
+            else if (ii>8500 && ii<=9500)
             {
+                if (ii == 9250)
+                {
+                    pc.printf("Flex your right tricep now please. \r\n");
+                }
                 //chillen
             }
-            else if (ii>7000 && ii<9500)
+            else if (ii>9500 && ii<12000)
             {
                 l = l + emg2.read();
             }
-            else if (ii == 9500)
+            else if (ii == 12000)
             {
                 Flex_Tri_R = l / 2500.0f;
+                pc.printf("You can relax your right tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Tri_R);
             }
-            else if (ii>9500 && ii <=10500)
+            else if (ii>12000 && ii <=13000)
             {
+                if (ii == 12750)
+                {
+                    pc.printf("Flex your left tricep now please. \r\n");
+                }
                 //chillen
             }
-            else if (ii>10500 && ii<13000)
+            else if (ii>13000 && ii<15500)
             {
                 k = k + emg3.read();
             }
-            else if (ii == 13000)
+            else if (ii == 15500)
             {
                 Flex_Tri_L = k / 2500.0f;
-            }          
+                pc.printf("You can relax your left tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\nThe calibration has been completed, the system is now operatable. \r\n",Flex_Tri_L);
+            }
+            
+Threshold_Bi_R  = 0.75f * Flex_Bi_R;
+Threshold_Bi_L  = 0.75f * Flex_Bi_L;
+Threshold_Tri_R = 0.75f * Flex_Tri_R;
+Threshold_Tri_L = 0.75f * Flex_Tri_L;                  
+
+            if (ii == 16500)
+            {
+                pc.printf("\r\nThreshold value right bicep  = %f\r\nThreshold value left bicep   = %f\r\nThreshold value right tricep = %f\r\nThreshold value left tricep  = %f\r\n\r\n",Threshold_Bi_R,Threshold_Bi_L,Threshold_Tri_R,Threshold_Tri_L);
+            }
+            else if (ii == 20000)
+            {
+                pc.printf("\r\nAutomatic switch to Homing State\r\n");
+                Active_State = Homing;
+            }
+}
+
+void Start_Up()
+{
+    i++;
+    iii++;
+    if (iii == 1)
+    {
+        pc.printf("System is starting...\r\nWaiting for further input...\r\n");
+    }
+    
+    else if (iii == 30000)
+    {
+        pc.printf("1 minute without input..\r\nReseting start-up...\r\n");
+        iii = 0;
+    }    
+    else if (iii == 40001)                                                      // sleeping state is only added for designing purposes and will most likely never be used
+    {                                                                           // when working with patients. Furthermore it cannot be reached automaticly
+        pc.printf("Sleeping... Press button 4 to wake me up!\r\n\r\n");
+        iii++;
+    }
+    else if (iii == 45000)
+    {
+        iii = 40000;
     }
 }
 
@@ -239,12 +366,58 @@
     PwmPin1 = PwmPin2 = 0;
 }
 
+void Going_Home()
+{
+    //Here we will reset the position of the arm back to the home state
+    
+    if (counts1 == 0)                                                           // this 0 is a filler value and can later be determined to the angle of the 
+    {                                                                           // 0-state of the arm
+        PwmPin1=0.0f;
+    }
+    else if (counts1 > 0)
+    {
+        DirectionPin1 = true;                                                   //uitzoeken of dit klopt, is afhankelijk welke richting opgedraaid moet worden..
+        PwmPin1 = 0.6f;
+    }
+    else if (counts1 < 0)
+    {
+        DirectionPin1 = false;
+        PwmPin1 = 0.6f;
+    }
+    
+    if (counts2 == 0)                                                           //Homing for M1 naar begin staat
+    {
+        PwmPin2=0.0f;
+    }
+    else if (counts2 > 0)
+    {
+        DirectionPin2 = true;                                                   //uitzoeken of dit klopt, is afhankelijk welke richting opgedraaid moet worden..
+        PwmPin2 = 0.6f;
+    }
+    else if (counts2 < 0)
+    {
+        DirectionPin2 = false;
+        PwmPin2 = 0.6f;
+    } 
+    
+    if (counts1 == 0 && counts2 == 0)
+    {
+        pc.printf("Homing is completed\r\nAutomatic switch to the Functioning State\r\n");
+        Active_State = Function;
+    }
+}
+
 void Printing()
 {
     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);
+    
+        if (Active_State == Function)
+        {
+            pc.printf("q1    = %f [rad] \r\nq2    = %f [rad] \r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2, v1, v2);
+        }
+        
+    pc.printf("%f %f",vx, vy);
 }
 
 void StateMachine() 
@@ -253,21 +426,20 @@
     {
         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");
-                }
-            }                           
+        Start_Up();
+        BlinkLed();
+        
+        if (!Knop4 == true)
+        {
+            Active_State = Calibration;
+            pc.printf("Entering Calibration State \r\n");
+        }
+        else if (!Knop3 == true)
+        {
+            Active_State = Homing;
+            pc.printf("Entering Homing State \r\n");
+        }
+                                   
         break;
         
         case Calibration:
@@ -276,11 +448,20 @@
                 
                 Calibrating();
                 OFF();
-                if (Knop1==false)
+                
+                if (!Knop1 && !Knop2)
                 {
-                    pc.printf("Entering Homing state \n");
+                    pc.printf("Switched to Sleeping State\r\n");
+                    Active_State = Starting;
+                    iii = 40001;
+                }
+                else if (Knop1==false)
+                {
+                    pc.printf("Manual switch to Homing state \r\n");
                     Active_State = Homing;
                 }
+
+                    
                 
                 sample();
                 EMG_Read();
@@ -290,11 +471,25 @@
         case Homing:
                 //Homing actions
                 //pc.printf("Homing State");
-                if (Knop2==false)
+                Going_Home();
+                
+                if (!Knop1 && !Knop2)
                 {
-                    pc.printf("Entering Funtioning State \n");
+                    pc.printf("Switched to Sleeping State\r\n");
+                    Active_State = Starting;
+                    iii = 40000;
+                }
+                else if (Knop2==false)
+                {
+                    pc.printf("Manual switch to Funtioning State \r\n");
                     Active_State = Function;
                 }
+                else if (Knop3==false)
+                {
+                    Active_State = Calibration;
+                    pc.printf("Re-entering Calibration State \r\n");
+                }
+                
                 
                 sample();
                 EMG_Read();
@@ -304,17 +499,29 @@
         case Function:
             //pc.printf("Funtioning State");
                 
-                if (Knop3==false)
+                if (Knop4==false)
                 { 
-                    pc.printf("Re-entering Calibration State \n");
+                    pc.printf("Re-entering Calibration State \r\n");
                     Active_State = Calibration;
+                    ii=0;
+                }
+                else if (Knop3==false)
+                {
+                    pc.printf("Re-entering Homing State \r\n");
+                    Active_State = Homing;
+                }
+                else if (!Knop1 && !Knop2)
+                {
+                    pc.printf("Switched to Sleeping State\r\n");
+                    Active_State = Starting;
+                    iii = 40000;
                 }
                
                 sample();
                 EMG_Read();
                 Encoding();
-                velocity1();
-                velocity2();
+                //velocity1();
+                //velocity2();
                 motor1();
                 motor2();                
         break;