Diff: PROJECT_main.cpp

Revision:

18:5467bcc5cbf5

Parent:

17:7641d7934b91

Child:

19:fe284ddaa88a

--- a/PROJECT_main.cpp	Tue Nov 04 08:34:08 2014 +0000
+++ b/PROJECT_main.cpp	Tue Nov 04 09:32:07 2014 +0000
@@ -98,7 +98,7 @@
 float derivative = 0;
 float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
 float balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
-float kalibratie = 0;
+float kalibratie = 1; //NOG AAN TE PASSEN NAAR 0 
 float controlerror = 0;
 float previouserror = 0;
 float pwm = 0;
@@ -119,12 +119,12 @@
 float setpoint1 = 0; //te behalen speed van motor1 (37D)
 float setpoint2 = 0; //te behalen hoek van motor2 (25D)
 
-float Kp1 = 1.2; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
+float Kp1 = 2.0; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
 float Ki1 = 0.0; //Kp en Ki van motor1, voor de slag
 float Kd1 = 0.0;
 
-float Kp2 = 8.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 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 Kd2 = 0.0;
 
 volatile bool looptimerflag; //voor motorcontrol TSAMP
@@ -291,6 +291,8 @@
 
     //kalibratie
 
+motor2cal:
+
     //motorarm naar nul-positie
     blink.attach(kalbi, 0.2);
     blink2.attach(kaltri, 0.2);
@@ -361,7 +363,7 @@
 
 directionchoice:
     log_timer.attach(looper, TSAMP_EMG);
-    direction = 1;
+    direction = 3;
     force = 1;
     goto motorcontrol;
 
@@ -543,20 +545,20 @@
             switch(state) {
                 case 1: {
                     setpoint1=0;
-                    setpoint2 += 0.4*TSAMP;
+                    setpoint2 += 100*TSAMP;
                     switch (direction) {
                         case 1:
-                            angle = 0.436332313+0.197222205;  //(0.436332313+0.197222205);    //25 graden + 11,3 graden, slag naar linkerdoel
+                            angle = 0.18;
                             break;
                         case 2:
-                            angle = 0.436332313;
+                            angle = 0.11;
                             break;
                         case 3:
-                            angle = 0.436332313-0.197222205;
+                            angle = 0.08;
                             break;
                     }
-                    if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1
-                        setpoint2 = angle;
+                    if(setpoint2>angle) { 
+                        setpoint2 = angle; //setpoint2 = motor2.getPosition()*omrekenfactor2;
                         count = 0;
                         state=2;
                     }
@@ -611,14 +613,15 @@
                     if(count>3000) {
                         count = 0;
                         state = 1;
-                        goto directionchoice;
+                        goto motor2cal;
                     }
                     break;
                 }
             }
 
             //motor regeling
-
+            
+            /*
             //regelaar motor1, bepaalt positie
             controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1;
             integral1 = integral1 + controlerror1*TSAMP;
@@ -634,7 +637,7 @@
             } else {
                 motor1dir = 0;
             }
-
+            */
 
             //regelaar motor2, bepaalt positie
             controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;