Diff: PROJECT_main.cpp

Revision:

22:77d7065cc431

Parent:

21:779de788eb92

Child:

23:913bf8a6f7d8

--- a/PROJECT_main.cpp	Tue Nov 04 10:13:43 2014 +0000
+++ b/PROJECT_main.cpp	Tue Nov 04 10:27:56 2014 +0000
@@ -123,7 +123,7 @@
 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 Kp2 = 0.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
 float Ki2 = 0.0; //0.30 en 0.20
 float Kd2 = 0.0;
 
@@ -548,16 +548,30 @@
                     //setpoint2 += 1*TSAMP;
                     switch (direction) {
                         case 1:
-                            angle = 0.1; //0.15 links
+                            pwm_motor2.write(0.8);
+                            motor2dir = 1;
+                            count++;
+                            if (count>50) {
+                                state=2;
+                                count = 0;
+                                pwm_motor2.write(0);
+                            }
                             break;
                         case 2:
-                            angle = 0.13;
+                            pwm_motor2.write(0.8);
+                            motor2dir = 1;
+                            count++;
+                            if (count>100) {
+                                state=2;
+                                count = 0;
+                                pwm_motor2.write(0);
+                            }
                             break;
                         case 3:
                             angle = 0.08;
                             break;                         
                     }                    
-                    
+                    /*
                     setpoint2 = angle;   
                                                         
                     if(motor2.getPosition()>angle) {  // batjeset (controle bereik)                        
@@ -565,12 +579,12 @@
                         state=2;
                     }                    
                     
-                    /*
+                    
                     if(setpoint2>angle) {  // batjeset (controle bereik)
                         setpoint2 = angle; //setpoint2 = motor2.getPosition()*omrekenfactor2;
                         count = 0;
                         state=2;
-                    }      */             
+                    }     */              
                     break;
                 }
                 case 2: {                    
@@ -644,7 +658,7 @@
                 motor1dir = 0;
             }
             */
-
+/*
             //regelaar motor2, bepaalt positie
             controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
             integral = integral + controlerror*TSAMP;
@@ -659,7 +673,7 @@
             } else {
                 motor2dir = 0;
             }
-
+*/
             /*
                     //controleert of batje positie heeft bepaald
                     if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol