odometry_tank.h

#pragma once
 
#include "core/subsystems/custom_encoder.h"
#include "core/subsystems/odometry/odometry_base.h"
#include "core/utils/geometry.h"
#include "core/utils/moving_average.h"
 
#include "core/robot_specs.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);
};