Diff: main.cpp

Revision:

4:91b583d4d8c4

Parent:

3:e40763e01a80

Child:

5:7fb05dfead4d

--- a/main.cpp	Thu Oct 23 12:46:45 2014 +0000
+++ b/main.cpp	Thu Oct 23 13:47:21 2014 +0000
@@ -3,7 +3,7 @@
 #include "HIDScope.h"
 
 
-HIDScope scope(3);
+HIDScope scope(4);
 
 int main()
 {
@@ -21,34 +21,34 @@
 
 
     float integral1 = 0;
-    float setpoint1 = 9.5;
+    float setpoint1 = 3;
     float dt1 = 0.01;
-    float Kp1 = 0.1;
-    float Ki1 = 0.1;
+    float Kp1 = 1;
+    float Ki1 = 0.5;
     float error1 = 0;
     float output1 = 0;
-    float omrekenfactor = 4480*6.28;
+    float omrekenfactor = 4480.0/6.28;
 
     while(1) {
 
-        error1 = setpoint1 - motor1.getPosition()/(omrekenfactor)/dt1;
+        error1 = setpoint1 - motor1.getSpeed()/omrekenfactor;     // (omrekenfactor)/dt1;
         //motorpositie omgerekend naar rad/s
         integral1 = integral1 + error1*dt1;
         output1 = Kp1*error1 + Ki1*integral1;
         pwm_motor1.write(abs(output1));
-        wait(dt1); 
+        wait(dt1);
 
 
         if(output1 > 0) {
-            motor1dir = 1;
+            motor1dir = 0;
         } else {
-            motor1dir = 0;
+            motor1dir = 1;
         }
+        scope.set(0, error1);
+        scope.set(1, output1);
+        scope.set(2, motor1.getSpeed()/omrekenfactor);
+        scope.set(3, motor1.getPosition()/(omrekenfactor));
+        scope.send();
 
-        scope.set(0, output1);
-        scope.set(1, motor1.getPosition());
-        scope.set(2, motor1.getPosition()/(omrekenfactor)/dt1);
-        scope.send();
-        
     }
 }
\ No newline at end of file