Diff: main.cpp

Revision:

6:2887ad4c5d97

Parent:

5:7fb05dfead4d

Child:

7:59116528d895

--- a/main.cpp	Thu Oct 23 14:18:58 2014 +0000
+++ b/main.cpp	Tue Oct 28 11:55:34 2014 +0000
@@ -3,7 +3,7 @@
 #include "HIDScope.h"
 
 
-HIDScope scope(4);
+HIDScope scope(6);
 
 void keep_in_range(float * in, float min, float max);
 
@@ -25,21 +25,35 @@
     float integral1 = 0;
     float setpoint1 = 3;
     float dt1 = 0.01;
-    float Kp1 = 1;
-    float Ki1 = 0.1;
+    float Kp1 = 2;
+    float Ki1 = 0.5;
     float error1 = 0;
     float output1 = 0;
-    float omrekenfactor = 4480.0/6.28;
+    float omrekenfactor1 = (32*70)/6.28;
+
+    float integral2 = 0;
+    float setpoint2 = 3.14;
+    float dt2 = 0.01;
+    float Kp2 = 0.55;
+    float Ki2 = 0.45;
+    float error2 = 0;
+    float output2 = 0;
+    float omrekenfactor2 = (24*172)/6.28;
 
     while(1) {
+        if(motor1.getPosition()/omrekenfactor1 < 1.3) {
+            error1 = setpoint1 - motor1.getSpeed()/omrekenfactor1;
+            //motorpositie omgerekend naar rad/s
+            integral1 = integral1 + error1*dt1;
+            output1 = Kp1*error1 + Ki1*integral1;
+            keep_in_range(&output1,-1,1);
+            pwm_motor1.write(abs(output1));
+            wait(dt1);
+        }
 
-        error1 = setpoint1 - motor1.getSpeed()/omrekenfactor;     // (omrekenfactor)/dt1;
-        //motorpositie omgerekend naar rad/s
-        integral1 = integral1 + error1*dt1;
-        output1 = Kp1*error1 + Ki1*integral1;
-        keep_in_range(&output1,-1,1);
-        pwm_motor1.write(abs(output1));
-        wait(dt1);
+        else {
+            output1=0;
+        }
 
 
         if(output1 > 0) {
@@ -47,12 +61,28 @@
         } else {
             motor1dir = 0;
         }
-        scope.set(0, error1);
-        scope.set(1, output1);
-        scope.set(2, motor1.getSpeed()/omrekenfactor);
-        scope.set(3, motor1.getPosition()/(omrekenfactor));
+
+        error2 = setpoint2 - motor2.getPosition()/omrekenfactor2;     // (omrekenfactor)/dt1;
+        //motorpositie omgerekend naar rad/s
+        integral2 = integral2+ error2*dt2;
+        output2 = Kp2*error2 + Ki2*integral2;
+        keep_in_range(&output2,-1,1);
+        pwm_motor2.write(abs(output2));
+        wait(dt2);
+
+
+        if(output2 > 0) {
+            motor2dir = 1;
+        } else {
+            motor2dir = 0;
+        }
+        scope.set(0, output1);
+        scope.set(1, motor1.getSpeed()/omrekenfactor1);
+        scope.set(2, motor1.getPosition()/(omrekenfactor1));
+        scope.set(3, output2);
+        scope.set(4, motor2.getSpeed()/omrekenfactor2);
+        scope.set(5, motor2.getPosition()/(omrekenfactor2));
         scope.send();
-
     }
 }
 void keep_in_range(float * in, float min, float max)