Diff: begintotaalscript.cpp

Revision:

5:4adfd729a291

Parent:

4:c79922420b70

Child:

6:d4f355d72f66

--- a/begintotaalscript.cpp	Tue Nov 05 09:30:32 2013 +0000
+++ b/begintotaalscript.cpp	Tue Nov 05 10:01:57 2013 +0000
@@ -107,11 +107,11 @@
     ybegin=0; //beginpositie
     qbegin=5.50;  //beginhoek (in rad) vanaf de y-as, rechtsom gedraaid is positief
     MaxsnelheidV=150.0;     //in mm/s
-    MaxsnelheidD=0.5*pi;    //in rad/s eerst 0.26 rad/s (15 deg/sec)
+    MaxsnelheidD=0.5;    //in rad/s eerst 0.26 rad/s (15 deg/sec)
 //constanten regelaar
     kp_r = 0.006;
-    ki_r = 0.005; 
-    
+    ki_r = 0.005;
+
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,Ts);
 
@@ -336,25 +336,22 @@
         if (state == 6) {
             // pc.printf("state 6 tekenen  |    knop3= rust \n\r");
 
-            //  EMG
-            BereikD = MaxwaardeD-GrenswaardeD;    //Bereik van grafiek EMG (Voltage t.o.v. tijd)
-            BereikV = MaxwaardeV-GrenswaardeV;      //Bereik van grafiek EMG (Voltage t.o.v. tijd)
-                       
-            SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV);  // Snelheid van pen (in cm/sec), lineair verband met het Bereik
+            // EMG
+            BereikD = MaxwaardeD-GrenswaardeD; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
+            BereikV = MaxwaardeV-GrenswaardeV; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
+
+            SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV); // Snelheid van pen (in cm/sec), lineair verband met het Bereik
             SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik
 
-            if (SnelheidV<MaxsnelheidD) {
-                SnelheidV=MaxsnelheidD;
+            if (SnelheidV<0) {
+                SnelheidV=0;
             }
-
-            if (SnelheidD<MaxsnelheidD) {
-                SnelheidD=MaxsnelheidD;
+            if (SnelheidD<0) {
+                SnelheidD=0;
             }
-
             if (SnelheidV>MaxsnelheidV) {
                 SnelheidV=MaxsnelheidV;
             }
-
             if (SnelheidD>MaxsnelheidV) {
                 SnelheidD=MaxsnelheidV;
             }
@@ -467,7 +464,7 @@
             // terug naar state 1 knop
             if (knop3 == false ) {  // terug naar state 1
                 state=1;
-                meting=false; 
+                meting=false;
                 wait(0.05);
 
                 while(knop3 == false) {}