Diff: main.cpp

Revision:

2:bc01b1ce8fb6

Parent:

1:99754fe781b0

--- a/main.cpp	Mon Oct 23 10:34:30 2017 +0000
+++ b/main.cpp	Mon Oct 23 11:18:57 2017 +0000
@@ -43,7 +43,7 @@
 const float Ts = 0.1;                   // tickettijd/ sample time
 float e_prev = 0; 
 float e_int = 0;
-float refP = 0;
+float refP_prev = 0;
 
 //FILTERS
 /*void emgverwerk ()
@@ -66,22 +66,23 @@
 
 
 // PID CONTROLLER
-float GetReferencePosition(double emgFilt, float refP )  //    anders met emg
+float GetReferencePosition(double emgFilt, double &refP_prev )  //    anders met emg
 {
+    double refP; 
     if (.45<emgFilt<.80)
     {
-        refP += 1;
+        refP= refP_prev + 0.001;
         }
     else if (emgFilt>=.80)
     {
-        refP += 4;
+        refP= refP_prev + 0.04;
     }
     else 
     {  }
     
     int maxwaarde = 4096;                   // = 64x64 aantal posities die de motor kan hebben
-    float refPos = refP*maxwaarde;
-    return refPos;                            // value between 0 and 4096 
+    refP = refP*maxwaarde;
+    return refP;                            // value between 0 and 4096 
 }
     
 float FeedBackControl(float error, float &e_prev, float &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
@@ -148,7 +149,7 @@
     scope.send();    
     
     // hier the control of the control system
-    float refPos = GetReferencePosition(emgFiltered, refP); 
+    float refP = GetReferencePosition(emgFiltered, refP_prev); 
     float Huidigepositie = Encoder(); 
     float error = (refP - Huidigepositie);// make an error
     float motorValue = FeedBackControl(error, e_prev, e_int);