Diff: main.cpp

Revision:

27:9cca2ad74ec0

Parent:

26:5b6c577fb3c1

Child:

28:b7d01a55530f

--- a/main.cpp	Thu Oct 15 15:08:49 2015 +0000
+++ b/main.cpp	Thu Oct 15 16:03:40 2015 +0000
@@ -65,7 +65,6 @@
 
 // Motor 2 (Rotatie)
 double          n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
-
 int             Pulses2;
 double          Rotatie2 = 0;
 double          Goal2;
@@ -91,11 +90,12 @@
 
 void MotorSet()
 {
+    // Eerst motor 1 (translatie)
     if (OutRange) {
         Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
         Errord = (Error-Errorp)/Fs;
         Errorp = Error;
-        if (fabs(Error) <= 0.1) {
+        if (fabs(Error) <= 0.5) {
             Errori = Errori + Error*1/Fs;
         } else {
             Errori = 0;
@@ -109,28 +109,29 @@
     }
     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));
+    // Dan motor 2 (rotatie)
+    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
+    Rotatie = (Pulses*twopi)/4200; 
     Pulses2 = Encoder2.getPulses();
     Rotatie2 = (Pulses2*twopi)/4200;
     scope.set(0,Goal);
@@ -181,17 +182,16 @@
         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); 
+            pc.printf("Rotatie2 = %f \n", Rotatie2);
             if (Rotatie >= upperlimit) { //Als hij buiten bereik is
-                Goal = upperlimit - margin-0.005;
+                Goal = upperlimit - margin;
                 OutRange = true;
             }
             if (Rotatie <= downlimit) { //Als hij buiten bereik is
-                Goal = 0 + margin+0.05;
+                Goal = 0 + margin;
                 OutRange = true;
             }
             if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
@@ -239,9 +239,9 @@
 //                    v = 0;
 //                }
 //            }
-//            if (Button2 == 0) { //Afvuren van de RBG
-//                Fire();
-//            }
+            if (Button2 == 0) { //Afvuren van de RBG
+                Fire();
+            }
         }
 
         while (Home) { //Terugkeren naar vaste positie