Diff: main.cpp

Revision:

4:8f67b8327300

Parent:

3:c8f0fc045505

Child:

5:312186a0604d

--- a/main.cpp	Mon Oct 22 14:51:46 2018 +0000
+++ b/main.cpp	Tue Oct 23 13:08:22 2018 +0000
@@ -9,6 +9,7 @@
 PwmOut PwmPin1(D5);
 PwmOut PwmPin2(D6);
 DigitalIn Knop1(D2);
+DigitalIn Knop2(D3);
 AnalogIn pot1 (A5);
 AnalogIn pot2 (A4);
 AnalogIn emg0( A0 );
@@ -16,18 +17,9 @@
 AnalogIn emg2( A2 );
 AnalogIn emg3( A3 );
 
-DigitalIn a1( D10);
-DigitalIn b1( D11);
-DigitalIn a2( D12);
-DigitalIn b2( D13);
-
-
+Ticker      StateTicker;
 Ticker      EncodingTicker;
 Ticker      printTicker;
-Ticker      mycontrollerTicker1;
-Ticker      mycontrollerTicker2;
-Ticker      Velo1;
-Ticker      Velo2;
 Ticker      EMG_Read_Ticker;
 Ticker      sample_timer;
 HIDScope    scope( 4 );
@@ -37,20 +29,29 @@
 volatile float Tricep_Right         = 0.0;
 volatile float Tricep_Left          = 0.0;
 volatile const float maxVelocity    = 8.4; // in rad/s
+volatile const double pi            = 3.14159265358979; 
 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 states Active_State = Calibration;
+
 volatile int counts1;
 volatile int counts2;
-
+    
 void Encoding()
 {
-    
-    QEI Encoder1(D12,D13,NC,32);
+
+    QEI Encoder1(D12,D13,NC,64);
     QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
     counts1 = Encoder1.getPulses();
     counts2 = Encoder2.getPulses();
     
+    float rad_m1 = ((2*pi)/32.0)* (float)counts1;
+    float rad_m2 = ((2*pi)/32.0)* (float)counts2;
+    
+   // pc.printf("%f  &  %f ....\n",rad_m1, rad_m2);
 }
 
 void EMG_Read()
@@ -136,26 +137,59 @@
     
     //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
     pc.printf("%f ", counts1);
-    
-    
 }
 
+void StateMachine() 
+{
+    switch (Active_State)
+    {
+        case Calibration:
+        //calibration actions
+        //pc.printf("Calibration State");
+            if (Knop1==false)
+            {
+                pc.printf("Entering Homing state \n");
+                Active_State = Homing;
+            }
+        break;
+        
+        case Homing:
+        // Homing actions
+        //pc.printf("Homing State");
+            if (Knop2==false)
+            {
+                pc.printf("Entering Funtioning State \n");
+                Active_State = Function;
+            }
+        break;
+        
+        case Function:
+        //pc.printf("Funtioning State");
+        
+            velocity1();
+            velocity2();
+            motor1();
+            motor2();
+        break;    
+            
+        default:
+        pc.printf("UNKNOWN COMMAND");
+    }
+}    
+
 int main()
 {
     pc.baud(115200);    
-    PwmPin1.period_us(40); //60 microseconds pwm period, 16.7 kHz 
+    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);
     
-    mycontrollerTicker1.attach(motor1, 0.002);//500Hz
-    Velo1.attach(velocity1, 0.002);
-    mycontrollerTicker2.attach(motor2, 0.002);
-    Velo2.attach(velocity2, 0.002);
-    
-    printTicker.attach(&Printing, 2.0);
+    StateTicker.attach(StateMachine, 0.002);
+   
+    //printTicker.attach(&Printing, 2.0);
     
     while(true)
     {