diff --git a/taproot b/taproot index 6d87b001..22b708e6 160000 --- a/taproot +++ b/taproot @@ -1 +1 @@ -Subproject commit 6d87b001bb3d9d4ca67fe6bbdf31877ff46aa570 +Subproject commit 22b708e6e1833464914ff7b2279b0cd7214e023f diff --git a/ut-robomaster/project.xml b/ut-robomaster/project.xml index 47eb9af8..7a9c89cf 100644 --- a/ut-robomaster/project.xml +++ b/ut-robomaster/project.xml @@ -29,6 +29,22 @@ + + + + taproot:core diff --git a/ut-robomaster/src/main.cpp b/ut-robomaster/src/main.cpp index 73c64ad7..21f43fe3 100644 --- a/ut-robomaster/src/main.cpp +++ b/ut-robomaster/src/main.cpp @@ -42,6 +42,12 @@ static void updateIo(src::Drivers *drivers) drivers->remote.read(); } +static void updateImu(src::Drivers *drivers) +{ + drivers->bmi088.read(); + drivers->bmi088.periodicIMUUpdate(); +} + src::Drivers drivers; RobotControl control{&drivers}; @@ -61,7 +67,7 @@ int main() if (refreshTimer.execute()) { - PROFILE(drivers.profiler, drivers.bmi088.periodicIMUUpdate, ()); + PROFILE(drivers.profiler, updateImu, (&drivers)); PROFILE(drivers.profiler, drivers.commandScheduler.run, ()); PROFILE(drivers.profiler, drivers.djiMotorTxHandler.encodeAndSendCanData, ()); PROFILE(drivers.profiler, drivers.terminalSerial.update, ()); diff --git a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp index e4751225..34aecc8d 100644 --- a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp +++ b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp @@ -2,13 +2,9 @@ namespace subsystems::agitator { - using tap::algorithms::compareFloatClose; using tap::arch::clock::getTimeMilliseconds; -/** - * AgitatorSubsystem class instantiation - */ #if defined(TARGET_STANDARD) || defined(TARGET_SENTRY) AgitatorSubsystem::AgitatorSubsystem( src::Drivers* drivers, diff --git a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.hpp b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.hpp index 678c5097..0687e64c 100644 --- a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.hpp +++ b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.hpp @@ -9,9 +9,7 @@ #include "drivers.hpp" -namespace subsystems -{ -namespace agitator +namespace subsystems::agitator { using flywheel::FlywheelSubsystem; using motors::MotorController; @@ -20,11 +18,7 @@ class AgitatorSubsystem : public tap::control::Subsystem { public: AgitatorSubsystem(src::Drivers *drivers, FlywheelSubsystem *flywheel, MotorConfig motor); - - ~AgitatorSubsystem() = default; - void initialize() override; - void refresh() override; float getShapedVelocity(float time, float a, float phi, float ballsPerSecond); @@ -44,6 +38,4 @@ class AgitatorSubsystem : public tap::control::Subsystem MotorController feeder; #endif }; - -} // namespace agitator -} // namespace subsystems +} // namespace subsystems::agitator diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index 9fa28cf5..ff31882b 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp @@ -4,12 +4,10 @@ #include "robots/robot_constants.hpp" +namespace subsystems::chassis +{ using namespace tap::algorithms; -namespace subsystems -{ -namespace chassis -{ ChassisSubsystem::ChassisSubsystem(src::Drivers* drivers) : tap::control::Subsystem(drivers), drivers(drivers), @@ -66,11 +64,6 @@ void ChassisSubsystem::limitChassisPower() } } -void ChassisSubsystem::runHardwareTests() -{ - // TODO -} - void ChassisSubsystem::input(Vector2f move, float spin) { Vector2f v = move * MAX_LINEAR_VEL; @@ -124,5 +117,4 @@ Vector3f ChassisSubsystem::measureVelocity() // Rotated -90 deg to match our reference frame return Vector3f(ya, -xa, wa) * WHEEL_RADIUS / 4.0f * M_TWOPI; } -} // namespace chassis -} // namespace subsystems +} // namespace subsystems::chassis diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp index b134d0d2..dc75dfb8 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp @@ -1,5 +1,4 @@ -#ifndef CHASSIS_SUBSYSTEM_HPP_ -#define CHASSIS_SUBSYSTEM_HPP_ +#pragma once #include "tap/control/subsystem.hpp" @@ -10,27 +9,21 @@ #include "drivers.hpp" +namespace subsystems::chassis +{ using namespace tap::communication::sensors::imu; using namespace modm; using motors::MotorController; -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]) @@ -41,8 +34,6 @@ class ChassisSubsystem : public tap::control::Subsystem /// @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; @@ -64,7 +55,4 @@ class ChassisSubsystem : public tap::control::Subsystem static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 30.0f; #endif }; -} // namespace chassis -} // namespace subsystems - -#endif +} // namespace subsystems::chassis diff --git a/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.hpp b/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.hpp index aa313678..9a74f8d1 100644 --- a/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.hpp +++ b/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.hpp @@ -1,5 +1,4 @@ -#ifndef SUBSYSTEMS_SHOOTER_FLYWHEEL_SUBSYSTEM_HPP_ -#define SUBSYSTEMS_SHOOTER_FLYWHEEL_SUBSYSTEM_HPP_ +#pragma once #include "tap/control/subsystem.hpp" @@ -8,22 +7,15 @@ #include "drivers.hpp" -namespace subsystems +namespace subsystems::flywheel { -namespace flywheel -{ - using motors::MotorController; class FlywheelSubsystem : public tap::control::Subsystem { public: FlywheelSubsystem(src::Drivers* drivers); - - ~FlywheelSubsystem() = default; - void initialize() override; - void refresh() override; /// @brief Change flywheel velocity. @@ -37,8 +29,4 @@ class FlywheelSubsystem : public tap::control::Subsystem MotorController motors[FLYWHEELS]; float velocity = 0.0f; }; - -} // namespace flywheel -} // namespace subsystems - -#endif // SUBSYSTEMS_SHOOTER_FLYWHEEL_SUBSYSTEM_HPP_ +} // namespace subsystems::flywheel diff --git a/ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp b/ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp index 89e62bc6..4d5c95fa 100644 --- a/ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp +++ b/ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp @@ -1,9 +1,8 @@ #include "odometry_subsystem.hpp" #include "robots/robot_constants.hpp" -namespace subsystems -{ -namespace odometry + +namespace subsystems::odometry { OdometrySubsystem::OdometrySubsystem( src::Drivers* drivers, @@ -15,9 +14,9 @@ OdometrySubsystem::OdometrySubsystem( turret(turret), chassisDisplacement(drivers, chassis), chassisYaw(drivers), - chassisTracker(&chassisYaw, &chassisDisplacement){}; + chassisTracker(&chassisYaw, &chassisDisplacement) {}; -void OdometrySubsystem::initialize(){}; +void OdometrySubsystem::initialize() {}; void OdometrySubsystem::refresh() { chassisTracker.update(); } Vector2f OdometrySubsystem::getPosition() @@ -31,5 +30,4 @@ float OdometrySubsystem::getChassisAngularVelocity() { return chassis->measureVe float OdometrySubsystem::getTurretLocalYaw() { return turret->getCurrentLocalPitch(); } float OdometrySubsystem::getTurretLocalPitch() { return turret->getCurrentLocalYaw(); } -}; // namespace odometry -} // namespace subsystems \ No newline at end of file +} // namespace subsystems::odometry \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp b/ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp index 71f15e80..098243c3 100644 --- a/ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp +++ b/ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp @@ -15,12 +15,10 @@ #include "drivers.hpp" -namespace subsystems +namespace subsystems::odometry { -namespace odometry -{ - using chassis::ChassisSubsystem; +using modm::Vector2f; using tap::algorithms::odometry::Odometry2DTracker; using turret::TurretSubsystem; @@ -30,7 +28,6 @@ class OdometrySubsystem : public tap::control::Subsystem OdometrySubsystem(src::Drivers* drivers, ChassisSubsystem* chassis, TurretSubsystem* turret); void initialize() override; void refresh() override; - const char* getName() override { return "Odometry subsystem"; } Vector2f getPosition(); Vector2f getLinearVelocity(); @@ -50,5 +47,4 @@ class OdometrySubsystem : public tap::control::Subsystem ChassisWorldYawObserver chassisYaw; Odometry2DTracker chassisTracker; }; -} // namespace odometry -} // namespace subsystems \ No newline at end of file +} // namespace subsystems::odometry \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/sound/sound_subsystem.cpp b/ut-robomaster/src/subsystems/sound/sound_subsystem.cpp index b7309ccd..c2df5ef1 100644 --- a/ut-robomaster/src/subsystems/sound/sound_subsystem.cpp +++ b/ut-robomaster/src/subsystems/sound/sound_subsystem.cpp @@ -1,8 +1,11 @@ #include "sound_subsystem.hpp" -namespace subsystems -{ -namespace sound + +#include "tap/communication/sensors/buzzer/buzzer.hpp" + +namespace subsystems::sound { +using tap::gpio::Pwm; + SoundSubsystem::SoundSubsystem(src::Drivers* drivers) : tap::control::Subsystem(drivers), drivers(drivers) @@ -10,7 +13,6 @@ SoundSubsystem::SoundSubsystem(src::Drivers* drivers) } void SoundSubsystem::initialize() {} - void SoundSubsystem::refresh() {} void SoundSubsystem::silence() { tap::buzzer::silenceBuzzer(&drivers->pwm); } @@ -18,7 +20,5 @@ void SoundSubsystem::silence() { tap::buzzer::silenceBuzzer(&drivers->pwm); } void SoundSubsystem::setBuzzerFrequency(int frequency) { tap::buzzer::playNote(&drivers->pwm, frequency); - // define keys with numerical frequencies } -}; // namespace sound -} // namespace subsystems +} // namespace subsystems::sound diff --git a/ut-robomaster/src/subsystems/sound/sound_subsystem.hpp b/ut-robomaster/src/subsystems/sound/sound_subsystem.hpp index 0b615ab8..2f88c89b 100644 --- a/ut-robomaster/src/subsystems/sound/sound_subsystem.hpp +++ b/ut-robomaster/src/subsystems/sound/sound_subsystem.hpp @@ -1,24 +1,17 @@ #pragma once -// #include "tap/communication/gpio/pwm.hpp" -#include "tap/communication/sensors/buzzer/buzzer.hpp" #include "tap/control/subsystem.hpp" #include "drivers.hpp" -namespace subsystems +namespace subsystems::sound { -namespace sound -{ -using tap::gpio::Pwm; - class SoundSubsystem : public tap::control::Subsystem { public: SoundSubsystem(src::Drivers* drivers); void initialize() override; void refresh() override; - const char* getName() override { return "Sound subsystem"; } void setBuzzerFrequency(int frequency); void silence(); @@ -26,5 +19,4 @@ class SoundSubsystem : public tap::control::Subsystem private: src::Drivers* drivers; }; -} // namespace sound } // namespace subsystems \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp b/ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp index 18befc57..73de592a 100644 --- a/ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp +++ b/ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp @@ -6,6 +6,7 @@ namespace commands { using namespace tap::algorithms::ballistics; using communication::TurretData; +using modm::Vector3f; void CommandMoveTurretAimbot::initialize() {} @@ -40,7 +41,7 @@ void CommandMoveTurretAimbot::execute() targetVel = rotMat * targetVel; targetAcc = rotMat * targetAcc; - MeasuredKinematicState kinState{targetPos, targetVel, targetAcc}; + SecondOrderKinematicState kinState{targetPos, targetVel, targetAcc}; float turretPitch = 0.0f; float turretYaw = 0.0f; diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 9d770935..21cc285f 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -1,18 +1,10 @@ #include "turret_subsystem.hpp" -#include "tap/algorithms/ballistics.hpp" - #include "modm/math.hpp" #include "robots/robot_constants.hpp" -namespace subsystems -{ -namespace turret +namespace subsystems::turret { -using namespace tap::algorithms::ballistics; -using communication::TurretData; -using modm::Vector2f; - TurretSubsystem::TurretSubsystem(src::Drivers* drivers) : tap::control::Subsystem(drivers), drivers(drivers), @@ -91,10 +83,4 @@ float TurretSubsystem::getCurrentLocalPitch() } bool TurretSubsystem::getIsCalibrated() { return isCalibrated; } - -void TurretSubsystem::runHardwareTests() -{ - // TODO -} -} // namespace turret -} // namespace subsystems \ No newline at end of file +} // namespace subsystems::turret \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp index e793836f..8bbae511 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp @@ -4,7 +4,6 @@ #include "tap/control/subsystem.hpp" #include "drivers/as5600.hpp" -#include "modm/math/filter/moving_average.hpp" #include "modm/math/geometry.hpp" #include "robots/robot_constants.hpp" @@ -12,13 +11,10 @@ #include "drivers.hpp" #include "turret_motor.hpp" -using modm::Vector3f; - -namespace subsystems -{ -namespace turret +namespace subsystems::turret { using driver::As5600; +using modm::Vector3f; using tap::algorithms::ContiguousFloat; class TurretSubsystem : public tap::control::Subsystem @@ -26,34 +22,20 @@ class TurretSubsystem : public tap::control::Subsystem public: TurretSubsystem(src::Drivers* drivers); void initialize() override; + void refresh() override; /// @brief Input target data from CV (relative to camera) void inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration); - void setTargetWorldAngles(float yaw, float pitch); - float getChassisYaw(); - float getTargetLocalYaw(); - float getTargetLocalPitch(); - float getTargetWorldYaw() { return targetWorldYaw; } - float getTargetWorldPitch() { return targetWorldPitch; } - float getCurrentLocalYaw(); - float getCurrentLocalPitch(); - bool getIsCalibrated(); - void refresh() override; - - void runHardwareTests() override; - - const char* getName() override { return "Turret subsystem"; } - private: src::Drivers* drivers; @@ -78,6 +60,4 @@ class TurretSubsystem : public tap::control::Subsystem ContiguousFloat turretOffset; }; - -} // namespace turret -} // namespace subsystems +} // namespace subsystems::turret diff --git a/ut-robomaster/taproot/modm/ext/tlsf/tlsf.h b/ut-robomaster/taproot/modm/ext/tlsf/tlsf.h index 34675dbf..9a7b5cdb 100644 --- a/ut-robomaster/taproot/modm/ext/tlsf/tlsf.h +++ b/ut-robomaster/taproot/modm/ext/tlsf/tlsf.h @@ -11,10 +11,10 @@ ** ** This implementation was written to the specification ** of the document, therefore no GPL restrictions apply. -** +** ** Copyright (c) 2006-2016, Matthew Conte ** All rights reserved. -** +** ** Redistribution and use in source and binary forms, with or without ** modification, are permitted provided that the following conditions are met: ** * Redistributions of source code must retain the above copyright @@ -25,7 +25,7 @@ ** * Neither the name of the nor the ** names of its contributors may be used to endorse or promote products ** derived from this software without specific prior written permission. -** +** ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ** ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED ** WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -41,48 +41,47 @@ #include #if defined(__cplusplus) -extern "C" -{ +extern "C" { #endif - /* tlsf_t: a TLSF structure. Can contain 1 to N pools. */ - /* pool_t: a block of memory that TLSF can manage. */ - typedef void* tlsf_t; - typedef void* pool_t; +/* tlsf_t: a TLSF structure. Can contain 1 to N pools. */ +/* pool_t: a block of memory that TLSF can manage. */ +typedef void* tlsf_t; +typedef void* pool_t; - /* Create/destroy a memory pool. */ - tlsf_t tlsf_create(void* mem); - tlsf_t tlsf_create_with_pool(void* mem, size_t bytes); - void tlsf_destroy(tlsf_t tlsf); - pool_t tlsf_get_pool(tlsf_t tlsf); +/* Create/destroy a memory pool. */ +tlsf_t tlsf_create(void* mem); +tlsf_t tlsf_create_with_pool(void* mem, size_t bytes); +void tlsf_destroy(tlsf_t tlsf); +pool_t tlsf_get_pool(tlsf_t tlsf); - /* Add/remove memory pools. */ - pool_t tlsf_add_pool(tlsf_t tlsf, void* mem, size_t bytes); - void tlsf_remove_pool(tlsf_t tlsf, pool_t pool); +/* Add/remove memory pools. */ +pool_t tlsf_add_pool(tlsf_t tlsf, void* mem, size_t bytes); +void tlsf_remove_pool(tlsf_t tlsf, pool_t pool); - /* malloc/memalign/realloc/free replacements. */ - void* tlsf_malloc(tlsf_t tlsf, size_t bytes); - void* tlsf_memalign(tlsf_t tlsf, size_t align, size_t bytes); - void* tlsf_realloc(tlsf_t tlsf, void* ptr, size_t size); - void tlsf_free(tlsf_t tlsf, void* ptr); +/* malloc/memalign/realloc/free replacements. */ +void* tlsf_malloc(tlsf_t tlsf, size_t bytes); +void* tlsf_memalign(tlsf_t tlsf, size_t align, size_t bytes); +void* tlsf_realloc(tlsf_t tlsf, void* ptr, size_t size); +void tlsf_free(tlsf_t tlsf, void* ptr); - /* Returns internal block size, not original request size */ - size_t tlsf_block_size(void* ptr); +/* Returns internal block size, not original request size */ +size_t tlsf_block_size(void* ptr); - /* Overheads/limits of internal structures. */ - size_t tlsf_size(void); - size_t tlsf_align_size(void); - size_t tlsf_block_size_min(void); - size_t tlsf_block_size_max(void); - size_t tlsf_pool_overhead(void); - size_t tlsf_alloc_overhead(void); +/* Overheads/limits of internal structures. */ +size_t tlsf_size(void); +size_t tlsf_align_size(void); +size_t tlsf_block_size_min(void); +size_t tlsf_block_size_max(void); +size_t tlsf_pool_overhead(void); +size_t tlsf_alloc_overhead(void); - /* Debugging. */ - typedef void (*tlsf_walker)(void* ptr, size_t size, int used, void* user); - void tlsf_walk_pool(pool_t pool, tlsf_walker walker, void* user); - /* Returns nonzero if any internal consistency check fails. */ - int tlsf_check(tlsf_t tlsf); - int tlsf_check_pool(pool_t pool); +/* Debugging. */ +typedef void (*tlsf_walker)(void* ptr, size_t size, int used, void* user); +void tlsf_walk_pool(pool_t pool, tlsf_walker walker, void* user); +/* Returns nonzero if any internal consistency check fails. */ +int tlsf_check(tlsf_t tlsf); +int tlsf_check_pool(pool_t pool); #if defined(__cplusplus) }; diff --git a/ut-robomaster/taproot/project.xml b/ut-robomaster/taproot/project.xml index 9a49a9f9..e119fbfa 100644 --- a/ut-robomaster/taproot/project.xml +++ b/ut-robomaster/taproot/project.xml @@ -11,9 +11,9 @@ - - + + @@ -62,5 +62,6 @@ modm:platform:adc:3 modm:platform:i2c:2 modm:platform:rtt + \ No newline at end of file diff --git a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h index 500803b2..20c741ec 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h +++ b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h @@ -85,7 +85,8 @@ class Mahony float getYaw() { if (!anglesComputed) computeAngles(); - return yaw * 57.29578f + 180.0f; + float yawDegrees = yaw * 57.29578f; + return fmod(yawDegrees + 360.0f, 360.0f); } float getRollRadians() { diff --git a/ut-robomaster/taproot/src/tap/algorithms/ballistics.cpp b/ut-robomaster/taproot/src/tap/algorithms/ballistics.cpp index ccfcd2e3..7dc43552 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/ballistics.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/ballistics.cpp @@ -74,7 +74,7 @@ bool computeTravelTime( } bool findTargetProjectileIntersection( - MeasuredKinematicState targetInitialState, + const AbstractKinematicState &targetInitialState, float bulletVelocity, uint8_t numIterations, float *turretPitch, @@ -82,7 +82,7 @@ bool findTargetProjectileIntersection( float *projectedTravelTime, const float pitchAxisOffset) { - modm::Vector3f projectedTargetPosition = targetInitialState.position; + modm::Vector3f projectedTargetPosition = targetInitialState.projectForward(0); if (projectedTargetPosition.x == 0 && projectedTargetPosition.y == 0 && projectedTargetPosition.z == 0) diff --git a/ut-robomaster/taproot/src/tap/algorithms/ballistics.hpp b/ut-robomaster/taproot/src/tap/algorithms/ballistics.hpp index ddb6b433..89e98602 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/ballistics.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/ballistics.hpp @@ -36,11 +36,10 @@ namespace tap::algorithms::ballistics * - Velocity Units: m/s * - Acceleration Units: m/s^2 */ -struct MeasuredKinematicState + +struct AbstractKinematicState { - modm::Vector3f position; // m - modm::Vector3f velocity; // m/s - modm::Vector3f acceleration; // m/s^2 + virtual modm::Vector3f projectForward(float dt) const = 0; /** * @param[in] dt: The amount of time to project forward. @@ -54,6 +53,23 @@ struct MeasuredKinematicState { return s + v * dt + 0.5f * a * powf(dt, 2.0f); } +}; + +struct SecondOrderKinematicState : public AbstractKinematicState +{ + inline SecondOrderKinematicState( + modm::Vector3f position, + modm::Vector3f velocity, + modm::Vector3f acceleration) + : position(position), + velocity(velocity), + acceleration(acceleration) + { + } + + modm::Vector3f position; // m + modm::Vector3f velocity; // m/s + modm::Vector3f acceleration; // m/s^2 /** * @param[in] dt: The amount of time to project the state forward. @@ -61,7 +77,7 @@ struct MeasuredKinematicState * @return The future 3D position of this object using a quadratic (constant acceleration) * model. */ - inline modm::Vector3f projectForward(float dt) + inline modm::Vector3f projectForward(float dt) const override { return modm::Vector3f( quadraticKinematicProjection(dt, position.x, velocity.x, acceleration.x), @@ -117,7 +133,7 @@ bool computeTravelTime( * @return Whether or not a valid aiming solution was found. Out parameters only valid if true. */ bool findTargetProjectileIntersection( - MeasuredKinematicState targetInitialState, + const AbstractKinematicState &targetInitialState, float bulletVelocity, uint8_t numIterations, float *turretPitch, diff --git a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp index 71ca5659..5d69b46f 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp @@ -93,7 +93,7 @@ class LinearInterpolationPredictorContiguous uint32_t lastUpdateCallTime; ///< The previous timestamp from when update was called. ContiguousFloat previousValue; ///< The previous data value. float slope; ///< The current slope, calculated using the previous and most current data. -}; // class LinearInterpolationPredictorContiguous +}; // class LinearInterpolationPredictorContiguous } // namespace tap::algorithms diff --git a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_wrapped.cpp b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_wrapped.cpp new file mode 100644 index 00000000..9200194e --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_wrapped.cpp @@ -0,0 +1,55 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#include "linear_interpolation_predictor_wrapped.hpp" + +namespace tap::algorithms +{ +LinearInterpolationPredictorWrapped::LinearInterpolationPredictorWrapped( + float lowerBound, + float upperBound) + : lastUpdateCallTime(0), + previousValue(0.0f, lowerBound, upperBound), + slope(0.0f) +{ +} + +void LinearInterpolationPredictorWrapped::update(float newValue, uint32_t currTime) +{ + if (currTime <= lastUpdateCallTime) + { + slope = 0; + return; + } + slope = (previousValue.minDifference(newValue)) / (currTime - lastUpdateCallTime); + previousValue.setWrappedValue(newValue); + lastUpdateCallTime = currTime; +} + +void LinearInterpolationPredictorWrapped::reset(float initialValue, uint32_t initialTime) +{ + previousValue.setWrappedValue(initialValue); + lastUpdateCallTime = initialTime; + slope = 0.0f; +} +} // namespace tap::algorithms diff --git a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_wrapped.hpp b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_wrapped.hpp new file mode 100644 index 00000000..d1f0697c --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_wrapped.hpp @@ -0,0 +1,100 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_WRAPPED_HPP_ +#define TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_WRAPPED_HPP_ + +#include + +#include "wrapped_float.hpp" + +namespace tap::algorithms +{ +/** + * An object that is similar in every respect to the `LinearInterpolationPredictor` + * object except that it uses `WrappedFloat`'s instead. + */ +class LinearInterpolationPredictorWrapped +{ +public: + /** + * @param[in] lowerBound Lower bound for linear interpolation WrappedFloat. + * @param[in] upperBound Upper bound for linear interpolation WrappedFloat. + */ + LinearInterpolationPredictorWrapped(float lowerBound, float upperBound); + + /** + * Updates the interpolation using the newValue. + * + * @note Only call this when you receive a new value (use remote rx + * counter to tell when there is new data from the remote, for + * example). + * @note This function should be called with increasing values of `currTime`. + * @param[in] newValue The new data used in the interpolation. + * @param[in] currTime The time that this function was called. + */ + void update(float newValue, uint32_t currTime); + + /** + * Returns the current value, that is: \f$y\f$ in the equation + * \f$y=slope\cdot (currTime - lastUpdateCallTime) + previousValue\f$ + * in the units of whatever value you are inputting in the `update` function. + * + * @note Slope is defined by the previous two values passed into the `update` + * function, a period preceeding `lastUpdateCallTime`. + * @note Use a millisecond-resolution timer, e.g. + * `tap::arch::clock::getTimeMilliseconds()`. + * @param[in] currTime The current clock time, in ms. + * @return The interpolated value. + */ + float getInterpolatedValue(uint32_t currTime) + { + return WrappedFloat( + slope * static_cast(currTime - lastUpdateCallTime) + + previousValue.getWrappedValue(), + previousValue.getLowerBound(), + previousValue.getUpperBound()) + .getWrappedValue(); + } + + /** + * Resets the predictor. The slope will be reset to 0 and the initial values + * and time will be used to initialize the predictor. + * + * @note It is highly recommended that you call this function before calling + * `update` to "initialize" the system. + * + * @param[in] initialValue The value to set the previous value to when resetting. + * @param[in] initialTime The value to set the previous time to when resetting. + */ + void reset(float initialValue, uint32_t initialTime); + +private: + uint32_t lastUpdateCallTime; ///< The previous timestamp from when update was called. + WrappedFloat previousValue; ///< The previous data value. + float slope; ///< The current slope, calculated using the previous and most current data. +}; // class LinearInterpolationPredictorWrapped + +} // namespace tap::algorithms + +#endif // TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_WRAPPED_HPP_ diff --git a/ut-robomaster/taproot/src/tap/algorithms/math_user_utils.cpp b/ut-robomaster/taproot/src/tap/algorithms/math_user_utils.cpp index 11fa1675..3b16c387 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/math_user_utils.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/math_user_utils.cpp @@ -70,3 +70,25 @@ tap::algorithms::CMSISMat<3, 3> tap::algorithms::fromEulerAngles( cosf(pitch) * sinf(roll), cosf(pitch) * cosf(roll)}); } + +modm::Vector3f tap::algorithms::eulerAnglesFromQuaternion(modm::Quaternion& q) +{ + modm::Vector3f eulerAngles; + + // roll (x-axis rotation) + float sinr_cosp = 2 * (q.w * q.x + q.y * q.z); + float cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y); + eulerAngles.x = std::atan2(sinr_cosp, cosr_cosp); + + // pitch (y-axis rotation) + float sinp = std::sqrt(1 + 2 * (q.w * q.y - q.x * q.z)); + float cosp = std::sqrt(1 - 2 * (q.w * q.y - q.x * q.z)); + eulerAngles.y = 2 * std::atan2(sinp, cosp) - M_PI / 2; + + // yaw (z-axis rotation) + float siny_cosp = 2 * (q.w * q.z + q.x * q.y); + float cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); + eulerAngles.z = std::atan2(siny_cosp, cosy_cosp); + + return eulerAngles; +} diff --git a/ut-robomaster/taproot/src/tap/algorithms/math_user_utils.hpp b/ut-robomaster/taproot/src/tap/algorithms/math_user_utils.hpp index aa45afd9..c722cc93 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/math_user_utils.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/math_user_utils.hpp @@ -31,6 +31,8 @@ #include "modm/architecture/interface/assert.hpp" #include "modm/math/geometry/angle.hpp" +#include "modm/math/geometry/quaternion.hpp" +#include "modm/math/geometry/vector3.hpp" #include "cmsis_mat.hpp" @@ -165,6 +167,11 @@ constexpr int32_t ceil(float num) : static_cast(num) + ((num > 0) ? 1 : 0); } +/** + * Returns decoded from q + */ +modm::Vector3f eulerAnglesFromQuaternion(modm::Quaternion& q); + /** * Returns the sign of the value passed in. Either -1, 0, or 1. Works for all base types and any * types that implement construction from an int. Faster than copysign. diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/position.cpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/position.cpp index 8fcc6b39..0da17be3 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/position.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/position.cpp @@ -27,14 +27,42 @@ namespace tap::algorithms::transforms { -inline Vector Position::operator-(const Vector& other) const +Position Position::operator-(const Vector& other) const +{ + return Position(this->coordinates_ - other.coordinates()); +} + +Vector Position::operator-(const Position& other) const { return Vector(this->coordinates_ - other.coordinates()); } -inline Position Position::operator+(const Position& vector) const +Position Position::operator+(const Vector& vector) const +{ + return Position(this->coordinates_ + vector.coordinates()); +} + +Position Position::operator+(const Position& vector) const { return Position(this->coordinates_ + vector.coordinates_); } +Position Position::operator*(const float scalar) const +{ + return Position(this->coordinates_ * scalar); +} + +Position& Position::operator=(const Position& other) +{ + this->coordinates_ = other.coordinates_; + return *this; +} + +bool Position::operator==(const Position& other) const +{ + return this->coordinates_.data == other.coordinates_.data; +} + +float Position::distance(const Position& a, const Position& b) { return (b - a).magnitude(); } + } // namespace tap::algorithms::transforms diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp index f6637127..227a8fa9 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp @@ -28,6 +28,7 @@ namespace tap::algorithms::transforms { +// Forward declaration to avoid circular dependency class Vector; class Position @@ -54,12 +55,29 @@ class Position inline float z() const { return coordinates_.data[2]; } /* Operators */ - inline Vector operator-(const Vector& other) const; + Position operator-(const Vector& other) const; - inline Position operator+(const Position& vector) const; + Vector operator-(const Position& other) const; + + Position operator+(const Vector& vector) const; + + Position operator+(const Position& vector) const; + + Position operator*(const float scalar) const; + + Position& operator=(const Position& other); + + bool operator==(const Position& other) const; inline CMSISMat<3, 1> coordinates() const { return this->coordinates_; } + static inline Position interpolate(const Position& a, const Position& b, const float t) + { + return a * (1 - t) + b * t; + } + + static float distance(const Position& a, const Position& b); + private: CMSISMat<3, 1> coordinates_; }; // class Position diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp index 0f9ad7f1..d4bb2df9 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp @@ -25,7 +25,6 @@ #define TAPROOT_VECTOR_HPP_ #include "tap/algorithms/cmsis_mat.hpp" -#include "tap/algorithms/transforms/position.hpp" namespace tap::algorithms::transforms { @@ -51,16 +50,31 @@ class Vector inline float z() const { return coordinates_.data[2]; } + inline Vector& operator=(const Vector& other) + { + this->coordinates_ = other.coordinates_; + return *this; + } + inline Vector operator+(const Vector& other) const; inline Vector operator+(const Position& other) const; inline Vector operator*(const float scale) const { return Vector(this->coordinates_ * scale); } + inline static float dot(const Vector& a, const Vector& b) + { + return a.x() * b.x() + a.y() * b.y() + a.z() * b.z(); + } + + inline float dot(const Vector& other) const { return dot(*this, other); } + inline Vector operator/(const float scale) const { return Vector(this->coordinates_ / scale); } const inline CMSISMat<3, 1>& coordinates() const { return coordinates_; } + inline float magnitude() const { return sqrt(dot(*this, *this)); } + private: CMSISMat<3, 1> coordinates_; }; // class Vector diff --git a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp new file mode 100644 index 00000000..e040f579 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp @@ -0,0 +1,229 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#include "wrapped_float.hpp" + +namespace tap +{ +namespace algorithms +{ +WrappedFloat::WrappedFloat(const float value, const float lowerBound, const float upperBound) + : wrapped(value), + lowerBound(lowerBound), + upperBound(upperBound) +{ + assert(upperBound > lowerBound); + + wrapValue(); +} + +bool WrappedFloat::operator==(const WrappedFloat& other) const +{ + assertBoundsEqual(other); + + return this->wrapped == other.wrapped; +} + +void WrappedFloat::operator+=(const WrappedFloat& other) +{ + assertBoundsEqual(other); + + this->wrapped += other.wrapped; + wrapValue(); + this->revolutions += other.revolutions; +} + +void WrappedFloat::operator-=(const WrappedFloat& other) +{ + assertBoundsEqual(other); + + this->wrapped -= other.wrapped; + wrapValue(); + this->revolutions -= other.revolutions; +} + +WrappedFloat WrappedFloat::operator+(const WrappedFloat& other) const +{ + assertBoundsEqual(other); + + WrappedFloat temp(*this); + temp += other; + return temp; +} + +WrappedFloat WrappedFloat::operator-(const WrappedFloat& other) const +{ + assertBoundsEqual(other); + + WrappedFloat temp = *this; + temp -= other; + return temp; +} + +void WrappedFloat::operator+=(float value) { *this += this->withSameBounds(value); } + +void WrappedFloat::operator-=(float value) { *this -= this->withSameBounds(value); } + +WrappedFloat WrappedFloat::operator+(float value) const +{ + return *this + this->withSameBounds(value); +} + +WrappedFloat WrappedFloat::operator-(float value) const +{ + return *this - this->withSameBounds(value); +} + +float WrappedFloat::minDifference(const WrappedFloat& other) const +{ + assertBoundsEqual(other); + + float interval = this->getUpperBound() - this->getLowerBound(); + float difference_between = other.getWrappedValue() - this->getWrappedValue(); + float difference_around = + difference_between + ((difference_between < 0) ? interval : -interval); + return (abs(difference_between) < abs(difference_around)) ? difference_between + : difference_around; +} + +float WrappedFloat::minDifference(const float& unwrappedValue) const +{ + return minDifference(this->withSameBounds(unwrappedValue)); +} + +WrappedFloat WrappedFloat::minInterpolate(const WrappedFloat& other, const float alpha) const +{ + assertBoundsEqual(other); + + return *this + (minDifference(other) * alpha); +} + +void WrappedFloat::shiftBounds(const float shiftMagnitude) +{ + upperBound += shiftMagnitude; + lowerBound += shiftMagnitude; + wrapValue(); +} + +void WrappedFloat::wrapValue() +{ + float oldValue = wrapped; + if (oldValue < lowerBound) + { + this->wrapped = upperBound + fmodf(oldValue - upperBound, upperBound - lowerBound); + } + else if (oldValue >= upperBound) + { + this->wrapped = lowerBound + fmodf(oldValue - lowerBound, upperBound - lowerBound); + } + this->revolutions += floor((oldValue - lowerBound) / (upperBound - lowerBound)); +} + +float WrappedFloat::limitValue( + const WrappedFloat& valueToLimit, + const float min, + const float max, + int* status) +{ + WrappedFloat minWrapped = valueToLimit.withSameBounds(min); + WrappedFloat maxWrapped = valueToLimit.withSameBounds(max); + return limitValue(valueToLimit, minWrapped, maxWrapped, status); +} + +float WrappedFloat::limitValue( + const WrappedFloat& valueToLimit, + const WrappedFloat& min, + const WrappedFloat& max, + int* status) +{ + WrappedFloat::assertBoundsEqual(min, max); + WrappedFloat::assertBoundsEqual(valueToLimit, min); + + if (min.getWrappedValue() == max.getWrappedValue()) + { + return valueToLimit.getWrappedValue(); + } + if (!valueToLimit.withinRange(min, max)) + { + // valueToLimit is not "within" min and max + float targetMinDifference = valueToLimit.minDifference(min); + float targetMaxDifference = valueToLimit.minDifference(max); + + if (fabs(targetMinDifference) < fabs(targetMaxDifference)) + { + *status = 1; + return min.getWrappedValue(); + } + else + { + *status = 2; + return max.getWrappedValue(); + } + } + else + { + *status = 0; + return valueToLimit.getWrappedValue(); + } +} + +bool WrappedFloat::withinRange(const WrappedFloat& lowerBound, const WrappedFloat& upperBound) const +{ + return (lowerBound.getWrappedValue() < upperBound.getWrappedValue() && + (this->getWrappedValue() > lowerBound.getWrappedValue() && + this->getWrappedValue() < upperBound.getWrappedValue())) || + (lowerBound.getWrappedValue() > upperBound.getWrappedValue() && + (this->getWrappedValue() > lowerBound.getWrappedValue() || + this->getWrappedValue() < upperBound.getWrappedValue())); +} + +float WrappedFloat::rangeOverlap( + const WrappedFloat& lowerA, + const WrappedFloat& upperA, + const WrappedFloat& lowerB, + const WrappedFloat& upperB) +{ + assertBoundsEqual(lowerA, upperA); + assertBoundsEqual(upperA, lowerB); + assertBoundsEqual(lowerB, upperB); + + float origin = lowerA.getLowerBound(); + float offset = lowerA.getWrappedValue() - origin; + + float upperAShifted = (upperA - offset).getWrappedValue(); + float lowerBShifted = (lowerB - offset).getWrappedValue(); + float upperBShifted = (upperB - offset).getWrappedValue(); + + if (upperBShifted < lowerBShifted) + { + float leftRange = std::min(upperBShifted, upperAShifted); + float rightRange = std::max(origin, upperAShifted - lowerBShifted); + return leftRange + rightRange; + } + + return std::max(0.0f, std::min(upperAShifted, upperBShifted) - std::max(origin, lowerBShifted)); +} + +} // namespace algorithms + +} // namespace tap diff --git a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp new file mode 100644 index 00000000..57b27049 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp @@ -0,0 +1,406 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2020-2023 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_WRAPPED_FLOAT_HPP_ +#define TAPROOT_WRAPPED_FLOAT_HPP_ + +#include + +#include + +#include +#include + +#include "math_user_utils.hpp" + +namespace tap +{ +namespace algorithms +{ +/** + * Wraps a float to allow easy comparison and manipulation of sensor readings + * that wrap (e.g. 0 to 360). Lower bound is "inclusive" and upper bound is "exclusive". + * The range would be represented as [0, 360). + * + * For bounds 0 - 10, logically: + * - 10 + 1 == 1 + * - 0 - 1 == 9 + * - 0 == 10 + */ +class WrappedFloat +{ +public: + /** + * @param[in] value: value to initialize with (doesn't have to be wrapped) + * @param[in] lowerBound: lower wrapping bound, must be less than `upperBound` + * @param[in] lowerBound: upper wrapping bound, must be higher than `lowerBound` + */ + WrappedFloat(float value, float lowerBound, float upperBound); + + inline WrappedFloat withSameBounds(const float value) const + { + return WrappedFloat(value, this->lowerBound, this->upperBound); + } + + // Overloaded Operators ---------------- + + /** + * Two WrappedFloats are considered equal if their wrapped values are equal. This does not + * account for floating point imprecision, so for robust equality checks, `minDifference` should + * be used. + * + * @param[in] other: The WrappedFloat to be compared `this` WrappedFloat. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + bool operator==(const WrappedFloat& other) const; + + /** + * Adds a WrappedFloat to `this` WrappedFloat given they have the same lower and + * upper bounds. + * + * @param[in] other: The WrappedFloat to be added to `this` WrappedFloat. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + void operator+=(const WrappedFloat& other); + + /** + * Subtracts a WrappedFloat from `this` WrappedFloat given they have the same lower and + * upper bounds. + * + * @param[in] other: The WrappedFloat to be subtracted from `this` WrappedFloat. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + void operator-=(const WrappedFloat& other); + + /** + * Adds a given WrappedFloat to `this` WrappedFloat given they have the same lower and upper + * bounds, returning the resultant WrappedFloat. + * + * @param[in] other: The WrappedFloat to be added with `this` WrappedFloat. + * @return: A new WrappedFloat with the additive value of `other` and `this`. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + WrappedFloat operator+(const WrappedFloat& other) const; + + /** + * Subtracts a given WrappedFloat from `this` WrappedFloat given they have the same lower and + * upper bounds, returning the resultant WrappedFloat. + * + * @param[in] other: The WrappedFloat to be subtracted from `this` WrappedFloat. + * @return: A new WrappedFloat with the subtractive value of `other` from `this`. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + WrappedFloat operator-(const WrappedFloat& other) const; + + /** + * Adds a WrappedFloat to `this` WrappedFloat given they have the same lower and + * upper bounds. + * + * @param[in] other: The WrappedFloat to be added to `this` WrappedFloat. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + void operator+=(float other); + + /** + * Subtracts a WrappedFloat from `this` WrappedFloat given they have the same lower and + * upper bounds. + * + * @param[in] other: The WrappedFloat to be subtracted from `this` WrappedFloat. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + void operator-=(float other); + + /** + * Adds a given WrappedFloat to `this` WrappedFloat given they have the same lower and upper + * bounds, returning the resultant WrappedFloat. + * + * @param[in] other: The WrappedFloat to be added with `this` WrappedFloat. + * @return: A new WrappedFloat with the additive value of `other` and `this`. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + WrappedFloat operator+(float other) const; + + /** + * Subtracts a given WrappedFloat from `this` WrappedFloat given they have the same lower and + * upper bounds, returning the resultant WrappedFloat. + * + * @param[in] other: The WrappedFloat to be subtracted from `this` WrappedFloat. + * @return: A new WrappedFloat with the subtractive value of `other` from `this`. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + WrappedFloat operator-(float other) const; + + /** + * Finds the minimum difference against another wrapped value. Can be thought of as the minimum + * distance between two points on a circle's perimeter. + * + * @param[in] other: The WrappedFloat to compute the minDifference with. + * @return: A float with the signed minimum distance. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + float minDifference(const WrappedFloat& other) const; + + /** + * Finds the minimum difference against another value. Can be thought of as the minimum + * distance between two points on a circle's perimeter. + * + * @param[in] unwrappedValue: The float to compute the minDifference with. It's wrapped before + * computing + * @return: A float with the signed minimum distance. + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + float minDifference(const float& unwrappedValue) const; + + /** + * Interpolates along the smallest difference with another WrappedFloat. + * + * @param[in] other: The WrappedFloat to interpolate between. + * @param[in] alpha: A float between 0-1 (0 returns this WrappedFloat's value, 1 returns the + * other's) + * @throws: An assertion error if the two WrappedFloats have different lower and upper bounds. + */ + WrappedFloat minInterpolate(const WrappedFloat& other, float alpha) const; + + /** + * Shifts both bounds by the specified amount. + * + * @param[in] shiftMagnitude the amount to add to each bound. + */ + void shiftBounds(float shiftMagnitude); + + /** + * Limits the passed WrappedFloat between the closest of the + * min or max value if outside the min and max value's wrapped range. + * + * The min and max must have the same wrapped bounds as the valueToLimit. + * + * + * For example given a value wrapped from -10 to 10, with the following + * conditions: + * - valueToLimit: 5, min: 1, max: 4, returns 4. + * - valueToLimit: 9, min: 1, max: 3, returns 1 (since valueToLimit is closest to 1). + * - valueToLimit: 9, min: 2, max: 1, returns 9 (since the range between min and max + * starts at 2, goes up to 9, then wraps around to 1). + * + * @param[in] valueToLimit the WrappedFloat whose value it is to limit + * @param[in] min the WrappedFloat with the same bounds as valueToLimit that + * valueToLimit will be limited below. + * @param[in] max the WrappedFloat with the same bounds as valueToLimit that + * valueToLimit will be limited above. + * @param[out] status the status result (what operation the limitValue function performed). The + * status codes are described below: + * - 0: No limiting performed + * - 1: Limited to min value + * - 2: Limited to max value + * @return the limited value. + * @throws: An assertion error if the WrappedFloats have different lower and upper bounds. + */ + static float limitValue( + const WrappedFloat& valueToLimit, + const WrappedFloat& min, + const WrappedFloat& max, + int* status); + + /** + * Runs the limitValue function from above, wrapping the min and max passed in to + * the same bounds as those of valueToLimit's. + * + * @see limitValue. + * @param[in] valueToLimit the WrappedFloat whose value it is to limit + * @param[in] min the WrappedFloat with the same bounds as valueToLimit that + * valueToLimit will be limited below. + * @param[in] max the WrappedFloat with the same bounds as valueToLimit that + * valueToLimit will be limited above. + * @param[out] status the status result (what operation the limitValue function performed). The + * status codes are described below: + * - 0: No limiting performed + * - 1: Limited to min value + * - 2: Limited to max value + * @return the limited value. + * @throws: An assertion error if the WrappedFloats have different lower and upper bounds. + */ + static float limitValue( + const WrappedFloat& valueToLimit, + const float min, + const float max, + int* status); + + /** + * Checks whether `this` is within the wrapped range defined from `lowerBound` to `upperBound`. + * For example given a value wrapped from 0 to 10, with the following conditions: + * - this: 9, min: 3, max: 7, returns false. + * - this: 5, min: 3, max: 7, returns true. + * - this: 9, min: 7, max: 3, returns true. + * - this: 5, min: 7, max: 3, returns false. + * + * @param[in] lowerBound + * @param[in] upperBound + * @return whether `this` is within the specified range + * @throws: An assertion error if the WrappedFloats themselves have different bounds. + */ + bool withinRange(const WrappedFloat& lowerBound, const WrappedFloat& upperBound) const; + + /** + * Calculates how much of the two given wrapped ranges overlap. If mentally visualizing on a + * circle, this method takes two arbitrary arcs on the perimeter and returns the length of the + * overlapping portion(s). + * + * @param[in] lowerA the first range's lower bound + * @param[in] upperA the first range's upper bound + * @param[in] lowerB the second range's lower bound + * @param[in] upperB the second range's upper bound + * @return the total length of the overlapping region(s) as a float + * @throws: An assertion error if the WrappedFloats themselves have different bounds. + */ + static float rangeOverlap( + const WrappedFloat& lowerA, + const WrappedFloat& upperA, + const WrappedFloat& lowerB, + const WrappedFloat& upperB); + + // Getters/Setters ---------------- + + /** + * Returns the unwrapped value. + */ + inline float getUnwrappedValue() const + { + return wrapped + (upperBound - lowerBound) * revolutions; + }; + + /** + * Returns the wrapped value. + */ + inline float getWrappedValue() const { return wrapped; }; + + /** + * Sets the wrapped value. + */ + inline void setWrappedValue(float newWrappedValue) + { + this->wrapped = newWrappedValue; + wrapValue(); + }; + + /** + * Sets the unwrapped value. + */ + inline void setUnwrappedValue(float newUnwrappedValue) + { + this->wrapped = newUnwrappedValue; + this->revolutions = 0; + wrapValue(); + }; + + inline WrappedFloat getNormalized() const + { + WrappedFloat out(*this); + out.revolutions = 0; + return out; + } + + /** + * + */ + inline int getRevolutions() const { return revolutions; }; + + /** + * Returns the value's upper bound. + */ + inline float getUpperBound() const { return upperBound; }; + + /** + * Returns the value's lower bound. + */ + inline float getLowerBound() const { return lowerBound; }; + + /** + * Maximum value between floats representing bounds at which + * they're considered to be "equal" for assertions. + */ + static constexpr float EPSILON = 1E-8; + +private: + /** + * The wrapped value. Guaranteed to be between lower and upper bound. + */ + float wrapped; + + /** + * Number of total revolutions. + */ + int revolutions{0}; + + /** + * The lower bound to wrap around. + */ + float lowerBound; + + /** + * The upper bound to wrap around. + */ + float upperBound; + + /** + * Helper function for wrapping value within bounds. + */ + void wrapValue(); + + inline static void assertBoundsEqual(const WrappedFloat& a, const WrappedFloat& b) + { + modm_assert( + compareFloatClose(a.getLowerBound(), b.getLowerBound(), EPSILON), + "WrappedFloat::assertBoundsEqual", + "Lower bounds do not match"); + modm_assert( + compareFloatClose(a.getUpperBound(), b.getUpperBound(), EPSILON), + "WrappedFloat::assertBoundsEqual", + "Upper bounds do not match"); + } + + inline void assertBoundsEqual(const WrappedFloat& other) const + { + assertBoundsEqual(*this, other); + } + +}; // class WrappedFloat + +/** + * Represents an angle in radians. + */ +class Angle : public WrappedFloat +{ +public: + inline Angle(const float value) : WrappedFloat(value, 0, M_TWOPI){}; + + static inline WrappedFloat fromDegrees(const float degrees) + { + return Angle(modm::toRadian(degrees)); + } +}; + +} // namespace algorithms + +} // namespace tap + +#endif // TAPROOT_WRAPPED_FLOAT_HPP_ diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp index 1197cf4d..74f7e1c7 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp @@ -138,8 +138,7 @@ void Bmi088::initializeAcc() setAndCheckAccRegister( Acc::ACC_CONF, - Acc::AccBandwidth_t(Acc::AccBandwidth::NORMAL) | - Acc::AccOutputRate_t(Acc::AccOutputRate::Hz800)); + Acc::AccBandwidth_t(accOversampling) | Acc::AccOutputRate_t(accOutputRate)); setAndCheckAccRegister(Acc::ACC_RANGE, ACC_RANGE); } @@ -166,9 +165,7 @@ void Bmi088::initializeGyro() // extra 0x80 is because the bandwidth register will always have 0x80 masked // so when checking, we want to mask as well to avoid an error - setAndCheckGyroRegister( - Gyro::GYRO_BANDWIDTH, - Gyro::GyroBandwidth::ODR1000_BANDWIDTH116 | Gyro::GyroBandwidth_t(0x80)); + setAndCheckGyroRegister(Gyro::GYRO_BANDWIDTH, gyroOutputRate | Gyro::GyroBandwidth_t(0x80)); setAndCheckGyroRegister(Gyro::GYRO_LPM1, Gyro::GyroLpm1::PWRMODE_NORMAL); } @@ -181,44 +178,12 @@ void Bmi088::periodicIMUUpdate() return; } - uint8_t rxBuff[6] = {}; - - Bmi088Hal::bmi088AccReadMultiReg(Acc::ACC_X_LSB, rxBuff, 6); - - prevIMUDataReceivedTime = tap::arch::clock::getTimeMicroseconds(); - - data.accRaw[ImuData::X] = bigEndianInt16ToFloat(rxBuff); - data.accRaw[ImuData::Y] = bigEndianInt16ToFloat(rxBuff + 2); - data.accRaw[ImuData::Z] = bigEndianInt16ToFloat(rxBuff + 4); - - Bmi088Hal::bmi088GyroReadMultiReg(Gyro::RATE_X_LSB, rxBuff, 6); - data.gyroRaw[ImuData::X] = bigEndianInt16ToFloat(rxBuff); - data.gyroRaw[ImuData::Y] = bigEndianInt16ToFloat(rxBuff + 2); - data.gyroRaw[ImuData::Z] = bigEndianInt16ToFloat(rxBuff + 4); - - Bmi088Hal::bmi088AccReadMultiReg(Acc::TEMP_MSB, rxBuff, 2); - data.temperature = parseTemp(rxBuff[0], rxBuff[1]); - if (imuState == ImuState::IMU_CALIBRATING) { computeOffsets(); } else { - data.gyroDegPerSec[ImuData::X] = - GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::X] - data.gyroOffsetRaw[ImuData::X]); - data.gyroDegPerSec[ImuData::Y] = - GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::Y] - data.gyroOffsetRaw[ImuData::Y]); - data.gyroDegPerSec[ImuData::Z] = - GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::Z] - data.gyroOffsetRaw[ImuData::Z]); - - data.accG[ImuData::X] = - ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::X] - data.accOffsetRaw[ImuData::X]); - data.accG[ImuData::Y] = - ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::Y] - data.accOffsetRaw[ImuData::Y]); - data.accG[ImuData::Z] = - ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::Z] - data.accOffsetRaw[ImuData::Z]); - mahonyAlgorithm.updateIMU( data.gyroDegPerSec[ImuData::X], data.gyroDegPerSec[ImuData::Y], @@ -257,6 +222,41 @@ void Bmi088::computeOffsets() } } +void Bmi088::read() +{ + uint8_t rxBuff[6] = {}; + + Bmi088Hal::bmi088AccReadMultiReg(Acc::ACC_X_LSB, rxBuff, 6); + + prevIMUDataReceivedTime = tap::arch::clock::getTimeMicroseconds(); + + data.accRaw[ImuData::X] = bigEndianInt16ToFloat(rxBuff); + data.accRaw[ImuData::Y] = bigEndianInt16ToFloat(rxBuff + 2); + data.accRaw[ImuData::Z] = bigEndianInt16ToFloat(rxBuff + 4); + + Bmi088Hal::bmi088GyroReadMultiReg(Gyro::RATE_X_LSB, rxBuff, 6); + data.gyroRaw[ImuData::X] = bigEndianInt16ToFloat(rxBuff); + data.gyroRaw[ImuData::Y] = bigEndianInt16ToFloat(rxBuff + 2); + data.gyroRaw[ImuData::Z] = bigEndianInt16ToFloat(rxBuff + 4); + + Bmi088Hal::bmi088AccReadMultiReg(Acc::TEMP_MSB, rxBuff, 2); + data.temperature = parseTemp(rxBuff[0], rxBuff[1]); + + data.gyroDegPerSec[ImuData::X] = + GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::X] - data.gyroOffsetRaw[ImuData::X]); + data.gyroDegPerSec[ImuData::Y] = + GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::Y] - data.gyroOffsetRaw[ImuData::Y]); + data.gyroDegPerSec[ImuData::Z] = + GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::Z] - data.gyroOffsetRaw[ImuData::Z]); + + data.accG[ImuData::X] = + ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::X] - data.accOffsetRaw[ImuData::X]); + data.accG[ImuData::Y] = + ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::Y] - data.accOffsetRaw[ImuData::Y]); + data.accG[ImuData::Z] = + ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::Z] - data.accOffsetRaw[ImuData::Z]); +} + void Bmi088::setAndCheckAccRegister(Acc::Register reg, Acc::Registers_t value) { Bmi088Hal::bmi088AccWriteSingleReg(reg, value); diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp index c462d634..1c546658 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp @@ -96,12 +96,17 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface mockable void initialize(float sampleFrequency, float mahonyKp, float mahonyKi); /** - * Call this function at 500 Hz. Reads IMU data and performs the mahony AHRS algorithm to - * compute pitch/roll/yaw. + * Call this function at same rate as intialized sample frequency. + * Performs the mahony AHRS algorithm to compute pitch/roll/yaw. + */ + mockable void periodicIMUUpdate(); + + /** + * This function reads the IMU data from SPI * * @note This function blocks for 129 microseconds to read registers from the BMI088. */ - mockable void periodicIMUUpdate(); + mockable void read(); /** * Returns the state of the IMU. Can be not connected, connected but not calibrated, or @@ -143,6 +148,19 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface inline void setOffsetSamples(float samples) { BMI088_OFFSET_SAMPLES = samples; } + inline void setAccOversampling(Acc::AccBandwidth oversampling) + { + accOversampling = oversampling; + } + inline void setAccOutputRate(Acc::AccOutputRate outputRate) { accOutputRate = outputRate; } + + inline void setGyroOutputRate(Gyro::GyroBandwidth outputRate) { gyroOutputRate = outputRate; } + + inline void setTargetTemperature(float temperatureC) + { + imuHeater.setDesiredTemperature(temperatureC); + } + private: static constexpr uint16_t RAW_TEMPERATURE_TO_APPLY_OFFSET = 1023; /// Offset parsed temperature reading by this amount if > RAW_TEMPERATURE_TO_APPLY_OFFSET. @@ -179,7 +197,12 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface uint32_t prevIMUDataReceivedTime = 0; + Acc::AccBandwidth accOversampling = Acc::AccBandwidth::NORMAL; + Acc::AccOutputRate accOutputRate = Acc::AccOutputRate::Hz800; + void initializeAcc(); + + Gyro::GyroBandwidth gyroOutputRate = Gyro::GyroBandwidth::ODR1000_BANDWIDTH116; void initializeGyro(); void computeOffsets(); diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu_heater/imu_heater.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu_heater/imu_heater.cpp index 90a9a264..194a0695 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu_heater/imu_heater.cpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu_heater/imu_heater.cpp @@ -55,7 +55,7 @@ void ImuHeater::runTemperatureController(float temperature) } // Run PID controller to find desired output, output units PWM frequency - imuTemperatureController.update(IMU_DESIRED_TEMPERATURE - temperature); + imuTemperatureController.update(imuDesiredTemperature - temperature); // Set heater PWM output, limit output so it is not < 0 drivers->pwm.write( diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu_heater/imu_heater.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu_heater/imu_heater.hpp index ce0640de..130f1a70 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu_heater/imu_heater.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu_heater/imu_heater.hpp @@ -38,12 +38,6 @@ namespace tap::communication::sensors::imu_heater class ImuHeater { public: - /** - * Normal operating temperature is ~40 degrees C, and RM manual says the optimal operating - * temperature is ~15-20 degrees C above the normal operating temperature of the board. - */ - static constexpr float IMU_DESIRED_TEMPERATURE = 50.0f; - ImuHeater(Drivers *drivers); DISALLOW_COPY_AND_ASSIGN(ImuHeater) ~ImuHeater() = default; @@ -56,10 +50,17 @@ class ImuHeater /** * Runs a PID controller to regulate the temperature of the IMU. * - * @param[in] temperature The temperature of the mpu6500, units degrees C. + * @param[in] temperature The temperature of the imu, units degrees C. */ void runTemperatureController(float temperature); + /** + * @brief Set the target temperature for the IMU heater. + * + * @param temperature Setpoint in degrees C. + */ + inline void setDesiredTemperature(float temperature) { imuDesiredTemperature = temperature; } + private: /** * PID constants for temperature control. @@ -76,6 +77,12 @@ class ImuHeater */ static constexpr float HEATER_PWM_FREQUENCY = 1000.0f; + /** + * Normal operating temperature is ~40 degrees C, and RM manual says the optimal operating + * temperature is ~15-20 degrees C above the normal operating temperature of the board. + */ + float imuDesiredTemperature = 50.0f; + Drivers *drivers; modm::Pid imuTemperatureController; diff --git a/ut-robomaster/taproot/src/tap/communication/serial/dji_serial.hpp b/ut-robomaster/taproot/src/tap/communication/serial/dji_serial.hpp index 54bee611..de0d4299 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/dji_serial.hpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/dji_serial.hpp @@ -128,7 +128,7 @@ class DJISerial uint16_t CRC16; } modm_packed; - static const uint16_t SERIAL_RX_BUFF_SIZE = 256; + static const uint16_t SERIAL_RX_BUFF_SIZE = 1024; static const uint16_t SERIAL_HEAD_BYTE = 0xA5; using ReceivedSerialMessage = SerialMessage; diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp index f73867a4..988534d1 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp @@ -24,6 +24,7 @@ #ifndef TAPROOT_REF_SERIAL_HPP_ #define TAPROOT_REF_SERIAL_HPP_ +#include #include #include @@ -171,7 +172,8 @@ class RefSerial : public DJISerial, public RefSerialData mockable void releaseTransmissionSemaphore(uint32_t sentMsgLen) { transmissionSemaphore.release(); - transmissionDelayTimer.restart(sentMsgLen * 1'000 / Tx::MAX_TRANSMIT_SPEED_BYTES_PER_S); + transmissionDelayTimer.restart( + std::ceil(sentMsgLen * 1000.0f / Tx::MAX_TRANSMIT_SPEED_BYTES_PER_S)); } /** diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp index 82ee4375..2d8c401a 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp @@ -655,8 +655,10 @@ class RefSerialData * You cannot send messages faster than this speed to the referee system. * * Source: https://bbs.robomaster.com/forum.php?mod=viewthread&tid=9120 + * + * Changed from 1280 to 1000 as the HUD was still unreliable. */ - static constexpr uint32_t MAX_TRANSMIT_SPEED_BYTES_PER_S = 1280; + static constexpr uint32_t MAX_TRANSMIT_SPEED_BYTES_PER_S = 1000; /** * Get the min wait time after which you can send more data to the client. Sending faster diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp index 529eff80..22272145 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp @@ -234,7 +234,7 @@ modm::ResumableResult RefSerialTransmitter::deleteGraphicLayer( reinterpret_cast(&deleteGraphicLayerMessage), sizeof(Tx::DeleteGraphicLayerMessage)); - drivers->refSerial.releaseTransmissionSemaphore(sizeof(Tx::DeleteGraphicOperation)); + drivers->refSerial.releaseTransmissionSemaphore(sizeof(Tx::DeleteGraphicLayerMessage)); RF_END(); } diff --git a/ut-robomaster/taproot/src/tap/control/command.hpp b/ut-robomaster/taproot/src/tap/control/command.hpp index c8b84055..3a0ec49a 100644 --- a/ut-robomaster/taproot/src/tap/control/command.hpp +++ b/ut-robomaster/taproot/src/tap/control/command.hpp @@ -135,6 +135,7 @@ class Command */ const int globalIdentifier; +protected: command_scheduler_bitmap_t commandRequirementsBitwise = 0; }; // class Command diff --git a/ut-robomaster/taproot/src/tap/control/command_mapper.cpp b/ut-robomaster/taproot/src/tap/control/command_mapper.cpp index be3e079e..0366bed3 100644 --- a/ut-robomaster/taproot/src/tap/control/command_mapper.cpp +++ b/ut-robomaster/taproot/src/tap/control/command_mapper.cpp @@ -64,18 +64,7 @@ void CommandMapper::handleKeyStateChange( } } -void CommandMapper::addMap(CommandMapping *mapping) -{ - for (const CommandMapping *const cmap : commandsToRun) - { - if (mapStateEqual(*cmap, *mapping)) - { - RAISE_ERROR(drivers, "failed to insert io mapping"); - return; - } - } - commandsToRun.push_back(mapping); -} +void CommandMapper::addMap(CommandMapping *mapping) { commandsToRun.push_back(mapping); } const CommandMapping *CommandMapper::getAtIndex(std::size_t index) const { diff --git a/ut-robomaster/taproot/src/tap/control/command_mapping.cpp b/ut-robomaster/taproot/src/tap/control/command_mapping.cpp index af10b94e..c0e97074 100644 --- a/ut-robomaster/taproot/src/tap/control/command_mapping.cpp +++ b/ut-robomaster/taproot/src/tap/control/command_mapping.cpp @@ -51,9 +51,6 @@ bool operator==(const CommandMapping &cm1, const CommandMapping &cm2) bool mapStateEqual(const CommandMapping &cm1, const CommandMapping &cm2) { - // When inserting mappings into the CommandMapper, we want to check for equality based - // on the mapState since we don't want two identical map_states with unique map_commands. - // Even if mappedCommand vectors are different we want insertion to fail. return cm1.mapState == cm2.mapState; } diff --git a/ut-robomaster/taproot/src/tap/control/command_scheduler.cpp b/ut-robomaster/taproot/src/tap/control/command_scheduler.cpp index 3a40c874..02bc8d96 100644 --- a/ut-robomaster/taproot/src/tap/control/command_scheduler.cpp +++ b/ut-robomaster/taproot/src/tap/control/command_scheduler.cpp @@ -171,22 +171,6 @@ void CommandScheduler::run() uint32_t runStart = arch::clock::getTimeMicroseconds(); #endif - if (runningHardwareTests) - { - // Call runHardwareTests on all subsystems in the registeredSubsystemBitmap - // if a hardware test is not already complete - for (auto it = subMapBegin(); it != subMapEnd(); it++) - { - Subsystem *sub = *it; - if (!sub->isHardwareTestComplete()) - { - sub->runHardwareTests(); - } - sub->refresh(); - } - return; - } - if (safeDisconnected()) { // End all commands running. They were interrupted by the remote disconnecting. @@ -214,6 +198,19 @@ void CommandScheduler::run() // Refresh subsystems in the registeredSubsystemBitmap for (auto it = subMapBegin(); it != subMapEnd(); it++) { + Command *testCommand; + if (!safeDisconnected() && + !(subsystemsAssociatedWithCommandBitmap & + (LSB_ONE_HOT_SUBSYSTEM_BITMAP << (*it)->getGlobalIdentifier())) && + (testCommand = (*it)->getTestCommand()) != nullptr) + { + if (testCommand->isFinished()) + { + this->subsystemsPassingHardwareTests |= + (LSB_ONE_HOT_SUBSYSTEM_BITMAP << (*it)->getGlobalIdentifier()); + } + } + // Call appropriate refresh function for each of the subsystems if (safeDisconnected()) { @@ -258,11 +255,6 @@ void CommandScheduler::addCommand(Command *commandToAdd) { return; } - else if (runningHardwareTests) - { - RAISE_ERROR(drivers, "attempting to add command while running tests"); - return; - } else if (commandToAdd == nullptr) { RAISE_ERROR(drivers, "attempting to add nullptr command"); @@ -363,35 +355,71 @@ bool CommandScheduler::isSubsystemRegistered(const Subsystem *subsystem) const registeredSubsystemBitmap); } -void CommandScheduler::startHardwareTests() +void CommandScheduler::runAllHardwareTests() { - // End all commands that are currently being run - runningHardwareTests = true; - for (auto it = cmdMapBegin(); it != cmdMapEnd(); it++) + for (auto it = subMapBegin(); it != subMapEnd(); it++) { - (*it)->end(true); + this->runHardwareTest(*it); } +} - // Clear command bitmap (now all commands are removed) - addedCommandBitmap = 0; - // No more subsystems associated with commands, so clear this bitmap as well - subsystemsAssociatedWithCommandBitmap = 0; +void CommandScheduler::runHardwareTest(const Subsystem *subsystem) +{ + Command *testCommand = subsystem->getTestCommand(); + if (testCommand != nullptr) + { + this->subsystemsPassingHardwareTests &= + ~(LSB_ONE_HOT_SUBSYSTEM_BITMAP << subsystem->getGlobalIdentifier()); + this->addCommand(testCommand); + } +} +void CommandScheduler::stopAllHardwareTests() +{ + Command *testCommand; // Start hardware tests for (auto it = subMapBegin(); it != subMapEnd(); it++) { - (*it)->setHardwareTestsIncomplete(); + // schedule the test command if it exists + if ((testCommand = (*it)->getTestCommand()) != nullptr) + { + this->removeCommand(testCommand, true); + } } } -void CommandScheduler::stopHardwareTests() +void CommandScheduler::stopHardwareTest(const Subsystem *subsystem) { - // Stop all hardware tests + Command *testCommand = subsystem->getTestCommand(); + if (testCommand != nullptr) + { + this->removeCommand(testCommand, true); + } +} + +int CommandScheduler::countRunningHardwareTests() +{ + int total = 0; for (auto it = subMapBegin(); it != subMapEnd(); it++) { - (*it)->setHardwareTestsComplete(); + if (this->isRunningTest(*it)) + { + total += 1; + } } - runningHardwareTests = false; + + return total; +} + +bool CommandScheduler::isRunningTest(const Subsystem *subsystem) +{ + return this->isCommandScheduled(subsystem->getTestCommand()); +} + +bool CommandScheduler::hasPassedTest(const Subsystem *subsystem) +{ + return (this->subsystemsPassingHardwareTests & + (LSB_ONE_HOT_SUBSYSTEM_BITMAP << subsystem->getGlobalIdentifier())) != 0; } int CommandScheduler::subsystemListSize() const diff --git a/ut-robomaster/taproot/src/tap/control/command_scheduler.hpp b/ut-robomaster/taproot/src/tap/control/command_scheduler.hpp index 651ae55b..65d7c5d0 100644 --- a/ut-robomaster/taproot/src/tap/control/command_scheduler.hpp +++ b/ut-robomaster/taproot/src/tap/control/command_scheduler.hpp @@ -213,8 +213,46 @@ class CommandScheduler */ mockable bool isSubsystemRegistered(const Subsystem* subsystem) const; - mockable void startHardwareTests(); - mockable void stopHardwareTests(); + /** + * Runs all hardware tests from subsystems that have a test command. + */ + mockable void runAllHardwareTests(); + + /** + * Runs the hardware test command, if available, for the specific subsystem. + * + * @param[in] subsystem the subsystem to run + */ + mockable void runHardwareTest(const Subsystem* subsystem); + + /** + * Stop all hardware tests from subsystems that have a running test command. + */ + mockable void stopAllHardwareTests(); + + /** + * Stops the hardware test command, if running, for the specific subsystem. + * + * @param[in] subsystem the subsystem to stop + */ + mockable void stopHardwareTest(const Subsystem* subsystem); + + /** + * @return the count of subsystems currently running a test. + */ + mockable int countRunningHardwareTests(); + + /** + * @param[in] subsystem the subsystem to check + * @return `true` if the test command is running, `false` otherwise. + */ + mockable bool isRunningTest(const Subsystem* subsystem); + + /** + * @param[in] subsystem the subsystem to check + * @return `true` if the test command has passed, `false` otherwise. + */ + mockable bool hasPassedTest(const Subsystem* subsystem); /** * @return The number of subsystems registered with the scheduler. @@ -370,10 +408,17 @@ class CommandScheduler /** * Each bit in the bitmap corresponds to an index into the subsystem registrar. If a * bit is set, it means that the subsystem in the registrar has a command associated - * in in this scheduler. + * in this scheduler. */ subsystem_scheduler_bitmap_t subsystemsAssociatedWithCommandBitmap = 0; + /** + * Each bit in the bitmap corresponds to an index into the subsystem registrar. If a + * bit is set, it means that the subsystem in the registrar has passed a hardware test + * in this scheduler. + */ + subsystem_scheduler_bitmap_t subsystemsPassingHardwareTests = 0; + /** * If a command has been added and is running, the associated bit in this bitmap will * be set to 1. @@ -381,8 +426,6 @@ class CommandScheduler command_scheduler_bitmap_t addedCommandBitmap = 0; bool isMasterScheduler = false; - - bool runningHardwareTests = false; }; // class CommandScheduler } // namespace control diff --git a/ut-robomaster/taproot/src/tap/control/concurrent_command.hpp b/ut-robomaster/taproot/src/tap/control/concurrent_command.hpp new file mode 100644 index 00000000..d6f478a9 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/control/concurrent_command.hpp @@ -0,0 +1,159 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_CONCURRENT_COMMAND_HPP_ +#define TAPROOT_CONCURRENT_COMMAND_HPP_ + +#include + +#include "modm/architecture/interface/assert.hpp" + +#include "command.hpp" +#include "command_scheduler_types.hpp" + +namespace tap +{ +namespace control +{ +/** + * A command that runs multiple commands in parallel. Waits for all passed in commands to be ready + * before being ready itself. When RACE is false, it continues executing until all passed in + * commands have finished and then the concurrent command finishes. When RACE is true, only one + * passed in command needs to finish for the concurrent command to finish. + */ +template +class ConcurrentTemplateCommand : public Command +{ +public: + ConcurrentTemplateCommand(std::array commands, const char* name) + : Command(), + commands(commands), + name(name), + finishedCommands(0), + allCommands(0) + { + for (Command* command : commands) + { + modm_assert( + command != nullptr, + "ConcurrentCommand::ConcurrentCommand", + "Null pointer command passed into concurrent command."); + auto requirements = command->getRequirementsBitwise(); + modm_assert( + (this->commandRequirementsBitwise & requirements) == 0, + "ConcurrentCommand::ConcurrentCommand", + "Multiple commands to concurrent command have overlapping requirements."); + this->commandRequirementsBitwise |= requirements; + this->allCommands |= (1ull << command->getGlobalIdentifier()); + } + } + + const char* getName() const override { return this->name; } + + bool isReady() override + { + for (Command* command : commands) + { + if (!command->isReady()) + { + return false; + } + } + return true; + } + + void initialize() override + { + for (Command* command : commands) + { + command->initialize(); + } + } + + void execute() override + { + for (Command* command : commands) + { + if (!(this->finishedCommands & (1ull << command->getGlobalIdentifier()))) + { + command->execute(); + if (command->isFinished()) + { + command->end(false); + this->finishedCommands |= 1ull << command->getGlobalIdentifier(); + } + } + } + } + + void end(bool interrupted) override + { + for (Command* command : commands) + { + if (!(this->finishedCommands & (1ull << command->getGlobalIdentifier()))) + { + if (RACE) + { + command->end(true); + } + else + { + command->end(interrupted); + } + } + } + } + + bool isFinished() const override + { + if (RACE) + { + return this->finishedCommands != 0; + } + return this->finishedCommands == this->allCommands; + } + +private: + std::array commands; + const char* name; + command_scheduler_bitmap_t finishedCommands; + command_scheduler_bitmap_t allCommands; +}; // class ConcurrentTemplateCommand + +/** + * Runs commands in parallel until all are finished. + */ +template +using ConcurrentCommand = ConcurrentTemplateCommand; + +/** + * Runs commands in parallel until one is finished. + */ +template +using ConcurrentRaceCommand = ConcurrentTemplateCommand; + +} // namespace control + +} // namespace tap + +#endif // TAPROOT_CONCURRENT_COMMAND_HPP_ diff --git a/ut-robomaster/taproot/src/tap/control/governor/governor_with_fallback_command.hpp b/ut-robomaster/taproot/src/tap/control/governor/governor_with_fallback_command.hpp index 9656efd2..45decefa 100644 --- a/ut-robomaster/taproot/src/tap/control/governor/governor_with_fallback_command.hpp +++ b/ut-robomaster/taproot/src/tap/control/governor/governor_with_fallback_command.hpp @@ -51,10 +51,12 @@ class GovernorWithFallbackCommand : public Command std::vector subRequirements, Command &commandWhenGovernorsReady, Command &fallbackCommand, - const std::array &commandGovernorList) + const std::array &commandGovernorList, + const bool stopFallbackCommandIfGovernorsReady = false) : commandWhenGovernorsReady(commandWhenGovernorsReady), fallbackCommand(fallbackCommand), - commandGovernorList(commandGovernorList) + commandGovernorList(commandGovernorList), + stopFallbackCommandIfGovernorsReady(stopFallbackCommandIfGovernorsReady) { std::for_each(subRequirements.begin(), subRequirements.end(), [&](auto sub) { addSubsystemRequirement(sub); @@ -65,22 +67,23 @@ class GovernorWithFallbackCommand : public Command assert(fallbackCommand.getRequirementsBitwise() == this->getRequirementsBitwise()); } - const char *getName() const override { return "Governor w/fallback"; } + const char *getName() const override + { + return governedCommandSelected ? commandWhenGovernorsReady.getName() + : fallbackCommand.getName(); + } bool isReady() override { - currentGovernorReadiness = - std::all_of(commandGovernorList.begin(), commandGovernorList.end(), [](auto governor) { - return governor->isReady(); - }); + governedCommandSelected = checkGovernorReadiness(); - return (currentGovernorReadiness && commandWhenGovernorsReady.isReady()) || - (!currentGovernorReadiness && fallbackCommand.isReady()); + return (governedCommandSelected && commandWhenGovernorsReady.isReady()) || + (!governedCommandSelected && fallbackCommand.isReady()); } void initialize() override { - if (currentGovernorReadiness) + if (governedCommandSelected) { commandWhenGovernorsReady.initialize(); } @@ -92,7 +95,7 @@ class GovernorWithFallbackCommand : public Command void execute() override { - if (currentGovernorReadiness) + if (governedCommandSelected) { commandWhenGovernorsReady.execute(); } @@ -104,7 +107,7 @@ class GovernorWithFallbackCommand : public Command void end(bool interrupted) override { - if (currentGovernorReadiness) + if (governedCommandSelected) { commandWhenGovernorsReady.end(interrupted); } @@ -116,16 +119,37 @@ class GovernorWithFallbackCommand : public Command bool isFinished() const override { - return currentGovernorReadiness ? commandWhenGovernorsReady.isFinished() - : fallbackCommand.isFinished(); + if (governedCommandSelected) + { + return commandWhenGovernorsReady.isFinished() || checkAnyGovernorFinished(); + } + return fallbackCommand.isFinished() || + (stopFallbackCommandIfGovernorsReady && checkGovernorReadiness()); } private: - bool currentGovernorReadiness = false; + bool governedCommandSelected = false; Command &commandWhenGovernorsReady; Command &fallbackCommand; std::array commandGovernorList; + const bool stopFallbackCommandIfGovernorsReady; + + bool checkGovernorReadiness() const + { + return std::all_of( + commandGovernorList.begin(), + commandGovernorList.end(), + [](auto governor) { return governor->isReady(); }); + } + + bool checkAnyGovernorFinished() const + { + return std::any_of( + commandGovernorList.begin(), + commandGovernorList.end(), + [](auto governor) { return governor->isFinished(); }); + } }; } // namespace tap::control::governor diff --git a/ut-robomaster/taproot/src/tap/control/sequential_command.hpp b/ut-robomaster/taproot/src/tap/control/sequential_command.hpp new file mode 100644 index 00000000..402dd279 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/control/sequential_command.hpp @@ -0,0 +1,121 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_SEQUENTIAL_COMMAND_HPP_ +#define TAPROOT_SEQUENTIAL_COMMAND_HPP_ + +#include + +#include "modm/architecture/interface/assert.hpp" + +#include "command.hpp" +#include "command_scheduler_types.hpp" + +namespace tap +{ +namespace control +{ +/** + * A generic extendable class for implementing a command. Each + * command is attached to a subsystem. To create a new command, + * extend the Command class and instantiate the virtual functions + * in this class. See example_command.hpp for example of this. + */ +template +class SequentialCommand : public Command +{ +public: + SequentialCommand(std::array commands) + : Command(), + commands(commands), + currentCommand(0) + { + for (Command* command : commands) + { + modm_assert( + command != nullptr, + "SequentialCommand::SequentialCommand", + "Null pointer command passed into sequential command."); + this->commandRequirementsBitwise |= (command->getRequirementsBitwise()); + } + } + + const char* getName() const override + { + return this->currentCommand == COMMANDS ? "" + : this->commands[this->currentCommand]->getName(); + } + + bool isReady() override { return this->commands[0]->isReady(); } + + void initialize() override + { + this->currentCommand = 0; + this->commandInitialized = false; + } + + void execute() override + { + if (!this->commandInitialized) + { + if (this->commands[this->currentCommand]->isReady()) + { + this->commands[this->currentCommand]->initialize(); + this->commandInitialized = true; + } + } + + if (this->commandInitialized) + { + this->commands[this->currentCommand]->execute(); + + if (this->commands[this->currentCommand]->isFinished()) + { + this->commands[this->currentCommand]->end(false); + this->commandInitialized = false; + this->currentCommand++; + } + } + } + + void end(bool interrupted) override + { + if (this->currentCommand != COMMANDS) + { + this->commands[this->currentCommand]->end(interrupted); + } + } + + bool isFinished() const override { return this->currentCommand == COMMANDS; } + +private: + std::array commands; + size_t currentCommand; + bool commandInitialized; +}; // class SequentialCommand + +} // namespace control + +} // namespace tap + +#endif // TAPROOT_SEQUENTIAL_COMMAND_HPP_ diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_unjam_integral_comprised_command.cpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_unjam_integral_comprised_command.cpp index 021ae0cc..4027b319 100644 --- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_unjam_integral_comprised_command.cpp +++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_unjam_integral_comprised_command.cpp @@ -33,11 +33,11 @@ MoveUnjamIntegralComprisedCommand::MoveUnjamIntegralComprisedCommand( tap::Drivers &drivers, IntegrableSetpointSubsystem &subsystem, MoveIntegralCommand &moveIntegralCommand, - UnjamIntegralCommand &unjamIntegralCommand) + UnjamCommandInterface &unjamCommand) : tap::control::ComprisedCommand(&drivers), subsystem(subsystem), moveIntegralCommand(moveIntegralCommand), - unjamCommand(unjamIntegralCommand), + unjamCommand(unjamCommand), unjamSequenceCommencing(false) { comprisedCommandScheduler.registerSubsystem(&subsystem); diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_unjam_integral_comprised_command.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_unjam_integral_comprised_command.hpp index 98f60638..544f971b 100644 --- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_unjam_integral_comprised_command.hpp +++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_unjam_integral_comprised_command.hpp @@ -29,7 +29,7 @@ #include "../interfaces/integrable_setpoint_subsystem.hpp" #include "move_integral_command.hpp" -#include "unjam_integral_command.hpp" +#include "unjam_command_interface.hpp" namespace tap::control::setpoint { @@ -47,7 +47,7 @@ class MoveUnjamIntegralComprisedCommand : public tap::control::ComprisedCommand * @param[in] drivers A reference to the `Drivers` struct. * @param[in] subsystem The IntegrableSetpointSubsystem that will be rotated or unjammed. * @param[in] moveIntegralCommand A command that rotates the agitator forward. - * @param[in] unjamIntegralCommand A command that unjams the agitator. + * @param[in] unjamCommand A command that unjams the agitator. * * @note The move integral and unjam integral commands must have the same subsystem requirement. * This subsystem requirement must be the subsystem passed in. @@ -56,7 +56,7 @@ class MoveUnjamIntegralComprisedCommand : public tap::control::ComprisedCommand tap::Drivers &drivers, IntegrableSetpointSubsystem &subsystem, MoveIntegralCommand &moveIntegralCommand, - UnjamIntegralCommand &unjamIntegralCommand); + UnjamCommandInterface &unjamCommand); bool isReady() override; @@ -75,7 +75,7 @@ class MoveUnjamIntegralComprisedCommand : public tap::control::ComprisedCommand MoveIntegralCommand &moveIntegralCommand; - UnjamIntegralCommand &unjamCommand; + UnjamCommandInterface &unjamCommand; /// True if the agitator is being unjammed bool unjamSequenceCommencing; diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command_interface.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command_interface.hpp new file mode 100644 index 00000000..2e6dde8a --- /dev/null +++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command_interface.hpp @@ -0,0 +1,45 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2022 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_UNJAM_COMMAND_INTERFACE_HPP_ +#define TAPROOT_UNJAM_COMMAND_INTERFACE_HPP_ + +#include + +#include "tap/architecture/timeout.hpp" +#include "tap/control/command.hpp" + +#include "../interfaces/integrable_setpoint_subsystem.hpp" + +namespace tap::control::setpoint +{ +/** + * Command that attempts to unjam an agitator subsystem. + */ +class UnjamCommandInterface : public tap::control::Command +{ +}; + +} // namespace tap::control::setpoint + +#endif // TAPROOT_UNJAM_COMMAND_INTERFACE_HPP_ diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_integral_command.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_integral_command.hpp index fade75eb..8818c701 100644 --- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_integral_command.hpp +++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_integral_command.hpp @@ -31,6 +31,8 @@ #include "../interfaces/integrable_setpoint_subsystem.hpp" +#include "unjam_command_interface.hpp" + namespace tap::control::setpoint { /** @@ -48,7 +50,7 @@ namespace tap::control::setpoint * Like most velocity commands this one will not schedule/will deschedule if * IntegrableSetpointSubsystem goes offline. */ -class UnjamIntegralCommand : public tap::control::Command +class UnjamIntegralCommand : public UnjamCommandInterface { public: /// Config struct that the user passes into the UnjamIntegralCommand's constructor. diff --git a/ut-robomaster/taproot/src/tap/control/subsystem.cpp b/ut-robomaster/taproot/src/tap/control/subsystem.cpp index d897ecd3..79ff5966 100644 --- a/ut-robomaster/taproot/src/tap/control/subsystem.cpp +++ b/ut-robomaster/taproot/src/tap/control/subsystem.cpp @@ -34,6 +34,7 @@ namespace control Subsystem::Subsystem(Drivers* drivers) : drivers(drivers), defaultCommand(nullptr), + testCommand(nullptr), globalIdentifier(CommandScheduler::constructSubsystem(this)) { } @@ -54,12 +55,21 @@ void Subsystem::setDefaultCommand(Command* command) } } -const char* Subsystem::getName() { return "Subsystem"; } +void Subsystem::setTestCommand(Command* command) +{ + if (command != nullptr) + { + testCommand = command; + } +} + +const char* Subsystem::getName() const { return "Subsystem"; } #if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) Subsystem::Subsystem() : drivers(nullptr), defaultCommand(nullptr), + testCommand(nullptr), globalIdentifier(CommandScheduler::constructSubsystem(this)) { } diff --git a/ut-robomaster/taproot/src/tap/control/subsystem.hpp b/ut-robomaster/taproot/src/tap/control/subsystem.hpp index d5652fde..a5b51f43 100644 --- a/ut-robomaster/taproot/src/tap/control/subsystem.hpp +++ b/ut-robomaster/taproot/src/tap/control/subsystem.hpp @@ -111,38 +111,37 @@ class Subsystem */ virtual void refreshSafeDisconnect() {} - mockable inline bool isHardwareTestComplete() const { return hardwareTestsComplete; } - - mockable inline void setHardwareTestsIncomplete() - { - hardwareTestsComplete = false; - onHardwareTestStart(); - } - - mockable inline void setHardwareTestsComplete() - { - hardwareTestsComplete = true; - onHardwareTestComplete(); - } + /** + * Sets the test command of the `Subsystem`. The test command can be run + * by calling `CommandScheduler::runHardwareTests`. + * + * Test commands must keep track of their state so that `Command::isFinished` + * continues to return true after the command has ended. + * + * @param testCommand the test Command to associate with this subsystem + */ + mockable void setTestCommand(Command* testCommand); - virtual void runHardwareTests() { setHardwareTestsComplete(); } + /** + * Gets the test command for this subsystem. Returns `nullptr` if no test + * command is currently associated with the subsystem. + * + * @return the test command associated with this subsystem + */ + mockable inline Command* getTestCommand() const { return testCommand; } - virtual const char* getName(); + virtual const char* getName() const; mockable inline int getGlobalIdentifier() const { return globalIdentifier; } protected: Drivers* drivers; - bool hardwareTestsComplete = false; - - virtual void onHardwareTestStart() {} - - virtual void onHardwareTestComplete() {} - private: Command* defaultCommand; + Command* testCommand; + /** * An identifier unique to a subsystem that will be assigned to it automatically upon * construction and unassigned during destruction. diff --git a/ut-robomaster/taproot/src/tap/control/turret_subsystem_interface.hpp b/ut-robomaster/taproot/src/tap/control/turret_subsystem_interface.hpp index 96a8c7e3..8e96c857 100644 --- a/ut-robomaster/taproot/src/tap/control/turret_subsystem_interface.hpp +++ b/ut-robomaster/taproot/src/tap/control/turret_subsystem_interface.hpp @@ -24,7 +24,7 @@ #ifndef TAPROOT_TURRET_SUBSYSTEM_INTERFACE_HPP_ #define TAPROOT_TURRET_SUBSYSTEM_INTERFACE_HPP_ -#include "tap/algorithms/contiguous_float.hpp" +#include "tap/algorithms/wrapped_float.hpp" #include "subsystem.hpp" @@ -61,11 +61,12 @@ class TurretSubsystemInterface : public Subsystem /** * @return The current value of the turret's physical yaw. */ - virtual const tap::algorithms::ContiguousFloat &getCurrentYawValue() const = 0; + virtual const tap::algorithms::WrappedFloat &getCurrentYawValue() const = 0; + /** - * @see getCurrentYawValue + * @return The current physical pitch of the turret. */ - virtual const tap::algorithms::ContiguousFloat &getCurrentPitchValue() const = 0; + virtual const tap::algorithms::WrappedFloat &getCurrentPitchValue() const = 0; /** * @return `true` if the turret is online (i.e.: is connected) diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp index 9149c604..3b970524 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp @@ -81,8 +81,25 @@ class DjiMotor : public can::CanRxListener, public MotorInterface static constexpr uint16_t ENC_RESOLUTION = 8192; // Maximum values for following motors + // Controller for the M2006, in mA output + static constexpr uint16_t MAX_OUTPUT_C610 = 10000; + // Controller for the M3508, in mA output (Mapped to a 20A range though, not 1:1) static constexpr uint16_t MAX_OUTPUT_C620 = 16384; + // Controller for the M3510, in mA output (Not known if mapped to 20A range) + static constexpr uint16_t MAX_OUTPUT_820R = 32767; + + // Output is in mV static constexpr uint16_t MAX_OUTPUT_GM6020 = 25000; + // Output is in mV + static constexpr uint16_t MAX_OUTPUT_GM3510 = 29000; + + // Internal gear ratio of the following motors + static constexpr float GEAR_RATIO_M3508 = 3591.0f / 187.0f; + static constexpr float GEAR_RATIO_M3510_L1 = 3.7f / 1.0f; + static constexpr float GEAR_RATIO_M3510_L2 = 5.2f / 1.0f; + static constexpr float GEAR_RATIO_M3510_L3 = 19.0f / 1.0f; + static constexpr float GEAR_RATIO_M3510_L4 = 27.0f / 1.0f; + static constexpr float GEAR_RATIO_M2006 = 36.0f / 1.0f; /** * @param drivers a pointer to the drivers struct diff --git a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp index cf6e8f54..c684fb26 100644 --- a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp +++ b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp @@ -70,13 +70,13 @@ class DoubleDjiMotor : public MotorInterface int16_t getTorque() const override; int16_t getShaftRPM() const override; -private: +protected: #if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) public: testing::NiceMock motorOne; testing::NiceMock motorTwo; -private: +protected: #else DjiMotor motorOne; DjiMotor motorTwo; diff --git a/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp b/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp index 39741a2b..157a9bdc 100644 --- a/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp @@ -38,6 +38,7 @@ class Bmi088Mock : public tap::communication::sensors::imu::bmi088::Bmi088 MOCK_METHOD(void, initialize, (float, float, float), (override)); MOCK_METHOD(void, periodicIMUUpdate, (), (override)); + MOCK_METHOD(void, read, (), (override)); MOCK_METHOD(ImuState, getImuState, (), (const override)); MOCK_METHOD(void, requestRecalibration, (), (override)); MOCK_METHOD(float, getYaw, (), (override)); diff --git a/ut-robomaster/taproot/test/tap/mock/command_scheduler_mock.hpp b/ut-robomaster/taproot/test/tap/mock/command_scheduler_mock.hpp index bbd4b314..9a701fdc 100644 --- a/ut-robomaster/taproot/test/tap/mock/command_scheduler_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/command_scheduler_mock.hpp @@ -46,8 +46,15 @@ class CommandSchedulerMock : public control::CommandScheduler MOCK_METHOD(bool, isCommandScheduled, (const control::Command *), (const override)); MOCK_METHOD(void, registerSubsystem, (control::Subsystem *), (override)); MOCK_METHOD(bool, isSubsystemRegistered, (const control::Subsystem *), (const override)); - MOCK_METHOD(void, startHardwareTests, (), (override)); - MOCK_METHOD(void, stopHardwareTests, (), (override)); + + MOCK_METHOD(void, runAllHardwareTests, (), (override)); + MOCK_METHOD(void, runHardwareTest, (const control::Subsystem *), (override)); + MOCK_METHOD(void, stopAllHardwareTests, (), (override)); + MOCK_METHOD(void, stopHardwareTest, (const control::Subsystem *), (override)); + MOCK_METHOD(int, countRunningHardwareTests, (), (override)); + MOCK_METHOD(bool, isRunningTest, (const control::Subsystem *), (override)); + MOCK_METHOD(bool, hasPassedTest, (const control::Subsystem *), (override)); + MOCK_METHOD(int, subsystemListSize, (), (const override)); MOCK_METHOD(int, commandListSize, (), (const override)); MOCK_METHOD(CommandIterator, cmdMapBegin, (), (override)); diff --git a/ut-robomaster/taproot/test/tap/mock/subsystem_mock.cpp b/ut-robomaster/taproot/test/tap/mock/subsystem_mock.cpp index 6cb88a92..49b66ba5 100644 --- a/ut-robomaster/taproot/test/tap/mock/subsystem_mock.cpp +++ b/ut-robomaster/taproot/test/tap/mock/subsystem_mock.cpp @@ -25,6 +25,13 @@ namespace tap::mock { -SubsystemMock::SubsystemMock(Drivers *drivers) : control::Subsystem(drivers) {} +SubsystemMock::SubsystemMock(Drivers *drivers) : control::Subsystem(drivers) +{ + // Most of the time tests expect that we don't have a test command + // for the subsystem. This makes tests cleaner + EXPECT_CALL(*this, getTestCommand) + .Times(testing::AnyNumber()) + .WillRepeatedly(testing::Return(nullptr)); +} SubsystemMock::~SubsystemMock() {} } // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/subsystem_mock.hpp b/ut-robomaster/taproot/test/tap/mock/subsystem_mock.hpp index 31226c4b..4b320bc7 100644 --- a/ut-robomaster/taproot/test/tap/mock/subsystem_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/subsystem_mock.hpp @@ -43,11 +43,9 @@ class SubsystemMock : public control::Subsystem MOCK_METHOD(control::Command *, getDefaultCommand, (), (const override)); MOCK_METHOD(void, refresh, (), (override)); MOCK_METHOD(void, refreshSafeDisconnect, (), ()); - MOCK_METHOD(bool, isHardwareTestComplete, (), (const override)); - MOCK_METHOD(void, setHardwareTestsComplete, (), (override)); - MOCK_METHOD(void, setHardwareTestsIncomplete, (), (override)); - MOCK_METHOD(void, runHardwareTests, (), (override)); - MOCK_METHOD(const char *, getName, (), (override)); + MOCK_METHOD(void, setTestCommand, (control::Command * testCommand), (override)); + MOCK_METHOD(control::Command *, getTestCommand, (), (const override)); + MOCK_METHOD(const char *, getName, (), (const override)); }; // class SubsystemMock } // namespace mock } // namespace tap