Diff: main.cpp

Revision:

33:4e3870ab4e17

Parent:

32:c2c80a2ca83d

Child:

34:67f211c08e74

--- a/main.cpp	Wed Oct 21 13:53:23 2015 +0000
+++ b/main.cpp	Tue Oct 27 15:33:48 2015 +0000
@@ -61,8 +61,8 @@
 const double    Ki = 0.2;
 double          v = 0;
 double          upperlimit; //max aantal rotaties omhoog
-const double    downlimit = 0.4;
-const double    margin = 0.4;
+const double    downlimit = 0.8;
+const double    margin = 0.8;
 bool            OutRange = false;
 
 // Motor 2 (Rotatie)
@@ -87,7 +87,7 @@
 bool            Home = false;
 
 // Filter
-double Fs2 = 1000; // in Hz
+double Fs2 = 500; // in Hz
 const double TijdCal = 5;
 double Max = 0;
 double Max2 = 0;
@@ -224,6 +224,11 @@
             Direction=0;
         }
         v=Kp*Error + Kd*Errord + Ki*Errori;
+         if (Home == true) {
+            if (v >0.15) {
+                v = 0.15;
+            }
+        }
     }
     PowerMotor.write(fabs(v));
 
@@ -252,12 +257,10 @@
     Rotatie = (Pulses*twopi)/4200;
     Pulses2 = Encoder2.getPulses();
     Rotatie2 = (Pulses2*twopi)/4200;
-    scope.set(0,Goal);
-    scope.set(1,Rotatie);
-    scope.set(2,y8);
-    scope.set(3,Goal2);
-    scope.set(4,Rotatie2);
-    scope.set(5,y18);
+    scope.set(0,Rotatie);
+    scope.set(1,y8);
+    scope.set(2,Rotatie2);
+    scope.set(3,y18);
     scope.send();
 }
 
@@ -278,11 +281,11 @@
         led = 1;
         TijdEMGCal.stop();
         TijdEMGCal.reset();
-        T1 = 0.1*Max;
-        T2 = 0.25*Max;
-        T3 = 0.1*Max2;
-        T4 = 0.25*Max2;
-        pc.printf("Max = %f\nT1 = %f\nT2 = %f", Max, T1, T2);
+        T1 = 0.2*Max;
+        T2 = 0.4*Max;
+        T3 = 0.2*Max2;
+        T4 = 0.4*Max2;
+        pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4);
         wait (3);
         Excecute = true;
 
@@ -304,6 +307,7 @@
     while (true) {
         Calibration();
         while (Excecute) {
+            pc.printf("Rotatie = %f \nRotatie2 = %f \n", Rotatie, Rotatie2);
             // Eerst wordt motor 1 aangestuurd
             if (Rotatie >= upperlimit) { //Als hij buiten bereik is
                 Goal = upperlimit - margin;
@@ -319,11 +323,11 @@
             if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
                 if (y8 >= T2 ) {
                     Direction = 1;
-                    v = 0.1;
+                    v = 0.15;
                 }
                 if (y8 > T1 && y8 < T2) {
                     Direction = 0;
-                    v = 0.1;
+                    v = 0.15;
                 }
                 if (y8 <= T1) {
                     Direction = 0;
@@ -346,11 +350,11 @@
             if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
                 if (y18 >= T4 ) {
                     Direction2 = 1;
-                    v2 = 0.1;
+                    v2 = 0.05;
                 }
                 if (y18 > T3 && y18 < T4) {
                     Direction2 = 0;
-                    v2 = 0.1;
+                    v2 = 0.05;
                 }
                 if (y18 <= T3) {
                     Direction2 = 0;
@@ -379,7 +383,7 @@
         while (Home) { //Terugkeren naar vaste positie
             pc.printf("Home\n");
             OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
-            Goal = 0;
+            Goal = margin;
             Goal2 = 0;
             if (fabs(Error)<=0.015 && fabs(Error2)<=0.015) {
                 timer.start();