Diff: main.cpp

Revision:

3:e40763e01a80

Parent:

2:302174acdac7

Child:

4:91b583d4d8c4

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