base_motor.h

#pragma once
#include "../../control/JacobianVelController.h"
#include "../../utils/scheduler.h"
#include "../data.h"

namespace robot {
class base_motor {
public:
    base_motor(robot::types::motorid_t motor, bool hasPosSensor);

    bool hasPositionSensor() const;

    virtual void setMotorPower(double power) = 0;

    virtual void setMotorPos(int32_t targetPos) = 0;

    virtual types::DataPoint<int32_t> getMotorPos() const = 0;

    virtual void setMotorVel(int32_t targetVel);
 
    virtual void setServoPos(uint8_t servo, int32_t position) = 0;
 
  virtual void setStepperTurnAngle(uint8_t stepper, int16_t angle) = 0;

    base_motor(const base_motor&) = delete;

    base_motor& operator=(const base_motor&) = delete;
 
protected:
    robot::types::motorid_t motor_id;
    bool posSensor;
    std::optional<util::PeriodicScheduler<std::chrono::steady_clock>::eventid_t> velEventID;
    std::optional<JacobianVelController<1, 1>> velController;
 
    inline static std::optional<util::PeriodicScheduler<std::chrono::steady_clock>> pSched;
    inline static std::mutex schedulerMutex;

    void constructVelController();

    void unscheduleVelocityEvent();
}; // abstract class base_motor
} // namespace robot