Diff: main.cpp

Revision:

3:eb5b57162e38

Parent:

0:50d492ea0fd0

Child:

4:6d88a281192f

diff -r 50d492ea0fd0 -r eb5b57162e38 main.cpp
--- a/main.cpp	Thu Oct 15 07:57:23 2015 +0000
+++ b/main.cpp	Fri Oct 23 07:36:45 2015 +0000
@@ -118,17 +118,6 @@
     m1_Ki = constante1*0.001;
 }
 
-double P(double error, const double Kp)       //P regelaar
-{
-    return Kp*error;                            //ingangssignaal voor de motor\plant) = dus voltage
-}
-
-double PI(double e, double Kp, const double Ki, double Ts, double &e_int)
-{
-    e_int = e_int + Ts * e;                     // e_int is changed globally because it’s ’by reference’ (&)
-    return Kp * e + Ki * e_int;
-}
-
 double biquad(double u, double &v1, double &v2, const double a1, const double a2,
     const double b0, const double b1, const double b2){
         double v = u-a1*v1-a2*v2;       //formule uit slides van biquad filter
@@ -151,90 +140,6 @@
     return Kp*e+Ki*e_int+Kd*e_der2;  //formule uit slides 
 } 
 
-// P controllers
-void motor2_P_Controller()
-{
-    reference2 = potmeter2.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
-    //reference2 = y;
-    double position =(encoder2.getPosition());              //waarde tussen 0 en 4200
-    double u = P(reference2 - position, motor1_Kp);      //P controller aanroepen
-    
-    if (u > 0) {                                  //directie bepalen
-        motor2direction=1;
-    } else if (u <= 0) {
-        motor2direction =0;
-    }
-    if (u <grens && u>-grens) {               //bepalen of de motor aan of uit moet
-        motor2speed = 0.0f;
-    } else {
-        motor2speed = 0.1f;
-    }
-}
-
-void motor1_P_Controller()
-{
-    reference1 = potmeter1.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
-    //reference1=x;
-    double position =(encoder1.getPosition());              // waarde tussen 0 en 4200
-    difference1 = reference1-position;
-    double u = P(reference1 - position, motor1_Kp);      //P controller aanroepen
-    if (u > 0) {                                  //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
-        motor1direction=0;
-    } else if (u <= 0) {
-        motor1direction =1;
-    }
-    if (u <grens && u >-grens) {              //bepalen of de motor aan of uit moet
-        motor1speed = 0.0f;
-    } else {
-        motor1speed = 0.1f;
-    }
-}
-
-//PI controllers
-void motor1_PI_Controller()
-{
-    //reference1 = potmeter1.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
-    float hoek1 = bereken_hoek1(countsX,countsY);
-    reference1=hoek1;
-    double position =(encoder1.getPosition());              // waarde tussen 0 en 4200
-    
-    difference1 = reference1 - position;
-    constanteBepalen();
-    double u = PI(reference1 - position, m1_Kp, m1_Ki, m1_Ts, m1_err_int);
-    if (u > 0) {                                  //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
-        motor1direction=0;
-    } else if (u <= 0) {
-        motor1direction =1;
-    }
-    if (u <grens && u >-grens) {              //bepalen of de motor aan of uit moet
-        motor1speed = 0.0f;
-    } else {
-        motor1speed = 0.1f;
-    }
-}
-
-void motor2_PI_Controller()
-{
-    //reference2 = potmeter2.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
-    float hoek2 = bereken_hoek2(countsX,countsY);
-    reference2 = hoek2;
-    double position =(encoder2.getPosition());              //waarde tussen 0 en 4200
-
-    difference2 = reference2 - position;
-    constanteBepalen();
-    double u = PI(reference2 - position, m1_Kp, m1_Ki, m1_Ts, m1_err_int);
-    if (u > 0) {                                  //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
-        motor2direction=1;
-    } else if (u <= 0) {
-        motor2direction =0;
-    }
-    if (u <grens && u >-grens) {              //bepalen of de motor aan of uit moet
-        motor2speed = 0.0f;
-    } else {
-        motor2speed = 0.1f;
-    }
-}
-
 void motor1_PID_Controller()
 {
     //reference1 = potmeter1.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
@@ -363,8 +268,5 @@
             checkButtonStateY_go = false;
             checkButtonStateY();
         }
-        
-        
-
     }
 }
\ No newline at end of file