Diff: main.cpp

Revision:

9:aa5d6636197b

Parent:

7:9a1007e35bac

Child:

10:548230cbdb8e

Child:

11:2375b538b631

--- a/main.cpp	Wed Mar 20 13:21:56 2019 +0000
+++ b/main.cpp	Fri Mar 22 10:54:48 2019 +0000
@@ -18,16 +18,17 @@
 PwmOut pwmpin1(D5);
 PwmOut pwmpin2(D6);
 DigitalOut direction2(D7);
-volatile float PWM1;
-volatile float PWM2;
+volatile float pwm1;
 volatile float pwm2;
 
 //Encoder
-DigitalIn EncoderA(D13);
-DigitalIn EncoderB(D12);
-QEI encoder2 (D13, D12, NC, 8400, QEI::X4_ENCODING);
+QEI encoder1 (D10, D9, NC, 1200, QEI::X4_ENCODING);
+QEI encoder2 (D13, D12, NC, 4800, QEI::X4_ENCODING);
+double Pulses1;
+double motor_position1;
 double Pulses2;
 double motor_position2;
+double error1;
 
 //Pot meter
 AnalogIn pot(A1);
@@ -42,39 +43,73 @@
 
 
 //Kinematica
-double stap;
+double stap1;
+double stap2;
 double KPot;
 float KPotabs;
+
 float ElbowReference;
 float Ellebooghoek1;
 float Ellebooghoek2;
 float Ellebooghoek3;
 float Ellebooghoek4;
-float Hoeknieuw;
+
+float PolsReference;
+float Polshoek1;
+float Polshoek2;
+float Polshoek3;
+float Polshoek4;
+
+float Hoeknieuw1;
+float Hoeknieuw2;
 
 //Limiet in graden
-float lowerlim = 0;
-float upperlim = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
+float lowerlim1 = 0;
+float upperlim1 = 748.8;
+float lowerlim2 = 0;
+float upperlim2 = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
 
 // VARIABLES PID CONTROLLER
-double Kp = 6;          // Zonder arm: 6,0,1
-double Ki = 0;          //
-double Kd = 1;          //
+double Kp1 = 6;          
+double Ki1 = 0;          
+double Kd1 = 1;
+double Kp2 = 6;          // Zonder arm: 6,0,1
+double Ki2 = 0;          
+double Kd2 = 1;          
 double Ts = 0.001;      // Sample time in seconds
 
-float Kinematics(float KPot)
+float Kinematics1(float KPot)
 {
 
     if (KPot > 0.45f) {
-        stap = KPot*150*Ts;         // 144 graden van de arm in 5 seconden
-        Hoeknieuw = ElbowReference + stap;
-        return Hoeknieuw;
+        stap1 = KPot*150*Ts;
+        Hoeknieuw1 = PolsReference + stap1;
+        return Hoeknieuw1;
     }
 
     else if (KPot < -0.45f) {
-        stap = KPot*150*Ts;
-        Hoeknieuw = ElbowReference + stap;
-        return Hoeknieuw;
+        stap1 = KPot*150*Ts;
+        Hoeknieuw1 = PolsReference + stap1;
+        return Hoeknieuw1;
+    }
+
+    else {
+        return PolsReference;
+    }
+}
+float Kinematics2(float KPot)
+{
+
+    if (KPot > 0.45f) {
+        stap2 = KPot*150*Ts;         // 144 graden van de arm in 5 seconden
+        Hoeknieuw2 = ElbowReference + stap2;
+        return Hoeknieuw2;
+    }
+
+    else if (KPot < -0.45f) {
+        stap2 = KPot*150*Ts;
+        Hoeknieuw2 = ElbowReference + stap2;
+        return Hoeknieuw2;
     }
 
     else {
@@ -82,18 +117,37 @@
     }
 }
 
