first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 3:b353ee86230a
- Parent:
- 2:2563d1d8461f
- Child:
- 4:5f7d1654108d
diff -r 2563d1d8461f -r b353ee86230a main.cpp --- 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