odometry_tank.h

#pragma once
 
#include "core/robot_specs.h"
#include "core/subsystems/custom_encoder.h"
#include "core/subsystems/odometry/odometry_base.h"
#include "core/utils/moving_average.h"
 
// static int background_task(void *odom_obj);
class OdometryTank : public OdometryBase {
   public:
    OdometryTank(
            vex::motor_group& left_side, vex::motor_group& right_side, robot_specs_t& config,
            vex::inertial* imu = NULL, bool is_async = true
    );

 
    OdometryTank(
            CustomEncoder& left_custom_enc, CustomEncoder& right_custom_enc, robot_specs_t& config,
            vex::inertial* imu = NULL, bool is_async = true
    );

 
    OdometryTank(
            vex::encoder& left_vex_enc, vex::encoder& right_vex_enc, robot_specs_t& config,
            vex::inertial* imu = NULL, bool is_async = true
    );

    Pose2d update() override;

    void set_position(const Pose2d& newpos = zero_pos) override;
 
   private:
    static Pose2d calculate_new_pos(
            robot_specs_t& config, Pose2d& stored_info, double lside_diff, double rside_diff,
            double angle_deg
    );
 
    vex::motor_group *left_side, *right_side;
    CustomEncoder *left_custom_enc, *right_custom_enc;
    vex::encoder *left_vex_enc, *right_vex_enc;
    vex::inertial* imu;
    robot_specs_t& config;
 
    double rotation_offset = 0;
    ExponentialMovingAverage ema = ExponentialMovingAverage(3);
};