Diff: main.cpp

Revision:

9:58f9e4f8229c

Parent:

8:5dab7ea40bc1

Child:

10:48d309d9bb1c

--- a/main.cpp	Wed Oct 29 15:07:47 2014 +0000
+++ b/main.cpp	Thu Oct 30 09:07:45 2014 +0000
@@ -91,18 +91,18 @@
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
-        //regelaar motor1, bepaalt snelheid
-        if (balhit == 0) {
+        if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
             error1 = setpoint1 - motor1.getSpeed()*omrekenfactor1;
             integral1 = integral1 + error1*TSAMP;
             pwm1 = Kp1*error1 + Ki1*integral1;
-            keep_in_range(&pwm1, -1,1);
-            pwm_motor1.write(abs(pwm1));
-        } else {
-            pwm_motor1.write(0);
-            goto motorcontroldone;
+        } else {            //regelaar motor1, bepaalt positie
+            batjeset = integral1 = integral2 = 0;
+            goto resetposition;
         }
 
+        keep_in_range(&pwm1, -1,1);
+        pwm_motor1.write(abs(pwm1));
+
         if(pwm1 > 0)
             motor1dir = 1;
         else
@@ -116,10 +116,58 @@
         scope.send();
 
         //controleert of batje balletje heeft bereikt
-        if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) {
+        //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
+        if (motor1.getPosition()*omrekenfactor1 > 1.10) {
             balhit = 1;
         }
+
     }
-motorcontroldone:
-    wait(1);
+resetposition:
+
+    while(1) {      // batje en slagarm worden weer in oorspronkelijke positie geplaatst
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
+
+        //regelaar motor2, bepaalt positie
+        error2 = motor2.getPosition()*omrekenfactor2;
+        integral2 = integral2 + error2*TSAMP;
+        pwm2 = Kp2*error2 + Ki2*integral2;
+        
+        keep_in_range(&pwm2, -1,1);
+        pwm_motor2.write(abs(pwm2));
+        if(pwm2 > 0)
+            motor2dir = 1;
+        else
+            motor2dir = 0;
+        
+        //regelaar motor1, bepaalt positie
+        error1 = motor1.getPosition()*omrekenfactor1;
+        integral1 = integral1 + error1*TSAMP;
+        pwm1 = Kp2*error1 + Ki2*integral1; //kp2 en ki2 worden hier gekozen (voorlopig), goede insteling voor motor2 om goed positie te bepalen, miss ook voor motor1
+        
+        keep_in_range(&pwm1, -1,1);
+        pwm_motor1.write(abs(pwm1));
+        if(pwm1 > 0)
+            motor1dir = 1;
+        else
+            motor1dir = 0;
+
+        //controleert of robot terug in oorspronkelijke positie is
+        if(batjeset < 200) {                            
+            if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03 || motor1.getPosition()*omrekenfactor1 < -0.03 || motor1.getPosition()*omrekenfactor1 >0.03) {
+                batjeset = 0;
+            } else {
+                batjeset++;
+            }
+        } else {
+            pwm_motor2.write(0);
+            pwm_motor2.write(0);
+            //direction = force = 0;
+            //goto directionchoice;
+            goto test;            
+        }        
+    }
+test:
+wait(2);
+
 }
\ No newline at end of file