Diff: main.cpp

Revision:

15:7fbee317af2d

Parent:

14:baebaef79aa6

Child:

16:5b729bd56155

--- a/main.cpp	Tue Sep 29 16:58:52 2015 +0000
+++ b/main.cpp	Tue Sep 29 17:15:46 2015 +0000
@@ -14,18 +14,15 @@
 Ticker ScopeTime;
 Ticker myControllerTicker;
 
-double Aantal_Degs;
-double Aantal_pulses;
 double reference;
 double position;
-double m2_ref;
+double m2_ref = 0;
 
 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
 {
     scope.set(0, motor2direction.read());
     scope.set(1, motor2speed.read());
-    scope.set(2, Aantal_Degs);
-    Aantal_Degs = Encoder.getPulses()*360/(0.5*128*131);
+    scope.set(2, position);
 
     scope.send();
     
@@ -70,12 +67,12 @@
 // Motor control
 void motor2_Controller() 
 {
-    reference = m2_ref;
+    reference = m2_ref; // Setpoint
     position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
     double P2 = PID( reference - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
     m2_f_b0, m2_f_b1, m2_f_b2);
-    motor2speed = abs(P2);
-    if(P2 > 0)
+    motor2speed = abs(P2); // Speed control
+    if(P2 > 0) // Direction control
     {  
         motor2direction = 0;
     }
@@ -83,7 +80,7 @@
     {
         motor2direction = 1;
     }
-
+    pc.printf("position = %f aantal degs = %f \n",reference,position);
 }
 
 int main()
@@ -104,7 +101,6 @@
         {
             m2_ref = m2_ref - 5;
         }
-       pc.printf("position = %f aantal degs = %f \n",reference,position);
     }
 
 }
\ No newline at end of file