![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
....
Dependencies: Library_Cntrl Library_Misc_cuboid
Fork of cuboid_balance_ros by
angle_estimator.cpp@0:acf871f26563, 2019-03-08 (annotated)
- Committer:
- altb2
- Date:
- Fri Mar 08 13:34:59 2019 +0000
- Revision:
- 0:acf871f26563
...
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:acf871f26563 | 1 | #include "angle_estimator.h" |
altb2 | 0:acf871f26563 | 2 | #include "mbed.h" |
altb2 | 0:acf871f26563 | 3 | |
altb2 | 0:acf871f26563 | 4 | #define PI 3.1415927 |
altb2 | 0:acf871f26563 | 5 | |
altb2 | 0:acf871f26563 | 6 | |
altb2 | 0:acf871f26563 | 7 | using namespace std; |
altb2 | 0:acf871f26563 | 8 | |
altb2 | 0:acf871f26563 | 9 | angle_estimator::angle_estimator(float Ts) : ax(PA_0), ay(PA_1), gz(PA_4){ |
altb2 | 0:acf871f26563 | 10 | u2ax.setup(0.29719f,0.69768f,-9.81f,9.81f); // use normalized input |
altb2 | 0:acf871f26563 | 11 | u2ay.setup(0.29890,0.70330f,-9.81f,9.81f); // use normalized input |
altb2 | 0:acf871f26563 | 12 | u2gz.setup(-4.6517f * 3.3f,0.45495f); // use normalized input |
altb2 | 0:acf871f26563 | 13 | // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180 |
altb2 | 0:acf871f26563 | 14 | float tau = 1.0f; |
altb2 | 0:acf871f26563 | 15 | f_ax.setup(tau,Ts,1.0); |
altb2 | 0:acf871f26563 | 16 | f_ay.setup(tau,Ts,1.0); |
altb2 | 0:acf871f26563 | 17 | f_gz.setup(tau,Ts,tau); |
altb2 | 0:acf871f26563 | 18 | |
altb2 | 0:acf871f26563 | 19 | } |
altb2 | 0:acf871f26563 | 20 | |
altb2 | 0:acf871f26563 | 21 | |
altb2 | 0:acf871f26563 | 22 | angle_estimator::~angle_estimator() {} |
altb2 | 0:acf871f26563 | 23 | |
altb2 | 0:acf871f26563 | 24 | |
altb2 | 0:acf871f26563 | 25 | |
altb2 | 0:acf871f26563 | 26 | // ************ calculate angle ***************** |
altb2 | 0:acf871f26563 | 27 | void angle_estimator::calc_angle_phi1(int do_reset){ |
altb2 | 0:acf871f26563 | 28 | gyro = u2gz(gz.read()); |
altb2 | 0:acf871f26563 | 29 | if(do_reset == 1){ |
altb2 | 0:acf871f26563 | 30 | f_ax.reset(u2ax(ax.read())); |
altb2 | 0:acf871f26563 | 31 | f_ay.reset(u2ax(ay.read())); |
altb2 | 0:acf871f26563 | 32 | f_gz.reset(gyro); |
altb2 | 0:acf871f26563 | 33 | } |
altb2 | 0:acf871f26563 | 34 | phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro); |
altb2 | 0:acf871f26563 | 35 | } |
altb2 | 0:acf871f26563 | 36 | |
altb2 | 0:acf871f26563 | 37 | float angle_estimator::get_phi1(void){ |
altb2 | 0:acf871f26563 | 38 | return phi1; |
altb2 | 0:acf871f26563 | 39 | } |
altb2 | 0:acf871f26563 | 40 | float angle_estimator::get_gyro(void){ |
altb2 | 0:acf871f26563 | 41 | return gyro; |
altb2 | 0:acf871f26563 | 42 | } |