diff --git a/.vscode/settings.json b/.vscode/settings.json index 749376cb..7985f7a4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -112,7 +112,19 @@ "strstream": "cpp", "cfenv": "cpp", "typeindex": "cpp", - "*.ipp": "cpp" + "*.ipp": "cpp", + "xlocmes": "cpp", + "xlocmon": "cpp", + "xlocnum": "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/.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/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/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 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/drivers.hpp b/aimbots-src/src/drivers.hpp index bd538bf6..2590c9d7 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/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 d52bacb7..13b0146c 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.cpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.cpp @@ -118,8 +118,12 @@ float KinematicInformant::getIMULinearAcceleration(LinearAxis axis) { // Gets I return 0; } +#ifdef CHASSIS_IMU + Vector3f chassisAnglesConvertedDisplay; Vector3f IMUAnglesDisplay; + + void KinematicInformant::updateChassisIMUAngles() { Vector3f IMUAngles = getLocalIMUAngles(); Vector3f IMUAngularVelocities = getIMUAngularVelocities(); @@ -154,19 +158,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); } @@ -245,6 +249,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); @@ -321,8 +349,10 @@ void KinematicInformant::updateRobotFrames() { #ifndef TARGET_TURRET // Update Chassis Stuff after IMU STUFF + #ifdef CHASSIS_IMU // old shi updateChassisIMUAngles(); updateChassisAcceleration(); +#endif chassisIMUHistoryBuffer.prependOverwrite( {getChassisIMUAngle(PITCH_AXIS, AngleUnit::Radians), @@ -377,4 +407,302 @@ void KinematicInformant::mirrorPastRobotFrame(uint32_t frameDelay_ms) { turretFrames.mirrorPastCameraFrame(gimbalFieldAngles.first, gimbalFieldAngles.second, AngleUnit::Radians); } +#endif + +//turret IMU stuff --------------------------------------------------------------------------------------------------------- +#ifdef TURRET_IMU + +Vector3f chassisAnglesConvertedDisplay; +// individual components for debug purposes +float chassisAnglesConvertedDisplayX; +float chassisAnglesConvertedDisplayY; +float chassisAnglesConvertedDisplayZ; +float chassisAngularVelocitiesDisplayZ; + +float YawAngularVelocityEncoder; +float YawPosEncoder; + +Vector3f IMUAnglesDisplay; +// individual components for debug purposes +float IMUAnglesDisplayX; +float IMUAnglesDisplayY; +float IMUAnglesDisplayZ; +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(); + Vector3f IMUAnglesYawSubtraction(IMUAngles.x,IMUAngles.y,IMUAngles.z - YawPosEncoder); + Vector3f IMUAngularVelocitiesYawSubtraction(IMUAngularVelocities.x,IMUAngularVelocities.y,(IMUAngularVelocities.z - YawAngularVelocityEncoder)); + + IMUAnglesDisplay = IMUAnglesYawSubtraction; + IMUAnglesDisplayX = IMUAnglesYawSubtraction.x; + IMUAnglesDisplayY = IMUAnglesYawSubtraction.y; + IMUAnglesDisplayZ = IMUAnglesYawSubtraction.z; + + IMUYawAngleDisplayGood = getLocalIMUAngles().z; + + // this transform doesn't exist yet + + // 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 = IMUAnglesYawSubtraction; + + 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); + + rawYawVel = getIMUAngularVelocities().getZ(); + Vector3f chassisAngularVelocities = IMUAngularVelocitiesYawSubtraction; + chassisAngularVelocitiesDisplayZ = IMUAngularVelocitiesYawSubtraction.z; + + 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; +float fieldRelativeYawDisplay = 0.0f; +tap::algorithms::WrappedFloat KinematicInformant::getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat() { + float currChassisAngle = getChassisIMUAngle(YAW_AXIS, AngleUnit::Radians); + chassisYawAngleDisplay = currChassisAngle; + fieldRelativeYawDisplay = getLocalIMUAngles().z; + return WrappedFloat(getLocalIMUAngles().z + 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() { + + return -getLocalIMUAngles().getX(); +} + +float KinematicInformant::getChassisPitchVelocityInGimbalDirection() { + + return -getIMUAngularVelocities().getX(); +} + +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(); + + + 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; +} + +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 + } // 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 fe0a374c..8b0cfcb1 100644 --- a/aimbots-src/src/informants/kinematics/kinematic_informant.hpp +++ b/aimbots-src/src/informants/kinematics/kinematic_informant.hpp @@ -75,25 +75,40 @@ class KinematicInformant { void updateIMUKinematicStateVector(); + void updateGimbalIMUAngles(); + void updateChassisIMUAngles(); // 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 void updateRobotFrames(); tap::algorithms::WrappedFloat getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat(); tap::algorithms::WrappedFloat getCurrentFieldRelativeGimbalPitchAngleAsWrappedFloat(); + tap::algorithms::WrappedFloat getCurrentFieldRelativeChassisYawAngleAsWrappedFloat(); + tap::algorithms::WrappedFloat getCurrentFieldRelativeChassisPitchAngleAsWrappedFloat(); + + // rad float getChassisPitchAngleInGimbalDirection(); // rad/s @@ -108,7 +123,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. @@ -149,10 +168,18 @@ class KinematicInformant { KinematicStateVector chassisLinearYState; KinematicStateVector chassisLinearZState; + KinematicStateVector gimbalLinearXState; + KinematicStateVector gimbalLinearYState; + KinematicStateVector gimbalLinearZState; + KinematicStateVector chassisAngularXState; KinematicStateVector chassisAngularYState; KinematicStateVector chassisAngularZState; + KinematicStateVector gimbalAngularXState; + KinematicStateVector gimbalAngularYState; + KinematicStateVector gimbalAngularZState; + KinematicStateVector turretIMULinearXState; KinematicStateVector turretIMULinearYState; KinematicStateVector turretIMULinearZState; @@ -172,11 +199,22 @@ class KinematicInformant { chassisLinearXState, chassisLinearYState, chassisLinearZState}; + + modm::Vector gimbalLinearState = { + gimbalLinearXState, + gimbalLinearYState, + gimbalLinearZState}; + modm::Vector chassisAngularState = { chassisAngularXState, chassisAngularYState, chassisAngularZState}; + modm::Vector gimbalAngularState = { + gimbalAngularXState, + gimbalAngularYState, + gimbalAngularZState}; + modm::Vector turretIMULinearState = { turretIMULinearXState, turretIMULinearYState, @@ -187,6 +225,7 @@ class KinematicInformant { turretIMUAngularZState}; src::Informants::Odometry::ChassisKFOdometry chassisKFOdometry; + // src::Informants::Odometry::GimbalKFOdometry gimbalKFOdometry; }; } // namespace src::Informants 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/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/main.cpp b/aimbots-src/src/main.cpp index 935f139a..40d85531 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 @@ -148,6 +149,7 @@ static void initializeIo(src::Drivers *drivers) { drivers->refSerial.initialize(); // drivers->magnetometer.init(); drivers->cvCommunicator.initialize(); + // drivers->powerCommunicator.initialize(); drivers->kinematicInformant.recalibrateIMU( {CIMU_CALIBRATION_EULER_X, CIMU_CALIBRATION_EULER_Y, CIMU_CALIBRATION_EULER_Z}); #else @@ -182,8 +184,8 @@ static void updateIo(src::Drivers *drivers) { drivers->canRxHandler.pollCanData(); // should probably also be updating for turret imu?? drivers->refSerial.updateSerial(); drivers->remote.read(); - drivers->cvCommunicator.updateSerial(); + // drivers->powerCommunicator.updateSerial(); #else drivers->turretCommunicator.sendIMUData(); #endif @@ -216,6 +218,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/robots/aerial/constants/aerial_chassis_constants.hpp b/aimbots-src/src/robots/aerial/constants/aerial_chassis_constants.hpp index 33d663c0..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,6 +3,7 @@ #include "utils/math/matrix_helpers.hpp" #define CHASSIS_COMPATIBLE +// #define CHASSIS_IMU // THE GREAT RETURN (tbd) 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 4a38a597..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; @@ -22,7 +23,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 +39,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_feeder_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp index 44d07c81..2e08e52f 100644 --- a/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp +++ b/aimbots-src/src/robots/hero/constants/hero_feeder_constants.hpp @@ -6,10 +6,10 @@ static constexpr uint8_t FEEDER_MOTOR_COUNT = 2; -static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG = { - .kp = 20.0f, +static constexpr SmoothPIDConfig FEEDER_VELOCITY_PID_CONFIG [FEEDER_MOTOR_COUNT] = {{ + .kp = 60.0f, .ki = 0.0f, - .kd = 0.8f, + .kd = 2.0f, .maxICumulative = 10.0f, .maxOutput = M3508_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, @@ -18,9 +18,23 @@ 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 = 100; +static constexpr int UNJAM_TIMER_MS = 200; static constexpr float FEEDER_DEFAULT_RPM = 12000.0f; @@ -29,9 +43,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_general_constants.hpp b/aimbots-src/src/robots/hero/constants/hero_general_constants.hpp index a1b71487..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 }; @@ -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 6980f8f8..439c762a 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,19 @@ 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}; +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(63.37f))}; 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.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. */ @@ -41,15 +41,15 @@ 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 /** * @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..25b59f3d 100644 --- a/aimbots-src/src/robots/hero/hero_control.cpp +++ b/aimbots-src/src/robots/hero/hero_control.cpp @@ -222,22 +222,22 @@ 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); 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/robots/sentry/constants/sentry_chassis_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_chassis_constants.hpp index d2fd2728..c87d48c4 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,47 @@ #pragma once -#include "utils/tools/common_types.hpp" #include "utils/math/matrix_helpers.hpp" +#include "utils/tools/common_types.hpp" #define CHASSIS_COMPATIBLE +#define TURRET_IMU // IMU migration and whatnot + +#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 = 30.0f, + .ki = 0.0f, + .kd = 0.0f, + .maxICumulative = 0.9f, + .maxOutput = GM6020_MAX_OUTPUT, + .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 = 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) +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; @@ -30,27 +63,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 = 2; +static constexpr uint8_t CHASSIS_SNAP_POSITIONS = 4; // CAN Bus 2 static constexpr CANBus CHASSIS_BUS = CANBus::CAN_BUS2; static constexpr MotorID LEFT_BACK_WHEEL_ID = MotorID::MOTOR1; -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 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; @@ -64,7 +97,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); @@ -82,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 = 1.0f, .ki = 0.0f, - .kd = 0.005f, + .kd = 0.0f, .maxICumulative = 0.9f, .maxOutput = 1.0f, .tQDerivativeKalman = 1.0f, @@ -95,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.6f; -// 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; -// 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_feeder_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_feeder_constants.hpp index de7720da..0ac0d653 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, @@ -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/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/robots/sentry/constants/sentry_gimbal_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_gimbal_constants.hpp index be327d88..fcb950ef 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_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 + +#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(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); +static constexpr float PITCH_AXIS_SOFTSTOP_HIGH = modm::toRadian(40.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_IDS = {MotorID::MOTOR8};//TODO 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 @@ -28,30 +48,26 @@ 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 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 +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 */ -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 + * */ + static constexpr SmoothPIDConfig YAW_POSITION_PID_CONFIG = { - .kp = 20'000.0f, + .kp = 0.0f, .ki = 0.0f, - .kd = 10.0f, + .kd = 0.0f, .maxICumulative = 0.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, @@ -78,9 +94,9 @@ static constexpr SmoothPIDConfig PITCH_POSITION_PID_CONFIG = { // VISION PID CONSTANTS static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { - .kp = 20.0f, // 35 + .kp = 25.0f, // 35 .ki = 0.0f, - .kd = 0.0f, + .kd = 0.15f, .maxICumulative = 1000.0f, .maxOutput = 40.0f, // 40 rad/s is maximum speed of 6020 .tQDerivativeKalman = 1.0f, @@ -94,7 +110,7 @@ static constexpr SmoothPIDConfig YAW_POSITION_CASCADE_PID_CONFIG = { 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, @@ -107,16 +123,16 @@ static constexpr SmoothPIDConfig PITCH_POSITION_CASCADE_PID_CONFIG = { // VELOCITY PID CONSTANTS static constexpr SmoothPIDConfig YAW_VELOCITY_PID_CONFIG = { - .kp = 2200.0f, - .ki = 25.0f, - .kd = 0.0f, + .kp = 500.0f, + .ki = 0.0f, + .kd = 0.0f, .maxICumulative = 2000.0f, .maxOutput = GM6020_MAX_OUTPUT, .tQDerivativeKalman = 1.0f, .tRDerivativeKalman = 1.0f, .tQProportionalKalman = 1.0f, .tRProportionalKalman = 1.0f, - .errDeadzone = 0.0f, + .errDeadzone = 0.1f, .errorDerivativeFloor = 0.0f, }; @@ -138,33 +154,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/constants/sentry_shooter_constants.hpp b/aimbots-src/src/robots/sentry/constants/sentry_shooter_constants.hpp index 0737f490..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, 7450}; // {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 @@ -36,11 +36,10 @@ 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; // 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/sentry/sentry_control.cpp b/aimbots-src/src/robots/sentry/sentry_control.cpp index 826b71f8..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,12 +267,13 @@ 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( drivers(), - {/*&chassisTokyoCommand,*/ &matchChassisControlCommand, &matchGimbalControlCommand, &matchFiringControlCommand + //{/*&chassisTokyoCommand,*/ &matchChassisControlCommand, &matchGimbalControlCommand, &matchFiringControlCommand + {&chassisTokyoCommand, &gimbalToggleAimCommand /*&gimbalChaseCommand*/}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP)); @@ -338,4 +351,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/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/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; 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..28a4bb0a 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; @@ -16,18 +17,20 @@ 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.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); +static const std::array PITCH_MOTOR_DIRECTIONS = {false}; // 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))}; +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); +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, }; 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..1fee0894 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, @@ -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} + 4600, // {ball m/s, flywheel rpm} 18, - 4850, + 5100, 30, - 7150}; + 9000}; // clang-format on static const Matrix SHOOTER_SPEED_MATRIX(shooter_speed_array); @@ -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/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 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..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 @@ -1,6 +1,6 @@ #include "sentry_match_chassis_control_command.hpp" -#ifdef TARGET_SENTRY +#ifdef ALL_SENTRIES #ifdef CHASSIS_COMPATIBLE namespace src::Chassis { @@ -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); 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..f148bdee 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()), @@ -101,6 +101,10 @@ int refSerialWorkingDisplay = 0; uint16_t chassisPowerLimitDisplay = 0; float motorOutputDisplay = 0.0f; +float yawMotorOutputDisplayRF = 0.0f; +float yawMotorOutputDisplayRB = 0.0f; +float yawMotorOutputDisplayLF = 0.0f; +float yawMotorOutputDisplayLB = 0.0f; void ChassisSubsystem::refresh() { ForAllChassisMotors(&ChassisSubsystem::updateMotorVelocityPID); @@ -109,8 +113,14 @@ void ChassisSubsystem::refresh() { limitChassisPower(); - motorOutputDisplay = motors[RB][0]->getOutputDesired(); -} +// 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(); @@ -190,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; @@ -215,14 +230,19 @@ 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 #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; @@ -234,45 +254,232 @@ 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 prev_left_front_yaw; +float prev_right_front_yaw; +float prev_left_back_yaw; +float prev_right_back_yaw; + +bool lockWatch; + +static inline float wrap0N(float x, float N) { + float y = std::fmod(x, N); + if (y < 0) y += N; + return y; +} + +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 += (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; +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 + +float driveVelocityPDebug = 0.0f; +float driveVelocityIDebug = 0.0f; +float driveVelocityDDebug = 0.0f; +bool updateDriveVelocityPIDsDebug = false; + +float steeringPositionPDebug = 0.0f; +float steeringPositionIDebug = 0.0f; +float steeringPositionDDebug = 0.0f; +bool updateSteeringPositionPIDsDebug = 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) { - // float theta = fieldRelativeInformant->getYaw(); - // 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 (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; + + 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 = 100.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; + } + }; + 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 + } + } + + 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); + + watchRBYawTarget = lastCmdYawRad[RB]; + watchLFYawTarget = lastCmdYawRad[LF]; + watchRFYawTarget = lastCmdYawRad[RF]; + watchLBYawTarget = lastCmdYawRad[LB]; } + + #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 29a3ce1a..af7db72d 100644 --- a/aimbots-src/src/subsystems/chassis/control/chassis.hpp +++ b/aimbots-src/src/subsystems/chassis/control/chassis.hpp @@ -97,7 +97,13 @@ class ChassisSubsystem : public tap::control::chassis::ChassisSubsystemInterface void calculateHolonomic(float x, float y, float r, float maxWheelSpeed); // normal 4wd mecanum robots #ifdef SWERVE - void calculateSwerve(float x, float y, float r, float maxWheelSpeed); // swerve drive robots + //converts yaw encoder count to radians + 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 + void calculateSwerve(float x, float y, float r, float maxWheelSpeed); #endif void calculateRail(float x, float maxWheelSpeed); // sentry rail robots 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 { 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..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 @@ -15,9 +15,13 @@ 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; +bool underHeat = false; int limitSwitchDownTime; FeederLimitCommand::FeederLimitCommand( @@ -40,19 +44,22 @@ 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; + 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 = 350; + limitSwitchDownTime = 200; // 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; + prevLoadState = currLoadState; + + if (!limitswitchInactive.isExpired()) { + limitPressed = false; // feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::deactivateFeederMotor); } @@ -62,20 +69,31 @@ void FeederLimitCommand::execute() { // } if (!limitPressed && !loaderDormant) { - if (fabs(feeder->getCurrentRPM(0)) <= 10.0f && startupThreshold.execute()) { - feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::unjamFeederMotor); + feeder->ForFeederMotorGroup(SECONDARY, &FeederSubsystem::activateFeederMotor); + 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); - if (!isFiring) { + currLoadState = false; + if (isFiring) { feeder->ForFeederMotorGroup(PRIMARY, &FeederSubsystem::deactivateFeederMotor); } loaderDormant = true; 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/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(); } diff --git a/aimbots-src/src/subsystems/feeder/control/feeder.hpp b/aimbots-src/src/subsystems/feeder/control/feeder.hpp index 4c7fd0d5..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); + + #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 + } } 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/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..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 @@ -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 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..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,14 +51,25 @@ float pitchGimbalMotorPositionTargetDisplay = 0.0f; float yawAngleErrorDisplay = 0; float chassisRelativeYawTargetDisplay = 0; float kinematicYawAngleDisplay = 0; +float kinematicYawAngleRadDisplay = 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 + fieldRelativeYawTargetRadDisplay = this->getTargetYaw(AngleUnit::Radians); fieldRelativeYawTargetDisplay = this->getTargetYaw(AngleUnit::Degrees); + kinematicYawAngleRadDisplay = drivers->kinematicInformant.getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat().getWrappedValue(); + kinematicYawAngleDisplay = modm::toDegree(drivers->kinematicInformant.getCurrentFieldRelativeGimbalYawAngleAsWrappedFloat().getWrappedValue()); @@ -77,6 +89,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( 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..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 @@ -42,6 +42,7 @@ 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; 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/motion/power_limiter/power_limiter.cpp b/aimbots-src/src/utils/motion/power_limiter/power_limiter.cpp index 52d7acde..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,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 = 0; // we're multiplying by 1'000'000.0f to convert from microwatts to watts const float dt = tap::arch::clock::getTimeMilliseconds() - prevTime; 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 diff --git a/aimbots-src/src/utils/tools/robot_specific_defines.hpp b/aimbots-src/src/utils/tools/robot_specific_defines.hpp index 6ea1938f..097fa4e6 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,14 @@ #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 TURRET_IMU #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 diff --git a/taproot b/taproot index bcbd843f..cd047d2f 160000 --- a/taproot +++ b/taproot @@ -1 +1 @@ -Subproject commit bcbd843fcaa69a26d165fa86b2a0528c9744b86b +Subproject commit cd047d2fd6d932320d03390e6e6e3fff4fcb38fd