Diff: main.cpp

Revision:

24:a165dcd86710

Parent:

23:8f7ce4894c58

Child:

25:28be2bf11ad7

Child:

27:b58e10cbd35b

--- a/main.cpp	Fri Oct 31 14:33:13 2014 +0000
+++ b/main.cpp	Fri Oct 31 14:54:49 2014 +0000
@@ -9,6 +9,9 @@
 #define K_I (0.02  *TSAMP1)
 #define K_D (0  /TSAMP1)
 #define I_LIMIT 1.
+#define K_Pp (0.2)
+#define K_Ip (0.02  *TSAMP1)
+#define K_Dp (0  /TSAMP1)
 
 #define TSAMP1 0.01
 #define TSAMP2 0.01
@@ -24,6 +27,7 @@
 void Calibratie_Triceps();
 void Calibratie_Biceps();
 float pid(float setspeed, float measurement, bool reset = false);
+float pidpositie(float setposition, float measurement);
 void motor2aansturing();
 void motor1aansturing();
 void motor1aansturingdeel2();
@@ -78,14 +82,13 @@
 bool stop;
 float new_pwm;
 float PWM2 = 0.3; //PWM voor instellen hoek batje
-int toestand = TERUGKEREN;
+int toestand = WACHTEN; //terugkeren?
 float setspeed = 0, V3=60, V2=40, V1 =30, Vreturn= 35;//V in counts/s
 
 
-//Encoder motor1(PTD5,PTD3); //actual
-//Encoder motor2(PTD0,PTD2);
-//Encoder motor1(PTD5,PTD3);
-//Encoder motor2(PTD0,PTD2);
+Encoder motor1(PTD5,PTD3);
+Encoder motor2(PTD0,PTD2);
+
 DigitalOut motordir1(PTA4);
 DigitalOut motordir2(PTC9);
 PwmOut pwm_motor1(PTA5);
@@ -426,6 +429,21 @@
     return out_p + out_i + out_d;
 }
 
+float pidpositie(float setposition, float measurement)
+{
+    float error;
+    static float prev_error = 0;
+    float           out_p = 0;
+    static float    out_i = 0;
+    float           out_d = 0;
+    error  = setspeed-measurement;
+    out_p  = error*K_Pp;
+    out_i += error*K_Ip;
+    out_d  = (error-prev_error)*K_Dp;
+    prev_error = error;
+    return out_p + out_i + out_d;
+}
+
 void Triceps()
 {
     //Triceps lezen
@@ -629,8 +647,6 @@
 
     if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
         toestand = WACHTEN;
-        //motor1.setPosition(0);
-        pid(0,0,true);
         pc.printf("if1\n");
     }
     if (snelheidsstand != 0 && toestand == WACHTEN && stop == 0) { // stop = 0 in het begin let op dat dit na reset nog zo is
@@ -653,11 +669,12 @@
         toestand = TERUGKEREN;
         //motor1.setPosition(0);
         pwm_motor1.write(0);
-        pid(0,0,true);
+        //pid(0,0,true);
         stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
         pc.printf("toestand = terugkeren\n\r");
     }
     if (toestand == WACHTEN) {
+        pidpositie(ANGLEMIN, motor1.getPosition());
         pwm_motor1.write(0);
         pc.printf("ifwachten\n");
     }
@@ -666,6 +683,10 @@
         pwm_motor1.write(new_pwm);
         motordir1 = 1;
         pc.printf("SLAAN\n");
+        
+    if (toestand == TERUGKEREN) {
+        pidpositie(ANGLEMAX, motor1.getPosition());
+        }
 
 
         /*if (toestand == TERUGKEREN) {
@@ -686,7 +707,7 @@
     if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
         toestand = WACHTEN;
         //motor1.setPosition(0);
-        pid(0,0,true);
+        //pid(0,0,true);
         //pc.printf("if2\n");
     }
     if (toestand == TERUGKEREN) {
@@ -698,6 +719,7 @@
     }
     if (toestand == WACHTEN) {
         pwm_motor1.write(0);
+        pidpositie(ANGLEMIN, motor1.getPosition());
         //pc.printf("ifwachten2\n");
     }
     //sturen naar HID Scope