comm_controller.hpp

Go to the documentation of this file.

#pragma once
#include <Arduino.h>
#include "ODriveCAN.h"
#include <FlexCAN_T4.h>
#include "ODriveFlexCAN.hpp"
#include "teensy_can.h"
#include "defines.h"
#include "odrive_utils/odrive_msgs.hpp"
#include "controllers/odrive_controller.hpp"
 
enum ControllerState : uint8_t
{
    INITIALIZING,
    READY,
    RUNNING,
    ERROR
};
 
template <CAN_DEV_TABLE ODRIVE_CAN0, CAN_DEV_TABLE ODRIVE_CAN1>
class CommsController
{
public:
    CommsController() = default;
 
    CommsController(
        VirtualTimerGroup &timer_group,
        FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_256> &odrive_can0,
        FlexCAN_T4<CAN3, RX_SIZE_256, TX_SIZE_256> &odrive_can1,
        OdriveController<CAN2> &odrv0,
        OdriveController<CAN3> &odrv1,
        _MB_ptr F,
        ICAN &teensy_can)
        : timer_group(timer_group),
          odrive_can0_(odrive_can0),
          odrive_can1_(odrive_can1),
          odrv0_(odrv0),
          odrv1_(odrv1),
          teensy_can(teensy_can)
    {
        // Init Odrive CAN
        odrive_can0_.begin();
        odrive_can0_.setBaudRate(CAN_BAUDRATE);
        odrive_can0_.setMaxMB(16);
        odrive_can0_.enableFIFO();
        odrive_can0_.enableFIFOInterrupt();
        odrive_can0_.onReceive(F);
 
        odrive_can1_.begin();
        odrive_can1_.setBaudRate(CAN_BAUDRATE);
        odrive_can1_.setMaxMB(16);
        odrive_can1_.enableFIFO();
        odrive_can1_.enableFIFOInterrupt();
        odrive_can1_.onReceive(F);
 
        // Todo: move TeensyCAN inside here and see if that works
    }
 
    VirtualTimerGroup &timer_group;
 
    FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_256> &odrive_can0_;
 
    FlexCAN_T4<CAN3, RX_SIZE_256, TX_SIZE_256> &odrive_can1_;
 
    OdriveController<CAN2> &odrv0_;
 
    OdriveController<CAN3> &odrv1_;
 
    bool teensy_heartbeat_timeout = false;
 
    ICAN &teensy_can;
 
    MakeSignedCANSignal(float, 0, 32, 0.0001, 0) end_eff_pos_0_tx{};
    MakeSignedCANSignal(float, 32, 32, 0.0001, 0) end_eff_pos_1_tx{};
    CANTXMessage<2> tx_end_eff_pos_message{teensy_can, END_EFFECTOR_POS_TX_ID, 8, 500, timer_group,
                                           end_eff_pos_0_tx, end_eff_pos_1_tx};
 
    MakeSignedCANSignal(float, 0, 32, 0.0001, 0) end_eff_pos_0_rx{};
    MakeSignedCANSignal(float, 32, 32, 0.0001, 0) end_eff_pos_1_rx{};
    CANRXMessage<2> rx_end_eff_pos_message{teensy_can, END_EFFECTOR_POS_RX_ID, end_eff_pos_0_rx, end_eff_pos_1_rx};
 
    MakeSignedCANSignal(float, 0, 32, 0.0001, 0) end_eff_vel_0_tx{};
    MakeSignedCANSignal(float, 32, 32, 0.0001, 0) end_eff_vel_1_tx{};
    CANTXMessage<2> tx_end_eff_vel_message{teensy_can, END_EFFECTOR_VEL_TX_ID, 8, 500, timer_group, end_eff_vel_0_tx,
                                           end_eff_vel_1_tx};
 
    MakeSignedCANSignal(float, 0, 32, 0.0001, 0) end_eff_vel_0_rx{};
    MakeSignedCANSignal(float, 32, 32, 0.0001, 0) end_eff_vel_1_rx{};
    CANRXMessage<2> rx_end_eff_vel_message{teensy_can, END_EFFECTOR_VEL_RX_ID, end_eff_vel_0_rx, end_eff_vel_1_rx};
 
    MakeUnsignedCANSignal(ControllerState, 0, 4, 1, 0) system_state_tx{};
    MakeUnsignedCANSignal(bool, 4, 1, 1, 0) odrive0_heartbeat_active_tx{};
    MakeUnsignedCANSignal(bool, 5, 1, 1, 0) encoder0_active_tx{};
    MakeUnsignedCANSignal(bool, 6, 1, 1, 0) motor0_active_tx{};
    MakeUnsignedCANSignal(bool, 7, 1, 1, 0) odrive1_heartbeat_active_tx{};
    MakeUnsignedCANSignal(bool, 8, 1, 1, 0) encoder1_active_tx{};
    MakeUnsignedCANSignal(bool, 9, 1, 1, 0) motor1_active_tx{};
    MakeUnsignedCANSignal(bool, 10, 1, 1, 0) mirror_system_active_tx{};
    CANTXMessage<8> tx_heartbeat_message{teensy_can, HEARTBEAT_TX_ID, 1, 10000, timer_group,
                                         odrive0_heartbeat_active_tx,
                                         encoder0_active_tx, motor0_active_tx, odrive1_heartbeat_active_tx,
                                         encoder1_active_tx, motor1_active_tx, mirror_system_active_tx, system_state_tx};
 
