main.cpp

Go to the documentation of this file.

#include <Arduino.h>
#include "ODriveCAN.h"
#include <FlexCAN_T4.h>
#include "ODriveFlexCAN.hpp"
#include "teensy_can.h"
#include "virtualTimer.h"
#include "defines.h"
 
#include "odrive_utils/odrive_errors.hpp"
 
#include "utils/angles.hpp"
#include "odrive_utils/odrive_msgs.hpp"
#include "utils/helpers.hpp"
#include "utils/filters.hpp"
#include "utils/led.hpp"
 
#include "controllers/comm_controller.hpp"
#include "controllers/bilateral_controller.hpp"
#include "controllers/odrive_controller.hpp"
#include <Adafruit_NeoPixel.h>
 
#include "tests.hpp"
 
#define ODRV_CAN0 CAN2
#define ODRV_CAN1 CAN3
 
VirtualTimerGroup timer_group{};
VirtualTimerGroup micro_timer_group{};
TeensyCAN<1> teensy_can;
 
struct ODriveStatus; // hack to prevent teensy compile error
 
uint32_t last_received_time;
 
// Declaring static members of the CommsController class
FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_256> odrive_can0;
FlexCAN_T4<CAN3, RX_SIZE_256, TX_SIZE_256> odrive_can1;
 
ODriveUserData odrv0_user_data{};
ODriveUserData odrv1_user_data{};
 
OdriveController<CAN2> odrv0 = OdriveController<CAN2>(odrive_can0, ODRIVE0_ID, odrv0_user_data);
 
OdriveController<CAN3> odrv1 = OdriveController<CAN3>(odrive_can1, ODRIVE1_ID, odrv1_user_data);
 
// Called for every message that arrives on the CAN bus
void onOdriveCanMessage(const CanMsg &msg)
{
    onReceive(msg, odrv0.odrv_);
    onReceive(msg, odrv1.odrv_);
}
 
CommsController<ODRV_CAN0, ODRV_CAN1> comms_controller{micro_timer_group,
                                                       odrive_can0,
                                                       odrive_can1,
                                                       odrv0,
                                                       odrv1,
                                                       onOdriveCanMessage,
                                                       teensy_can};
 
LowPassFilter filter = LowPassFilter(20.0);
 
MovingAverageFilter filter_phi_0{1};
MovingAverageFilter filter_phi_1{1};
MovingAverageFilter filter_phi_dot_0{1};
MovingAverageFilter filter_phi_dot_1{1};
 
std::vector<float> joint_thetas = {0.0f, 0.0f};
std::vector<float> joint_vels = {0.0f, 0.0f};
std::vector<float> pos = {0.0f, 0.0f};
std::vector<float> vel = {0.0f, 0.0f};
 
void endEffMirrorUpdate();
void jointMirrorUpdate();
 
void enterState(ControllerState state);
void exitState(ControllerState state);
 
struct SystemInfo
{
 
    SystemInfo() = default;
 
    SystemInfo(CommsController<ODRV_CAN0, ODRV_CAN1> &cc)
        : teensy_heartbeat_timeout(cc.teensy_heartbeat_timeout),
          odrive0_heartbeat_timeout(cc.get_odrv0_data().heartbeat_timeout),
          odrive1_heartbeat_timeout(cc.get_odrv1_data().heartbeat_timeout)
    {
    }
 
    bool teensy_heartbeat_timeout = false;
    bool &odrive0_heartbeat_timeout;
    bool &odrive1_heartbeat_timeout;
    bool estop_enabled = false;
    bool relays_open = false;
    bool deadman_switch_pressed = false;
    ControllerState mirror_teensy_state = ControllerState::INITIALIZING;
};
 
// Need to update members wherever they change
SystemInfo sys_inf = SystemInfo(comms_controller);
 
