motor.h

#pragma once
 
#include <Eigen/Dense>
#include <memory>
#include <string>
 
#include "proto/robot_status_msg.pb.h"
#include "proto/tbots_software_msgs.pb.h"
#include "shared/constants.h"
#include "shared/robot_constants.h"
#include "software/embedded/constants/constants.h"
#include "software/embedded/gpio.h"
#include "software/embedded/gpio_char_dev.h"
#include "software/embedded/gpio_sysfs.h"
#include "software/embedded/platform.h"
#include "software/physics/euclidean_to_wheel.h"
 
class MotorService
{
   public:
    // SPI Chip Selects
    static const uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT  = 0;
    static const uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3;
    static const uint8_t BACK_LEFT_MOTOR_CHIP_SELECT   = 1;
    static const uint8_t BACK_RIGHT_MOTOR_CHIP_SELECT  = 2;
 
    MotorService(const RobotConstants_t& robot_constants, int control_loop_frequency_hz);
 
    virtual ~MotorService();
 
    TbotsProto::MotorStatus poll(const TbotsProto::MotorControl& motor_control,
                                 double time_elapsed_since_last_poll_s);
 
    uint8_t tmc4671ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer);
    uint8_t tmc6100ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer);
 
    /*
     * For FOC to work, the controller needs to know the electical angle of the rotor
     * relative to the mechanical angle of the rotor. In an incremental-encoder-only
     * setup, we can energize the motor coils so that the rotor locks itself along
     * one of its pole-pairs, allowing us to reset the encoder.
     *
     * WARNING: Do not try to spin the motor without initializing the encoder!
     *          The motor can overheat if the TMC4671 doesn't auto shut-off.
     *
     *          There are some safety checks to ensure that the encoder is
     *          initialized, do not tamper with them. You have been warned.
     *
     * @param motor The motor to initialize the encoder for
     */
    void startEncoderCalibration(uint8_t motor);
    void endEncoderCalibration(uint8_t motor);
 
    void runOpenLoopCalibrationRoutine(uint8_t motor, size_t num_samples);
 
    void resetMotorBoard();
 
    void setup();
 
    struct MotorFaultIndicator
    {
        bool drive_enabled;
        std::unordered_set<TbotsProto::MotorFault> motor_faults;
 
        MotorFaultIndicator() : drive_enabled(true), motor_faults() {}
 
        MotorFaultIndicator(bool drive_enabled,
                            std::unordered_set<TbotsProto::MotorFault>& motor_faults)
            : drive_enabled(drive_enabled), motor_faults(motor_faults)
        {
        }
    };
 
    struct MotorFaultIndicator checkDriverFault(uint8_t motor);
 
    void setUpDriveMotor(uint8_t motor);
 
    void writeIntToTMC4671(uint8_t motor, uint8_t address, int32_t value);
 
    int readIntFromTMC4671(uint8_t motor, uint8_t address);
 
   private:
    void motorServiceInit(const RobotConstants_t& robot_constants,
                          int control_loop_frequency_hz);
 
    void startDriver(uint8_t motor);
    void startController(uint8_t motor, bool dribbler);
 
    void configurePWM(uint8_t motor);
    void configureDribblerPI(uint8_t motor);
    void configureDrivePI(uint8_t motor);
    void configureADC(uint8_t motor);
    void configureEncoder(uint8_t motor);
    void configureHall(uint8_t motor);
 
    void writeToControllerOrDieTrying(uint8_t motor, uint8_t address, int32_t value);
    void writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value);
 
    void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len,
                     uint32_t spi_speed);
 
    void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, uint8_t const* write_tx,
                                  uint8_t const* read_rx, uint32_t spi_speed);
 
    int32_t tmc4671ReadThenWriteValue(uint8_t motor, uint8_t read_addr,
                                      uint8_t write_addr, int32_t write_data);
 
    uint8_t readWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer,
                          uint32_t spi_speed);
 
 
    void checkEncoderConnections();
 
    TbotsProto::MotorStatus updateMotorStatus(double front_left_velocity_mps,
                                              double front_right_velocity_mps,
                                              double back_left_velocity_mps,
                                              double back_right_velocity_mps,
                                              double dribbler_rpm);
 
    template <typename T>
    static std::unique_ptr<Gpio> setupGpio(const T& gpio_number, GpioDirection direction,
                                           GpioState initial_state);
 
    bool requiresMotorReinit(uint8_t motor);
 
    // All trinamic RPMS are electrical RPMS, they don't factor in the number of pole
    // pairs of the drive motor.
    //
    // TODO (#2720): compute from robot constants (this was computed by hand and is
    // accurate)
    static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 0.000111;
    static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS =
        1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM;
 
    // to check if the motors have been calibrated
    bool is_initialized_ = false;
 
    // Select between driver and controller gpio
    std::unique_ptr<Gpio> spi_demux_select_0_;
    std::unique_ptr<Gpio> spi_demux_select_1_;
 
    // Enable driver gpio
    std::unique_ptr<Gpio> driver_control_enable_gpio_;
    std::unique_ptr<Gpio> reset_gpio_;
 
    // Transfer Buffers for spiTransfer
    uint8_t tx_[5] = {0};
    uint8_t rx_[5] = {0};
 
    // Transfer Buffers for readThenWriteSpiTransfer
    uint8_t write_tx_[5] = {0};
    uint8_t read_tx_[5]  = {0};
    uint8_t read_rx_[5]  = {0};
 
    // Transfer State
    bool transfer_started_  = false;
    bool currently_writing_ = false;
    bool currently_reading_ = false;
    uint8_t position_       = 0;
 
    // SPI File Descriptors
    std::unordered_map<int, int> file_descriptors_;
 
    RobotConstants_t robot_constants_;
 
    // Drive Motors
    EuclideanToWheel euclidean_to_four_wheel_;
    std::unordered_map<int, bool> encoder_calibrated_;
    std::unordered_map<int, MotorFaultIndicator> cached_motor_faults_;
 
    // Previous wheel velocities
    WheelSpace_t prev_wheel_velocities_;
 
    int front_left_target_rpm  = 0;
    int front_right_target_rpm = 0;
    int back_left_target_rpm   = 0;
    int back_right_target_rpm  = 0;
 
    // the motor cs id to check for motor faults
    uint8_t motor_fault_detector_;
 
    static const int NUM_CALIBRATION_ATTEMPTS = 10;
 
    int dribbler_ramp_rpm_;
 
    std::optional<std::chrono::time_point<std::chrono::system_clock>>
        tracked_motor_fault_start_time_;
    int num_tracked_motor_resets_;
 
    static const uint8_t DRIBBLER_MOTOR_CHIP_SELECT = 4;
 
    static const uint8_t NUM_DRIVE_MOTORS = 4;
    static const uint8_t NUM_MOTORS       = NUM_DRIVE_MOTORS + 1;
 
    static const int MOTOR_FAULT_TIME_THRESHOLD_S = 60;
    static const int MOTOR_FAULT_THRESHOLD_COUNT  = 3;
 
    // SPI Trinamic Motor Driver Paths (indexed with chip select above)
    static constexpr const char* SPI_PATHS[] = {"/dev/spidev0.0", "/dev/spidev0.1",
                                                "/dev/spidev0.2", "/dev/spidev0.3",
                                                "/dev/spidev0.4"};
 
    // Motor names (indexed with chip select above)
    static constexpr const char* MOTOR_NAMES[] = {"front_left", "back_left", "back_right",
                                                  "front_right", "dribbler"};
};
 
template <typename T>
std::unique_ptr<Gpio> MotorService::setupGpio(const T& gpio_number,
                                              GpioDirection direction,
                                              GpioState initial_state)
{
    if constexpr (PLATFORM == Platform::JETSON_NANO)
    {
        return std::make_unique<GpioSysfs>(gpio_number, direction, initial_state);
    }
    else
    {
        return std::make_unique<GpioCharDev>(gpio_number, direction, initial_state,
                                             "/dev/gpiochip4");
    }
}