Diff: main.cpp

Revision:

14:b1de410384c2

Parent:

13:95a4bb9daf63

Child:

15:2d2278caf861

--- a/main.cpp	Mon Nov 03 21:31:16 2014 +0000
+++ b/main.cpp	Mon Nov 03 22:34:24 2014 +0000
@@ -25,11 +25,11 @@
 #define M2_DIR PTC9
 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
 #define TSAMP 0.005
-#define K_P (80000)
+#define K_P (5000)
+#define K_I (0)
+#define K_D (0.1)
 #define K_P_motor2 (75)
 #define K_D_motor2 (0.01)
-#define K_I (0.01)
-#define K_D (0.01)
 #define I_LIMIT 100.
 #define lengte_arm 0.5
 
@@ -85,7 +85,6 @@
 float PWM2_percentage = 0;
 float PWM1;
 float PWM2;
-float setpoint = 0;
 float prev_setpoint = 0;
 float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203};
 float lowpass_1_states[4];
@@ -219,30 +218,31 @@
     doel_bepaling();
 }//void meten_richting
 
-void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge)
+void GotoPosition (float position_setpoint_rad, float speed_radpersecond)
 {
-    current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
-    pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
-    previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
+    static float setpoint = 0; 
+    if (setpoint < position_setpoint_rad) {
+        setpoint += (TSAMP * speed_radpersecond);
+        if (setpoint > position_setpoint_rad) {
+            setpoint = position_setpoint_rad;
+        }
+    }
 
-    //nu gaan we snelheid volgen d.m.v. positie regeling
-    if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) { //if position error < ...rad, then stop.
-        speed_radpersecond = 0;
-        setpoint = pos_motor1_rad;
+    else if (setpoint > position_setpoint_rad) {
+        setpoint -= (TSAMP * speed_radpersecond);
+        if (setpoint < position_setpoint_rad) {
+            setpoint = position_setpoint_rad;
+        }
+    } else if (setpoint == position_setpoint_rad) {
         current_herhalingen = previous_herhalingen + 1;
         previous_herhalingen = current_herhalingen;
-        //pc.printf("stop\n\r");
-        PWM1_percentage = 0;
-    }//if
-    else if(pos_motor1_rad - position_setpoint_rad < 0) {
-        setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
-        PWM1_percentage = pid(setpoint, pos_motor1_rad);
-    }//else if
-    else {
-        setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
-        PWM1_percentage = pid(setpoint, pos_motor1_rad);
-    }//else
-    //pc.printf("%f\n\r",PWM1_percentage);
+    }
+
+    current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
+    pos_motor1_rad = current_pos_motor1/(1600./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
+    PWM1_percentage = pid(setpoint, pos_motor1_rad);
+    pc.printf("setpoint : %f\r\n",setpoint);
+    pc.printf("pos_motor_1 :%f\r\n", pos_motor1_rad);
 
     if (PWM1_percentage < -100) {
         PWM1_percentage = -100;
@@ -258,7 +258,6 @@
     }//else
 
     pwm_motor1.write(abs(PWM1_percentage/100.));
-    prev_setpoint = setpoint;
 }//void GotoPosition
 
 float pid(float setpoint, float measurement)
@@ -279,6 +278,7 @@
 
 void translatie (float position2_setpoint_rad, float speed2_radpersecond, float marge2)
 {
+    static float setpoint = 0; 
     if (setpoint < position2_setpoint_rad) {
         setpoint = prev_setpoint +( 0.0005 * speed2_radpersecond);
         if (setpoint > position2_setpoint_rad) {
@@ -423,7 +423,7 @@
 
 
         case TEST: {
-            state = RUST;
+            state = SLAAN;
             break;
         }
 
@@ -443,27 +443,27 @@
             if (current_herhalingen >= 400) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                doel_richting = 0;
+                doel_richting = 0; 
                 state = RETURN2RUST;
             }//if (current_herhalingen == 100
             break;
         }//case INSTELLEN_RICHTING
 
         case SLAAN: {
-            GotoPosition(1.5 ,8, 0.1);
+            GotoPosition(1.5 ,8);
             if (current_herhalingen == 400) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 prev_setpoint =0;
-                setpoint =0;
+                //setpoint = 0;
                 state = RETURN2RUST;
             }//if (current_herhalingen == 100)
             break;
         }//case SLAAN
 
         case RETURN2RUST: {
-            translatie(0,10,0.1);
-            /*GotoPosition(0,4, 0.05);
+            //translatie(0,10,0.1);
+            GotoPosition(0,1);
             doel_richting = 0;
             doel_hoogte = 0;
             if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) {
@@ -473,7 +473,7 @@
                 current_herhalingen = 0;
                 current_herhalingen = 0;
             }//if (current_herhalingen == 100)
-            */
+            
             break;
         }// case RETURN2RUST