diff --git a/taproot b/taproot index 22b708e6..cd047d2f 160000 --- a/taproot +++ b/taproot @@ -1 +1 @@ -Subproject commit 22b708e6e1833464914ff7b2279b0cd7214e023f +Subproject commit cd047d2fd6d932320d03390e6e6e3fff4fcb38fd diff --git a/ut-robomaster/src/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp index db545381..4de325cd 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..c6123f7f 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 diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 0af76a76..05162afd 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -117,7 +117,8 @@ 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 @@ -140,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/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/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/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index e4bfa65e..0d71b82d 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() @@ -106,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) @@ -118,6 +123,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..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 { @@ -36,11 +37,14 @@ 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; 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 7feef19e..812472e7 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -3,27 +3,143 @@ 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() { - // wait until game starts then wait some more to avoid excess drifting - if (drivers->isGameActive() && moveTimer.isStopped()) + float yawAngle = turret->getTargetLocalYaw(); + Vector2f inputMove = Vector2f(0.0f); + float inputSpin = 0.0f; + + // get keyboard input + float keyboardInputSpin; + bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin); + + // get joystick input + Vector2f joystickInputMove; + float joystickInputSpin; + bool hasJoystickInput = applyJoystickInput(joystickInputMove, joystickInputSpin); + + // decide which input source to use (keyboard has inertia) + if (!hasKeyboardInput && hasJoystickInput) + { + inputMove = joystickInputMove; + inputSpin = joystickInputSpin; + } + else + { + inputMove = keyboardInputMove; + inputSpin = keyboardInputSpin; + } + + // auto-align chassis to turret when moving + if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f) + { + inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) * + CHASSIS_AUTOALIGN_FACTOR; + } + + // override spin input while beyblading + if (beyblade) { - moveTimer.restart(10'000); // 10s + inputSpin = 1.0f; } - if (!moveTimer.isExpired()) + // rotate movement vector relative to turret + if (turretRelative) { - chassis->input(Vector2f(0.0f), 0.0f); - return; + inputMove = inputMove.rotate(yawAngle); } - // speen - chassis->input(Vector2f(0.0f), 1.0f); + chassis->input(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); } + 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 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 37f22cf3..3e5e1b18 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,41 +32,50 @@ void TurretSubsystem::initialize() void TurretSubsystem::refresh() { - setAmputated(!hardwareOk()); - - yaw.updateMotorAngle(); - pitch.updateMotorAngle(); - -#if defined(TARGET_STANDARD) || defined(TARGET_HERO) - yawEncoder.update(); - - if (!isCalibrated && !isAmputated() && 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() && !isAmputated()) - { - yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); - pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); - } - else - { - yaw.reset(); - pitch.reset(); - } + // yaw.setOutput(0.5f); + // pitch.motor.setDesiredOutput(M3508.maxOutput * 0.5); + // who needs hardware checks? + // setAmputated(!hardwareOk()); + + Remote* remote = &drivers->remote; + float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL); + float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL); + yaw.setOutput(h); + pitch.motor.setDesiredOutput(GM6020.maxOutput * v); + + // 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)