Diff: main.cpp

Revision:

3:b353ee86230a

Parent:

2:2563d1d8461f

Child:

4:5f7d1654108d

--- a/main.cpp	Fri Oct 27 09:29:18 2017 +0000
+++ b/main.cpp	Tue Oct 31 21:35:54 2017 +0000
@@ -3,6 +3,7 @@
 #include "MODSERIAL.h"
 #include "math.h"
 #include "FastPWM.h"
+#include "encoder.h"
 
 
 
@@ -11,39 +12,94 @@
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
 DigitalOut motor1DC(D7);
+DigitalOut motor2DC(D4);
 FastPWM motor1PWM(D6);
-DigitalOut motor2DC(D4);
 FastPWM motor2PWM(D5);
 
 AnalogIn   potMeter1(A0);
 AnalogIn   potMeter2(A1);
 DigitalIn  button1(D11);
 DigitalIn  button2(D12);
-QEI Encoder1(D12,D13,NC,4200);
-QEI Encoder2(D9,D8,NC,4200);
+Encoder Encoder1(D12,D13);
+Encoder Encoder2(D8,D9);
 
 MODSERIAL pc(USBTX,USBRX);
 
 Ticker controller;
 
-int potmultiplier = 8000; // Multiplier for the pot meter reference which is normally between 0 and 1
+// ---- Motor Constants-------
+float Pwmperiod = 0.0001f;
+int potmultiplier = 800;    // Multiplier for the pot meter reference which is normally between 0 and 1
+float gainM1 = 1/35.17;     // encoder pulses per degree theta
+float gainM2 = 0.01;      // gain for radius r
 
-int frequency_pwm = 1000;
+// new PID constants, will have to be determined trough trial and error.
 
-//Start constants for the PID ------------------------------- 
+double kp = 250;
+double ki = 100;
+double kd = 0;
+
+
+volatile float motor1;
+volatile float motor2;
+
+//Start constants PID ------------------------------- 
 const double pi = 3.1415926535897;
-const double M1_TS = 1/100; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
+const double M1_TS = 0.001f; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
 
 //verplaatst
-const float RAD_PER_PULSE = (2*pi)/4200;
+const float     RAD_PER_PULSE = (2*pi)/4200;
+const float     CONTROLLER_TS = 0.01;     //TIME INTERVAL/ hZ
+
+//----PID constants
+const float     M1_KP = 10;
+const float     M1_KI = 0.5;
+const float     M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
+double          m1_err_int = 0; 
+double          m1_prev_err = 0;
+
+const float     M2_KP = 10;
+const float     M2_KI = 0.5;
+const float     M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
+double          m2_err_int = 0; 
+double          m2_prev_err = 0;
+
+//---- Biquad constants---------
+const double    M1_F_A1  = 1.0 ;
+const double    M1_F_A2  = 2.0; 
+const double    M1_F_B0  = 1.0; 
+const double    M1_F_B1  = 3.0; 
+const double    M1_F_B2  = 4.0;
+double          m1_f_v1  = 0; 
+double          m1_f_v2  = 0;
+
 
-const float M1_KP = 10, M1_KI = 0.5, M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
-double m1_err_int = 0, m1_prev_err = 0 ;
-const double M1_F_A1 = 1.0 , M1_F_A2  = 2.0 , M1_F_B0  = 1.0 , M1_F_B1  = 3.0 , M1_F_B2  = 4.0 ;
-double m1_f_v1  = 0  , m1_f_v2  = 0 ;
-//End of constant for the PID
+//-----------------Start PID part----------------------------START
+double PID1(double e1, const double Kp1, const double Ki1, const double Kd1, double Ts, double &e_int1, double &e_prev1, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
+
+    double e_der1 = (e1 - e_prev1)/Ts;         // Derivative, Ts = motor1-timestep
+    // biquad part, see slide
+    //e_der1 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
+    e_prev1 = e1;
+    e_int1 += Ts*e1;                          // Integral   
+    
+    return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1;
+
+}
 
-//Get reference position in different way-----------------START
+double PID2(double e2, const double Kp2, const double Ki2, const double Kd2, double Ts, double &e_int2, double &e_prev2, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
+
+    double e_der2 = (e2 - e_prev2)/Ts;         // Derivative, Ts = motor1-timestep
+    // biquad part, see slide
+    //e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
+    e_prev2 = e2;
+    e_int2 += Ts*e2;                          // Integral   
+    
+    return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2;
+
+}
+
+//------------Get reference position-----------------START
 float Get_X_Position(){
     double X = potMeter1 * potmultiplier;
     return X;
@@ -53,64 +109,55 @@
     double Y = potMeter2 * potmultiplier;
     return Y;
 }
