Diff: main.cpp

Revision:

5:312186a0604d

Parent:

4:8f67b8327300

Child:

6:056ad27636ff

--- a/main.cpp	Tue Oct 23 13:08:22 2018 +0000
+++ b/main.cpp	Wed Oct 24 10:30:47 2018 +0000
@@ -17,11 +17,14 @@
 AnalogIn emg2( A2 );
 AnalogIn emg3( A3 );
 
+QEI Encoder1(D12,D13,NC,64);
+QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
+
+//DigitalOut LED(LED_RED);
+
 Ticker      StateTicker;
-Ticker      EncodingTicker;
 Ticker      printTicker;
-Ticker      EMG_Read_Ticker;
-Ticker      sample_timer;
+
 HIDScope    scope( 4 );
 
 volatile float Bicep_Right          = 0.0;
@@ -37,17 +40,17 @@
     
 volatile states Active_State = Calibration;
 
-volatile int counts1;
-volatile int counts2;
+volatile int counts1 ;
+volatile int counts2 ;
+volatile float rad_m1;
+volatile float rad_m2;
     
 void Encoding()
 {
-
-    QEI Encoder1(D12,D13,NC,64);
-    QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
+ 
     counts1 = Encoder1.getPulses();
     counts2 = Encoder2.getPulses();
-    
+    // Hier gaat iets fout waardoor het 0 wordt!!!
     float rad_m1 = ((2*pi)/32.0)* (float)counts1;
     float rad_m2 = ((2*pi)/32.0)* (float)counts2;
     
@@ -82,7 +85,7 @@
                 referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; 
                 }
             
-            else if (pot1.read() == 0.5f || !Knop1 == (pot1.read()<0.5f))
+            else if (pot1.read() == 0.5f)
             {
                 referenceVelocity1 = pot1.read() * 0.0f; 
             } 
@@ -136,7 +139,7 @@
     //eventueel nog counts -> rad/s 
     
     //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
-    pc.printf("%f ", counts1);
+    pc.printf("%i    %i \n",counts1,counts2);
 }
 
 void StateMachine() 
@@ -144,32 +147,57 @@
     switch (Active_State)
     {
         case Calibration:
-        //calibration actions
-        //pc.printf("Calibration State");
-            if (Knop1==false)
-            {
-                pc.printf("Entering Homing state \n");
-                Active_State = Homing;
-            }
+                //calibration actions
+                //pc.printf("Calibration State");
+                if (Knop1==false)
+                {
+                    pc.printf("Entering Homing state \n");
+                    Active_State = Homing;
+                }
+                
+                sample();
+                EMG_Read();
+                Encoding();    
+                
         break;
         
         case Homing:
-        // Homing actions
-        //pc.printf("Homing State");
-            if (Knop2==false)
-            {
-                pc.printf("Entering Funtioning State \n");
-                Active_State = Function;
-            }
+                //Homing actions
+                //pc.printf("Homing State");
+                if (Knop2==false)
+                {
+                    pc.printf("Entering Funtioning State \n");
+                    Active_State = Function;
+                }
+                
+                sample();
+                EMG_Read();
+                Encoding();
         break;
         
         case Function:
-        //pc.printf("Funtioning State");
-        
-            velocity1();
-            velocity2();
-            motor1();
-            motor2();
+            //pc.printf("Funtioning State");
+                
+                if (Knop2==false)
+                {
+                    pc.printf("Re-entering Homing State \n");
+                    Active_State = Homing;
+                }
+                else if (Knop1==false)
+                {
+                    pc.printf("Re-entering Calibration State \n");
+                    Active_State = Calibration;
+                }
+                
+                
+                sample();
+                EMG_Read();
+                Encoding();
+                velocity1();
+                velocity2();
+                motor1();
+                motor2();
+                
         break;    
             
         default:
@@ -182,16 +210,11 @@
     pc.baud(115200);    
     PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz 
     
-    EncodingTicker.attach(&Encoding, 0.002);
-    
-    sample_timer.attach(&sample, 0.002);
-    EMG_Read_Ticker.attach(&EMG_Read, 0.002);
-    
     StateTicker.attach(StateMachine, 0.002);
    
-    //printTicker.attach(&Printing, 2.0);
+    printTicker.attach(&Printing, 2.0);
     
     while(true)
-    {        
+    {
     }
 }
\ No newline at end of file