From b5d6d1bad0e3a573eccedd5ab71eb6c601093759 Mon Sep 17 00:00:00 2001 From: vinle0 Date: Wed, 15 May 2024 20:28:42 +0000 Subject: [PATCH] Modified double yaw for conditions --- ut-robomaster/src/board.hpp | 32 +-- ut-robomaster/src/drivers.hpp | 62 ++--- ut-robomaster/src/drivers/as5600.hpp | 118 ++++---- ut-robomaster/src/drivers/encoder.hpp | 26 +- ut-robomaster/src/main.cpp | 232 ++++++++-------- .../src/robots/hero/hero_control.cpp | 2 +- .../subsystems/chassis/chassis_subsystem.cpp | 262 +++++++++--------- .../subsystems/chassis/chassis_subsystem.hpp | 142 +++++----- .../subsystems/turret/double_yaw_motor.cpp | 124 ++++----- .../subsystems/turret/double_yaw_motor.hpp | 88 +++--- .../subsystems/turret/turret_subsystem.cpp | 8 +- .../subsystems/turret/turret_subsystem.hpp | 20 +- 12 files changed, 567 insertions(+), 549 deletions(-) diff --git a/ut-robomaster/src/board.hpp b/ut-robomaster/src/board.hpp index 134c898a..a3455ee1 100644 --- a/ut-robomaster/src/board.hpp +++ b/ut-robomaster/src/board.hpp @@ -1,16 +1,16 @@ -#pragma once - -#include "tap/board/board.hpp" - -namespace Board -{ -using I2cSda = DigitalInPinPF0; -using I2cScl = DigitalInPinPF1; -using I2cMaster = I2cMaster2; - -inline void initialize_i2c() -{ - I2cMaster::connect(I2cMaster::PullUps::Internal); - I2cMaster::initialize(); -} -} // namespace Board +#pragma once + +#include "tap/board/board.hpp" + +namespace Board +{ +using I2cSda = DigitalInPinPF0; +using I2cScl = DigitalInPinPF1; +using I2cMaster = I2cMaster2; + +inline void initialize_i2c() +{ + I2cMaster::connect(I2cMaster::PullUps::Internal); + I2cMaster::initialize(); +} +} // namespace Board diff --git a/ut-robomaster/src/drivers.hpp b/ut-robomaster/src/drivers.hpp index 9371a033..e9d84683 100644 --- a/ut-robomaster/src/drivers.hpp +++ b/ut-robomaster/src/drivers.hpp @@ -1,31 +1,31 @@ -#ifndef DRIVERS_HPP_ -#define DRIVERS_HPP_ - -#include "tap/drivers.hpp" - -#include "communication/cv_board.hpp" -#include "drivers/as5600.hpp" -#include "utils/robot_comms.hpp" - -#include "board.hpp" - -namespace src -{ -class Drivers : public tap::Drivers -{ - friend class DriversSingleton; - -#ifdef ENV_UNIT_TESTS -public: -#endif - Drivers() : tap::Drivers(), cvBoard(this), terminal(this) {} - -public: - communication::CVBoard cvBoard; - comms::RobotComms terminal; - bool isKillSwitched() { return !remote.isConnected(); } -}; // class Drivers - -} // namespace src - -#endif // DRIVERS_HPP_ +#ifndef DRIVERS_HPP_ +#define DRIVERS_HPP_ + +#include "tap/drivers.hpp" + +#include "communication/cv_board.hpp" +#include "drivers/as5600.hpp" +#include "utils/robot_comms.hpp" + +#include "board.hpp" + +namespace src +{ +class Drivers : public tap::Drivers +{ + friend class DriversSingleton; + +#ifdef ENV_UNIT_TESTS +public: +#endif + Drivers() : tap::Drivers(), cvBoard(this), terminal(this) {} + +public: + communication::CVBoard cvBoard; + comms::RobotComms terminal; + bool isKillSwitched() { return !remote.isConnected(); } +}; // class Drivers + +} // namespace src + +#endif // DRIVERS_HPP_ diff --git a/ut-robomaster/src/drivers/as5600.hpp b/ut-robomaster/src/drivers/as5600.hpp index 999ccf16..02c5f34e 100644 --- a/ut-robomaster/src/drivers/as5600.hpp +++ b/ut-robomaster/src/drivers/as5600.hpp @@ -1,60 +1,60 @@ -#pragma once - -#include "modm/architecture/interface/i2c_device.hpp" -#include "modm/processing/protothread/protothread.hpp" - -#include "encoder.hpp" - -namespace driver -{ -template -class As5600 : public Encoder, public modm::I2cDevice, public modm::pt::Protothread -{ -public: - As5600() : modm::I2cDevice(ADDRESS){}; - - void update() { run(); } - - bool run() - { - PT_BEGIN(); - - while (true) - { - buffer[0] = uint8_t(Register::RAWANGLE); - PT_WAIT_UNTIL(this->startWriteRead(buffer, 1, buffer, 2)); - PT_WAIT_WHILE(this->isTransactionRunning()); - - if (this->wasTransactionSuccessful()) - { - angle = (buffer[0] << 8) | buffer[1]; - } - } - - PT_END(); - } - - float getAngle() override { return angle / 4096.0f; } - -protected: - enum class Register : uint8_t - { - ZPOS = 0x01, - MPOS = 0x03, - CONF = 0x07, - - RAWANGLE = 0x0C, - ANGLE = 0x0E, - - STATUS = 0x0B, - AGC = 0x1A, - MAG = 0x1B, - }; - -private: - static const uint8_t ADDRESS = 0x36; - uint16_t angle = 0; - uint8_t buffer[2]; -}; - +#pragma once + +#include "modm/architecture/interface/i2c_device.hpp" +#include "modm/processing/protothread/protothread.hpp" + +#include "encoder.hpp" + +namespace driver +{ +template +class As5600 : public Encoder, public modm::I2cDevice, public modm::pt::Protothread +{ +public: + As5600() : modm::I2cDevice(ADDRESS){}; + + void update() { run(); } + + bool run() + { + PT_BEGIN(); + + while (true) + { + buffer[0] = uint8_t(Register::RAWANGLE); + PT_WAIT_UNTIL(this->startWriteRead(buffer, 1, buffer, 2)); + PT_WAIT_WHILE(this->isTransactionRunning()); + + if (this->wasTransactionSuccessful()) + { + angle = (buffer[0] << 8) | buffer[1]; + } + } + + PT_END(); + } + + float getAngle() override { return angle / 4096.0f; } + +protected: + enum class Register : uint8_t + { + ZPOS = 0x01, + MPOS = 0x03, + CONF = 0x07, + + RAWANGLE = 0x0C, + ANGLE = 0x0E, + + STATUS = 0x0B, + AGC = 0x1A, + MAG = 0x1B, + }; + +private: + static const uint8_t ADDRESS = 0x36; + uint16_t angle = 0; + uint8_t buffer[2]; +}; + }; // namespace driver \ No newline at end of file diff --git a/ut-robomaster/src/drivers/encoder.hpp b/ut-robomaster/src/drivers/encoder.hpp index 8ca03f90..a3b06d74 100644 --- a/ut-robomaster/src/drivers/encoder.hpp +++ b/ut-robomaster/src/drivers/encoder.hpp @@ -1,14 +1,14 @@ -#pragma once - -namespace driver -{ -class Encoder -{ -public: - virtual ~Encoder() {} - - /// @brief Get current measured angle of the encoder - /// @return Angle (revs) - virtual float getAngle() = 0; -}; +#pragma once + +namespace driver +{ +class Encoder +{ +public: + virtual ~Encoder() {} + + /// @brief Get current measured angle of the encoder + /// @return Angle (revs) + virtual float getAngle() = 0; +}; } // namespace driver \ No newline at end of file diff --git a/ut-robomaster/src/main.cpp b/ut-robomaster/src/main.cpp index aca48ccc..1c4dc779 100644 --- a/ut-robomaster/src/main.cpp +++ b/ut-robomaster/src/main.cpp @@ -1,116 +1,116 @@ -#ifdef PLATFORM_HOSTED -/* hosted environment (simulator) includes --------------------------------- */ -#include - -#include "tap/communication/tcp-server/tcp_server.hpp" -#include "tap/motor/motorsim/sim_handler.hpp" -#endif - -#include "modm/architecture/interface/delay.hpp" -#include "modm/platform.hpp" - -#include "board.hpp" - -/* arch includes ------------------------------------------------------------*/ -#include "tap/architecture/periodic_timer.hpp" -#include "tap/architecture/profiler.hpp" - -/* communication includes ---------------------------------------------------*/ -#include "drivers.hpp" -#include "drivers_singleton.hpp" - -/* error handling includes --------------------------------------------------*/ -#include "tap/errors/create_errors.hpp" - -/* control includes ---------------------------------------------------------*/ -#include "tap/architecture/clock.hpp" - -#include "modm/platform/i2c/i2c_master_2.hpp" -#include "robots/robot_constants.hpp" -#include "robots/robot_control.hpp" - -/* define timers here -------------------------------------------------------*/ -tap::arch::PeriodicMilliTimer refreshTimer(REFRESH_PERIOD); - -// Place any sort of input/output initialization here. For example, place -// serial init stuff here. -static void initializeIo(src::Drivers *drivers); - -// Anything that you would like to be called place here. It will be called -// very frequently. Use PeriodicMilliTimers if you don't want something to be -// called as frequently. -static void updateIo(src::Drivers *drivers); - -using namespace tap::gpio; - -int main() -{ -#ifdef PLATFORM_HOSTED - std::cout << "Simulation starting..." << std::endl; -#endif - - /* - * NOTE: We are using DoNotUse_getDrivers here because in the main - * robot loop we must access the singleton drivers to update - * IO states and run the scheduler. - */ - src::Drivers *drivers = src::DoNotUse_getDrivers(); - - Board::initialize(); - initializeIo(drivers); - Board::initialize_i2c(); - control::initSubsystemCommands(drivers); - -#ifdef PLATFORM_HOSTED - tap::motorsim::SimHandler::resetMotorSims(); - // Blocking call, waits until Windows Simulator connects. - tap::communication::TCPServer::MainServer()->getConnection(); -#endif - - while (1) - { - // do this as fast as you can - PROFILE(drivers->profiler, updateIo, (drivers)); - - if (refreshTimer.execute()) - { - PROFILE(drivers->profiler, drivers->bmi088.periodicIMUUpdate, ()); - PROFILE(drivers->profiler, drivers->commandScheduler.run, ()); - PROFILE(drivers->profiler, drivers->djiMotorTxHandler.encodeAndSendCanData, ()); - PROFILE(drivers->profiler, drivers->terminalSerial.update, ()); - } - - modm::delay_us(10); - } - return 0; -} - -static void initializeIo(src::Drivers *drivers) -{ - drivers->analog.init(); - drivers->pwm.init(); - drivers->digital.init(); - drivers->leds.init(); - drivers->can.initialize(); - drivers->errorController.init(); - drivers->remote.initialize(); - drivers->refSerial.initialize(); - // drivers->cvBoard.initialize(); - drivers->terminalSerial.initialize(); - drivers->schedulerTerminalHandler.init(); - drivers->djiMotorTerminalSerialHandler.init(); - drivers->bmi088.initialize(IMU_SAMPLE_FREQUENCY, IMU_KP, IMU_KI); - drivers->bmi088.requestRecalibration(); -} - -static void updateIo(src::Drivers *drivers) -{ -#ifdef PLATFORM_HOSTED - tap::motorsim::SimHandler::updateSims(); -#endif - - drivers->canRxHandler.pollCanData(); - drivers->refSerial.updateSerial(); - // drivers->cvBoard.updateSerial(); - drivers->remote.read(); -} +#ifdef PLATFORM_HOSTED +/* hosted environment (simulator) includes --------------------------------- */ +#include + +#include "tap/communication/tcp-server/tcp_server.hpp" +#include "tap/motor/motorsim/sim_handler.hpp" +#endif + +#include "modm/architecture/interface/delay.hpp" +#include "modm/platform.hpp" + +#include "board.hpp" + +/* arch includes ------------------------------------------------------------*/ +#include "tap/architecture/periodic_timer.hpp" +#include "tap/architecture/profiler.hpp" + +/* communication includes ---------------------------------------------------*/ +#include "drivers.hpp" +#include "drivers_singleton.hpp" + +/* error handling includes --------------------------------------------------*/ +#include "tap/errors/create_errors.hpp" + +/* control includes ---------------------------------------------------------*/ +#include "tap/architecture/clock.hpp" + +#include "modm/platform/i2c/i2c_master_2.hpp" +#include "robots/robot_constants.hpp" +#include "robots/robot_control.hpp" + +/* define timers here -------------------------------------------------------*/ +tap::arch::PeriodicMilliTimer refreshTimer(REFRESH_PERIOD); + +// Place any sort of input/output initialization here. For example, place +// serial init stuff here. +static void initializeIo(src::Drivers *drivers); + +// Anything that you would like to be called place here. It will be called +// very frequently. Use PeriodicMilliTimers if you don't want something to be +// called as frequently. +static void updateIo(src::Drivers *drivers); + +using namespace tap::gpio; + +int main() +{ +#ifdef PLATFORM_HOSTED + std::cout << "Simulation starting..." << std::endl; +#endif + + /* + * NOTE: We are using DoNotUse_getDrivers here because in the main + * robot loop we must access the singleton drivers to update + * IO states and run the scheduler. + */ + src::Drivers *drivers = src::DoNotUse_getDrivers(); + + Board::initialize(); + initializeIo(drivers); + Board::initialize_i2c(); + control::initSubsystemCommands(drivers); + +#ifdef PLATFORM_HOSTED + tap::motorsim::SimHandler::resetMotorSims(); + // Blocking call, waits until Windows Simulator connects. + tap::communication::TCPServer::MainServer()->getConnection(); +#endif + + while (1) + { + // do this as fast as you can + PROFILE(drivers->profiler, updateIo, (drivers)); + + if (refreshTimer.execute()) + { + PROFILE(drivers->profiler, drivers->bmi088.periodicIMUUpdate, ()); + PROFILE(drivers->profiler, drivers->commandScheduler.run, ()); + PROFILE(drivers->profiler, drivers->djiMotorTxHandler.encodeAndSendCanData, ()); + PROFILE(drivers->profiler, drivers->terminalSerial.update, ()); + } + + modm::delay_us(10); + } + return 0; +} + +static void initializeIo(src::Drivers *drivers) +{ + drivers->analog.init(); + drivers->pwm.init(); + drivers->digital.init(); + drivers->leds.init(); + drivers->can.initialize(); + drivers->errorController.init(); + drivers->remote.initialize(); + drivers->refSerial.initialize(); + // drivers->cvBoard.initialize(); + drivers->terminalSerial.initialize(); + drivers->schedulerTerminalHandler.init(); + drivers->djiMotorTerminalSerialHandler.init(); + drivers->bmi088.initialize(IMU_SAMPLE_FREQUENCY, IMU_KP, IMU_KI); + drivers->bmi088.requestRecalibration(); +} + +static void updateIo(src::Drivers *drivers) +{ +#ifdef PLATFORM_HOSTED + tap::motorsim::SimHandler::updateSims(); +#endif + + drivers->canRxHandler.pollCanData(); + drivers->refSerial.updateSerial(); + // drivers->cvBoard.updateSerial(); + drivers->remote.read(); +} diff --git a/ut-robomaster/src/robots/hero/hero_control.cpp b/ut-robomaster/src/robots/hero/hero_control.cpp index e070e73d..3269369d 100644 --- a/ut-robomaster/src/robots/hero/hero_control.cpp +++ b/ut-robomaster/src/robots/hero/hero_control.cpp @@ -158,7 +158,7 @@ void initializeSubsystems() chassis.initialize(); agitator.initialize(); flywheel.initialize(); - turret.initialize(); + turret.initialize(); odometry.initialize(); sound.initialize(); } diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index 01d48de3..f29f4264 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp @@ -1,131 +1,131 @@ -#include "chassis_subsystem.hpp" - -#include "tap/algorithms/math_user_utils.hpp" -#include "tap/communication/sensors/buzzer/buzzer.hpp" - -#include "robots/robot_constants.hpp" - -using namespace tap::algorithms; - -namespace subsystems -{ -namespace chassis -{ -ChassisSubsystem::ChassisSubsystem(src::Drivers* drivers) - : tap::control::Subsystem(drivers), - drivers(drivers), - powerLimiter(drivers, ENERGY_BUFFER_LIMIT_THRESHOLD, ENERGY_BUFFER_CRIT_THRESHOLD), - wheels{ - {drivers, M3508, ID_WHEEL_LF, CAN_WHEELS, true, "left front", PID_WHEELS}, - {drivers, M3508, ID_WHEEL_RF, CAN_WHEELS, false, "right front", PID_WHEELS}, - {drivers, M3508, ID_WHEEL_LB, CAN_WHEELS, true, "left back", PID_WHEELS}, - {drivers, M3508, ID_WHEEL_RB, CAN_WHEELS, false, "right back", PID_WHEELS}, - } {}; - -void ChassisSubsystem::initialize() -{ - for (int8_t i = 0; i < WHEELS; i++) - { - wheels[i].initialize(); - } -} - -void ChassisSubsystem::refresh() -{ - for (int8_t i = 0; i < WHEELS; i++) - { - wheels[i].setActive(!drivers->isKillSwitched()); - wheels[i].update(targetWheelVels[i] / M_TWOPI); // rad/s to rev/s - } - - limitChassisPower(); -} - -void ChassisSubsystem::limitChassisPower() -{ - float powerScalar = powerLimiter.getPowerLimitRatio(); - if (compareFloatClose(1.0f, powerScalar, 1E-3)) - { - powerScalar = 1.0f; - } - - float totalError = 0.0f; - for (size_t i = 0; i < WHEELS; i++) - { - totalError += abs(wheels[i].getPid().getLastError()); - } - - for (int8_t i = 0; i < WHEELS; i++) - { - bool totalErrorZero = compareFloatClose(0.0f, totalError, 1E-3); - float velocityErrorScalar = totalErrorZero - ? (1.0f / WHEELS) - : (abs(wheels[i].getPid().getLastError()) / totalError); - float modifiedPowerScalar = - limitVal(WHEELS * powerScalar * velocityErrorScalar, 0.0f, 1.0f); - - wheels[i].setActive(!drivers->isKillSwitched()); - wheels[i].applyPowerScalar(modifiedPowerScalar); - } -} - -void ChassisSubsystem::runHardwareTests() -{ - // TODO -} - -void ChassisSubsystem::input(Vector2f move, float spin) -{ - Vector2f v = move * MAX_LINEAR_VEL; - float wZ = spin * MAX_ANGULAR_VEL; - - float linearTerm = (abs(v.x) + abs(v.y)) / WHEEL_RADIUS; - float angularTerm = abs(wZ) * WHEEL_LXY / WHEEL_RADIUS; - - // overdrive error - float overdrive = max(linearTerm + angularTerm - WHEEL_MAX_VEL, 0.0f); - - // angular velocity correction - if (angularTerm > 0.0f) - { - float correction = min(angularTerm, overdrive); - wZ *= 1.0f - correction / angularTerm; - overdrive -= correction; - } - - // linear velocity correction - if (linearTerm > 0.0f) - { - float correction = min(linearTerm, overdrive); - v *= 1.0f - correction / linearTerm; - overdrive -= correction; - } - - setMecanumWheelVelocities(v, wZ); -} - -void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ) -{ - // our velocity is rotated 90 deg, so y is forward/back and x is left/right - targetWheelVels[0] = (-v.y - v.x + wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s - targetWheelVels[1] = (-v.y + v.x - wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s - targetWheelVels[2] = (-v.y + v.x + wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s - targetWheelVels[3] = (-v.y - v.x - wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s -} - -Vector3f ChassisSubsystem::measureVelocity() -{ - float w1 = wheels[0].measureVelocity(); // rev/s - float w2 = wheels[1].measureVelocity(); // rev/s - float w3 = wheels[2].measureVelocity(); // rev/s - float w4 = wheels[3].measureVelocity(); // rev/s - - float xa = (w1 + w2 + w3 + w4); - float ya = (-w1 + w2 + w3 - w4); - float wa = (-w1 + w2 - w3 + w4) / WHEEL_LXY; - - // Rotated -90 deg to match our reference frame - return Vector3f(ya, -xa, wa) * WHEEL_RADIUS / 4.0f * M_TWOPI; -} -} // namespace chassis -} // namespace subsystems +#include "chassis_subsystem.hpp" + +#include "tap/algorithms/math_user_utils.hpp" +#include "tap/communication/sensors/buzzer/buzzer.hpp" + +#include "robots/robot_constants.hpp" + +using namespace tap::algorithms; + +namespace subsystems +{ +namespace chassis +{ +ChassisSubsystem::ChassisSubsystem(src::Drivers* drivers) + : tap::control::Subsystem(drivers), + drivers(drivers), + powerLimiter(drivers, ENERGY_BUFFER_LIMIT_THRESHOLD, ENERGY_BUFFER_CRIT_THRESHOLD), + wheels{ + {drivers, M3508, ID_WHEEL_LF, CAN_WHEELS, true, "left front", PID_WHEELS}, + {drivers, M3508, ID_WHEEL_RF, CAN_WHEELS, false, "right front", PID_WHEELS}, + {drivers, M3508, ID_WHEEL_LB, CAN_WHEELS, true, "left back", PID_WHEELS}, + {drivers, M3508, ID_WHEEL_RB, CAN_WHEELS, false, "right back", PID_WHEELS}, + } {}; + +void ChassisSubsystem::initialize() +{ + for (int8_t i = 0; i < WHEELS; i++) + { + wheels[i].initialize(); + } +} + +void ChassisSubsystem::refresh() +{ + for (int8_t i = 0; i < WHEELS; i++) + { + wheels[i].setActive(!drivers->isKillSwitched()); + wheels[i].update(targetWheelVels[i] / M_TWOPI); // rad/s to rev/s + } + + limitChassisPower(); +} + +void ChassisSubsystem::limitChassisPower() +{ + float powerScalar = powerLimiter.getPowerLimitRatio(); + if (compareFloatClose(1.0f, powerScalar, 1E-3)) + { + powerScalar = 1.0f; + } + + float totalError = 0.0f; + for (size_t i = 0; i < WHEELS; i++) + { + totalError += abs(wheels[i].getPid().getLastError()); + } + + for (int8_t i = 0; i < WHEELS; i++) + { + bool totalErrorZero = compareFloatClose(0.0f, totalError, 1E-3); + float velocityErrorScalar = totalErrorZero + ? (1.0f / WHEELS) + : (abs(wheels[i].getPid().getLastError()) / totalError); + float modifiedPowerScalar = + limitVal(WHEELS * powerScalar * velocityErrorScalar, 0.0f, 1.0f); + + wheels[i].setActive(!drivers->isKillSwitched()); + wheels[i].applyPowerScalar(modifiedPowerScalar); + } +} + +void ChassisSubsystem::runHardwareTests() +{ + // TODO +} + +void ChassisSubsystem::input(Vector2f move, float spin) +{ + Vector2f v = move * MAX_LINEAR_VEL; + float wZ = spin * MAX_ANGULAR_VEL; + + float linearTerm = (abs(v.x) + abs(v.y)) / WHEEL_RADIUS; + float angularTerm = abs(wZ) * WHEEL_LXY / WHEEL_RADIUS; + + // overdrive error + float overdrive = max(linearTerm + angularTerm - WHEEL_MAX_VEL, 0.0f); + + // angular velocity correction + if (angularTerm > 0.0f) + { + float correction = min(angularTerm, overdrive); + wZ *= 1.0f - correction / angularTerm; + overdrive -= correction; + } + + // linear velocity correction + if (linearTerm > 0.0f) + { + float correction = min(linearTerm, overdrive); + v *= 1.0f - correction / linearTerm; + overdrive -= correction; + } + + setMecanumWheelVelocities(v, wZ); +} + +void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ) +{ + // our velocity is rotated 90 deg, so y is forward/back and x is left/right + targetWheelVels[0] = (-v.y - v.x + wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s + targetWheelVels[1] = (-v.y + v.x - wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s + targetWheelVels[2] = (-v.y + v.x + wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s + targetWheelVels[3] = (-v.y - v.x - wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s +} + +Vector3f ChassisSubsystem::measureVelocity() +{ + float w1 = wheels[0].measureVelocity(); // rev/s + float w2 = wheels[1].measureVelocity(); // rev/s + float w3 = wheels[2].measureVelocity(); // rev/s + float w4 = wheels[3].measureVelocity(); // rev/s + + float xa = (w1 + w2 + w3 + w4); + float ya = (-w1 + w2 + w3 - w4); + float wa = (-w1 + w2 - w3 + w4) / WHEEL_LXY; + + // Rotated -90 deg to match our reference frame + return Vector3f(ya, -xa, wa) * WHEEL_RADIUS / 4.0f * M_TWOPI; +} +} // namespace chassis +} // namespace subsystems diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp index 54b510c2..13f1b5b2 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp @@ -1,71 +1,71 @@ -#ifndef CHASSIS_SUBSYSTEM_HPP_ -#define CHASSIS_SUBSYSTEM_HPP_ - -#include "tap/architecture/periodic_timer.hpp" -#include "tap/control/subsystem.hpp" - -#include "modm/math/geometry.hpp" -#include "robots/robot_constants.hpp" -#include "utils/motor_controller/motor_controller.hpp" -#include "utils/power_limiter/power_limiter.hpp" - -#include "drivers.hpp" - -using namespace tap::communication::sensors::imu; -using namespace modm; -using namespace motor_controller; - -namespace subsystems -{ -namespace chassis -{ -class ChassisSubsystem : public tap::control::Subsystem -{ -public: - ChassisSubsystem(src::Drivers* drivers); - - void initialize() override; - - void refresh() override; - - void limitChassisPower(); - - void runHardwareTests() override; - - /// @brief Update robot motion based on simple input controls. Inputs are scaled and corrected - /// to avoid over-driving motors. This logic can be adjusted to create various input schemes. - /// @param move Linear movement (magnitude should be within [0,1]) - /// @param spin Angular rotation (value should be within [-1,1]) - void input(Vector2f move, float spin); - - /// @brief Reconstruct current velocities based on measured wheel motion. - /// @return x,y is linear velocity (m/s) and z is angular velocity (rad/s) - Vector3f measureVelocity(); - - const char* getName() override { return "Chassis subsystem"; } - -private: - src::Drivers* drivers; - power_limiter::PowerLimiter powerLimiter; - MotorVelocityController wheels[WHEELS]; - float targetWheelVels[WHEELS] = {0.0f, 0.0f, 0.0f, 0.0f}; - - /// @brief Calculate and set wheel velocities for desired robot motion (based on - /// https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf). - /// @param v Linear velocity (m/s) - /// @param wZ Angular velocity (rad/s) - void setMecanumWheelVelocities(Vector2f v, float wZ); - - static constexpr float ENERGY_BUFFER_LIMIT_THRESHOLD = 60.0f; -#if defined(TARGET_STANDARD) - static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 10.0f; -#elif defined(TARGET_HERO) - static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 30.0f; -#elif defined(TARGET_SENTRY) - static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 30.0f; -#endif -}; -} // namespace chassis -} // namespace subsystems - -#endif +#ifndef CHASSIS_SUBSYSTEM_HPP_ +#define CHASSIS_SUBSYSTEM_HPP_ + +#include "tap/architecture/periodic_timer.hpp" +#include "tap/control/subsystem.hpp" + +#include "modm/math/geometry.hpp" +#include "robots/robot_constants.hpp" +#include "utils/motor_controller/motor_controller.hpp" +#include "utils/power_limiter/power_limiter.hpp" + +#include "drivers.hpp" + +using namespace tap::communication::sensors::imu; +using namespace modm; +using namespace motor_controller; + +namespace subsystems +{ +namespace chassis +{ +class ChassisSubsystem : public tap::control::Subsystem +{ +public: + ChassisSubsystem(src::Drivers* drivers); + + void initialize() override; + + void refresh() override; + + void limitChassisPower(); + + void runHardwareTests() override; + + /// @brief Update robot motion based on simple input controls. Inputs are scaled and corrected + /// to avoid over-driving motors. This logic can be adjusted to create various input schemes. + /// @param move Linear movement (magnitude should be within [0,1]) + /// @param spin Angular rotation (value should be within [-1,1]) + void input(Vector2f move, float spin); + + /// @brief Reconstruct current velocities based on measured wheel motion. + /// @return x,y is linear velocity (m/s) and z is angular velocity (rad/s) + Vector3f measureVelocity(); + + const char* getName() override { return "Chassis subsystem"; } + +private: + src::Drivers* drivers; + power_limiter::PowerLimiter powerLimiter; + MotorVelocityController wheels[WHEELS]; + float targetWheelVels[WHEELS] = {0.0f, 0.0f, 0.0f, 0.0f}; + + /// @brief Calculate and set wheel velocities for desired robot motion (based on + /// https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf). + /// @param v Linear velocity (m/s) + /// @param wZ Angular velocity (rad/s) + void setMecanumWheelVelocities(Vector2f v, float wZ); + + static constexpr float ENERGY_BUFFER_LIMIT_THRESHOLD = 60.0f; +#if defined(TARGET_STANDARD) + static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 10.0f; +#elif defined(TARGET_HERO) + static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 30.0f; +#elif defined(TARGET_SENTRY) + static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 30.0f; +#endif +}; +} // namespace chassis +} // namespace subsystems + +#endif diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp index 15ddd5d0..aa55de6b 100644 --- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp @@ -1,63 +1,63 @@ -#include "double_yaw_motor.hpp" - -#include "tap/algorithms/math_user_utils.hpp" -#include "tap/motor/dji_motor.hpp" - -namespace subsystems::turret -{ - -DoubleYawMotor::DoubleYawMotor( - src::Drivers *drivers, - MotorInterface *motor1, - MotorInterface *motor2, - Encoder *encoder, - const SmoothPidConfig &pidConfig) - : drivers(drivers), - motor1(motor1), - motor2(motor2), - encoder(encoder), - pid(pidConfig), - setpoint(0.0f, 0.0f, M_TWOPI), - currentAngle(0.0f, 0.0f, M_TWOPI) -{ -} - -void DoubleYawMotor::initialize() -{ - motor1->initialize(); - motor2->initialize(); -} - -void DoubleYawMotor::reset() -{ - motor1->setDesiredOutput(0); - motor2->setDesiredOutput(0); - pid.reset(); -} - -void DoubleYawMotor::updateMotorAngle() -{ - float encoderAngle = encoder->getAngle() * M_TWOPI; - currentAngle.setValue(encoderAngle); -} - -void DoubleYawMotor::setAngle(float desiredAngle, float dt) -{ - setpoint.setValue(desiredAngle); - - float positionControllerError = - ContiguousFloat(currentAngle.getValue(), 0, M_TWOPI).difference(setpoint.getValue()); - float rpm1 = motor1->getShaftRPM(); // rev / m - float rpm2 = motor2->getShaftRPM(); // rev / m - float avgRps = (rpm1 + rpm2) / 2.0f * (M_TWOPI / 60.0f); // rad / s - float output = pid.runController(positionControllerError, avgRps, dt); - - output = limitVal(output, -GM6020_MAX_OUTPUT, GM6020_MAX_OUTPUT); - motor1->setDesiredOutput(output); - motor2->setDesiredOutput(output); -} - -float DoubleYawMotor::getAngle() { return currentAngle.getValue(); } - -float DoubleYawMotor::getSetpoint() { return setpoint.getValue(); } +#include "double_yaw_motor.hpp" + +#include "tap/algorithms/math_user_utils.hpp" +#include "tap/motor/dji_motor.hpp" + +namespace subsystems::turret +{ + +DoubleYawMotor::DoubleYawMotor( + src::Drivers *drivers, + MotorInterface *motor1, + MotorInterface *motor2, + Encoder *encoder, + const SmoothPidConfig &pidConfig) + : drivers(drivers), + motor1(motor1), + motor2(motor2), + encoder(encoder), + pid(pidConfig), + setpoint(0.0f, 0.0f, M_TWOPI), + currentAngle(0.0f, 0.0f, M_TWOPI) +{ +} + +void DoubleYawMotor::initialize() +{ + motor1->initialize(); + motor2->initialize(); +} + +void DoubleYawMotor::reset() +{ + motor1->setDesiredOutput(0); + motor2->setDesiredOutput(0); + pid.reset(); +} + +void DoubleYawMotor::updateMotorAngle() +{ + float encoderAngle = encoder->getAngle() * M_TWOPI; + currentAngle.setValue(encoderAngle); +} + +void DoubleYawMotor::setAngle(float desiredAngle, float dt) +{ + setpoint.setValue(desiredAngle); + + float positionControllerError = + ContiguousFloat(currentAngle.getValue(), 0, M_TWOPI).difference(setpoint.getValue()); + float rpm1 = motor1->getShaftRPM(); // rev / m + float rpm2 = motor2->getShaftRPM(); // rev / m + float avgRps = (rpm1 + rpm2) / 2.0f * (M_TWOPI / 60.0f); // rad / s + float output = pid.runController(positionControllerError, avgRps, dt); + + output = limitVal(output, -GM6020_MAX_OUTPUT, GM6020_MAX_OUTPUT); + motor1->setDesiredOutput(output); + motor2->setDesiredOutput(output); +} + +float DoubleYawMotor::getAngle() { return currentAngle.getValue(); } + +float DoubleYawMotor::getSetpoint() { return setpoint.getValue(); } } // namespace subsystems::turret \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp index b13620e7..5bcd5174 100644 --- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp +++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp @@ -1,45 +1,45 @@ -#pragma once - -#include "tap/algorithms/contiguous_float.hpp" -#include "tap/algorithms/smooth_pid.hpp" -#include "tap/motor/motor_interface.hpp" - -#include "drivers/encoder.hpp" - -#include "drivers.hpp" - -namespace subsystems::turret -{ -using namespace tap::algorithms; -using namespace tap::motor; -using driver::Encoder; - -class DoubleYawMotor -{ -public: - DoubleYawMotor( - src::Drivers *drivers, - MotorInterface *motor1, - MotorInterface *motor2, - Encoder *encoder, - const SmoothPidConfig &pidConfig); - - void initialize(); - void reset(); - void updateMotorAngle(); - void setAngle(float desiredAngle, float dt); - float getAngle(); - float getSetpoint(); - -private: - src::Drivers *drivers; - MotorInterface *motor1; - MotorInterface *motor2; - Encoder *encoder; - SmoothPid pid; - ContiguousFloat setpoint; - ContiguousFloat currentAngle; - - static constexpr float GM6020_MAX_OUTPUT = 30000.0f; -}; +#pragma once + +#include "tap/algorithms/contiguous_float.hpp" +#include "tap/algorithms/smooth_pid.hpp" +#include "tap/motor/motor_interface.hpp" + +#include "drivers/encoder.hpp" + +#include "drivers.hpp" + +namespace subsystems::turret +{ +using namespace tap::algorithms; +using namespace tap::motor; +using driver::Encoder; + +class DoubleYawMotor +{ +public: + DoubleYawMotor( + src::Drivers *drivers, + MotorInterface *motor1, + MotorInterface *motor2, + Encoder *encoder, + const SmoothPidConfig &pidConfig); + + void initialize(); + void reset(); + void updateMotorAngle(); + void setAngle(float desiredAngle, float dt); + float getAngle(); + float getSetpoint(); + +private: + src::Drivers *drivers; + MotorInterface *motor1; + MotorInterface *motor2; + Encoder *encoder; + SmoothPid pid; + ContiguousFloat setpoint; + ContiguousFloat currentAngle; + + static constexpr float GM6020_MAX_OUTPUT = 30000.0f; +}; } // namespace subsystems::turret \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index b097624a..4c7972c9 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -20,7 +20,13 @@ TurretSubsystem::TurretSubsystem(src::Drivers* drivers) pitchMotor(drivers, ID_PITCH, CAN_TURRET, false, "pitch"), yawTurret(drivers, &yawMotor, YAW_PID_CONFIG), pitchTurret(drivers, &pitchMotor, PITCH_PID_CONFIG), - turretOffset(0.0f, 0.0f, M_TWOPI) + turretOffset(0.0f, 0.0f, M_TWOPI), + // placeholder motors + #if defined(TARGET_STANDARD) || defined(TARGET_HERO) + //placeholder motors, PID_Config + doubleYawTurret(drivers, &yawMotor, &pitchMotor, &encoder, YAW_PID_CONFIG) + #endif + { } diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp index 75463810..84a27c08 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp @@ -1,5 +1,4 @@ -#ifndef TURRET_SUBSYSTEM_HPP_ -#define TURRET_SUBSYSTEM_HPP_ +#pragma once #include "tap/algorithms/contiguous_float.hpp" #include "tap/control/subsystem.hpp" @@ -11,12 +10,18 @@ #include "drivers.hpp" #include "turret_motor.hpp" +// include double yaw motor, encoder +#include "drivers/as5600.hpp" +#include "double_yaw_motor.hpp" + using modm::Vector3f; namespace subsystems { namespace turret { +using driver::As5600; +using modm::platform::I2cMaster2; using tap::algorithms::ContiguousFloat; using tap::motor::DjiMotor; @@ -24,7 +29,6 @@ class TurretSubsystem : public tap::control::Subsystem { public: TurretSubsystem(src::Drivers* drivers); - void initialize() override; /// @brief Input target data from CV (relative to camera) @@ -65,7 +69,10 @@ class TurretSubsystem : public tap::control::Subsystem DjiMotor yawMotor; DjiMotor pitchMotor; + + TurretMotor yawTurret; + TurretMotor pitchTurret; float isCalibrated = false; @@ -73,9 +80,14 @@ class TurretSubsystem : public tap::control::Subsystem float baseYaw = 0.0f; ContiguousFloat turretOffset; + + #if defined(TARGET_STANDARD) || defined(TARGET_HERO) + DoubleYawMotor doubleYawTurret; + As5600 encoder; + #endif + }; } // namespace turret } // namespace subsystems -#endif