Diff: main.cpp

Revision:

24:711c7c388e57

Parent:

23:c97e206cf2a7

Child:

25:230bd4e1f3ef

--- a/main.cpp	Wed Oct 14 14:55:21 2015 +0000
+++ b/main.cpp	Wed Oct 14 15:27:45 2015 +0000
@@ -22,7 +22,7 @@
 double          emg_value;
 const double    twopi = 6.2831853071795;
 int             Pulses;
-const double    margin = 0.3;
+const double    margin = 0.4;
 double          Rotatie = 0; //aantal graden dat de motor is gedraaid
 double          Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
 double          Error = 0;
@@ -70,6 +70,7 @@
     Pulses = Encoder.getPulses();
     Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
     scope.set(0,Goal);
+    scope.set(1,Rotatie);
     scope.set(2,emg_value);
     scope.send();
 }
@@ -99,16 +100,18 @@
         while (Excecute ) { //Dit wordt gebruikt voor motor 1
             //pc.printf("Rotatie = %f \n", Rotatie);
             //pc.printf("Out of Range = %i \n", OutRange);
-            if (Rotatie >= upperlimit-margin) {
+            if (Rotatie >= upperlimit) {
                 Goal = upperlimit - margin;
                 OutRange = true;
             }
-            if (Rotatie <= downlimit+margin) {
+            if (Rotatie <= downlimit) {
                 Goal = 0 + margin;
                 OutRange = true;
             }
-            if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan
+            if (Rotatie >= margin && Rotatie <= upperlimit - margin) {
                 OutRange = false;
+            }
+            if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) {
                 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
                 if (emg_value >= T2 ) {
                     Direction = 1;
@@ -117,13 +120,14 @@
                 if (emg_value >= T1 && emg_value <= T2) {
                     Direction = 0;
                     v = 1;
-                } 
-                if (emg_value <= T1){
+                }
+                if (emg_value <= T1) {
                     Direction = 0;
                     v = 0;
                 }
             }
         }
+
         while (false ) { //Dit wordt gebruikt voor motor 2 MOET NOG OMGEZET WORDEN NAAR MOTOR 2!!!!
             if (Rotatie >= upperlimit-margin) {
                 Goal = upperlimit - margin;