diff --git a/rover-apps/arm_2021/include/Modules/ClawModule.h b/rover-apps/arm_2021/include/Modules/ClawModule.h new file mode 100644 index 00000000..826adb21 --- /dev/null +++ b/rover-apps/arm_2021/include/Modules/ClawModule.h @@ -0,0 +1,55 @@ +#pragma once + +#include + +#include "ActuatorControllerManager.h" +#include "Current.h" +#include "DCMotor.h" +#include "LookupTable.h" +#include "OpenLoop.h" +#include "PID.h" +#include "Position.h" +#include "Quadrature64CPR.h" +#include "Velocity.h" +#include "hw_bridge.h" + +class ClawModule final : public Module { + public: + ElbowModule(); + HWBRIDGE::CANSignalValue_t clawSetPoint; + + void periodic_1s(void) override; + void periodic_10s(void) override; + void periodic_100ms(void) override; + void periodic_10ms(void) override; + void periodic_1ms(CANInterface& can) override; + + Encoder::Quadrature64CPR encoder; + Actuator::DCMotor motor; + + // static Sensor::CurrentSensor currentSensor(CLAW_CRNT_SNS_SPI_CLK, CLAW_CRNT_SNS_SPI_MISO, CLAW_CRNT_SNS_SPI_CS); + // TODO: Add once current sensor driver works + + PID::PID velPID; + PID::PID posPID; + PID::PID curPID; + + constexpr float POLOLUMAXCURRENT = 3; + constexpr float POLOLUMADEGPERSEC = + std::numeric_limits::infinity(); // TODO: figure out MAXDEGPERSEC of motors (1680?); + + Controller::Velocity vel; + Controller::Position pos; + // static Controller::Current cur(motor, encoder, std::nullopt, curPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, + // LIM_CLAW_OPEN, NC); + Controller::OpenLoop open; + + const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, + {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, + // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, + {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; + + Controller::ActuatorControllerManager manager; + + private: +} \ No newline at end of file diff --git a/rover-apps/arm_2021/include/Modules/ElbowModule.h b/rover-apps/arm_2021/include/Modules/ElbowModule.h new file mode 100644 index 00000000..1826ac42 --- /dev/null +++ b/rover-apps/arm_2021/include/Modules/ElbowModule.h @@ -0,0 +1,58 @@ +#pragma once + +#include "AEAT6012.h" +#include "ActuatorControllerManager.h" +#include "Current.h" +#include "DCMotor.h" +#include "LookupTable.h" +#include "Module.h" +#include "OpenLoop.h" +#include "PID.h" +#include "Position.h" +#include "Velocity.h" +#include "hw_bridge.h" +#include "mbed.h" + +class ElbowModule final : public Module { + public: + ElbowModule(); + HWBRIDGE::CANSignalValue_t elbowSetPoint; + + void periodic_1s(void) override; + void periodic_10s(void) override; + void periodic_100ms(void) override; + void periodic_10ms(void) override; + void periodic_1ms(CANInterface& can) override; + + Encoder::AEAT6012 encoder; + Actuator::DCMotor motor; + + // static Sensor::CurrentSensor currentSensor(TRNTBL_CRNT_SNS_SPI_CLK, TRNTBL_CRNT_SNS_SPI_MISO, + // TRNTBL_CRNT_SNS_SPI_CS); + // TODO: Add once current sensor driver works + + PID::PID velPID; + PID::PID posPID; + PID::PID curPID; + + constexpr float ASSUNMAXCURRENT = 25.263; +constexpr float ASSUNMAXDEGPERSEC = + std::numeric_limits::infinity(); + // TODO: figure out MAXDEGPERSEC of motors (35580?); + + + Controller::Velocity vel; + Controller::Position pos; + // static Controller::Current cur(motor, encoder, std::nullopt, curPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, + // LIM_TRNTBL_RHS); + Controller::OpenLoop open; + + const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, + {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, + //{HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, + {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; + + Controller::ActuatorControllerManager manager; + + private: +} \ No newline at end of file diff --git a/rover-apps/arm_2021/include/Modules/ShoulderModule.h b/rover-apps/arm_2021/include/Modules/ShoulderModule.h new file mode 100644 index 00000000..4acb09ca --- /dev/null +++ b/rover-apps/arm_2021/include/Modules/ShoulderModule.h @@ -0,0 +1,56 @@ +#pragma once + +#include "AEAT6012.h" +#include "ActuatorControllerManager.h" +#include "Current.h" +#include "DCMotor.h" +#include "LookupTable.h" +#include "Module.h" +#include "OpenLoop.h" +#include "PID.h" +#include "Position.h" +#include "Velocity.h" +#include "hw_bridge.h" +#include "mbed.h" + +class ShoulderModule final : public Module { + public: + TurntableModule(); + HWBRIDGE::CANSignalValue_t shoulderSetPoint; + + void periodic_1s(void) override; + void periodic_10s(void) override; + void periodic_100ms(void) override; + void periodic_10ms(void) override; + void periodic_1ms(CANInterface& can) override; + + Encoder::AEAT6012 encoder; + Actuator::DCMotor motor; + + // static Sensor::CurrentSensor currentSensor(TRNTBL_CRNT_SNS_SPI_CLK, TRNTBL_CRNT_SNS_SPI_MISO, + // TRNTBL_CRNT_SNS_SPI_CS); + // TODO: Add once current sensor driver works + + PID::PID velPID; + PID::PID posPID; + PID::PID curPID; + + constexpr uint8_t PA04MAXCURRENT = 6; + constexpr float PA04MAXDEGPERSEC = std::numeric_limits::infinity(); + // TODO: figure out MAXDEGPERSEC of motors + + Controller::Velocity vel; + Controller::Position pos; + // static Controller::Current cur(motor, encoder, std::nullopt, curPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, + // LIM_TRNTBL_RHS); + Controller::OpenLoop open; + + const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, + {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, + //{HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, + {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; + + Controller::ActuatorControllerManager manager; + + private: +} \ No newline at end of file diff --git a/rover-apps/arm_2021/include/Modules/TooltipModule.h b/rover-apps/arm_2021/include/Modules/TooltipModule.h new file mode 100644 index 00000000..02d81dbc --- /dev/null +++ b/rover-apps/arm_2021/include/Modules/TooltipModule.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +#include "LimServo.h" + +class ClawModule final : public Module { + public: + ElbowModule(); + HWBRIDGE::CANSignalValue_t toolTipPosition; + + Actuator::LimServo clawTooltipServo; + + void periodic_1s(void) override; + void periodic_10s(void) override; + void periodic_100ms(void) override; + void periodic_10ms(void) override; + void periodic_1ms(CANInterface& can) override; + + Encoder::Quadrature64CPR encoder; + Actuator::DCMotor motor; + + // static Sensor::CurrentSensor currentSensor(CLAW_CRNT_SNS_SPI_CLK, CLAW_CRNT_SNS_SPI_MISO, CLAW_CRNT_SNS_SPI_CS); + // TODO: Add once current sensor driver works + + PID::PID velPID; + PID::PID posPID; + PID::PID curPID; + + constexpr float POLOLUMAXCURRENT = 3; + constexpr float POLOLUMADEGPERSEC = + std::numeric_limits::infinity(); // TODO: figure out MAXDEGPERSEC of motors (1680?); + + Controller::Velocity vel; + Controller::Position pos; + // static Controller::Current cur(motor, encoder, std::nullopt, curPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, + // LIM_CLAW_OPEN, NC); + Controller::OpenLoop open; + + const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, + {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, + // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, + {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; + + Controller::ActuatorControllerManager manager; + + private: +} \ No newline at end of file diff --git a/rover-apps/arm_2021/include/Modules/TurntableModule.h b/rover-apps/arm_2021/include/Modules/TurntableModule.h new file mode 100644 index 00000000..1c6daaac --- /dev/null +++ b/rover-apps/arm_2021/include/Modules/TurntableModule.h @@ -0,0 +1,54 @@ +#pragma once + +#include "AEAT6012.h" +#include "ActuatorControllerManager.h" +#include "Current.h" +#include "DCMotor.h" +#include "LookupTable.h" +#include "OpenLoop.h" +#include "PID.h" +#include "Position.h" +#include "Velocity.h" +#include "hw_bridge.h" + +class TurntableModule final : public Module { + public: + TurntableModule(); + HWBRIDGE::CANSignalValue_t turntableSetPoint; + + void periodic_1s(void) override; + void periodic_10s(void) override; + void periodic_100ms(void) override; + void periodic_10ms(void) override; + void periodic_1ms(CANInterface& can) override; + + Encoder::AEAT6012 encoder; + Actuator::DCMotor motor; + + // static Sensor::CurrentSensor currentSensor(TRNTBL_CRNT_SNS_SPI_CLK, TRNTBL_CRNT_SNS_SPI_MISO, + // TRNTBL_CRNT_SNS_SPI_CS); + // TODO: Add once current sensor driver works + + PID::PID velPID; + PID::PID posPID; + PID::PID curPID; + + constexpr uint8_t MAXCURRENT = 53; + constexpr float MAXDEGPERSEC = std::numeric_limits::infinity(); + // TODO: figure out MAXDEGPERSEC of motors (79080?) + + Controller::Velocity vel; + Controller::Position pos; + // static Controller::Current cur(motor, encoder, std::nullopt, curPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, + // LIM_TRNTBL_RHS); + Controller::OpenLoop open; + + const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, + {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, + // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, + {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; + + Controller::ActuatorControllerManager manager; + + private: +} \ No newline at end of file diff --git a/rover-apps/arm_2021/include/Modules/WristModule.h b/rover-apps/arm_2021/include/Modules/WristModule.h new file mode 100644 index 00000000..01ae090f --- /dev/null +++ b/rover-apps/arm_2021/include/Modules/WristModule.h @@ -0,0 +1,77 @@ +#pragma once + +#include + +#include "ActuatorControllerManager.h" +#include "Current.h" +#include "DCMotor.h" +#include "LookupTable.h" +#include "OpenLoop.h" +#include "PID.h" +#include "Position.h" +#include "Quadrature64CPR.h" +#include "Velocity.h" +#include "hw_bridge.h" + +class WristModule final : public Module { + public: + WristModule(); + HWBRIDGE::CANSignalValue_t leftWristSetPoint = 0; + HWBRIDGE::CANSignalValue_t rightWristSetPoint = 0; + + void periodic_1s(void) override; + void periodic_10s(void) override; + void periodic_100ms(void) override; + void periodic_10ms(void) override; + void periodic_1ms(CANInterface& can) override; + + Encoder::Quadrature64CPR leftEncoder; + Encoder::Quadrature64CPR rightEncoder; + + Actuator::DCMotor leftMotor; + Actuator::DCMotor rightMotor; + + /* TODO: Can we use wrist limit switches if motors are individually addressable */ + + // static Sensor::CurrentSensor leftCurrentSensor(WRSTL_CRNT_SNS_SPI_CLK, WRSTL_CRNT_SNS_SPI_MISO, + // WRSTL_CRNT_SNS_SPI_CS), + // rightCurrentSensor(WRSTR_CRNT_SNS_SPI_CLK, WRSTR_CRNT_SNS_SPI_MISO, WRSTR_CRNT_SNS_SPI_CS); + // TODO: Add once current sensor driver works + + PID::PID leftVelPID({1, 0, 0, -1, 1, 0, false, false}); + PID::PID leftPosPID({1, 0, 0, -1, 1, 0, false, false}); + PID::PID leftCurPID({1, 0, 0, -1, 1, 0, false, false}); + + PID::PID rightVelPID({1, 0, 0, -1, 1, 0, false, false}); + PID::PID rightPosPID({1, 0, 0, -1, 1, 0, false, false}); + PID::PID rightCurPID({1, 0, 0, -1, 1, 0, false, false}); + + constexpr uint8_t POLOLUMAXCURRENT = 3; + constexpr float POLOLUMAXDEGPERSEC = std::numeric_limits::infinity(); + // TODO: figure out MAXDEGPERSEC of motors (1680?) + + Controller::Velocity leftVel; + Controller::Position leftPos; + // Controller::Current leftCur(leftMotor, leftEncoder, std::nullopt, leftCurPID, POLOLUMAXDEGPERSEC, + // + Controller::OpenLoop leftOpen; + + Controller::Velocity rightVel; + Controller::Position rightPos; + // Controller::Current rightCur(rightMotor, rightEncoder, std::nullopt, rightCurPID, POLOLUMAXDEGPERSEC, + // + Controller::OpenLoop rightOpen; + + const Controller::ControlMap leftLut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &leftVel}, + {HWBRIDGE::CONTROL::Mode::POSITION, &leftPos}, + // {HWBRIDGE::CONTROL::Mode::CURRENT, &leftCur}, + {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &leftOpen}}; + const Controller::ControlMap rightLut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &rightVel}, + {HWBRIDGE::CONTROL::Mode::POSITION, &rightPos}, + // {HWBRIDGE::CONTROL::Mode::CURRENT, &rightCur}, + {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &rightOpen}}; + Controller::ActuatorControllerManager leftManager; + Controller::ActuatorControllerManager rightManager; + + private: +} \ No newline at end of file diff --git a/rover-apps/arm_2021/src/Modules/ClawModule.cpp b/rover-apps/arm_2021/src/Modules/ClawModule.cpp new file mode 100644 index 00000000..6063f636 --- /dev/null +++ b/rover-apps/arm_2021/src/Modules/ClawModule.cpp @@ -0,0 +1,53 @@ +#pragma once + +#include "ClawModule.h" + +ClawModule::ClawModule() + : clawSetPoint(0), + encoder(Encoder::Quadrature64CPR({ENC_QUAD_CLAW_A, ENC_QUAD_CLAW_B, NC, 0})), + motor(Actuator::DCMotor(MTR_PWM_TRNTBL, MTR_DIR_TRNTBL, false)), + velPid(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + posPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + curPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + vel(Controller::Velocity(motor, encoder, std::nullopt, velPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, LIM_CLAW_OPEN, + NC)), + pos(Controller::Position(motor, encoder, std::nullopt, posPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, LIM_CLAW_OPEN, + NC)), + open(Controller::OpenLoop(motor, encoder, std::nullopt, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, LIM_CLAW_OPEN, NC)), + manager(Controller::ActuatorControllerManager(lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP)) {} + +void periodic_1ms(CANInterface& can) override { + // Determine new claw set point + switch (Claw::manager.getActiveControlMode()) { + case HWBRIDGE::CONTROL::Mode::OPEN_LOOP: + case HWBRIDGE::CONTROL::Mode::POSITION: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_SET_CLAW_POSITION, + clawSetPoint); + clawSetPoint = RAD_TO_DEG(clawSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::VELOCITY: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_SET_CLAW_ANGULAR_VELOCITY, clawSetPoint); + clawSetPoint = RAD_TO_DEG(clawSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::CURRENT: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_CURRENT, HWBRIDGE::CANSIGNAL::ARM_SET_CLAW_CURRENT, + clawSetPoint); + break; + + default: + break; + } + + Claw::manager.getActiveController()->setSetPoint(static_cast(clawSetPoint)); + Claw::manager.getActiveController()->update(); + + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_REPORT_CLAW_POSITION, + DEG_TO_RAD(Claw::manager.getActiveController()->reportAngleDeg())); + + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_REPORT_CLAW_ANGULAR_VELOCITY, + DEG_TO_RAD(Claw::manager.getActiveController()->reportAngularVelocityDegPerSec())); +} diff --git a/rover-apps/arm_2021/src/Modules/ElbowModule.cpp b/rover-apps/arm_2021/src/Modules/ElbowModule.cpp new file mode 100644 index 00000000..bbda8046 --- /dev/null +++ b/rover-apps/arm_2021/src/Modules/ElbowModule.cpp @@ -0,0 +1,56 @@ +#pragma once + +#include "ElbowModule.h" + +ElbowModule::ElbowModule() + : elbowSetPoint(0), + encoder(Encoder::AEAT6012({ELBW_ENC_SPI_CLK, ELBW_ENC_SPI_MISO, NC, 0})), + motor(Actuator::DCMotor(MTR_PWM_ELBW, MTR_DIR_ELBW, false)), + velPid(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + posPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + curPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + vel(Controller::Velocity(motor, encoder, std::nullopt, velPID, ASSUNMAXDEGPERSEC, ASSUNMAXCURRENT, LIM_ELBW_DN, + LIM_ELBW_UP)), + pos(Controller::Position(motor, encoder, std::nullopt, posPID, ASSUNMAXDEGPERSEC, ASSUNMAXCURRENT, LIM_ELBW_DN, + LIM_ELBW_UP)), + open(Controller::OpenLoop(motor, encoder, std::nullopt, ASSUNMAXDEGPERSEC, ASSUNMAXCURRENT, LIM_ELBW_DN, + LIM_ELBW_UP)), + manager(Controller::ActuatorControllerManager(lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP)) {} + +void periodic_1ms(CANInterface& can) override { + // Determine new elbow set point + switch (Elbow::manager.getActiveControlMode()) { + case HWBRIDGE::CONTROL::Mode::OPEN_LOOP: + case HWBRIDGE::CONTROL::Mode::POSITION: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_SET_ELBOW_POSITION, + elbowSetPoint); + elbowSetPoint = RAD_TO_DEG(elbowSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::VELOCITY: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_SET_ELBOW_ANGULAR_VELOCITY, elbowSetPoint); + elbowSetPoint = RAD_TO_DEG(elbowSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::CURRENT: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_CURRENT, HWBRIDGE::CANSIGNAL::ARM_SET_ELBOW_CURRENT, + elbowSetPoint); + break; + + default: + break; + } + +Elbow::manager.getActiveController()->setSetPoint(static_cast(elbowSetPoint)); + + Elbow::manager.getActiveController()->update(); + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_REPORT_ELBOW_POSITION, + DEG_TO_RAD(Elbow::manager.getActiveController()->reportAngleDeg())); + + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_REPORT_ELBOW_ANGULAR_VELOCITY, + DEG_TO_RAD(Elbow::manager.getActiveController()->reportAngularVelocityDegPerSec())); + + +} diff --git a/rover-apps/arm_2021/src/Modules/ShoulderModule.cpp b/rover-apps/arm_2021/src/Modules/ShoulderModule.cpp new file mode 100644 index 00000000..8fb6fdc3 --- /dev/null +++ b/rover-apps/arm_2021/src/Modules/ShoulderModule.cpp @@ -0,0 +1,52 @@ +#pragma once + +#include "ShoulderModule.h" + +ShoulderModule::ShoulderModule() + : turntableSetPoint(0), + encoder(Encoder::AEAT6012({SHLDR_ENC_SPI_CLK, SHLDR_ENC_SPI_MISO, SHLDR_ENC_SPI_CS, 0})), + motor(Actuator::DCMotor(MTR_PWM_SHLDR, MTR_DIR_SHLDR, false)), + velPid(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + posPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + curPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + vel(Controller::Velocity(motor, encoder, std::nullopt, velPID, PA04MAXDEGPERSEC, PA04MAXCURRENT, LIM_SHLDR_DN, + LIM_SHLDR_UP)), + pos(Controller::Position(motor, encoder, std::nullopt, posPID, PA04MAXDEGPERSEC, PA04MAXCURRENT, LIM_SHLDR_DN, + LIM_SHLDR_UP)), + open(Controller::OpenLoop(motor, encoder, std::nullopt, PA04MAXDEGPERSEC, PA04MAXCURRENT, LIM_SHLDR_DN, + LIM_SHLDR_UP)), + manager(Controller::ActuatorControllerManager(lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP)) {} + +void periodic_1ms(CANInterface& can) override { + // Determine new shoulder set point + switch (Shoulder::manager.getActiveControlMode()) { + case HWBRIDGE::CONTROL::Mode::OPEN_LOOP: + case HWBRIDGE::CONTROL::Mode::POSITION: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_SET_SHOULDER_POSITION, + shoulderSetPoint); + shoulderSetPoint = RAD_TO_DEG(shoulderSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::VELOCITY: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_SET_SHOULDER_ANGULAR_VELOCITY, shoulderSetPoint); + shoulderSetPoint = RAD_TO_DEG(shoulderSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::CURRENT: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_CURRENT, HWBRIDGE::CANSIGNAL::ARM_SET_SHOULDER_CURRENT, + shoulderSetPoint); + break; + + default: + break; + } + + manager.getActiveController()->setSetPoint(static_cast(shoulderSetPoint)); + manager.getActiveController()->update(); + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_REPORT_SHOULDER_POSITION, + DEG_TO_RAD(manager.getActiveController()->reportAngleDeg())); + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_REPORT_SHOULDER_ANGULAR_VELOCITY, + DEG_TO_RAD(manager.getActiveController()->reportAngularVelocityDegPerSec())); +} diff --git a/rover-apps/arm_2021/src/Modules/TooltipModule.cpp b/rover-apps/arm_2021/src/Modules/TooltipModule.cpp new file mode 100644 index 00000000..4b5cc49c --- /dev/null +++ b/rover-apps/arm_2021/src/Modules/TooltipModule.cpp @@ -0,0 +1,14 @@ +#pragma once + +#include "TooltipModule.h" + +ClawModule::ClawModule() + : toolTipPosition(0), clawTooltipServo(Actuator::LimServo(SRVO_PWM_TOOLTIP, 180.0, 2ms, 1ms)) {} + +void periodic_1ms(CANInterface& can) override { + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_SET_TOOL_TIP_POSITION, + toolTipPosition); + toolTipPosition = RAD_TO_DEG(toolTipPosition); + + Tooltip::clawTooltipServo.setValue(static_cast(toolTipPosition)); +} diff --git a/rover-apps/arm_2021/src/Modules/TurntableModule.cpp b/rover-apps/arm_2021/src/Modules/TurntableModule.cpp new file mode 100644 index 00000000..8fbdc942 --- /dev/null +++ b/rover-apps/arm_2021/src/Modules/TurntableModule.cpp @@ -0,0 +1,51 @@ +#pragma once + +#include "TurntableModule.h" + +TurntableModule::TurntableModule() + : turntableSetPoint(0), + encoder(Encoder::AEAT6012({TRNTBL_ENC_SPI_CLK, TRNTBL_ENC_SPI_MISO, NC, 0})), + motor(Actuator::DCMotor(MTR_PWM_TRNTBL, MTR_DIR_TRNTBL, false)), + velPid(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + posPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + curPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + vel(Controller::Velocity(motor, encoder, std::nullopt, velPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, + LIM_TRNTBL_RHS)), + pos(Controller::Position(motor, encoder, std::nullopt, posPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, + LIM_TRNTBL_RHS)), + open( + Controller::OpenLoop(motor, encoder, std::nullopt, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, LIM_TRNTBL_RHS)), + manager(Controller::ActuatorControllerManager(Internal::lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP)) {} + +void periodic_1ms(CANInterface& can) override { + switch (manager.getActiveControlMode()) { + case HWBRIDGE::CONTROL::Mode::OPEN_LOOP: + case HWBRIDGE::CONTROL::Mode::POSITION: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_SET_TURNTABLE_POSITION, + turntableSetPoint); + turntableSetPoint = RAD_TO_DEG(turntableSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::VELOCITY: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_SET_TURNTABLE_ANGULAR_VELOCITY, turntableSetPoint); + turntableSetPoint = RAD_TO_DEG(turntableSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::CURRENT: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_CURRENT, HWBRIDGE::CANSIGNAL::ARM_SET_TURNTABLE_CURRENT, + turntableSetPoint); + break; + + default: + break; + } + + manager.getActiveController()->setSetPoint(static_cast(turntableSetPoint)); + Turntable::manager.getActiveController()->update(); + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_REPORT_TURNTABLE_POSITION, + DEG_TO_RAD(Turntable::manager.getActiveController()->reportAngleDeg())); + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_REPORT_TURNTABLE_ANGULAR_VELOCITY, + DEG_TO_RAD(Turntable::manager.getActiveController()->reportAngularVelocityDegPerSec())); +} diff --git a/rover-apps/arm_2021/src/Modules/WristModule.cpp b/rover-apps/arm_2021/src/Modules/WristModule.cpp new file mode 100644 index 00000000..6e94dae8 --- /dev/null +++ b/rover-apps/arm_2021/src/Modules/WristModule.cpp @@ -0,0 +1,106 @@ +#pragma once + +#include "ElbowModule.h" + +ElbowModule::ElbowModule() + : elbowSetPoint(0), + leftEncoder(Encoder::Quadrature64CPR({ENC_QUAD_WRST_LHS_A, ENC_QUAD_WRST_LHS_B, NC, 0})), + leftEncoder(Encoder::Quadrature64CPR({ENC_QUAD_WRST_RHS_A, ENC_QUAD_WRST_RHS_B, NC, 0})), + leftMotor(Actuator::DCMotor(MTR_PWM_WRST_LHS, MTR_DIR_WRST_LHS, false)), + rightMotor(Actuator::DCMotor(MTR_PWM_WRST_RHS, MTR_DIR_WRST_RHS, false)), +/* TODO: Can we use wrist limit switches if motors are individually addressable */ + +// static Sensor::CurrentSensor leftCurrentSensor(WRSTL_CRNT_SNS_SPI_CLK, WRSTL_CRNT_SNS_SPI_MISO, +// WRSTL_CRNT_SNS_SPI_CS), +// rightCurrentSensor(WRSTR_CRNT_SNS_SPI_CLK, WRSTR_CRNT_SNS_SPI_MISO, WRSTR_CRNT_SNS_SPI_CS); +// TODO: Add once current sensor driver works + + leftVelPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + leftPosPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + leftCurPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + rightVelPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + rightPosPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + rightCurPID(PID::PID({1, 0, 0, -1, 1, 0, false, false})), + leftVel(Controller::Velocity(leftMotor, leftEncoder, std::nullopt, leftVelPID, POLOLUMAXDEGPERSEC, + POLOLUMAXCURRENT, NC, NC)), + leftPos(Controller::Position(leftMotor, leftEncoder, std::nullopt, leftPosPID, POLOLUMAXDEGPERSEC, + POLOLUMAXCURRENT, NC, NC)), + leftOpen(Controller::OpenLoop(leftMotor, leftEncoder, std::nullopt, POLOLUMAXDEGPERSEC, POLOLUMAXCURRENT, NC, + NC)), + rightVel(Controller::Velocity(rightMotor, rightEncoder, std::nullopt, rightVelPID, POLOLUMAXDEGPERSEC, + POLOLUMAXCURRENT, NC, NC))), + rightPos(Controller::Position(rightMotor, rightEncoder, std::nullopt, rightPosPID, POLOLUMAXDEGPERSEC, + POLOLUMAXCURRENT, NC, NC)), + rightOpen(Controller::OpenLoop(rightMotor, rightEncoder, std::nullopt, POLOLUMAXDEGPERSEC, POLOLUMAXCURRENT, NC, + NC)), + leftManager(Controller::ActuatorControllerManager(leftLut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP), + rightManager(Controller::ActuatorControllerManager(rightLut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP)) {} +) {} + +void periodic_1ms(CANInterface& can) override { + // Determine new left wrist set point + switch (Wrist::leftManager.getActiveControlMode()) { + case HWBRIDGE::CONTROL::Mode::OPEN_LOOP: + case HWBRIDGE::CONTROL::Mode::POSITION: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_SET_LEFT_WRIST_POSITION, + leftWristSetPoint); + leftWristSetPoint = RAD_TO_DEG(leftWristSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::VELOCITY: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_SET_LEFT_WRIST_ANGULAR_VELOCITY, leftWristSetPoint); + leftWristSetPoint = RAD_TO_DEG(leftWristSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::CURRENT: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_CURRENT, HWBRIDGE::CANSIGNAL::ARM_SET_LEFT_WRIST_CURRENT, + leftWristSetPoint); + break; + + default: + break; + } + + // Determine new right wrist set point + switch (Wrist::rightManager.getActiveControlMode()) { + case HWBRIDGE::CONTROL::Mode::OPEN_LOOP: + case HWBRIDGE::CONTROL::Mode::POSITION: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_SET_RIGHT_WRIST_POSITION, + rightWristSetPoint); + rightWristSetPoint = RAD_TO_DEG(rightWristSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::VELOCITY: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_SET_RIGHT_WRIST_ANGULAR_VELOCITY, rightWristSetPoint); + rightWristSetPoint = RAD_TO_DEG(rightWristSetPoint); + break; + + case HWBRIDGE::CONTROL::Mode::CURRENT: + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_CURRENT, HWBRIDGE::CANSIGNAL::ARM_SET_RIGHT_WRIST_CURRENT, + rightWristSetPoint); + break; + + default: + break; + } + + Wrist::leftManager.getActiveController()->setSetPoint(static_cast(leftWristSetPoint)); + Wrist::rightManager.getActiveController()->setSetPoint(static_cast(rightWristSetPoint)); + Wrist::leftManager.getActiveController()->update(); + Wrist::rightManager.getActiveController()->update(); + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_REPORT_LEFT_WRIST_POSITION, + DEG_TO_RAD(Wrist::leftManager.getActiveController()->reportAngleDeg())); + + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_POSITION, HWBRIDGE::CANSIGNAL::ARM_REPORT_RIGHT_WRIST_POSITION, + DEG_TO_RAD(Wrist::rightManager.getActiveController()->reportAngleDeg())); + + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_REPORT_LEFT_WRIST_ANGULAR_VELOCITY, + DEG_TO_RAD(Wrist::leftManager.getActiveController()->reportAngularVelocityDegPerSec())); + + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_JOINT_ANGULAR_VELOCITY, + HWBRIDGE::CANSIGNAL::ARM_REPORT_RIGHT_WRIST_ANGULAR_VELOCITY, + DEG_TO_RAD(Wrist::rightManager.getActiveController()->reportAngularVelocityDegPerSec())); +} diff --git a/rover-apps/arm_2021/src/NewMain.cpp b/rover-apps/arm_2021/src/NewMain.cpp new file mode 100644 index 00000000..3c9d8dd7 --- /dev/null +++ b/rover-apps/arm_2021/src/NewMain.cpp @@ -0,0 +1,275 @@ +#include + +#include "CANConfig.h" +#include "LimServo.h" +#include "Logger.h" +#include "TurntableModule.h" + +CANInterface can(CANConfig::config); + +// initialize modules +ClawModue claw(); +ElbowModule elbow(); +ShoulderModule shoulder(); +TooltipModule tooltip(); +TurntableModule turntable(); +WristModule wrist(); + +// initalize polling threads +Thread periodic_1ms_thread(osPriorityRealtime); + +void periodic_1ms() { + auto startTime = Kernel::Clock::now(); + // call each module's polling function periodic_1ms + + for (Module *module : gModules) { + module->periodic_1ms(); + } + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_FAULTS, HWBRIDGE::CANSIGNAL::ARM_NUM_CANRX_FAULTS, + can.getNumCANRXFaults()); + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_FAULTS, HWBRIDGE::CANSIGNAL::ARM_NUM_CANTX_FAULTS, + can.getNumCANTXFaults()); + + // Report diagnostics + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_DIAGNOSTICS, + HWBRIDGE::CANSIGNAL::ARM_REPORT_NUM_STREAMED_MSGS_RECEIVED, can.getNumStreamedMsgsReceived()); + can.setTXSignalValue(HWBRIDGE::CANID::ARM_REPORT_DIAGNOSTICS, + HWBRIDGE::CANSIGNAL::ARM_REPORT_NUM_ONE_SHOT_MSGS_RECEIVED, can.getNumOneShotMsgsReceived()); + + auto nextStartTime = startTime + 1ms; + if (Kernel::Clock::now() > nextStartTime) { + Utility::logger << "Periodic 1ms task failed to hit the deadline!\n"; + } + ThisThread::sleep_until(nextStartTime); +} + +int main() { + Utility::logger << ""; // Band-aid fix for logger bug (issue #328) + + printf("\r\n\r\n"); + printf("ARM APPLICATION STARTED\r\n"); + printf("=======================\r\n"); + + // Set CAN filters + can.setFilter(HWBRIDGE::CANFILTER::ARM_RX_FILTER, CANStandard, HWBRIDGE::ROVER_CANID_FILTER_MASK, 0); + can.setFilter(HWBRIDGE::CANFILTER::COMMON_FILTER, CANStandard, HWBRIDGE::ROVER_CANID_FILTER_MASK, 1); + + // Start polling threads + periodic_1ms_thread.start(periodic_1ms); +} + +// Convert degrees to radians +static inline float DEG_TO_RAD(float deg) { + return deg * M_PI / 180.0f; +} + +// Convert radians to degrees +static inline float RAD_TO_DEG(float rad) { + return rad * 180.0f / M_PI; +} + +static mbed_error_status_t armSetControlMode(void) { + bool success = true; + HWBRIDGE::CANSignalValue_t controlMode; + + // Set turntable control mode + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_CONTROL_MODE, HWBRIDGE::CANSIGNAL::ARM_TURNTABLE_CONTROL_MODE, + controlMode)) { + if (static_cast(controlMode) != Turntable::manager.getActiveControlMode()) { + success &= Turntable::manager.switchControlMode(static_cast(controlMode)); + } + } else { + success = false; + } + + // Set shoulder control mode + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_CONTROL_MODE, HWBRIDGE::CANSIGNAL::ARM_SHOULDER_CONTROL_MODE, + controlMode)) { + if (static_cast(controlMode) != Shoulder::manager.getActiveControlMode()) { + success &= Shoulder::manager.switchControlMode(static_cast(controlMode)); + } + } else { + success = false; + } + + // Set elbow control mode + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_CONTROL_MODE, HWBRIDGE::CANSIGNAL::ARM_ELBOW_CONTROL_MODE, + controlMode)) { + if (static_cast(controlMode) != Elbow::manager.getActiveControlMode()) { + success &= Elbow::manager.switchControlMode(static_cast(controlMode)); + } + } else { + success = false; + } + + // Set left wrist control mode + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_CONTROL_MODE, HWBRIDGE::CANSIGNAL::ARM_LEFT_WRIST_CONTROL_MODE, + controlMode)) { + if (static_cast(controlMode) != Wrist::leftManager.getActiveControlMode()) { + success &= Wrist::leftManager.switchControlMode(static_cast(controlMode)); + } + } else { + success = false; + } + + // Set right wrist control mode + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_CONTROL_MODE, HWBRIDGE::CANSIGNAL::ARM_RIGHT_WRIST_CONTROL_MODE, + controlMode)) { + if (static_cast(controlMode) != Wrist::rightManager.getActiveControlMode()) { + success &= Wrist::rightManager.switchControlMode(static_cast(controlMode)); + } + } else { + success = false; + } + + // Set claw control mode + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_CONTROL_MODE, HWBRIDGE::CANSIGNAL::ARM_CLAW_CONTROL_MODE, + controlMode)) { + if (static_cast(controlMode) != Claw::manager.getActiveControlMode()) { + success &= Claw::manager.switchControlMode(static_cast(controlMode)); + } + } else { + success = false; + } + + if (success) { + // Send ACK message back + sendACK(HWBRIDGE::ARM_ACK_VALUES::ARM_SET_CONTROL_MODE_ACK); + } + + return success ? MBED_SUCCESS : MBED_ERROR_CODE_FAILED_OPERATION; +} + +static mbed_error_status_t armSetJointPIDParams(void) { + bool success = true; + + HWBRIDGE::CANSignalValue_t jointID; + HWBRIDGE::CANSignalValue_t p, i, d, deadzone; + + // Extract signal values + success &= + can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_PID_PARAMS, HWBRIDGE::CANSIGNAL::ARM_JOINT_PIDID, jointID); + success &= can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_PID_PARAMS, + HWBRIDGE::CANSIGNAL::ARM_JOINT_PID_PROPORTIONAL_GAIN, p); + success &= can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_PID_PARAMS, + HWBRIDGE::CANSIGNAL::ARM_JOINT_PID_INTEGRAL_GAIN, i); + success &= can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_PID_PARAMS, + HWBRIDGE::CANSIGNAL::ARM_JOINT_PID_DERIVATIVE_GAIN, d); + success &= can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_PID_PARAMS, + HWBRIDGE::CANSIGNAL::ARM_JOINT_PID_DEADZONE, deadzone); + + if (success) { + const Utility::LookupTable lut = { + {HWBRIDGE::ARM_JOINT_PIDID_VALUES::TURNTABLE, &Turntable::manager}, + {HWBRIDGE::ARM_JOINT_PIDID_VALUES::SHOULDER, &Shoulder::manager}, + {HWBRIDGE::ARM_JOINT_PIDID_VALUES::ELBOW, &Elbow::manager}, + {HWBRIDGE::ARM_JOINT_PIDID_VALUES::LEFT_WRIST, &Wrist::leftManager}, + {HWBRIDGE::ARM_JOINT_PIDID_VALUES::RIGHT_WRIST, &Wrist::rightManager}, + {HWBRIDGE::ARM_JOINT_PIDID_VALUES::CLAW, &Claw::manager}}; + + auto act = lut.at(static_cast(jointID)).value_or(nullptr); + if (!act) { + return MBED_ERROR_INVALID_ARGUMENT; + } + + // Set PID params + if (auto pid = act->getActiveController()->getPID()) { + pid.value().get().updateProportionalGain(p); + pid.value().get().updateIntegralGain(i); + pid.value().get().updateDerivativeGain(d); + pid.value().get().updateDeadzone(deadzone); + + // Send ACK message back + sendACK(HWBRIDGE::ARM_ACK_VALUES::ARM_SET_JOINT_PID_PARAMS_ACK); + + } else { + // PID controller doesn't exist! + success = false; + } + } + + return success ? MBED_SUCCESS : MBED_ERROR_CODE_FAILED_OPERATION; +} + +static mbed_error_status_t armSetSafetyCheck(void) { + bool success = true; + HWBRIDGE::CANSignalValue_t jointID; + HWBRIDGE::CANSignalValue_t safetyCheck; + + // Get joint ID to apply safety checks to + success = can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_SAFETY_CHECK, + HWBRIDGE::CANSIGNAL::ARM_SAFETY_CHECK_JOINT_ID, jointID); + + if (success) { + const Utility::LookupTable + lut = {{HWBRIDGE::ARM_SAFETY_CHECK_JOINT_ID_VALUES::TURNTABLE, &Turntable::manager}, + {HWBRIDGE::ARM_SAFETY_CHECK_JOINT_ID_VALUES::SHOULDER, &Shoulder::manager}, + {HWBRIDGE::ARM_SAFETY_CHECK_JOINT_ID_VALUES::ELBOW, &Elbow::manager}, + {HWBRIDGE::ARM_SAFETY_CHECK_JOINT_ID_VALUES::LEFT_WRIST, &Wrist::leftManager}, + {HWBRIDGE::ARM_SAFETY_CHECK_JOINT_ID_VALUES::RIGHT_WRIST, &Wrist::rightManager}, + {HWBRIDGE::ARM_SAFETY_CHECK_JOINT_ID_VALUES::CLAW, &Claw::manager}}; + + auto act = lut.at(static_cast(jointID)).value_or(nullptr); + if (!act) { + return MBED_ERROR_INVALID_ARGUMENT; + } + auto temp = act->getActiveController(); + + // Current check + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_SAFETY_CHECK, HWBRIDGE::CANSIGNAL::ARM_JOINT_CURRENT_CHECK, + safetyCheck)) { + static_cast(safetyCheck) ? temp->activateCurrentChecks() : temp->deactivateCurrentChecks(); + } else { + success = false; + } + + // Angular velocity check + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_SAFETY_CHECK, + HWBRIDGE::CANSIGNAL::ARM_JOINT_ANGULAR_VELOCITY_CHECK, safetyCheck)) { + static_cast(safetyCheck) ? temp->activateDegPerSecChecks() : temp->deactivateDegPerSecChecks(); + } else { + success = false; + } + + // Limit switch check + if (can.getRXSignalValue(HWBRIDGE::CANID::ARM_SET_JOINT_SAFETY_CHECK, + HWBRIDGE::CANSIGNAL::ARM_JOINT_LIMIT_SWITCH_CHECK, safetyCheck)) { + static_cast(safetyCheck) ? temp->activateLimitSwitchChecks() : temp->deactivateLimitSwitchChecks(); + } else { + success = false; + } + } + + return success ? MBED_SUCCESS : MBED_ERROR_CODE_FAILED_OPERATION; +} + +static mbed_error_status_t commonSwitchCANBus(void) { + bool success = true; + HWBRIDGE::CANSignalValue_t canBusID; + + success &= + can.getRXSignalValue(HWBRIDGE::CANID::COMMON_SWITCH_CAN_BUS, HWBRIDGE::CANSIGNAL::COMMON_CAN_BUS_ID, canBusID) && + can.switchCANBus(static_cast(canBusID)); + + if (success) { + // Send ACK message back + sendACK(HWBRIDGE::ARM_ACK_VALUES::CAN_BUS_SWITCH_ACK); + } + + return success ? MBED_SUCCESS : MBED_ERROR_CODE_FAILED_OPERATION; +} + +static void sendACK(HWBRIDGE::ARM_ACK_VALUES ackValue) { + struct uwrt_mars_rover_can_arm_report_ack_t ackMsgStruct = { + .arm_ack = static_cast(ackValue), + }; + + HWBRIDGE::CANMsgData_t ackMsgData; + uwrt_mars_rover_can_arm_report_ack_pack(ackMsgData.raw, &ackMsgStruct, UWRT_MARS_ROVER_CAN_ARM_REPORT_ACK_LENGTH); + + CANMsg msgACK; + msgACK.setID(HWBRIDGE::CANID::ARM_REPORT_ACK); + msgACK.setPayload(ackMsgData, UWRT_MARS_ROVER_CAN_ARM_REPORT_ACK_LENGTH); + + can.sendOneShotMessage(msgACK, 1ms); +}