Diff: main.cpp

Revision:

7:59116528d895

Parent:

6:2887ad4c5d97

Child:

8:5dab7ea40bc1

--- a/main.cpp	Tue Oct 28 11:55:34 2014 +0000
+++ b/main.cpp	Tue Oct 28 13:49:24 2014 +0000
@@ -23,9 +23,9 @@
 
 
     float integral1 = 0;
-    float setpoint1 = 3;
-    float dt1 = 0.01;
-    float Kp1 = 2;
+    float setpoint1 = 8;
+    float dt1 = 0.001;
+    float Kp1 = 1.5;
     float Ki1 = 0.5;
     float error1 = 0;
     float output1 = 0;
@@ -33,49 +33,68 @@
 
     float integral2 = 0;
     float setpoint2 = 3.14;
-    float dt2 = 0.01;
+    float dt2 = 0.001;
     float Kp2 = 0.55;
     float Ki2 = 0.45;
     float error2 = 0;
     float output2 = 0;
     float omrekenfactor2 = (24*172)/6.28;
 
+    float setpointrem1 = 0;
+    float integralrem1 = 0;
+    float Kprem1 = 1.5;
+    float Kirem1 = 0.5;
+    float errorrem1 = 0;
+    float outputrem1 = 0;
+
+    
     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);
+        error2 = setpoint2 - motor2.getPosition()/omrekenfactor2;
+        integral2 = integral2+ error2*dt2;
+        output2 = Kp2*error2 + Ki2*integral2;
+        keep_in_range(&output2,-1,1);
+        pwm_motor2.write(abs(output2));
+        if(output2 > 0) {
+            motor2dir = 1;
+        } else {
+            motor2dir = 0;
         }
+        wait(dt2);
 
-        else {
-            output1=0;
-        }
-
-
+        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));
         if(output1 > 0) {
             motor1dir = 1;
         } else {
             motor1dir = 0;
         }
+        wait(dt1);
 
-        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(motor1.getPosition()/omrekenfactor1 < 1.3) {
+            setpoint1=8;
+        }
+
+        else {
+            setpoint1=0;
 
+            /*errorrem1 = setpointrem1 - motor1.getSpeed()/omrekenfactor1;
+            //motorpositie omgerekend naar rad/s
+            integralrem1 = integralrem1 + errorrem1*dt1;
+            outputrem1 = Kprem1*errorrem1 + Kirem1*integralrem1;
+            keep_in_range(&outputrem1,-1,1);
+            pwm_motor1.write(abs(outputrem1));
+            if(outputrem1 > 0) {
+                motor1dir = 1;
+            } else {
+                motor1dir = 0;
+            }
+            wait(dt1);*/
+        }
 
-        if(output2 > 0) {
-            motor2dir = 1;
-        } else {
-            motor2dir = 0;
-        }
         scope.set(0, output1);
         scope.set(1, motor1.getSpeed()/omrekenfactor1);
         scope.set(2, motor1.getPosition()/(omrekenfactor1));