James Kiwic
/
RT2_P3_students
Fertig
Fork of RT2_P3_students by
Diff: main.cpp
- Revision:
- 12:1aa5ff8333e8
- Parent:
- 11:a0074351e154
- Child:
- 13:724759951a6f
--- a/main.cpp Mon May 07 06:14:56 2018 +0000 +++ b/main.cpp Mon May 07 09:00:43 2018 +0000 @@ -52,9 +52,10 @@ // ... here define variables like gains etc. //------------------------------------------ //LinearCharacteristics i2u(0.8f,-2.0f); -LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f); -LinearCharacteristics u2ax(0.69670f,0.29817,-9.81f,9.81f); -LinearCharacteristics u2ay(0.69988f,0.29792,-9.81f,9.81f); +//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); @@ -66,7 +67,8 @@ //------------------------------------------ // ... here define instantiate classes //------------------------------------------ -PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.4f); +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; IIR_filter LPF_gyro(tau,Ts,tau); @@ -114,9 +116,11 @@ float accX = LPF_accX.filter(u2ax(ax.read())); float accY = LPF_accY.filter(u2ay(ay.read())); - float phi_mes = (PI/4) + atan2(-accX, accY) + gyroV; + // komplementärfilter + float phi_mes = ((PI/4) + atan2(-accX, accY) + gyroV); - float torque = -(-3.7f*phi_mes - 0.42f * gyro_dir); + // regler + float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro_dir); out.write(i2u(torque/0.217f)/3.3f); @@ -136,4 +140,6 @@ // pressed button //****************************************************************************** -//... \ No newline at end of file +//... + +