![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
main.cpp
- Committer:
- Arnoud113
- Date:
- 2017-11-02
- Revision:
- 14:54cbd8f0efe4
- Parent:
- 13:b5868fd8ffe9
- Child:
- 15:65f295a49a4b
File content as of revision 14:54cbd8f0efe4:
#include "mbed.h" #include "QEI.h" #include "MODSERIAL.h" #include "math.h" #include "FastPWM.h" #include "encoder.h" #include "BiQuad.h" DigitalOut gpo(D0); DigitalOut ledb(LED_BLUE); DigitalOut ledr(LED_RED); DigitalOut ledg(LED_GREEN); DigitalOut motor1DC(D7); DigitalOut motor2DC(D4); FastPWM motor1PWM(D6); FastPWM motor2PWM(D5); AnalogIn potMeter1(A0); AnalogIn potMeter2(A1); AnalogIn emg1(A2); AnalogIn emg2(A3); AnalogIn emg3(A4); AnalogIn emg4(A5); DigitalIn button1(D11); DigitalIn button2(D12); DigitalIn button3(SW2); DigitalIn button4(SW3); Encoder Encoder1(D12,D13); Encoder Encoder2(D8,D9); MODSERIAL pc(USBTX,USBRX); Ticker controller; // Constants EMG ----------------------- Start double X; double X_maxTreshold = 480; double X_minTreshold = 20; const double X_Callibrate = 300; double Y; double Y_maxTreshold = 480; double Y_minTreshold = 20; const double Y_Callibrate = 300; // ---- Motor Constants------- const double pi = 3.1415926535897; float Pwmperiod = 0.001f; int potmultiplier = 600; // Multiplier for the pot meter reference which is normally between 0 and 1 const float gearM1 = 6.2; const double gainM1 = 1/38.17; // encoder pulses per degree theta const double gainM2 = 1/100.8; // pulses per mm double motor1; double motor2; double reference_motor1 = 0; // reference for Theta double reference_motor2 = 0; double pos_M1 = 0; // start angle theta double pos_M2 = 0; // start radius //Start constants PID ------------------------------- const double M1_TS = 0.01; // (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 CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ 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 = 30; 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; // Constants Biquad BiQuad bq1(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01); BiQuad bq2(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01); BiQuadChain bqc; 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; //---------------------------------End of constants 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; // Ts = motor1-timestep // Derivative e_der1 = bqc.step(e_der1); // biquad part, see slide //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); e_prev1 = e1; e_int1 += Ts*e1; if(button3 == 0 || button4 ==0){ e1 = 0; e_prev1 = 0; e_int1 = 0; e_der1 = 0; } // Integral return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1; //PID } 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; // Ts = motor1-timestep // Derivative // biquad part, see slide //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); e_der2 = bqc.step(e_der2); e_prev2 = e2; e_int2 += Ts*e2; if(button3 == 0 || button4 ==0){ e2 = 0; e_prev2 = 0; e_int2 = 0; e_der2 = 0; } // Integral return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2; //PID } //------------Get reference position-----------------START float Get_X_Position(){ // X = potMeter1 * potmultiplier; //--------- for Potmerter use // -- Potmeter use ----------------------------------- if (potMeter1 < 0.3) { X = X-0.5; } else if (potMeter1> 0.7) { X = X+0.5; } else { X = X; } /* double in1 = emg1.read(); double in2 = emg2.read(); double RA = in1+in2; if (RA < 0.5) { X = X; } else if (RA > 1.5) { X = X-0.1; } else { X = X+0.1; } */ if (X >= X_maxTreshold){ X = X_maxTreshold; } else if (X <= X_minTreshold){ X = X_minTreshold; } else{ X = X; } if(button3 == 0){ X = X_minTreshold; } else if (button4 == 0){ X = X_Callibrate; } else{ X = X; } //pc.baud(115200); //pc.printf("\r (in1,in2):(%f,%f), RA = %f, X = %f \n",in1, in2, RA, X); return X; } float Get_Y_Position(){ //Y = potMeter2 * potmultiplier; //--------- for Potmerter use // ---- Potmeter Use-------------------------- if (potMeter2 < 0.3) { Y = Y-0.5; } else if (potMeter2 > 0.7) { Y = Y+0.5; } else { Y = Y; } /* double in3 = emg3.read(); double in4 = emg4.read(); double LA = in3+in4; if (LA < 0.5) { Y = Y; } else if (LA > 1.5) { Y = Y-0.1; } else { Y = Y+0.1; } */ if (Y >= Y_maxTreshold){ Y = Y_maxTreshold; } else if (Y <= Y_minTreshold){ Y = Y_minTreshold; } else{ Y = Y; } if(button3 == 0){ Y = Y_minTreshold; } else if (button4 == 0){ Y = Y_Callibrate; } else{ Y = Y; } //pc.printf("\r (in3,in4):(%f,%f), LA = %f, Y = %f \n",in3, in4, LA, Y); return Y; } //----------------------------------------------------END //-------------Get current Position-------------------START double motor1_Position(){ // has as output Theta if (button3 == 0){ int T1 = ((atan(Y_minTreshold/X_minTreshold)*180)/pi)/gainM1; Encoder1.setPosition(T1); } else if (button4 ==0){ int T1 = ((atan(Y_Callibrate/X_Callibrate)*180)/pi)/gainM1; Encoder1.setPosition(T1); } else{ } double pos_M1 = gainM1*Encoder1.getPosition(); // current position for theta double countM1 = Encoder1.getPosition(); pc.baud(115200); pc.printf("\r counts encoder: %f\n",countM1); return pos_M1; } double motor2_Position(){ //output R int R1; if (button3 == 0){ R1 = (sqrt(X_minTreshold*X_minTreshold+Y_minTreshold*Y_minTreshold))/gainM2; Encoder2.setPosition(R1); } else if (button4 ==0){ R1 = (sqrt(X_Callibrate*X_Callibrate+Y_Callibrate*Y_Callibrate))/gainM2; Encoder2.setPosition(R1); } else{ } double pos_M2 = gainM2 *Encoder2.getPosition(); // current position for the radius; pc.baud(115200); // pc.printf("\r R1 = %f, pos_m2 = %f\n", R1,pos_m2); return pos_M2; } //-----------------------------------------------------END //------------Controller-------------------------------START void Controller(){ double x = Get_X_Position(); double y = Get_Y_Position(); reference_motor1 = (atan(y/x)*180)/pi; // reference for Theta reference_motor2 = sqrt((x*x+y*y)); // reference for radius pos_M1 = motor1_Position(); // current position for theta pos_M2 = motor2_Position(); // 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, m2_err_int, m2_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; pc.baud(115200); //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (delta1,delta2):(%f,%f)\n",x,y, dTheta ,dRadius,delta1, delta2); //pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2); //motor1PWM = motor1; //motor2PWM = motor2; if(delta1 > 0.5){ motor1DC = 0; ledr = 1; ledg = 1; //Blau ledb = 0; } else if (delta1< -0.5) { motor1DC = 1; ledb = 1; ledr = 1; ledg = 0; //Groen } else{ motor1PWM = 0; ledb = 1; //Rood ledr = 0; ledg = 1; } motor1 = abs(delta1)/1000.0; if(motor1 >= 0.20) { motor1 = 0.20; //pc.baud(115200); //pc.printf("\r val motor1: %f\n", motor1); } if(delta2 > 2.0){ motor2DC = 0; ledr = 1; ledg = 1; //Blau ledb = 0; } else if (delta2<-2.0) { motor2DC = 1; ledb = 1; ledr = 1; ledg = 0; //Groen } else{ motor2PWM = 0; ledb = 1; //Rood ledr = 0; ledg = 1; } motor2 = abs(delta2)/1000.0; if(motor2 >= 0.50) { motor2 = 0.50; } motor1PWM = motor1 + 0.80; motor2PWM = motor2 + 0.50; //pc.printf("\r delta(1,2):(%f,%f)\n", delta1,delta2); //pc.printf("\r motorvalues (M1,M2):(%f,%f),\n", motor1 + 0.65, motor2 + 0.20); //pc.printf("\r } int main() { controller.attach(&Controller, M1_TS); motor1PWM.period(Pwmperiod); motor2PWM.period(Pwmperiod); bqc.add(&bq1).add(&bq2); 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); */ } }