From 241a2bfcec840f62ca634712e7a5db29ab3e8fd8 Mon Sep 17 00:00:00 2001 From: HeroOfWind243 <67348400+HeroOfWind243@users.noreply.github.com> Date: Tue, 6 May 2025 13:30:09 -0500 Subject: [PATCH 01/67] New robot definitions setup Removed SENTRY and STANDARD_WARTORTLE defines. Added SENTRY_BRAVO (renamed from SENTRY), SENTRY_SWERVE, and STANDARD_2025 defines --- aimbots-src/build_tools/extract_robot_type.py | 5 +- .../communication_message.hpp | 4 +- .../communication_request_commnads.hpp | 14 +-- .../communication_request_handler.cpp | 7 +- .../communication_request_handler.hpp | 12 +-- .../communication_request_transmiter.cpp | 11 ++- .../communication_request_transmiter.hpp | 9 +- .../communication_response_subsytem.cpp | 10 ++- .../robot-states/robot_state_interface.cpp | 6 +- .../robot-states/robot_state_interface.hpp | 4 +- .../constants/sentry_chassis_constants.hpp | 36 +++++++- .../constants/sentry_gimbal_constants.hpp | 85 +++++++++++-------- .../src/robots/sentry/sentry_control.cpp | 2 +- .../constants/standard_general_constants.hpp | 2 +- .../constants/standard_gimbal_constants.hpp | 2 +- .../sentry_match_chassis_control_command.cpp | 2 +- .../sentry_match_chassis_control_command.hpp | 10 ++- .../subsystems/chassis/control/chassis.cpp | 2 +- .../sentry_match_firing_control_command.cpp | 5 +- .../sentry_match_firing_control_command.hpp | 8 +- .../subsystems/shooter/control/shooter.cpp | 8 +- .../subsystems/shooter/control/shooter.hpp | 4 +- .../utils/tools/robot_specific_defines.hpp | 10 +-- aimbots-src/test/subsystems/chassis_tests.cpp | 5 +- aimbots-src/test/subsystems/gimbal_tests.cpp | 5 +- 25 files changed, 165 insertions(+), 103 deletions(-) diff --git a/aimbots-src/build_tools/extract_robot_type.py b/aimbots-src/build_tools/extract_robot_type.py index 7254a676..bae901ca 100644 --- a/aimbots-src/build_tools/extract_robot_type.py +++ b/aimbots-src/build_tools/extract_robot_type.py @@ -20,11 +20,12 @@ ROBOT_TYPE_FILE = "robot-type/robot_type.hpp" VALID_ROBOT_TYPES = [ "STANDARD_2023", "STANDARD_BLASTOISE", - "STANDARD_WARTORTLE", "STANDARD_SQUIRTLE", + "STANDARD_2025", "AERIAL", "ENGINEER", - "SENTRY", + "SENTRY_BRAVO", + "SENTRY_SWERVE", "HERO", "DART", "CVTEST_HAN", diff --git a/aimbots-src/src/communicators/ref_custom_serial/communication_message.hpp b/aimbots-src/src/communicators/ref_custom_serial/communication_message.hpp index 8da2d703..4223bb91 100644 --- a/aimbots-src/src/communicators/ref_custom_serial/communication_message.hpp +++ b/aimbots-src/src/communicators/ref_custom_serial/communication_message.hpp @@ -4,7 +4,7 @@ #ifdef REF_COMM_COMPATIBLE namespace src::Communication { -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES struct robot_state_message_team { /* data */ uint16_t standardX, standardY, heroX, heroY, sentryX, sentryY; @@ -26,7 +26,7 @@ struct robot_state_message_enemy { #endif enum class MessageType : uint8_t { -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES TEAM_MESSAGE_STANDARD, TEAM_MESSAGE_HERO, TEAM_MESSAGE_SENTRY, diff --git a/aimbots-src/src/communicators/ref_custom_serial/communication_request_commnads.hpp b/aimbots-src/src/communicators/ref_custom_serial/communication_request_commnads.hpp index 863fe66e..92c87b19 100644 --- a/aimbots-src/src/communicators/ref_custom_serial/communication_request_commnads.hpp +++ b/aimbots-src/src/communicators/ref_custom_serial/communication_request_commnads.hpp @@ -2,17 +2,18 @@ #include "tap/control/command.hpp" -//this doesn't exist -// #include "communication_outbound_subsystem.hpp" +// this doesn't exist +// #include "communication_outbound_subsystem.hpp" #ifdef REF_COMM_COMPATIBLE namespace src::Communication { -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES class TeamMessageStandard : public tap::control::Command { public: - TeamMessageStandard(CommunicationOutBoundSubsystem &communicationOutBoundSubsystem) : sub(communicationOutBoundSubsystem) { + TeamMessageStandard(CommunicationOutBoundSubsystem &communicationOutBoundSubsystem) + : sub(communicationOutBoundSubsystem) { this->addSubsyemRequirement(&communicationOutBoundSubsystem); } @@ -47,7 +48,8 @@ class TeamMessageHero : public tap::control::Command { #else class CommunicationMessage : public tap::control::Command { public: - communicationMessage(CommunicationOutBoundSubsystem &communicationOutBoundSubsystem) : sub(communicationOutBoundSubsystem) { + communicationMessage(CommunicationOutBoundSubsystem &communicationOutBoundSubsystem) + : sub(communicationOutBoundSubsystem) { this->addSubsyemRequirement(&communicationOutBoundSubsystem); } @@ -82,4 +84,4 @@ class EnemyStateMessage : public tap::control::Command { #endif } // namespace src::Communication -#endif //#ifdef REF_COMM_COMPATIBLE +#endif //#ifdef REF_COMM_COMPATIBLE diff --git a/aimbots-src/src/communicators/ref_custom_serial/communication_request_handler.cpp b/aimbots-src/src/communicators/ref_custom_serial/communication_request_handler.cpp index bfe48b9e..aa0cc7b1 100644 --- a/aimbots-src/src/communicators/ref_custom_serial/communication_request_handler.cpp +++ b/aimbots-src/src/communicators/ref_custom_serial/communication_request_handler.cpp @@ -18,10 +18,11 @@ namespace src::Communication { CommunicationRequestHandler::CommunicationRequestHandler(src::Drivers* drivers) : drivers(drivers) {} void CommunicationRequestHandler::operator()(const tap::communication::serial::DJISerial::ReceivedSerialMessage& message) { - MessageType type = static_cast(message.data[sizeof(tap::communication::serial::RefSerialData::Tx::InteractiveHeader)]); + MessageType type = + static_cast(message.data[sizeof(tap::communication::serial::RefSerialData::Tx::InteractiveHeader)]); switch (type) { -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES // case MessageType::TEAM_MESSAGE_STANDARD: // if (teamMesssageHandlerStandard != nullptr) { // updateRobotStateStandard(); @@ -51,4 +52,4 @@ void CommunicationRequestHandler::operator()(const tap::communication::serial::D } // namespace src::Communication -#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file +#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file diff --git a/aimbots-src/src/communicators/ref_custom_serial/communication_request_handler.hpp b/aimbots-src/src/communicators/ref_custom_serial/communication_request_handler.hpp index 396a77c0..b268de3e 100644 --- a/aimbots-src/src/communicators/ref_custom_serial/communication_request_handler.hpp +++ b/aimbots-src/src/communicators/ref_custom_serial/communication_request_handler.hpp @@ -3,10 +3,10 @@ #include "tap/communication/serial/ref_serial.hpp" // #include "tap/communication/serial/ref_serial_transmitter.hpp" -#include "modm/architecture/interface/register.hpp" -#include "modm/processing/protothread.hpp" #include "informants/robot-states/robot_state.hpp" #include "informants/robot-states/robot_state_interface.hpp" +#include "modm/architecture/interface/register.hpp" +#include "modm/processing/protothread.hpp" #include "drivers.hpp" @@ -23,7 +23,7 @@ class CommunicationRequestHandler : public tap::communication::serial::RefSerial bool recive(); void updateStates(); -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES void attachTeamMessageHandlerStandard(MessageReceivedCallback message) { teamMesssageHandlerStandard = message; } void attachTeamMessageHandlerHero(MessageReceivedCallback message) { teamMesssageHandlerHero = message; } @@ -34,7 +34,7 @@ class CommunicationRequestHandler : public tap::communication::serial::RefSerial private: src::Drivers* drivers; -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES MessageReceivedCallback teamMesssageHandlerStandard = nullptr; MessageReceivedCallback teamMesssageHandlerHero = nullptr; MessageReceivedCallback teamMessageHandlerSentry = nullptr; @@ -43,6 +43,6 @@ class CommunicationRequestHandler : public tap::communication::serial::RefSerial MessageReceivedCallback enemyRobotStateHandler = nullptr; #endif }; -} // namespace src::robotStates +} // namespace src::Communication -#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file +#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file diff --git a/aimbots-src/src/communicators/ref_custom_serial/communication_request_transmiter.cpp b/aimbots-src/src/communicators/ref_custom_serial/communication_request_transmiter.cpp index f2567573..9409146d 100644 --- a/aimbots-src/src/communicators/ref_custom_serial/communication_request_transmiter.cpp +++ b/aimbots-src/src/communicators/ref_custom_serial/communication_request_transmiter.cpp @@ -14,7 +14,9 @@ #ifdef REF_COMM_COMPATIBLE namespace src::Communication { -CommunicationRequestTransmiter::CommunicationRequestTransmiter(src::Drivers* drivers) : drivers(drivers), refSerial(drivers) {} +CommunicationRequestTransmiter::CommunicationRequestTransmiter(src::Drivers* drivers) + : drivers(drivers), + refSerial(drivers) {} bool CommunicationRequestTransmiter::send() { PT_BEGIN(); @@ -27,7 +29,8 @@ bool CommunicationRequestTransmiter::send() { PT_CALL(refSerial.sendRobotToRobotMsg( &robotToRobotMessage, SENTRY_REQUEST_ROBOT_ID, - drivers->refSerial.getRobotIdBasedOnCurrentRobotTeam(tap::communication::serial::RefSerialData::RobotId::BLUE_SENTINEL), + drivers->refSerial.getRobotIdBasedOnCurrentRobotTeam( + tap::communication::serial::RefSerialData::RobotId::BLUE_SENTINEL), 1)); } else { PT_YIELD(); @@ -43,7 +46,7 @@ bool CommunicationRequestTransmiter::send() { void CommunicationRequestTransmiter::updateQue(MessageType type) { queuedMessageType |= (1 << static_cast(type)); - // #ifdef TARGET_SENTRY + // #ifdef ALL_SENTRIES // // Team color = drivers->refSerial->getRobotData().robotID == 7 ? Team::RED : Team::BLUE; // Team color = Team::RED; // // Team color = drivers->getRobotData->robotData.robotId == RED_SENTINEL ? Team::RED : Team::BLUE; @@ -54,4 +57,4 @@ void CommunicationRequestTransmiter::updateQue(MessageType type) { } // namespace src::Communication -#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file +#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file diff --git a/aimbots-src/src/communicators/ref_custom_serial/communication_request_transmiter.hpp b/aimbots-src/src/communicators/ref_custom_serial/communication_request_transmiter.hpp index 472784f0..3f6ce00c 100644 --- a/aimbots-src/src/communicators/ref_custom_serial/communication_request_transmiter.hpp +++ b/aimbots-src/src/communicators/ref_custom_serial/communication_request_transmiter.hpp @@ -28,10 +28,10 @@ class CommunicationRequestTransmiter : public modm::pt::Protothread { // RobotStates* robotStateInterface; - // #ifdef TARGET_SENTRY + // #ifdef ALL_SENTRIES static constexpr uint16_t SENTRY_REQUEST_ROBOT_ID = 0x200; -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES MessageType lastSentMessage = MessageType::TEAM_MESSAGE_STANDARD; #else MessageType lastSentMessage = MessageType::ROBOT_STATE; @@ -51,7 +51,8 @@ class CommunicationRequestTransmiter : public modm::pt::Protothread { // otherwise, iterate through message types until you find one that is queued auto nextMessageType = [](MessageType type) { - return static_cast((static_cast(type) + 1) % static_cast(MessageType::NUM_MESSAGE_TYPES)); + return static_cast( + (static_cast(type) + 1) % static_cast(MessageType::NUM_MESSAGE_TYPES)); }; lastSentMessage = nextMessageType(lastSentMessage); @@ -72,4 +73,4 @@ class CommunicationRequestTransmiter : public modm::pt::Protothread { } // namespace src::Communication -#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file +#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file diff --git a/aimbots-src/src/communicators/ref_custom_serial/communication_response_subsytem.cpp b/aimbots-src/src/communicators/ref_custom_serial/communication_response_subsytem.cpp index dee8ce29..ebc82d8c 100644 --- a/aimbots-src/src/communicators/ref_custom_serial/communication_response_subsytem.cpp +++ b/aimbots-src/src/communicators/ref_custom_serial/communication_response_subsytem.cpp @@ -30,7 +30,7 @@ uint8_t value2; uint8_t value3; uint8_t value4; bool CommunicationResponseSubsytem::run() { -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES // ms = states.createMessage(); // uint16_t sx = ms.standardX; test = static_cast(rand() % 0xff); @@ -68,7 +68,8 @@ bool CommunicationResponseSubsytem::run() { this->robotToRobotMessage.dataAndCRC16[1] = value2; // static_cast(sx); this->robotToRobotMessage.dataAndCRC16[2] = value3; // static_cast(sx); this->robotToRobotMessage.dataAndCRC16[3] = value4; // static_cast(sy); - // this->robotToRobotMessage.dataAndCRC16[4] = static_cast(rand() % 0xff); // static_cast(sy); + // this->robotToRobotMessage.dataAndCRC16[4] = static_cast(rand() % 0xff); // + // static_cast(sy); // if (this->sentryMoving != this->getDriveStatus()) { // this->sentryMoving = this->getDriveStatus(); @@ -85,7 +86,8 @@ bool CommunicationResponseSubsytem::run() { PT_CALL(this->refSerialTransmitter.sendRobotToRobotMsg( &this->robotToRobotMessage, SENTRY_RESPONSE_MESSAGE_ID, - drivers.refSerial.getRobotIdBasedOnCurrentRobotTeam(tap::communication::serial::RefSerialData::RobotId::BLUE_HERO), + drivers.refSerial.getRobotIdBasedOnCurrentRobotTeam( + tap::communication::serial::RefSerialData::RobotId::BLUE_HERO), 4)); // } @@ -107,4 +109,4 @@ bool CommunicationResponseSubsytem::run() { } // namespace src::Communication -#endif // #ifdef REF_COMM_COMPATIBLE +#endif // #ifdef REF_COMM_COMPATIBLE diff --git a/aimbots-src/src/informants/robot-states/robot_state_interface.cpp b/aimbots-src/src/informants/robot-states/robot_state_interface.cpp index f6bc5c8a..899bdeac 100644 --- a/aimbots-src/src/informants/robot-states/robot_state_interface.cpp +++ b/aimbots-src/src/informants/robot-states/robot_state_interface.cpp @@ -52,7 +52,7 @@ void RobotStates::updateRobotStateHealth(int number, Team teamColor, int health) robotStates[teamColor][number - 1].setHealth(health); } -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES void RobotStates::updateTeamMessage() { // will update all of the struct messages for the coms // tap::communication::serial::RefSerial::RobotId id = &drivers->refSerial.getRobotData().robotId; @@ -113,7 +113,7 @@ void RobotStates::respond() { void RobotStates::refresh() { this->respond(); -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES // TODO:: the code that will update stuff based on the recived messages this->updateTeamMessage(); #endif @@ -126,4 +126,4 @@ void RobotStates::refresh() { } } // namespace src::RobotStates -#endif //#ifdef REF_COMM_COMPATIBLE \ No newline at end of file +#endif //#ifdef REF_COMM_COMPATIBLE \ No newline at end of file diff --git a/aimbots-src/src/informants/robot-states/robot_state_interface.hpp b/aimbots-src/src/informants/robot-states/robot_state_interface.hpp index 83088261..767a1a17 100644 --- a/aimbots-src/src/informants/robot-states/robot_state_interface.hpp +++ b/aimbots-src/src/informants/robot-states/robot_state_interface.hpp @@ -35,7 +35,7 @@ class RobotStates : public tap::control::Subsystem { void updateRobotStateHealth(int robotNumber, Team teamColor, int health); void updateRobotStatePosition(int robotNumber, Team teamColor, short x, short y, short z); -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES void updateTeamMessage(); uint8_t teamMessage[115]; #endif @@ -56,4 +56,4 @@ class RobotStates : public tap::control::Subsystem { } // namespace src::RobotStates -#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file +#endif // #ifdef REF_COMM_COMPATIBLE \ No newline at end of file diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index d2fd2728..518e0d1f 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -1,14 +1,46 @@ #pragma once -#include "utils/tools/common_types.hpp" #include "utils/math/matrix_helpers.hpp" +#include "utils/tools/common_types.hpp" #define CHASSIS_COMPATIBLE +#if defined(TARGET_SENTRY_SWERVE) +#define SWERVE +static constexpr uint8_t MOTORS_PER_WHEEL = 2; + +static constexpr MotorID LEFT_BACK_YAW_ID = MotorID::MOTOR5; +static constexpr MotorID LEFT_FRONT_YAW_ID = MotorID::MOTOR6; +static constexpr MotorID RIGHT_FRONT_YAW_ID = MotorID::MOTOR7; +static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; + +static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { + .kp = 1.0f, + .ki = 0.0f, + .kd = 0.01f, + .maxICumulative = 0.9f, + .maxOutput = 1.0f, + .tQDerivativeKalman = 1.0f, + .tRDerivativeKalman = 1.0f, + .tQProportionalKalman = 1.0f, + .tRProportionalKalman = 1.0f, + .errDeadzone = 0.0f, + .errorDerivativeFloor = 0.0f, +}; + +static constexpr int LEFT_FRONT_YAW_OFFSET = 0; +static constexpr int RIGHT_FRONT_YAW_OFFSET = 0; +static constexpr int LEFT_BACK_YAW_OFFSET = 0; +static constexpr int RIGHT_BACK_YAW_OFFSET = 0; + +#elif defined(TARGET_SENTRY_BRAVO) +static constexpr uint8_t MOTORS_PER_WHEEL = 1; + +#endif + /** * @brief Defines the number of motors created for the chassis. */ static constexpr uint8_t DRIVEN_WHEEL_COUNT = 4; -static constexpr uint8_t MOTORS_PER_WHEEL = 1; static constexpr float CHASSIS_VELOCITY_YAW_LOAD_FEEDFORWARD = 1.0f; static constexpr float CHASSIS_VELOCITY_PITCH_LOAD_FEEDFORWARD = 1.0f; diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index be327d88..31dc35ee 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -1,6 +1,6 @@ #pragma once -#include "utils/tools/common_types.hpp" #include "utils/math/matrix_helpers.hpp" +#include "utils/tools/common_types.hpp" #define GIMBAL_COMPATIBLE @@ -9,18 +9,38 @@ /** * @brief GIMBAL SETUP */ -static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS2; + static constexpr CANBus PITCH_GIMBAL_BUS = CANBus::CAN_BUS1; static constexpr uint8_t YAW_MOTOR_COUNT = 1; static constexpr uint8_t PITCH_MOTOR_COUNT = 1; +#if defined(TARGET_SENTRY_BRAVO) +static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS2; +/* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ +static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(42.58f))}; +static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(229.0f))}; + +static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-2.0f); +static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(30.0f); +// LOW should be lesser than HIGH, otherwise switch the motor direction + +#elif defined(TARGET_SENTRY_SWERVE) +static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS1; +/* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ +static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(42.58f))}; +static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(229.0f))}; + +static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-2.0f); +static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(30.0f); +// LOW should be lesser than HIGH, otherwise switch the motor direction + +#endif + static const std::array YAW_MOTOR_DIRECTIONS = {false}; static const std::array YAW_MOTOR_IDS = {MotorID::MOTOR5}; static const std::array YAW_MOTOR_NAMES = {"Yaw Motor 1"}; -/* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = { - wrapTo0To2PIRange(modm::toRadian(42.58f))}; // 198.2 + static constexpr float YAW_AXIS_START_ANGLE = modm::toRadian(0.0f); static constexpr float GIMBAL_YAW_GEAR_RATIO = (1.0f / 2.0f); // for 2024 Sentry @@ -31,9 +51,7 @@ static constexpr float GIMBAL_YAW_GEAR_RATIO = (1.0f / 2.0f); // for 2024 Sentr static const std::array PITCH_MOTOR_DIRECTIONS = {true}; static const std::array PITCH_MOTOR_IDS = {MotorID::MOTOR6}; static const std::array PITCH_MOTOR_NAMES = {"Pitch Motor 1"}; -static const std::array PITCH_MOTOR_OFFSET_ANGLES = { - wrapTo0To2PIRange(modm::toRadian(229.0f))}; // 60.6 -/* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ + static constexpr float PITCH_AXIS_START_ANGLE = modm::toRadian(0.0f); static constexpr float GIMBAL_PITCH_GEAR_RATIO = (30.0f / 102.0f); // for 2023 Sentry @@ -41,10 +59,6 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (30.0f / 102.0f); // for 2023 * encoder readings will repeat. We will assume that the range of the pitch axis is hardware-limited to not exceed this * range, but the motor angle may cross 0 in this range. Example Range: 278deg to 28deg */ -static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-2.0f); -static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(30.0f); -// LOW should be lesser than HIGH, otherwise switch the motor direction - /** * @brief Position PID constants */ @@ -138,33 +152,30 @@ static constexpr float kGRAVITY = 0.0f; static constexpr float HORIZON_OFFSET = 0.0f; const modm::Pair YAW_FEEDFORWARD_VELOCITIES[11] = { - {0.0f, 0.0f}, - {1.5f, 3'000.0f}, - {5.15f, 6'000.0f}, - {8.8f, 9'000.0f}, - {12.6f, 12'000.0f}, - {16.45f, 15'000.0f}, - {20.25f, 18'000.0f}, - {24.11f, 21'000.0f}, - {27.97f, 24'000.0f}, - {29.18f, 27'000.0f}, - {29.2f, 30'000.0f} - }; - + {0.0f, 0.0f}, + {1.5f, 3'000.0f}, + {5.15f, 6'000.0f}, + {8.8f, 9'000.0f}, + {12.6f, 12'000.0f}, + {16.45f, 15'000.0f}, + {20.25f, 18'000.0f}, + {24.11f, 21'000.0f}, + {27.97f, 24'000.0f}, + {29.18f, 27'000.0f}, + {29.2f, 30'000.0f}}; const modm::Pair PITCH_FEEDFORWARD_VELOCITIES[11] = { - {0.0f, 0.0f}, - {3.75f, 3'000.0f}, - {8.5f, 6'000.0f}, - {12.75f, 9'000.0f}, - {17.67f, 12'000.0f}, - {22.5f, 15'000.0f}, - {26.75f, 18'000.0f}, - {31.5f, 21'000.0f}, - {35.5f, 24'000.0f}, - {36.15f, 27'000.0f}, - {36.35f, 30'000.0f} - }; + {0.0f, 0.0f}, + {3.75f, 3'000.0f}, + {8.5f, 6'000.0f}, + {12.75f, 9'000.0f}, + {17.67f, 12'000.0f}, + {22.5f, 15'000.0f}, + {26.75f, 18'000.0f}, + {31.5f, 21'000.0f}, + {35.5f, 24'000.0f}, + {36.15f, 27'000.0f}, + {36.35f, 30'000.0f}}; // clang-format on const modm::interpolation::Linear> YAW_VELOCITY_FEEDFORWARD(YAW_FEEDFORWARD_VELOCITIES, 11); diff --git a/aimbots-src/src/robots/sentry/sentry_control.cpp b/aimbots-src/src/robots/sentry/sentry_control.cpp index 826b71f8..3d230680 100644 --- a/aimbots-src/src/robots/sentry/sentry_control.cpp +++ b/aimbots-src/src/robots/sentry/sentry_control.cpp @@ -338,4 +338,4 @@ void initializeSubsystemCommands(src::Drivers *drivers) { } } // namespace src::Control -#endif // TARGET_SENTRY \ No newline at end of file +#endif // ALL_SENTRIES \ No newline at end of file diff --git a/aimbots-src/src/robots/standard/constants/standard_general_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_general_constants.hpp index 657e6606..3ad8b0fc 100644 --- a/aimbots-src/src/robots/standard/constants/standard_general_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_general_constants.hpp @@ -50,7 +50,7 @@ static Vector3f BARREL_POSITION_FROM_GIMBAL_ORIGIN{ static constexpr float CHASSIS_START_ANGLE_WORLD = modm::toRadian(0.0f); // theta (about z axis) -#if defined(TARGET_STANDARD_BLASTOISE) || defined(TARGET_STANDARD_WARTORTLE) || defined(TARGET_STANDARD_SQUIRTLE) +#if defined(TARGET_STANDARD_BLASTOISE) || defined(TARGET_STANDARD_2025) || defined(TARGET_STANDARD_SQUIRTLE) static constexpr float CIMU_CALIBRATION_EULER_X = modm::toRadian(0.0f); static constexpr float CIMU_CALIBRATION_EULER_Y = modm::toRadian(0.0f); diff --git a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp index 60555737..2ff98625 100644 --- a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp @@ -22,7 +22,7 @@ static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-14.5f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(17.5f); // LOW should be lesser than HIGH, otherwise switch the motor direction -#elif defined(TARGET_STANDARD_WARTORTLE) +#elif defined(TARGET_STANDARD_2025) /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot cassis? */ static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(84.73f))}; static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(180.0f))}; diff --git a/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp b/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp index f9779497..f6d508bd 100644 --- a/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp +++ b/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp @@ -1,6 +1,6 @@ #include "sentry_match_chassis_control_command.hpp" -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES #ifdef CHASSIS_COMPATIBLE namespace src::Chassis { diff --git a/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.hpp b/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.hpp index 5152694d..6dc5df40 100644 --- a/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.hpp +++ b/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.hpp @@ -1,15 +1,17 @@ #pragma once -#ifdef TARGET_SENTRY +#include "utils/tools/robot_specific_defines.hpp" -#include "subsystems/chassis/control/chassis.hpp" +#ifdef ALL_SENTRIES + +#include "subsystems/chassis/basic_commands/chassis_tokyo_command.hpp" #include "subsystems/chassis/complex_commands/chassis_auto_nav_command.hpp" #include "subsystems/chassis/complex_commands/chassis_auto_nav_tokyo_command.hpp" -#include "subsystems/chassis/basic_commands/chassis_tokyo_command.hpp" +#include "subsystems/chassis/control/chassis.hpp" #include "subsystems/gimbal/control/gimbal.hpp" -#include "utils/tools/common_types.hpp" #include "utils/motion/auto_nav/auto_navigator_holonomic.hpp" #include "utils/ref_system/ref_helper_turreted.hpp" +#include "utils/tools/common_types.hpp" #include "drivers.hpp" diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 4dd1cbbf..9748f422 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -32,8 +32,8 @@ ChassisSubsystem::ChassisSubsystem(src::Drivers* drivers) rightBackYaw(drivers, RIGHT_BACK_YAW_ID, CHASSIS_BUS, false, "Right Back Yaw Motor"), leftBackYawPosPID(CHASSIS_YAW_PID_CONFIG), leftFrontYawPosPID(CHASSIS_YAW_PID_CONFIG), - rightBackYawPosPID(CHASSIS_YAW_PID_CONFIG), rightFrontYawPosPID(CHASSIS_YAW_PID_CONFIG), + rightBackYawPosPID(CHASSIS_YAW_PID_CONFIG), #endif targetRPMs(Matrix::zeroMatrix()), desiredOutputs(Matrix::zeroMatrix()), diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/sentry_match_firing_control_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/sentry_match_firing_control_command.cpp index 1cfcb304..f286535e 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/sentry_match_firing_control_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/sentry_match_firing_control_command.cpp @@ -1,8 +1,7 @@ - -#ifdef TARGET_SENTRY - #include "sentry_match_firing_control_command.hpp" +#ifdef ALL_SENTRIES + namespace src::Control { static constexpr int BASE_BURST_LENGTH = 3; diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/sentry_match_firing_control_command.hpp b/aimbots-src/src/subsystems/feeder/complex_commands/sentry_match_firing_control_command.hpp index d93a6131..9d4074c0 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/sentry_match_firing_control_command.hpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/sentry_match_firing_control_command.hpp @@ -1,5 +1,9 @@ #pragma once +#include "utils/tools/robot_specific_defines.hpp" + +#ifdef ALL_SENTRIES + #include "utils/ballistics/ballistics_solver.hpp" #include "utils/tools/common_types.hpp" @@ -71,4 +75,6 @@ class SentryMatchFiringControlCommand : public TapComprisedCommand { MilliTimeout shootMinimumTime; }; -} // namespace src::Control \ No newline at end of file +} // namespace src::Control + +#endif \ No newline at end of file diff --git a/aimbots-src/src/subsystems/shooter/control/shooter.cpp b/aimbots-src/src/subsystems/shooter/control/shooter.cpp index e181d2c8..2fada797 100644 --- a/aimbots-src/src/subsystems/shooter/control/shooter.cpp +++ b/aimbots-src/src/subsystems/shooter/control/shooter.cpp @@ -17,7 +17,7 @@ ShooterSubsystem::ShooterSubsystem(tap::Drivers* drivers, src::Utils::RefereeHel flywheel2(drivers, SHOOTER_2_ID, SHOOTER_BUS, SHOOTER_2_DIRECTION, "Flywheel Two"), flywheel1PID(SHOOTER_VELOCITY_PID_CONFIG), flywheel2PID(SHOOTER_VELOCITY_PID_CONFIG), -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES flywheel3(drivers, SHOOTER_3_ID, SHOOTER_BUS, SHOOTER_3_DIRECTION, "Flywheel Three"), flywheel4(drivers, SHOOTER_4_ID, SHOOTER_BUS, SHOOTER_4_DIRECTION, "Flywheel Four"), flywheel3PID(SHOOTER_VELOCITY_PID_CONFIG), @@ -35,7 +35,7 @@ ShooterSubsystem::ShooterSubsystem(tap::Drivers* drivers, src::Utils::RefereeHel motors[LEFT][0] = &flywheel2; // BOT_RIGHT == LEFT velocityPIDs[RIGHT][0] = &flywheel1PID; velocityPIDs[LEFT][0] = &flywheel2PID; -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES motors[TOP_LEFT][0] = &flywheel3; motors[BOT_LEFT][0] = &flywheel4; velocityPIDs[TOP_LEFT][0] = &flywheel3PID; @@ -54,7 +54,7 @@ float shaftSpeedDisplay = 0.0f; float FWRight1 = 0.0f; // RIGHT / TOP_RIGHT float FWLeft1 = 0.0f; // LEFT / BOT_LEFT -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES float FWTopLeft = 0.0f; // TOP_LEFT float FWBotLeft = 0.0f; // BOT_LEFT #endif @@ -71,7 +71,7 @@ void ShooterSubsystem::refresh() { if (flywheel2.isMotorOnline()) { FWLeft1 = flywheel2.getShaftRPM(); } -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES if (flywheel3.isMotorOnline()) { FWTopLeft = flywheel3.getShaftRPM(); } diff --git a/aimbots-src/src/subsystems/shooter/control/shooter.hpp b/aimbots-src/src/subsystems/shooter/control/shooter.hpp index 31fbaad5..0c2ef412 100644 --- a/aimbots-src/src/subsystems/shooter/control/shooter.hpp +++ b/aimbots-src/src/subsystems/shooter/control/shooter.hpp @@ -15,7 +15,7 @@ namespace src::Shooter { enum MotorIndex { RIGHT = 0, LEFT = 1, -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES TOP_RIGHT = 0, BOT_RIGHT = 1, TOP_LEFT = 2, @@ -113,7 +113,7 @@ class ShooterSubsystem : public tap::control::Subsystem { DJIMotor flywheel1, flywheel2; SmoothPID flywheel1PID, flywheel2PID; -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES DJIMotor flywheel3, flywheel4; SmoothPID flywheel3PID, flywheel4PID; #endif diff --git a/aimbots-src/src/utils/tools/robot_specific_defines.hpp b/aimbots-src/src/utils/tools/robot_specific_defines.hpp index 6ea1938f..19e9da18 100644 --- a/aimbots-src/src/utils/tools/robot_specific_defines.hpp +++ b/aimbots-src/src/utils/tools/robot_specific_defines.hpp @@ -1,7 +1,7 @@ #pragma once #if defined(TARGET_AERIAL) -#include "robots/aerial/constants/aerial_general_constants.hpp" #include "robots/aerial/aerial_control_interface.hpp" +#include "robots/aerial/constants/aerial_general_constants.hpp" #define ALL_AERIALS #elif defined(TARGET_DART) @@ -9,7 +9,7 @@ #include "robots/dart/dart_control_interface.hpp" #define ALL_DARTS -#elif defined(TARGET_ENGINEER) || defined(TARGET_SWERVE_ENGINEER) +#elif defined(TARGET_ENGINEER) #include "robots/engineer/constants/engineer_general_constants.hpp" #include "robots/engineer/engineer_control_interface.hpp" #define ALL_ENGINEERS @@ -19,13 +19,13 @@ #include "robots/hero/hero_control_interface.hpp" #define ALL_HEROES -#elif defined(TARGET_SENTRY) +#elif defined(TARGET_SENTRY_BRAVO) || defined(TARGET_SENTRY_SWERVE) #include "robots/sentry/constants/sentry_general_constant.hpp" #include "robots/sentry/sentry_control_interface.hpp" #define ALL_SENTRIES -#elif defined(TARGET_STANDARD_BLASTOISE) || defined(TARGET_STANDARD_WARTORTLE) || defined(TARGET_STANDARD_SQUIRTLE) || \ - defined(TARGET_STANDARD_2023) +#elif defined(TARGET_STANDARD_BLASTOISE) || defined(TARGET_STANDARD_SQUIRTLE) || defined(TARGET_STANDARD_2023) || \ + defined(TARGET_STANDARD_2025) #include "robots/standard/constants/standard_general_constants.hpp" #include "robots/standard/standard_control_interface.hpp" #define ALL_STANDARDS diff --git a/aimbots-src/test/subsystems/chassis_tests.cpp b/aimbots-src/test/subsystems/chassis_tests.cpp index d67ebe57..2223664f 100644 --- a/aimbots-src/test/subsystems/chassis_tests.cpp +++ b/aimbots-src/test/subsystems/chassis_tests.cpp @@ -1,8 +1,9 @@ #include -#include "drivers.hpp" #include "subsystems/chassis/control/chassis.hpp" +#include "drivers.hpp" + using ::testing::NiceMock; TEST(ChassisSubsystemTest, initialize_runs_initialize_on_all_motors) { @@ -11,7 +12,7 @@ TEST(ChassisSubsystemTest, initialize_runs_initialize_on_all_motors) { // ON_CALL(c.leftBackWheel, getEncoderUnwrapped); -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES EXPECT_CALL(c.railWheel, initialize); #else EXPECT_CALL(c.leftBackWheel, initialize); diff --git a/aimbots-src/test/subsystems/gimbal_tests.cpp b/aimbots-src/test/subsystems/gimbal_tests.cpp index f9b61431..56ed1a12 100644 --- a/aimbots-src/test/subsystems/gimbal_tests.cpp +++ b/aimbots-src/test/subsystems/gimbal_tests.cpp @@ -1,8 +1,9 @@ #include -#include "drivers.hpp" #include "subsystems/gimbal/control/gimbal.hpp" +#include "drivers.hpp" + using ::testing::NiceMock; TEST(GimbalSubsystemTest, initialize_runs_initialize_on_all_motors) { @@ -11,7 +12,7 @@ TEST(GimbalSubsystemTest, initialize_runs_initialize_on_all_motors) { // ON_CALL(c.leftBackWheel, getEncoderUnwrapped); -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES EXPECT_CALL(g., initialize); #else From 95a52fb7d89fedf1917a5e9590c792be61daafaa Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 27 May 2025 19:18:11 -0500 Subject: [PATCH 02/67] fixed swerve motor constant that prevented movement --- .../robots/sentry/constants/sentry_chassis_constants.hpp | 4 ++-- aimbots-src/src/subsystems/chassis/control/chassis.cpp | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 518e0d1f..a653077a 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -14,11 +14,11 @@ static constexpr MotorID RIGHT_FRONT_YAW_ID = MotorID::MOTOR7; static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { - .kp = 1.0f, + .kp = 10.0f, .ki = 0.0f, .kd = 0.01f, .maxICumulative = 0.9f, - .maxOutput = 1.0f, + .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 9748f422..9f4d3bcb 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -101,6 +101,7 @@ int refSerialWorkingDisplay = 0; uint16_t chassisPowerLimitDisplay = 0; float motorOutputDisplay = 0.0f; +float yawMotorOutputDisplay = 0.0f; void ChassisSubsystem::refresh() { ForAllChassisMotors(&ChassisSubsystem::updateMotorVelocityPID); @@ -109,7 +110,10 @@ void ChassisSubsystem::refresh() { limitChassisPower(); - motorOutputDisplay = motors[RB][0]->getOutputDesired(); + motorOutputDisplay = motors[RF][0]->getOutputDesired(); + #ifdef SWERVE + yawMotorOutputDisplay = motors[RF][1]->getOutputDesired(); + #endif } void ChassisSubsystem::limitChassisPower() { From 50d9f6414abca723d56581e4ba740298c0e59369 Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 27 May 2025 20:16:15 -0500 Subject: [PATCH 03/67] adjusted motor constants --- .vscode/settings.json | 6 +++++- .../sentry/constants/sentry_chassis_constants.hpp | 8 ++++---- aimbots-src/src/subsystems/chassis/control/chassis.cpp | 10 ++++++++-- 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 749376cb..7df5d886 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -112,7 +112,11 @@ "strstream": "cpp", "cfenv": "cpp", "typeindex": "cpp", - "*.ipp": "cpp" + "*.ipp": "cpp", + "xlocmes": "cpp", + "xlocmon": "cpp", + "xlocnum": "cpp", + "xloctime": "cpp" }, "search.exclude": { "**/node_modules": true, diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index a653077a..c4be6080 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -27,10 +27,10 @@ static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { .errorDerivativeFloor = 0.0f, }; -static constexpr int LEFT_FRONT_YAW_OFFSET = 0; -static constexpr int RIGHT_FRONT_YAW_OFFSET = 0; -static constexpr int LEFT_BACK_YAW_OFFSET = 0; -static constexpr int RIGHT_BACK_YAW_OFFSET = 0; +static constexpr int LEFT_FRONT_YAW_OFFSET = 2350; +static constexpr int RIGHT_FRONT_YAW_OFFSET = 1680; +static constexpr int LEFT_BACK_YAW_OFFSET = 280; +static constexpr int RIGHT_BACK_YAW_OFFSET = 6300; #elif defined(TARGET_SENTRY_BRAVO) static constexpr uint8_t MOTORS_PER_WHEEL = 1; diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 9f4d3bcb..3a08f4b7 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -101,7 +101,10 @@ int refSerialWorkingDisplay = 0; uint16_t chassisPowerLimitDisplay = 0; float motorOutputDisplay = 0.0f; -float yawMotorOutputDisplay = 0.0f; +float yawMotorOutputDisplayRF = 0.0f; +float yawMotorOutputDisplayRB = 0.0f; +float yawMotorOutputDisplayLF = 0.0f; +float yawMotorOutputDisplayLB = 0.0f; void ChassisSubsystem::refresh() { ForAllChassisMotors(&ChassisSubsystem::updateMotorVelocityPID); @@ -112,7 +115,10 @@ void ChassisSubsystem::refresh() { motorOutputDisplay = motors[RF][0]->getOutputDesired(); #ifdef SWERVE - yawMotorOutputDisplay = motors[RF][1]->getOutputDesired(); + yawMotorOutputDisplayRF = motors[RF][1]->getEncoderWrapped(); + yawMotorOutputDisplayRB = motors[RB][1]->getEncoderWrapped(); + yawMotorOutputDisplayLF = motors[LF][1]->getEncoderWrapped(); + yawMotorOutputDisplayLB = motors[LB][1]->getEncoderWrapped(); #endif } From 8f5950e9603ef894c8c6bc4708d9635f19d57aea Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 28 May 2025 17:32:58 -0500 Subject: [PATCH 04/67] fixed motor offsets and tuned pid some --- .../sentry/constants/sentry_chassis_constants.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index c4be6080..3393466d 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -14,7 +14,7 @@ static constexpr MotorID RIGHT_FRONT_YAW_ID = MotorID::MOTOR7; static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { - .kp = 10.0f, + .kp = 40.0f, .ki = 0.0f, .kd = 0.01f, .maxICumulative = 0.9f, @@ -27,10 +27,10 @@ static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { .errorDerivativeFloor = 0.0f, }; -static constexpr int LEFT_FRONT_YAW_OFFSET = 2350; -static constexpr int RIGHT_FRONT_YAW_OFFSET = 1680; -static constexpr int LEFT_BACK_YAW_OFFSET = 280; -static constexpr int RIGHT_BACK_YAW_OFFSET = 6300; +static constexpr int LEFT_FRONT_YAW_OFFSET = 2410; +static constexpr int RIGHT_FRONT_YAW_OFFSET = 1670; +static constexpr int LEFT_BACK_YAW_OFFSET = 255; +static constexpr int RIGHT_BACK_YAW_OFFSET = 6450; #elif defined(TARGET_SENTRY_BRAVO) static constexpr uint8_t MOTORS_PER_WHEEL = 1; From 006b915e8ae0ed286c43084d4ea9b942d116e5bc Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 28 May 2025 18:24:36 -0500 Subject: [PATCH 05/67] fixed inverted chassis controls --- .../robots/sentry/constants/sentry_chassis_constants.hpp | 2 +- aimbots-src/src/subsystems/chassis/control/chassis.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 3393466d..8b3f114e 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -16,7 +16,7 @@ static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { .kp = 40.0f, .ki = 0.0f, - .kd = 0.01f, + .kd = 5.0f, .maxICumulative = 0.9f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 3a08f4b7..0bb941b4 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -252,10 +252,10 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); - float a = x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float b = x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float c = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - float d = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; From a8884236304fb26ecb0dfbd8cd6054cf2a44fda2 Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 3 Jun 2025 20:05:24 -0500 Subject: [PATCH 06/67] added defense mode --- .../constants/sentry_chassis_constants.hpp | 2 +- .../subsystems/chassis/control/chassis.cpp | 115 +++++++++++------- 2 files changed, 75 insertions(+), 42 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 8b3f114e..fd6c204d 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -62,7 +62,7 @@ static constexpr SmoothPIDConfig CHASSIS_VELOCITY_PID_CONFIG = { }; // 1 for no symmetry, 2 for 180 degree symmetry, 4 for 90 degree symmetry -static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 2; +static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 4; // CAN Bus 2 static constexpr CANBus CHASSIS_BUS = CANBus::CAN_BUS2; diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 0bb941b4..76ae0d47 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -113,14 +113,14 @@ void ChassisSubsystem::refresh() { limitChassisPower(); - motorOutputDisplay = motors[RF][0]->getOutputDesired(); - #ifdef SWERVE - yawMotorOutputDisplayRF = motors[RF][1]->getEncoderWrapped(); - yawMotorOutputDisplayRB = motors[RB][1]->getEncoderWrapped(); - yawMotorOutputDisplayLF = motors[LF][1]->getEncoderWrapped(); - yawMotorOutputDisplayLB = motors[LB][1]->getEncoderWrapped(); - #endif -} +// motorOutputDisplay = motors[RF][0]->getOutputDesired(); +// #ifdef SWERVE +// yawMotorOutputDisplayRF = motors[RF][1]->getEncoderWrapped(); +// yawMotorOutputDisplayRB = motors[RB][1]->getEncoderWrapped(); +// yawMotorOutputDisplayLF = motors[LF][1]->getEncoderWrapped(); +// yawMotorOutputDisplayLB = motors[LB][1]->getEncoderWrapped(); +// #endif + } void ChassisSubsystem::limitChassisPower() { float powerLimitFrac = powerLimiter.getPowerLimitRatio(); @@ -249,39 +249,72 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel // float temp = y*cos(theta)+x*sin(theta); // x = -y*sin(theta)+x*cos(theta); // y = temp; - - float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); - - float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - - targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; - targetRPMs[LF][1] = left_front_yaw % 8191; - // targetRPMs[LF][1] = 35/360 * 8191; - left_front_yaw_db = targetRPMs[LF][1]; - left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - - targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - right_front_yaw = (atan2f(c, b) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - targetRPMs[RF][1] = right_front_yaw % 8191; - right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - right_front_yaw_db = targetRPMs[RF][1]; - - targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_back_yaw = (atan2f(d, a) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - targetRPMs[LB][1] = left_back_yaw % 8191; - left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - left_back_yaw_db = targetRPMs[LB][1]; - - targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - int right_back_yaw = (atan2f(c, a) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - targetRPMs[RB][1] = right_back_yaw % 8191; - right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - right_back_yaw_db = targetRPMs[RB][1]; - // wooo! just for commants + if(x==0 && y==0 && r==0){ + float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); + + float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + + targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + left_front_yaw = ( 3 *M_PI/4 ) * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; + targetRPMs[LF][1] = left_front_yaw % 8191; + // targetRPMs[LF][1] = 35/360 * 8191; + left_front_yaw_db = targetRPMs[LF][1]; + left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); + + targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + right_front_yaw = (M_PI/4) * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; + targetRPMs[RF][1] = right_front_yaw % 8191; + right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); + right_front_yaw_db = targetRPMs[RF][1]; + + targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + left_back_yaw = (M_PI / 4) * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; + targetRPMs[LB][1] = left_back_yaw % 8191; + left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); + left_back_yaw_db = targetRPMs[LB][1]; + + targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + int right_back_yaw = (3 * M_PI / 4) * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; + targetRPMs[RB][1] = right_back_yaw % 8191; + right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); + right_back_yaw_db = targetRPMs[RB][1]; + }else{ + float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); + + float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + + targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; + targetRPMs[LF][1] = left_front_yaw % 8191; + // targetRPMs[LF][1] = 35/360 * 8191; + left_front_yaw_db = targetRPMs[LF][1]; + left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); + + targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + right_front_yaw = (atan2f(c, b) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; + targetRPMs[RF][1] = right_front_yaw % 8191; + right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); + right_front_yaw_db = targetRPMs[RF][1]; + + targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + left_back_yaw = (atan2f(d, a) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; + targetRPMs[LB][1] = left_back_yaw % 8191; + left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); + left_back_yaw_db = targetRPMs[LB][1]; + + targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + int right_back_yaw = (atan2f(c, a) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; + targetRPMs[RB][1] = right_back_yaw % 8191; + right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); + right_back_yaw_db = targetRPMs[RB][1]; + // wooo! just for commants + } } #endif From fdaf930c1539bff08a5352f7e006dec3d28d087a Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 5 Jun 2025 16:37:32 -0500 Subject: [PATCH 07/67] began work on swerve optimize func --- aimbots-src/src/subsystems/chassis/control/chassis.cpp | 5 +++++ aimbots-src/src/subsystems/chassis/control/chassis.hpp | 1 + 2 files changed, 6 insertions(+) diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 76ae0d47..834e85a4 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -244,6 +244,11 @@ int right_front_yaw; int left_back_yaw; int right_back_yaw; +float left_front_drive; +float right_front_drive; +float left_back_drive; +float right_back_drive; + void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { // float theta = fieldRelativeInformant->getYaw(); // float temp = y*cos(theta)+x*sin(theta); diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.hpp b/aimbots-src/src/subsystems/chassis/control/chassis.hpp index 29a3ce1a..a8286189 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.hpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.hpp @@ -97,6 +97,7 @@ class ChassisSubsystem : public tap::control::chassis::ChassisSubsystemInterface void calculateHolonomic(float x, float y, float r, float maxWheelSpeed); // normal 4wd mecanum robots #ifdef SWERVE + void optimizeSwerve(float& targetRPMDrive, ); // finds best path for the void calculateSwerve(float x, float y, float r, float maxWheelSpeed); // swerve drive robots #endif void calculateRail(float x, float maxWheelSpeed); // sentry rail robots From 156fe5f4e6966fd8b2a5dea94567460e600a0da1 Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 5 Jun 2025 17:02:50 -0500 Subject: [PATCH 08/67] began fleshing out optimizer --- .../src/subsystems/chassis/control/chassis.cpp | 13 +++++++++++++ .../src/subsystems/chassis/control/chassis.hpp | 3 ++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 834e85a4..0ec3e5d3 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -229,6 +229,7 @@ void ChassisSubsystem::calculateHolonomic(float x, float y, float r, float maxWh #endif #ifdef SWERVE + float left_front_yaw_actual = 0.0f; float right_front_yaw_actual = 0.0f; float left_back_yaw_actual = 0.0f; @@ -249,6 +250,18 @@ float right_front_drive; float left_back_drive; float right_back_drive; +float ChassisSubsystem::yawToRad(float targetYaw){ + return ((targetYaw-offset)/((180 / M_PI) / 360 * 8191))-3 * M_PI / 2; +} + +void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, float currYaw){ + float deta = targetYaw-currYaw; + if(math.abs()>(M_PI/2)){ + targetRPMDrive = -targetRPMDrive; + targetYaw = delta > + } +} + void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { // float theta = fieldRelativeInformant->getYaw(); // float temp = y*cos(theta)+x*sin(theta); diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.hpp b/aimbots-src/src/subsystems/chassis/control/chassis.hpp index a8286189..dc3315cb 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.hpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.hpp @@ -97,7 +97,8 @@ class ChassisSubsystem : public tap::control::chassis::ChassisSubsystemInterface void calculateHolonomic(float x, float y, float r, float maxWheelSpeed); // normal 4wd mecanum robots #ifdef SWERVE - void optimizeSwerve(float& targetRPMDrive, ); // finds best path for the + float yawToRad(float targetYaw, int offset); + void optimizeSwerve(float& targetRPMDrive, float& targetYaw, float currYaw); // finds best path for the void calculateSwerve(float x, float y, float r, float maxWheelSpeed); // swerve drive robots #endif void calculateRail(float x, float maxWheelSpeed); // sentry rail robots From 9e5701bf2b1ecfda4ce3c5431170454a8d94cb10 Mon Sep 17 00:00:00 2001 From: zDagger Date: Sun, 8 Jun 2025 18:35:00 -0500 Subject: [PATCH 09/67] fixed many hero constants --- .../hero/constants/hero_feeder_constants.hpp | 8 ++++---- .../hero/constants/hero_gimbal_constants.hpp | 17 ++++++++--------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp index 44d07c81..d2b79ae9 100644 --- a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp @@ -7,7 +7,7 @@ static constexpr uint8_t FEEDER_MOTOR_COUNT = 2; static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG = { - .kp = 20.0f, + .kp = 40.0f, .ki = 0.0f, .kd = 0.8f, .maxICumulative = 10.0f, @@ -20,7 +20,7 @@ static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG = { .errorDerivativeFloor = 0.0f, }; -static constexpr int UNJAM_TIMER_MS = 100; +static constexpr int UNJAM_TIMER_MS = 50; static constexpr float FEEDER_DEFAULT_RPM = 12000.0f; @@ -29,9 +29,9 @@ static constexpr CANBus FEEDER_BUS = CANBus::CAN_BUS1; static const std::array FEEDER_MOTOR_IDS = {MotorID::MOTOR7, MotorID::MOTOR8}; static const std::array FEEDER_MOTOR_NAMES = {"Feeder Motor 1", "Feeder Motor 2"}; static const std::array FEEDER_NORMAL_RPMS = {500, 4000}; -static const std::array FEEDER_UNJAM_RPMS = {3000, 3000}; // Absolute values +static const std::array FEEDER_UNJAM_RPMS = {1000, 3000}; // Absolute values static const std::array FEEDER_MOTOR_GROUPS = {SECONDARY, PRIMARY}; static constexpr float PROJECTILES_PER_FEEDER_ROTATION = 0.5; static constexpr std::array FEEDER_GEAR_RATIOS = {36, 36}; -static const std::array FEEDER_DIRECTION = {true, false}; +static const std::array FEEDER_DIRECTION = {true, true}; // \ No newline at end of file diff --git a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp index 6980f8f8..fb180f69 100644 --- a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp @@ -11,19 +11,18 @@ static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS2; static constexpr CANBus PITCH_GIMBAL_BUS = CANBus::CAN_BUS1; -static constexpr uint8_t YAW_MOTOR_COUNT = 2; +static constexpr uint8_t YAW_MOTOR_COUNT = 1; static constexpr uint8_t PITCH_MOTOR_COUNT = 1; -static const std::array YAW_MOTOR_DIRECTIONS = {false, false}; -static const std::array YAW_MOTOR_IDS = {MotorID::MOTOR5, MotorID::MOTOR7}; -static const std::array YAW_MOTOR_NAMES = {"Yaw Motor 1", "Yaw Motor 2"}; +static const std::array YAW_MOTOR_DIRECTIONS = {false}; +static const std::array YAW_MOTOR_IDS = {MotorID::MOTOR5};//maybe7 +static const std::array YAW_MOTOR_NAMES = {"Yaw Motor 1"}; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ static const std::array YAW_MOTOR_OFFSET_ANGLES = { - wrapTo0To2PIRange(modm::toRadian(181.36f)), - wrapTo0To2PIRange(modm::toRadian(185.75f))}; + wrapTo0To2PIRange(modm::toRadian(264.0f))}; static constexpr float YAW_AXIS_START_ANGLE = modm::toRadian(0.0f); -static constexpr float GIMBAL_YAW_GEAR_RATIO = 0.5f; // for 2023 Hero +static constexpr float GIMBAL_YAW_GEAR_RATIO = 1/2.0754716981f; // for 2025 Hero /* Changing this means the encoder-readable range of the YAW axis is reduced to 360deg * GIMBAL_YAW_GEAR_RATIO before the * encoder readings will repeat. We will assume that the robot will be started within the same GIMBAL_YAW_GEAR_RATIO range * every time. We also assume that 1 / GIMBAL_YAW_GEAR_RATIO is an integer multiple of 360deg. */ @@ -41,8 +40,8 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (30.0f / 102.0f); // for 2023 * encoder readings will repeat. We will assume that the range of the pitch axis is hardware-limited to not exceed this * range, but the motor angle may cross 0 in this range. Example Range: 278deg to 28deg */ -static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-25.0f); -static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(22.0f); +static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-82.0f); +static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(-25.0f); // LOW should be lesser than HIGH, otherwise switch the motor direction /** From 80218fde1b999274ad31a5a4fd993f54138bcc9e Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 10 Jun 2025 20:54:44 -0500 Subject: [PATCH 10/67] finished swerve optimizer --- .../subsystems/chassis/control/chassis.cpp | 119 ++++++++++-------- 1 file changed, 67 insertions(+), 52 deletions(-) diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 0ec3e5d3..273fa4ad 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -230,10 +230,10 @@ void ChassisSubsystem::calculateHolonomic(float x, float y, float r, float maxWh #ifdef SWERVE -float left_front_yaw_actual = 0.0f; -float right_front_yaw_actual = 0.0f; -float left_back_yaw_actual = 0.0f; -float right_back_yaw_actual = 0.0f; +float left_front_yaw_actual; +float right_front_yaw_actual; +float left_back_yaw_actual; +float right_back_yaw_actual; int left_front_yaw_db; int right_front_yaw_db; @@ -245,20 +245,29 @@ int right_front_yaw; int left_back_yaw; int right_back_yaw; +float target_left_front_yaw; +float target_right_front_yaw; +float target_left_back_yaw; +float target_right_back_yaw; + float left_front_drive; float right_front_drive; float left_back_drive; float right_back_drive; -float ChassisSubsystem::yawToRad(float targetYaw){ - return ((targetYaw-offset)/((180 / M_PI) / 360 * 8191))-3 * M_PI / 2; +float ChassisSubsystem::yawToRad(float targetYaw, int offset){ + return ((targetYaw-offset)/((180 / M_PI) / 360 * 8191)); } void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, float currYaw){ - float deta = targetYaw-currYaw; - if(math.abs()>(M_PI/2)){ + float delta = targetYaw-currYaw; + if(delta > M_PI){ + delta = (2 * M_PI)-delta; + } + if(std::abs(delta)>(M_PI/2)){ targetRPMDrive = -targetRPMDrive; - targetYaw = delta > + targetYaw += M_PI; + targetYaw = wrapTo0To2PIRange(targetYaw); } } @@ -267,39 +276,38 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel // float temp = y*cos(theta)+x*sin(theta); // x = -y*sin(theta)+x*cos(theta); // y = temp; - if(x==0 && y==0 && r==0){ - float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); - - float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - - targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_front_yaw = ( 3 *M_PI/4 ) * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; - targetRPMs[LF][1] = left_front_yaw % 8191; - // targetRPMs[LF][1] = 35/360 * 8191; - left_front_yaw_db = targetRPMs[LF][1]; - left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - - targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - right_front_yaw = (M_PI/4) * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - targetRPMs[RF][1] = right_front_yaw % 8191; - right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - right_front_yaw_db = targetRPMs[RF][1]; - - targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_back_yaw = (M_PI / 4) * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - targetRPMs[LB][1] = left_back_yaw % 8191; - left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - left_back_yaw_db = targetRPMs[LB][1]; - - targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - int right_back_yaw = (3 * M_PI / 4) * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - targetRPMs[RB][1] = right_back_yaw % 8191; - right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - right_back_yaw_db = targetRPMs[RB][1]; - }else{ + // if(x==0 && y==0 && r==0){ + // float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); + + // float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + // float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + // float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + // float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + + // targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // left_front_yaw = ( 3 *M_PI/4 ) * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; + // targetRPMs[LF][1] = left_front_yaw % 8191; + // left_front_yaw_db = targetRPMs[LF][1]; + // left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); + + // targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // right_front_yaw = (M_PI/4) * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; + // targetRPMs[RF][1] = right_front_yaw % 8191; + // right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); + // right_front_yaw_db = targetRPMs[RF][1]; + + // targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // left_back_yaw = (M_PI / 4) * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; + // targetRPMs[LB][1] = left_back_yaw % 8191; + // left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); + // left_back_yaw_db = targetRPMs[LB][1]; + + // targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // int right_back_yaw = (3 * M_PI / 4) * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; + // targetRPMs[RB][1] = right_back_yaw % 8191; + // right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); + // right_back_yaw_db = targetRPMs[RB][1]; + // }else{ float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); @@ -308,31 +316,38 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; + left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); + target_left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2); + optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); + left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; targetRPMs[LF][1] = left_front_yaw % 8191; - // targetRPMs[LF][1] = 35/360 * 8191; left_front_yaw_db = targetRPMs[LF][1]; - left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - right_front_yaw = (atan2f(c, b) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - targetRPMs[RF][1] = right_front_yaw % 8191; right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); + target_right_front_yaw = (atan2f(c, b) + 3 * M_PI / 2); + optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); + right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; + targetRPMs[RF][1] = right_front_yaw % 8191; right_front_yaw_db = targetRPMs[RF][1]; targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_back_yaw = (atan2f(d, a) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - targetRPMs[LB][1] = left_back_yaw % 8191; left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); + target_left_back_yaw = atan2f(d, a) + 3 * M_PI / 2; + optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); + left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; + targetRPMs[LB][1] = left_back_yaw % 8191; left_back_yaw_db = targetRPMs[LB][1]; targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - int right_back_yaw = (atan2f(c, a) + 3 * M_PI / 2) * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - targetRPMs[RB][1] = right_back_yaw % 8191; right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); + target_right_back_yaw = atan2f(c, a) + 3 * M_PI / 2; + optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); + right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; + targetRPMs[RB][1] = right_back_yaw % 8191; right_back_yaw_db = targetRPMs[RB][1]; // wooo! just for commants - } + //} } #endif From 0373a3bd9d8d577bbfbb30693e6b65f3fe4a0825 Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 10 Jun 2025 21:32:04 -0500 Subject: [PATCH 11/67] optimized defense mode --- .../subsystems/chassis/control/chassis.cpp | 79 ++++++++++--------- .../subsystems/chassis/control/chassis.hpp | 7 +- 2 files changed, 47 insertions(+), 39 deletions(-) diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 273fa4ad..f5d395ae 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -272,42 +272,47 @@ void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, f } void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { - // float theta = fieldRelativeInformant->getYaw(); - // float temp = y*cos(theta)+x*sin(theta); - // x = -y*sin(theta)+x*cos(theta); - // y = temp; - // if(x==0 && y==0 && r==0){ - // float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); - - // float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - // float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - // float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - // float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - - // targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // left_front_yaw = ( 3 *M_PI/4 ) * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; - // targetRPMs[LF][1] = left_front_yaw % 8191; - // left_front_yaw_db = targetRPMs[LF][1]; - // left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - - // targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // right_front_yaw = (M_PI/4) * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - // targetRPMs[RF][1] = right_front_yaw % 8191; - // right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - // right_front_yaw_db = targetRPMs[RF][1]; - - // targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // left_back_yaw = (M_PI / 4) * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - // targetRPMs[LB][1] = left_back_yaw % 8191; - // left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - // left_back_yaw_db = targetRPMs[LB][1]; - - // targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // int right_back_yaw = (3 * M_PI / 4) * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - // targetRPMs[RB][1] = right_back_yaw % 8191; - // right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - // right_back_yaw_db = targetRPMs[RB][1]; - // }else{ + //defense mode (X) when robot is at rest + if(x==0 && y==0 && r==0){ + float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); + + float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + + targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); + target_left_front_yaw = (3 * M_PI/4); + optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); + left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; + targetRPMs[LF][1] = left_front_yaw % 8191; + left_front_yaw_db = targetRPMs[LF][1]; + + targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); + target_right_front_yaw = (M_PI / 4); + optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); + right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; + targetRPMs[RF][1] = right_front_yaw % 8191; + right_front_yaw_db = targetRPMs[RF][1]; + + targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); + target_left_back_yaw = M_PI / 4; + optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); + left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; + targetRPMs[LB][1] = left_back_yaw % 8191; + left_back_yaw_db = targetRPMs[LB][1]; + + targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); + target_right_back_yaw = 3 * M_PI / 4; + optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); + right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; + targetRPMs[RB][1] = right_back_yaw % 8191; + right_back_yaw_db = targetRPMs[RB][1]; + }else{ float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); @@ -347,7 +352,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[RB][1] = right_back_yaw % 8191; right_back_yaw_db = targetRPMs[RB][1]; // wooo! just for commants - //} + } } #endif diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.hpp b/aimbots-src/src/subsystems/chassis/control/chassis.hpp index dc3315cb..fd00a2e2 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.hpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.hpp @@ -97,9 +97,12 @@ class ChassisSubsystem : public tap::control::chassis::ChassisSubsystemInterface void calculateHolonomic(float x, float y, float r, float maxWheelSpeed); // normal 4wd mecanum robots #ifdef SWERVE + //converts yaw encoder count to radians float yawToRad(float targetYaw, int offset); - void optimizeSwerve(float& targetRPMDrive, float& targetYaw, float currYaw); // finds best path for the - void calculateSwerve(float x, float y, float r, float maxWheelSpeed); // swerve drive robots + //finds the shortest path for the swerve drive to point in + void optimizeSwerve(float& targetRPMDrive, float& targetYaw, float currYaw); + //calculates targets for swerves yaw and pitch given the desired direction and speed + void calculateSwerve(float x, float y, float r, float maxWheelSpeed); #endif void calculateRail(float x, float maxWheelSpeed); // sentry rail robots From 373cc6cb8cca4866baa607fbbb575cb338d57793 Mon Sep 17 00:00:00 2001 From: zDagger Date: Fri, 13 Jun 2025 19:38:22 -0500 Subject: [PATCH 12/67] updated hero constants and began swap to limit fire --- .vscode/settings.json | 10 +++++++++- .../robots/hero/constants/hero_general_constants.hpp | 2 +- .../robots/hero/constants/hero_gimbal_constants.hpp | 6 +++--- aimbots-src/src/robots/hero/hero_control.cpp | 4 ++-- .../feeder/complex_commands/feeder_limit_command.cpp | 2 +- aimbots-src/src/subsystems/feeder/control/feeder.cpp | 2 +- 6 files changed, 17 insertions(+), 9 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 7df5d886..7985f7a4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -116,7 +116,15 @@ "xlocmes": "cpp", "xlocmon": "cpp", "xlocnum": "cpp", - "xloctime": "cpp" + "xloctime": "cpp", + "charconv": "cpp", + "xfacet": "cpp", + "xiosbase": "cpp", + "xlocale": "cpp", + "xlocbuf": "cpp", + "xlocinfo": "cpp", + "xmemory": "cpp", + "xtr1common": "cpp" }, "search.exclude": { "**/node_modules": true, diff --git a/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp index a1b71487..3caae760 100644 --- a/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp @@ -44,7 +44,7 @@ static Vector3f BARREL_POSITION_FROM_GIMBAL_ORIGIN{ static constexpr float CIMU_CALIBRATION_EULER_X = modm::toRadian(0.0f); static constexpr float CIMU_CALIBRATION_EULER_Y = modm::toRadian(0.0f); -static constexpr float CIMU_CALIBRATION_EULER_Z = modm::toRadian(180.0f); +static constexpr float CIMU_CALIBRATION_EULER_Z = modm::toRadian(90.0f); static constexpr float TIMU_CALIBRATION_EULER_X = modm::toRadian(0.0f); static constexpr float TIMU_CALIBRATION_EULER_Y = modm::toRadian(0.0f); diff --git a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp index fb180f69..2fd773d5 100644 --- a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp @@ -15,11 +15,11 @@ static constexpr uint8_t YAW_MOTOR_COUNT = 1; static constexpr uint8_t PITCH_MOTOR_COUNT = 1; static const std::array YAW_MOTOR_DIRECTIONS = {false}; -static const std::array YAW_MOTOR_IDS = {MotorID::MOTOR5};//maybe7 +static const std::array YAW_MOTOR_IDS = {MotorID::MOTOR5}; static const std::array YAW_MOTOR_NAMES = {"Yaw Motor 1"}; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ static const std::array YAW_MOTOR_OFFSET_ANGLES = { - wrapTo0To2PIRange(modm::toRadian(264.0f))}; + wrapTo0To2PIRange(modm::toRadian(218.0f))}; static constexpr float YAW_AXIS_START_ANGLE = modm::toRadian(0.0f); static constexpr float GIMBAL_YAW_GEAR_RATIO = 1/2.0754716981f; // for 2025 Hero @@ -48,7 +48,7 @@ static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(-25.0f); * @brief Position PID constants */ static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { - .kp = 0.0f, + .kp = 5.0f, .ki = 0.0f, .kd = 0.0f, .maxICumulative = 5000.0f, diff --git a/aimbots-src/src/robots/hero/hero_control.cpp b/aimbots-src/src/robots/hero/hero_control.cpp index 7ffa620d..06b08f1f 100644 --- a/aimbots-src/src/robots/hero/hero_control.cpp +++ b/aimbots-src/src/robots/hero/hero_control.cpp @@ -222,13 +222,13 @@ HoldCommandMapping leftSwitchUp( HoldCommandMapping rightSwitchMid( drivers(), - {/*&feederlimitcommand,*/ &runShooterCommand}, + {&feederLimitCommand, &runShooterCommand}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::MID)); // Runs shooter with feeder and closes hopper HoldRepeatCommandMapping rightSwitchUp( drivers(), - {&runFeederCommand, &runShooterWithFeederCommand}, + {/*&runFeederCommand,*/&feederLimitCommand, &runShooterWithFeederCommand}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::UP), true); diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index 41ec2592..4bec94bc 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -49,7 +49,7 @@ void FeederLimitCommand::execute() { // States how long the limit switch is ignored when firing a projectile limitSwitchDownTime = 350; // States the speed of the feeder wheel when firing - // Checks if the limit switch is pressed & is "not killed" (refer to ) + // Checks if the limit switch is pressed & is "not killed" if (limitswitchInactive.isExpired()) { isFiring = false; diff --git a/aimbots-src/src/subsystems/feeder/control/feeder.cpp b/aimbots-src/src/subsystems/feeder/control/feeder.cpp index e3e3b62a..55c22a8d 100644 --- a/aimbots-src/src/subsystems/feeder/control/feeder.cpp +++ b/aimbots-src/src/subsystems/feeder/control/feeder.cpp @@ -9,7 +9,7 @@ namespace src::Feeder { FeederSubsystem::FeederSubsystem(src::Drivers* drivers) : tap::control::Subsystem(drivers), drivers(drivers), - limitSwitch(static_cast("C7"), src::Informants::EdgeType::RISING) { + limitSwitch(static_cast("C6"), src::Informants::EdgeType::RISING) { BuildFeederMotors(); BuildPIDControllers(); } From bb8f7043673604b7d7799a923803b0e8eaa9cc0b Mon Sep 17 00:00:00 2001 From: zDagger Date: Fri, 13 Jun 2025 22:05:40 -0500 Subject: [PATCH 13/67] Co-authored-by: Tyler Hedge Co-authored-by: svukryggi Co-authored-by: Baconsizzle1738 Co-authored-by: Noe Co-authored-by: Zheng, Kenny Co-authored-by: JaX-Y1 --- .../hero/constants/hero_feeder_constants.hpp | 16 +++++++++++++++- .../src/subsystems/feeder/control/feeder.hpp | 2 +- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp index d2b79ae9..cd930df5 100644 --- a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp @@ -6,7 +6,7 @@ static constexpr uint8_t FEEDER_MOTOR_COUNT = 2; -static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG = { +static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG [FEEDER_MOTOR_COUNT] = {{ .kp = 40.0f, .ki = 0.0f, .kd = 0.8f, @@ -18,6 +18,20 @@ static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG = { .tRProportionalKalman = 1.0f, .errDeadzone = 0.0f, .errorDerivativeFloor = 0.0f, + }, + { + .kp = 10.0f, + .ki = 0.0f, + .kd = 0.8f, + .maxICumulative = 10.0f, + .maxOutput = M2006_MAX_OUTPUT, + .tQDerivativeKalman = 1.0f, + .tRDerivativeKalman = 1.0f, + .tQProportionalKalman = 1.0f, + .tRProportionalKalman = 1.0f, + .errDeadzone = 0.0f, + .errorDerivativeFloor = 0.0f, + } }; static constexpr int UNJAM_TIMER_MS = 50; diff --git a/aimbots-src/src/subsystems/feeder/control/feeder.hpp b/aimbots-src/src/subsystems/feeder/control/feeder.hpp index 4c7fd0d5..191dc99b 100644 --- a/aimbots-src/src/subsystems/feeder/control/feeder.hpp +++ b/aimbots-src/src/subsystems/feeder/control/feeder.hpp @@ -49,7 +49,7 @@ class FeederSubsystem : public tap::control::Subsystem { void BuildPIDControllers() { for (auto i = 0; i < FEEDER_MOTOR_COUNT; i++) { - feederVelocityPIDs[i] = new SmoothPID(FEEDER_VELOCITY_PID_CONFIG); + feederVelocityPIDs[i] = new SmoothPID(FEEDER_VELOCITY_PID_CONFIG[i]); } } From e43abaa73dea5faa522cc84cdaee5c73acf2e06d Mon Sep 17 00:00:00 2001 From: zDagger Date: Fri, 13 Jun 2025 22:53:58 -0500 Subject: [PATCH 14/67] adjusting feeder limit and added pid for heros second feeder --- .../robots/hero/constants/hero_feeder_constants.hpp | 6 +++--- .../feeder/complex_commands/feeder_limit_command.cpp | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp index cd930df5..3053e78f 100644 --- a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp @@ -7,9 +7,9 @@ static constexpr uint8_t FEEDER_MOTOR_COUNT = 2; static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG [FEEDER_MOTOR_COUNT] = {{ - .kp = 40.0f, + .kp = 60.0f, .ki = 0.0f, - .kd = 0.8f, + .kd = 2.0f, .maxICumulative = 10.0f, .maxOutput = M3508_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, @@ -34,7 +34,7 @@ static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG [FEEDER_MOTOR_COUNT] } }; -static constexpr int UNJAM_TIMER_MS = 50; +static constexpr int UNJAM_TIMER_MS = 100; static constexpr float FEEDER_DEFAULT_RPM = 12000.0f; diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index 4bec94bc..311e7447 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -47,12 +47,12 @@ void FeederLimitCommand::execute() { // Updates the current controller switch state currFireState = drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP; // States how long the limit switch is ignored when firing a projectile - limitSwitchDownTime = 350; + limitSwitchDownTime = 300; // States the speed of the feeder wheel when firing // Checks if the limit switch is pressed & is "not killed" - if (limitswitchInactive.isExpired()) { - isFiring = false; + if (!limitswitchInactive.isExpired()) { + limitPressed = false; // feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::deactivateFeederMotor); } @@ -69,8 +69,8 @@ void FeederLimitCommand::execute() { if (unjamTimer.execute()) { feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::activateFeederMotor); - feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::setFeederCustomMulti, 1.0f); - feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::activateFeederMotor); + //feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::setFeederCustomMulti, 1.0f); + //feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::activateFeederMotor); startupThreshold.restart(500); } } else { From 5660fe99e8bce4b58700060d2fa693f5e61b551a Mon Sep 17 00:00:00 2001 From: zDagger Date: Sat, 14 Jun 2025 22:11:57 -0500 Subject: [PATCH 15/67] fully fixed hero single shot --- .../robots/hero/constants/hero_gimbal_constants.hpp | 5 +++-- aimbots-src/src/robots/hero/hero_control.cpp | 10 +++++----- .../feeder/complex_commands/feeder_limit_command.cpp | 10 +++++++--- 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp index 2fd773d5..c67e61bc 100644 --- a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp @@ -19,10 +19,11 @@ static const std::array YAW_MOTOR_IDS = {MotorID::MOTO static const std::array YAW_MOTOR_NAMES = {"Yaw Motor 1"}; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ static const std::array YAW_MOTOR_OFFSET_ANGLES = { - wrapTo0To2PIRange(modm::toRadian(218.0f))}; + wrapTo0To2PIRange(modm::toRadian(3.5f))}; static constexpr float YAW_AXIS_START_ANGLE = modm::toRadian(0.0f); -static constexpr float GIMBAL_YAW_GEAR_RATIO = 1/2.0754716981f; // for 2025 Hero + static constexpr float GIMBAL_YAW_GEAR_RATIO = 1.0f/2.0754716981f; // for 2025 Hero +//static constexpr float GIMBAL_YAW_GEAR_RATIO = 1.0f/2.0f; // for 2025 Hero /* Changing this means the encoder-readable range of the YAW axis is reduced to 360deg * GIMBAL_YAW_GEAR_RATIO before the * encoder readings will repeat. We will assume that the robot will be started within the same GIMBAL_YAW_GEAR_RATIO range * every time. We also assume that 1 / GIMBAL_YAW_GEAR_RATIO is an integer multiple of 360deg. */ diff --git a/aimbots-src/src/robots/hero/hero_control.cpp b/aimbots-src/src/robots/hero/hero_control.cpp index 06b08f1f..25b59f3d 100644 --- a/aimbots-src/src/robots/hero/hero_control.cpp +++ b/aimbots-src/src/robots/hero/hero_control.cpp @@ -234,10 +234,10 @@ HoldRepeatCommandMapping rightSwitchUp( PressCommandMapping bCtrlPressed(drivers(), {&clientDisplayCommand}, RemoteMapState({Remote::Key::CTRL, Remote::Key::B})); -HoldCommandMapping leftClickMouse( - drivers(), - {&runFeederCommandFromMouse}, - RemoteMapState(RemoteMapState::MouseButton::LEFT)); +// HoldCommandMapping leftClickMouse( +// drivers(), +// {&feederLimitCommand}, +// RemoteMapState(RemoteMapState::MouseButton::LEFT)); // Register subsystems here ----------------------------------------------- void registerSubsystems(src::Drivers *drivers) { @@ -289,7 +289,7 @@ void registerIOMappings(src::Drivers *drivers) { drivers->commandMapper.addMap(&rightSwitchUp); drivers->commandMapper.addMap(&rightSwitchMid); drivers->commandMapper.addMap(&bCtrlPressed); - drivers->commandMapper.addMap(&leftClickMouse); + // drivers->commandMapper.addMap(&leftClickMouse); } } // namespace HeroControl diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index 311e7447..c39891fb 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -15,6 +15,7 @@ namespace src::Feeder { bool limitPressed = false; bool currFireState = false; +bool watchFire = false; bool prevFireState = false; bool isFiring = false; bool loaderDormant = false; @@ -37,6 +38,7 @@ void FeederLimitCommand::initialize() { startupThreshold.restart(500); // delay to wait before attempting unjam unjamTimer.restart(0); limitswitchInactive.restart(0); + watchFire=true; } void FeederLimitCommand::execute() { @@ -45,7 +47,8 @@ void FeederLimitCommand::execute() { // Updates the previous controller switch state (is up or not) prevFireState = currFireState; // Updates the current controller switch state - currFireState = drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP; + watchFire = false; + currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true); // States how long the limit switch is ignored when firing a projectile limitSwitchDownTime = 300; // States the speed of the feeder wheel when firing @@ -62,12 +65,13 @@ void FeederLimitCommand::execute() { // } if (!limitPressed && !loaderDormant) { + feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::activateFeederMotor); if (fabs(feeder->getCurrentRPM(0)) <= 10.0f && startupThreshold.execute()) { feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::unjamFeederMotor); unjamTimer.restart(UNJAM_TIMER_MS); } - if (unjamTimer.execute()) { + if (!unjamTimer.execute()) { feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::activateFeederMotor); //feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::setFeederCustomMulti, 1.0f); //feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::activateFeederMotor); @@ -75,7 +79,7 @@ void FeederLimitCommand::execute() { } } else { feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::deactivateFeederMotor); - if (!isFiring) { + if (isFiring) { feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::deactivateFeederMotor); } loaderDormant = true; From 4bd7db295ea04583befa4ccfd9b4109acd6ac051 Mon Sep 17 00:00:00 2001 From: zDagger Date: Sun, 15 Jun 2025 15:40:42 -0500 Subject: [PATCH 16/67] fixed timers foe single shot --- .../hero/constants/hero_feeder_constants.hpp | 2 +- .../hero/constants/hero_gimbal_constants.hpp | 2 +- .../complex_commands/feeder_limit_command.cpp | 30 +++++++++++++------ 3 files changed, 23 insertions(+), 11 deletions(-) diff --git a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp index 3053e78f..2e08e52f 100644 --- a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp @@ -34,7 +34,7 @@ static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG [FEEDER_MOTOR_COUNT] } }; -static constexpr int UNJAM_TIMER_MS = 100; +static constexpr int UNJAM_TIMER_MS = 200; static constexpr float FEEDER_DEFAULT_RPM = 12000.0f; diff --git a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp index c67e61bc..35400e97 100644 --- a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp @@ -19,7 +19,7 @@ static const std::array YAW_MOTOR_IDS = {MotorID::MOTO static const std::array YAW_MOTOR_NAMES = {"Yaw Motor 1"}; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ static const std::array YAW_MOTOR_OFFSET_ANGLES = { - wrapTo0To2PIRange(modm::toRadian(3.5f))}; + wrapTo0To2PIRange(modm::toRadian(252.4f))}; static constexpr float YAW_AXIS_START_ANGLE = modm::toRadian(0.0f); static constexpr float GIMBAL_YAW_GEAR_RATIO = 1.0f/2.0754716981f; // for 2025 Hero diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index c39891fb..d7c88a09 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -15,8 +15,10 @@ namespace src::Feeder { bool limitPressed = false; bool currFireState = false; +bool currLoadState = false; bool watchFire = false; bool prevFireState = false; +bool prevLoadState = false; bool isFiring = false; bool loaderDormant = false; int limitSwitchDownTime; @@ -38,7 +40,6 @@ void FeederLimitCommand::initialize() { startupThreshold.restart(500); // delay to wait before attempting unjam unjamTimer.restart(0); limitswitchInactive.restart(0); - watchFire=true; } void FeederLimitCommand::execute() { @@ -47,13 +48,14 @@ void FeederLimitCommand::execute() { // Updates the previous controller switch state (is up or not) prevFireState = currFireState; // Updates the current controller switch state - watchFire = false; currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true); // States how long the limit switch is ignored when firing a projectile limitSwitchDownTime = 300; // States the speed of the feeder wheel when firing // Checks if the limit switch is pressed & is "not killed" + prevLoadState = currLoadState; + if (!limitswitchInactive.isExpired()) { limitPressed = false; // feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::deactivateFeederMotor); @@ -66,19 +68,29 @@ void FeederLimitCommand::execute() { if (!limitPressed && !loaderDormant) { feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::activateFeederMotor); - if (fabs(feeder->getCurrentRPM(0)) <= 10.0f && startupThreshold.execute()) { - feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::unjamFeederMotor); + if (fabs(feeder->getCurrentRPM(0)) <= 5.0f && unjamTimer.isExpired()){ unjamTimer.restart(UNJAM_TIMER_MS); } - - if (!unjamTimer.execute()) { + if (!unjamTimer.isExpired() && startupThreshold.isExpired()) { + feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::unjamFeederMotor); + currLoadState=false; + }else{ feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::activateFeederMotor); - //feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::setFeederCustomMulti, 1.0f); - //feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::activateFeederMotor); - startupThreshold.restart(500); + currLoadState = true; + } + if(prevLoadState == false && currLoadState == true){ + startupThreshold.restart(1000); } + + // if (!unjamTimer.execute()) { + // feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::activateFeederMotor); + // //feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::setFeederCustomMulti, 1.0f); + // //feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::activateFeederMotor); + // startupThreshold.restart(500); + // } } else { feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::deactivateFeederMotor); + currLoadState = false; if (isFiring) { feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::deactivateFeederMotor); } From 9ff9a32b5808d3ae1c9add89f66827ae364a601d Mon Sep 17 00:00:00 2001 From: zDagger Date: Sun, 15 Jun 2025 17:49:11 -0500 Subject: [PATCH 17/67] lobotomized hero partially --- .../src/informants/kinematics/kinematic_informant.cpp | 6 ++++++ .../src/robots/hero/constants/hero_general_constants.hpp | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index d52bacb7..5ffc8c7f 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -264,10 +264,16 @@ float KinematicInformant::getChassisPitchAngleInGimbalDirection() { float cosGimbYaw = cosf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); float chassisRoll = getChassisIMUAngle(src::Informants::AngularAxis::ROLL_AXIS, AngleUnit::Radians); + #ifdef HERO + float chassisRoll = 0; + #endif float sinChasRoll = sinf(chassisRoll); float cosChasRoll = cosf(chassisRoll); float chassisPitch = getChassisIMUAngle(src::Informants::AngularAxis::PITCH_AXIS, AngleUnit::Radians); + #ifdef HERO + float chassisPitch = 0; + #endif float sinChasPitch = sinf(chassisPitch); float cosChasPitch = cosf(chassisPitch); diff --git a/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp index 3caae760..3d939d39 100644 --- a/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp @@ -30,8 +30,8 @@ static Vector3f TURRET_ORIGIN_RELATIVE_TO_CHASSIS_ORIGIN{ }; static Vector3f CHASSIS_START_POSITION_RELATIVE_TO_WORLD{ - 0.0f, // x - 0.0f, // y + 0.157f, // x + 0.0335f, // y 0.0f, // z }; From b9bd723b7c487304c5db798ba84e56a75be78b19 Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 17 Jun 2025 20:41:56 -0500 Subject: [PATCH 18/67] updated sentry constants --- .../robots/sentry/constants/sentry_chassis_constants.hpp | 8 ++++---- .../robots/sentry/constants/sentry_gimbal_constants.hpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index fd6c204d..41779a6e 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -65,12 +65,12 @@ static constexpr SmoothPIDConfig CHASSIS_VELOCITY_PID_CONFIG = { static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 4; // CAN Bus 2 -static constexpr CANBus CHASSIS_BUS = CANBus::CAN_BUS2; +static constexpr CANBus CHASSIS_BUS = CANBus::CAN_BUS1; -static constexpr MotorID LEFT_BACK_WHEEL_ID = MotorID::MOTOR1; +static constexpr MotorID LEFT_BACK_WHEEL_ID = MotorID::MOTOR4; static constexpr MotorID LEFT_FRONT_WHEEL_ID = MotorID::MOTOR2; -static constexpr MotorID RIGHT_FRONT_WHEEL_ID = MotorID::MOTOR3; -static constexpr MotorID RIGHT_BACK_WHEEL_ID = MotorID::MOTOR4; +static constexpr MotorID RIGHT_FRONT_WHEEL_ID = MotorID::MOTOR1; +static constexpr MotorID RIGHT_BACK_WHEEL_ID = MotorID::MOTOR3; // Mechanical chassis constants, all in m /** diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 31dc35ee..63ad9c45 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -38,7 +38,7 @@ static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(30.0f); #endif static const std::array YAW_MOTOR_DIRECTIONS = {false}; -static const std::array YAW_MOTOR_IDS = {MotorID::MOTOR5}; +static const std::array YAW_MOTOR_IDS = {MotorID::MOTOR8};//TODO static const std::array YAW_MOTOR_NAMES = {"Yaw Motor 1"}; static constexpr float YAW_AXIS_START_ANGLE = modm::toRadian(0.0f); From 901f218e71020f82310ec6fcf646c45f2923a2f9 Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 17 Jun 2025 21:15:22 -0500 Subject: [PATCH 19/67] fixed hero feeder definitions --- aimbots-src/src/subsystems/feeder/control/feeder.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/aimbots-src/src/subsystems/feeder/control/feeder.hpp b/aimbots-src/src/subsystems/feeder/control/feeder.hpp index 191dc99b..98307b21 100644 --- a/aimbots-src/src/subsystems/feeder/control/feeder.hpp +++ b/aimbots-src/src/subsystems/feeder/control/feeder.hpp @@ -49,7 +49,13 @@ class FeederSubsystem : public tap::control::Subsystem { void BuildPIDControllers() { for (auto i = 0; i < FEEDER_MOTOR_COUNT; i++) { - feederVelocityPIDs[i] = new SmoothPID(FEEDER_VELOCITY_PID_CONFIG[i]); + + #ifndef ALL_HEROES //TODO add ability for multiple pids for all robot feeders + feederVelocityPIDs[i] = new SmoothPID(FEEDER_VELOCITY_PID_CONFIG); + #else + feederVelocityPIDs[i] = new SmoothPID(FEEDER_VELOCITY_PID_CONFIG[i]); + #endif + } } From 825261b9f91a426e05f578a51c2250320cd34653 Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 17 Jun 2025 23:43:10 -0500 Subject: [PATCH 20/67] began fleshing out i2c power limiting code --- .../src/communicators/I2C/power_com.hpp | 18 ++++++++++++++++++ .../src/communicators/I2C/power_com_data.hpp | 16 ++++++++++++++++ 2 files changed, 34 insertions(+) create mode 100644 aimbots-src/src/communicators/I2C/power_com.hpp create mode 100644 aimbots-src/src/communicators/I2C/power_com_data.hpp diff --git a/aimbots-src/src/communicators/I2C/power_com.hpp b/aimbots-src/src/communicators/I2C/power_com.hpp new file mode 100644 index 00000000..0890d99d --- /dev/null +++ b/aimbots-src/src/communicators/I2C/power_com.hpp @@ -0,0 +1,18 @@ +//for reading the power limiting board for robots +//TODO fix targets, currently just not defined for engineer +#ifndef TARGET_ENGINEER +#include "modm/architecture/interface/i2c_device.hpp" +#include "modm/architecture/interface/register.hpp" +#include "modm/processing/protothread.hpp" +#include "modm/processing/resumable.hpp" + +namespace src::Communication { + class PowerComm : public tap::communication::serial::DJISerial{ + public: + + } +} + + + +#endif \ No newline at end of file diff --git a/aimbots-src/src/communicators/I2C/power_com_data.hpp b/aimbots-src/src/communicators/I2C/power_com_data.hpp new file mode 100644 index 00000000..6e07d412 --- /dev/null +++ b/aimbots-src/src/communicators/I2C/power_com_data.hpp @@ -0,0 +1,16 @@ +#ifndef TARGET_ENGINEER + +#include "tap/algorithms/math_user_utils.hpp" + +#include "modm/architecture/interface/register.hpp" +#include "modm/math/utils.hpp" + +namespace src::communicators::I2C::power_comm { +enum Register : uint8_t{ + DEVICE_ADRESS = 0x6A, + WHO_AM_I = 0x0F, + +} +} + +#endif \ No newline at end of file From 7c211cc2516aa748ed8c7603491be31761dd1e86 Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 25 Jun 2025 02:19:39 -0700 Subject: [PATCH 21/67] tap --- taproot | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/taproot b/taproot index bcbd843f..cd047d2f 160000 --- a/taproot +++ b/taproot @@ -1 +1 @@ -Subproject commit bcbd843fcaa69a26d165fa86b2a0528c9744b86b +Subproject commit cd047d2fd6d932320d03390e6e6e3fff4fcb38fd From d9590f21e22c05515a06cc8113325770cd342746 Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 25 Jun 2025 03:45:17 -0700 Subject: [PATCH 22/67] final touches on hero --- .../src/robots/hero/constants/hero_chassis_constants.hpp | 6 +++--- .../src/robots/hero/constants/hero_gimbal_constants.hpp | 6 +++--- .../feeder/complex_commands/feeder_limit_command.cpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp index 4a38a597..f49ff7ec 100644 --- a/aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp @@ -22,7 +22,7 @@ static constexpr SmoothPIDConfig CHASSIS_VELOCITY_PID_CONFIG = { }; // 1 for no symmetry, 2 for 180 degree symmetry, 4 for 90 degree symmetry -static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 2; +static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 4; // CAN Bus 2 static constexpr CANBus CHASSIS_BUS = CANBus::CAN_BUS2; @@ -38,9 +38,9 @@ static constexpr MotorID RIGHT_BACK_WHEEL_ID = MotorID::MOTOR4; */ static constexpr float WHEEL_RADIUS = 0.07663f; -static constexpr float WHEELBASE_WIDTH = 0.357f; +static constexpr float WHEELBASE_WIDTH = 0.500f; -static constexpr float WHEELBASE_LENGTH = 0.357f; +static constexpr float WHEELBASE_LENGTH = 0.5372f; static constexpr float CHASSIS_GEARBOX_RATIO = (1.0f / 19.0f); diff --git a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp index 35400e97..439c762a 100644 --- a/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_gimbal_constants.hpp @@ -19,11 +19,11 @@ static const std::array YAW_MOTOR_IDS = {MotorID::MOTO static const std::array YAW_MOTOR_NAMES = {"Yaw Motor 1"}; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ static const std::array YAW_MOTOR_OFFSET_ANGLES = { - wrapTo0To2PIRange(modm::toRadian(252.4f))}; + wrapTo0To2PIRange(modm::toRadian(63.37f))}; static constexpr float YAW_AXIS_START_ANGLE = modm::toRadian(0.0f); - static constexpr float GIMBAL_YAW_GEAR_RATIO = 1.0f/2.0754716981f; // for 2025 Hero -//static constexpr float GIMBAL_YAW_GEAR_RATIO = 1.0f/2.0f; // for 2025 Hero + //static constexpr float GIMBAL_YAW_GEAR_RATIO = 1.0f/2.0754716981f; // for 2025 Hero +static constexpr float GIMBAL_YAW_GEAR_RATIO = 1.0f/2.0f; // for 2025 Hero /* Changing this means the encoder-readable range of the YAW axis is reduced to 360deg * GIMBAL_YAW_GEAR_RATIO before the * encoder readings will repeat. We will assume that the robot will be started within the same GIMBAL_YAW_GEAR_RATIO range * every time. We also assume that 1 / GIMBAL_YAW_GEAR_RATIO is an integer multiple of 360deg. */ diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index d7c88a09..837bc5f6 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -50,7 +50,7 @@ void FeederLimitCommand::execute() { // Updates the current controller switch state currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true); // States how long the limit switch is ignored when firing a projectile - limitSwitchDownTime = 300; + limitSwitchDownTime = 200; // States the speed of the feeder wheel when firing // Checks if the limit switch is pressed & is "not killed" From ba5226825d22dc0615f61412ffbc054153e6503d Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 25 Jun 2025 05:08:29 -0700 Subject: [PATCH 23/67] fixed standard gimball pid --- .../constants/standard_gimbal_constants.hpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp index 2ff98625..2e6956da 100644 --- a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp @@ -8,6 +8,7 @@ /** * @brief GIMBAL SETUP */ + static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS2; static constexpr CANBus PITCH_GIMBAL_BUS = CANBus::CAN_BUS1; @@ -20,14 +21,16 @@ static const std::array YAW_MOTOR_OFFSET_ANGLES = {modm: static const std::array PITCH_MOTOR_OFFSET_ANGLES = {modm::toRadian(11.11f)}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-14.5f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(17.5f); +static const std::array PITCH_MOTOR_DIRECTIONS = {false}; // LOW should be lesser than HIGH, otherwise switch the motor direction #elif defined(TARGET_STANDARD_2025) /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot cassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(84.73f))}; -static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(180.0f))}; +static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(-211.2f))}; +static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(298.25f))}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-14.5f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(17.5f); +static const std::array PITCH_MOTOR_DIRECTIONS = {true}; // LOW should be lesser than HIGH, otherwise switch the motor direction #elif defined(TARGET_STANDARD_SQUIRTLE) @@ -36,6 +39,7 @@ static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapT static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(-110.64f))}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-14.5f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(17.5f); +static const std::array PITCH_MOTOR_DIRECTIONS = {false}; // LOW should be lesser than HIGH, otherwise switch the motor direction #elif defined(TARGET_STANDARD_2023) @@ -44,6 +48,7 @@ static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapT static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(166.86f))}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-24.0f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(22.0f); +static const std::array PITCH_MOTOR_DIRECTIONS = {false}; // LOW should be lesser than HIGH, otherwise switch the motor direction #endif @@ -57,7 +62,6 @@ static constexpr float GIMBAL_YAW_GEAR_RATIO = (1.0f / 2.0f); // for 2024 Stand /*Changing this means the encoder-readable range of the YAW axis is reduced to 360deg * GIMBAL_YAW_GEAR_RATIO before the * encoder readings will repeat. We will assume that the robot will be started within the same GIMBAL_YAW_GEAR_RATIO range * every time. We also assume that 1 / GIMBAL_YAW_GEAR_RATIO is an integer multiple of 360deg. */ -static const std::array PITCH_MOTOR_DIRECTIONS = {false}; static const std::array PITCH_MOTOR_IDS = {MotorID::MOTOR6}; static const std::array PITCH_MOTOR_NAMES = {"Pitch Motor 1"}; static constexpr float PITCH_AXIS_START_ANGLE = modm::toRadian(0.0f); @@ -80,7 +84,7 @@ static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.0f, + .errDeadzone = 10000.0f, .errorDerivativeFloor = 0.0f, }; @@ -100,9 +104,9 @@ static constexpr SmoothPIDConfig PITCH_POSITION_PID_CONFIG = { // VISION PID CONSTANTS static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { - .kp = 20.0f, // 30 + .kp = 30.0f, // 30 .ki = 0.0f, - .kd = 0.0f, + .kd = 0.5f, .maxICumulative = 1.0f, .maxOutput = 40.0f, // 40 rad/s is maximum speed of 6020 .tQDerivativeKalman = 1.0f, @@ -138,7 +142,7 @@ static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.0f, + .errDeadzone = 1000.0f, .errorDerivativeFloor = 0.0f, }; From 207bfb76f3779e2103b8709de7e0cde3b8f87b42 Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 25 Jun 2025 06:01:58 -0700 Subject: [PATCH 24/67] unlobotomized hero --- .../src/informants/kinematics/kinematic_informant.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index 5ffc8c7f..d52bacb7 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -264,16 +264,10 @@ float KinematicInformant::getChassisPitchAngleInGimbalDirection() { float cosGimbYaw = cosf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); float chassisRoll = getChassisIMUAngle(src::Informants::AngularAxis::ROLL_AXIS, AngleUnit::Radians); - #ifdef HERO - float chassisRoll = 0; - #endif float sinChasRoll = sinf(chassisRoll); float cosChasRoll = cosf(chassisRoll); float chassisPitch = getChassisIMUAngle(src::Informants::AngularAxis::PITCH_AXIS, AngleUnit::Radians); - #ifdef HERO - float chassisPitch = 0; - #endif float sinChasPitch = sinf(chassisPitch); float cosChasPitch = cosf(chassisPitch); From 93759e0ed67c84859e2f16dca3dbd671a9360a18 Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 25 Jun 2025 07:40:39 -0700 Subject: [PATCH 25/67] fixed sentry const --- .../constants/sentry_chassis_constants.hpp | 32 +++++++++---------- .../constants/sentry_feeder_constants.hpp | 2 +- .../constants/sentry_gimbal_constants.hpp | 24 +++++++------- .../constants/sentry_shooter_constants.hpp | 4 +-- 4 files changed, 31 insertions(+), 31 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 41779a6e..b3c9c934 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -14,9 +14,9 @@ static constexpr MotorID RIGHT_FRONT_YAW_ID = MotorID::MOTOR7; static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { - .kp = 40.0f, + .kp = 15.0f, .ki = 0.0f, - .kd = 5.0f, + .kd = 2.0f, .maxICumulative = 0.9f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, @@ -27,10 +27,10 @@ static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { .errorDerivativeFloor = 0.0f, }; -static constexpr int LEFT_FRONT_YAW_OFFSET = 2410; -static constexpr int RIGHT_FRONT_YAW_OFFSET = 1670; -static constexpr int LEFT_BACK_YAW_OFFSET = 255; -static constexpr int RIGHT_BACK_YAW_OFFSET = 6450; +static constexpr int LEFT_FRONT_YAW_OFFSET = 2000; +static constexpr int RIGHT_FRONT_YAW_OFFSET = 7500; +static constexpr int LEFT_BACK_YAW_OFFSET = 6200; +static constexpr int RIGHT_BACK_YAW_OFFSET = 7500; #elif defined(TARGET_SENTRY_BRAVO) static constexpr uint8_t MOTORS_PER_WHEEL = 1; @@ -62,27 +62,27 @@ static constexpr SmoothPIDConfig CHASSIS_VELOCITY_PID_CONFIG = { }; // 1 for no symmetry, 2 for 180 degree symmetry, 4 for 90 degree symmetry -static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 4; +static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 2; // CAN Bus 2 -static constexpr CANBus CHASSIS_BUS = CANBus::CAN_BUS1; +static constexpr CANBus CHASSIS_BUS = CANBus::CAN_BUS2; -static constexpr MotorID LEFT_BACK_WHEEL_ID = MotorID::MOTOR4; -static constexpr MotorID LEFT_FRONT_WHEEL_ID = MotorID::MOTOR2; -static constexpr MotorID RIGHT_FRONT_WHEEL_ID = MotorID::MOTOR1; -static constexpr MotorID RIGHT_BACK_WHEEL_ID = MotorID::MOTOR3; +static constexpr MotorID LEFT_BACK_WHEEL_ID = MotorID::MOTOR1; +static constexpr MotorID LEFT_FRONT_WHEEL_ID = MotorID::MOTOR3; +static constexpr MotorID RIGHT_FRONT_WHEEL_ID = MotorID::MOTOR4; +static constexpr MotorID RIGHT_BACK_WHEEL_ID = MotorID::MOTOR2; // Mechanical chassis constants, all in m /** * Radius of the wheels (m). */ -static constexpr float WHEEL_RADIUS = 0.07663f; +static constexpr float WHEEL_RADIUS = 0.0635f; -static constexpr float WHEELBASE_WIDTH = 0.357f; +static constexpr float WHEELBASE_WIDTH = 0.393f; -static constexpr float WHEELBASE_LENGTH = 0.357f; +static constexpr float WHEELBASE_LENGTH = 0.393f; -static constexpr float CHASSIS_GEARBOX_RATIO = (187.0f / 3591.0f); +static constexpr float CHASSIS_GEARBOX_RATIO = (17.0f / 268.0f); // Power limiting constants, will explain later static constexpr float POWER_LIMIT_SAFETY_FACTOR = 0.85f; diff --git a/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp index de7720da..5bfba6ac 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp @@ -5,7 +5,7 @@ #define FEEDER_COMPATIBLE static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG = { - .kp = 15.0f, // 40 + .kp = 50.0f, // 40 .ki = 0.0f, .kd = 0.8f, // 0.01 .maxICumulative = 10.0f, diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 63ad9c45..905928e8 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -16,7 +16,7 @@ static constexpr uint8_t YAW_MOTOR_COUNT = 1; static constexpr uint8_t PITCH_MOTOR_COUNT = 1; #if defined(TARGET_SENTRY_BRAVO) -static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS2; +static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS1; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(42.58f))}; static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(229.0f))}; @@ -28,11 +28,11 @@ static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(30.0f); #elif defined(TARGET_SENTRY_SWERVE) static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS1; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(42.58f))}; -static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(229.0f))}; +static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(54.58f))}; +static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(147.8f))}; -static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-2.0f); -static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(30.0f); +static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(0.0f); +static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(40.0f); // LOW should be lesser than HIGH, otherwise switch the motor direction #endif @@ -48,13 +48,13 @@ static constexpr float GIMBAL_YAW_GEAR_RATIO = (1.0f / 2.0f); // for 2024 Sentr * encoder readings will repeat. We will assume that the robot will be started within the same GIMBAL_YAW_GEAR_RATIO range * every time. We also assume that 1 / GIMBAL_YAW_GEAR_RATIO is an integer multiple of 360deg. */ -static const std::array PITCH_MOTOR_DIRECTIONS = {true}; +static const std::array PITCH_MOTOR_DIRECTIONS = {false}; static const std::array PITCH_MOTOR_IDS = {MotorID::MOTOR6}; static const std::array PITCH_MOTOR_NAMES = {"Pitch Motor 1"}; static constexpr float PITCH_AXIS_START_ANGLE = modm::toRadian(0.0f); -static constexpr float GIMBAL_PITCH_GEAR_RATIO = (30.0f / 102.0f); // for 2023 Sentry +static constexpr float GIMBAL_PITCH_GEAR_RATIO = (5.0f / 17.0f); // for 2023 Sentry /*Changing this means the encoder-readable range of the PITCH axis is reduced to 360deg * GIMBAL_PITCH_GEAR_RATIO before the * encoder readings will repeat. We will assume that the range of the pitch axis is hardware-limited to not exceed this * range, but the motor angle may cross 0 in this range. Example Range: 278deg to 28deg */ @@ -63,16 +63,16 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (30.0f / 102.0f); // for 2023 * @brief Position PID constants */ static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { - .kp = 20'000.0f, + .kp = 1'000.0f, .ki = 0.0f, - .kd = 10.0f, + .kd = 2000.0f, .maxICumulative = 0.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.0f, + .errDeadzone = 10.0f, .errorDerivativeFloor = 0.0f, }; @@ -121,7 +121,7 @@ static constexpr SmoothPIDConfig PITCH_POSITION_CASCADE_PID_CONFIG = { // VELOCITY PID CONSTANTS static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { - .kp = 2200.0f, + .kp = 2000.0f, .ki = 25.0f, .kd = 0.0f, .maxICumulative = 2000.0f, @@ -130,7 +130,7 @@ static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.0f, + .errDeadzone = 10.0f, .errorDerivativeFloor = 0.0f, }; diff --git a/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp index 0737f490..c8430df5 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp @@ -36,8 +36,8 @@ static constexpr MotorID SHOOTER_2_ID = MotorID::MOTOR2; static constexpr MotorID SHOOTER_3_ID = MotorID::MOTOR3; static constexpr MotorID SHOOTER_4_ID = MotorID::MOTOR4; -static constexpr bool SHOOTER_1_DIRECTION = true; -static constexpr bool SHOOTER_2_DIRECTION = false; +static constexpr bool SHOOTER_1_DIRECTION = false; +static constexpr bool SHOOTER_2_DIRECTION = true; static constexpr bool SHOOTER_3_DIRECTION = false; static constexpr bool SHOOTER_4_DIRECTION = true; From 88d41728baa2eabe65414bd42dd544828bee1883 Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 25 Jun 2025 10:25:02 -0700 Subject: [PATCH 26/67] merges power reading with pre rebuild branch --- .../INA260/INA260_communicator.cpp | 134 ++++++++++++++++++ .../INA260/INA260_communicator.hpp | 53 +++++++ .../communicators/INA260/INA260_protocol.hpp | 20 +++ aimbots-src/src/drivers.hpp | 3 + aimbots-src/src/main.cpp | 11 +- .../motion/power_limiter/power_limiter.cpp | 3 +- 6 files changed, 221 insertions(+), 3 deletions(-) create mode 100644 aimbots-src/src/communicators/INA260/INA260_communicator.cpp create mode 100644 aimbots-src/src/communicators/INA260/INA260_communicator.hpp create mode 100644 aimbots-src/src/communicators/INA260/INA260_protocol.hpp diff --git a/aimbots-src/src/communicators/INA260/INA260_communicator.cpp b/aimbots-src/src/communicators/INA260/INA260_communicator.cpp new file mode 100644 index 00000000..f3095e77 --- /dev/null +++ b/aimbots-src/src/communicators/INA260/INA260_communicator.cpp @@ -0,0 +1,134 @@ + +#ifndef ALL_SENTRIES + +#include "INA260_communicator.hpp" + +#include + +#define READ(data, length) drivers->uart.read(INA260_UART_PORT, data, length) +#define WRITE(data, length) drivers->uart.write(INA260_UART_PORT, data, length) + +namespace src::Informants::INA260 { + +float powerDisplay = 0.0f; +float voltage_mVDisplay = 0.0f; +float current_mADisplay = 0.0f; + +INA260Communicator::INA260Communicator(src::Drivers* drivers) + : drivers(drivers), + currentSerialState(INA260CommunicatorSerialState::SearchingForMagic), + nextByteIndex(0), + INA260OfflineTimeout(), + lastMessage() +{} + +void INA260Communicator::initialize() { + INA260OfflineTimeout.restart(INA260_OFFLINE_TIMEOUT_MILLISECONDS); + drivers->uart.init(); +} + +uint8_t displayBuffer[INA260_MESSAGE_SIZE]; +int displayBufIndex = 0; + +int lastMsgTimeDisplay = 0; +int msBetweenLastMessageDisplay = 0; + +/** + * @brief Rewrote jetson communictor to use UART to + * recieve data from esp32 that sends INA260 data + * Last mintue comp decision + */ +alignas(INA260Message) uint8_t rawSerialDisplay[sizeof(INA260Message)]; + +void INA260Communicator::updateSerial() { + uint32_t currTime = tap::arch::clock::getTimeMilliseconds(); + + size_t bytesRead = READ(&rawSerialBuffer[nextByteIndex], 1); // attempts to pull one byte from the buffer + if (bytesRead != 1) return; + + INA260OfflineTimeout.restart(INA260_OFFLINE_TIMEOUT_MILLISECONDS); + + displayBuffer[displayBufIndex] = rawSerialBuffer[0]; // copy byte to display buffer + displayBufIndex = (displayBufIndex + 1) % INA260_MESSAGE_SIZE; // increment display index and wrap around if necessary + + switch (currentSerialState) { + // ...looking for message start... + case INA260CommunicatorSerialState::SearchingForMagic: { + // Check if the byte we just read is the byte we expected in the magic number. + if (rawSerialBuffer[nextByteIndex] == ((INA260_MESSAGE_MAGIC >> (8 * nextByteIndex)) & 0xff)) { + nextByteIndex++; + } else { + nextByteIndex = 0; // if not, reset the index and start over. + } + + // Wait until we've reached the end of the magic number. If any of the bytes in the magic number weren't a match, + // we wouldn't have gotten this far. + if (nextByteIndex == sizeof(decltype(INA260_MESSAGE_MAGIC))) { + currentSerialState = INA260CommunicatorSerialState::AssemblingMessage; + } + break; + } + // ...found message start, assemble message... + case INA260CommunicatorSerialState::AssemblingMessage: { + nextByteIndex++; + + // Increment the byte index until we reach the expected end of a message, then parse the message. + if (nextByteIndex == INA260_MESSAGE_SIZE) { + // Reinterpret the received bytes into a JetsonMessage + lastMessage = *reinterpret_cast(&rawSerialBuffer); + powerDisplay = lastMessage.power; + voltage_mVDisplay = lastMessage.voltage_mV; + current_mADisplay = lastMessage.current_mA; + + // Update last message time and time between last message and now. + if (lastMsgTimeDisplay == 0) { + lastMsgTimeDisplay = tap::arch::clock::getTimeMilliseconds(); + } else { + msBetweenLastMessageDisplay = + currTime - lastMsgTimeDisplay; // Should be pretty close to the message send rate. + lastMsgTimeDisplay = currTime; + } + + // As we've received a full message, reset the byte index and go back to searching for the magic number. + nextByteIndex = 0; + currentSerialState = INA260CommunicatorSerialState::SearchingForMagic; + } else { + rawSerialDisplay[nextByteIndex] = rawSerialBuffer[nextByteIndex]; + } + + break; + } + } + +} + +INA260Message const& INA260Communicator::getLastValidMessage() const { + return lastMessage; +} + +// float INA260Communicator::twoBytesToFloat(uint16_t bytes) const{ +// uint32_t empty = 0x00000000; +// uint32_t firstSignBit = empty; +// uint32_t mantissaBits = empty; +// uint32_t exponentBits = empty; +// uint32_t floatBits = empty; + +// firstSignBit |= bytes & 0x8000; +// firstSignBit = firstSignBit << 16; // copy first bit + +// exponentBits |= bytes & 0x7C00; +// exponentBits = exponentBits >> 10; +// int32_t exponent = ((int32_t)exponentBits) - 15; // subtract half-precision bias +// exponent += 127; // add single percision bias +// exponentBits = ((uint32_t)exponent) << 23; // move to position + +// mantissaBits |= bytes & 0x03FF; +// mantissaBits = mantissaBits << 13; // start mantissa bits on 9th bit + +// floatBits = firstSignBit | exponentBits | mantissaBits; + +// return * (float *) &floatBits; // Carmack style 😎 +// } + +} // namespace src::Informants::INA260 +#endif // ALL_SENTRIES \ No newline at end of file diff --git a/aimbots-src/src/communicators/INA260/INA260_communicator.hpp b/aimbots-src/src/communicators/INA260/INA260_communicator.hpp new file mode 100644 index 00000000..b65fd95b --- /dev/null +++ b/aimbots-src/src/communicators/INA260/INA260_communicator.hpp @@ -0,0 +1,53 @@ +#pragma once + +#ifndef ALL_SENTRIES + +#include +#include +#include + + +#include "INA260_protocol.hpp" + +namespace src { +class Drivers; +} + +namespace src::Informants::INA260 { + +enum class INA260CommunicatorSerialState : uint8_t { + SearchingForMagic = 0, + AssemblingMessage, +}; + +class INA260Communicator{ +public: + INA260Communicator(src::Drivers* drivers); + DISALLOW_COPY_AND_ASSIGN(INA260Communicator); + ~INA260Communicator() = default; + + void initialize(); + void updateSerial(); + + inline bool isINA260Online() const { return !INA260OfflineTimeout.isExpired(); } + + INA260Message const& getLastValidMessage() const; + +private: + src::Drivers* drivers; + + alignas(INA260Message) uint8_t rawSerialBuffer[sizeof(INA260Message)]; + + INA260CommunicatorSerialState currentSerialState; + size_t nextByteIndex; + + tap::arch::MilliTimeout INA260OfflineTimeout; + + static constexpr uint32_t INA260_BAUD_RATE = 115200; + static constexpr uint16_t INA260_OFFLINE_TIMEOUT_MILLISECONDS = 2000; + static constexpr UartPort INA260_UART_PORT = UartPort::Uart1; + + INA260Message lastMessage; +}; +} // namespace src::Informants::INA260 +#endif // ALL_SENTRIES \ No newline at end of file diff --git a/aimbots-src/src/communicators/INA260/INA260_protocol.hpp b/aimbots-src/src/communicators/INA260/INA260_protocol.hpp new file mode 100644 index 00000000..986f2550 --- /dev/null +++ b/aimbots-src/src/communicators/INA260/INA260_protocol.hpp @@ -0,0 +1,20 @@ +#pragma once + +#ifndef ALL_SENTRIES + +namespace src::Informants::INA260 { + +static constexpr uint8_t INA260_MESSAGE_MAGIC = 'c'; + +struct INA260Message { + uint8_t magic; + float power; + float voltage_mV; + float current_mA; +} __attribute__((packed)); + +static_assert(sizeof(INA260Message) == 13, "INA260Message is not the correct size"); + +static constexpr size_t INA260_MESSAGE_SIZE = sizeof(INA260Message); +} // namespace src::Informants::INA260 +#endif // ALL_SENTRIES \ No newline at end of file diff --git a/aimbots-src/src/drivers.hpp b/aimbots-src/src/drivers.hpp index bd538bf6..8755d46e 100644 --- a/aimbots-src/src/drivers.hpp +++ b/aimbots-src/src/drivers.hpp @@ -27,6 +27,7 @@ // #include "informants/ultrasonic_distance_sensor.hpp" #include "communicators/devboard/turret_can_communicator.hpp" #include "communicators/jetson/jetson_communicator.hpp" +#include "communicators/INA260/INA260_communicator.hpp" #include "utils/music/jukebox_player.hpp" #include "utils/nxp_imu/magnetometer/ist8310.hpp" #include "utils/tools/robot_specific_defines.hpp" @@ -44,6 +45,7 @@ class Drivers : public tap::Drivers { controlOperatorInterface(this), magnetometer(), cvCommunicator(this), + powerCommunicator(this), kinematicInformant(this), hitTracker(this), turretCommunicator(this, CANBus::CAN_BUS1), @@ -53,6 +55,7 @@ class Drivers : public tap::Drivers { Control::OperatorInterface controlOperatorInterface; utils::Ist8310 magnetometer; Informants::Vision::JetsonCommunicator cvCommunicator; + Informants::INA260::INA260Communicator powerCommunicator; Informants::KinematicInformant kinematicInformant; Informants::HitTracker hitTracker; Informants::TurretComms::TurretCommunicator turretCommunicator; diff --git a/aimbots-src/src/main.cpp b/aimbots-src/src/main.cpp index 935f139a..c01b7f83 100644 --- a/aimbots-src/src/main.cpp +++ b/aimbots-src/src/main.cpp @@ -67,6 +67,7 @@ uint32_t loopTimeDisplay = 0; uint16_t currHeat = 69; uint16_t currHeatLimit = 420; uint16_t chassisPowerLimit = 77; +float powerDis = 69.0f; SongTitle playSongWatch = PACMAN; // Watch variable @@ -147,7 +148,11 @@ static void initializeIo(src::Drivers *drivers) { drivers->remote.initialize(); drivers->refSerial.initialize(); // drivers->magnetometer.init(); +#ifdef ALL_SENTRIES drivers->cvCommunicator.initialize(); +#else + drivers->powerCommunicator.initialize(); +#endif drivers->kinematicInformant.recalibrateIMU( {CIMU_CALIBRATION_EULER_X, CIMU_CALIBRATION_EULER_Y, CIMU_CALIBRATION_EULER_Z}); #else @@ -182,8 +187,11 @@ static void updateIo(src::Drivers *drivers) { drivers->canRxHandler.pollCanData(); // should probably also be updating for turret imu?? drivers->refSerial.updateSerial(); drivers->remote.read(); - +#ifdef ALL_SENTRIES drivers->cvCommunicator.updateSerial(); +#else + drivers->powerCommunicator.updateSerial(); +#endif #else drivers->turretCommunicator.sendIMUData(); #endif @@ -216,6 +224,7 @@ static void updateIo(src::Drivers *drivers) { hitDisplay = drivers->hitTracker.getHitAngle_gimbalRelative(); wasHit = drivers->hitTracker.wasHit(); recentlyHit = drivers->hitTracker.recentlyHit(); + powerDis = drivers->powerCommunicator.getLastValidMessage().power; // yawDisplay = modm::toDegree(yaw); // pitchDisplay = modm::toDegree(pitch); diff --git a/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp b/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp index 52d7acde..67751807 100644 --- a/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp +++ b/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp @@ -38,8 +38,7 @@ float PowerLimiter::getPowerLimitRatio() { void PowerLimiter::updatePowerAndEnergyBuffer() { const auto &robotData = drivers->refSerial.getRobotData(); const auto &chassisData = robotData.chassis; - const float current = chassisData.current; - const float newChassisPower = (chassisData.volt * current) / 1'000'000.0f; + const float newChassisPower = drivers->powerCommunicator.getLastValidMessage().power; // we're multiplying by 1'000'000.0f to convert from microwatts to watts const float dt = tap::arch::clock::getTimeMilliseconds() - prevTime; From 3ebb0a44a339b42ebecaede5519e5ab5df3b193d Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Thu, 26 Jun 2025 00:13:36 -0500 Subject: [PATCH 27/67] slowed flywheels on standard/sentry --- .../src/robots/sentry/constants/sentry_shooter_constants.hpp | 2 +- .../robots/standard/constants/standard_shooter_constants.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp index c8430df5..08cf23bc 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp @@ -22,7 +22,7 @@ static constexpr SmoothPIDConfig SHOOTER_VELOCITY_PID_CONFIG = { // clang-format off; // Sentry shoots at the speed of death -static constexpr uint16_t shooter_speed_array[2] = {30, 7450}; // {m/s, rpm} +static constexpr uint16_t shooter_speed_array[2] = {30, 6254}; // {m/s, rpm} // clang-format on static const Matrix SHOOTER_SPEED_MATRIX(shooter_speed_array); diff --git a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp index 562ddfb1..6c956b1a 100644 --- a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp @@ -27,7 +27,7 @@ static constexpr uint16_t shooter_speed_array[6] = { // ONLY TUNE WITH FULL BAT 18, 4850, 30, - 7150}; + 6254}; // clang-format on static const Matrix SHOOTER_SPEED_MATRIX(shooter_speed_array); From 6b54934808f9a2d9fa0f36ff69d9842f41a0daa9 Mon Sep 17 00:00:00 2001 From: zDagger Date: Wed, 25 Jun 2025 23:20:23 -0700 Subject: [PATCH 28/67] fixed blastoise offset --- .../src/robots/standard/constants/standard_gimbal_constants.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp index 2e6956da..02fd73cf 100644 --- a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp @@ -17,7 +17,7 @@ static constexpr uint8_t PITCH_MOTOR_COUNT = 1; #if defined(TARGET_STANDARD_BLASTOISE) /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = {modm::toRadian(309.199f)}; +static const std::array YAW_MOTOR_OFFSET_ANGLES = {modm::toRadian(144.27f)}; static const std::array PITCH_MOTOR_OFFSET_ANGLES = {modm::toRadian(11.11f)}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-14.5f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(17.5f); From 84e6c9edef5e5e124cdbc1f48003358a24c36aa6 Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 26 Jun 2025 00:22:21 -0700 Subject: [PATCH 29/67] removed old power limit --- aimbots-src/src/drivers.hpp | 6 +++--- aimbots-src/src/main.cpp | 7 ++----- .../src/utils/motion/power_limiter/power_limiter.cpp | 2 +- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/aimbots-src/src/drivers.hpp b/aimbots-src/src/drivers.hpp index 8755d46e..2590c9d7 100644 --- a/aimbots-src/src/drivers.hpp +++ b/aimbots-src/src/drivers.hpp @@ -27,7 +27,7 @@ // #include "informants/ultrasonic_distance_sensor.hpp" #include "communicators/devboard/turret_can_communicator.hpp" #include "communicators/jetson/jetson_communicator.hpp" -#include "communicators/INA260/INA260_communicator.hpp" +//#include "communicators/INA260/INA260_communicator.hpp" #include "utils/music/jukebox_player.hpp" #include "utils/nxp_imu/magnetometer/ist8310.hpp" #include "utils/tools/robot_specific_defines.hpp" @@ -45,7 +45,7 @@ class Drivers : public tap::Drivers { controlOperatorInterface(this), magnetometer(), cvCommunicator(this), - powerCommunicator(this), + // powerCommunicator(this), kinematicInformant(this), hitTracker(this), turretCommunicator(this, CANBus::CAN_BUS1), @@ -55,7 +55,7 @@ class Drivers : public tap::Drivers { Control::OperatorInterface controlOperatorInterface; utils::Ist8310 magnetometer; Informants::Vision::JetsonCommunicator cvCommunicator; - Informants::INA260::INA260Communicator powerCommunicator; + // Informants::INA260::INA260Communicator powerCommunicator; Informants::KinematicInformant kinematicInformant; Informants::HitTracker hitTracker; Informants::TurretComms::TurretCommunicator turretCommunicator; diff --git a/aimbots-src/src/main.cpp b/aimbots-src/src/main.cpp index c01b7f83..2e8da10c 100644 --- a/aimbots-src/src/main.cpp +++ b/aimbots-src/src/main.cpp @@ -187,11 +187,8 @@ static void updateIo(src::Drivers *drivers) { drivers->canRxHandler.pollCanData(); // should probably also be updating for turret imu?? drivers->refSerial.updateSerial(); drivers->remote.read(); -#ifdef ALL_SENTRIES drivers->cvCommunicator.updateSerial(); -#else - drivers->powerCommunicator.updateSerial(); -#endif + // drivers->powerCommunicator.updateSerial(); #else drivers->turretCommunicator.sendIMUData(); #endif @@ -224,7 +221,7 @@ static void updateIo(src::Drivers *drivers) { hitDisplay = drivers->hitTracker.getHitAngle_gimbalRelative(); wasHit = drivers->hitTracker.wasHit(); recentlyHit = drivers->hitTracker.recentlyHit(); - powerDis = drivers->powerCommunicator.getLastValidMessage().power; + // powerDis = drivers->powerCommunicator.getLastValidMessage().power; // yawDisplay = modm::toDegree(yaw); // pitchDisplay = modm::toDegree(pitch); diff --git a/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp b/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp index 67751807..d7b95d24 100644 --- a/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp +++ b/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp @@ -38,7 +38,7 @@ float PowerLimiter::getPowerLimitRatio() { void PowerLimiter::updatePowerAndEnergyBuffer() { const auto &robotData = drivers->refSerial.getRobotData(); const auto &chassisData = robotData.chassis; - const float newChassisPower = drivers->powerCommunicator.getLastValidMessage().power; + const float newChassisPower = 0; // we're multiplying by 1'000'000.0f to convert from microwatts to watts const float dt = tap::arch::clock::getTimeMilliseconds() - prevTime; From 7f67cd50c259444d3c9ed03da98dc50497331bd0 Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 26 Jun 2025 01:00:33 -0700 Subject: [PATCH 30/67] added barrel over heat limit for single shot fire --- aimbots-src/src/main.cpp | 5 +---- .../feeder/complex_commands/feeder_limit_command.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/aimbots-src/src/main.cpp b/aimbots-src/src/main.cpp index 2e8da10c..40d85531 100644 --- a/aimbots-src/src/main.cpp +++ b/aimbots-src/src/main.cpp @@ -148,11 +148,8 @@ static void initializeIo(src::Drivers *drivers) { drivers->remote.initialize(); drivers->refSerial.initialize(); // drivers->magnetometer.init(); -#ifdef ALL_SENTRIES drivers->cvCommunicator.initialize(); -#else - drivers->powerCommunicator.initialize(); -#endif + // drivers->powerCommunicator.initialize(); drivers->kinematicInformant.recalibrateIMU( {CIMU_CALIBRATION_EULER_X, CIMU_CALIBRATION_EULER_Y, CIMU_CALIBRATION_EULER_Z}); #else diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index 837bc5f6..fc22c894 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -21,6 +21,7 @@ bool prevFireState = false; bool prevLoadState = false; bool isFiring = false; bool loaderDormant = false; +bool underHeat = false; int limitSwitchDownTime; FeederLimitCommand::FeederLimitCommand( @@ -43,18 +44,19 @@ void FeederLimitCommand::initialize() { } void FeederLimitCommand::execute() { + underHeat = refHelper->canCurrBarrelShootSafely(); // Updates the limit switch state (is pressed or not) limitPressed = !feeder->getPressed(); // Logic inverted because of a wire oopsie // Updates the previous controller switch state (is up or not) prevFireState = currFireState; // Updates the current controller switch state - currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true); + currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true) && underHeat; // States how long the limit switch is ignored when firing a projectile limitSwitchDownTime = 200; // States the speed of the feeder wheel when firing // Checks if the limit switch is pressed & is "not killed" - prevLoadState = currLoadState; + prevLoadState = currLoadState; if (!limitswitchInactive.isExpired()) { limitPressed = false; From ab096a7e846f0c6d704f53d411dddb53a53ae2dc Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Thu, 26 Jun 2025 03:18:41 -0500 Subject: [PATCH 31/67] starting yaw tuning --- .../robots/sentry/constants/sentry_chassis_constants.hpp | 2 +- .../robots/sentry/constants/sentry_gimbal_constants.hpp | 4 ++-- aimbots-src/src/subsystems/chassis/control/chassis.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index b3c9c934..d63ce659 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -62,7 +62,7 @@ static constexpr SmoothPIDConfig CHASSIS_VELOCITY_PID_CONFIG = { }; // 1 for no symmetry, 2 for 180 degree symmetry, 4 for 90 degree symmetry -static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 2; +static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 4; // CAN Bus 2 static constexpr CANBus CHASSIS_BUS = CANBus::CAN_BUS2; diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 905928e8..2a11b376 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -64,8 +64,8 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (5.0f / 17.0f); // for 2023 Se */ static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { .kp = 1'000.0f, - .ki = 0.0f, - .kd = 2000.0f, + .ki = 10.0f, + .kd = 3'000.0f, .maxICumulative = 0.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index f5d395ae..54c5e9e9 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -283,7 +283,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - target_left_front_yaw = (3 * M_PI/4); + target_left_front_yaw = ( M_PI/4); optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; targetRPMs[LF][1] = left_front_yaw % 8191; @@ -291,7 +291,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - target_right_front_yaw = (M_PI / 4); + target_right_front_yaw = (3*M_PI / 4); optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; targetRPMs[RF][1] = right_front_yaw % 8191; @@ -299,7 +299,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - target_left_back_yaw = M_PI / 4; + target_left_back_yaw = 3*M_PI / 4; optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; targetRPMs[LB][1] = left_back_yaw % 8191; @@ -307,7 +307,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - target_right_back_yaw = 3 * M_PI / 4; + target_right_back_yaw = M_PI / 4; optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; targetRPMs[RB][1] = right_back_yaw % 8191; From afdece711415c960a8645369e3b70a1b6701568c Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 26 Jun 2025 02:29:56 -0700 Subject: [PATCH 32/67] updated cv gimbal scheduler --- .../sentry_match_gimbal_control_command.cpp | 8 +++++++- .../sentry_match_gimbal_control_command.hpp | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.cpp b/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.cpp index eda32ebb..09b396aa 100644 --- a/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.cpp +++ b/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.cpp @@ -21,7 +21,7 @@ SentryMatchGimbalControlCommand::SentryMatchGimbalControlCommand( patrolCommand(drivers, gimbal, controller, patrolConfig, chassisState), chaseCommand(drivers, gimbal, controller, refHelper, ballisticsSolver, 30.0f), chassisState(chassisState), - chaseTimeout(0), + chaseTimeout(1000), chaseTimeoutMillis(chaseTimeoutMillis) // { addSubsystemRequirement(gimbal); @@ -37,6 +37,7 @@ void SentryMatchGimbalControlCommand::initialize() { void SentryMatchGimbalControlCommand::execute() { if (!drivers->cvCommunicator.isJetsonOnline()) { scheduleIfNotScheduled(this->comprisedCommandScheduler, &patrolCommand); + descheduleIfScheduled(this->comprisedCommandScheduler, &chaseCommand, interrupted); this->comprisedCommandScheduler.run(); return; } @@ -44,10 +45,15 @@ void SentryMatchGimbalControlCommand::execute() { if (drivers->cvCommunicator.getLastValidMessage().cvState == src::Informants::Vision::FOUND || drivers->cvCommunicator.getLastValidMessage().cvState == src::Informants::Vision::FIRE) { scheduleIfNotScheduled(this->comprisedCommandScheduler, &chaseCommand); + descheduleIfScheduled(this->comprisedCommandScheduler, &patrolCommand, interrupted); chaseTimeout.restart(chaseTimeoutMillis); + }else if(!chaseTimeout.isExpired()){ + scheduleIfNotScheduled(this->comprisedCommandScheduler, &chaseCommand); + descheduleIfScheduled(this->comprisedCommandScheduler, &patrolCommand, interrupted); } if (chaseTimeout.isExpired()) { scheduleIfNotScheduled(this->comprisedCommandScheduler, &patrolCommand); + descheduleIfScheduled(this->comprisedCommandScheduler, &chaseCommand, interrupted); } this->comprisedCommandScheduler.run(); diff --git a/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.hpp b/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.hpp index 1664a8c1..9f8b61ce 100644 --- a/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.hpp +++ b/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.hpp @@ -49,7 +49,7 @@ class SentryMatchGimbalControlCommand : public TapComprisedCommand { src::Chassis::ChassisMatchStates& chassisState; MilliTimeout chaseTimeout; - int chaseTimeoutMillis = 0; + int chaseTimeoutMillis = 1000; }; } // namespace src::Gimbal From 3feb192057a5da744885f6d1abd7261fd6a26dce Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 26 Jun 2025 08:53:23 -0700 Subject: [PATCH 33/67] removed descheduler from sentry gimbal --- .../sentry_match_gimbal_control_command.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.cpp b/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.cpp index 09b396aa..2632d52e 100644 --- a/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.cpp +++ b/aimbots-src/src/subsystems/gimbal/complex_commands/sentry_match_gimbal_control_command.cpp @@ -37,7 +37,7 @@ void SentryMatchGimbalControlCommand::initialize() { void SentryMatchGimbalControlCommand::execute() { if (!drivers->cvCommunicator.isJetsonOnline()) { scheduleIfNotScheduled(this->comprisedCommandScheduler, &patrolCommand); - descheduleIfScheduled(this->comprisedCommandScheduler, &chaseCommand, interrupted); + // descheduleIfScheduled(this->comprisedCommandScheduler, &chaseCommand, interrupted); this->comprisedCommandScheduler.run(); return; } @@ -45,15 +45,15 @@ void SentryMatchGimbalControlCommand::execute() { if (drivers->cvCommunicator.getLastValidMessage().cvState == src::Informants::Vision::FOUND || drivers->cvCommunicator.getLastValidMessage().cvState == src::Informants::Vision::FIRE) { scheduleIfNotScheduled(this->comprisedCommandScheduler, &chaseCommand); - descheduleIfScheduled(this->comprisedCommandScheduler, &patrolCommand, interrupted); + // descheduleIfScheduled(this->comprisedCommandScheduler, &patrolCommand, interrupted); chaseTimeout.restart(chaseTimeoutMillis); }else if(!chaseTimeout.isExpired()){ scheduleIfNotScheduled(this->comprisedCommandScheduler, &chaseCommand); - descheduleIfScheduled(this->comprisedCommandScheduler, &patrolCommand, interrupted); + // descheduleIfScheduled(this->comprisedCommandScheduler, &patrolCommand, interrupted); } if (chaseTimeout.isExpired()) { scheduleIfNotScheduled(this->comprisedCommandScheduler, &patrolCommand); - descheduleIfScheduled(this->comprisedCommandScheduler, &chaseCommand, interrupted); + // descheduleIfScheduled(this->comprisedCommandScheduler, &chaseCommand, interrupted); } this->comprisedCommandScheduler.run(); From d3ca22a0220ef2771dc914f453cac55591898d38 Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 26 Jun 2025 11:57:51 -0700 Subject: [PATCH 34/67] removed barrel heat limit --- .../subsystems/feeder/complex_commands/feeder_limit_command.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index fc22c894..9a366ce1 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -50,7 +50,7 @@ void FeederLimitCommand::execute() { // Updates the previous controller switch state (is up or not) prevFireState = currFireState; // Updates the current controller switch state - currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true) && underHeat; + currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true); // States how long the limit switch is ignored when firing a projectile limitSwitchDownTime = 200; // States the speed of the feeder wheel when firing From e32f57aed48fbe39477492814d0f9f520d4fb306 Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 26 Jun 2025 12:04:57 -0700 Subject: [PATCH 35/67] removed barrel heat limiter --- .../feeder/complex_commands/feeder_limit_command.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index fc22c894..871db8f4 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -44,13 +44,13 @@ void FeederLimitCommand::initialize() { } void FeederLimitCommand::execute() { - underHeat = refHelper->canCurrBarrelShootSafely(); + //underHeat = refHelper->canCurrBarrelShootSafely(); // Updates the limit switch state (is pressed or not) limitPressed = !feeder->getPressed(); // Logic inverted because of a wire oopsie // Updates the previous controller switch state (is up or not) prevFireState = currFireState; // Updates the current controller switch state - currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true) && underHeat; + currFireState = (drivers->remote.getSwitch(Remote::Switch::RIGHT_SWITCH) == Remote::SwitchState::UP || drivers->remote.getMouseL()==true); // States how long the limit switch is ignored when firing a projectile limitSwitchDownTime = 200; // States the speed of the feeder wheel when firing From b3dc855e5b5615876f9cabcf7a6ba87e6c30ef99 Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 26 Jun 2025 12:06:58 -0700 Subject: [PATCH 36/67] removed barrel heat limit --- .../subsystems/feeder/complex_commands/feeder_limit_command.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp index 9a366ce1..871db8f4 100644 --- a/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp +++ b/aimbots-src/src/subsystems/feeder/complex_commands/feeder_limit_command.cpp @@ -44,7 +44,7 @@ void FeederLimitCommand::initialize() { } void FeederLimitCommand::execute() { - underHeat = refHelper->canCurrBarrelShootSafely(); + //underHeat = refHelper->canCurrBarrelShootSafely(); // Updates the limit switch state (is pressed or not) limitPressed = !feeder->getPressed(); // Logic inverted because of a wire oopsie // Updates the previous controller switch state (is up or not) From 0d8aadefcf71d87f89b286b5ea828ee6e5530529 Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 26 Jun 2025 15:31:32 -0700 Subject: [PATCH 37/67] lowred standard shooting speed --- .../standard/constants/standard_shooter_constants.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp index 6c956b1a..4bc42533 100644 --- a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp @@ -23,11 +23,11 @@ static constexpr SmoothPIDConfig SHOOTER_VELOCITY_PID_CONFIG = { // clang-format off static constexpr uint16_t shooter_speed_array[6] = { // ONLY TUNE WITH FULL BATTERY 15, - 4300, // {ball m/s, flywheel rpm} + 4250, // {ball m/s, flywheel rpm} 18, - 4850, + 4800, 30, - 6254}; + 6204}; // clang-format on static const Matrix SHOOTER_SPEED_MATRIX(shooter_speed_array); From c60913566800009826921885ba08c0402a38f691 Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Fri, 27 Jun 2025 00:08:16 -0500 Subject: [PATCH 38/67] chassis pid tuning --- .../robots/sentry/constants/sentry_chassis_constants.hpp | 8 ++++---- .../robots/sentry/constants/sentry_gimbal_constants.hpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index d63ce659..f9e0231f 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -14,9 +14,9 @@ static constexpr MotorID RIGHT_FRONT_YAW_ID = MotorID::MOTOR7; static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { - .kp = 15.0f, + .kp = 50.0f, .ki = 0.0f, - .kd = 2.0f, + .kd = 15.0f, .maxICumulative = 0.9f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, @@ -50,7 +50,7 @@ static constexpr float CHASSIS_LINEAR_ACCELERATION_PITCH_COMPENSATION = 0.0f; static constexpr SmoothPIDConfig CHASSIS_VELOCITY_PID_CONFIG = { .kp = 18.0f, .ki = 0.0f, - .kd = 1.0f, + .kd = 5.0f, .maxICumulative = 10.0f, .maxOutput = M3508_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, @@ -96,7 +96,7 @@ static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 15.0f; static constexpr int MIN_WHEEL_SPEED_SINGLE_MOTOR = 4000; static constexpr int MAX_WHEEL_SPEED_SINGLE_MOTOR = 8000; static constexpr int MIN_CHASSIS_POWER = 50; -static constexpr int MAX_CHASSIS_POWER = 120; +static constexpr int MAX_CHASSIS_POWER = 200; static constexpr int WHEEL_SPEED_OVER_CHASSIS_POWER_SLOPE = (MAX_WHEEL_SPEED_SINGLE_MOTOR - MIN_WHEEL_SPEED_SINGLE_MOTOR) / (MAX_CHASSIS_POWER - MIN_CHASSIS_POWER); static_assert(WHEEL_SPEED_OVER_CHASSIS_POWER_SLOPE >= 0); diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 2a11b376..30e92f6d 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -63,8 +63,8 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (5.0f / 17.0f); // for 2023 Se * @brief Position PID constants */ static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { - .kp = 1'000.0f, - .ki = 10.0f, + .kp = 2'000.0f, + .ki = 100.0f, .kd = 3'000.0f, .maxICumulative = 0.0f, .maxOutput = GM6020_MAX_OUTPUT, From 38d05187b6df5708ef2cd09040769dfdca149ee7 Mon Sep 17 00:00:00 2001 From: zDagger Date: Fri, 27 Jun 2025 02:03:22 -0700 Subject: [PATCH 39/67] tuned sentry --- .../constants/sentry_chassis_constants.hpp | 2 +- .../constants/sentry_gimbal_constants.hpp | 10 +-- .../src/robots/sentry/sentry_control.cpp | 3 +- .../sentry_match_chassis_control_command.cpp | 12 +-- .../subsystems/chassis/control/chassis.cpp | 78 +++++++++---------- 5 files changed, 53 insertions(+), 52 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index f9e0231f..a6a50ae2 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -14,7 +14,7 @@ static constexpr MotorID RIGHT_FRONT_YAW_ID = MotorID::MOTOR7; static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { - .kp = 50.0f, + .kp = 30.0f, .ki = 0.0f, .kd = 15.0f, .maxICumulative = 0.9f, diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 30e92f6d..8f56a80f 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -64,15 +64,15 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (5.0f / 17.0f); // for 2023 Se */ static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { .kp = 2'000.0f, - .ki = 100.0f, - .kd = 3'000.0f, + .ki = 0.0f, + .kd = 12000.0f, .maxICumulative = 0.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 10.0f, + .errDeadzone = 0.0f, .errorDerivativeFloor = 0.0f, }; @@ -123,14 +123,14 @@ static constexpr SmoothPIDConfig PITCH_POSITION_CASCADE_PID_CONFIG = { static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { .kp = 2000.0f, .ki = 25.0f, - .kd = 0.0f, + .kd = 1.0f, .maxICumulative = 2000.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 10.0f, + .errDeadzone = 0.0f, .errorDerivativeFloor = 0.0f, }; diff --git a/aimbots-src/src/robots/sentry/sentry_control.cpp b/aimbots-src/src/robots/sentry/sentry_control.cpp index 3d230680..d1eef407 100644 --- a/aimbots-src/src/robots/sentry/sentry_control.cpp +++ b/aimbots-src/src/robots/sentry/sentry_control.cpp @@ -260,7 +260,8 @@ HoldCommandMapping leftSwitchMid( HoldCommandMapping leftSwitchUp( drivers(), - {/*&chassisTokyoCommand,*/ &matchChassisControlCommand, &matchGimbalControlCommand, &matchFiringControlCommand + //{/*&chassisTokyoCommand,*/ &matchChassisControlCommand, &matchGimbalControlCommand, &matchFiringControlCommand + {&chassisTokyoCommand, &gimbalToggleAimCommand /*&gimbalChaseCommand*/}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP)); diff --git a/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp b/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp index f6d508bd..b55cc2e8 100644 --- a/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp +++ b/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp @@ -72,7 +72,7 @@ bool activeDisplay = false; bool activeDisplay2 = false; void SentryMatchChassisControlCommand::execute() { - if (refHelper->getGameStage() == GamePeriod::IN_GAME) { // Remove the true later + // if (refHelper->getGameStage() == GamePeriod::IN_GAME) { // Remove the true later stateDisplay = chassisState; activeDisplay = activeMovement; @@ -143,11 +143,11 @@ void SentryMatchChassisControlCommand::execute() { lastChassisState = chassisState; } - } else { - descheduleIfScheduled(this->comprisedCommandScheduler, &autoNavCommand, true); - descheduleIfScheduled(this->comprisedCommandScheduler, &autoNavTokyoCommand, true); - descheduleIfScheduled(this->comprisedCommandScheduler, &tokyoCommand, true); - } + // } else { + // descheduleIfScheduled(this->comprisedCommandScheduler, &autoNavCommand, true); + // descheduleIfScheduled(this->comprisedCommandScheduler, &autoNavTokyoCommand, true); + // descheduleIfScheduled(this->comprisedCommandScheduler, &tokyoCommand, true); + // } this->comprisedCommandScheduler.run(); } diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 54c5e9e9..14096bb5 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -273,46 +273,46 @@ void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, f void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { //defense mode (X) when robot is at rest - if(x==0 && y==0 && r==0){ - float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); + // if(x==0 && y==0 && r==0){ + // float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); - float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + // float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + // float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + // float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + // float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - target_left_front_yaw = ( M_PI/4); - optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); - left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; - targetRPMs[LF][1] = left_front_yaw % 8191; - left_front_yaw_db = targetRPMs[LF][1]; - - targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - target_right_front_yaw = (3*M_PI / 4); - optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); - right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - targetRPMs[RF][1] = right_front_yaw % 8191; - right_front_yaw_db = targetRPMs[RF][1]; - - targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - target_left_back_yaw = 3*M_PI / 4; - optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); - left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - targetRPMs[LB][1] = left_back_yaw % 8191; - left_back_yaw_db = targetRPMs[LB][1]; - - targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - target_right_back_yaw = M_PI / 4; - optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); - right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - targetRPMs[RB][1] = right_back_yaw % 8191; - right_back_yaw_db = targetRPMs[RB][1]; - }else{ + // targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); + // target_left_front_yaw = ( M_PI/4); + // optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); + // left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; + // targetRPMs[LF][1] = left_front_yaw % 8191; + // left_front_yaw_db = targetRPMs[LF][1]; + + // targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); + // target_right_front_yaw = (3*M_PI / 4); + // optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); + // right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; + // targetRPMs[RF][1] = right_front_yaw % 8191; + // right_front_yaw_db = targetRPMs[RF][1]; + + // targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); + // target_left_back_yaw = 3*M_PI / 4; + // optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); + // left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; + // targetRPMs[LB][1] = left_back_yaw % 8191; + // left_back_yaw_db = targetRPMs[LB][1]; + + // targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); + // target_right_back_yaw = M_PI / 4; + // optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); + // right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; + // targetRPMs[RB][1] = right_back_yaw % 8191; + // right_back_yaw_db = targetRPMs[RB][1]; + // }else{ float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); @@ -352,7 +352,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[RB][1] = right_back_yaw % 8191; right_back_yaw_db = targetRPMs[RB][1]; // wooo! just for commants - } + // } } #endif From 4869a3d7bb5649233d17a9467d0eecd4e768c89f Mon Sep 17 00:00:00 2001 From: zDagger Date: Mon, 30 Jun 2025 23:25:11 -0500 Subject: [PATCH 40/67] attempt at fixing wheel jerk --- .../constants/sentry_gimbal_constants.hpp | 2 +- .../subsystems/chassis/control/chassis.cpp | 104 ++++++++++-------- 2 files changed, 59 insertions(+), 47 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 8f56a80f..0852a27b 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -65,7 +65,7 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (5.0f / 17.0f); // for 2023 Se static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { .kp = 2'000.0f, .ki = 0.0f, - .kd = 12000.0f, + .kd = 8000.0f, .maxICumulative = 0.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 14096bb5..96159c31 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -255,6 +255,13 @@ float right_front_drive; float left_back_drive; float right_back_drive; +float prev_left_front_yaw; +float prev_right_front_yaw; +float prev_left_back_yaw; +float prev_right_back_yaw; + +bool lockWatch; + float ChassisSubsystem::yawToRad(float targetYaw, int offset){ return ((targetYaw-offset)/((180 / M_PI) / 360 * 8191)); } @@ -272,47 +279,8 @@ void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, f } void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { - //defense mode (X) when robot is at rest - // if(x==0 && y==0 && r==0){ - // float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); - - // float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - // float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - // float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - // float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - - // targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - // target_left_front_yaw = ( M_PI/4); - // optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); - // left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; - // targetRPMs[LF][1] = left_front_yaw % 8191; - // left_front_yaw_db = targetRPMs[LF][1]; - - // targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - // target_right_front_yaw = (3*M_PI / 4); - // optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); - // right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - // targetRPMs[RF][1] = right_front_yaw % 8191; - // right_front_yaw_db = targetRPMs[RF][1]; - - // targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - // target_left_back_yaw = 3*M_PI / 4; - // optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); - // left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - // targetRPMs[LB][1] = left_back_yaw % 8191; - // left_back_yaw_db = targetRPMs[LB][1]; - - // targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - // target_right_back_yaw = M_PI / 4; - // optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); - // right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - // targetRPMs[RB][1] = right_back_yaw % 8191; - // right_back_yaw_db = targetRPMs[RB][1]; - // }else{ + if(x < 0.1 && y < 0.1 && r<0.1){ + lockWatch = true; float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); @@ -322,7 +290,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - target_left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2); + target_left_front_yaw = prev_left_front_yaw; optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; targetRPMs[LF][1] = left_front_yaw % 8191; @@ -330,7 +298,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - target_right_front_yaw = (atan2f(c, b) + 3 * M_PI / 2); + target_right_front_yaw = prev_right_front_yaw; optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; targetRPMs[RF][1] = right_front_yaw % 8191; @@ -338,7 +306,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - target_left_back_yaw = atan2f(d, a) + 3 * M_PI / 2; + target_left_back_yaw = prev_left_back_yaw; optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; targetRPMs[LB][1] = left_back_yaw % 8191; @@ -346,13 +314,57 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - target_right_back_yaw = atan2f(c, a) + 3 * M_PI / 2; + target_right_back_yaw = prev_right_back_yaw; optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; targetRPMs[RB][1] = right_back_yaw % 8191; right_back_yaw_db = targetRPMs[RB][1]; + }else{ + lockWatch = false; + float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); + + float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + + targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + left_front_yaw_actual = yawToRad(motors[LF][1]->getEncoderWrapped(), LEFT_FRONT_YAW_OFFSET); + target_left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2); + prev_left_front_yaw = left_front_yaw_actual; + optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, left_front_yaw_actual); + left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; + targetRPMs[LF][1] = left_front_yaw % 8191; + left_front_yaw_db = targetRPMs[LF][1]; + + targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + right_front_yaw_actual = yawToRad(motors[RF][1]->getEncoderWrapped(), RIGHT_FRONT_YAW_OFFSET); + target_right_front_yaw = (atan2f(c, b) + 3 * M_PI / 2); + prev_right_front_yaw = right_front_yaw_actual; + optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, right_front_yaw_actual); + right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; + targetRPMs[RF][1] = right_front_yaw % 8191; + right_front_yaw_db = targetRPMs[RF][1]; + + targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + left_back_yaw_actual = yawToRad(motors[LB][1]->getEncoderWrapped(),LEFT_BACK_YAW_OFFSET); + target_left_back_yaw = atan2f(d, a) + 3 * M_PI / 2; + prev_left_back_yaw = left_back_yaw_actual; + optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, left_back_yaw_actual); + left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; + targetRPMs[LB][1] = left_back_yaw % 8191; + left_back_yaw_db = targetRPMs[LB][1]; + + targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + right_back_yaw_actual = yawToRad(motors[RB][1]->getEncoderWrapped(),RIGHT_BACK_YAW_OFFSET); + target_right_back_yaw = atan2f(c, a) + 3 * M_PI / 2; + prev_right_back_yaw = right_back_yaw_actual; + optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,right_back_yaw_actual); + right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; + targetRPMs[RB][1] = right_back_yaw % 8191; + right_back_yaw_db = targetRPMs[RB][1]; // wooo! just for commants - // } + } } #endif From cbd3020b1795607ed0caaf8d8056543d452e8557 Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 1 Jul 2025 00:13:10 -0500 Subject: [PATCH 41/67] added dune theme for sentry --- .../robots/sentry/constants/sentry_general_constant.hpp | 2 +- aimbots-src/src/utils/music/jukebox_player.cpp | 3 ++- aimbots-src/src/utils/music/sheetmusic_constants.hpp | 9 +++++++++ aimbots-src/src/utils/tools/common_types.hpp | 2 +- 4 files changed, 13 insertions(+), 3 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_general_constant.hpp b/aimbots-src/src/robots/sentry/constants/sentry_general_constant.hpp index 98377a90..f0aa9945 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_general_constant.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_general_constant.hpp @@ -2,7 +2,7 @@ #include "utils/tools/common_types.hpp" #include "utils/math/matrix_helpers.hpp" -static constexpr SongTitle STARTUP_SONG = SongTitle::WE_ARE_NUMBER_ONE; +static constexpr SongTitle STARTUP_SONG = SongTitle::DUNE; static Vector3f IMU_MOUNT_POSITION{0.0f, 0.0f, 0.0f}; diff --git a/aimbots-src/src/utils/music/jukebox_player.cpp b/aimbots-src/src/utils/music/jukebox_player.cpp index 056f278c..92d64751 100644 --- a/aimbots-src/src/utils/music/jukebox_player.cpp +++ b/aimbots-src/src/utils/music/jukebox_player.cpp @@ -13,7 +13,8 @@ static constexpr Song* songsList[] = { &mysterySong, &crabRaveSong, &legendOfZeldaSong, - &LG_WashSong}; + &LG_WashSong, + &Dune_Theme}; JukeboxPlayer::JukeboxPlayer(src::Drivers* drivers) : drivers(drivers), currentSongTitle(NONE) {} diff --git a/aimbots-src/src/utils/music/sheetmusic_constants.hpp b/aimbots-src/src/utils/music/sheetmusic_constants.hpp index 05197b3f..49cc68be 100644 --- a/aimbots-src/src/utils/music/sheetmusic_constants.hpp +++ b/aimbots-src/src/utils/music/sheetmusic_constants.hpp @@ -169,4 +169,13 @@ Song LG_WashSong = {LG_BPM, Q_N, {{Db6, QH_N}, {Gb6, E_N}, {F6, E_N}, {Eb6, E_ // https://musescore.com/user/35424120/scores/6208111 // Measures 1 - 8 +//Dune theme + +static constexpr uint32_t DUNE_BPM = 48; +Song Dune_Theme = {DUNE_BPM, Q_N, {{D5,Q_N}, {A5,Q_N}, {Bb5,E_N}, {D5,E_N}, {A5,E_N}, {Bb5,H_N}, + {C6,Q_N}, {A5,Q_N}, {Bb5,E_N}, {D5,E_N}, {A5,E_N}, {Bb5,H_N}, + {END, Q_N}}}; +//Based on: +//https://onlinesequencer.net/4735533 + } // namespace utils::Jukebox \ No newline at end of file diff --git a/aimbots-src/src/utils/tools/common_types.hpp b/aimbots-src/src/utils/tools/common_types.hpp index eec0820e..72164cd3 100644 --- a/aimbots-src/src/utils/tools/common_types.hpp +++ b/aimbots-src/src/utils/tools/common_types.hpp @@ -70,7 +70,7 @@ enum Dimensions { X = 0, Y = 1, Z = 2, TIME = 2 }; // :) enum LinearAxis : uint8_t { X_AXIS = 0, Y_AXIS = 1, Z_AXIS = 2 }; -enum SongTitle : uint8_t { NONE = 0, PACMAN, WE_ARE_NUMBER_ONE, CHAINSAW_MAN, MYSTERY, CRAB_RAVE, ZELDA, LG_WASH }; +enum SongTitle : uint8_t { NONE = 0, PACMAN, WE_ARE_NUMBER_ONE, CHAINSAW_MAN, MYSTERY, CRAB_RAVE, ZELDA, LG_WASH, DUNE }; enum FeederGroup : uint8_t { ALL = 0, // Command goes to all feeder motors From cd2f9ddcc6c12c18e2e066e3658b29afc4a7ee7c Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 10 Jul 2025 18:42:14 -0500 Subject: [PATCH 42/67] re added sentry match points --- .../sentry_match_chassis_control_command.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp b/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp index b55cc2e8..b14468b3 100644 --- a/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp +++ b/aimbots-src/src/subsystems/chassis/complex_commands/sentry_match_chassis_control_command.cpp @@ -40,10 +40,10 @@ SentryMatchChassisControlCommand::SentryMatchChassisControlCommand( void SentryMatchChassisControlCommand::initialize() { // This is a necessary step, do not question it. - // updateChassisState(ChassisMatchStates::GUARDING); - // updateChassisState(ChassisMatchStates::START); + updateChassisState(ChassisMatchStates::GUARDING); + updateChassisState(ChassisMatchStates::START); chassisState = ChassisMatchStates::GUARDING; - // chassisState = ChassisMatchStates::START; + chassisState = ChassisMatchStates::START; updateChassisState(ChassisMatchStates::START); delayTimer.restart(500); lockoutTimer.restart(0); @@ -72,7 +72,7 @@ bool activeDisplay = false; bool activeDisplay2 = false; void SentryMatchChassisControlCommand::execute() { - // if (refHelper->getGameStage() == GamePeriod::IN_GAME) { // Remove the true later + if (refHelper->getGameStage() == GamePeriod::IN_GAME) { // Remove the true later stateDisplay = chassisState; activeDisplay = activeMovement; @@ -143,11 +143,11 @@ void SentryMatchChassisControlCommand::execute() { lastChassisState = chassisState; } - // } else { - // descheduleIfScheduled(this->comprisedCommandScheduler, &autoNavCommand, true); - // descheduleIfScheduled(this->comprisedCommandScheduler, &autoNavTokyoCommand, true); - // descheduleIfScheduled(this->comprisedCommandScheduler, &tokyoCommand, true); - // } + } else { + descheduleIfScheduled(this->comprisedCommandScheduler, &autoNavCommand, true); + descheduleIfScheduled(this->comprisedCommandScheduler, &autoNavTokyoCommand, true); + descheduleIfScheduled(this->comprisedCommandScheduler, &tokyoCommand, true); + } this->comprisedCommandScheduler.run(); } From 0ca922bb2a8b99615b4fa042679def4a1c9a8130 Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 14 Aug 2025 11:52:24 -0500 Subject: [PATCH 43/67] fixed 2025 yaw offset --- .../src/robots/standard/constants/standard_gimbal_constants.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp index 02fd73cf..f295e84e 100644 --- a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp @@ -26,7 +26,7 @@ static const std::array PITCH_MOTOR_DIRECTIONS = {false #elif defined(TARGET_STANDARD_2025) /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot cassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(-211.2f))}; +static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(260.11f))}; static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(298.25f))}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-14.5f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(17.5f); From 80f629aaff1898a09b87e341d70bebb23df197be Mon Sep 17 00:00:00 2001 From: zDagger Date: Thu, 14 Aug 2025 15:37:56 -0500 Subject: [PATCH 44/67] also updated stadard offsets here --- .../src/robots/standard/constants/standard_gimbal_constants.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp index 02fd73cf..f295e84e 100644 --- a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp @@ -26,7 +26,7 @@ static const std::array PITCH_MOTOR_DIRECTIONS = {false #elif defined(TARGET_STANDARD_2025) /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot cassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(-211.2f))}; +static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(260.11f))}; static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(298.25f))}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-14.5f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(17.5f); From cba0e7f52f418acf604db6d5910a6339e96273e4 Mon Sep 17 00:00:00 2001 From: zDagger Date: Tue, 9 Sep 2025 17:44:46 -0500 Subject: [PATCH 45/67] removed temporary imu uart communicator from comp --- .../INA260/INA260_communicator.cpp | 134 ------------------ .../INA260/INA260_communicator.hpp | 53 ------- .../communicators/INA260/INA260_protocol.hpp | 20 --- 3 files changed, 207 deletions(-) delete mode 100644 aimbots-src/src/communicators/INA260/INA260_communicator.cpp delete mode 100644 aimbots-src/src/communicators/INA260/INA260_communicator.hpp delete mode 100644 aimbots-src/src/communicators/INA260/INA260_protocol.hpp diff --git a/aimbots-src/src/communicators/INA260/INA260_communicator.cpp b/aimbots-src/src/communicators/INA260/INA260_communicator.cpp deleted file mode 100644 index f3095e77..00000000 --- a/aimbots-src/src/communicators/INA260/INA260_communicator.cpp +++ /dev/null @@ -1,134 +0,0 @@ - -#ifndef ALL_SENTRIES - -#include "INA260_communicator.hpp" - -#include - -#define READ(data, length) drivers->uart.read(INA260_UART_PORT, data, length) -#define WRITE(data, length) drivers->uart.write(INA260_UART_PORT, data, length) - -namespace src::Informants::INA260 { - -float powerDisplay = 0.0f; -float voltage_mVDisplay = 0.0f; -float current_mADisplay = 0.0f; - -INA260Communicator::INA260Communicator(src::Drivers* drivers) - : drivers(drivers), - currentSerialState(INA260CommunicatorSerialState::SearchingForMagic), - nextByteIndex(0), - INA260OfflineTimeout(), - lastMessage() -{} - -void INA260Communicator::initialize() { - INA260OfflineTimeout.restart(INA260_OFFLINE_TIMEOUT_MILLISECONDS); - drivers->uart.init(); -} - -uint8_t displayBuffer[INA260_MESSAGE_SIZE]; -int displayBufIndex = 0; - -int lastMsgTimeDisplay = 0; -int msBetweenLastMessageDisplay = 0; - -/** - * @brief Rewrote jetson communictor to use UART to - * recieve data from esp32 that sends INA260 data - * Last mintue comp decision - */ -alignas(INA260Message) uint8_t rawSerialDisplay[sizeof(INA260Message)]; - -void INA260Communicator::updateSerial() { - uint32_t currTime = tap::arch::clock::getTimeMilliseconds(); - - size_t bytesRead = READ(&rawSerialBuffer[nextByteIndex], 1); // attempts to pull one byte from the buffer - if (bytesRead != 1) return; - - INA260OfflineTimeout.restart(INA260_OFFLINE_TIMEOUT_MILLISECONDS); - - displayBuffer[displayBufIndex] = rawSerialBuffer[0]; // copy byte to display buffer - displayBufIndex = (displayBufIndex + 1) % INA260_MESSAGE_SIZE; // increment display index and wrap around if necessary - - switch (currentSerialState) { - // ...looking for message start... - case INA260CommunicatorSerialState::SearchingForMagic: { - // Check if the byte we just read is the byte we expected in the magic number. - if (rawSerialBuffer[nextByteIndex] == ((INA260_MESSAGE_MAGIC >> (8 * nextByteIndex)) & 0xff)) { - nextByteIndex++; - } else { - nextByteIndex = 0; // if not, reset the index and start over. - } - - // Wait until we've reached the end of the magic number. If any of the bytes in the magic number weren't a match, - // we wouldn't have gotten this far. - if (nextByteIndex == sizeof(decltype(INA260_MESSAGE_MAGIC))) { - currentSerialState = INA260CommunicatorSerialState::AssemblingMessage; - } - break; - } - // ...found message start, assemble message... - case INA260CommunicatorSerialState::AssemblingMessage: { - nextByteIndex++; - - // Increment the byte index until we reach the expected end of a message, then parse the message. - if (nextByteIndex == INA260_MESSAGE_SIZE) { - // Reinterpret the received bytes into a JetsonMessage - lastMessage = *reinterpret_cast(&rawSerialBuffer); - powerDisplay = lastMessage.power; - voltage_mVDisplay = lastMessage.voltage_mV; - current_mADisplay = lastMessage.current_mA; - - // Update last message time and time between last message and now. - if (lastMsgTimeDisplay == 0) { - lastMsgTimeDisplay = tap::arch::clock::getTimeMilliseconds(); - } else { - msBetweenLastMessageDisplay = - currTime - lastMsgTimeDisplay; // Should be pretty close to the message send rate. - lastMsgTimeDisplay = currTime; - } - - // As we've received a full message, reset the byte index and go back to searching for the magic number. - nextByteIndex = 0; - currentSerialState = INA260CommunicatorSerialState::SearchingForMagic; - } else { - rawSerialDisplay[nextByteIndex] = rawSerialBuffer[nextByteIndex]; - } - - break; - } - } - -} - -INA260Message const& INA260Communicator::getLastValidMessage() const { - return lastMessage; -} - -// float INA260Communicator::twoBytesToFloat(uint16_t bytes) const{ -// uint32_t empty = 0x00000000; -// uint32_t firstSignBit = empty; -// uint32_t mantissaBits = empty; -// uint32_t exponentBits = empty; -// uint32_t floatBits = empty; - -// firstSignBit |= bytes & 0x8000; -// firstSignBit = firstSignBit << 16; // copy first bit - -// exponentBits |= bytes & 0x7C00; -// exponentBits = exponentBits >> 10; -// int32_t exponent = ((int32_t)exponentBits) - 15; // subtract half-precision bias -// exponent += 127; // add single percision bias -// exponentBits = ((uint32_t)exponent) << 23; // move to position - -// mantissaBits |= bytes & 0x03FF; -// mantissaBits = mantissaBits << 13; // start mantissa bits on 9th bit - -// floatBits = firstSignBit | exponentBits | mantissaBits; - -// return * (float *) &floatBits; // Carmack style 😎 -// } - -} // namespace src::Informants::INA260 -#endif // ALL_SENTRIES \ No newline at end of file diff --git a/aimbots-src/src/communicators/INA260/INA260_communicator.hpp b/aimbots-src/src/communicators/INA260/INA260_communicator.hpp deleted file mode 100644 index b65fd95b..00000000 --- a/aimbots-src/src/communicators/INA260/INA260_communicator.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#ifndef ALL_SENTRIES - -#include -#include -#include - - -#include "INA260_protocol.hpp" - -namespace src { -class Drivers; -} - -namespace src::Informants::INA260 { - -enum class INA260CommunicatorSerialState : uint8_t { - SearchingForMagic = 0, - AssemblingMessage, -}; - -class INA260Communicator{ -public: - INA260Communicator(src::Drivers* drivers); - DISALLOW_COPY_AND_ASSIGN(INA260Communicator); - ~INA260Communicator() = default; - - void initialize(); - void updateSerial(); - - inline bool isINA260Online() const { return !INA260OfflineTimeout.isExpired(); } - - INA260Message const& getLastValidMessage() const; - -private: - src::Drivers* drivers; - - alignas(INA260Message) uint8_t rawSerialBuffer[sizeof(INA260Message)]; - - INA260CommunicatorSerialState currentSerialState; - size_t nextByteIndex; - - tap::arch::MilliTimeout INA260OfflineTimeout; - - static constexpr uint32_t INA260_BAUD_RATE = 115200; - static constexpr uint16_t INA260_OFFLINE_TIMEOUT_MILLISECONDS = 2000; - static constexpr UartPort INA260_UART_PORT = UartPort::Uart1; - - INA260Message lastMessage; -}; -} // namespace src::Informants::INA260 -#endif // ALL_SENTRIES \ No newline at end of file diff --git a/aimbots-src/src/communicators/INA260/INA260_protocol.hpp b/aimbots-src/src/communicators/INA260/INA260_protocol.hpp deleted file mode 100644 index 986f2550..00000000 --- a/aimbots-src/src/communicators/INA260/INA260_protocol.hpp +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#ifndef ALL_SENTRIES - -namespace src::Informants::INA260 { - -static constexpr uint8_t INA260_MESSAGE_MAGIC = 'c'; - -struct INA260Message { - uint8_t magic; - float power; - float voltage_mV; - float current_mA; -} __attribute__((packed)); - -static_assert(sizeof(INA260Message) == 13, "INA260Message is not the correct size"); - -static constexpr size_t INA260_MESSAGE_SIZE = sizeof(INA260Message); -} // namespace src::Informants::INA260 -#endif // ALL_SENTRIES \ No newline at end of file From d8f7ab2cc2ba754a07adef15a9e19c75b420764d Mon Sep 17 00:00:00 2001 From: Baconsizzle1738 Date: Tue, 9 Sep 2025 19:11:08 -0500 Subject: [PATCH 46/67] transfer progress from last week, compiles --- .../kinematics/kinematic_informant.cpp | 35 +++++++++++++++++++ .../kinematics/kinematic_informant.hpp | 11 ++++++ 2 files changed, 46 insertions(+) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index d52bacb7..5b5b573e 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -120,6 +120,41 @@ float KinematicInformant::getIMULinearAcceleration(LinearAxis axis) { // Gets I Vector3f chassisAnglesConvertedDisplay; Vector3f IMUAnglesDisplay; + + +void KinematicInformant::updateGimbalIMUAngles() { + Vector3f IMUAngles = getLocalIMUAngles(); + Vector3f IMUAngularVelocities = getIMUAngularVelocities(); + + IMUAnglesDisplay = IMUAngles; + + // Gets chassis angles + Vector3f gimbalAngles = + drivers->kinematicInformant.getRobotFrames() + .getFrame(Transformers::FrameType::GIMBAL_FRAME) + .getPointInFrame( + drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::GIMBAL_FRAME), + IMUAngles); + + chassisAnglesConvertedDisplay = gimbalAngles; + + Vector3f gimbalAngularVelocities = + drivers->kinematicInformant.getRobotFrames() + .getFrame(Transformers::FrameType::GIMBAL_IMU_FRAME) + .getPointInFrame( + drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::GIMBAL_FRAME), + IMUAngularVelocities); + + gimbalAngularState[X_AXIS].updateFromPosition(gimbalAngles[X_AXIS]); + gimbalAngularState[Y_AXIS].updateFromPosition(gimbalAngles[Y_AXIS]); + gimbalAngularState[Z_AXIS].updateFromPosition(gimbalAngles[Z_AXIS]); + + gimbalAngularState[X_AXIS].updateFromVelocity(gimbalAngularVelocities[X_AXIS], false); + gimbalAngularState[Y_AXIS].updateFromVelocity(gimbalAngularVelocities[Y_AXIS], false); + gimbalAngularState[Z_AXIS].updateFromVelocity(gimbalAngularVelocities[Z_AXIS], false); +} + + void KinematicInformant::updateChassisIMUAngles() { Vector3f IMUAngles = getLocalIMUAngles(); Vector3f IMUAngularVelocities = getIMUAngularVelocities(); diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.hpp b/aimbots-src/src/informants/kinematics/kinematic_informant.hpp index fe0a374c..cd88ec9b 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.hpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.hpp @@ -75,6 +75,8 @@ class KinematicInformant { void updateIMUKinematicStateVector(); + void updateGimbalIMUAngles(); + void updateChassisIMUAngles(); // Returns angle in rad or deg @@ -153,6 +155,10 @@ class KinematicInformant { KinematicStateVector chassisAngularYState; KinematicStateVector chassisAngularZState; + KinematicStateVector gimbalAngularXState; + KinematicStateVector gimbalAngularYState; + KinematicStateVector gimbalAngularZState; + KinematicStateVector turretIMULinearXState; KinematicStateVector turretIMULinearYState; KinematicStateVector turretIMULinearZState; @@ -177,6 +183,11 @@ class KinematicInformant { chassisAngularYState, chassisAngularZState}; + modm::Vector gimbalAngularState = { + gimbalAngularXState, + gimbalAngularYState, + gimbalAngularZState}; + modm::Vector turretIMULinearState = { turretIMULinearXState, turretIMULinearYState, From b0ee8334e89b3b72bab319a71bcb8bae64cc0871 Mon Sep 17 00:00:00 2001 From: Baconsizzle1738 Date: Tue, 9 Sep 2025 20:46:44 -0500 Subject: [PATCH 47/67] compiles yippie :D (pls test soon) --- .../kinematics/kinematic_informant.cpp | 42 ++++++++++++++++--- .../kinematics/kinematic_informant.hpp | 22 +++++++++- .../constants/sentry_chassis_constants.hpp | 2 + 3 files changed, 60 insertions(+), 6 deletions(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index 5b5b573e..b6471f86 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -128,7 +128,6 @@ void KinematicInformant::updateGimbalIMUAngles() { IMUAnglesDisplay = IMUAngles; - // Gets chassis angles Vector3f gimbalAngles = drivers->kinematicInformant.getRobotFrames() .getFrame(Transformers::FrameType::GIMBAL_FRAME) @@ -189,19 +188,19 @@ void KinematicInformant::updateChassisIMUAngles() { float KinematicInformant::getChassisIMUAngle(AngularAxis axis, AngleUnit unit) { float angle = chassisAngularState[axis].getPosition(); - + return unit == AngleUnit::Radians ? angle : modm::toDegree(angle); +} +float KinematicInformant::getGimbalIMUAngle(AngularAxis axis, AngleUnit unit) { + float angle = gimbalAngularState[axis].getPosition(); return unit == AngleUnit::Radians ? angle : modm::toDegree(angle); } float KinematicInformant::getChassisIMUAngularVelocity(AngularAxis axis, AngleUnit unit) { float angularVelocity = chassisAngularState[axis].getVelocity(); - return unit == AngleUnit::Radians ? angularVelocity : modm::toDegree(angularVelocity); } - float KinematicInformant::getIMUAngularAcceleration(AngularAxis axis, AngleUnit unit) { float angularAcceleration = imuAngularState[axis].getAcceleration(); - return unit == AngleUnit::Radians ? angularAcceleration : modm::toDegree(angularAcceleration); } @@ -280,6 +279,30 @@ void KinematicInformant::updateChassisAcceleration() { chassisLinearState[Z_AXIS].updateFromAcceleration(linearChassisAcceleration.getZ()); } +void KinematicInformant::updateGimbalAcceleration() { + // gimbalAngleXDisplay = gimbalAngularState[X_AXIS].getPosition(); + // gimbalAngleYDisplay = gimbalAngularState[Y_AXIS].getPosition(); + // gimbalAngleZDisplay = gimbalAngularState[Z_AXIS].getPosition(); + + Vector3f linearIMUAcceleration = removeFalseAcceleration(imuLinearState, imuAngularState, IMU_MOUNT_POSITION); + + Vector3f linearGimbalAcceleration = + drivers->kinematicInformant.getRobotFrames() + .getFrame(Transformers::FrameType::GIMBAL_IMU_FRAME) + .getPointInFrame( + drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::GIMBAL_FRAME), + linearIMUAcceleration); + + linearIMUAccelerationDisplay = linearIMUAcceleration; + linearIMUAccelerationXDisplay = linearGimbalAcceleration.getX(); + linearIMUAccelerationYDisplay = linearGimbalAcceleration.getY(); + linearIMUAccelerationZDisplay = linearGimbalAcceleration.getZ(); + + gimbalLinearState[X_AXIS].updateFromAcceleration(linearGimbalAcceleration.getX()); + gimbalLinearState[Y_AXIS].updateFromAcceleration(linearGimbalAcceleration.getY()); + gimbalLinearState[Z_AXIS].updateFromAcceleration(linearGimbalAcceleration.getZ()); +} + float chassisYawAngleDisplay = 0.0f; tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat() { float currGimbalAngle = gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians); @@ -356,8 +379,17 @@ void KinematicInformant::updateRobotFrames() { #ifndef TARGET_TURRET // Update Chassis Stuff after IMU STUFF + #ifdef CHASSIS_IMU // old shi updateChassisIMUAngles(); + + updateChassisAcceleration(); + #endif + + #ifdef TURRET_IMU // Gimbal IMU migration + updateGimbalIMUAngles(); + updateChassisAcceleration(); + #endif chassisIMUHistoryBuffer.prependOverwrite( {getChassisIMUAngle(PITCH_AXIS, AngleUnit::Radians), diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.hpp b/aimbots-src/src/informants/kinematics/kinematic_informant.hpp index cd88ec9b..9abaeb26 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.hpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.hpp @@ -82,13 +82,18 @@ class KinematicInformant { // Returns angle in rad or deg float getChassisIMUAngle(AngularAxis axis, AngleUnit unit); + float getGimbalIMUAngle(AngularAxis axis, AngleUnit unit); + // Returns angular velocity in rad/s or deg/s float getChassisIMUAngularVelocity(AngularAxis axis, AngleUnit unit); + float getGimbalIMUAngularVelocity(AngularAxis axis, AngleUnit unit); + Vector3f getIMUAngularAccelerations(); float getIMUAngularAcceleration(AngularAxis axis, AngleUnit unit); // Returns lnothing!!! void updateChassisAcceleration(); + void updateGimbalAcceleration(); // This updates more than just the robot frames, so will also update turret frames void updateRobotFrames(); @@ -110,7 +115,11 @@ class KinematicInformant { int index = std::min(time_ms / KINEMATIC_REFRESH_RATE, CHASSIS_IMU_BUFFER_SIZE - 1); return chassisIMUHistoryBuffer[index]; } - + // inline Vector3f getGimbalIMUOrientationAtTime(uint32_t time_ms) { + // // assume 2 ms delay between gimbal updates + // int index = std::min(time_ms / KINEMATIC_REFRESH_RATE, GIMBAL_IMU_BUFFER_SIZE - 1); + // return chassisIMUHistoryBuffer[index]; + // } inline std::pair& getGimbalFieldOrientation(int index) { return gimbalFieldOrientationBuffer[index]; } // put in your time, we get the closest orientation entry at that time. @@ -151,6 +160,10 @@ class KinematicInformant { KinematicStateVector chassisLinearYState; KinematicStateVector chassisLinearZState; + KinematicStateVector gimbalLinearXState; + KinematicStateVector gimbalLinearYState; + KinematicStateVector gimbalLinearZState; + KinematicStateVector chassisAngularXState; KinematicStateVector chassisAngularYState; KinematicStateVector chassisAngularZState; @@ -178,6 +191,12 @@ class KinematicInformant { chassisLinearXState, chassisLinearYState, chassisLinearZState}; + + modm::Vector gimbalLinearState = { + gimbalLinearXState, + gimbalLinearYState, + gimbalLinearZState}; + modm::Vector chassisAngularState = { chassisAngularXState, chassisAngularYState, @@ -198,6 +217,7 @@ class KinematicInformant { turretIMUAngularZState}; src::Informants::Odometry::ChassisKFOdometry chassisKFOdometry; + // src::Informants::Odometry::GimbalKFOdometry gimbalKFOdometry; }; } // namespace src::Informants diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index a6a50ae2..ee035ed7 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -3,6 +3,8 @@ #include "utils/tools/common_types.hpp" #define CHASSIS_COMPATIBLE +// #define CHASSIS_IMU // old shi +#define TURRET_IMU // IMU migration and whatnot #if defined(TARGET_SENTRY_SWERVE) #define SWERVE From 2486a6c8927df5155115fdf0d5cd5db6018a9ad6 Mon Sep 17 00:00:00 2001 From: Baconsizzle1738 Date: Tue, 16 Sep 2025 20:23:21 -0500 Subject: [PATCH 48/67] numbers seeem reasonable --- .../informants/kinematics/kinematic_informant.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index b6471f86..76f73b88 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -119,14 +119,17 @@ float KinematicInformant::getIMULinearAcceleration(LinearAxis axis) { // Gets I } Vector3f chassisAnglesConvertedDisplay; -Vector3f IMUAnglesDisplay; - +float IMUAnglesDisplayX; +float IMUAnglesDisplayY; +float IMUAnglesDisplayZ; void KinematicInformant::updateGimbalIMUAngles() { Vector3f IMUAngles = getLocalIMUAngles(); Vector3f IMUAngularVelocities = getIMUAngularVelocities(); - IMUAnglesDisplay = IMUAngles; + IMUAnglesDisplayX = IMUAngles[0]; + IMUAnglesDisplayY = IMUAngles[1]; + IMUAnglesDisplayZ = IMUAngles[2]; Vector3f gimbalAngles = drivers->kinematicInformant.getRobotFrames() @@ -153,12 +156,12 @@ void KinematicInformant::updateGimbalIMUAngles() { gimbalAngularState[Z_AXIS].updateFromVelocity(gimbalAngularVelocities[Z_AXIS], false); } - +float chassisXDisplay = -100000.0f; void KinematicInformant::updateChassisIMUAngles() { Vector3f IMUAngles = getLocalIMUAngles(); Vector3f IMUAngularVelocities = getIMUAngularVelocities(); - IMUAnglesDisplay = IMUAngles; + chassisXDisplay = IMUAngles[0]; // Gets chassis angles Vector3f chassisAngles = From 6949efc98ef250182866ea310539fe36bfdd7b10 Mon Sep 17 00:00:00 2001 From: Baconsizzle1738 Date: Tue, 16 Sep 2025 21:00:58 -0500 Subject: [PATCH 49/67] chassis display WONT FKN GRAPH WTF --- .../subsystems/chassis/control/chassis.cpp | 94 +++++++++++-------- 1 file changed, 53 insertions(+), 41 deletions(-) diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 96159c31..e9430f57 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -200,6 +200,11 @@ void ChassisSubsystem::setDesiredOutput(WheelIndex WheelIdx, MotorOnWheelIndex M float xInputDisplay = 0.0f; float yInputDisplay = 0.0f; float rInputDisplay = 0.0f; +int chassisRotateDisplay; +int leftBackRotationRatioDisplay; +int xDisplay; +int yDisplay; + void ChassisSubsystem::calculateHolonomic(float x, float y, float r, float maxWheelSpeed) { xInputDisplay = x; yInputDisplay = y; @@ -225,6 +230,10 @@ void ChassisSubsystem::calculateHolonomic(float x, float y, float r, float maxWh limitVal(-x - y + chassisRotateTranslated * rightBackRotationRatio, -maxWheelSpeed, maxWheelSpeed); desiredRotation = r; + chassisRotateDisplay = chassisRotateTranslated; + leftBackRotationRatioDisplay = leftBackRotationRatio; + xDisplay = x; + yDisplay = y; } #endif @@ -278,48 +287,50 @@ void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, f } } -void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { - if(x < 0.1 && y < 0.1 && r<0.1){ - lockWatch = true; - float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); +float wheelRPMDisplay = 0.0f; - float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); +void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { + // if(x < 0.1 && y < 0.1 && r<0.1){ + // lockWatch = true; + // float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); + + // float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + // float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + // float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + // float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - target_left_front_yaw = prev_left_front_yaw; - optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); - left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; - targetRPMs[LF][1] = left_front_yaw % 8191; - left_front_yaw_db = targetRPMs[LF][1]; - - targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - target_right_front_yaw = prev_right_front_yaw; - optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); - right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - targetRPMs[RF][1] = right_front_yaw % 8191; - right_front_yaw_db = targetRPMs[RF][1]; - - targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - target_left_back_yaw = prev_left_back_yaw; - optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); - left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - targetRPMs[LB][1] = left_back_yaw % 8191; - left_back_yaw_db = targetRPMs[LB][1]; - - targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - target_right_back_yaw = prev_right_back_yaw; - optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); - right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - targetRPMs[RB][1] = right_back_yaw % 8191; - right_back_yaw_db = targetRPMs[RB][1]; - }else{ + // targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); + // target_left_front_yaw = prev_left_front_yaw; + // optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); + // left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; + // targetRPMs[LF][1] = left_front_yaw % 8191; + // left_front_yaw_db = targetRPMs[LF][1]; + + // targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); + // target_right_front_yaw = prev_right_front_yaw; + // optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); + // right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; + // targetRPMs[RF][1] = right_front_yaw % 8191; + // right_front_yaw_db = targetRPMs[RF][1]; + + // targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); + // target_left_back_yaw = prev_left_back_yaw; + // optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); + // left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; + // targetRPMs[LB][1] = left_back_yaw % 8191; + // left_back_yaw_db = targetRPMs[LB][1]; + + // targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + // right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); + // target_right_back_yaw = prev_right_back_yaw; + // optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); + // right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; + // targetRPMs[RB][1] = right_back_yaw % 8191; + // right_back_yaw_db = targetRPMs[RB][1]; + // }else{ lockWatch = false; float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); @@ -364,7 +375,8 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[RB][1] = right_back_yaw % 8191; right_back_yaw_db = targetRPMs[RB][1]; // wooo! just for commants - } + // } + wheelRPMDisplay = targetRPMs[LB][0]; } #endif From 5d5465cecdf95355ec181bdf7ee659c202d97baf Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Fri, 19 Sep 2025 17:37:36 -0500 Subject: [PATCH 50/67] Sentry drives again! --- .../informants/imu/calibrate_imu_command.cpp | 6 +- .../kinematics/kinematic_informant.cpp | 355 +++++++++++++++--- .../kinematics/kinematic_informant.hpp | 8 + .../informants/kinematics/robot_frames.cpp | 16 +- .../constants/sentry_gimbal_constants.hpp | 2 +- .../gimbal_field_relative_control_command.cpp | 2 + .../utils/tools/robot_specific_defines.hpp | 1 + 7 files changed, 334 insertions(+), 56 deletions(-) diff --git a/aimbots-src/src/informants/imu/calibrate_imu_command.cpp b/aimbots-src/src/informants/imu/calibrate_imu_command.cpp index 79f09379..8629ef24 100644 --- a/aimbots-src/src/informants/imu/calibrate_imu_command.cpp +++ b/aimbots-src/src/informants/imu/calibrate_imu_command.cpp @@ -59,9 +59,9 @@ void IMUCalibrateCommand::execute() { if (gimbalController.allOnlineYawControllersSettled(modm::toRadian(1.0f), 1000) && gimbalController.allOnlinePitchControllersSettled(modm::toRadian(1.0f), 1000)) { -#ifdef TURRET_IMU - drivers->turretCommunicator.requestTurretIMUCalibrate(); -#endif +// #ifdef TURRET_IMU +// drivers->turretCommunicator.requestTurretIMUCalibrate(); +// #endif #ifndef TARGET_TURRET drivers->kinematicInformant.recalibrateIMU( {CIMU_CALIBRATION_EULER_X, CIMU_CALIBRATION_EULER_Y, CIMU_CALIBRATION_EULER_Z}); diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index 76f73b88..6a91c25a 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -118,50 +118,17 @@ float KinematicInformant::getIMULinearAcceleration(LinearAxis axis) { // Gets I return 0; } -Vector3f chassisAnglesConvertedDisplay; -float IMUAnglesDisplayX; -float IMUAnglesDisplayY; -float IMUAnglesDisplayZ; - -void KinematicInformant::updateGimbalIMUAngles() { - Vector3f IMUAngles = getLocalIMUAngles(); - Vector3f IMUAngularVelocities = getIMUAngularVelocities(); - - IMUAnglesDisplayX = IMUAngles[0]; - IMUAnglesDisplayY = IMUAngles[1]; - IMUAnglesDisplayZ = IMUAngles[2]; - - Vector3f gimbalAngles = - drivers->kinematicInformant.getRobotFrames() - .getFrame(Transformers::FrameType::GIMBAL_FRAME) - .getPointInFrame( - drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::GIMBAL_FRAME), - IMUAngles); - - chassisAnglesConvertedDisplay = gimbalAngles; - - Vector3f gimbalAngularVelocities = - drivers->kinematicInformant.getRobotFrames() - .getFrame(Transformers::FrameType::GIMBAL_IMU_FRAME) - .getPointInFrame( - drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::GIMBAL_FRAME), - IMUAngularVelocities); +#ifdef CHASSIS_IMU - gimbalAngularState[X_AXIS].updateFromPosition(gimbalAngles[X_AXIS]); - gimbalAngularState[Y_AXIS].updateFromPosition(gimbalAngles[Y_AXIS]); - gimbalAngularState[Z_AXIS].updateFromPosition(gimbalAngles[Z_AXIS]); +Vector3f chassisAnglesConvertedDisplay; +Vector3f IMUAnglesDisplay; - gimbalAngularState[X_AXIS].updateFromVelocity(gimbalAngularVelocities[X_AXIS], false); - gimbalAngularState[Y_AXIS].updateFromVelocity(gimbalAngularVelocities[Y_AXIS], false); - gimbalAngularState[Z_AXIS].updateFromVelocity(gimbalAngularVelocities[Z_AXIS], false); -} -float chassisXDisplay = -100000.0f; void KinematicInformant::updateChassisIMUAngles() { Vector3f IMUAngles = getLocalIMUAngles(); Vector3f IMUAngularVelocities = getIMUAngularVelocities(); - chassisXDisplay = IMUAngles[0]; + IMUAnglesDisplay = IMUAngles; // Gets chassis angles Vector3f chassisAngles = @@ -384,15 +351,314 @@ void KinematicInformant::updateRobotFrames() { // Update Chassis Stuff after IMU STUFF #ifdef CHASSIS_IMU // old shi updateChassisIMUAngles(); - updateChassisAcceleration(); - #endif - - #ifdef TURRET_IMU // Gimbal IMU migration - updateGimbalIMUAngles(); +#endif + + chassisIMUHistoryBuffer.prependOverwrite( + {getChassisIMUAngle(PITCH_AXIS, AngleUnit::Radians), + getChassisIMUAngle(ROLL_AXIS, AngleUnit::Radians), + getChassisIMUAngle(YAW_AXIS, AngleUnit::Radians)}); + + chassisKFOdometry.update( + getChassisIMUAngle(YAW_AXIS, AngleUnit::Radians), + chassisLinearState[X_AXIS].getAcceleration(), + chassisLinearState[Y_AXIS].getAcceleration()); + + // update gimbal orientation buffer + std::pair orientation; + orientation.first = getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat().getWrappedValue(); + orientation.second = getCurrentFieldRelativeGimbalPitchAngleAsWrappedFloat().getWrappedValue(); + + gimbalFieldOrientationBuffer.prependOverwrite(orientation); + + modm::Location2D robotLocation = chassisKFOdometry.getCurrentLocation2D(); + + robotLocationXDisplay = robotLocation.getX(); + robotLocationYDisplay = robotLocation.getY(); + + robotFrames.updateFrames( + gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians), + gimbalSubsystem->getCurrentPitchAxisAngle(AngleUnit::Radians), + getChassisIMUAngle(YAW_AXIS, AngleUnit::Radians) + CHASSIS_START_ANGLE_WORLD, + {robotLocation.getX(), robotLocation.getY(), 0}, + AngleUnit::Radians); + + turretFrames.updateFrames( + getChassisIMUAngle(YAW_AXIS, AngleUnit::Radians), + getChassisIMUAngle(PITCH_AXIS, AngleUnit::Radians), + getChassisIMUAngle(ROLL_AXIS, AngleUnit::Radians), + AngleUnit::Radians); + + robotLocationDisplay = robotLocation; +#endif +} + +float frameDelayDisplay = 0; + +void KinematicInformant::mirrorPastRobotFrame(uint32_t frameDelay_ms) { + frameDelayDisplay = frameDelay_ms; + + std::pair gimbalAngles = gimbalSubsystem->getGimbalOrientationAtTime(frameDelay_ms); + + robotFrames.mirrorPastCameraFrame(gimbalAngles.first, gimbalAngles.second, AngleUnit::Radians); + + std::pair gimbalFieldAngles = getGimbalFieldOrientationAtTime(frameDelay_ms); + + turretFrames.mirrorPastCameraFrame(gimbalFieldAngles.first, gimbalFieldAngles.second, AngleUnit::Radians); +} + +#endif + +//turret IMU stuff --------------------------------------------------------------------------------------------------------- +#ifdef TURRET_IMU + +Vector3f chassisAnglesConvertedDisplay; +float chassisAnglesConvertedDisplayX; +float chassisAnglesConvertedDisplayY; +float chassisAnglesConvertedDisplayZ; + +Vector3f IMUAnglesDisplay; +float IMUAnglesDisplayX; +float IMUAnglesDisplayY; +float IMUAnglesDisplayZ; + +void KinematicInformant::updateChassisIMUAngles() { + Vector3f IMUAngles = getLocalIMUAngles(); + Vector3f IMUAngularVelocities = getIMUAngularVelocities(); + Vector3f IMUAnglesYawSubtraction(IMUAngles.x,IMUAngles.y,IMUAngles.z - gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); + Vector3f IMUAngularVelocitiesYawSubtraction(IMUAngularVelocities.x,IMUAngularVelocities.y,IMUAngularVelocities.z - gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); + + IMUAnglesDisplay = IMUAnglesYawSubtraction; + IMUAnglesDisplayX = IMUAnglesYawSubtraction.x; + IMUAnglesDisplayY = IMUAnglesYawSubtraction.y; + IMUAnglesDisplayZ = IMUAnglesYawSubtraction.z; + + // Gets chassis angles + // Vector3f chassisAngles = + // drivers->kinematicInformant.getRobotFrames() + // .getFrame(Transformers::FrameType::GIMBAL_IMU_FRAME) + // .getPointInFrame( + // drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::CHASSIS_FRAME), + // IMUAnglesYawSubtraction); + + Vector3f chassisAngles = IMUAnglesDisplay; + + chassisAnglesConvertedDisplay = chassisAngles; + chassisAnglesConvertedDisplayX = chassisAngles.x; + chassisAnglesConvertedDisplayY = chassisAngles.y; + chassisAnglesConvertedDisplayZ = chassisAngles.z; + + // Vector3f chassisAngularVelocities = + // drivers->kinematicInformant.getRobotFrames() + // .getFrame(Transformers::FrameType::GIMBAL_IMU_FRAME) + // .getPointInFrame( + // drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::CHASSIS_FRAME), + // IMUAngularVelocitiesYawSubtraction); + + Vector3f chassisAngularVelocities = IMUAngularVelocitiesYawSubtraction; + + chassisAngularState[X_AXIS].updateFromPosition(chassisAngles[X_AXIS]); + chassisAngularState[Y_AXIS].updateFromPosition(chassisAngles[Y_AXIS]); + chassisAngularState[Z_AXIS].updateFromPosition(chassisAngles[Z_AXIS]); + + chassisAngularState[X_AXIS].updateFromVelocity(chassisAngularVelocities[X_AXIS], false); + chassisAngularState[Y_AXIS].updateFromVelocity(chassisAngularVelocities[Y_AXIS], false); + chassisAngularState[Z_AXIS].updateFromVelocity(chassisAngularVelocities[Z_AXIS], false); +} + +float KinematicInformant::getChassisIMUAngle(AngularAxis axis, AngleUnit unit) { + float angle = chassisAngularState[axis].getPosition(); + return unit == AngleUnit::Radians ? angle : modm::toDegree(angle); +} +float KinematicInformant::getGimbalIMUAngle(AngularAxis axis, AngleUnit unit) { + float angle = gimbalAngularState[axis].getPosition(); + return unit == AngleUnit::Radians ? angle : modm::toDegree(angle); +} + +float KinematicInformant::getChassisIMUAngularVelocity(AngularAxis axis, AngleUnit unit) { + float angularVelocity = chassisAngularState[axis].getVelocity(); + return unit == AngleUnit::Radians ? angularVelocity : modm::toDegree(angularVelocity); +} +float KinematicInformant::getIMUAngularAcceleration(AngularAxis axis, AngleUnit unit) { + float angularAcceleration = imuAngularState[axis].getAcceleration(); + return unit == AngleUnit::Radians ? angularAcceleration : modm::toDegree(angularAcceleration); +} + +Vector3f wDisplay = {0.0f, 0.0f, 0.0f}; +Vector3f alphaDisplay = {0.0f, 0.0f, 0.0f}; +Vector3f rDisplay = {0.0f, 0.0f, 0.0f}; +Vector3f aDisplay = {0.0f, 0.0f, 0.0f}; + +float aXDisplay = 0.0f; +float aYDisplay = 0.0f; +float aZDisplay = 0.0f; +Vector3f KinematicInformant::removeFalseAcceleration( + Vector imuLinearKSV, + Vector imuAngularKSV, + Vector3f r) { + Vector3f w = { + imuAngularKSV[X_AXIS].getVelocity(), + imuAngularKSV[Y_AXIS].getVelocity(), + imuAngularKSV[Z_AXIS].getVelocity()}; + + // Vector3f alpha = { + // imuAngularKSV[X_AXIS].getAcceleration(), + // imuAngularKSV[Y_AXIS].getAcceleration(), + // imuAngularKSV[Z_AXIS].getAcceleration()}; + // was broken so i made it 0, requires investigation on why it's broken (was setting stuff to infinity) + + Vector3f alpha = {0.0f, 0.0f, 0.0f}; + + Vector3f a = { + imuLinearKSV[X_AXIS].getAcceleration(), + imuLinearKSV[Y_AXIS].getAcceleration(), + imuLinearKSV[Z_AXIS].getAcceleration()}; + + aXDisplay = a.getX(); + aYDisplay = a.getY(); + aZDisplay = a.getZ(); + + wDisplay = w; + alphaDisplay = alpha; + rDisplay = r; + aDisplay = a; + + Vector3f linearIMUAcceleration = a - (alpha ^ r) - (w ^ (w ^ r)); + return linearIMUAcceleration; +} + +Vector3f linearIMUAccelerationDisplay; +float linearIMUAccelerationXDisplay = 0.0f; +float linearIMUAccelerationYDisplay = 0.0f; +float linearIMUAccelerationZDisplay = 0.0f; + +float chassisAngleXDisplay = 0.0f; +float chassisAngleYDisplay = 0.0f; +float chassisAngleZDisplay = 0.0f; +void KinematicInformant::updateChassisAcceleration() { + chassisAngleXDisplay = chassisAngularState[X_AXIS].getPosition(); + chassisAngleYDisplay = chassisAngularState[Y_AXIS].getPosition(); + chassisAngleZDisplay = chassisAngularState[Z_AXIS].getPosition(); + + Vector3f linearIMUAcceleration = removeFalseAcceleration(imuLinearState, imuAngularState, IMU_MOUNT_POSITION); + + Vector3f linearChassisAcceleration = + drivers->kinematicInformant.getRobotFrames() + .getFrame(Transformers::FrameType::GIMBAL_IMU_FRAME) + .getPointInFrame( + drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::CHASSIS_FRAME), + linearIMUAcceleration); + + linearIMUAccelerationDisplay = linearIMUAcceleration; + linearIMUAccelerationXDisplay = linearChassisAcceleration.getX(); + linearIMUAccelerationYDisplay = linearChassisAcceleration.getY(); + linearIMUAccelerationZDisplay = linearChassisAcceleration.getZ(); + + chassisLinearState[X_AXIS].updateFromAcceleration(linearChassisAcceleration.getX()); + chassisLinearState[Y_AXIS].updateFromAcceleration(linearChassisAcceleration.getY()); + chassisLinearState[Z_AXIS].updateFromAcceleration(linearChassisAcceleration.getZ()); +} + +void KinematicInformant::updateGimbalAcceleration() { + // gimbalAngleXDisplay = gimbalAngularState[X_AXIS].getPosition(); + // gimbalAngleYDisplay = gimbalAngularState[Y_AXIS].getPosition(); + // gimbalAngleZDisplay = gimbalAngularState[Z_AXIS].getPosition(); + + Vector3f linearIMUAcceleration = removeFalseAcceleration(imuLinearState, imuAngularState, IMU_MOUNT_POSITION); + + Vector3f linearGimbalAcceleration = + drivers->kinematicInformant.getRobotFrames() + .getFrame(Transformers::FrameType::GIMBAL_IMU_FRAME) + .getPointInFrame( + drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::GIMBAL_FRAME), + linearIMUAcceleration); + linearIMUAccelerationDisplay = linearIMUAcceleration; + linearIMUAccelerationXDisplay = linearGimbalAcceleration.getX(); + linearIMUAccelerationYDisplay = linearGimbalAcceleration.getY(); + linearIMUAccelerationZDisplay = linearGimbalAcceleration.getZ(); + + gimbalLinearState[X_AXIS].updateFromAcceleration(linearGimbalAcceleration.getX()); + gimbalLinearState[Y_AXIS].updateFromAcceleration(linearGimbalAcceleration.getY()); + gimbalLinearState[Z_AXIS].updateFromAcceleration(linearGimbalAcceleration.getZ()); +} + +float chassisYawAngleDisplay = 0.0f; +tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat() { + float currGimbalAngle = gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians); + float currChassisAngle = getChassisIMUAngle(YAW_AXIS, AngleUnit::Radians); + chassisYawAngleDisplay = currChassisAngle; + return WrappedFloat(currGimbalAngle + currChassisAngle - YAW_AXIS_START_ANGLE, -M_PI, M_PI); +} + +tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalPitchAngleAsWrappedFloat() { + float currGimbalAngle = gimbalSubsystem->getCurrentPitchAxisAngle(AngleUnit::Radians); + float currChassisAngle = getChassisPitchAngleInGimbalDirection(); + return WrappedFloat(currGimbalAngle + currChassisAngle - PITCH_AXIS_START_ANGLE, -M_PI, M_PI); +} + +float KinematicInformant::getChassisPitchAngleInGimbalDirection() { + float sinGimbYaw = sinf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); + float cosGimbYaw = cosf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); + + float chassisRoll = getChassisIMUAngle(src::Informants::AngularAxis::ROLL_AXIS, AngleUnit::Radians); + float sinChasRoll = sinf(chassisRoll); + float cosChasRoll = cosf(chassisRoll); + + float chassisPitch = getChassisIMUAngle(src::Informants::AngularAxis::PITCH_AXIS, AngleUnit::Radians); + float sinChasPitch = sinf(chassisPitch); + float cosChasPitch = cosf(chassisPitch); + + float chassisPitchAngleInGimbalDirection = atan2f( + cosGimbYaw * sinChasPitch + sinGimbYaw * sinChasRoll, + sqrtf(pow2(sinGimbYaw) * pow2(cosChasRoll) + pow2(cosGimbYaw) * pow2(cosChasPitch))); + + return chassisPitchAngleInGimbalDirection; +} + +float KinematicInformant::getChassisPitchVelocityInGimbalDirection() { + float sinGimbYaw = sinf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); + float cosGimbYaw = cosf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); + + float chassisRollVelocity = getChassisIMUAngularVelocity(src::Informants::AngularAxis::ROLL_AXIS, AngleUnit::Radians); + float sinChasRollVelocity = sinf(chassisRollVelocity); + float cosChasRollVelocity = cosf(chassisRollVelocity); + + float chassisPitchVelocity = getChassisIMUAngularVelocity(src::Informants::AngularAxis::PITCH_AXIS, AngleUnit::Radians); + float sinChasPitchVelocity = sinf(chassisPitchVelocity); + float cosChasPitchVelocity = cosf(chassisPitchVelocity); + + float chassisPitchVelocityInGimbalDirection = atan2f( + cosGimbYaw * sinChasPitchVelocity + sinGimbYaw * sinChasRollVelocity, + sqrtf(pow2(sinGimbYaw) * pow2(cosChasRollVelocity) + pow2(cosGimbYaw) * pow2(cosChasPitchVelocity))); + + return chassisPitchVelocityInGimbalDirection; +} + +float chassisLinearStateXDisplay = 0.0f; +float KinematicInformant::getChassisLinearAccelerationInGimbalDirection() { + // remember to convert linear accel from in/s^2 to m/s^2 + // @luke help pwease 🥺👉👈 + float ang = gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians); + + chassisLinearStateXDisplay = chassisLinearState[X_AXIS].getAcceleration(); + + float accel = + chassisLinearState[X_AXIS].getAcceleration() * sinf(ang) + chassisLinearState[Y_AXIS].getAcceleration() * cosf(ang); + + return accel; +} + +modm::Location2D robotLocationDisplay; +float robotLocationXDisplay = 0.0f; +float robotLocationYDisplay = 0.0f; + +void KinematicInformant::updateRobotFrames() { + // Update IMU Stuff + updateIMUKinematicStateVector(); + updateChassisIMUAngles(); updateChassisAcceleration(); - #endif + chassisIMUHistoryBuffer.prependOverwrite( {getChassisIMUAngle(PITCH_AXIS, AngleUnit::Radians), @@ -430,7 +696,6 @@ void KinematicInformant::updateRobotFrames() { AngleUnit::Radians); robotLocationDisplay = robotLocation; -#endif } float frameDelayDisplay = 0; @@ -447,4 +712,6 @@ void KinematicInformant::mirrorPastRobotFrame(uint32_t frameDelay_ms) { turretFrames.mirrorPastCameraFrame(gimbalFieldAngles.first, gimbalFieldAngles.second, AngleUnit::Radians); } +#endif + } // namespace src::Informants \ No newline at end of file diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.hpp b/aimbots-src/src/informants/kinematics/kinematic_informant.hpp index 9abaeb26..8b0cfcb1 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.hpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.hpp @@ -82,17 +82,21 @@ class KinematicInformant { // Returns angle in rad or deg float getChassisIMUAngle(AngularAxis axis, AngleUnit unit); + // for turret IMU float getGimbalIMUAngle(AngularAxis axis, AngleUnit unit); // Returns angular velocity in rad/s or deg/s float getChassisIMUAngularVelocity(AngularAxis axis, AngleUnit unit); + // for turret IMU float getGimbalIMUAngularVelocity(AngularAxis axis, AngleUnit unit); Vector3f getIMUAngularAccelerations(); float getIMUAngularAcceleration(AngularAxis axis, AngleUnit unit); // Returns lnothing!!! void updateChassisAcceleration(); + + // for turret IMU void updateGimbalAcceleration(); // This updates more than just the robot frames, so will also update turret frames @@ -101,6 +105,10 @@ class KinematicInformant { tap::algorithms::WrappedFloat getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat(); tap::algorithms::WrappedFloat getCurrentFieldRelativeGimbalPitchAngleAsWrappedFloat(); + tap::algorithms::WrappedFloat getCurrentFieldRelativeChassisYawAngleAsWrappedFloat(); + tap::algorithms::WrappedFloat getCurrentFieldRelativeChassisPitchAngleAsWrappedFloat(); + + // rad float getChassisPitchAngleInGimbalDirection(); // rad/s diff --git a/aimbots-src/src/informants/kinematics/robot_frames.cpp b/aimbots-src/src/informants/kinematics/robot_frames.cpp index b4dc1016..0d74390c 100644 --- a/aimbots-src/src/informants/kinematics/robot_frames.cpp +++ b/aimbots-src/src/informants/kinematics/robot_frames.cpp @@ -31,14 +31,14 @@ RobotFrames::RobotFrames() { cameraAtCVUpdateFrame.setOrientation(gimbal_orientation_relative_to_chassis_orientation); cameraAtCVUpdateFrame.setOrigin(camera_origin_relative_to_chassis_origin); #endif -#ifdef TURRET_IMU - // gimbal imu frame relative to gimbal (i.e. how is the imu mounted. In an ideal world this is all 0, 0, 0 but - // hardware kids arent that good. - Matrix3f gimbalIMUOrientation = rotationMatrix(TIMU_CALIBRATION_EULER_X, X_AXIS, AngleUnit::Radians) * - rotationMatrix(TIMU_CALIBRATION_EULER_Y, Y_AXIS, AngleUnit::Radians) * - rotationMatrix(TIMU_CALIBRATION_EULER_Z, Z_AXIS, AngleUnit::Radians); - gimbalIMUFrame.setOrientation(gimbal_orientation_relative_to_chassis_orientation * gimbalIMUOrientation); -#endif +// #ifdef TURRET_IMU +// // gimbal imu frame relative to gimbal (i.e. how is the imu mounted. In an ideal world this is all 0, 0, 0 but +// // hardware kids arent that good. +// Matrix3f gimbalIMUOrientation = rotationMatrix(TIMU_CALIBRATION_EULER_X, X_AXIS, AngleUnit::Radians) * +// rotationMatrix(TIMU_CALIBRATION_EULER_Y, Y_AXIS, AngleUnit::Radians) * +// rotationMatrix(TIMU_CALIBRATION_EULER_Z, Z_AXIS, AngleUnit::Radians); +// gimbalIMUFrame.setOrientation(gimbal_orientation_relative_to_chassis_orientation * gimbalIMUOrientation); +// #endif } void RobotFrames::updateFrames( diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 0852a27b..7d06ca1f 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -130,7 +130,7 @@ static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.0f, + .errDeadzone = 0.1f, .errorDerivativeFloor = 0.0f, }; diff --git a/aimbots-src/src/subsystems/gimbal/complex_commands/gimbal_field_relative_control_command.cpp b/aimbots-src/src/subsystems/gimbal/complex_commands/gimbal_field_relative_control_command.cpp index 461f7100..f975ec53 100644 --- a/aimbots-src/src/subsystems/gimbal/complex_commands/gimbal_field_relative_control_command.cpp +++ b/aimbots-src/src/subsystems/gimbal/complex_commands/gimbal_field_relative_control_command.cpp @@ -24,8 +24,10 @@ void GimbalFieldRelativeControlCommand::initialize() {} float gimbalYawInputDisplay = 0.0f; float targetPitchAngleDisplay = 0; +float PitchAngleTest = 0; void GimbalFieldRelativeControlCommand::execute() { + PitchAngleTest = drivers->bmi088.getPitch(); /*if (drivers->remote.keyPressed(Remote::Key::V) && !wasVPressed) wasVPRessed = true; if (wasVPRessed && !drivers->remote.keyPressed(Remote::Key::V)) { diff --git a/aimbots-src/src/utils/tools/robot_specific_defines.hpp b/aimbots-src/src/utils/tools/robot_specific_defines.hpp index 19e9da18..097fa4e6 100644 --- a/aimbots-src/src/utils/tools/robot_specific_defines.hpp +++ b/aimbots-src/src/utils/tools/robot_specific_defines.hpp @@ -22,6 +22,7 @@ #elif defined(TARGET_SENTRY_BRAVO) || defined(TARGET_SENTRY_SWERVE) #include "robots/sentry/constants/sentry_general_constant.hpp" #include "robots/sentry/sentry_control_interface.hpp" +#define TURRET_IMU #define ALL_SENTRIES #elif defined(TARGET_STANDARD_BLASTOISE) || defined(TARGET_STANDARD_SQUIRTLE) || defined(TARGET_STANDARD_2023) || \ From e7388fbccfcb4adb13bc74ed49ce0a162684bc33 Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Mon, 22 Sep 2025 19:14:40 -0500 Subject: [PATCH 51/67] fixed pitch stabilization --- .../kinematics/kinematic_informant.cpp | 41 ++++--------------- 1 file changed, 8 insertions(+), 33 deletions(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index 6a91c25a..15c39247 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -413,11 +413,13 @@ void KinematicInformant::mirrorPastRobotFrame(uint32_t frameDelay_ms) { #ifdef TURRET_IMU Vector3f chassisAnglesConvertedDisplay; +// individual components for debug purposes float chassisAnglesConvertedDisplayX; float chassisAnglesConvertedDisplayY; float chassisAnglesConvertedDisplayZ; Vector3f IMUAnglesDisplay; +// individual components for debug purposes float IMUAnglesDisplayX; float IMUAnglesDisplayY; float IMUAnglesDisplayZ; @@ -433,6 +435,8 @@ void KinematicInformant::updateChassisIMUAngles() { IMUAnglesDisplayY = IMUAnglesYawSubtraction.y; IMUAnglesDisplayZ = IMUAnglesYawSubtraction.z; + // this transform doesn't exist yet + // Gets chassis angles // Vector3f chassisAngles = // drivers->kinematicInformant.getRobotFrames() @@ -441,7 +445,7 @@ void KinematicInformant::updateChassisIMUAngles() { // drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::CHASSIS_FRAME), // IMUAnglesYawSubtraction); - Vector3f chassisAngles = IMUAnglesDisplay; + Vector3f chassisAngles = IMUAnglesYawSubtraction; chassisAnglesConvertedDisplay = chassisAngles; chassisAnglesConvertedDisplayX = chassisAngles.x; @@ -585,10 +589,9 @@ void KinematicInformant::updateGimbalAcceleration() { float chassisYawAngleDisplay = 0.0f; tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat() { - float currGimbalAngle = gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians); float currChassisAngle = getChassisIMUAngle(YAW_AXIS, AngleUnit::Radians); chassisYawAngleDisplay = currChassisAngle; - return WrappedFloat(currGimbalAngle + currChassisAngle - YAW_AXIS_START_ANGLE, -M_PI, M_PI); + return WrappedFloat(getLocalIMUAngles().z - YAW_AXIS_START_ANGLE, -M_PI, M_PI); } tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalPitchAngleAsWrappedFloat() { @@ -598,41 +601,13 @@ tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalP } float KinematicInformant::getChassisPitchAngleInGimbalDirection() { - float sinGimbYaw = sinf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); - float cosGimbYaw = cosf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); - - float chassisRoll = getChassisIMUAngle(src::Informants::AngularAxis::ROLL_AXIS, AngleUnit::Radians); - float sinChasRoll = sinf(chassisRoll); - float cosChasRoll = cosf(chassisRoll); - - float chassisPitch = getChassisIMUAngle(src::Informants::AngularAxis::PITCH_AXIS, AngleUnit::Radians); - float sinChasPitch = sinf(chassisPitch); - float cosChasPitch = cosf(chassisPitch); - - float chassisPitchAngleInGimbalDirection = atan2f( - cosGimbYaw * sinChasPitch + sinGimbYaw * sinChasRoll, - sqrtf(pow2(sinGimbYaw) * pow2(cosChasRoll) + pow2(cosGimbYaw) * pow2(cosChasPitch))); - return chassisPitchAngleInGimbalDirection; + return -getLocalIMUAngles().getX(); } float KinematicInformant::getChassisPitchVelocityInGimbalDirection() { - float sinGimbYaw = sinf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); - float cosGimbYaw = cosf(gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); - - float chassisRollVelocity = getChassisIMUAngularVelocity(src::Informants::AngularAxis::ROLL_AXIS, AngleUnit::Radians); - float sinChasRollVelocity = sinf(chassisRollVelocity); - float cosChasRollVelocity = cosf(chassisRollVelocity); - - float chassisPitchVelocity = getChassisIMUAngularVelocity(src::Informants::AngularAxis::PITCH_AXIS, AngleUnit::Radians); - float sinChasPitchVelocity = sinf(chassisPitchVelocity); - float cosChasPitchVelocity = cosf(chassisPitchVelocity); - - float chassisPitchVelocityInGimbalDirection = atan2f( - cosGimbYaw * sinChasPitchVelocity + sinGimbYaw * sinChasRollVelocity, - sqrtf(pow2(sinGimbYaw) * pow2(cosChasRollVelocity) + pow2(cosGimbYaw) * pow2(cosChasPitchVelocity))); - return chassisPitchVelocityInGimbalDirection; + return -getIMUAngularVelocities().getX(); } float chassisLinearStateXDisplay = 0.0f; From 98b47d8d6ab401ef5b77c5bc519355600d2258dc Mon Sep 17 00:00:00 2001 From: zDagger Date: Mon, 22 Sep 2025 21:32:40 -0500 Subject: [PATCH 52/67] updated sentry constants and added watch variables for setting swerve yaw offsets --- .../robots/sentry/constants/sentry_chassis_constants.hpp | 6 +++--- .../robots/sentry/constants/sentry_gimbal_constants.hpp | 2 +- aimbots-src/src/subsystems/chassis/control/chassis.cpp | 8 ++++++++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index ee035ed7..3c8161c3 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -29,9 +29,9 @@ static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { .errorDerivativeFloor = 0.0f, }; -static constexpr int LEFT_FRONT_YAW_OFFSET = 2000; -static constexpr int RIGHT_FRONT_YAW_OFFSET = 7500; -static constexpr int LEFT_BACK_YAW_OFFSET = 6200; +static constexpr int LEFT_FRONT_YAW_OFFSET = 2090; +static constexpr int RIGHT_FRONT_YAW_OFFSET = 7460; +static constexpr int LEFT_BACK_YAW_OFFSET = 6117; static constexpr int RIGHT_BACK_YAW_OFFSET = 7500; #elif defined(TARGET_SENTRY_BRAVO) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 7d06ca1f..1efbca2d 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -28,7 +28,7 @@ static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(30.0f); #elif defined(TARGET_SENTRY_SWERVE) static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS1; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(54.58f))}; +static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(19.5f))}; static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(147.8f))}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(0.0f); diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index e9430f57..accfd164 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -288,6 +288,10 @@ void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, f } float wheelRPMDisplay = 0.0f; +uint16_t watchLFYaw = 0; +uint16_t watchRFYaw = 0; +uint16_t watchLBYaw = 0; +uint16_t watchRBYaw = 0; void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { // if(x < 0.1 && y < 0.1 && r<0.1){ @@ -341,6 +345,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); left_front_yaw_actual = yawToRad(motors[LF][1]->getEncoderWrapped(), LEFT_FRONT_YAW_OFFSET); + watchLFYaw = motors[LF][1]->getEncoderUnwrapped(); target_left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2); prev_left_front_yaw = left_front_yaw_actual; optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, left_front_yaw_actual); @@ -349,6 +354,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel left_front_yaw_db = targetRPMs[LF][1]; targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); + watchRFYaw = motors[RF][1]->getEncoderUnwrapped(); right_front_yaw_actual = yawToRad(motors[RF][1]->getEncoderWrapped(), RIGHT_FRONT_YAW_OFFSET); target_right_front_yaw = (atan2f(c, b) + 3 * M_PI / 2); prev_right_front_yaw = right_front_yaw_actual; @@ -359,6 +365,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); left_back_yaw_actual = yawToRad(motors[LB][1]->getEncoderWrapped(),LEFT_BACK_YAW_OFFSET); + watchLBYaw = motors[LB][1]->getEncoderUnwrapped(); target_left_back_yaw = atan2f(d, a) + 3 * M_PI / 2; prev_left_back_yaw = left_back_yaw_actual; optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, left_back_yaw_actual); @@ -368,6 +375,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); right_back_yaw_actual = yawToRad(motors[RB][1]->getEncoderWrapped(),RIGHT_BACK_YAW_OFFSET); + watchRBYaw = motors[RB][1]->getEncoderUnwrapped(); target_right_back_yaw = atan2f(c, a) + 3 * M_PI / 2; prev_right_back_yaw = right_back_yaw_actual; optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,right_back_yaw_actual); From cd534338ba1051f47761188b0001341a7652b425 Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Tue, 23 Sep 2025 00:37:42 -0500 Subject: [PATCH 53/67] tuned some pids for bullying kids --- .../informants/kinematics/kinematic_informant.cpp | 9 ++++++++- .../sentry/constants/sentry_chassis_constants.hpp | 2 +- .../sentry/constants/sentry_gimbal_constants.hpp | 14 +++++++------- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index 15c39247..9bb108ee 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -417,18 +417,23 @@ Vector3f chassisAnglesConvertedDisplay; float chassisAnglesConvertedDisplayX; float chassisAnglesConvertedDisplayY; float chassisAnglesConvertedDisplayZ; +float chassisAngularVelocitiesDisplayZ; + +float YawAngularVelocityEncoder; Vector3f IMUAnglesDisplay; // individual components for debug purposes float IMUAnglesDisplayX; float IMUAnglesDisplayY; float IMUAnglesDisplayZ; +float rawYawVel; void KinematicInformant::updateChassisIMUAngles() { Vector3f IMUAngles = getLocalIMUAngles(); Vector3f IMUAngularVelocities = getIMUAngularVelocities(); + YawAngularVelocityEncoder = (gimbalSubsystem->getYawMotorRPM(0) * (3.14159265358979323846 * 2.0) / 60.0f)/2.0f; Vector3f IMUAnglesYawSubtraction(IMUAngles.x,IMUAngles.y,IMUAngles.z - gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); - Vector3f IMUAngularVelocitiesYawSubtraction(IMUAngularVelocities.x,IMUAngularVelocities.y,IMUAngularVelocities.z - gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); + Vector3f IMUAngularVelocitiesYawSubtraction(IMUAngularVelocities.x,IMUAngularVelocities.y,IMUAngularVelocities.z - YawAngularVelocityEncoder); IMUAnglesDisplay = IMUAnglesYawSubtraction; IMUAnglesDisplayX = IMUAnglesYawSubtraction.x; @@ -459,7 +464,9 @@ void KinematicInformant::updateChassisIMUAngles() { // drivers->kinematicInformant.getRobotFrames().getFrame(Transformers::FrameType::CHASSIS_FRAME), // IMUAngularVelocitiesYawSubtraction); + rawYawVel = getIMUAngularVelocities().getZ(); Vector3f chassisAngularVelocities = IMUAngularVelocitiesYawSubtraction; + chassisAngularVelocitiesDisplayZ = IMUAngularVelocitiesYawSubtraction.z; chassisAngularState[X_AXIS].updateFromPosition(chassisAngles[X_AXIS]); chassisAngularState[Y_AXIS].updateFromPosition(chassisAngles[Y_AXIS]); diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 3c8161c3..796b9ced 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -118,7 +118,7 @@ static constexpr float FOLLOW_GIMBAL_ANGLE_THRESHOLD = modm::toRadian(20.0f); static constexpr SmoothPIDConfig ROTATION_POSITION_PID_CONFIG = { .kp = 1.65f, .ki = 0.0f, - .kd = 0.005f, + .kd = 0.01f, .maxICumulative = 0.9f, .maxOutput = 1.0f, .tQDerivativeKalman = 1.0f, diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 1efbca2d..71d0077f 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -94,21 +94,21 @@ static constexpr SmoothPIDConfig PITCH_POSITION_PID_CONFIG = { static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { .kp = 20.0f, // 35 .ki = 0.0f, - .kd = 0.0f, + .kd = 0.1f, .maxICumulative = 1000.0f, .maxOutput = 40.0f, // 40 rad/s is maximum speed of 6020 .tQDerivativeKalman = 1.0f, .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.0f, + .errDeadzone = 0.1f, .errorDerivativeFloor = 0.0f, }; static constexpr SmoothPIDConfig PITCH_POSITION_CASCADE_PID_CONFIG = { .kp = 25.0f, .ki = 0.0f, - .kd = 0.0f, + .kd = 0.1f, .maxICumulative = 1000.0f, .maxOutput = 35.0f, .tQDerivativeKalman = 1.0f, @@ -121,16 +121,16 @@ static constexpr SmoothPIDConfig PITCH_POSITION_CASCADE_PID_CONFIG = { // VELOCITY PID CONSTANTS static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { - .kp = 2000.0f, - .ki = 25.0f, - .kd = 1.0f, + .kp = 100.0f, + .ki = 0.0f, + .kd = 0.5f, .maxICumulative = 2000.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.1f, + .errDeadzone = 0.5f, .errorDerivativeFloor = 0.0f, }; From 33f64d391c5b5333ea8696e3989409359cb59563 Mon Sep 17 00:00:00 2001 From: svukryggi Date: Tue, 23 Sep 2025 19:50:56 -0500 Subject: [PATCH 54/67] chassis imu defined ITS BACK RAAAAAAAA --- .../src/robots/aerial/constants/aerial_chassis_constants.hpp | 1 + aimbots-src/src/robots/dart/constants/dart_chassis_constants.hpp | 1 + .../src/robots/engineer/constants/engineer_chassis_constants.hpp | 1 + aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp | 1 + .../src/robots/sentry/constants/sentry_chassis_constants.hpp | 1 - .../src/robots/standard/constants/standard_chassis_constants.hpp | 1 + 6 files changed, 5 insertions(+), 1 deletion(-) diff --git a/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp b/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp index 33d663c0..a59e08f0 100644 --- a/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp +++ b/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp @@ -3,6 +3,7 @@ #include "utils/math/matrix_helpers.hpp" #define CHASSIS_COMPATIBLE +#define CHASSIS_IMU // THE GREAT RETURN static constexpr uint8_t DRIVEN_WHEEL_COUNT = 4; static constexpr uint8_t MOTORS_PER_WHEEL = 1; diff --git a/aimbots-src/src/robots/dart/constants/dart_chassis_constants.hpp b/aimbots-src/src/robots/dart/constants/dart_chassis_constants.hpp index 0ad32f29..d05af949 100644 --- a/aimbots-src/src/robots/dart/constants/dart_chassis_constants.hpp +++ b/aimbots-src/src/robots/dart/constants/dart_chassis_constants.hpp @@ -3,6 +3,7 @@ #include "utils/math/matrix_helpers.hpp" #define CHASSIS_COMPATIBLE +#define CHASSIS_IMU // THE GREAT RETURN static constexpr uint8_t DRIVEN_WHEEL_COUNT = 4; static constexpr uint8_t MOTORS_PER_WHEEL = 1; diff --git a/aimbots-src/src/robots/engineer/constants/engineer_chassis_constants.hpp b/aimbots-src/src/robots/engineer/constants/engineer_chassis_constants.hpp index 9d476efc..95f50f05 100644 --- a/aimbots-src/src/robots/engineer/constants/engineer_chassis_constants.hpp +++ b/aimbots-src/src/robots/engineer/constants/engineer_chassis_constants.hpp @@ -3,6 +3,7 @@ #include "utils/tools/common_types.hpp" #define CHASSIS_COMPATIBLE +#define CHASSIS_IMU // THE GREAT RETURN /** * @brief Defines the number of motors created for the chassis. diff --git a/aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp index f49ff7ec..9a7934ec 100644 --- a/aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_chassis_constants.hpp @@ -3,6 +3,7 @@ #include "utils/tools/common_types.hpp" #define CHASSIS_COMPATIBLE +#define CHASSIS_IMU // THE GREAT RETURN static constexpr uint8_t DRIVEN_WHEEL_COUNT = 4; static constexpr uint8_t MOTORS_PER_WHEEL = 1; diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 796b9ced..459f41ce 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -3,7 +3,6 @@ #include "utils/tools/common_types.hpp" #define CHASSIS_COMPATIBLE -// #define CHASSIS_IMU // old shi #define TURRET_IMU // IMU migration and whatnot #if defined(TARGET_SENTRY_SWERVE) diff --git a/aimbots-src/src/robots/standard/constants/standard_chassis_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_chassis_constants.hpp index 23178f51..9a40973c 100644 --- a/aimbots-src/src/robots/standard/constants/standard_chassis_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_chassis_constants.hpp @@ -3,6 +3,7 @@ #include "utils/tools/common_types.hpp" #define CHASSIS_COMPATIBLE +#define CHASSIS_IMU // DIMITRI static constexpr uint8_t DRIVEN_WHEEL_COUNT = 4; static constexpr uint8_t MOTORS_PER_WHEEL = 1; From 4cd9738d683650dbdda04ceac5a7434106dc430d Mon Sep 17 00:00:00 2001 From: svukryggi Date: Tue, 23 Sep 2025 20:11:21 -0500 Subject: [PATCH 55/67] IT WORKED WE are goated --- .../src/robots/aerial/constants/aerial_chassis_constants.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp b/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp index a59e08f0..a60a75e8 100644 --- a/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp +++ b/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp @@ -3,7 +3,7 @@ #include "utils/math/matrix_helpers.hpp" #define CHASSIS_COMPATIBLE -#define CHASSIS_IMU // THE GREAT RETURN +// #define CHASSIS_IMU // THE GREAT RETURN (tbd) static constexpr uint8_t DRIVEN_WHEEL_COUNT = 4; static constexpr uint8_t MOTORS_PER_WHEEL = 1; From de0ffad4e3206302b7091567cb08bf4fb18254ac Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Thu, 25 Sep 2025 15:52:45 -0500 Subject: [PATCH 56/67] no more chassis angular jiggle --- .../informants/kinematics/kinematic_informant.cpp | 2 +- .../sentry/constants/sentry_chassis_constants.hpp | 12 ++++++------ .../sentry/constants/sentry_gimbal_constants.hpp | 8 +++++--- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index 9bb108ee..9c0e7244 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -433,7 +433,7 @@ void KinematicInformant::updateChassisIMUAngles() { Vector3f IMUAngularVelocities = getIMUAngularVelocities(); YawAngularVelocityEncoder = (gimbalSubsystem->getYawMotorRPM(0) * (3.14159265358979323846 * 2.0) / 60.0f)/2.0f; Vector3f IMUAnglesYawSubtraction(IMUAngles.x,IMUAngles.y,IMUAngles.z - gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); - Vector3f IMUAngularVelocitiesYawSubtraction(IMUAngularVelocities.x,IMUAngularVelocities.y,IMUAngularVelocities.z - YawAngularVelocityEncoder); + Vector3f IMUAngularVelocitiesYawSubtraction(IMUAngularVelocities.x,IMUAngularVelocities.y,round((IMUAngularVelocities.z - YawAngularVelocityEncoder)*10000.0f)/10000.0f); IMUAnglesDisplay = IMUAnglesYawSubtraction; IMUAnglesDisplayX = IMUAnglesYawSubtraction.x; diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 459f41ce..21b90452 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -17,7 +17,7 @@ static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { .kp = 30.0f, .ki = 0.0f, - .kd = 15.0f, + .kd = 1.0f, .maxICumulative = 0.9f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, @@ -51,7 +51,7 @@ static constexpr float CHASSIS_LINEAR_ACCELERATION_PITCH_COMPENSATION = 0.0f; static constexpr SmoothPIDConfig CHASSIS_VELOCITY_PID_CONFIG = { .kp = 18.0f, .ki = 0.0f, - .kd = 5.0f, + .kd = 1.0f, .maxICumulative = 10.0f, .maxOutput = M3508_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, @@ -115,9 +115,9 @@ static constexpr float MIN_ROTATION_THRESHOLD = 800.0f; static constexpr float FOLLOW_GIMBAL_ANGLE_THRESHOLD = modm::toRadian(20.0f); static constexpr SmoothPIDConfig ROTATION_POSITION_PID_CONFIG = { - .kp = 1.65f, + .kp = 0.5f, .ki = 0.0f, - .kd = 0.01f, + .kd = 0.0f, .maxICumulative = 0.9f, .maxOutput = 1.0f, .tQDerivativeKalman = 1.0f, @@ -132,13 +132,13 @@ static constexpr SmoothPIDConfig ROTATION_POSITION_PID_CONFIG = { * @brief TOKYO CONSTANTS */ // Fraction that user input is multiplied by when "drifting" -static constexpr float TOKYO_TRANSLATIONAL_SPEED_MULTIPLIER = 0.6f; +static constexpr float TOKYO_TRANSLATIONAL_SPEED_MULTIPLIER = 0.7f; // Fraction of the maximum translation speed for when rotation speed should be reduced static constexpr float TOKYO_TRANSLATION_THRESHOLD_TO_DECREASE_ROTATION_SPEED = 0.5f; // Fraction of max chassis speed applied to rotation speed static constexpr float TOKYO_ROTATIONAL_SPEED_FRACTION_OF_MAX = 0.75f; // Fraction to cut rotation speed by when the robot is "drifting" -static constexpr float TOKYO_ROTATIONAL_SPEED_MULTIPLIER_WHEN_TRANSLATING = 0.7f; +static constexpr float TOKYO_ROTATIONAL_SPEED_MULTIPLIER_WHEN_TRANSLATING = 0.6f; // Rotational speed increment per iteration to apply until rotation setpoint is reached static constexpr float TOKYO_ROTATIONAL_SPEED_INCREMENT = 50.0f; // rpm diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 71d0077f..4439612a 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -61,7 +61,9 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (5.0f / 17.0f); // for 2023 Se /** * @brief Position PID constants + * */ + static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { .kp = 2'000.0f, .ki = 0.0f, @@ -92,7 +94,7 @@ static constexpr SmoothPIDConfig PITCH_POSITION_PID_CONFIG = { // VISION PID CONSTANTS static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { - .kp = 20.0f, // 35 + .kp = 35.0f, // 35 .ki = 0.0f, .kd = 0.1f, .maxICumulative = 1000.0f, @@ -101,7 +103,7 @@ static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.1f, + .errDeadzone = 0.0f, .errorDerivativeFloor = 0.0f, }; @@ -130,7 +132,7 @@ static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.5f, + .errDeadzone = 0.0f, .errorDerivativeFloor = 0.0f, }; From 04588a392c89162de21b83ac1ca37b2473c81bd8 Mon Sep 17 00:00:00 2001 From: CaptainGold1 Date: Fri, 26 Sep 2025 18:16:36 -0500 Subject: [PATCH 57/67] Allow on-the-fly PID tuning for swerve motors and gimbal rotate. --- .../kinematics/kinematic_informant.cpp | 4 ++ .../constants/sentry_gimbal_constants.hpp | 2 +- .../subsystems/chassis/control/chassis.cpp | 67 +++++++++++++++++++ .../gimbal_field_relative_controller.cpp | 15 +++++ 4 files changed, 87 insertions(+), 1 deletion(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index 9c0e7244..4cdb6c1a 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -428,6 +428,8 @@ float IMUAnglesDisplayY; float IMUAnglesDisplayZ; float rawYawVel; +float IMUYawAngleDisplayGood; + void KinematicInformant::updateChassisIMUAngles() { Vector3f IMUAngles = getLocalIMUAngles(); Vector3f IMUAngularVelocities = getIMUAngularVelocities(); @@ -439,6 +441,8 @@ void KinematicInformant::updateChassisIMUAngles() { IMUAnglesDisplayX = IMUAnglesYawSubtraction.x; IMUAnglesDisplayY = IMUAnglesYawSubtraction.y; IMUAnglesDisplayZ = IMUAnglesYawSubtraction.z; + + IMUYawAngleDisplayGood = getLocalIMUAngles().z; // this transform doesn't exist yet diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 4439612a..707e80bc 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -94,7 +94,7 @@ static constexpr SmoothPIDConfig PITCH_POSITION_PID_CONFIG = { // VISION PID CONSTANTS static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { - .kp = 35.0f, // 35 + .kp = 10.0f, // 35 .ki = 0.0f, .kd = 0.1f, .maxICumulative = 1000.0f, diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index accfd164..73ccf001 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -293,6 +293,22 @@ uint16_t watchRFYaw = 0; uint16_t watchLBYaw = 0; uint16_t watchRBYaw = 0; +// uint16_t watchLFSpeed = 0; + +// for tunning PID system through Ozone + +float driveVelocityPDebug = 0.0f; +float driveVelocityIDebug = 0.0f; +float driveVelocityDDebug = 0.0f; +bool updateDriveVelocityPIDsDebug = false; + +float steeringVelocityPDebug = 0.0f; +float steeringVelocityIDebug = 0.0f; +float steeringVelocityDDebug = 0.0f; +bool updateSteeringVelocityPIDsDebug = false; + +float maxWheelSpeedDisplay = 0.0f; + void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { // if(x < 0.1 && y < 0.1 && r<0.1){ // lockWatch = true; @@ -335,6 +351,57 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel // targetRPMs[RB][1] = right_back_yaw % 8191; // right_back_yaw_db = targetRPMs[RB][1]; // }else{ + + if (updateDriveVelocityPIDsDebug) { + velocityPIDs[LB][0]->pid.setP(driveVelocityPDebug); + velocityPIDs[LB][0]->pid.setI(driveVelocityIDebug); + velocityPIDs[LB][0]->pid.setD(driveVelocityDDebug); + velocityPIDs[LB][0]->pid.reset(); + + velocityPIDs[RB][0]->pid.setP(driveVelocityPDebug); + velocityPIDs[RB][0]->pid.setI(driveVelocityIDebug); + velocityPIDs[RB][0]->pid.setD(driveVelocityDDebug); + velocityPIDs[RB][0]->pid.reset(); + + velocityPIDs[RF][0]->pid.setP(driveVelocityPDebug); + velocityPIDs[RF][0]->pid.setI(driveVelocityIDebug); + velocityPIDs[RF][0]->pid.setD(driveVelocityDDebug); + velocityPIDs[RF][0]->pid.reset(); + + velocityPIDs[LF][0]->pid.setP(driveVelocityPDebug); + velocityPIDs[LF][0]->pid.setI(driveVelocityIDebug); + velocityPIDs[LF][0]->pid.setD(driveVelocityDDebug); + velocityPIDs[LF][0]->pid.reset(); + + updateDriveVelocityPIDsDebug = false; + } + + if (updateSteeringVelocityPIDsDebug) { + velocityPIDs[LB][1]->pid.setP(steeringVelocityPDebug); + velocityPIDs[LB][1]->pid.setI(steeringVelocityIDebug); + velocityPIDs[LB][1]->pid.setD(steeringVelocityDDebug); + velocityPIDs[LB][1]->pid.reset(); + + velocityPIDs[RB][1]->pid.setP(steeringVelocityPDebug); + velocityPIDs[RB][1]->pid.setI(steeringVelocityIDebug); + velocityPIDs[RB][1]->pid.setD(steeringVelocityDDebug); + velocityPIDs[RB][1]->pid.reset(); + + velocityPIDs[RF][1]->pid.setP(steeringVelocityPDebug); + velocityPIDs[RF][1]->pid.setI(steeringVelocityIDebug); + velocityPIDs[RF][1]->pid.setD(steeringVelocityDDebug); + velocityPIDs[RF][1]->pid.reset(); + + velocityPIDs[LF][1]->pid.setP(steeringVelocityPDebug); + velocityPIDs[LF][1]->pid.setI(steeringVelocityIDebug); + velocityPIDs[LF][1]->pid.setD(steeringVelocityDDebug); + velocityPIDs[LF][1]->pid.reset(); + + updateSteeringVelocityPIDsDebug = false; + } + + maxWheelSpeedDisplay = maxWheelSpeed; + lockWatch = false; float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); diff --git a/aimbots-src/src/subsystems/gimbal/control/gimbal_field_relative_controller.cpp b/aimbots-src/src/subsystems/gimbal/control/gimbal_field_relative_controller.cpp index 9114d320..db177ec9 100644 --- a/aimbots-src/src/subsystems/gimbal/control/gimbal_field_relative_controller.cpp +++ b/aimbots-src/src/subsystems/gimbal/control/gimbal_field_relative_controller.cpp @@ -53,6 +53,13 @@ float kinematicYawAngleDisplay = 0; float speedTarget = 0.0f; +// ozone pid tuning + +float gimbalYawPositionCascadePDebug = 0.0f; +float gimbalYawPositionCascadeIDebug = 0.0f; +float gimbalYawPositionCascadeDDebug = 0.0f; +bool updateGimbalYawPositionCascadeDebug = false; + void GimbalFieldRelativeController::runYawController( std::optional velocityLimit) { // using cascade controller for yaw @@ -77,6 +84,14 @@ void GimbalFieldRelativeController::runYawController( // speedTarget += 1 / 200.0f; for (auto i = 0; i < YAW_MOTOR_COUNT; i++) { + if (updateGimbalYawPositionCascadeDebug) { + yawPositionCascadePIDs[i]->pid.setP(gimbalYawPositionCascadePDebug); + yawPositionCascadePIDs[i]->pid.setI(gimbalYawPositionCascadeIDebug); + yawPositionCascadePIDs[i]->pid.setD(gimbalYawPositionCascadeDDebug); + yawPositionCascadePIDs[i]->pid.reset(); + updateGimbalYawPositionCascadeDebug = false; + } + yawVelocityFilters[i]->update(RPM_TO_RADPS(gimbal->getYawMotorRPM(i))); float fieldRelativeVelocityTarget = yawPositionCascadePIDs[i]->runController( From cf7d77429a0859d2b0d15fccb819c3a21892776a Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Sat, 27 Sep 2025 13:23:52 -0500 Subject: [PATCH 58/67] swerve optimization --- .../constants/sentry_chassis_constants.hpp | 2 +- .../constants/sentry_gimbal_constants.hpp | 4 +- .../subsystems/chassis/control/chassis.cpp | 303 +++++++++--------- .../subsystems/chassis/control/chassis.hpp | 3 +- 4 files changed, 159 insertions(+), 153 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 21b90452..34d3f847 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -138,7 +138,7 @@ static constexpr float TOKYO_TRANSLATION_THRESHOLD_TO_DECREASE_ROTATION_SPEED = // Fraction of max chassis speed applied to rotation speed static constexpr float TOKYO_ROTATIONAL_SPEED_FRACTION_OF_MAX = 0.75f; // Fraction to cut rotation speed by when the robot is "drifting" -static constexpr float TOKYO_ROTATIONAL_SPEED_MULTIPLIER_WHEN_TRANSLATING = 0.6f; +static constexpr float TOKYO_ROTATIONAL_SPEED_MULTIPLIER_WHEN_TRANSLATING = 0.4f; // Rotational speed increment per iteration to apply until rotation setpoint is reached static constexpr float TOKYO_ROTATIONAL_SPEED_INCREMENT = 50.0f; // rpm diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 707e80bc..7c95d370 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -65,9 +65,9 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (5.0f / 17.0f); // for 2023 Se */ static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { - .kp = 2'000.0f, + .kp = 0'000.0f, .ki = 0.0f, - .kd = 8000.0f, + .kd = 0.0f, .maxICumulative = 0.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 73ccf001..ee55f0e0 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -271,22 +271,52 @@ float prev_right_back_yaw; bool lockWatch; -float ChassisSubsystem::yawToRad(float targetYaw, int offset){ - return ((targetYaw-offset)/((180 / M_PI) / 360 * 8191)); +static inline float wrap0N(float x, float N) { + float y = std::fmod(x, N); + if (y < 0) y += N; + return y; } -void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, float currYaw){ - float delta = targetYaw-currYaw; - if(delta > M_PI){ - delta = (2 * M_PI)-delta; - } - if(std::abs(delta)>(M_PI/2)){ +static inline float wrapToPi(float a) { + return std::atan2(std::sin(a), std::cos(a)); +} + +static inline float wrap0To2Pi(float a) { + return wrap0N(a, 2.0f * static_cast(M_PI)); +} + +static constexpr float kTicksPerRev = 8192.0f; // why wasn't there a variable for this already? +static constexpr float kTwoPi = 2.0f * static_cast(M_PI); + +float ChassisSubsystem::yawToRad(float ticks, int offsetTicks) { + const float deltaTicks = wrap0N(ticks - static_cast(offsetTicks), kTicksPerRev); + return deltaTicks * (kTwoPi / kTicksPerRev); +} + +static inline float radToTicksFloat(float radians, int offsetTicks) { + const float ticks = static_cast(offsetTicks) + radians * (kTicksPerRev / kTwoPi); + return wrap0N(ticks, kTicksPerRev); +} +static inline int radToTicksInt (float radians, int offsetTicks) { + return static_cast(std::lround(radToTicksFloat(radians, offsetTicks))); +} + +void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, float currYaw) { + const float hysteresis = static_cast(M_PI) * 2.0f / 180.0f; // ~2° + float delta = wrapToPi(targetYaw - currYaw); + + targetYaw = currYaw + delta; + + //flip wheel direction if turn is greater than 90 + hysteresis degrees + if (std::fabs(delta) > (static_cast(M_PI) / 2.0f + hysteresis)) { targetRPMDrive = -targetRPMDrive; - targetYaw += M_PI; - targetYaw = wrapTo0To2PIRange(targetYaw); + targetYaw += (delta > 0.0f) ? -static_cast(M_PI) : static_cast(M_PI); } + + targetYaw = wrap0To2Pi(targetYaw); } + float wheelRPMDisplay = 0.0f; uint16_t watchLFYaw = 0; uint16_t watchRFYaw = 0; @@ -309,150 +339,125 @@ bool updateSteeringVelocityPIDsDebug = false; float maxWheelSpeedDisplay = 0.0f; +float xDisplay = 0.0f; +float yDisplay = 0.0f; +float rotDisplay = 0.0f; + void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { - // if(x < 0.1 && y < 0.1 && r<0.1){ - // lockWatch = true; - // float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); - - // float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - // float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - // float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - // float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - - // targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // left_front_yaw_actual = motors[LF][1]->getEncoderWrapped(); - // target_left_front_yaw = prev_left_front_yaw; - // optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, yawToRad(left_front_yaw_actual,LEFT_FRONT_YAW_OFFSET)); - // left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; - // targetRPMs[LF][1] = left_front_yaw % 8191; - // left_front_yaw_db = targetRPMs[LF][1]; - - // targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // right_front_yaw_actual = motors[RF][1]->getEncoderWrapped(); - // target_right_front_yaw = prev_right_front_yaw; - // optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, yawToRad(right_front_yaw_actual,RIGHT_FRONT_YAW_OFFSET)); - // right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - // targetRPMs[RF][1] = right_front_yaw % 8191; - // right_front_yaw_db = targetRPMs[RF][1]; - - // targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // left_back_yaw_actual = motors[LB][1]->getEncoderWrapped(); - // target_left_back_yaw = prev_left_back_yaw; - // optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, yawToRad(left_back_yaw_actual,LEFT_BACK_YAW_OFFSET)); - // left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - // targetRPMs[LB][1] = left_back_yaw % 8191; - // left_back_yaw_db = targetRPMs[LB][1]; - - // targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - // right_back_yaw_actual = motors[RB][1]->getEncoderWrapped(); - // target_right_back_yaw = prev_right_back_yaw; - // optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,yawToRad(right_back_yaw_actual,RIGHT_BACK_YAW_OFFSET)); - // right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - // targetRPMs[RB][1] = right_back_yaw % 8191; - // right_back_yaw_db = targetRPMs[RB][1]; - // }else{ - - if (updateDriveVelocityPIDsDebug) { - velocityPIDs[LB][0]->pid.setP(driveVelocityPDebug); - velocityPIDs[LB][0]->pid.setI(driveVelocityIDebug); - velocityPIDs[LB][0]->pid.setD(driveVelocityDDebug); - velocityPIDs[LB][0]->pid.reset(); - - velocityPIDs[RB][0]->pid.setP(driveVelocityPDebug); - velocityPIDs[RB][0]->pid.setI(driveVelocityIDebug); - velocityPIDs[RB][0]->pid.setD(driveVelocityDDebug); - velocityPIDs[RB][0]->pid.reset(); - - velocityPIDs[RF][0]->pid.setP(driveVelocityPDebug); - velocityPIDs[RF][0]->pid.setI(driveVelocityIDebug); - velocityPIDs[RF][0]->pid.setD(driveVelocityDDebug); - velocityPIDs[RF][0]->pid.reset(); - - velocityPIDs[LF][0]->pid.setP(driveVelocityPDebug); - velocityPIDs[LF][0]->pid.setI(driveVelocityIDebug); - velocityPIDs[LF][0]->pid.setD(driveVelocityDDebug); - velocityPIDs[LF][0]->pid.reset(); - - updateDriveVelocityPIDsDebug = false; + xDisplay = x; + yDisplay = y; + rotDisplay = r; + + updateSteeringVelocityPIDsDebug = false; + + maxWheelSpeedDisplay = maxWheelSpeed; + lockWatch = false; + + // deadband system to prevent chassis from updating when x,y and r are less than a specified value + // tune these to prevent yaw motors from always being in "rotation mode" due to chassis follow gimbal pid + auto deadband = [](float v, float eps) { return (std::fabs(v) < eps) ? 0.0f : v; }; + static constexpr float kXYDeadband = 0.02f; + static constexpr float kRDeadband = 70.0f; + x = deadband(x, kXYDeadband); + y = deadband(y, kXYDeadband); + r = deadband(r, kRDeadband); + + // chassis geometry + const float halfW = WHEELBASE_WIDTH * 0.5f; + const float halfL = WHEELBASE_LENGTH * 0.5f; + const float wheelbaseCenterDist = std::sqrt(halfW * halfW + halfL * halfL); + + // rotational contribution about center + const float rx = r * (WHEELBASE_LENGTH / wheelbaseCenterDist); + const float ry = r * (WHEELBASE_WIDTH / wheelbaseCenterDist); + const float a = -x + rx; + const float b = -x - rx; + const float c = y - ry; + const float d = y + ry; + + // yaw values in radians + left_front_yaw_actual = yawToRad(motors[LF][1]->getEncoderWrapped(), LEFT_FRONT_YAW_OFFSET); + right_front_yaw_actual = yawToRad(motors[RF][1]->getEncoderWrapped(), RIGHT_FRONT_YAW_OFFSET); + left_back_yaw_actual = yawToRad(motors[LB][1]->getEncoderWrapped(), LEFT_BACK_YAW_OFFSET); + right_back_yaw_actual = yawToRad(motors[RB][1]->getEncoderWrapped(), RIGHT_BACK_YAW_OFFSET); + + watchLFYaw = motors[LF][1]->getEncoderUnwrapped(); + watchRFYaw = motors[RF][1]->getEncoderUnwrapped(); + watchLBYaw = motors[LB][1]->getEncoderUnwrapped(); + watchRBYaw = motors[RB][1]->getEncoderUnwrapped(); + + // last angle input is stored here and only gets updated when a new value is greater than the deadband + static bool lastYawInit[4] = {false, false, false, false}; + static float lastCmdYawRad[4] = {0, 0, 0, 0}; + + auto ensureInit = [&](int idx, float actual) { + if (!lastYawInit[idx]) { + lastCmdYawRad[idx] = wrap0To2Pi(actual); + lastYawInit[idx] = true; } - - if (updateSteeringVelocityPIDsDebug) { - velocityPIDs[LB][1]->pid.setP(steeringVelocityPDebug); - velocityPIDs[LB][1]->pid.setI(steeringVelocityIDebug); - velocityPIDs[LB][1]->pid.setD(steeringVelocityDDebug); - velocityPIDs[LB][1]->pid.reset(); - - velocityPIDs[RB][1]->pid.setP(steeringVelocityPDebug); - velocityPIDs[RB][1]->pid.setI(steeringVelocityIDebug); - velocityPIDs[RB][1]->pid.setD(steeringVelocityDDebug); - velocityPIDs[RB][1]->pid.reset(); - - velocityPIDs[RF][1]->pid.setP(steeringVelocityPDebug); - velocityPIDs[RF][1]->pid.setI(steeringVelocityIDebug); - velocityPIDs[RF][1]->pid.setD(steeringVelocityDDebug); - velocityPIDs[RF][1]->pid.reset(); - - velocityPIDs[LF][1]->pid.setP(steeringVelocityPDebug); - velocityPIDs[LF][1]->pid.setI(steeringVelocityIDebug); - velocityPIDs[LF][1]->pid.setD(steeringVelocityDDebug); - velocityPIDs[LF][1]->pid.reset(); - - updateSteeringVelocityPIDsDebug = false; + }; + ensureInit(LF, left_front_yaw_actual); + ensureInit(RF, right_front_yaw_actual); + ensureInit(LB, left_back_yaw_actual); + ensureInit(RB, right_back_yaw_actual); + + // holdAngles = true when x,y and r is close to 0 + static constexpr float kModuleVecEps = 1e-3f; + const bool noTranslate = (std::hypot(x, y) < 1e-4f); + const bool noRotate = (std::fabs(r) < 1e-4f); + const bool holdAngles = noTranslate && noRotate; + + // FRC implementation of swerve module computation + auto computeModule = [&](float vx, float vy, + int offsetTicks, + float currYawRad, + int moduleIdx, + float& outDriveRPM, + float& outSteerTicksFloat, + int& outSteerDbgInt) { + float targetYawRad; + + if (holdAngles) { + // set drive to last valid command above deadband limits + targetYawRad = lastCmdYawRad[moduleIdx]; + outDriveRPM = 0.0f; + } else { + const float mag = std::hypot(vx, vy); + outDriveRPM = limitVal(mag, -maxWheelSpeed, maxWheelSpeed); + + if (mag < kModuleVecEps) { + // Rare single-module degeneracy: hold its last angle + targetYawRad = lastCmdYawRad[moduleIdx]; + outDriveRPM = 0.0f; + } else { + targetYawRad = std::atan2(vy, vx) + 1.5f * static_cast(M_PI); + optimizeSwerve(outDriveRPM, targetYawRad, currYawRad); + lastCmdYawRad[moduleIdx] = targetYawRad; // update latch + } } - maxWheelSpeedDisplay = maxWheelSpeed; - - lockWatch = false; - float wheelbaseCenterDist = sqrtf(powf(WHEELBASE_WIDTH / 2.0f, 2.0f) + powf(WHEELBASE_LENGTH / 2.0f, 2.0f)); - - float a = -x + r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float b = -x - r * (WHEELBASE_LENGTH / wheelbaseCenterDist); - float c = y - r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - float d = y + r * (WHEELBASE_WIDTH / wheelbaseCenterDist); - - targetRPMs[LF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_front_yaw_actual = yawToRad(motors[LF][1]->getEncoderWrapped(), LEFT_FRONT_YAW_OFFSET); - watchLFYaw = motors[LF][1]->getEncoderUnwrapped(); - target_left_front_yaw = (atan2f(d, b) + 3 * M_PI / 2); - prev_left_front_yaw = left_front_yaw_actual; - optimizeSwerve(targetRPMs[LF][0],target_left_front_yaw, left_front_yaw_actual); - left_front_yaw = target_left_front_yaw * (180 / M_PI) / 360 * 8191 + LEFT_FRONT_YAW_OFFSET; - targetRPMs[LF][1] = left_front_yaw % 8191; - left_front_yaw_db = targetRPMs[LF][1]; - - targetRPMs[RF][0] = limitVal(sqrtf(powf(b, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - watchRFYaw = motors[RF][1]->getEncoderUnwrapped(); - right_front_yaw_actual = yawToRad(motors[RF][1]->getEncoderWrapped(), RIGHT_FRONT_YAW_OFFSET); - target_right_front_yaw = (atan2f(c, b) + 3 * M_PI / 2); - prev_right_front_yaw = right_front_yaw_actual; - optimizeSwerve(targetRPMs[RF][0],target_right_front_yaw, right_front_yaw_actual); - right_front_yaw = target_right_front_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_FRONT_YAW_OFFSET; - targetRPMs[RF][1] = right_front_yaw % 8191; - right_front_yaw_db = targetRPMs[RF][1]; - - targetRPMs[LB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(d, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - left_back_yaw_actual = yawToRad(motors[LB][1]->getEncoderWrapped(),LEFT_BACK_YAW_OFFSET); - watchLBYaw = motors[LB][1]->getEncoderUnwrapped(); - target_left_back_yaw = atan2f(d, a) + 3 * M_PI / 2; - prev_left_back_yaw = left_back_yaw_actual; - optimizeSwerve(targetRPMs[LB][0],target_left_back_yaw, left_back_yaw_actual); - left_back_yaw = target_left_back_yaw * (180 / M_PI) / 360 * 8191 + LEFT_BACK_YAW_OFFSET; - targetRPMs[LB][1] = left_back_yaw % 8191; - left_back_yaw_db = targetRPMs[LB][1]; - - targetRPMs[RB][0] = limitVal(sqrtf(powf(a, 2.0f) + powf(c, 2.0f)), -maxWheelSpeed, maxWheelSpeed); - right_back_yaw_actual = yawToRad(motors[RB][1]->getEncoderWrapped(),RIGHT_BACK_YAW_OFFSET); - watchRBYaw = motors[RB][1]->getEncoderUnwrapped(); - target_right_back_yaw = atan2f(c, a) + 3 * M_PI / 2; - prev_right_back_yaw = right_back_yaw_actual; - optimizeSwerve(targetRPMs[RB][0],target_right_back_yaw,right_back_yaw_actual); - right_back_yaw = target_right_back_yaw * (180 / M_PI) / 360 * 8191 + RIGHT_BACK_YAW_OFFSET; - targetRPMs[RB][1] = right_back_yaw % 8191; - right_back_yaw_db = targetRPMs[RB][1]; - // wooo! just for commants - // } - wheelRPMDisplay = targetRPMs[LB][0]; + outSteerTicksFloat = radToTicksFloat(targetYawRad, offsetTicks); + outSteerDbgInt = static_cast(std::lround(outSteerTicksFloat)); + }; + + // LF + computeModule(b, d, LEFT_FRONT_YAW_OFFSET, left_front_yaw_actual, + LF, targetRPMs[LF][0], targetRPMs[LF][1], left_front_yaw_db); + + // RF + computeModule(b, c, RIGHT_FRONT_YAW_OFFSET, right_front_yaw_actual, + RF, targetRPMs[RF][0], targetRPMs[RF][1], right_front_yaw_db); + + // LB + computeModule(a, d, LEFT_BACK_YAW_OFFSET, left_back_yaw_actual, + LB, targetRPMs[LB][0], targetRPMs[LB][1], left_back_yaw_db); + + // RB + computeModule(a, c, RIGHT_BACK_YAW_OFFSET, right_back_yaw_actual, + RB, targetRPMs[RB][0], targetRPMs[RB][1], right_back_yaw_db); } + + #endif void ChassisSubsystem::calculateRail(float x, float maxWheelSpeed) { diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.hpp b/aimbots-src/src/subsystems/chassis/control/chassis.hpp index fd00a2e2..af7db72d 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.hpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.hpp @@ -98,7 +98,8 @@ class ChassisSubsystem : public tap::control::chassis::ChassisSubsystemInterface void calculateHolonomic(float x, float y, float r, float maxWheelSpeed); // normal 4wd mecanum robots #ifdef SWERVE //converts yaw encoder count to radians - float yawToRad(float targetYaw, int offset); + float yawToRad(float ticks, int offsetTicks); + float radToTicks(float radians, int offsetTicks); //finds the shortest path for the swerve drive to point in void optimizeSwerve(float& targetRPMDrive, float& targetYaw, float currYaw); //calculates targets for swerves yaw and pitch given the desired direction and speed From 9149cf1d4c0883bfaff9e086cf661eabf85b39d7 Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Tue, 30 Sep 2025 07:53:17 -0500 Subject: [PATCH 59/67] Yaw PIDs are tuned but slow --- .../informants/kinematics/kinematic_informant.cpp | 13 +++++++++---- .../sentry/constants/sentry_chassis_constants.hpp | 2 +- .../sentry/constants/sentry_gimbal_constants.hpp | 12 ++++++------ .../src/robots/sentry/sentry_control_interface.cpp | 2 +- .../src/subsystems/chassis/control/chassis.cpp | 13 ++++++++++++- .../control/gimbal_field_relative_controller.cpp | 5 +++++ 6 files changed, 34 insertions(+), 13 deletions(-) diff --git a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp index 4cdb6c1a..13b0146c 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -420,6 +420,7 @@ float chassisAnglesConvertedDisplayZ; float chassisAngularVelocitiesDisplayZ; float YawAngularVelocityEncoder; +float YawPosEncoder; Vector3f IMUAnglesDisplay; // individual components for debug purposes @@ -431,11 +432,13 @@ float rawYawVel; float IMUYawAngleDisplayGood; void KinematicInformant::updateChassisIMUAngles() { + YawPosEncoder = gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians); + YawAngularVelocityEncoder = (gimbalSubsystem->getYawMotorRPM(0) * (3.14159265358979323846 * 2.0) / 60.0f)/2.0f; + Vector3f IMUAngles = getLocalIMUAngles(); Vector3f IMUAngularVelocities = getIMUAngularVelocities(); - YawAngularVelocityEncoder = (gimbalSubsystem->getYawMotorRPM(0) * (3.14159265358979323846 * 2.0) / 60.0f)/2.0f; - Vector3f IMUAnglesYawSubtraction(IMUAngles.x,IMUAngles.y,IMUAngles.z - gimbalSubsystem->getCurrentYawAxisAngle(AngleUnit::Radians)); - Vector3f IMUAngularVelocitiesYawSubtraction(IMUAngularVelocities.x,IMUAngularVelocities.y,round((IMUAngularVelocities.z - YawAngularVelocityEncoder)*10000.0f)/10000.0f); + Vector3f IMUAnglesYawSubtraction(IMUAngles.x,IMUAngles.y,IMUAngles.z - YawPosEncoder); + Vector3f IMUAngularVelocitiesYawSubtraction(IMUAngularVelocities.x,IMUAngularVelocities.y,(IMUAngularVelocities.z - YawAngularVelocityEncoder)); IMUAnglesDisplay = IMUAnglesYawSubtraction; IMUAnglesDisplayX = IMUAnglesYawSubtraction.x; @@ -599,10 +602,12 @@ void KinematicInformant::updateGimbalAcceleration() { } float chassisYawAngleDisplay = 0.0f; +float fieldRelativeYawDisplay = 0.0f; tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat() { float currChassisAngle = getChassisIMUAngle(YAW_AXIS, AngleUnit::Radians); chassisYawAngleDisplay = currChassisAngle; - return WrappedFloat(getLocalIMUAngles().z - YAW_AXIS_START_ANGLE, -M_PI, M_PI); + fieldRelativeYawDisplay = getLocalIMUAngles().z; + return WrappedFloat(getLocalIMUAngles().z + YAW_AXIS_START_ANGLE, -M_PI, M_PI); } tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalPitchAngleAsWrappedFloat() { diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 34d3f847..2ffa0726 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -115,7 +115,7 @@ static constexpr float MIN_ROTATION_THRESHOLD = 800.0f; static constexpr float FOLLOW_GIMBAL_ANGLE_THRESHOLD = modm::toRadian(20.0f); static constexpr SmoothPIDConfig ROTATION_POSITION_PID_CONFIG = { - .kp = 0.5f, + .kp = 1.0f, .ki = 0.0f, .kd = 0.0f, .maxICumulative = 0.9f, diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 7c95d370..497f173a 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -28,7 +28,7 @@ static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(30.0f); #elif defined(TARGET_SENTRY_SWERVE) static constexpr CANBus YAW_GIMBAL_BUS = CANBus::CAN_BUS1; /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(19.5f))}; +static const std::array YAW_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(0.416577296)}; static const std::array PITCH_MOTOR_OFFSET_ANGLES = {wrapTo0To2PIRange(modm::toRadian(147.8f))}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(0.0f); @@ -65,7 +65,7 @@ static constexpr float GIMBAL_PITCH_GEAR_RATIO = (5.0f / 17.0f); // for 2023 Se */ static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { - .kp = 0'000.0f, + .kp = 0.0f, .ki = 0.0f, .kd = 0.0f, .maxICumulative = 0.0f, @@ -94,9 +94,9 @@ static constexpr SmoothPIDConfig PITCH_POSITION_PID_CONFIG = { // VISION PID CONSTANTS static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { - .kp = 10.0f, // 35 + .kp = 7.0f, // 35 .ki = 0.0f, - .kd = 0.1f, + .kd = 0.2f, .maxICumulative = 1000.0f, .maxOutput = 40.0f, // 40 rad/s is maximum speed of 6020 .tQDerivativeKalman = 1.0f, @@ -123,9 +123,9 @@ static constexpr SmoothPIDConfig PITCH_POSITION_CASCADE_PID_CONFIG = { // VELOCITY PID CONSTANTS static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { - .kp = 100.0f, + .kp = 400.0f, .ki = 0.0f, - .kd = 0.5f, + .kd = 0.0f, .maxICumulative = 2000.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, diff --git a/aimbots-src/src/robots/sentry/sentry_control_interface.cpp b/aimbots-src/src/robots/sentry/sentry_control_interface.cpp index cc0b3983..46e4b335 100644 --- a/aimbots-src/src/robots/sentry/sentry_control_interface.cpp +++ b/aimbots-src/src/robots/sentry/sentry_control_interface.cpp @@ -16,7 +16,7 @@ using namespace tap::algorithms; int8_t finalXWatch = 0; uint32_t timeCtr = 0; -static constexpr float YAW_JOYSTICK_INPUT_SENSITIVITY = 0.015f; +static constexpr float YAW_JOYSTICK_INPUT_SENSITIVITY = 0.008f; static constexpr float PITCH_JOYSTICK_INPUT_SENSITIVITY = 0.015f; static constexpr int16_t MOUSE_YAW_MAX = 1000; diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index ee55f0e0..89c69928 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -318,11 +318,17 @@ void ChassisSubsystem::optimizeSwerve(float& targetRPMDrive, float& targetYaw, f float wheelRPMDisplay = 0.0f; + uint16_t watchLFYaw = 0; uint16_t watchRFYaw = 0; uint16_t watchLBYaw = 0; uint16_t watchRBYaw = 0; +float watchLFYawTarget = 0; +float watchRFYawTarget = 0; +float watchLBYawTarget = 0; +float watchRBYawTarget = 0; + // uint16_t watchLFSpeed = 0; // for tunning PID system through Ozone @@ -357,7 +363,7 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel // tune these to prevent yaw motors from always being in "rotation mode" due to chassis follow gimbal pid auto deadband = [](float v, float eps) { return (std::fabs(v) < eps) ? 0.0f : v; }; static constexpr float kXYDeadband = 0.02f; - static constexpr float kRDeadband = 70.0f; + static constexpr float kRDeadband = 100.0f; x = deadband(x, kXYDeadband); y = deadband(y, kXYDeadband); r = deadband(r, kRDeadband); @@ -455,6 +461,11 @@ void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheel // RB computeModule(a, c, RIGHT_BACK_YAW_OFFSET, right_back_yaw_actual, RB, targetRPMs[RB][0], targetRPMs[RB][1], right_back_yaw_db); + + watchRBYawTarget = lastCmdYawRad[RB]; + watchLFYawTarget = lastCmdYawRad[LF]; + watchRFYawTarget = lastCmdYawRad[RF]; + watchLBYawTarget = lastCmdYawRad[LB]; } diff --git a/aimbots-src/src/subsystems/gimbal/control/gimbal_field_relative_controller.cpp b/aimbots-src/src/subsystems/gimbal/control/gimbal_field_relative_controller.cpp index db177ec9..37b2aae9 100644 --- a/aimbots-src/src/subsystems/gimbal/control/gimbal_field_relative_controller.cpp +++ b/aimbots-src/src/subsystems/gimbal/control/gimbal_field_relative_controller.cpp @@ -27,6 +27,7 @@ void GimbalFieldRelativeController::initialize() { float fieldRelativeYawTargetDisplay = 0.0f; float targetYawAxisAngleDisplay = 0.0f; +float fieldRelativeYawTargetRadDisplay = 0.0f; float fieldRelativeYawOutputDisplay = 0.0f; @@ -50,6 +51,7 @@ float pitchGimbalMotorPositionTargetDisplay = 0.0f; float yawAngleErrorDisplay = 0; float chassisRelativeYawTargetDisplay = 0; float kinematicYawAngleDisplay = 0; +float kinematicYawAngleRadDisplay = 0; float speedTarget = 0.0f; @@ -63,8 +65,11 @@ bool updateGimbalYawPositionCascadeDebug = false; void GimbalFieldRelativeController::runYawController( std::optional velocityLimit) { // using cascade controller for yaw + fieldRelativeYawTargetRadDisplay = this->getTargetYaw(AngleUnit::Radians); fieldRelativeYawTargetDisplay = this->getTargetYaw(AngleUnit::Degrees); + kinematicYawAngleRadDisplay = drivers->kinematicInformant.getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat().getWrappedValue(); + kinematicYawAngleDisplay = modm::toDegree(drivers->kinematicInformant.getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat().getWrappedValue()); From 40d8e54b5c13245a0d9c4d96ad03a732d7ace59e Mon Sep 17 00:00:00 2001 From: Kenny Zheng Date: Tue, 30 Sep 2025 22:17:09 -0500 Subject: [PATCH 60/67] 9/30/25 PID Test kd lowered, debug testing or smt --- .../constants/sentry_chassis_constants.hpp | 2 +- .../subsystems/chassis/control/chassis.cpp | 19 +++++++++++++++---- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 2ffa0726..b7d849ae 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -17,7 +17,7 @@ static constexpr MotorID RIGHT_BACK_YAW_ID = MotorID::MOTOR8; static constexpr SmoothPIDConfig CHASSIS_YAW_PID_CONFIG = { .kp = 30.0f, .ki = 0.0f, - .kd = 1.0f, + .kd = 0.0f, .maxICumulative = 0.9f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 89c69928..7c3ad53d 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -338,10 +338,10 @@ float driveVelocityIDebug = 0.0f; float driveVelocityDDebug = 0.0f; bool updateDriveVelocityPIDsDebug = false; -float steeringVelocityPDebug = 0.0f; -float steeringVelocityIDebug = 0.0f; -float steeringVelocityDDebug = 0.0f; -bool updateSteeringVelocityPIDsDebug = false; +float steeringPositionPDebug = 0.0f; +float steeringPositionIDebug = 0.0f; +float steeringPositionDDebug = 0.0f; +bool updateSteeringPositionPIDsDebug = false; float maxWheelSpeedDisplay = 0.0f; @@ -350,6 +350,17 @@ float yDisplay = 0.0f; float rotDisplay = 0.0f; void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { + if (updateSteeringVelocityPIDsDebug) { + for (size_t i = 0; i < 4; i++) { + velocityPIDs[i][1]->pid.setP(steeringPositionPDebug); + velocityPIDs[i][1]->pid.setI(steeringPositionIDebug); + velocityPIDs[i][1]->pid.setD(steeringPositionDDebug); + } + + + } + + xDisplay = x; yDisplay = y; rotDisplay = r; From c783947751fa31bcadc600d90a666ab6dd29125f Mon Sep 17 00:00:00 2001 From: zDagger Date: Fri, 3 Oct 2025 16:19:46 -0500 Subject: [PATCH 61/67] corrected sentry feeder consts --- .../sentry/constants/sentry_feeder_constants.hpp | 2 +- .../src/subsystems/chassis/control/chassis.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp index 5bfba6ac..0ac0d653 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp @@ -25,7 +25,7 @@ static constexpr uint8_t FEEDER_MOTOR_COUNT = 1; static const std::array FEEDER_MOTOR_IDS = {MotorID::MOTOR7}; static const std::array FEEDER_MOTOR_NAMES = {"Feeder Motor 1"}; -static constexpr float PROJECTILES_PER_FEEDER_ROTATION = 10; +static constexpr float PROJECTILES_PER_FEEDER_ROTATION = 22; static constexpr std::array FEEDER_GEAR_RATIOS = {36}; static const std::array FEEDER_NORMAL_RPMS = {2550.0f}; static const std::array FEEDER_UNJAM_RPMS = {3000}; // Absolute values diff --git a/aimbots-src/src/subsystems/chassis/control/chassis.cpp b/aimbots-src/src/subsystems/chassis/control/chassis.cpp index 7c3ad53d..f148bdee 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.cpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.cpp @@ -350,22 +350,22 @@ float yDisplay = 0.0f; float rotDisplay = 0.0f; void ChassisSubsystem::calculateSwerve(float x, float y, float r, float maxWheelSpeed) { - if (updateSteeringVelocityPIDsDebug) { - for (size_t i = 0; i < 4; i++) { - velocityPIDs[i][1]->pid.setP(steeringPositionPDebug); - velocityPIDs[i][1]->pid.setI(steeringPositionIDebug); - velocityPIDs[i][1]->pid.setD(steeringPositionDDebug); - } + // if (updateSteeringPositioPIDsDebug) { + // for (size_t i = 0; i < 4; i++) { + // velocityPIDs[i][1]->pid.setP(steeringPositionPDebug); + // velocityPIDs[i][1]->pid.setI(steeringPositionIDebug); + // velocityPIDs[i][1]->pid.setD(steeringPositionDDebug); + // } - } + // } xDisplay = x; yDisplay = y; rotDisplay = r; - updateSteeringVelocityPIDsDebug = false; + // updateSteeringVelocityPIDsDebug = false; maxWheelSpeedDisplay = maxWheelSpeed; lockWatch = false; From ca671e2b376f5c1262b9349974148bd5842373e0 Mon Sep 17 00:00:00 2001 From: TechSheepy Date: Sun, 5 Oct 2025 00:28:37 -0500 Subject: [PATCH 62/67] tuned tokyo and added chassis ignore gimbal command --- .../constants/sentry_chassis_constants.hpp | 14 --- .../constants/sentry_gimbal_constants.hpp | 8 +- .../src/robots/sentry/sentry_control.cpp | 24 +++-- .../chassis_ignore_gimbal_command.cpp | 86 +++++++++++++++++ .../chassis_ignore_gimbal_command.hpp | 47 ++++++++++ ...sis_toggle_drive_ignore_gimbal_command.cpp | 92 +++++++++++++++++++ ...sis_toggle_drive_ignore_gimbal_command.hpp | 53 +++++++++++ 7 files changed, 300 insertions(+), 24 deletions(-) create mode 100644 aimbots-src/src/subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.cpp create mode 100644 aimbots-src/src/subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.hpp create mode 100644 aimbots-src/src/subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.cpp create mode 100644 aimbots-src/src/subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.hpp diff --git a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index 2ffa0726..cdf7d6bb 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp @@ -128,20 +128,6 @@ static constexpr SmoothPIDConfig ROTATION_POSITION_PID_CONFIG = { .errorDerivativeFloor = 0.0f, }; -/** - * @brief TOKYO CONSTANTS - */ -// Fraction that user input is multiplied by when "drifting" -static constexpr float TOKYO_TRANSLATIONAL_SPEED_MULTIPLIER = 0.7f; -// Fraction of the maximum translation speed for when rotation speed should be reduced -static constexpr float TOKYO_TRANSLATION_THRESHOLD_TO_DECREASE_ROTATION_SPEED = 0.5f; -// Fraction of max chassis speed applied to rotation speed -static constexpr float TOKYO_ROTATIONAL_SPEED_FRACTION_OF_MAX = 0.75f; -// Fraction to cut rotation speed by when the robot is "drifting" -static constexpr float TOKYO_ROTATIONAL_SPEED_MULTIPLIER_WHEN_TRANSLATING = 0.4f; -// Rotational speed increment per iteration to apply until rotation setpoint is reached -static constexpr float TOKYO_ROTATIONAL_SPEED_INCREMENT = 50.0f; // rpm - static constexpr float CHASSIS_START_ANGLE_WORLD = modm::toRadian(0.0f); // theta (about z axis) // Sentry Chassis Travel Waypoints diff --git a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index 497f173a..fcb950ef 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp @@ -94,9 +94,9 @@ static constexpr SmoothPIDConfig PITCH_POSITION_PID_CONFIG = { // VISION PID CONSTANTS static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { - .kp = 7.0f, // 35 + .kp = 25.0f, // 35 .ki = 0.0f, - .kd = 0.2f, + .kd = 0.15f, .maxICumulative = 1000.0f, .maxOutput = 40.0f, // 40 rad/s is maximum speed of 6020 .tQDerivativeKalman = 1.0f, @@ -123,7 +123,7 @@ static constexpr SmoothPIDConfig PITCH_POSITION_CASCADE_PID_CONFIG = { // VELOCITY PID CONSTANTS static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { - .kp = 400.0f, + .kp = 500.0f, .ki = 0.0f, .kd = 0.0f, .maxICumulative = 2000.0f, @@ -132,7 +132,7 @@ static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.0f, + .errDeadzone = 0.1f, .errorDerivativeFloor = 0.0f, }; diff --git a/aimbots-src/src/robots/sentry/sentry_control.cpp b/aimbots-src/src/robots/sentry/sentry_control.cpp index d1eef407..8bba2611 100644 --- a/aimbots-src/src/robots/sentry/sentry_control.cpp +++ b/aimbots-src/src/robots/sentry/sentry_control.cpp @@ -21,6 +21,7 @@ #include "subsystems/chassis/basic_commands/chassis_manual_drive_command.hpp" #include "subsystems/chassis/basic_commands/chassis_tokyo_command.hpp" #include "subsystems/chassis/complex_commands/chassis_toggle_drive_command.hpp" +#include "subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.hpp" #include "subsystems/chassis/control/chassis.hpp" // #include "subsystems/feeder/basic_commands/dual_barrel_feeder_command.hpp" @@ -102,11 +103,11 @@ SnapSymmetryConfig defaultSnapConfig = { }; TokyoConfig defaultTokyoConfig = { - .translationalSpeedMultiplier = 0.6f, - .translationThresholdToDecreaseRotationSpeed = 0.5f, - .rotationalSpeedFractionOfMax = 0.75f, - .rotationalSpeedMultiplierWhenTranslating = 0.7f, - .rotationalSpeedIncrement = 50.0f, + .translationalSpeedMultiplier = 1.0f, + .translationThresholdToDecreaseRotationSpeed = 0.25f, + .rotationalSpeedFractionOfMax = 0.8f, + .rotationalSpeedMultiplierWhenTranslating = 0.2f, + .rotationalSpeedIncrement = 20.0f, }; SpinRandomizerConfig randomizerConfig = { @@ -158,6 +159,8 @@ SentryMatchGimbalControlCommand matchGimbalControlCommand( // Define commands here --------------------------------------------------- ChassisManualDriveCommand chassisManualDriveCommand(drivers(), &chassis); + +//chassis follow gimbal toggle command ChassisToggleDriveCommand chassisToggleDriveCommand( drivers(), &chassis, @@ -166,6 +169,15 @@ ChassisToggleDriveCommand chassisToggleDriveCommand( defaultTokyoConfig, false, randomizerConfig); + +//chassis ignore gimbal toggle command +ChassisToggleDriveIgnoreGimbalCommand chassisToggleDriveIgnoreGimbalCommand( + drivers(), + &chassis, + &gimbal, + defaultTokyoConfig, + false, + randomizerConfig); ChassisTokyoCommand chassisTokyoCommand(drivers(), &chassis, &gimbal, defaultTokyoConfig, 0, false, randomizerConfig); ChassisAutoNavCommand chassisAutoNavCommand(drivers(), &chassis, defaultLinearConfig, defaultRotationConfig); @@ -255,7 +267,7 @@ ToggleHopperCommand toggleHopperCommand(drivers(), &hopper, HOPPER_CLOSED_ANGLE, // Autonomous Match Control Switch Mapping ----------------------------- HoldCommandMapping leftSwitchMid( drivers(), - {/*&imuCalibrateCommand,*/ &chassisToggleDriveCommand, &gimbalFieldRelativeControlCommand}, + {/*&imuCalibrateCommand,*/ &chassisToggleDriveIgnoreGimbalCommand, &gimbalFieldRelativeControlCommand}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::MID)); HoldCommandMapping leftSwitchUp( diff --git a/aimbots-src/src/subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.cpp b/aimbots-src/src/subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.cpp new file mode 100644 index 00000000..d6006b67 --- /dev/null +++ b/aimbots-src/src/subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.cpp @@ -0,0 +1,86 @@ + +#include "chassis_ignore_gimbal_command.hpp" + +#ifdef GIMBAL_UNTETHERED +#ifdef CHASSIS_COMPATIBLE + +#include "subsystems/chassis/control/chassis_helper.hpp" + +namespace src::Chassis { + +ChassisIgnoreGimbalCommand::ChassisIgnoreGimbalCommand( + src::Drivers* drivers, + ChassisSubsystem* chassis, + src::Gimbal::GimbalSubsystem* gimbal) + : drivers(drivers), + chassis(chassis), + gimbal(gimbal), + rotationController(ROTATION_POSITION_PID_CONFIG) // +{ + addSubsystemRequirement(dynamic_cast(chassis)); +} + +void ChassisIgnoreGimbalCommand::initialize() {} + +// Debug variables +// float yawAngleFromChassisCenterDisplay2 = 0.0f; +// float chassisYawDisplay = 0.0f; +// float rotationControllerOutputDisplay = 0.0f; +// float rotationLimitedMaxTranslationalSpeedDisplay = 0.0f; + +// bool gimbalOnlineDisplay = false; +// float chassisErrorAngleDisplay = 0.0f; + +void ChassisIgnoreGimbalCommand::execute() { + chassis->setTokyoDrift(false); + float desiredX = 0.0f; + float desiredY = 0.0f; + float desiredRotation = 0.0f; + // gets desired user input from operator interface + Chassis::Helper::getUserDesiredInput(drivers, chassis, &desiredX, &desiredY, &desiredRotation); + + //gimbalOnlineDisplay = gimbal->isOnline(); + + if (gimbal->isOnline()) { + float yawAngleFromChassisCenter = gimbal->getCurrentYawAxisAngle(AngleUnit::Radians); + + float chassisErrorAngle = yawAngleFromChassisCenter; + + //chassisErrorAngleDisplay = chassisErrorAngle; + + // Find rotation correction power + rotationController.runController( + chassisErrorAngle, + -RADPS_TO_RPM(drivers->kinematicInformant.getChassisIMUAngularVelocity( + src::Informants::AngularAxis::YAW_AXIS, + AngleUnit::Radians))); + // rotationController.runControllerDerivateError(chassisErrorAngle); + //rotationControllerOutputDisplay = rotationController.getOutput(); + + // overwrite desired rotation with rotation controller output, range [-1, 1] + desiredRotation = rotationController.getOutput(); + + Chassis::Helper::rescaleDesiredInputToPowerLimitedSpeeds(drivers, chassis, &desiredX, &desiredY, &desiredRotation); + + tap::algorithms::rotateVector(&desiredX, &desiredY, yawAngleFromChassisCenter); + } else { // if the gimbal is offline, run the normal manual drive command + Chassis::Helper::rescaleDesiredInputToPowerLimitedSpeeds(drivers, chassis, &desiredX, &desiredY, &desiredRotation); + } + + //chassis does not care about rotation + chassis->setTargetRPMs(desiredX, desiredY, 0.0f); +} + +void ChassisIgnoreGimbalCommand::end(bool interrupted) { + UNUSED(interrupted); + chassis->setTargetRPMs(0.0f, 0.0f, 0.0f); +} + +bool ChassisIgnoreGimbalCommand::isReady() { return true; } + +bool ChassisIgnoreGimbalCommand::isFinished() const { return false; } + +} // namespace src::Chassis +#endif //#ifdef CHASSIS_COMPATIBLE + +#endif //#ifdef GIMBAL_UNTETHERED \ No newline at end of file diff --git a/aimbots-src/src/subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.hpp b/aimbots-src/src/subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.hpp new file mode 100644 index 00000000..94f82d95 --- /dev/null +++ b/aimbots-src/src/subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include "tap/control/command.hpp" + +#include "subsystems/chassis/control/chassis.hpp" +#include "subsystems/gimbal/control/gimbal.hpp" +#include "utils/tools/common_types.hpp" + + +#include "subsystems/chassis/control/chassis_helper.hpp" +#include "drivers.hpp" + +#ifdef GIMBAL_UNTETHERED +#ifdef CHASSIS_COMPATIBLE + +namespace src::Chassis { + +// field relative chassis locking command +class ChassisIgnoreGimbalCommand : public TapCommand { +public: + ChassisIgnoreGimbalCommand( + src::Drivers*, + ChassisSubsystem*, + src::Gimbal::GimbalSubsystem*); + void initialize() override; + + void execute() override; + void end(bool interrupted) override; + bool isReady() override; + + bool isFinished() const override; + + const char* getName() const override { return "Chassis Ignore Gimbal"; } + +private: + src::Drivers* drivers; + ChassisSubsystem* chassis; + src::Gimbal::GimbalSubsystem* gimbal; + + SmoothPID rotationController; +}; + +} // namespace src::Chassis + +#endif //#ifdef CHASSIS_COMPATIBLE + +#endif //#ifdef GIMBAL_UNTETHERED \ No newline at end of file diff --git a/aimbots-src/src/subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.cpp b/aimbots-src/src/subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.cpp new file mode 100644 index 00000000..73cb3aa6 --- /dev/null +++ b/aimbots-src/src/subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.cpp @@ -0,0 +1,92 @@ +#include "chassis_toggle_drive_ignore_gimbal_command.hpp" + +#ifdef CHASSIS_COMPATIBLE + +namespace src::Chassis { + +ChassisToggleDriveIgnoreGimbalCommand::ChassisToggleDriveIgnoreGimbalCommand( + src::Drivers* drivers, + ChassisSubsystem* chassis, + Gimbal::GimbalSubsystem* gimbal, + const TokyoConfig& tokyoConfig, + bool randomizeSpinRate, + const SpinRandomizerConfig& randomizerConfig) + : TapComprisedCommand(drivers), + drivers(drivers), + chassis(chassis), + ignoreGimbalCommand(drivers, chassis, gimbal), + tokyoCommand(drivers, chassis, gimbal, tokyoConfig, 0, randomizeSpinRate, randomizerConfig), + tokyoLeftCommand(drivers, chassis, gimbal, tokyoConfig, -1, randomizeSpinRate, randomizerConfig), + tokyoRightCommand(drivers, chassis, gimbal, tokyoConfig, 1, randomizeSpinRate, randomizerConfig) // +{ + addSubsystemRequirement(dynamic_cast(chassis)); + comprisedCommandScheduler.registerSubsystem(dynamic_cast(chassis)); +} + +void ChassisToggleDriveIgnoreGimbalCommand::initialize() { + // TODO: Logic is backwards maybe? + // if (!comprisedCommandScheduler.isCommandScheduled(&tokyoCommand)) + // comprisedCommandScheduler.removeCommand(&tokyoCommand, true); + scheduleIfNotScheduled(this->comprisedCommandScheduler, &ignoreGimbalCommand); + // if (!comprisedCommandScheduler.isCommandScheduled(&followGimbalCommand)) + // comprisedCommandScheduler.addCommand(&followGimbalCommand); + qPressed.restart(0); + ePressed.restart(0); +} + + +void ChassisToggleDriveIgnoreGimbalCommand::execute() { + // This needs to match the button in Gimbal Toggle Aiming! + if (drivers->remote.keyPressed(Remote::Key::F)) wasFPressed = true; + + if (drivers->remote.keyPressed(Remote::Key::E)) { + ePressed.restart(800); + } + if (drivers->remote.keyPressed(Remote::Key::Q)) { + qPressed.restart(800); + } + + if (wasFPressed && !drivers->remote.keyPressed(Remote::Key::F)) { + wasFPressed = false; + preferSpecificSpin = !ePressed.isExpired() || !qPressed.isExpired(); + + if (comprisedCommandScheduler.isCommandScheduled(&ignoreGimbalCommand)) { + if (preferSpecificSpin) { + if (!ePressed.isExpired()) { + scheduleIfNotScheduled(this->comprisedCommandScheduler, &tokyoRightCommand); + // qPressed.stop(); + // ePressed.stop(); + } + if (!qPressed.isExpired()) { + scheduleIfNotScheduled(this->comprisedCommandScheduler, &tokyoLeftCommand); + // qPressed.stop(); + // ePressed.stop(); + } + } else { + scheduleIfNotScheduled(this->comprisedCommandScheduler, &tokyoCommand); + } + } else if ( + comprisedCommandScheduler.isCommandScheduled(&tokyoCommand) || + comprisedCommandScheduler.isCommandScheduled(&tokyoLeftCommand) || + comprisedCommandScheduler.isCommandScheduled(&tokyoRightCommand)) { + comprisedCommandScheduler.addCommand(&ignoreGimbalCommand); + } + } + comprisedCommandScheduler.run(); +} + +void ChassisToggleDriveIgnoreGimbalCommand::end(bool interrupted) { + descheduleIfScheduled(this->comprisedCommandScheduler, &ignoreGimbalCommand, interrupted); + descheduleIfScheduled(this->comprisedCommandScheduler, &tokyoCommand, interrupted); + descheduleIfScheduled(this->comprisedCommandScheduler, &tokyoLeftCommand, interrupted); + descheduleIfScheduled(this->comprisedCommandScheduler, &tokyoRightCommand, interrupted); + chassis->setTargetRPMs(0.0f, 0.0f, 0.0f); +} + +bool ChassisToggleDriveIgnoreGimbalCommand::isReady() { return true; } + +bool ChassisToggleDriveIgnoreGimbalCommand::isFinished() const { return false; } + +} // namespace src::Chassis + +#endif //#ifdef CHASSIS_COMPATIBLE \ No newline at end of file diff --git a/aimbots-src/src/subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.hpp b/aimbots-src/src/subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.hpp new file mode 100644 index 00000000..ea3f4c60 --- /dev/null +++ b/aimbots-src/src/subsystems/chassis/complex_commands/chassis_toggle_drive_ignore_gimbal_command.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include "subsystems/chassis/control/chassis.hpp" +#include "subsystems/chassis/basic_commands/chassis_ignore_gimbal_command.hpp" +#include "subsystems/chassis/basic_commands/chassis_tokyo_command.hpp" +#include "subsystems/gimbal/control/gimbal.hpp" +#include "utils/tools/common_types.hpp" + +#include "subsystems/chassis/control/chassis_helper.hpp" +#include "drivers.hpp" + +#ifdef CHASSIS_COMPATIBLE + +namespace src::Chassis { + +class ChassisToggleDriveIgnoreGimbalCommand : public TapComprisedCommand { +public: + ChassisToggleDriveIgnoreGimbalCommand( + src::Drivers*, + ChassisSubsystem*, + Gimbal::GimbalSubsystem*, + const TokyoConfig& tokyoConfig = TokyoConfig(), + bool randomizeSpinRate = false, + const SpinRandomizerConfig& randomizerConfig = SpinRandomizerConfig()); + + void initialize() override; + void execute() override; + + void end(bool interupted) override; + bool isReady() override; + bool isFinished() const override; + + char const* getName() const override { return "Chassis Toggle Drive Ignore Gimbal Command"; } + +private: + src::Drivers* drivers; + ChassisSubsystem* chassis; + + ChassisIgnoreGimbalCommand ignoreGimbalCommand; + ChassisTokyoCommand tokyoCommand; + ChassisTokyoCommand tokyoLeftCommand; + ChassisTokyoCommand tokyoRightCommand; + bool wasFPressed = false; + + bool preferSpecificSpin = false; + + MilliTimeout qPressed; + MilliTimeout ePressed; +}; + +} // namespace src::Chassis + +#endif //#ifdef CHASSIS_COMPATIBLE \ No newline at end of file From 82974926fd58dce7a43c273d0983ab32171d6eba Mon Sep 17 00:00:00 2001 From: HRNAE <148918405+HRNAE@users.noreply.github.com> Date: Fri, 17 Oct 2025 17:06:24 -0500 Subject: [PATCH 63/67] Tuned Standard Shooter Velocity PID --- .../robots/standard/constants/standard_shooter_constants.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp index 4bc42533..d4f0440c 100644 --- a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp @@ -7,7 +7,7 @@ static constexpr uint8_t SHOOTER_MOTOR_COUNT = 2; static constexpr SmoothPIDConfig SHOOTER_VELOCITY_PID_CONFIG = { - .kp = 40.0f, + .kp = 15.0f, .ki = 0.10f, // 0.10f; .kd = 0.00f, .maxICumulative = 10.0f, From fddbc7ff8bbd8d28da6e49df84a7ddfca3e2ce8c Mon Sep 17 00:00:00 2001 From: jkvelagapudi Date: Fri, 17 Oct 2025 18:12:53 -0500 Subject: [PATCH 64/67] Sentry Shooter Speed Tuning --- aimbots-src/.vscode/settings.json | 5 +++++ .../robots/sentry/constants/sentry_shooter_constants.hpp | 9 ++++----- .../standard/constants/standard_shooter_constants.hpp | 1 + .../shooter/basic_commands/run_shooter_command.cpp | 1 - 4 files changed, 10 insertions(+), 6 deletions(-) create mode 100644 aimbots-src/.vscode/settings.json diff --git a/aimbots-src/.vscode/settings.json b/aimbots-src/.vscode/settings.json new file mode 100644 index 00000000..f2678ea4 --- /dev/null +++ b/aimbots-src/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "optional": "cpp" + } +} \ No newline at end of file diff --git a/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp index 08cf23bc..9dc3791c 100644 --- a/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp +++ b/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp @@ -9,7 +9,7 @@ static constexpr uint8_t SHOOTER_MOTOR_COUNT = 4; static constexpr SmoothPIDConfig SHOOTER_VELOCITY_PID_CONFIG = { .kp = 40.0f, .ki = 0.10f, - .kd = 0.00f, + .kd = 1.00f, .maxICumulative = 10.0f, .maxOutput = 30000.0f, .tQDerivativeKalman = 1.0f, @@ -22,10 +22,10 @@ static constexpr SmoothPIDConfig SHOOTER_VELOCITY_PID_CONFIG = { // clang-format off; // Sentry shoots at the speed of death -static constexpr uint16_t shooter_speed_array[2] = {30, 6254}; // {m/s, rpm} +static constexpr uint16_t shooter_speed_array[6] = {15, 4000, 18, 4580, 30, 9100}; // {m/s, rpm} // clang-format on -static const Matrix SHOOTER_SPEED_MATRIX(shooter_speed_array); +static const Matrix SHOOTER_SPEED_MATRIX(shooter_speed_array); static constexpr int DEFAULT_BURST_LENGTH = 10; // total balls in burst @@ -42,5 +42,4 @@ static constexpr bool SHOOTER_3_DIRECTION = false; static constexpr bool SHOOTER_4_DIRECTION = true; // This array holds the IDs of all speed monitor barrels on the robot -static const std::array BARREL_IDS = {BarrelID::TURRET_17MM_1, BarrelID::TURRET_17MM_2}; - +static const std::array BARREL_IDS = {BarrelID::TURRET_17MM_1, BarrelID::TURRET_17MM_2}; \ No newline at end of file diff --git a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp index 4bc42533..d87460ed 100644 --- a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp @@ -40,3 +40,4 @@ static constexpr MotorID SHOOTER_2_ID = MotorID::MOTOR4; static constexpr bool SHOOTER_1_DIRECTION = false; static constexpr bool SHOOTER_2_DIRECTION = true; + diff --git a/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp b/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp index 811324b2..7278acd0 100644 --- a/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp +++ b/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp @@ -43,7 +43,6 @@ void RunShooterCommand::execute() { // defaults to slowest usable speed for robot uint16_t flywheelRPM = SHOOTER_SPEED_MATRIX[0][1]; uint16_t refSpeedLimit = refHelper->getCurrBarrelProjectileSpeedLimit().value_or(/*SHOOTER_SPEED_MATRIX[0][0]*/ 5); - refSpeedLimitDisplay = refSpeedLimit; for (int i = 0; i < SHOOTER_SPEED_MATRIX.getNumberOfRows(); i++) { From 9e2301c1a2824d5bedb96af87e18ca80bf71585c Mon Sep 17 00:00:00 2001 From: HRNAE <148918405+HRNAE@users.noreply.github.com> Date: Fri, 17 Oct 2025 18:15:00 -0500 Subject: [PATCH 65/67] tuned shooter speed array --- .../constants/standard_shooter_constants.hpp | 6 +++--- .../basic_commands/run_shooter_command.cpp | 17 +++++++++-------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp index d4f0440c..4732547e 100644 --- a/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_shooter_constants.hpp @@ -23,11 +23,11 @@ static constexpr SmoothPIDConfig SHOOTER_VELOCITY_PID_CONFIG = { // clang-format off static constexpr uint16_t shooter_speed_array[6] = { // ONLY TUNE WITH FULL BATTERY 15, - 4250, // {ball m/s, flywheel rpm} + 4600, // {ball m/s, flywheel rpm} 18, - 4800, + 5100, 30, - 6204}; + 9000}; // clang-format on static const Matrix SHOOTER_SPEED_MATRIX(shooter_speed_array); diff --git a/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp b/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp index 811324b2..0caf5a8c 100644 --- a/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp +++ b/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp @@ -41,17 +41,18 @@ void RunShooterCommand::execute() { heatLimitDisplay = refHelper->getCurrBarrelLimit(); // defaults to slowest usable speed for robot - uint16_t flywheelRPM = SHOOTER_SPEED_MATRIX[0][1]; + uint16_t flywheelRPM = 5100; //SHOOTER_SPEED_MATRIX[0][1]; + uint16_t refSpeedLimit = refHelper->getCurrBarrelProjectileSpeedLimit().value_or(/*SHOOTER_SPEED_MATRIX[0][0]*/ 5); - refSpeedLimitDisplay = refSpeedLimit; + // refSpeedLimitDisplay = refSpeedLimit; - for (int i = 0; i < SHOOTER_SPEED_MATRIX.getNumberOfRows(); i++) { - if (SHOOTER_SPEED_MATRIX[i][0] == refSpeedLimit) { - flywheelRPM = SHOOTER_SPEED_MATRIX[i][1]; - break; - } - } + // for (int i = 0; i < SHOOTER_SPEED_MATRIX.getNumberOfRows(); i++) { + // if (SHOOTER_SPEED_MATRIX[i][0] == refSpeedLimit) { + // flywheelRPM = SHOOTER_SPEED_MATRIX[i][1]; + // break; + // } + // } flywheelRPMDisplay = flywheelRPM; flywheelCurrentRPMDisplay = shooter->getMotorSpeed(src::Shooter::MotorIndex::LEFT); From 07c620f41ce29363af86c73768f997022875f78b Mon Sep 17 00:00:00 2001 From: HRNAE <148918405+HRNAE@users.noreply.github.com> Date: Fri, 17 Oct 2025 18:17:01 -0500 Subject: [PATCH 66/67] Small fix on standard --- .../shooter/basic_commands/run_shooter_command.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp b/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp index 783474b9..c445d899 100644 --- a/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp +++ b/aimbots-src/src/subsystems/shooter/basic_commands/run_shooter_command.cpp @@ -41,18 +41,18 @@ void RunShooterCommand::execute() { heatLimitDisplay = refHelper->getCurrBarrelLimit(); // defaults to slowest usable speed for robot - uint16_t flywheelRPM = 5100; //SHOOTER_SPEED_MATRIX[0][1]; + uint16_t flywheelRPM = SHOOTER_SPEED_MATRIX[0][1]; uint16_t refSpeedLimit = refHelper->getCurrBarrelProjectileSpeedLimit().value_or(/*SHOOTER_SPEED_MATRIX[0][0]*/ 5); refSpeedLimitDisplay = refSpeedLimit; - // for (int i = 0; i < SHOOTER_SPEED_MATRIX.getNumberOfRows(); i++) { - // if (SHOOTER_SPEED_MATRIX[i][0] == refSpeedLimit) { - // flywheelRPM = SHOOTER_SPEED_MATRIX[i][1]; - // break; - // } - // } + for (int i = 0; i < SHOOTER_SPEED_MATRIX.getNumberOfRows(); i++) { + if (SHOOTER_SPEED_MATRIX[i][0] == refSpeedLimit) { + flywheelRPM = SHOOTER_SPEED_MATRIX[i][1]; + break; + } + } flywheelRPMDisplay = flywheelRPM; flywheelCurrentRPMDisplay = shooter->getMotorSpeed(src::Shooter::MotorIndex::LEFT); From debf433aabf6bddcef9a07791d0628c7cd6bcc08 Mon Sep 17 00:00:00 2001 From: HRNAE <148918405+HRNAE@users.noreply.github.com> Date: Fri, 24 Oct 2025 17:15:46 -0500 Subject: [PATCH 67/67] Co-authored-by: Jayadeep Velagapudi Co-authored-by: jprock23 --- .../robots/standard/constants/standard_gimbal_constants.hpp | 2 +- .../feeder/basic_commands/full_auto_feeder_command.cpp | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp index f295e84e..28a4bb0a 100644 --- a/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp +++ b/aimbots-src/src/robots/standard/constants/standard_gimbal_constants.hpp @@ -17,7 +17,7 @@ static constexpr uint8_t PITCH_MOTOR_COUNT = 1; #if defined(TARGET_STANDARD_BLASTOISE) /* What motor angles ensures that the barrel is pointing straight forward and level relative to the robot chassis? */ -static const std::array YAW_MOTOR_OFFSET_ANGLES = {modm::toRadian(144.27f)}; +static const std::array YAW_MOTOR_OFFSET_ANGLES = {modm::toRadian(144.93f)}; static const std::array PITCH_MOTOR_OFFSET_ANGLES = {modm::toRadian(11.11f)}; static constexpr float PITCH_AXIS_SOFTSTOP_LOW = modm::toRadian(-14.5f); static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(17.5f); diff --git a/aimbots-src/src/subsystems/feeder/basic_commands/full_auto_feeder_command.cpp b/aimbots-src/src/subsystems/feeder/basic_commands/full_auto_feeder_command.cpp index b69a49ca..890105f2 100644 --- a/aimbots-src/src/subsystems/feeder/basic_commands/full_auto_feeder_command.cpp +++ b/aimbots-src/src/subsystems/feeder/basic_commands/full_auto_feeder_command.cpp @@ -53,6 +53,8 @@ void FullAutoFeederCommand::initialize() { // uint16_t heatLimitDisplay = 0; float lastProjectileSpeedDisplay = 0.0f; +float overheat = 0.0f; + void FullAutoFeederCommand::execute() { isCommandRunningDisplay = true; @@ -60,6 +62,8 @@ void FullAutoFeederCommand::execute() { // overheat, set the RPM to 0, otherwise run as normal currentRotationDisplay = feeder->getEncoderUnwrapped(0); + overheat = feeder->getEncoderUnwrapped(); + if (/*false && */ feeder->getEncoderUnwrapped() >= antiOverheatEncoderThreshold) { feeder->ForFeederMotorGroup(ALL, &FeederSubsystem::deactivateFeederMotor); } else {