odometry_base.h

#pragma once
 
#include "../core/include/robot_specs.h"
#include "../core/include/utils/command_structure/auto_command.h"
#include "../core/include/utils/geometry.h"
#include "vex.h"
 
#ifndef PI
#define PI 3.141592654
#endif
 
class OdometryBase {
public:
  OdometryBase(bool is_async);
 
  pose_t get_position(void);
 
  virtual void set_position(const pose_t &newpos = zero_pos);
  AutoCommand *SetPositionCmd(const pose_t &newpos = zero_pos);
  virtual pose_t update() = 0;
 
  static int background_task(void *ptr);
 
  void end_async();
 
  static double pos_diff(pose_t start_pos, pose_t end_pos);
 
  static double rot_diff(pose_t pos1, pose_t pos2);
 
  static double smallest_angle(double start_deg, double end_deg);
 
  bool end_task = false;
 
  double get_speed();
 
  double get_accel();
 
  double get_angular_speed_deg();
 
  double get_angular_accel_deg();
 
  inline static constexpr pose_t zero_pos = {.x = 0.0L, .y = 0.0L, .rot = 90.0L};
 
protected:
  vex::task *handle;
 
  vex::mutex mut;
 
  pose_t current_pos;
 
  double speed;         
  double accel;         
  double ang_speed_deg; 
  double ang_accel_deg; 
};