Diff: main.cpp

Revision:

10:80fe931a71e4

Parent:

9:774fc3c6a39e

Child:

11:0793a78109a2

--- a/main.cpp	Tue Sep 29 13:25:09 2015 +0000
+++ b/main.cpp	Tue Sep 29 13:37:22 2015 +0000
@@ -17,19 +17,23 @@
 
 double Aantal_Degs;
 double Aantal_pulses;
+double reference;
+double position;
 
 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/128/131;
+    Aantal_Degs = Encoder.getPulses()*360/(0.5*128*131);
 
     scope.send();
     
 }
 // Controller gain
 const double motor1_Kp = 0.05;
+
+
 // Reusable P controller
 double P( double error, const double Kp ) 
 {
@@ -39,8 +43,8 @@
 // Next task, measure the error and apply the output to the plant
 void motor1_Controller() 
 {
-    double reference = potmeter2.read()*360;
-    double position = Encoder.getPulses()*360/128/131; // Aantal Degs
+    reference = potmeter2.read()*360;
+    position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
     double P2 = P( reference - position, motor1_Kp );
     motor2speed = abs(P2);
     if(P2 > 0)
@@ -63,8 +67,8 @@
     myControllerTicker.attach( &motor1_Controller, 0.01f ); // 100 Hz
     while(true)
     {
-       pc.printf("position = %f aantal degs = %f \n",potmeter2.read()*360,Aantal_Degs);
-       wait(0.1f);
+       pc.printf("position = %f aantal degs = %f \n",reference,position);
+       wait(0.2f);
     }
 
 }
\ No newline at end of file