-//-----------------------------------------------------------END
-
-//Start PID part ------------------------------------------START
-double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
-
-// Derivative
-double e_der = (e - e_prev)/Ts; // Ts = motor1-timestep
-
-// biquad part, see slide
-//e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
+//----------------------------------------------------END
 
-e_prev = e;
-    
-// Integral
-e_int += Ts*e;
-    
-    
-//PID
-return Kp*e + Ki*e_int + Kd * e_der;
-
+//-------------Get current Position-------------------START
+double motor1_Position(){ // has as output Theta
+    double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta   
+    return pos_m1;       
 }
+double motor2_Position(){ //output R
+    double pos_m2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
+    return pos_m2;
+}
+//-----------------------------------------------------END
 
 
-
-
-
-
+//------------Controller-------------------------------START
 void Controller(){
-    //double x = potMeter1 * potmultiplier;
+    
     double x = Get_X_Position();
-    
     double y = Get_Y_Position();
-    //double y = potMeter2 * potmultiplier;
+     
+    double reference_motor1 = (atan(y/x)*180)/pi;       // reference for Theta
+    double reference_motor2 = sqrt((x*x+y*y));          // reference for radius
     
-    double reference_motor1 = atan(y/x); // reference for Theta
-    double reference_motor2 = sqrt((x*x+y*y));  // reference for radius
+    float pos_M1 = motor1_Position(); // current position for theta   
+    float pos_M2 = motor2_Position(); // current position for the radius
     
-    int position_motor1 = Encoder1.getPulses(); // current position for theta
-    int position_motor2 = Encoder2.getPulses(); // current position for the radius
+    double delta1 = PID1(reference_motor1 - pos_M1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+    double delta2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+
+    double dTheta   = reference_motor1 - pos_M1;
+    double dRadius  = reference_motor2 - pos_M2;
     
-    double motor1 = PID(reference_motor1 - position_motor1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
-    double motor2 = PID(reference_motor2 - position_motor2, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+    
 
     pc.baud(115200);
-    pc.printf("\r Position(X,Y)=(%f,%f), Ref(Theta,R): (%f,%f), Pos(Theta,R):(%i,%i), Motor Value(M1,M2):(%f,%f).\n",x, y, reference_motor1, reference_motor2, position_motor1, position_motor2, motor1, motor2);
+    pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, delta1 ,delta2);
     
     //motor1PWM = motor1;
     //motor2PWM = motor2;
 
-    if(motor1 > 0.3){
-        motor1DC = 1;
+    if(delta1 > 10.0){
+        motor1DC = 0;
         
         ledr = 1;
         ledg = 1;       //Blau
         ledb = 0;
     }
-    else if (motor1<-0.3) {
-        motor1DC = 0; 
+    else if (delta1< -10.0) {
+        motor1DC = 1; 
         
         ledb = 1;
         ledr = 1;
@@ -124,16 +171,21 @@
         ledr = 0;
         ledg = 1;
     }
+    
+    motor1 = abs(delta1)/1000.0f;
+    if(motor1 >= 0.50f) {
+        motor1 = 0.50f;
+    }
 
-if(motor2 > 0.3){
-        motor1DC = 1;
+if(delta2 > 10.0){
+        motor2DC = 0;
         
         ledr = 1;
         ledg = 1;       //Blau
         ledb = 0;
     }
-    else if (motor2<-0.3) {
-        motor1DC = 0; 
+    else if (delta2<-10.0) {
+        motor2DC = 1; 
         
         ledb = 1;
         ledr = 1;
@@ -147,12 +199,36 @@
         ledr = 0;
         ledg = 1;
     }
+    
+    motor2 = abs(delta2)/1000.0f;
+    if(motor1 >= 0.50f) {
+        motor1 = 0.50f;
+    }
+    
+    motor1PWM = motor1 + 0.50f;
+    motor2PWM = motor1 + 0.50f;
+    
+    //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n", , motor1PWM, motor2PWM);
+    //pc.printf("\r 
 }
 
 int main()
 {
     controller.attach(&Controller, M1_TS);
+    //motor1PWM.period(Pwmperiod);
+    //motor2PWM.period(Pwmperiod);
     
-    while(1){}    
+    while(1){
+    /*
+    double x = Get_X_Position();
+    double y = Get_Y_Position(); 
+    double reference_motor1 = atan(y/x);  
+    int position_Motor1 = motor1_Position();  
+    double motor1 = PID(reference_motor1 - position_Motor1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+    
+    pc.baud(115200);
+    pc.printf("\r Position(X)=(%f), Ref(Theta,R): (%f,), Pos(Theta,R):(%i,), Motor Value(M1,M2):(%f,).\n",x, reference_motor1, position_Motor1, motor1);    
+    */   
+    }    
     
 }
\ No newline at end of file