CANMotor.h

#pragma once
 
#include "../world_interface/data.h"
#include "CAN.h"
#include "CANUtils.h"
 
#include <chrono>
#include <optional>

namespace can::motor {

enum class motormode_t {
    pwm = MOTOR_UNIT_MODE_PWM,
    pid = MOTOR_UNIT_MODE_PID
};

struct sensor_t {
    enum {
        encoder = 0,
        potentiometer = 1
    };
};

void emergencyStopMotors();

void initMotor(devicegroup_t group, deviceserial_t serial);

void initEncoder(devicegroup_t group, deviceserial_t serial, bool invertEncoder,
                 bool zeroEncoder, int32_t pulsesPerJointRev,
                 std::optional<std::chrono::milliseconds> telemetryPeriod);

void setLimitSwitchLimits(devicegroup_t group, deviceserial_t serial, int32_t lo, int32_t hi);

void initPotentiometer(devicegroup_t group, deviceserial_t serial, int32_t posLo,
                       int32_t posHi, uint16_t adcLo, uint16_t adcHi,
                       std::optional<std::chrono::milliseconds> telemetryPeriod);

void setMotorPIDConstants(devicegroup_t group, deviceserial_t serial, int32_t kP, int32_t kI,
                          int32_t kD);

void setMotorMode(devicegroup_t group, deviceserial_t serial, motormode_t mode);

void setMotorPower(devicegroup_t group, deviceserial_t serial, double power);

void setMotorPower(devicegroup_t group, deviceserial_t serial, int16_t power);

void setMotorPIDTarget(devicegroup_t group, deviceserial_t serial, int32_t target);

void setServoPos(devicegroup_t group, deviceserial_t serial, uint8_t servoNum, int32_t angle);
 
void setStepperTurnAngle(devicegroup_t group, deviceserial_t serial, uint8_t stepper, int16_t angle);

robot::types::DataPoint<int32_t> getMotorPosition(devicegroup_t group, deviceserial_t serial);

void pullMotorPosition(devicegroup_t group, deviceserial_t serial);

callbackid_t addLimitSwitchCallback(
    devicegroup_t group, deviceserial_t serial,
    const std::function<void(
        devicegroup_t group, deviceserial_t serial,
        robot::types::DataPoint<robot::types::LimitSwitchData> limitSwitchData)>& callback);

void removeLimitSwitchCallback(callbackid_t id);
} // namespace can::motor