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/robot_specs.h"
#include "core/utils/command_structure/auto_command.h"
#include "core/utils/math/geometry/pose2d.h"
#include "vex.h"
 
#ifndef PI
#define PI 3.141592654
#endif

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();
 
    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; 
};