-float Limits(float Ellebooghoek2)
+float Limits1(float Polshoek2)
 {
 
-    if (Ellebooghoek2 <= upperlim && Ellebooghoek2 >= lowerlim) {       //Binnen de limieten
+    if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) {       //Binnen de limieten
+        Polshoek3 = Polshoek2;
+    }
+
+    else {
+        if (Polshoek2 >= upperlim1) {                            //Boven de limiet
+            Polshoek3 = upperlim1;
+        } else {                                                    //Onder de limiet
+            Polshoek3 = lowerlim1;
+        }
+    }
+
+    return Polshoek3;
+}
+
+
+float Limits2(float Ellebooghoek2)
+{
+
+    if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) {       //Binnen de limieten
         Ellebooghoek3 = Ellebooghoek2;
     }
 
     else {
-        if (Ellebooghoek2 >= upperlim) {                            //Boven de limiet
-            Ellebooghoek3 = upperlim;
+        if (Ellebooghoek2 >= upperlim2) {                            //Boven de limiet
+            Ellebooghoek3 = upperlim2;
         } else {                                                    //Onder de limiet
-            Ellebooghoek3 = lowerlim;
+            Ellebooghoek3 = lowerlim2;
         }
     }
 
@@ -104,72 +158,127 @@
 
 
 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
-
-double PID_controller(double error)
+double PID_controller1(double error1)
 {
-    static double error_integral = 0;
-    static double error_prev = error; // initialization with this value only done once!
+    static double error1_integral = 0;
+    static double error1_prev = error1; // initialization with this value only done once!
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
     
     /* PID testing
-    Kp = 10 * Pot2;
-    Ki = 10 * Pot1;
+    Kp1 = 10 * Pot2;
+    Ki1 = 10 * Pot1;
     
     if (!But2) {
-        Kd = Kd + 0.01;
+        Kd1 = Kd1 + 0.01;
     }
     if (!But1){
-        Kd = Kd - 0.01;
+        Kd1 = Kd1 - 0.01;
     }
     */
     
     // Proportional part:
-    double u_k = Kp * error;
+    double u_k1 = Kp1 * error1;
 
     // Integral part
-    error_integral = error_integral + error * Ts;
-    double u_i = Ki * error_integral;
+    error1_integral = error1_integral + error1 * Ts;
+    double u_i1 = Ki1* error1_integral;
 
     // Derivative part
-    double error_derivative = (error - error_prev)/Ts;
-    double filtered_error_derivative = LowPassFilter.step(error_derivative);
-    double u_d = Kd * filtered_error_derivative;
-    error_prev = error;
+    double error1_derivative = (error1 - error1_prev)/Ts;
+    double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
+    double u_d1 = Kd1 * filtered_error1_derivative;
+    error1_prev = error1;
 
     // Sum all parts and return it
-    return u_k + u_i + u_d;
+    return u_k1 + u_i1 + u_d1;
+}
+double PID_controller2(double error2)
+{
+    static double error2_integral = 0;
+    static double error2_prev = error2; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
+    
+    /* PID testing
+    Kp2 = 10 * Pot2;
+    Ki2 = 10 * Pot1;
+    
+    if (!But2) {
+        Kd2 = Kd2 + 0.01;
+    }
+    if (!But1){
+        Kd2 = Kd2 - 0.01;
+    }
+    */
+    
+    // Proportional part:
+    double u_k2 = Kp2 * error2;
+
+    // Integral part
+    error2_integral = error2_integral + error2 * Ts;
+    double u_i2 = Ki2 * error2_integral;
+
+    // Derivative part
+    double error2_derivative = (error2 - error2_prev)/Ts;
+    double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
+    double u_d2 = Kd2 * filtered_error2_derivative;
+    error2_prev = error2;
+
+    // Sum all parts and return it
+    return u_k2 + u_i2 + u_d2;
 }
 
-void moter2_control(double u)
+void moter1_control(double u1)
 {
-    direction2= u < 0.0f; //positief = CW
-    if (fabs(u)> 0.7f) {
-        u = 0.7f;
+    direction1= u1 > 0.0f; //positief = CW
+    if (fabs(u1)> 0.7f) {
+        u1 = 0.7f;
     } else {
-        u= u;
+        u1= u1;
     }
-    pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+    pwmpin1= fabs(u1); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+}
+
+void moter2_control(double u2)
+{
+    direction2= u2 < 0.0f; //positief = CW
+    if (fabs(u2)> 0.7f) {
+        u2 = 0.7f;
+    } else {
+        u2= u2;
+    }
+    pwmpin2= fabs(u2); //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
 void PwmMotor(void)
 {
     // Reference hoek berekenen, in graden
-    float Ellebooghoek1 = Kinematics(pwm2);
-    float Ellebooghoek4 = Limits(Ellebooghoek1);
+    float Ellebooghoek1 = Kinematics2(pwm2);
+    float Ellebooghoek4 = Limits2(Ellebooghoek1);
     ElbowReference = Ellebooghoek4;
+    
+    float Polshoek1 = Kinematics1(pwm2);
+    float Polshoek4 = Limits1(Polshoek1);
+    PolsReference = Polshoek4;
 
     // Positie motor berekenen, in graden
+    Pulses1 = encoder1.getPulses();
+    motor_position1 = -(Pulses1/1200)*360;
     Pulses2 = encoder2.getPulses();
-    motor_position2 = -(Pulses2/8400)*360;
+    motor_position2 = -(Pulses2/4800)*360;
 
-    double error = ElbowReference - motor_position2;
-    double u = PID_controller(error);
-    moter2_control(u);
+    double error1 = PolsReference - motor_position1;
+    double u1 = PID_controller1(error1);
+    moter1_control(u1);
+    
+    double error2 = ElbowReference - motor_position2;
+    double u2 = PID_controller2(error2);
+    moter2_control(u2);
 
 }
 
 void MotorOn(void)
 {
+    pwmpin1 = 0;
     pwmpin2 = 0;
     Pwm.attach (PwmMotor, Ts);
 
@@ -178,6 +287,7 @@
 {
     Pwm.detach ();
     pwmpin2 = 0;
+    pwmpin1 = 0;
 }
 
 void ContinuousReader(void)
@@ -185,15 +295,16 @@
     Pot2 = pot.read();
     Pot1 = pot0.read();
     pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
+    pwm1 =(Pot1*2)-1;
 }
 
 /*
 void Kdcount (void)             // Voor het testen van de PID waardes
 {
     int count = 0;
-    ElbowReference = ElbowReference + 10;
+    PolsReference = PolsReference + 10;
     if (count == 7) {
-        ElbowReference = 0;
+        PolsReference = 0;
         count = 0;
     }
     count ++;
@@ -230,7 +341,7 @@
         led = 0;
         if(counter==10) {
             float tmp = t.read();
-            printf("%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Kp,Ki,Kd);
+            printf("%f,%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position1,PolsReference,error1,Kp1,Ki1,Kd1);
             counter = 0;
         }
         counter++;