From 7cd4568dffde851398fb99e879601c089d901877 Mon Sep 17 00:00:00 2001 From: NottheIRS <92690588+NottheIRS@users.noreply.github.com> Date: Fri, 15 Mar 2024 18:41:32 -0400 Subject: [PATCH] Added config messages for inversions, increased number of inversion retries from 10 to 50, changed config messages to show whether the intake or launcher is being configured. --- src/main/cpp/NeoDriveMotor.cpp | 9 ++ src/main/cpp/NeoTurnMotor.cpp | 18 ++++ src/main/cpp/subsystems/IntakeHAL.cpp | 112 +++++++++++++++++------- src/main/cpp/subsystems/LauncherHAL.cpp | 56 ++++++++---- 4 files changed, 142 insertions(+), 53 deletions(-) diff --git a/src/main/cpp/NeoDriveMotor.cpp b/src/main/cpp/NeoDriveMotor.cpp index 8e7a1da..d221586 100644 --- a/src/main/cpp/NeoDriveMotor.cpp +++ b/src/main/cpp/NeoDriveMotor.cpp @@ -46,6 +46,15 @@ void NeoDriveMotor::Configure(SwerveDriveMotorConfig &config){ successful = true; } } + + if (successful) + { + std::cout << "Configured NEO_DRIVE_MTR_SET_IDLE_MODE in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure NEO_DRIVE_MTR_SET_IDLE_MODE" << std::endl; + } } START_RETRYING(NEO_DRIVE_MTR_SET_SMART_CURRENT_LIMIT) diff --git a/src/main/cpp/NeoTurnMotor.cpp b/src/main/cpp/NeoTurnMotor.cpp index 1b84697..9c56d08 100644 --- a/src/main/cpp/NeoTurnMotor.cpp +++ b/src/main/cpp/NeoTurnMotor.cpp @@ -29,6 +29,15 @@ void NeoTurnMotor::Configure(SwerveTurnMotorConfig &config){ successful = true; } } + + if (successful) + { + std::cout << "Configured NEO_TURN_MTR_SET_IDLE_MODE in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure NEO_TURN_MTR_SET_IDLE_MODE" << std::endl; + } } START_RETRYING(NEO_TURN_PID_SETP) @@ -64,6 +73,15 @@ void NeoTurnMotor::Configure(SwerveTurnMotorConfig &config){ successful = true; } } + + if (successful) + { + std::cout << "Configured NEO_TURN_MTR_SET_INVERTED in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure NEO_TURN_MTR_SET_INVERTED" << std::endl; + } } START_RETRYING(NEO_TURN_MTR_ENABLE_VOLTAGE_COMPENSATION) diff --git a/src/main/cpp/subsystems/IntakeHAL.cpp b/src/main/cpp/subsystems/IntakeHAL.cpp index 9561702..23cf7c6 100644 --- a/src/main/cpp/subsystems/IntakeHAL.cpp +++ b/src/main/cpp/subsystems/IntakeHAL.cpp @@ -3,29 +3,29 @@ IntakeHAL::IntakeHAL(){ - START_RETRYING(LFT_ACT_MTR_RESTORE_FACTORY_DEFAULT) + START_RETRYING(INTAKE_LFT_ACT_MTR_RESTORE_FACTORY_DEFAULT) m_LFTActMotor.RestoreFactoryDefaults(); END_RETRYING - START_RETRYING(LFT_PVT_MTR_RESTORE_FACTORY_DEFAULT) + START_RETRYING(INTAKE_LFT_PVT_MTR_RESTORE_FACTORY_DEFAULT) m_LFTPvtMotor.RestoreFactoryDefaults(); END_RETRYING - START_RETRYING(RGT_ACT_MTR_RESTORE_FACTORY_DEFAULT) + START_RETRYING(INTAKE_RGT_ACT_MTR_RESTORE_FACTORY_DEFAULT) m_RGTActMotor.RestoreFactoryDefaults(); END_RETRYING - START_RETRYING(RGT_PVT_MTR_RESTORE_FACTORY_DEFAULT) + START_RETRYING(INTAKE_RGT_PVT_MTR_RESTORE_FACTORY_DEFAULT) m_RGTPvtMotor.RestoreFactoryDefaults(); END_RETRYING - START_RETRYING(LFT_ACT_MTR_SET_CAN_TIMEOUT) + START_RETRYING(INTAKE_LFT_ACT_MTR_SET_CAN_TIMEOUT) m_LFTActMotor.SetCANTimeout(50); END_RETRYING - START_RETRYING(LFT_PVT_MTR_SET_CAN_TIMEOUT) + START_RETRYING(INTAKE_LFT_PVT_MTR_SET_CAN_TIMEOUT) m_LFTPvtMotor.SetCANTimeout(50); END_RETRYING - START_RETRYING(RGT_ACT_MTR_SET_CAN_TIMEOUT) + START_RETRYING(INTAKE_RGT_ACT_MTR_SET_CAN_TIMEOUT) m_RGTActMotor.SetCANTimeout(50); END_RETRYING - START_RETRYING(RGT_PVT_MTR_SET_CAN_TIMEOUT) + START_RETRYING(INTAKE_RGT_PVT_MTR_SET_CAN_TIMEOUT) m_RGTPvtMotor.SetCANTimeout(50); END_RETRYING @@ -33,7 +33,7 @@ IntakeHAL::IntakeHAL(){ bool successful = false; int retries = 0; - while (!successful && retries <= 10) + while (!successful && retries <= 50) { retries++; m_RGTActMotor.SetInverted(INVERT_INTAKE_ACT); @@ -43,52 +43,60 @@ IntakeHAL::IntakeHAL(){ successful = true; } } + + if (successful) + { + std::cout << "Configured INTAKE_RGT_ACT_MTR_SET_INVERTED in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure INTAKE_RGT_ACT_MTR_SET_INVERTED" << std::endl; + } } - START_RETRYING(LFT_ACT_MTR_FOLLOW) + START_RETRYING(INTAKE_LFT_ACT_MTR_FOLLOW) m_LFTActMotor.Follow(m_RGTActMotor, true); END_RETRYING - START_RETRYING(RGT_PVT_MTR_FOLLOW) - + START_RETRYING(INTAKE_RGT_PVT_MTR_FOLLOW) m_RGTPvtMotor.Follow(m_LFTPvtMotor, false); END_RETRYING - START_RETRYING(LFT_ACT_MTR_ENABLE_VOLTAGE_COMPENSATION) + START_RETRYING(INTAKE_LFT_ACT_MTR_ENABLE_VOLTAGE_COMPENSATION) m_LFTActMotor.EnableVoltageCompensation(VOLT_COMP); END_RETRYING - START_RETRYING(LFT_PVT_MTR_ENABLE_VOLTAGE_COMPENSATION) + START_RETRYING(INTAKE_LFT_PVT_MTR_ENABLE_VOLTAGE_COMPENSATION) m_LFTPvtMotor.EnableVoltageCompensation(VOLT_COMP); END_RETRYING - START_RETRYING(RGT_ACT_MTR_ENABLE_VOLTAGE_COMPENSATION) + START_RETRYING(INTAKE_RGT_ACT_MTR_ENABLE_VOLTAGE_COMPENSATION) m_RGTActMotor.EnableVoltageCompensation(VOLT_COMP); END_RETRYING - START_RETRYING(RGT_PVT_MTR_ENABLE_VOLTAGE_COMPENSATION) + START_RETRYING(INTAKE_RGT_PVT_MTR_ENABLE_VOLTAGE_COMPENSATION) m_RGTPvtMotor.EnableVoltageCompensation(VOLT_COMP); END_RETRYING - START_RETRYING(LFT_PVT_PID_SETP) + START_RETRYING(INTAKE_LFT_PVT_PID_SETP) m_LFTPvtPID.SetP(INTAKE_P); END_RETRYING - START_RETRYING(LFT_PVT_PID_SETI) + START_RETRYING(INTAKE_LFT_PVT_PID_SETI) m_LFTPvtPID.SetI(INTAKE_I); END_RETRYING - START_RETRYING(LFT_PVT_PID_SETD) + START_RETRYING(INTAKE_LFT_PVT_PID_SETD) m_LFTPvtPID.SetD(INTAKE_D); END_RETRYING - START_RETRYING(LFT_ACT_MTR_SET_SMART_CURRENT_LIMIT) + START_RETRYING(INTAKE_LFT_ACT_MTR_SET_SMART_CURRENT_LIMIT) m_LFTActMotor.SetSmartCurrentLimit(INTAKE_ACT_CURRENT_LIMIT); END_RETRYING - START_RETRYING(LFT_PVT_MTR_SET_SMART_CURRENT_LIMIT) + START_RETRYING(INTAKE_LFT_PVT_MTR_SET_SMART_CURRENT_LIMIT) m_LFTPvtMotor.SetSmartCurrentLimit(INTAKE_PVT_CURRENT_LIMIT); END_RETRYING - START_RETRYING(RGT_ACT_MTR_SET_SMART_CURRENT_LIMIT) + START_RETRYING(INTAKE_RGT_ACT_MTR_SET_SMART_CURRENT_LIMIT) m_RGTActMotor.SetSmartCurrentLimit(INTAKE_ACT_CURRENT_LIMIT); END_RETRYING - START_RETRYING(RGT_PVT_MTR_SET_SMART_CURRENT_LIMIT) + START_RETRYING(INTAKE_RGT_PVT_MTR_SET_SMART_CURRENT_LIMIT) m_RGTPvtMotor.SetSmartCurrentLimit(INTAKE_PVT_CURRENT_LIMIT); END_RETRYING - START_RETRYING(LFT_PVT_PID_SET_FEEDBACK_DEVICE) + START_RETRYING(INTAKE_LFT_PVT_PID_SET_FEEDBACK_DEVICE) m_LFTPvtPID.SetFeedbackDevice(m_LFTPvtAbsEncoder); END_RETRYING @@ -96,7 +104,7 @@ IntakeHAL::IntakeHAL(){ bool successful = false; int retries = 0; - while (!successful && retries <= 10) + while (!successful && retries <= 50) { retries++; m_LFTPvtMotor.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); @@ -106,13 +114,22 @@ IntakeHAL::IntakeHAL(){ successful = true; } } + + if (successful) + { + std::cout << "Configured INTAKE_LFT_PVT_MTR_SET_IDLE_MODE in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure INTAKE_LFT_PVT_MTR_SET_IDLE_MODE" << std::endl; + } } { bool successful = false; int retries = 0; - while (!successful && retries <= 10) + while (!successful && retries <= 50) { retries++; m_RGTPvtMotor.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); @@ -122,13 +139,22 @@ IntakeHAL::IntakeHAL(){ successful = true; } } + + if (successful) + { + std::cout << "Configured INTAKE_RGT_PVT_MTR_SET_IDLE_MODE in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure INTAKE_RGT_PVT_MTR_SET_IDLE_MODE" << std::endl; + } } { bool successful = false; int retries = 0; - while (!successful && retries <= 10) + while (!successful && retries <= 50) { retries++; m_LFTPvtAbsEncoder.SetInverted(LFT_PVT_ABS_ENC_INVERTED); @@ -138,12 +164,21 @@ IntakeHAL::IntakeHAL(){ successful = true; } } + + if (successful) + { + std::cout << "Configured INTAKE_LFT_PVT_ABS_ENC_SET_INVERTED in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure INTAKE_LFT_PVT_ABS_ENC_SET_INVERTED" << std::endl; + } } - START_RETRYING(LFT_PVT_ABS_ENC_SET_POSITION_CONVERSION_FACTOR) + START_RETRYING(INTAKE_LFT_PVT_ABS_ENC_SET_POSITION_CONVERSION_FACTOR) m_LFTPvtAbsEncoder.SetPositionConversionFactor(LFT_PVT_ABS_ENC_CONVERSION_FACTOR); END_RETRYING - START_RETRYING(LFT_PVT_ABS_ENC_SET_ZERO_OFFSET) + START_RETRYING(INTAKE_LFT_PVT_ABS_ENC_SET_ZERO_OFFSET) m_LFTPvtAbsEncoder.SetZeroOffset(INTAKE_ZERO_OFFSET); END_RETRYING @@ -151,7 +186,7 @@ IntakeHAL::IntakeHAL(){ bool successful = false; int retries = 0; - while (!successful && retries <= 10) + while (!successful && retries <= 50) { retries++; m_LFTPvtMotor.SetInverted(LFT_PVT_MTR_INVERTED); @@ -161,18 +196,27 @@ IntakeHAL::IntakeHAL(){ successful = true; } } + + if (successful) + { + std::cout << "Configured INTAKE_LFT_PVT_MTR in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure INTAKE_LFT_PVT_MTR" << std::endl; + } } - START_RETRYING(LFT_ACT_MTR_BURN_FLASH) + START_RETRYING(INTAKE_LFT_ACT_MTR_BURN_FLASH) m_LFTActMotor.BurnFlash(); END_RETRYING - START_RETRYING(LFT_PVT_MTR_BURN_FLASH) + START_RETRYING(INTAKE_LFT_PVT_MTR_BURN_FLASH) m_LFTPvtMotor.BurnFlash(); END_RETRYING - START_RETRYING(RGT_ACT_MTR_BURN_FLASH) + START_RETRYING(INTAKE_RGT_ACT_MTR_BURN_FLASH) m_RGTActMotor.BurnFlash(); END_RETRYING - START_RETRYING(RGT_PVT_MTR_BURN_FLASH) + START_RETRYING(INTAKE_RGT_PVT_MTR_BURN_FLASH) m_RGTPvtMotor.BurnFlash(); END_RETRYING diff --git a/src/main/cpp/subsystems/LauncherHAL.cpp b/src/main/cpp/subsystems/LauncherHAL.cpp index 13b9f50..a9f5388 100644 --- a/src/main/cpp/subsystems/LauncherHAL.cpp +++ b/src/main/cpp/subsystems/LauncherHAL.cpp @@ -7,35 +7,35 @@ LauncherHAL::LauncherHAL() { - START_RETRYING(PVT_MTR_RESTORE_FACOTRY_DEFAULT) + START_RETRYING(LAUNCHER_PVT_MTR_RESTORE_FACOTRY_DEFAULT) m_PvtMotor.RestoreFactoryDefaults(); END_RETRYING - START_RETRYING(IND_MTR_RESTORE_FACOTRY_DEFAULT) + START_RETRYING(LAUNCHER_IND_MTR_RESTORE_FACOTRY_DEFAULT) m_IndMotor.RestoreFactoryDefaults(); END_RETRYING - START_RETRYING(PVT_MTR_SET_CAN_TIMEOUT) + START_RETRYING(LAUNCHER_PVT_MTR_SET_CAN_TIMEOUT) m_PvtMotor.SetCANTimeout(50); END_RETRYING - START_RETRYING(IND_MTR_RESTORE_FACOTRY_DEFAULT) + START_RETRYING(LAUNCHER_IND_MTR_RESTORE_FACOTRY_DEFAULT) m_IndMotor.SetCANTimeout(50); END_RETRYING - START_RETRYING(PVT_MTR_ENABLE_VOLTAGE_COMPENSATION) + START_RETRYING(LAUNCHER_PVT_MTR_ENABLE_VOLTAGE_COMPENSATION) m_PvtMotor.EnableVoltageCompensation(VOLT_COMP); END_RETRYING - START_RETRYING(IND_MTR_ENABLE_VOLTAGE_COMPENSATION) + START_RETRYING(LAUNCHER_IND_MTR_ENABLE_VOLTAGE_COMPENSATION) m_IndMotor.EnableVoltageCompensation(VOLT_COMP); END_RETRYING - START_RETRYING(PVT_MTR_SET_SMART_CURRENT_LIMIT) + START_RETRYING(LAUNCHER_PVT_MTR_SET_SMART_CURRENT_LIMIT) m_PvtMotor.SetSmartCurrentLimit(LAUNCHER_PVT_CURRENT_LIMIT); END_RETRYING - START_RETRYING(IND_MTR_SET_SMART_CURRENT_LIMIT) + START_RETRYING(LAUNCHER_IND_MTR_SET_SMART_CURRENT_LIMIT) m_IndMotor.SetSmartCurrentLimit(LAUNCHER_IND_CURRENT_LIMIT); END_RETRYING - START_RETRYING(PVT_MTR_SET_IDLE_MODE) + START_RETRYING(LAUNCHER_PVT_MTR_SET_IDLE_MODE) m_PvtMotor.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); END_RETRYING @@ -43,7 +43,7 @@ LauncherHAL::LauncherHAL() bool successful = false; int retries = 0; - while (!successful && retries <= 10) + while (!successful && retries <= 50) { retries++; m_PvtMotor.SetInverted(true); @@ -52,30 +52,39 @@ LauncherHAL::LauncherHAL() successful = true; } } + + if (successful) + { + std::cout << "Configured LAUNCHER_PVT_MTR_SET_INVERTED in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure LAUNCHER_PVT_MTR_SET_INVERTED in " << retries << " retries" << std::endl; + } } - START_RETRYING(PVT_MTR_BURN_FLASH) + START_RETRYING(LAUNCHER_PVT_MTR_BURN_FLASH) m_PvtMotor.BurnFlash(); END_RETRYING - START_RETRYING(IND_MTR_BURN_FLASH) + START_RETRYING(LAUNCHER_IND_MTR_BURN_FLASH) m_IndMotor.BurnFlash(); END_RETRYING - START_RETRYING(PVT_PID_SET_POSITION_PID_WRAPPING_ENABLED) + START_RETRYING(LAUNCHER_PVT_PID_SET_POSITION_PID_WRAPPING_ENABLED) m_PvtPID.SetPositionPIDWrappingEnabled(false); END_RETRYING - START_RETRYING(PVT_PID_SETP) + START_RETRYING(LAUNCHER_PVT_PID_SETP) m_PvtPID.SetP(PVT_P); END_RETRYING - START_RETRYING(PVT_PID_SETI) + START_RETRYING(LAUNCHER_PVT_PID_SETI) m_PvtPID.SetI(PVT_I); END_RETRYING - START_RETRYING(PVT_PID_SETD) + START_RETRYING(LAUNCHER_PVT_PID_SETD) m_PvtPID.SetD(PVT_D); END_RETRYING - START_RETRYING(PVT_PID_SET_FEEDBACK_DEVICE) + START_RETRYING(LAUNCHER_PVT_PID_SET_FEEDBACK_DEVICE) m_PvtPID.SetFeedbackDevice(m_PvtAbsEncoder); END_RETRYING @@ -83,7 +92,7 @@ LauncherHAL::LauncherHAL() bool successful = false; int retries = 0; - while (!successful && retries <= 10) + while (!successful && retries <= 50) { retries++; m_PvtAbsEncoder.SetInverted(true); @@ -93,6 +102,15 @@ LauncherHAL::LauncherHAL() successful = true; } } + + if (successful) + { + std::cout << "Configured LAUNCHER_PVT_ABS_ENC_SET_INVERTED in " << retries << " retries" << std::endl; + } + else + { + std::cout << "Failed to configure LAUNCHER_PVT_ABS_ENC_SET_INVERTED in " << retries << " retries" << std::endl; + } } START_RETRYING(PVT_ABS_ENC_SET_POSITION_CONVERSION_FACTOR) @@ -102,7 +120,7 @@ LauncherHAL::LauncherHAL() m_PvtAbsEncoder.SetZeroOffset(LAUNCHER_ZERO_OFFSET); END_RETRYING - std::cout << "Flashing complete" << std::endl; + std::cout << "Launcher flashing complete" << std::endl; ctre::phoenix6::configs::TalonFXConfiguration configs{};