Diff: PROJECT_main.cpp

Revision:

19:fe284ddaa88a

Parent:

18:5467bcc5cbf5

Child:

20:5007ddcd03ef

--- a/PROJECT_main.cpp	Tue Nov 04 09:32:07 2014 +0000
+++ b/PROJECT_main.cpp	Tue Nov 04 09:47:10 2014 +0000
@@ -123,8 +123,8 @@
 float Ki1 = 0.0; //Kp en Ki van motor1, voor de slag
 float Kd1 = 0.0;
 
-float Kp2 = 5.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
-float Ki2 = 1.5; //0.30 en 0.20
+float Kp2 = 2.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
+float Ki2 = 0.1; //0.30 en 0.20
 float Kd2 = 0.0;
 
 volatile bool looptimerflag; //voor motorcontrol TSAMP
@@ -363,7 +363,7 @@
 
 directionchoice:
     log_timer.attach(looper, TSAMP_EMG);
-    direction = 3;
+    direction = 1;
     force = 1;
     goto motorcontrol;
 
@@ -545,26 +545,39 @@
             switch(state) {
                 case 1: {
                     setpoint1=0;
-                    setpoint2 += 100*TSAMP;
+                    //setpoint2 += 1*TSAMP;
                     switch (direction) {
                         case 1:
                             angle = 0.18;
                             break;
                         case 2:
-                            angle = 0.11;
+                            angle = 0.13;
                             break;
                         case 3:
                             angle = 0.08;
-                            break;
-                    }
-                    if(setpoint2>angle) { 
+                            break;                         
+                    }                    
+                    
+                    setpoint2 = angle;
+                    
+                    if(batjeset < 1000) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+                        if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
+                            batjeset = 0;
+                        } else {
+                            batjeset++;
+                        }
+                    } else {
+                        state=2;
+                        count=0;
+                        batjeset = 0;
+                    }                    
+                     
+                    /*
+                    if(setpoint2>angle) {  // batjeset (controle bereik)
                         setpoint2 = angle; //setpoint2 = motor2.getPosition()*omrekenfactor2;
                         count = 0;
                         state=2;
-                    }
-                    /*if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.02) {
-                        state = 2;
-                    }*/
+                    }*/                    
                     break;
                 }
                 case 2: {