odometry_base.h

#pragma once
 
// These are required for Eigen to compile
// https://www.vexforum.com/t/eigen-integration-issue/61474/5
#undef __ARM_NEON__
#undef __ARM_NEON
#include <Eigen/Dense>
 
#include "core/competition/command_structure/auto_command.h"
#include "core/math/geometry/pose2d.h"
#include "vex/vex.h"
#include "core/math/kinematics/chassis_velocities.h"
#include "core/math/kinematics/chassis_accelerations.h"

class OdometryBase {
 public:
  OdometryBase(bool is_async);

  virtual Pose2d get_position(void);

  virtual void set_position(const Pose2d& newpos = zero_pos);
  AutoCommand* SetPositionCmd(const Pose2d& newpos = zero_pos);
  virtual Pose2d update() = 0;

  static int background_task(void* ptr);

  void end_async();

  static double smallest_angle(double start_deg, double end_deg);

  bool end_task = false;

  virtual double get_speed();

  virtual double get_accel();

  double get_angular_speed_deg();

  double get_angular_accel_deg();

  ChassisVelocities get_chassis_velocity();

  ChassisAccelerations get_chassis_accel();
 
  inline static constexpr Pose2d zero_pos = Pose2d();

  vex::task* handle;

  vex::mutex mut;

  Pose2d current_pos;
 
  double speed;         
  double accel;         
  double ang_speed_deg; 
  double ang_accel_deg; 
};