Fertig
Dependencies: mbed
Fork of RT2_P3_students by
Diff: main.cpp
- Revision:
- 13:724759951a6f
- Parent:
- 12:1aa5ff8333e8
diff -r 1aa5ff8333e8 -r 724759951a6f main.cpp --- a/main.cpp Mon May 07 09:00:43 2018 +0000 +++ b/main.cpp Mon May 07 09:06:54 2018 +0000 @@ -52,33 +52,28 @@ // ... here define variables like gains etc. //------------------------------------------ //LinearCharacteristics i2u(0.8f,-2.0f); -//LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f); // schwach parametrierung -LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f); // starke parametrierung +//LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f); // schwach parametrierung +LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f); // starke parametrierung LinearCharacteristics u2ax(0.69630f,0.29792,9.81f,-9.81f); LinearCharacteristics u2ay(0.70012f,0.29750,9.81f,-9.81f); LinearCharacteristics u2gyro(-4.65f,1.5f); - - //------------------------------------------ Ticker ControllerLoopTimer; // interrupt for control loop EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7 DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data + //------------------------------------------ // ... here define instantiate classes //------------------------------------------ PI_Cntrl vel_cntrl(0.5f,0.05f,Ts,0.4f); PI_Cntrl omega2zero(-0.002f,2.0f,Ts,0.5); -float tau = 1.0f; +float tau = 1.0f; //Zeitkonstante Filter IIR_filter LPF_gyro(tau,Ts,tau); IIR_filter LPF_accX(tau,Ts,1.0f); IIR_filter LPF_accY(tau,Ts,1.0f); - -//GPA gpa1(1.0f, 200.0f, 150, 4, 400, Ts, 10.0f, 0.3f); -//float excWobble = 0.0f; -// GPA(t fMin, t fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1) // ... define some linear characteristics ----------------------------------------- // ----- User defined functions ----------- @@ -104,15 +99,10 @@ void updateControllers(void){ short counts = counter1; // get counts from Encoder float vel = diff(counts); // motor velocity - //desTorque = vel_cntrl(w_soll - vel); - /* outWobble = omega; - excWobble = Wobble(excWobble, outWobble); */ - //float torq_des = vel_cntrl(w_soll - vel); - //out.write(i2u(torq_des/0.217f)); // the controller! convert torque to Amps km = 0.217 Nm/A - - float gyro_dir = (u2gyro(gz.read()*3.3f)); - float gyroV = LPF_gyro.filter(gyro_dir); + // filterung der Input Daten + float gyro = (u2gyro(gz.read()*3.3f)); + float gyroV = LPF_gyro.filter(gyro); float accX = LPF_accX.filter(u2ax(ax.read())); float accY = LPF_accY.filter(u2ay(ay.read())); @@ -120,17 +110,14 @@ float phi_mes = ((PI/4) + atan2(-accX, accY) + gyroV); // regler - float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro_dir); + float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro); out.write(i2u(torque/0.217f)/3.3f); // OUTPUT if(++k >= 150){ k = 0; - //pc.printf("Des. velocity: %3.3f, Velocity: %3.3f\r\n",w_soll,vel); pc.printf("Uax val %3.5f; ax val %3.5f; Uay val %3.5f; ay val %3.5f; gyro %3.5f\r\n",ax.read(),u2ax(ax.read()),ay.read(),u2ay(ay.read()), gz.read()); - //pc.printf("Gyro: %3.3f rad/s\r\n",gyroCalc(gz.read())); - //pc.printf("Gyro filt: %3.3f, Gyro roh; %3.3f, accx filt: %3.3f, accy filt: %3.3f\r\n", gyroV,gz.read()*3.3f, accX, accY); pc.printf("Phii: %3.3f\r\n", phi_mes); } }