    MakeUnsignedCANSignal(ControllerState, 0, 4, 1, 0) system_state_rx{};
    MakeUnsignedCANSignal(bool, 4, 1, 1, 0) odrive0_heartbeat_active_rx{};
    MakeUnsignedCANSignal(bool, 5, 1, 1, 0) encoder0_active_rx{};
    MakeUnsignedCANSignal(bool, 6, 1, 1, 0) motor0_active_rx{};
    MakeUnsignedCANSignal(bool, 7, 1, 1, 0) odrive1_heartbeat_active_rx{};
    MakeUnsignedCANSignal(bool, 8, 1, 1, 0) encoder1_active_rx{};
    MakeUnsignedCANSignal(bool, 9, 1, 1, 0) motor1_active_rx{};
    MakeUnsignedCANSignal(bool, 10, 1, 1, 0) mirror_system_active_rx{};
    CANRXMessage<8> rx_heartbeat_message{teensy_can, HEARTBEAT_RX_ID, [this]()
                                         { handleHeartbeat(); },
                                         odrive0_heartbeat_active_rx, encoder0_active_rx, motor0_active_rx, odrive1_heartbeat_active_rx,
                                         encoder1_active_rx, motor1_active_rx, mirror_system_active_rx, system_state_rx};
 
    bool Initialize()
    {
        find_odrive(odrv0_);
        find_odrive(odrv1_);
 
        odrv0_.startup_odrive_checks();
        odrv1_.startup_odrive_checks();
 
        startup_odrive(odrv0_);
        startup_odrive(odrv1_);
 
        teensy_can.RegisterRXMessage(rx_end_eff_pos_message);
        teensy_can.RegisterRXMessage(rx_end_eff_vel_message);
        teensy_can.RegisterRXMessage(rx_heartbeat_message);
 
        // Uncomment these for unmapped ODrives
        // odrv1_.enable_anticogging();
        // odrv0_.enable_anticogging();
 
        return true;
    }
 
    void Tick()
    {
        // Serial.println("Comms Controller Tick");
        // Serial.println(system_state_rx);
 
        pumpEvents(odrive_can0_);
        pumpEvents(odrive_can1_);
        odrv0_.odrv_user_data_.heartbeat_timeout = millis() -
                                                       odrv0_.odrv_user_data_.last_heartbeat_time >
                                                   ODRIVE_HEARTBEAT_TIMEOUT;
        odrv1_.odrv_user_data_.heartbeat_timeout = millis() -
                                                       odrv1_.odrv_user_data_.last_heartbeat_time >
                                                   ODRIVE_HEARTBEAT_TIMEOUT;
        teensy_heartbeat_timeout = millis() - last_received_time > TEENSY_HEARTBEAT_TIMEOUT;
    }
 
    template <typename T>
    void find_odrive(T odrv_ctrl)
    {
        Serial.println("Waiting for ODrive" + String(odrv_ctrl.odrv_user_data_.node_id) + "...");
        while (!odrv_ctrl.odrv_user_data_.received_heartbeat)
        {
            Tick();
            delay(100);
            Serial.println("Waiting for ODrive" + String(odrv_ctrl.odrv_user_data_.node_id) + "...");
            // Serial.print("Recv hb cc: ");
            // Serial.println(odrv_ctrl.odrv_user_data_.received_heartbeat);
        }
        Serial.println("Found ODrive");
    }
 
    template <typename T>
    void startup_odrive(T odrv_ctrl)
    {
        Serial.println("Enabling closed loop control...");
        while (odrv_ctrl.odrv_user_data_.last_heartbeat.Axis_State !=
               ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL)
        {
            // odrv_ctrl.odrv_.clearErrors();
            delay(1);
            odrv_ctrl.odrv_.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
            for (int i = 0; i < 15; ++i)
            {
                delay(10);
                CommsController::Tick();
            }
        }
        Serial.println("ODrive running!");
    }
 
    inline ODriveUserData &get_odrv0_data()
    {
        return odrv0_.odrv_user_data_;
    }
 
    inline ODriveUserData &get_odrv1_data()
    {
        return odrv1_.odrv_user_data_;
    }
 
    inline ODriveCAN &get_odrv0()
    {
        return odrv0_.odrv_;
    }
 
    inline ODriveCAN &get_odrv1()
    {
        return odrv1_.odrv_;
    }
 
    bool comms_timeout()
    {
        return teensy_heartbeat_timeout || odrv0_.odrv_user_data_.heartbeat_timeout ||
               odrv1_.odrv_user_data_.heartbeat_timeout;
    }
 
private:
    uint32_t last_received_time;
 
    void handleHeartbeat()
    {
        last_received_time = millis();
    }
};