odometry_3wheel.h

#pragma once
#include "../core/include/subsystems/custom_encoder.h"
#include "../core/include/subsystems/odometry/odometry_base.h"
#include "../core/include/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);
 
  pose_t update() override;
 
  void tune(vex::controller &con, TankDrive &drive);
 
private:
  static pose_t calculate_new_pos(double lside_delta_deg, double rside_delta_deg, double offax_delta_deg,
                                  pose_t old_pos, odometry3wheel_cfg_t cfg);
 
  CustomEncoder &lside_fwd, &rside_fwd, &off_axis;
  odometry3wheel_cfg_t &cfg;
};