From e04adf4ba0ea15aec223110a1c010c163a454064 Mon Sep 17 00:00:00 2001 From: suhas Date: Fri, 11 Apr 2025 22:01:26 -0500 Subject: [PATCH 1/7] write an echo thing? --- ut-robomaster/src/communication/cv_board.cpp | 18 ++++++++++++++++++ ut-robomaster/src/communication/cv_board.hpp | 5 +++++ ut-robomaster/src/communication/cv_message.hpp | 3 +++ 3 files changed, 26 insertions(+) diff --git a/ut-robomaster/src/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp index db545381..da7dfa31 100644 --- a/ut-robomaster/src/communication/cv_board.cpp +++ b/ut-robomaster/src/communication/cv_board.cpp @@ -24,6 +24,8 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message) case CV_MESSAGE_TYPE_TURRET_AIM: decodeTurretData(message); break; + case CV_MESSAGE_TYPE_ECHO: + echoData(message); default: break; } @@ -35,6 +37,22 @@ void CVBoard::sendMessage() sendColorData(); } +void CVBoard::echoData(const ReceivedSerialMessage& message) +{ + std::string data_string; + memcpy(&data_string, &message.data, sizeof(message.header.dataLength)); + + DJISerial::SerialMessage msg; + msg.messageType = CV_MESSAGE_TYPE_ECHO; + + std::string* data = reinterpret_cast(msg.data); + + data = &data_string; + + msg.setCRC16(); + drivers->uart.write(UART_PORT, reinterpret_cast(&msg), sizeof(msg)); +} + void CVBoard::sendOdometryData() { DJISerial::SerialMessage message; diff --git a/ut-robomaster/src/communication/cv_board.hpp b/ut-robomaster/src/communication/cv_board.hpp index 89e00d0e..6590e56c 100644 --- a/ut-robomaster/src/communication/cv_board.hpp +++ b/ut-robomaster/src/communication/cv_board.hpp @@ -43,6 +43,11 @@ class CVBoard : public tap::communication::serial::DJISerial */ void sendMessage(); + /** + * Echo the message sent by the CV board to the CV board + */ + void echoData(const ReceivedSerialMessage& message); + /** * Sends odometry data to the CV board */ diff --git a/ut-robomaster/src/communication/cv_message.hpp b/ut-robomaster/src/communication/cv_message.hpp index 263164df..5899757a 100644 --- a/ut-robomaster/src/communication/cv_message.hpp +++ b/ut-robomaster/src/communication/cv_message.hpp @@ -1,6 +1,8 @@ #pragma once #include +#include + namespace communication { @@ -9,6 +11,7 @@ enum MessageTypes : uint8_t CV_MESSAGE_TYPE_ODOMETRY_DATA = 1, CV_MESSAGE_TYPE_TURRET_AIM = 2, CV_MESSAGE_TYPE_COLOR_DATA = 3, + CV_MESSAGE_TYPE_ECHO = 4, }; enum ColorTypes : uint8_t From e1f2ac09f7e17f08d71ef8004432fb23effd8fdc Mon Sep 17 00:00:00 2001 From: suhas Date: Wed, 16 Apr 2025 21:54:28 +0000 Subject: [PATCH 2/7] OMNI WHEEL CODE !!!!!! Co-authored-by: arekCodez --- ut-robomaster/src/communication/cv_board.cpp | 26 +++++----- ut-robomaster/src/communication/cv_board.hpp | 2 +- .../subsystems/chassis/chassis_subsystem.cpp | 8 ++++ .../subsystems/chassis/chassis_subsystem.hpp | 2 + .../chassis/command_sentry_position.cpp | 47 +++++++++++++++---- 5 files changed, 63 insertions(+), 22 deletions(-) diff --git a/ut-robomaster/src/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp index da7dfa31..4de325cd 100644 --- a/ut-robomaster/src/communication/cv_board.cpp +++ b/ut-robomaster/src/communication/cv_board.cpp @@ -24,8 +24,8 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message) case CV_MESSAGE_TYPE_TURRET_AIM: decodeTurretData(message); break; - case CV_MESSAGE_TYPE_ECHO: - echoData(message); + // case CV_MESSAGE_TYPE_ECHO: + // echoData(message); default: break; } @@ -37,21 +37,21 @@ void CVBoard::sendMessage() sendColorData(); } -void CVBoard::echoData(const ReceivedSerialMessage& message) -{ - std::string data_string; - memcpy(&data_string, &message.data, sizeof(message.header.dataLength)); +// void CVBoard::echoData(const ReceivedSerialMessage& message) +// { +// std::string data_string; +// memcpy(&data_string, &message.data, sizeof(message.header.dataLength)); - DJISerial::SerialMessage msg; - msg.messageType = CV_MESSAGE_TYPE_ECHO; +// DJISerial::SerialMessage msg; +// msg.messageType = CV_MESSAGE_TYPE_ECHO; - std::string* data = reinterpret_cast(msg.data); +// std::string* data = reinterpret_cast(msg.data); - data = &data_string; +// data = &data_string; - msg.setCRC16(); - drivers->uart.write(UART_PORT, reinterpret_cast(&msg), sizeof(msg)); -} +// msg.setCRC16(); +// drivers->uart.write(UART_PORT, reinterpret_cast(&msg), sizeof(msg)); +// } void CVBoard::sendOdometryData() { diff --git a/ut-robomaster/src/communication/cv_board.hpp b/ut-robomaster/src/communication/cv_board.hpp index 6590e56c..c6123f7f 100644 --- a/ut-robomaster/src/communication/cv_board.hpp +++ b/ut-robomaster/src/communication/cv_board.hpp @@ -46,7 +46,7 @@ class CVBoard : public tap::communication::serial::DJISerial /** * Echo the message sent by the CV board to the CV board */ - void echoData(const ReceivedSerialMessage& message); + // void echoData(const ReceivedSerialMessage& message); /** * Sends odometry data to the CV board diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index e4bfa65e..8a85f5df 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp @@ -118,6 +118,14 @@ void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ) targetWheelVels[3] = (-v.y - v.x - wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s } +void ChassisSubsystem::setOmniVelocities(Vector2f v, float wZ) +{ + targetWheelVels[0] = (v.y + v.x + wZ) / WHEEL_RADIUS; // rad/s + targetWheelVels[1] = (v.y - v.x - wZ) / WHEEL_RADIUS; // rad/s + targetWheelVels[2] = (v.y - v.x + wZ) / WHEEL_RADIUS; // rad/s + targetWheelVels[3] = (v.y + v.x - wZ) / WHEEL_RADIUS; // rad/s +} + Vector3f ChassisSubsystem::measureVelocity() { float w1 = wheels[0].measureVelocity(); // rev/s diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp index dcc70d18..05c0b327 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp @@ -36,6 +36,8 @@ class ChassisSubsystem : public Subsystem /// @return x,y is linear velocity (m/s) and z is angular velocity (rad/s) Vector3f measureVelocity(); + void setOmniVelocities(Vector2f v, float wZ); + private: src::Drivers* drivers; power_limiter::PowerLimiter powerLimiter; diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index 7feef19e..fa3edcdc 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -7,20 +7,51 @@ void CommandSentryPosition::initialize() { moveTimer.stop(); } void CommandSentryPosition::execute() { - // wait until game starts then wait some more to avoid excess drifting - if (drivers->isGameActive() && moveTimer.isStopped()) + Remote *remote = &drivers->remote; + + Vector2f inputMove = Vector2f( + remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL), + remote->getChannel(Remote::Channel::RIGHT_VERTICAL)); + + float inputSpin = remote->getChannel(Remote::Channel::WHEEL); + + float inputMoveLen = inputMove.getLength(); + if (inputMoveLen < ANALOG_DEAD_ZONE) + { + inputMove = Vector2f(0.0f); + } + else { - moveTimer.restart(10'000); // 10s + inputMove /= max(1.0f, inputMoveLen); // clamp length } - if (!moveTimer.isExpired()) + if (abs(inputSpin) < ANALOG_DEAD_ZONE) { - chassis->input(Vector2f(0.0f), 0.0f); - return; + inputSpin = 0.0f; } - // speen - chassis->input(Vector2f(0.0f), 1.0f); + // apply quadratic input ramping + inputMove *= inputMove.getLength(); + inputSpin *= abs(inputSpin); + + inputMove *= MAX_LINEAR_VEL; + inputSpin *= MAX_ANGULAR_VEL; + + chassis->setOmniVelocities(inputMove, inputSpin); + + // if (drivers->isGameActive() && moveTimer.isStopped()) + // { + // moveTimer.restart(10'000); // 10s + // } + + // if (!moveTimer.isExpired()) + // { + // chassis->input(Vector2f(0.0f), 0.0f); + // return; + // } + + // // speen + // chassis->input(Vector2f(0.0f), 1.0f); } void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); } From 04937d0b59bc9071fb08b884db4b237462826ad4 Mon Sep 17 00:00:00 2001 From: suhas Date: Thu, 17 Apr 2025 04:43:26 +0000 Subject: [PATCH 3/7] something something --- ut-robomaster/src/robots/hero/hero_constants.hpp | 7 ++++--- ut-robomaster/src/robots/sentry/sentry_constants.hpp | 4 ++-- ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp | 1 + ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp | 2 ++ .../src/subsystems/chassis/command_sentry_position.cpp | 2 ++ ut-robomaster/src/subsystems/turret/turret_subsystem.cpp | 7 ++++--- 6 files changed, 15 insertions(+), 8 deletions(-) diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 0af76a76..219a1df9 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -107,7 +107,7 @@ constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2; // chassis const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}}; const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; +const MotorConfig WHEEL_LB{M3508, MOTOR7, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}}; // flywheels @@ -117,14 +117,15 @@ const MotorConfig FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, true, "flywheel right", PID_FLYWHEEL, {}}; // agitator -const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}}; +const MotorConfig + AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}}; // water wheel const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}}; // turret const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}}; const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}}; -const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; +const MotorConfig PITCH{GM6020, MOTOR3, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; const float YAW_OFFSET = 0; const float PITCH_OFFSET = 0; diff --git a/ut-robomaster/src/robots/sentry/sentry_constants.hpp b/ut-robomaster/src/robots/sentry/sentry_constants.hpp index b4c0d2d2..967a8a5e 100644 --- a/ut-robomaster/src/robots/sentry/sentry_constants.hpp +++ b/ut-robomaster/src/robots/sentry/sentry_constants.hpp @@ -25,8 +25,8 @@ const Sound SOUND_STARTUP = SOUND_SMB_POWERUP; // chassis ------------ static constexpr int WHEELS = 4; -static constexpr float WHEEL_DISTANCE_X = 0.391f; // meters -static constexpr float WHEEL_DISTANCE_Y = 0.315f; // meters +static constexpr float WHEEL_DISTANCE_X = 0.346f; // meters +static constexpr float WHEEL_DISTANCE_Y = 0.346f; // meters static constexpr float WHEEL_RADIUS = 0.1524f; // meters static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f; diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index 8a85f5df..d20bfd93 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp @@ -26,6 +26,7 @@ void ChassisSubsystem::initialize() { wheels[i].initialize(); } + logTimer.stop(); } void ChassisSubsystem::refresh() diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp index 05c0b327..f45c5be8 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp @@ -9,6 +9,7 @@ #include "utils/power_limiter/power_limiter.hpp" #include "drivers.hpp" +using tap::arch::MilliTimeout; namespace subsystems::chassis { @@ -43,6 +44,7 @@ class ChassisSubsystem : public Subsystem power_limiter::PowerLimiter powerLimiter; MotorController wheels[WHEELS]; float targetWheelVels[WHEELS] = {0.0f, 0.0f, 0.0f, 0.0f}; + MilliTimeout logTimer; /// @brief Calculate and set wheel velocities for desired robot motion (based on /// https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf). diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index fa3edcdc..a4530771 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -13,6 +13,8 @@ void CommandSentryPosition::execute() remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL), remote->getChannel(Remote::Channel::RIGHT_VERTICAL)); + inputMove *= -1; + float inputSpin = remote->getChannel(Remote::Channel::WHEEL); float inputMoveLen = inputMove.getLength(); diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 37f22cf3..ba0074e6 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -29,7 +29,8 @@ void TurretSubsystem::initialize() void TurretSubsystem::refresh() { - setAmputated(!hardwareOk()); + // who needs hardware checks? + // setAmputated(!hardwareOk()); yaw.updateMotorAngle(); pitch.updateMotorAngle(); @@ -37,7 +38,7 @@ void TurretSubsystem::refresh() #if defined(TARGET_STANDARD) || defined(TARGET_HERO) yawEncoder.update(); - if (!isCalibrated && !isAmputated() && yawEncoder.isOnline()) + if (!isCalibrated && yawEncoder.isOnline()) { baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); isCalibrated = true; @@ -54,7 +55,7 @@ void TurretSubsystem::refresh() } #endif - if (isCalibrated && !drivers->isKillSwitched() && !isAmputated()) + if (isCalibrated && !drivers->isKillSwitched()) { yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); From 188fc1b998d67b8df47498291a52c264cce7519c Mon Sep 17 00:00:00 2001 From: arek Date: Fri, 25 Apr 2025 19:06:29 -0500 Subject: [PATCH 4/7] New chasis code --- .../subsystems/chassis/chassis_subsystem.cpp | 4 + .../chassis/command_sentry_position.cpp | 123 +++++++++++++++--- .../chassis/command_sentry_position.hpp | 36 ++++- 3 files changed, 137 insertions(+), 26 deletions(-) diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index d20bfd93..0d71b82d 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp @@ -107,7 +107,11 @@ void ChassisSubsystem::input(Vector2f move, float spin) overdrive -= correction; } +#if defined(TARGET_SENTRY) + setOmniVelocities(v, wZ); +#else setMecanumWheelVelocities(v, wZ); +#endif } void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ) diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index a4530771..812472e7 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -3,43 +3,58 @@ namespace commands { -void CommandSentryPosition::initialize() { moveTimer.stop(); } +// old ard code +// void CommandSentryPosition::initialize() { moveTimer.stop(); } + +void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); } void CommandSentryPosition::execute() { - Remote *remote = &drivers->remote; - - Vector2f inputMove = Vector2f( - remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL), - remote->getChannel(Remote::Channel::RIGHT_VERTICAL)); + float yawAngle = turret->getTargetLocalYaw(); + Vector2f inputMove = Vector2f(0.0f); + float inputSpin = 0.0f; - inputMove *= -1; + // get keyboard input + float keyboardInputSpin; + bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin); - float inputSpin = remote->getChannel(Remote::Channel::WHEEL); + // get joystick input + Vector2f joystickInputMove; + float joystickInputSpin; + bool hasJoystickInput = applyJoystickInput(joystickInputMove, joystickInputSpin); - float inputMoveLen = inputMove.getLength(); - if (inputMoveLen < ANALOG_DEAD_ZONE) + // decide which input source to use (keyboard has inertia) + if (!hasKeyboardInput && hasJoystickInput) { - inputMove = Vector2f(0.0f); + inputMove = joystickInputMove; + inputSpin = joystickInputSpin; } else { - inputMove /= max(1.0f, inputMoveLen); // clamp length + inputMove = keyboardInputMove; + inputSpin = keyboardInputSpin; } - if (abs(inputSpin) < ANALOG_DEAD_ZONE) + // auto-align chassis to turret when moving + if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f) { - inputSpin = 0.0f; + inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) * + CHASSIS_AUTOALIGN_FACTOR; } - // apply quadratic input ramping - inputMove *= inputMove.getLength(); - inputSpin *= abs(inputSpin); + // override spin input while beyblading + if (beyblade) + { + inputSpin = 1.0f; + } - inputMove *= MAX_LINEAR_VEL; - inputSpin *= MAX_ANGULAR_VEL; + // rotate movement vector relative to turret + if (turretRelative) + { + inputMove = inputMove.rotate(yawAngle); + } - chassis->setOmniVelocities(inputMove, inputSpin); + chassis->input(inputMove, inputSpin); // if (drivers->isGameActive() && moveTimer.isStopped()) // { @@ -57,6 +72,74 @@ void CommandSentryPosition::execute() } void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); } + bool CommandSentryPosition::isFinished() const { return false; } + +bool CommandSentryPosition::applyKeyboardInput(Vector2f &inputMove, float &inputSpin) +{ + Remote *remote = &drivers->remote; + + inputSpin = 0.0f; // no keyboard spin controls + + Vector2f rawMoveInput = Vector2f( + remote->keyPressed(Remote::Key::D) - remote->keyPressed(Remote::Key::A), + remote->keyPressed(Remote::Key::W) - remote->keyPressed(Remote::Key::S)); + + float rawInputLen = rawMoveInput.getLength(); + + if (rawInputLen > 0.0f) + { + Vector2f moveDir = rawMoveInput / rawInputLen; // normalize input + inputMove += moveDir * KEYBOARD_ACCEL * DT; // incorporate input + inputMove /= max(1.0f, inputMove.getLength()); // clamp length + } + else + { + // decelerate when input stops + float len = inputMove.getLength(); + if (len > 0.0f) + { + inputMove *= max(1.0f - KEYBOARD_DECEL * DT / len, 0.0f); + } + } + + return rawMoveInput != Vector2f(0.0f); +} + + + +bool CommandSentryPosition::applyJoystickInput(Vector2f &inputMove, float &inputSpin) +{ + Remote *remote = &drivers->remote; + + inputMove = Vector2f( + remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL), + remote->getChannel(Remote::Channel::RIGHT_VERTICAL)); + + inputSpin = remote->getChannel(Remote::Channel::WHEEL); + + float inputMoveLen = inputMove.getLength(); + if (inputMoveLen < ANALOG_DEAD_ZONE) + { + inputMove = Vector2f(0.0f); + } + else + { + inputMove /= max(1.0f, inputMoveLen); // clamp length + } + + if (abs(inputSpin) < ANALOG_DEAD_ZONE) + { + inputSpin = 0.0f; + } + + // apply quadratic input ramping + inputMove *= inputMove.getLength(); + inputSpin *= abs(inputSpin); + + return inputMove != Vector2f(0.0f) || inputSpin != 0.0f; +} + + } // namespace commands diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp index 69aa2653..0067a949 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp @@ -1,9 +1,16 @@ #pragma once -#include "tap/architecture/timeout.hpp" +//#include "tap/architecture/timeout.hpp" #include "tap/control/command.hpp" -#include "chassis_subsystem.hpp" + +#include "robots/robot_constants.hpp" +//this is older sentry code dont know why it had chopped path +//#include "chassis_subsystem.hpp" +#include "subsystems/chassis/chassis_subsystem.hpp" +#include "subsystems/turret/turret_subsystem.hpp" +#include "utils/chassis_auto_align.hpp" + #include "drivers.hpp" namespace commands @@ -11,14 +18,23 @@ namespace commands using namespace tap::communication::serial; using namespace modm; using subsystems::chassis::ChassisSubsystem; -using tap::arch::MilliTimeout; +// using tap::arch::MilliTimeout; +using subsystems::turret::TurretSubsystem; class CommandSentryPosition : public tap::control::Command { public: - CommandSentryPosition(src::Drivers *drivers, ChassisSubsystem *chassis) + CommandSentryPosition( + src::Drivers *drivers, + ChassisSubsystem *chassis, + TurretSubsystem *turret, + bool turretRelative = false, + bool beyblade = false) : drivers(drivers), - chassis(chassis) + chassis(chassis), + turret(turret), + turretRelative(turretRelative), + beyblade(beyblade) { addSubsystemRequirement(chassis); } @@ -36,6 +52,14 @@ class CommandSentryPosition : public tap::control::Command private: src::Drivers *drivers; ChassisSubsystem *chassis; - MilliTimeout moveTimer; + //MilliTimeout moveTimer; + TurretSubsystem *turret; + + Vector2f keyboardInputMove = Vector2f(0.0f); + const bool turretRelative = false; + const bool beyblade = false; + + bool applyKeyboardInput(Vector2f &moveOut, float &spinOut); + bool applyJoystickInput(Vector2f &moveOut, float &spinOut); }; } // namespace commands \ No newline at end of file From 5108bfd13c42bcd2366fbc748ada5662982baa46 Mon Sep 17 00:00:00 2001 From: suhas Date: Tue, 10 Jun 2025 19:31:30 -0500 Subject: [PATCH 5/7] idk --- .../src/robots/hero/hero_constants.hpp | 6 +- .../agitator/agitator_subsystem.cpp | 5 +- .../flywheel/flywheel_subsystem.cpp | 2 +- .../src/subsystems/turret/turret_motor.cpp | 4 +- .../src/subsystems/turret/turret_motor.hpp | 4 +- .../subsystems/turret/turret_subsystem.cpp | 77 +++++++++++-------- 6 files changed, 56 insertions(+), 42 deletions(-) diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 219a1df9..05162afd 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -107,7 +107,7 @@ constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2; // chassis const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}}; const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_LB{M3508, MOTOR7, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; +const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}}; // flywheels @@ -125,7 +125,7 @@ const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}}; const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}}; -const MotorConfig PITCH{GM6020, MOTOR3, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; +const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; const float YAW_OFFSET = 0; const float PITCH_OFFSET = 0; @@ -141,7 +141,7 @@ static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS; static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY; // rad/s const float TARGET_PROJECTILE_VELOCITY = 16; // m/s -const float FLYWHEEL_SPEED = 190.0f; +const float FLYWHEEL_SPEED = 47.5f; const float BALLS_PER_SEC = 4.0f; const float BALLS_PER_REV = 6.0f; diff --git a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp index 6bad9af2..91250a5c 100644 --- a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp +++ b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp @@ -46,11 +46,12 @@ void AgitatorSubsystem::refresh() return; #endif - setAmputated(!hardwareOk()); + // setAmputated(!hardwareOk()); float time = getTimeMilliseconds() / 1000.0f; float velocity = getShapedVelocity(time, 1.0f, 0.0f, ballsPerSecond); - bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive(); + // bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive(); + bool killSwitch = drivers->isKillSwitched(); agitator.setActive(!killSwitch); agitator.updateVelocity(velocity); diff --git a/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp b/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp index b0aa4cc9..5a60d31f 100644 --- a/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp +++ b/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp @@ -41,7 +41,7 @@ void FlywheelSubsystem::refresh() for (int i = 0; i < FLYWHEELS; i++) { - motors[i].setActive(!killSwitch && !isAmputated()); + motors[i].setActive(!killSwitch); motors[i].updateVelocity(velocity); } } diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.cpp b/ut-robomaster/src/subsystems/turret/turret_motor.cpp index 8273a68a..c5faf15f 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.cpp @@ -9,8 +9,8 @@ TurretMotor::TurretMotor( src::Drivers *drivers, MotorConfig motorConfig, const SmoothPidConfig &pidConfig) - : drivers(drivers), - motor(drivers, motorConfig.id, motorConfig.canBus, motorConfig.inverted, motorConfig.name), + : motor(drivers, motorConfig.id, motorConfig.canBus, motorConfig.inverted, motorConfig.name), + drivers(drivers), pid(pidConfig), setpoint(0.0f, 0.0f, M_TWOPI), currentAngle(0.0f, 0.0f, M_TWOPI) diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.hpp b/ut-robomaster/src/subsystems/turret/turret_motor.hpp index 12fcc8d2..d82ee7ef 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.hpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.hpp @@ -27,9 +27,11 @@ class TurretMotor float getSetpoint(); bool isOnline(); + DjiMotor motor; + private: src::Drivers *drivers; - DjiMotor motor; + // DjiMotor motor; SmoothPid pid; ContiguousFloat setpoint; float unwrappedAngle = 0; diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index ba0074e6..5a31875e 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -1,11 +1,14 @@ #include "turret_subsystem.hpp" +#include "tap/communication/serial/remote.hpp" + #include "modm/math.hpp" #include "robots/robot_constants.hpp" #include "subsystems/subsystem.hpp" namespace subsystems::turret { +using tap::communication::serial::Remote; TurretSubsystem::TurretSubsystem(src::Drivers* drivers) : Subsystem(drivers), drivers(drivers), @@ -29,42 +32,50 @@ void TurretSubsystem::initialize() void TurretSubsystem::refresh() { + // yaw.setOutput(0.5f); + // pitch.motor.setDesiredOutput(M3508.maxOutput * 0.5); // who needs hardware checks? // setAmputated(!hardwareOk()); - yaw.updateMotorAngle(); - pitch.updateMotorAngle(); - -#if defined(TARGET_STANDARD) || defined(TARGET_HERO) - yawEncoder.update(); - - if (!isCalibrated && yawEncoder.isOnline()) - { - baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); - isCalibrated = true; - - setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); - } -#else - if (!isCalibrated && !isAmputated()) - { - baseYaw = -YAW_OFFSET; - isCalibrated = true; - - setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); - } -#endif - - if (isCalibrated && !drivers->isKillSwitched()) - { - yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); - pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); - } - else - { - yaw.reset(); - pitch.reset(); - } + Remote* remote = &drivers->remote; + float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL); + float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL); + yaw.setOutput(v); + pitch.motor.setDesiredOutput(GM6020.maxOutput * 0.5f); + + // yaw.updateMotorAngle(); + // pitch.updateMotorAngle(); + + // #if defined(TARGET_STANDARD) || defined(TARGET_HERO) + // yawEncoder.update(); + + // if (!isCalibrated && yawEncoder.isOnline()) + // { + // baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); + // isCalibrated = true; + + // setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); + // } + // #else + // if (!isCalibrated && !isAmputated()) + // { + // baseYaw = -YAW_OFFSET; + // isCalibrated = true; + + // setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); + // } + // #endif + + // if (isCalibrated && !drivers->isKillSwitched()) + // { + // yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); + // pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); + // } + // else + // { + // yaw.reset(); + // pitch.reset(); + // } } void TurretSubsystem::inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration) From 71b87b7a1a5cf09405824c2e30e913ff091718c3 Mon Sep 17 00:00:00 2001 From: suhas Date: Tue, 10 Jun 2025 20:11:35 -0500 Subject: [PATCH 6/7] bad turret: --- ut-robomaster/src/subsystems/turret/turret_subsystem.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 5a31875e..3e5e1b18 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -40,8 +40,8 @@ void TurretSubsystem::refresh() Remote* remote = &drivers->remote; float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL); float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL); - yaw.setOutput(v); - pitch.motor.setDesiredOutput(GM6020.maxOutput * 0.5f); + yaw.setOutput(h); + pitch.motor.setDesiredOutput(GM6020.maxOutput * v); // yaw.updateMotorAngle(); // pitch.updateMotorAngle(); From 8b436a37f9fe58be41e38d0d1202334634293fa5 Mon Sep 17 00:00:00 2001 From: suhas Date: Mon, 23 Jun 2025 20:37:43 -0700 Subject: [PATCH 7/7] g --- taproot | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/taproot b/taproot index 22b708e6..cd047d2f 160000 --- a/taproot +++ b/taproot @@ -1 +1 @@ -Subproject commit 22b708e6e1833464914ff7b2279b0cd7214e023f +Subproject commit cd047d2fd6d932320d03390e6e6e3fff4fcb38fd