Diff: PROJECT_main.cpp

Revision:

12:b09b7fe5550c

Parent:

11:b517e73a98ab

Child:

13:05697c9b13d7

Child:

14:c2389571f8d6

--- a/PROJECT_main.cpp	Mon Nov 03 22:16:30 2014 +0000
+++ b/PROJECT_main.cpp	Mon Nov 03 22:47:59 2014 +0000
@@ -535,13 +535,7 @@
     while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
-        setpoint1=0;
-        setpoint2=0;
-        integral1 = integral = 0;
-        previouserror1 = previouserror = 0;
-
-
-       
+      
 // FORMAT_CODE_START
 
             scope.set(0, motor2.getPosition()*omrekenfactor2);
@@ -587,7 +581,7 @@
                 case 3: {
                     switch (force) {
                         case 1:
-                            setpoint1 += 2*TSAMP; //6.8*TSAMP;
+                            setpoint1 += 2.5*TSAMP; //6.8*TSAMP;
                             break;
                         case 2:
                             setpoint1 += 0.4*TSAMP; //7.4*TSAMP;
@@ -597,6 +591,7 @@
                             break;
                     }
                     if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
+                        setpoint1 = 2.36;
                         state = 4;
                     }
                     break;
@@ -630,8 +625,8 @@
             //motor regeling
 
             //regelaar motor1, bepaalt positie
-            controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1;
-            integral1 = integral1 + controlerror1*TSAMP;
+            controlerror1 = setpoint1 - (motor1.getPosition()*omrekenfactor1);
+            integral1 = integral1 + (controlerror1*TSAMP);
             derivative1 = (controlerror1 - previouserror1)/TSAMP;
             pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1;
             previouserror1 = controlerror1;
@@ -646,8 +641,8 @@
 
 
             //regelaar motor2, bepaalt positie
-            controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
-            integral = integral + controlerror*TSAMP;
+            controlerror = setpoint2 - (motor2.getPosition()*omrekenfactor2);
+            integral = integral + (controlerror*TSAMP);
             derivative = (controlerror - previouserror)/TSAMP;
             pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
             previouserror = controlerror;