odrive_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 "odrive_utils/odrive_callbacks.hpp"
#include "utils/helpers.hpp"
 
template<CAN_DEV_TABLE CAN>
class OdriveController
{
public:
  OdriveController() = default;
  ~OdriveController() = default;
 
  OdriveController(
    FlexCAN_T4<CAN, RX_SIZE_256, TX_SIZE_256> & odrive_can,
    int CAN_ID,
    ODriveUserData & odrv_user_data)
  : odrive_can_(odrive_can),
    odrv_(ODriveCAN(wrap_can_intf(odrive_can), CAN_ID)),
    odrv_user_data_(odrv_user_data)
  {
    odrv_user_data_.node_id = CAN_ID;
    // Register callbacks for the heartbeat and encoder feedback messages
    odrv_.onFeedback(onFeedback, &odrv_user_data_);
    odrv_.onStatus(onHeartbeat, &odrv_user_data_);
    odrv_.onTemperature(onTemperature, &odrv_user_data_);
  }
  FlexCAN_T4<CAN, RX_SIZE_256, TX_SIZE_256> & odrive_can_;
 
  ODriveCAN odrv_;
 
  ODriveUserData & odrv_user_data_;
 
  Limits<float> motor_limits_;
 
  float alpha_ = 0.95; // This is the percentage of the joint limit before we begin limiting the outputted torque
 
  bool set_params()
  {
    Serial.println("Setting parameters: ");
    Set_Param_msg_t<float> param;
    for (u_int i = 0; i < fconfig_params.size(); i++) {
      param.Endpoint_ID = fconfig_params[i].endpoint_id;
      param.Value = fconfig_params[i].value;
 
      Serial.print("Setting param: ");
      Serial.println(param.Endpoint_ID);
      odrv_.send(param);
      pumpEvents(odrive_can_);
      delay(100);
    }
 
    Set_Param_msg_t<uint32_t> bparam;
    for (u_int i = 0; i < bconfig_params.size(); i++) {
      bparam.Endpoint_ID = bconfig_params[i].endpoint_id;
      bparam.Value = bconfig_params[i].value;
      Serial.print("Setting param: ");
      Serial.println(bparam.Endpoint_ID);
      odrv_.send(bparam);
      pumpEvents(odrive_can_);
      delay(100);
    }
 
    return true;
  }
 
  bool full_calibration()
  {
    Serial.println("Starting Calibration");
    odrv_.setState(ODriveAxisState::AXIS_STATE_FULL_CALIBRATION_SEQUENCE);
    delay(1000);
    Serial.println("Calibration Done!");
    return true;
  }
 
  bool move_to_limits()
  {
    odrv_.setControllerMode(
      ODriveControlMode::CONTROL_MODE_POSITION_CONTROL,
      ODriveInputMode::INPUT_MODE_PASSTHROUGH);
    odrv_.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
    Serial.println("Moving to upper limit.");
    odrv_.setPosition(motor_limits_.upper);
    delay(1000);
    Serial.println("Moving to lower limit.");
    odrv_.setPosition(motor_limits_.lower);
    delay(1000);
    Serial.println("Done Checking Limits");
    odrv_.setControllerMode(
      ODriveControlMode::CONTROL_MODE_TORQUE_CONTROL,
      ODriveInputMode::INPUT_MODE_PASSTHROUGH);
    return true;
  }
 
  bool enable_anticogging(bool doCalibration = false)
  {
    Serial.println("Attempting to turn on anticogging");
    Set_Param_msg_t<float> fparam;
    for (u_int i = 0; i < fanticogging_config_params.size(); i++) {
      fparam.Endpoint_ID = fanticogging_config_params[i].endpoint_id;
      fparam.Value = fanticogging_config_params[i].value;
 
      Serial.print("Setting param: ");
      Serial.println(fparam.Endpoint_ID);
      odrv_.send(fparam);
      pumpEvents(odrive_can_);
      delay(100);
    }
 
    Set_Param_msg_t<bool> bparam;
    for (u_int i = 0; i < banticogging_config_params.size(); i++) {
      bparam.Endpoint_ID = banticogging_config_params[i].endpoint_id;
      bparam.Value = banticogging_config_params[i].value;
 
      Serial.print("Setting param: ");
      Serial.println(bparam.Endpoint_ID);
      odrv_.send(bparam);
      pumpEvents(odrive_can_);
      delay(100);
    }
    Serial.println("Successfully set anticogging configuration");
 
    if (doCalibration) {
      odrv_.setState(ODriveAxisState::AXIS_STATE_ANTICOGGING_CALIBRATION);
      delay(1000);
      Serial.println("Starting anticogging calibration");
      // Check if done
      delay(480 * 1000);
      Serial.println("Finished anticogging calibration");
    }
 
    bparam.Endpoint_ID = benable_config_param.endpoint_id;
    bparam.Value = benable_config_param.value;
    Serial.print("Setting param: ");
    Serial.println(bparam.Endpoint_ID);
    odrv_.send(bparam);
    pumpEvents(odrive_can_);
    delay(100);
    Serial.println("Enabled anticogging");
 
    Set_Reboot_msg_t reboot;
    reboot.Action = 1;
    odrv_.send(reboot);
    delay(10 * 1000);
    Serial.println("Reboot successful");
    return true;
  }
 
  bool startup_odrive_checks()
  {
    Serial.println("\nAttempting to set configuration and start torque mode!");
 
    set_params();
    Serial.println("Configuration set");
 
    Serial.println("Checking Temperature...");
 
    odrv_.setControllerMode(
      ODriveControlMode::CONTROL_MODE_TORQUE_CONTROL,
      ODriveInputMode::INPUT_MODE_PASSTHROUGH);
    Serial.println("Setting to Torque mode");
 
    return true;
  }
 
  void set_torque(float torque)
  {
    const auto pos = odrv_user_data_.last_feedback.Pos_Estimate;
 
    // The position is outside of the joint limits
    if (pos >= motor_limits_.upper || pos <= motor_limits_.lower) {
      odrv_.setTorque(0);
      return;
    }
 
    // Here the joint limits are with in the acceptable range. No throttling required
    if (pos <= motor_limits_.upper * alpha_ || pos >= motor_limits_.lower * alpha_) {
      odrv_.setTorque(torque);
      return;
    }
 
    // If we are in the upper buffer
    if (pos >= motor_limits_.upper * alpha_) {
      const auto gain = (motor_limits_.upper - pos) / motor_limits_.upper;
      odrv_.setTorque(torque * gain);
      return;
    }
 
    // Only other condition is if we are in the lower buffer
    const auto gain = (motor_limits_.lower - pos) / motor_limits_.lower;
    odrv_.setTorque(torque * gain);
    return;
  }
};