54 lines
1.2 KiB
C++
54 lines
1.2 KiB
C++
#ifndef COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
|
|
#define COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
|
|
|
|
|
|
#include <cstdint>
|
|
#include <array>
|
|
#include <thread>
|
|
#include <mutex>
|
|
|
|
#include "include/PID.hpp"
|
|
#include "include/Robot.hpp"
|
|
|
|
class ClosedLoopMotorController {
|
|
public:
|
|
static ClosedLoopMotorController &getInstance() {
|
|
static ClosedLoopMotorController instance;
|
|
return instance;
|
|
}
|
|
|
|
ClosedLoopMotorController(ClosedLoopMotorController const &) = delete;
|
|
|
|
void operator=(ClosedLoopMotorController const &) = delete;
|
|
|
|
void set_power(uint8_t port, double power);
|
|
|
|
void set_speed(uint8_t port, double speed_rad_s);
|
|
|
|
void generate_step_response_data();
|
|
|
|
void generate_tuned_step_response_data();
|
|
|
|
void calibrate_wheel_ticks(double turns, double ticks_per_turn);
|
|
|
|
private:
|
|
enum ControlMode : uint8_t {
|
|
NONE = 0,
|
|
POWER = 1,
|
|
SPEED = 2
|
|
};
|
|
|
|
ClosedLoopMotorController();
|
|
|
|
[[noreturn]] void speed_control_loop();
|
|
|
|
std::array<PID, ROBOT_MOTOR_COUNT> pids;
|
|
std::array<ControlMode, ROBOT_MOTOR_COUNT> control_modes{ControlMode::NONE};
|
|
std::array<double, ROBOT_MOTOR_COUNT> speed_targets{0};
|
|
|
|
std::thread speed_control_thread;
|
|
std::recursive_mutex speed_control_mutex;
|
|
};
|
|
|
|
|
|
#endif //COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
|