aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: odometry/odom.cpp
- Revision:
- 8:80708bacb5b5
- Parent:
- 6:20a32baeff79
- Child:
- 10:3b47050b1652
diff -r b240464868e8 -r 80708bacb5b5 odometry/odom.cpp --- a/odometry/odom.cpp Tue Dec 18 22:06:34 2018 +0000 +++ b/odometry/odom.cpp Thu Dec 20 20:54:35 2018 +0000 @@ -15,6 +15,7 @@ } offset_angle_ = 0; + enable_oled_ = true; //Set TIM IT Callback it_tick = new Ticker; @@ -22,14 +23,17 @@ } void Odom::calc_odometry(){ - velocity_.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); - velocity_.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); + Vec3f velocity; + Vec3f world_velocity; + + velocity.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); + velocity.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); - world_velocity_.x(((velocity_.x() * cosf(-world_.angle())) - (velocity_.y() * sinf(-world_.angle())))); - world_velocity_.y(((velocity_.x() * sinf(-world_.angle())) + (velocity_.y() * cosf(-world_.angle())))); + world_velocity.x(((velocity.x() * cosf(-odom_.angle())) - (velocity.y() * sinf(-odom_.angle())))); + world_velocity.y(((velocity.x() * sinf(-odom_.angle())) + (velocity.y() * cosf(-odom_.angle())))); - world_.x(world_.x() + world_velocity_.x() * 0.02f); - world_.y(world_.y() + world_velocity_.y() * 0.02f); + odom_.x(odom_.x() + world_velocity.x() * 0.02f); + odom_.y(odom_.y() + world_velocity.y() * 0.02f); } void Odom::update_angle(){ @@ -58,7 +62,7 @@ } //オフセットを考慮して角度を設定 - world_.angle(actual_angle_ - offset_angle_); + odom_.angle(actual_angle_ - offset_angle_); //値更新 prev_angle = z_angle;