odometry_3wheel.h

#pragma once
#include "core/subsystems/custom_encoder.h"
#include "core/subsystems/odometry/odometry_base.h"
#include "core/subsystems/tank_drive.h"

class Odometry3Wheel : public OdometryBase {
public:
  typedef struct {
    double wheelbase_dist;       
    double off_axis_center_dist; 
    double wheel_diam;           
 
  } odometry3wheel_cfg_t;

  Odometry3Wheel(CustomEncoder &lside_fwd, CustomEncoder &rside_fwd, CustomEncoder &off_axis, odometry3wheel_cfg_t &cfg,
                 bool is_async = true);

  Pose2d update() override;

  void tune(vex::controller &con, TankDrive &drive);
 
private:
  static Pose2d calculate_new_pos(double lside_delta_deg, double rside_delta_deg, double offax_delta_deg,
    Pose2d old_pos, odometry3wheel_cfg_t cfg);
 
  CustomEncoder &lside_fwd, &rside_fwd, &off_axis;
  odometry3wheel_cfg_t &cfg;
};