Diff: main.cpp

Revision:

26:5b6c577fb3c1

Parent:

25:230bd4e1f3ef

Child:

27:9cca2ad74ec0

--- a/main.cpp	Thu Oct 15 14:24:49 2015 +0000
+++ b/main.cpp	Thu Oct 15 15:08:49 2015 +0000
@@ -109,30 +109,30 @@
     }
     PowerMotor.write(fabs(v));
 
-    if (OutRange2) {
-        Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
-        Errord2 = (Error2-Errorp2)/Fs;
-        Errorp2 = Error2;
-        if (fabs(Error2) <= 0.5) {
-            Errori2 = Errori2 + Error2*1/Fs;
-        } else {
-            Errori2 = 0;
-        }
-        if (Error2>=0) {
-            Direction=1;
-        } else {
-            Direction=0;
-        }
-        v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
-    }
-    PowerMotor2.write(fabs(v2));
+    //if (OutRange2) {
+//        Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
+//        Errord2 = (Error2-Errorp2)/Fs;
+//        Errorp2 = Error2;
+//        if (fabs(Error2) <= 0.5) {
+//            Errori2 = Errori2 + Error2*1/Fs;
+//        } else {
+//            Errori2 = 0;
+//        }
+//        if (Error2>=0) {
+//            Direction=1;
+//        } else {
+//            Direction=0;
+//        }
+//        v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
+//    }
+//    PowerMotor2.write(fabs(v2));
 }
 void Send()
 {
     Pulses = Encoder.getPulses();
     Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
     Pulses2 = Encoder2.getPulses();
-    Rotatie2 = (Pulses*twopi)/4200;
+    Rotatie2 = (Pulses2*twopi)/4200;
     scope.set(0,Goal);
     scope.set(1,Rotatie);
     scope.set(2,emg_value);
@@ -181,15 +181,17 @@
         if (Button == 0) {
             Excecute =! Excecute;
         }
-
+        pc.printf("dafuq");
         while (Excecute ) {
             // Eerst wordt motor 1 aangestuurd
+            pc.printf("PotMeter = %f \n", PotMeter.read());
+            pc.printf("Rotatie = %f \n", Rotatie); 
             if (Rotatie >= upperlimit) { //Als hij buiten bereik is
-                Goal = upperlimit - margin;
+                Goal = upperlimit - margin-0.005;
                 OutRange = true;
             }
             if (Rotatie <= downlimit) { //Als hij buiten bereik is
-                Goal = 0 + margin;
+                Goal = 0 + margin+0.05;
                 OutRange = true;
             }
             if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
@@ -210,36 +212,36 @@
                 }
             }
 
-            // Vanaf hier wordt motor 2 aangestuurd
-            if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
-                Goal2 = turnlimit;
-                OutRange2 = true;
-            }
-            if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
-                Goal2 = -turnlimit;
-                OutRange2 = true;
-            }
-            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
-                OutRange2 = false;
-            }
-            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
-
-                if (emg_value2 >= T2 ) {
-                    Direction = 1;
-                    v = 1;
-                }
-                if (emg_value2 >= T1 && emg_value2 <= T2) {
-                    Direction = 0;
-                    v = 1;
-                }
-                if (emg_value2 <= T1) {
-                    Direction = 0;
-                    v = 0;
-                }
-            }
-            if (Button2 == 0) { //Afvuren van de RBG
-                Fire();
-            }
+            //// Vanaf hier wordt motor 2 aangestuurd
+//            if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
+//                Goal2 = turnlimit;
+//                OutRange2 = true;
+//            }
+//            if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
+//                Goal2 = -turnlimit;
+//                OutRange2 = true;
+//            }
+//            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
+//                OutRange2 = false;
+//            }
+//            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
+//
+//                if (emg_value2 >= T2 ) {
+//                    Direction = 1;
+//                    v = 1;
+//                }
+//                if (emg_value2 >= T1 && emg_value2 <= T2) {
+//                    Direction = 0;
+//                    v = 1;
+//                }
+//                if (emg_value2 <= T1) {
+//                    Direction = 0;
+//                    v = 0;
+//                }
+//            }
+//            if (Button2 == 0) { //Afvuren van de RBG
+//                Fire();
+//            }
         }
 
         while (Home) { //Terugkeren naar vaste positie
@@ -260,8 +262,6 @@
                 Errori2 = 0;
                 Errord2 = 0;
                 Errorp2 = 0;
-                wait (10);
-                Excecute = true;
             }
         }
     }