odometry_tank.h

#pragma once
 
#include "../core/include/subsystems/custom_encoder.h"
#include "../core/include/subsystems/odometry/odometry_base.h"
#include "../core/include/utils/geometry.h"
#include "../core/include/utils/moving_average.h"
#include "../core/include/utils/vector2d.h"
 
#include "../core/include/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);
 
  pose_t update() override;
 
  void set_position(const pose_t &newpos = zero_pos) override;
 
private:
  static pose_t calculate_new_pos(robot_specs_t &config, pose_t &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);
};