void printEverything()
{
    // db_println("Possible Errors:");
    // db_print("Teensy heartbeat timeout: ");
    // db_println(sys_inf.teensy_heartbeat_timeout);
    // db_print("ODrive 0 heartbeat timeout: ");
    // db_println(sys_inf.odrive0_heartbeat_timeout);
    // db_print("ODrive 1 heartbeat timeout: ");
    // db_println(sys_inf.odrive1_heartbeat_timeout);
    // db_print("Estop enabled: ");
    // db_println(sys_inf.estop_enabled);
    // db_print("Relays open: ");
    // db_println(sys_inf.relays_open);
    // db_print("Deadman switch pressed: ");
    // db_print(sys_inf.deadman_switch_pressed);
    // db_println("\n");
    // db_print("Mirror Teensy State: ");
    // db_println(sys_inf.mirror_teensy_state);
    const auto mirror_pos_0 = static_cast<float>(comms_controller.end_eff_pos_0_rx);
    const auto mirror_pos_2 = static_cast<float>(comms_controller.end_eff_pos_1_rx);
    #ifdef FINGER
      db_printVal(pos.at(0));
      db_printVal(pos.at(2));
      db_printVal(vel.at(0));
      db_printVal(vel.at(1));
      db_printVal(mirror_pos_0);
      db_printVal(mirror_pos_2);
    #else
      db_printVal(joint_thetas.at(0));
      db_printVal(joint_thetas.at(1));
      db_printVal(joint_vels.at(0));
      db_printVal(joint_vels.at(1));
      db_printVal(mirror_pos_0);
      db_printVal(mirror_pos_2);
    #endif
}
 
