Diff: PROJECT_main.cpp

Revision:

9:0bc7f83b761e

Parent:

8:75980dc35763

Child:

10:6bf3e25f020a

--- a/PROJECT_main.cpp	Mon Nov 03 18:52:57 2014 +0000
+++ b/PROJECT_main.cpp	Mon Nov 03 20:17:43 2014 +0000
@@ -110,7 +110,8 @@
 float previouserror1 = 0;
 
 int state = 1;
-int count = 1;
+int count = 0;
+float angle = 0;
 
 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
 float omrekenfactor2 =  0.0015213178; // 6.28/(24*172);
@@ -118,21 +119,13 @@
 float setpoint1 = 0; //te behalen speed van motor1 (37D)
 float setpoint2 = 0; //te behalen hoek van motor2 (25D)
 
-float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
-float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag
+float Kp1 = 12.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 Kp3 = 0.15; //Kp en Ki van motor1, voor de return
-float Ki3 = 0.05; //0.09, 0.05
-float Kd3 = 0.1;
-
-float Kp2 = 1.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
-float Ki2 = 0.8; //0.30 en 0.20
-float Kd2 = 0.1;
-
-float Kp4 = 1.0; //Kp en Ki van motor2, voor de return
-float Ki4 = 0.8;
-float Kd4 = 0.1;
+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 Kd2 = 0.0;
 
 volatile bool looptimerflag; //voor motorcontrol TSAMP
 
@@ -364,6 +357,9 @@
 
 directionchoice:
     log_timer.attach(looper, TSAMP_EMG);
+    direction = 1;
+    force = 1;
+    goto motorcontrol;
 
     while(1) { //Loop keuze DIRECTION
         for(int i=1; i<4; i++) {
@@ -532,60 +528,75 @@
     previouserror1 = previouserror = 0;
     while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag)
-            looptimerflag = false; //clear flag
-
-        switch (direction) {
-            case 1:
-                setpoint2 = (0.436332313+0.197222205);    //25 graden + 11,3 graden, slag naar linkerdoel
-                break;
-            case 2:
-                setpoint2 = (0.436332313);                //25 graden vanaf nul-punt (precies midden)
-                break;
-            case 3:
-                setpoint2 = (0.436332313-0.197222205);    // 25 graden - 11,3 graden, slag naar rechterdoel
-                break;
-        }
+            {}
+        looptimerflag = false; //clear flag
 
         switch(state) {
             case 1:
                 setpoint1=0;
-                if(abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.01) {
-                    state = 2;
+                setpoint2 += 0.4*TSAMP;
+                switch (direction) {
+                    case 1:                        
+                        angle = 0.436332313+0.197222205;  //(0.436332313+0.197222205);    //25 graden + 11,3 graden, slag naar linkerdoel
+                        break;
+                    case 2:                        
+                        angle = 0.436332313; 
+                        break;
+                    case 3:                        
+                        angle = 0.436332313-0.197222205;
+                        break;
+                }                
+                if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1
+                    setpoint2 = angle;                   
+                }
+                if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.04) {
+                    state = 2;    
                 }
                 break;
-                
+
             case 2:
+                setpoint1 = 0;
+                count++;
+                if(count>1000) {
+                    count = 0;
+                    state = 3;
+                }
+            case 3:
                 switch (force) {
                     case 1:
-                        setpoint1 += 6.8*TSAMP;
+                        setpoint1 += 0.4*TSAMP; //6.8*TSAMP;
                         break;
                     case 2:
-                        setpoint1 += 7.4*TSAMP;
+                        setpoint1 += 0.4*TSAMP; //7.4*TSAMP;
                         break;
                     case 3:
-                        setpoint1 += 8.0*TSAMP;
+                        setpoint1 += 0.4*TSAMP; //8.0*TSAMP;
                         break;
                 }
-                if(abs(motor1.getPosition()*omrekenfactor1)>2.1){
-                    state = 3;    
-                }                
+                if(abs(motor1.getPosition()*omrekenfactor1)>2.1) {
+                    state = 4;
+                }
+                break;
+            case 4:
+                setpoint2 -= 0.25*TSAMP;
+                if(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) { // op 0 draait hij mogelijk in de arm
+                    state = 5;
+                }
                 break;
-            case 3:
-                setpoint2 = 0;
-                setpoint1 -= 1.0*TSAMP;
-                if(setpoint1 < 0){
-                    state = 4;    
-                }            
+            case 5:
+                setpoint1 -= 0.25*TSAMP;
+                if(setpoint1 < 0) {
+                    state = 6;
+                }
                 break;
-
-            case 4:
-                setpoint1 = setpoint2 = 0;       
+            case 6:
+                setpoint1 = 0; 
                 count++;
-                if(count>1000){
+                if(count>3000) {
                     count = 0;
                     state = 1;
                     goto directionchoice;
-                }     
+                }
                 break;
         }