Diff: main.cpp

Revision:

7:439940ae1197

Parent:

6:056ad27636ff

Child:

8:e8734a254818

--- a/main.cpp	Fri Oct 26 06:56:18 2018 +0000
+++ b/main.cpp	Fri Oct 26 07:58:07 2018 +0000
@@ -32,7 +32,8 @@
 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 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;
 
@@ -50,11 +51,9 @@
  
     counts1 = Encoder1.getPulses();
     counts2 = Encoder2.getPulses();
-    // Hier gaat iets fout waardoor het 0 wordt!!!
-    float rad_m1 = ((2.0*pi)/32.0)* (float)counts1;
-    float rad_m2 = ((2.0*pi)/32.0)* (float)counts2;
     
-   // pc.printf("%f  &  %f ....\n",rad_m1, rad_m2);
+    rad_m1 = rad_count * (float)counts1;
+    rad_m2 = rad_count * (float)counts2;
 }
 
 void EMG_Read()
@@ -139,7 +138,7 @@
     //eventueel nog counts -> rad/s 
     
     //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
-    pc.printf("%i    %i \n",counts1,counts2);
+    pc.printf("%f    %f \n",rad_m1,rad_m2);
 }
 
 void StateMachine() 
@@ -177,7 +176,7 @@
         
         case Function:
             //pc.printf("Funtioning State");
-                
+                /*
                 if (Knop2==false)
                 {
                     pc.printf("Re-entering Homing State \n");
@@ -188,7 +187,7 @@
                     pc.printf("Re-entering Calibration State \n");
                     Active_State = Calibration;
                 }
-                
+                */
                 
                 sample();
                 EMG_Read();