Diff: main.cpp

Revision:

54:2b54283b3b47

Parent:

53:bdcbf365a9aa

Child:

55:4eb229a35859

--- a/main.cpp	Sun Nov 02 14:48:45 2014 +0000
+++ b/main.cpp	Mon Nov 03 12:06:11 2014 +0000
@@ -9,16 +9,16 @@
 #define K_I (0.02  *TSAMP1)
 #define K_D (0  /TSAMP1)
 #define I_LIMIT 1.
-#define K_Pp (0.2) //-------------------------------Deze drie waarden moeten anders!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-#define K_Ip (0.02  *TSAMP2)//-----------------------------TSAMP1 veranderd naar TSAMP2
-#define K_Dp (0  /TSAMP2)//--------------------------------TSAMP1 veranderd naar TSAMP2
+#define K_Pp (0.0025) //-------------------------------Deze drie waarden moeten anders!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+#define K_Ip (0.0  *TSAMP2)//-----------------------------TSAMP1 veranderd naar TSAMP2
+#define K_Dp (0.000004  /TSAMP2)//--------------------------------TSAMP1 veranderd naar TSAMP2
 
 #define TSAMP1 0.01
 #define TSAMP2 0.01
 #define WACHTEN 1
 #define SLAAN 2
 #define TERUGKEREN 3
-#define ANGLEMAX -150
+#define ANGLEMAX -50
 #define ANGLEMIN 0
 
 //initiating functions
@@ -27,7 +27,7 @@
 void Calibratie_Triceps();
 void Calibratie_Biceps();
 float pid(float setspeed, float measurement, bool reset = false);
-float pidpositie(float setposition, float measurement);
+float pidpositie(float setposition, float measurement, bool reset =false);
 void motor2aansturing();
 void motor1aansturing();
 void motor1aansturingdeel2();
@@ -85,7 +85,7 @@
 float new_pwm;
 float PWM2 = 0.3; //PWM voor instellen hoek batje, kan waarschijnlijk een stuk langzamer
 int toestand = WACHTEN; //terugkeren?
-float setspeed = 0, V3=60, V2=40, V1 =30, Vreturn= 35;//V in counts/s
+float setspeed = 0, V3=60, V2=40, V1 =30;//V in counts/s
 
 
 Encoder motor1(PTD5,PTD3);
@@ -102,8 +102,8 @@
 
 DigitalOut rood(PTD1);
 DigitalOut groen(PTA13);
-DigitalOut blauw(PTA12);
-DigitalOut wit(PTD4);
+DigitalOut blauw(PTD4);
+DigitalOut wit(PTA12);
 
 /* FRDM-KL25Z built-in touch slider
 *******************
@@ -429,16 +429,17 @@
             looptimer1.attach(motor1aansturing,TSAMP1);
             wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer1.detach();
-            pc.printf("detachMotor1\n");
+            pc.printf("detachMotor1\n"); 
 
             pid(0,0,true);
-
+                        
             Ticker looptimer3;
             looptimer3.attach(motor1aansturingdeel2,TSAMP1);  //of TSAMP2?....
             wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer3.detach();
             pc.printf("detachMotor1\n");
-
+            
+            pidpositie(0,0,true);
             pwm_motor1.write(0);
             toestand = WACHTEN; //hierheen verplaatst vanaf motor1aansturingdeel2. Belangrijk!! niet weghalen!!
 
@@ -447,6 +448,7 @@
             myled3=1;
             groen=0;
             blauw=0;
+            
         }
     }
     //}
@@ -472,19 +474,23 @@
     return out_p + out_i + out_d;
 }
 
-float pidpositie(float setposition, float measurement)
+float pidpositie(float setposition, float measurement, bool reset)
 {
     float error;
     static float prev_error = 0;
     float           out_p = 0;
     static float    out_i = 0;
     float           out_d = 0;
+    if(reset==true) {
+        out_i = 0;
+        prev_error = 0;
+    }
     error  = setposition-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;
+    return abs(out_p + out_i + out_d);
 }
 
 void Triceps()
@@ -655,8 +661,7 @@
 
 void motor2aansturing()
 {
-    switch(positie)
-    {
+    switch(positie) {
         case 0:
             if (motor2.getPosition()>= 0) {
                 motordir2 = 1;
@@ -698,11 +703,11 @@
             motordir1 = 1;
             if (motor1.getPosition() <= ANGLEMAX) {
                 toestand = TERUGKEREN;
-                pwm_motor1.write(0);//arvid had hier 0,0 gezet?!
+                pwm_motor1.write(0.0);//arvid had hier 0,0 gezet?!
                 pc.printf("toestand = terugkeren, wacht tot 2e ticker\n\r");
                 //stop = 1;
             }
-            
+
             break;
         case WACHTEN:
             pidpositie(ANGLEMIN,motor1.getPosition()); //dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
@@ -728,18 +733,18 @@
                 }//end switch
             } //end if
             break;
-        /*case TERUGKEREN:
-            if (motor1.getPosition()<=ANGLEMAX) {
-                new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
-                pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
-                motordir1 = 0;
-            }
-            if (motor1.getPosition()>= ANGLEMAX) {
-                new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
-                pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
-                motordir1 = 1;
-            }
-            break;*///overbodig!!! 
+            /*case TERUGKEREN:
+                if (motor1.getPosition()<=ANGLEMAX) {
+                    new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
+                    pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
+                    motordir1 = 0;
+                }
+                if (motor1.getPosition()>= ANGLEMAX) {
+                    new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
+                    pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
+                    motordir1 = 1;
+                }
+                break;*///overbodig!!!
     } //end switch
     scope.set(0,motor1.getPosition());
     scope.set(1,motor1.getPosition());