bool shouldError()
{
    return (sys_inf.teensy_heartbeat_timeout) ||
           (sys_inf.odrive0_heartbeat_timeout) ||
           (sys_inf.odrive1_heartbeat_timeout) ||
           (sys_inf.estop_enabled) ||
           (sys_inf.relays_open) ||
           (!clear_safe_errors(
               comms_controller.get_odrv0(),
               comms_controller.get_odrv0_data().last_error)) ||
           (!clear_safe_errors(
               comms_controller.get_odrv1(),
               comms_controller.get_odrv1_data().last_error)) ||
            (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) ||
            (odrv1_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
    //  (sys_inf.mirror_teensy_state == ControllerState::ERROR);
}
 
ControllerState controller_state = ControllerState::RUNNING;
 
void doReboot()
{
    SCB_AIRCR = 0x05FA0004;
}
 
void openRelays()
{
    digitalWrite(RELAY_PIN_OUTPUT, LOW);
}
 
void closeRelays()
{
    digitalWrite(RELAY_PIN_OUTPUT, HIGH);
}
 
void request_disable()
{
    Estop_msg_t estop_msg;
    comms_controller.get_odrv0().send(estop_msg);
    comms_controller.get_odrv1().send(estop_msg);
}
 
void request_enable()
{
    clear_safe_errors(comms_controller.get_odrv0(), comms_controller.get_odrv0_data().last_error);
    comms_controller.get_odrv0().setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
    clear_safe_errors(comms_controller.get_odrv1(), comms_controller.get_odrv1_data().last_error);
    comms_controller.get_odrv1().setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
}
 
bool isInitialized()
{
    return true;
    // return float_close_compare(joint_0.get_position(), init_0.get_position()) &&
    //        float_close_compare(joint_1.get_position(), init_1.get_position());
}
 
bool isSynced()
{
    const auto other_end_eff_x = static_cast<float>(comms_controller.end_eff_pos_0_rx);
    const auto other_end_eff_z = static_cast<float>(comms_controller.end_eff_pos_1_rx);
 
    // mirror_x_state.update_position(other_end_eff_x, 0.01);
    // mirror_z_state.update_position(other_end_eff_z, 0.01);
 
    #ifdef FINGER
      return float_close_compare(other_end_eff_x, pos.at(0), 0.03) &&
            float_close_compare(other_end_eff_z, pos.at(2), 0.03);
    #else
      return float_close_compare(other_end_eff_x, joint_thetas.at(0), 0.34) &&
            float_close_compare(other_end_eff_z, joint_thetas.at(1), 0.34);
    #endif
}
 
void ledUpdate()
{
    if (millis() - last_received_time > 1000)
    {
        digitalWrite(LED_BUILTIN, LOW);
    }
    else
    {
        digitalWrite(LED_BUILTIN, HIGH);
    }
 
    switch (controller_state)
    {
    case ControllerState::INITIALIZING:
        setColor(1, blue);
        break;
    case ControllerState::READY:
        setColor(1, yellow);
        break;
    case ControllerState::RUNNING:
        setColor(1, green);
        break;
    case ControllerState::ERROR:
        setColor(1, red);
        break;
    }
 
    if (sys_inf.teensy_heartbeat_timeout)
    {
        setColor(2, red);
    }
    else
    {
        setColor(2, green);
    }
 
    if (sys_inf.odrive0_heartbeat_timeout && sys_inf.odrive1_heartbeat_timeout)
    {
        setColor(3, red);
    }
    else if (sys_inf.odrive0_heartbeat_timeout)
    {
        setColor(3, cyan);
    }
    else if (sys_inf.odrive1_heartbeat_timeout)
    {
        setColor(3, magneta);
    }
    else
    {
        setColor(3, green);
    }
}
 
void doState()
{
    // db_print("Current state: ");
    // db_println(controller_state);
    switch (controller_state)
    {
    case ControllerState::INITIALIZING:
        // do nothing
        break;
    case ControllerState::READY:
        // check position syncing
        break;
    case ControllerState::RUNNING:
        // check position syncing
        // mirror positiong
        #ifdef FINGER
          endEffMirrorUpdate();
        #else
          jointMirrorUpdate();
        #endif
        break;
    case ControllerState::ERROR:
        if (sys_inf.odrive0_heartbeat_timeout || sys_inf.odrive1_heartbeat_timeout)
        {
            openRelays();
        }
        break;
    }
 
    // things that should constantly be happening
    // check if teensy heartbeat timeout
    // check if odrives heartbeat timeout
    // check if estop enabled -> idk how to do this
    // check if relays are open
    // check mirror teensy state
    // light leds -> 1 for teensy heartbeat, 1 for odrive heartbeat, 1 for cur state
    // check deadman switch
}
 
void changeState()
{
    // check errors, if errors, change state to ERROR and return
    // check if estop enabled, change state to ERROR and return
    // check if teensy heartbeat timeout, change state to ERROR and return
    // check if odrives heartbeats timeout, change state to ERROR and return
    // check if relays are open, change state to ERROR and return
    if (shouldError())
    {
        if (controller_state == ControllerState::ERROR)
        {
            return;
        }
        exitState(controller_state);
        controller_state = ControllerState::ERROR;
        enterState(controller_state);
        db_println("Switching to ERROR");
        printEverything();
        return;
    }
 
    switch (controller_state)
    {
    case ControllerState::INITIALIZING:
        // check if in init position
        // check if comms controller is ready
        // if both, change state to READY
        //   printEverything();
        if (isInitialized() && !comms_controller.comms_timeout())
        {
            exitState(controller_state);
            controller_state = ControllerState::READY;
            enterState(controller_state);
            db_println("Switching to READY");
        }
        break;
    case ControllerState::READY:
        // if mirroring teensy in ready
        // AND if position synced
        // AND deadman switch pressed
        // change state to RUNNING
        // db_println("In ready state: ");
        // db_println(sys_inf.mirror_teensy_state);
        // db_println(sys_inf.deadman_switch_pressed);
        if (sys_inf.deadman_switch_pressed &&
            ((sys_inf.mirror_teensy_state == ControllerState::READY) ||
             (sys_inf.mirror_teensy_state == ControllerState::RUNNING)) &&
            isSynced())
        {
            exitState(controller_state);
            controller_state = ControllerState::RUNNING;
            enterState(controller_state);
            db_println("Switching to RUNNING");
        }
        break;
    case ControllerState::RUNNING:
        // if deadman switch not pressed
        // OR if teensy not in running
        // OR out of sync
        // change state to READY
        if (!sys_inf.deadman_switch_pressed ||
            ((sys_inf.mirror_teensy_state != ControllerState::READY) &&
             (sys_inf.mirror_teensy_state != ControllerState::RUNNING)) ||
            !isSynced())
        {
            exitState(controller_state);
            controller_state = ControllerState::READY;
            enterState(controller_state);
            db_println("Switching to READY");
        }
        break;
    case ControllerState::ERROR:
        // check if manual error clear was pressed, if so, change state to initializing
        // if estop disabled, comms controller ready, then change state to initializing
        if (!comms_controller.comms_timeout())
        {
            exitState(controller_state);
            controller_state = ControllerState::INITIALIZING;
            enterState(controller_state);
            db_println("Switching to INITIALIZING");
        }
        break;
    }
}
 
void enterState(ControllerState state)
{
    comms_controller.system_state_tx = state;
    switch (state)
    {
    case ControllerState::INITIALIZING:
        request_enable();
        break;
    case ControllerState::READY:
        break;
    case ControllerState::RUNNING:
        break;
    case ControllerState::ERROR:
        request_disable();
        // if both teensy and odrive heartbeat timeout, reboot
        if (sys_inf.teensy_heartbeat_timeout && sys_inf.odrive0_heartbeat_timeout &&
            sys_inf.odrive1_heartbeat_timeout)
        {
            doReboot();
        }
        break;
    }
}
 
void exitState(ControllerState state)
{
    switch (state)
    {
    case ControllerState::INITIALIZING:
        break;
    case ControllerState::READY:
        break;
    case ControllerState::RUNNING:
        comms_controller.get_odrv0().setTorque(0.0);
        comms_controller.get_odrv1().setTorque(0.0);
        break;
    case ControllerState::ERROR:
        closeRelays();
        break;
    }
}
 
void updateSystemInfo()
{
    sys_inf.teensy_heartbeat_timeout = comms_controller.teensy_heartbeat_timeout;
    sys_inf.mirror_teensy_state = comms_controller.system_state_rx;
}
 
void updateAndSendSystemInfo()
{
    // Motor angles in radians
    const auto phi0 = filter_phi_0.update(
        convert_angular_units(
            odrv0_user_data.last_feedback.Pos_Estimate,
            AngleUnits::REVS,
            AngleUnits::RADIANS));
    const auto phi1 = filter_phi_1.update(
        convert_angular_units(
            odrv1_user_data.last_feedback.Pos_Estimate,
            AngleUnits::REVS,
            AngleUnits::RADIANS));
 
    const auto phi_dot_0 = filter_phi_dot_0.update(
        convert_angular_units(
            odrv0_user_data.last_feedback.Vel_Estimate,
            AngleUnits::REVS,
            AngleUnits::RADIANS));
    const auto phi_dot_1 = filter_phi_dot_1.update(
        convert_angular_units(
            odrv1_user_data.last_feedback.Vel_Estimate,
            AngleUnits::REVS,
            AngleUnits::RADIANS));
 
    joint_thetas = phi_to_theta({phi0, phi1});
    joint_vels = motors_to_joints({phi_dot_0, phi_dot_1});
 
    #ifdef FINGER
    pos = forward_kinematics(joint_thetas);
 
    const auto J_s = spatial_jacobian(joint_thetas);
 
    vel = J_s * joint_vels;
 
    comms_controller.end_eff_pos_0_tx = pos.at(0);
    comms_controller.end_eff_pos_1_tx = pos.at(2);
 
    comms_controller.end_eff_vel_0_tx = vel.at(0);
    // .at(1) is correct here, vel only contains x and z velocity due to y be === 0
    comms_controller.end_eff_vel_1_tx = vel.at(1);
    #else
    comms_controller.end_eff_pos_0_tx = joint_thetas.at(0);
    comms_controller.end_eff_pos_1_tx = joint_thetas.at(1);
 
    comms_controller.end_eff_vel_0_tx = joint_vels.at(0);
    // .at(1) is correct here, vel only contains x and z velocity due to y be === 0
    comms_controller.end_eff_vel_1_tx = joint_vels.at(1);
 
    #endif
 
    // const auto mirror_pos_0 = static_cast<float>(comms_controller.end_eff_pos_0_rx);
    // const auto mirror_pos_2 = static_cast<float>(comms_controller.end_eff_pos_1_rx);
    // const auto mirror_vel_0 = static_cast<float>(comms_controller.end_eff_vel_0_rx);
    // const auto mirror_vel_2 = static_cast<float>(comms_controller.end_eff_vel_1_rx);
 
    // db_printVal(mirror_vel_0);
    // db_printVal(mirror_vel_2);
}
 
void jointMirrorUpdate()
{
    const auto mirror_joint_0 = static_cast<float>(comms_controller.end_eff_pos_0_rx);
    const auto mirror_joint_1 = static_cast<float>(comms_controller.end_eff_pos_1_rx);
    const auto mirror_joint_0_vel = static_cast<float>(comms_controller.end_eff_vel_0_rx);
    const auto mirror_joint_1_vel = static_cast<float>(comms_controller.end_eff_vel_1_rx);
 
    const auto delta_0 = mirror_joint_0 - joint_thetas.at(0);
    const auto delta_1 = mirror_joint_1 - joint_thetas.at(1);
 
    const auto delta_vel_0 = mirror_joint_0_vel - joint_vels.at(0);
    const auto delta_vel_1 = mirror_joint_1_vel - joint_vels.at(1);
 
    const float k_0 = 4.0f;
    const float k_1 = 2.0f;
 
    const float c_0 = 0.00f;
    const float c_1 = 0.00f;
 
    const float joint_0_torque = k_0 * delta_0 + c_0 * delta_vel_0;
    const float joint_1_torque = k_1 * delta_1 + c_1 * delta_vel_1;
 
    const auto soft_limit = soft_limit_torques(joint_thetas);
 
    const auto torques = tau_to_torque({joint_0_torque + soft_limit.at(0), joint_1_torque + soft_limit.at(1), 0.0});
 
    const auto clamped_torque_0 = limit<float>(torques.at(0), motor_torque_limit);
    const auto clamped_torque_1 = limit<float>(torques.at(1), motor_torque_limit);
 
    // May not be needed anymore since it is being lmited in OdriveController as long as motor limits is set
    comms_controller.get_odrv0().setTorque(clamped_torque_0);
    comms_controller.get_odrv1().setTorque(clamped_torque_1);
}
 
 
void endEffMirrorUpdate()
{
    // db_printVal(joint_thetas.at(0));
    // db_printVal(joint_thetas.at(1));
 
    // db_printVal(pos.at(0));
    // db_printVal(pos.at(2));
 
    auto J_s = spatial_jacobian(joint_thetas);
 
    const auto mirror_x = static_cast<float>(comms_controller.end_eff_pos_0_rx);
    const auto mirror_z = static_cast<float>(comms_controller.end_eff_pos_1_rx);
    const auto mirror_x_vel = static_cast<float>(comms_controller.end_eff_vel_0_rx);
    const auto mirror_z_vel = static_cast<float>(comms_controller.end_eff_vel_1_rx);
 
    // db_printVal(mirror_x);
    // db_printVal(mirror_x_vel);
 
    // db_printVal(mirror_z);
    // db_printVal(mirror_z_vel);
    // Uses the last estimated position and the new position
    // If the first reading is not near 0 this may be very large
    const auto delta_x = mirror_x - pos.at(0);
    const auto delta_z = mirror_z - pos.at(2);
 
    const auto delta_x_vel = mirror_x_vel - vel.at(0);
    const auto delta_z_vel = mirror_z_vel - vel.at(1);
 
    // db_printVal(delta_x);
    // db_printVal(delta_z);
 
    const auto k_x = 500.0f; // 170
    const auto k_z = 500.0f; // 170
 
    const auto c_x = 1.65f;
    const auto c_z = 1.65f;
 
    const auto F_stiffness_x = k_x * delta_x;
    const auto F_stiffness_z = k_z * delta_z;
 
    const auto F_damping_x = c_x * delta_x_vel;
    const auto F_damping_z = c_z * delta_z_vel;
 
    const auto F_kx = F_stiffness_x + F_damping_x;
    const auto F_kz = F_stiffness_z + F_damping_z;
 
    auto tau = J_s.T() * std::vector<float>{F_kx, F_kz};
    const auto soft_limit = soft_limit_torques(joint_thetas);
    // db_printVal(tau);
    tau.at(0) += soft_limit.at(0);
    tau.at(1) += soft_limit.at(1);
    const auto torques = tau_to_torque(tau);
 
 
    const auto clamped_torque_0 = limit<float>(torques.at(0), motor_torque_limit);
    const auto clamped_torque_1 = limit<float>(torques.at(1), motor_torque_limit);
 
    // db_printVal(soft_limit.at(0));
    // db_printVal(soft_limit.at(1));
 
    // db_printVal(clamped_torque_0);
    // db_printVal(clamped_torque_1);
 
    // May not be needed anymore since it is being lmited in OdriveController as long as motor limits is set
    comms_controller.get_odrv0().setTorque(clamped_torque_0);
    comms_controller.get_odrv1().setTorque(clamped_torque_1);
}
 
void checkDeadman()
{
    comms_controller.Tick();
    // Needed?
    delay(100);
 
    if (digitalRead(DEADMAN_SWITCH) == LOW)
    {
        sys_inf.deadman_switch_pressed = false;
        return;
    }
    sys_inf.deadman_switch_pressed = true;
    return;
}
 
void setup()
{
    // Serial setup
    Serial.begin(115200);
    for (int i = 0; i < 30 && !Serial; ++i)
    {
        delay(100);
    }
    delay(200);
 
#ifdef TESTING
    allUnitTests();
#endif
 
    // CAN setup
    db_println("Starting CAN setup");
    teensy_can.Initialize(ICAN::BaudRate::kBaud1M);
    if (!comms_controller.Initialize())
    {
        db_println("CAN failed to initialize: reset required");
    }
 
    // Led setup
    ledSetup();
    pinMode(DEADMAN_SWITCH, INPUT);
    pinMode(RELAY_PIN_OUTPUT, OUTPUT);
    ledSetup();
 
    // Interrupt setup
    attachInterrupt(digitalPinToInterrupt(DEADMAN_SWITCH), checkDeadman, CHANGE);
    attachInterrupt(
        digitalPinToInterrupt(CLEAR_ERROR_BUTTON), []()
        {
      comms_controller.get_odrv0().clearErrors();
      comms_controller.get_odrv1().clearErrors();
      comms_controller.get_odrv0().setAbsolutePosition(0.0f);
      comms_controller.get_odrv1().setAbsolutePosition(0.0f);
      exitState(ControllerState::ERROR);
      controller_state = ControllerState::INITIALIZING;
      enterState(ControllerState::INITIALIZING);
      db_println("Switching to INITIALIZING"); },
        RISING);
 
    // Start initial state
    checkDeadman();
    db_println("Switching to INITIALIZING");
    controller_state = ControllerState::INITIALIZING;
    enterState(ControllerState::INITIALIZING);
 
    // Millisecond timer setup
    db_println("Starting timer setup");
    timer_group.AddTimer(10, updateSystemInfo);
    timer_group.AddTimer(10, ledUpdate);
    timer_group.AddTimer(10, changeState);
    timer_group.AddTimer(10, printEverything);
 
    // Microsecond timer setup
    micro_timer_group.AddTimer(500, doState);
    micro_timer_group.AddTimer(500, updateAndSendSystemInfo);
    db_println("Timer complete");
 
    db_println("Setup complete\n\n");
}
 
void loop()
{
    db_endTiming("loop");
    db_startTiming("loop");
    timer_group.Tick(millis());
    micro_timer_group.Tick(micros());
    comms_controller.Tick();
    teensy_can.Tick();
}