diff --git a/.gitignore b/.gitignore index e303e2c..d5a3c47 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ +# CLion +.idea/ + # Compiled Object files *.o *.obj diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3f8fe0d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.20) +project(DEVIL5-RobotCode LANGUAGES C CXX) + +# Modern C++ +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Include source and header files +include_directories( + ${CMAKE_SOURCE_DIR}/src + ${CMAKE_SOURCE_DIR}/include +) + +file(GLOB_RECURSE SOURCES + src/*.c + src/*.cpp + include/*.h + include/*.hpp +) + +# Dummy build target for Intellisense purposes +add_executable(dummy_index ${SOURCES}) +set_target_properties(dummy_index PROPERTIES + LINKER_LANGUAGE CXX + EXCLUDE_FROM_ALL TRUE + CXX_STANDARD 20 + CXX_STANDARD_REQUIRED YES + CXX_EXTENSIONS NO +) + +# Actual build target that points to PROS's Makefile +add_custom_target(vex_build ALL + COMMAND make + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + COMMENT "Building project with PROS Makefile" +) \ No newline at end of file diff --git a/Makefile b/Makefile index a56b121..6a835b9 100644 --- a/Makefile +++ b/Makefile @@ -14,7 +14,7 @@ INCDIR=$(ROOT)/include WARNFLAGS+= EXTRA_CFLAGS= -EXTRA_CXXFLAGS= +EXTRA_CXXFLAGS=-Wall -Wextra -Wpedantic -Wdouble-promotion -Wno-unused-parameter # Set to 1 to enable hot/cold linking USE_PACKAGE:=1 diff --git a/arm-none-eabi-toolchain.cmake b/arm-none-eabi-toolchain.cmake new file mode 100644 index 0000000..4d89b57 --- /dev/null +++ b/arm-none-eabi-toolchain.cmake @@ -0,0 +1,19 @@ +# Indicate cross-compilation +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_PROCESSOR arm) + +# Compilers +set(CMAKE_C_COMPILER "$ENV{PROS_TOOLCHAIN}/bin/arm-none-eabi-gcc.exe") +set(CMAKE_CXX_COMPILER "$ENV{PROS_TOOLCHAIN}/bin/arm-none-eabi-g++.exe") + +# Disable trying to compile test programs for the host +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) + +# Paths for finding includes and libraries +set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# Additional flags for ARM Cortex V5 (VEX) +set(CMAKE_C_FLAGS "--specs=nosys.specs -mcpu=cortex-m7 -mthumb") +set(CMAKE_CXX_FLAGS "--specs=nosys.specs -mcpu=cortex-m7 -mthumb") diff --git a/assets/test.txt b/assets/test.txt deleted file mode 100644 index ea795ae..0000000 --- a/assets/test.txt +++ /dev/null @@ -1,4 +0,0 @@ -JUMPTO -60.719 -50.196 0.000 -DRIVE 42.000 -ROTATE 60.000 -DRIVE 40.501 \ No newline at end of file diff --git a/common.mk b/common.mk index 8dc01bb..40da70c 100644 --- a/common.mk +++ b/common.mk @@ -1,7 +1,7 @@ ARCHTUPLE=arm-none-eabi- DEVICE=VEX EDR V5 -MFLAGS=-mcpu=cortex-a9 -mfpu=neon-fp16 -mfloat-abi=softfp -Os -g +MFLAGS=-mcpu=cortex-a9 -mfpu=neon-fp16 -mfloat-abi=hard -Os -g -mthumb CPPFLAGS=-D_POSIX_THREADS -D_UNIX98_THREAD_MUTEX_ATTRIBUTES -D_POSIX_TIMERS -D_POSIX_MONOTONIC_CLOCK GCCFLAGS=-ffunction-sections -fdata-sections -fdiagnostics-color -funwind-tables @@ -20,8 +20,8 @@ WARNFLAGS+=-Wno-psabi SPACE := $() $() COMMA := , -C_STANDARD?=gnu11 -CXX_STANDARD?=gnu++20 +C_STANDARD?=gnu2x +CXX_STANDARD?=gnu++23 DEPDIR := .d $(shell mkdir -p $(DEPDIR)) @@ -34,7 +34,7 @@ LIBRARIES+=$(wildcard $(FWDIR)/*.a) EXCLUDE_COLD_LIBRARIES+=$(FWDIR)/libc.a $(FWDIR)/libm.a COLD_LIBRARIES=$(filter-out $(EXCLUDE_COLD_LIBRARIES), $(LIBRARIES)) wlprefix=-Wl,$(subst $(SPACE),$(COMMA),$1) -LNK_FLAGS=--gc-sections --start-group $(strip $(LIBRARIES)) -lgcc -lstdc++ --end-group -T$(FWDIR)/v5-common.ld +LNK_FLAGS=--gc-sections --start-group $(strip $(LIBRARIES)) -lgcc -lstdc++ --end-group -T$(FWDIR)/v5-common.ld --no-warn-rwx-segments --sort-section=alignment --sort-common ASMFLAGS=$(MFLAGS) $(WARNFLAGS) CFLAGS=$(MFLAGS) $(CPPFLAGS) $(WARNFLAGS) $(GCCFLAGS) --std=$(C_STANDARD) @@ -188,7 +188,7 @@ quick: $(DEFAULT_BIN) all: clean $(DEFAULT_BIN) -clean: +clean:: @echo Cleaning project -$Drm -rf $(BINDIR) -$Drm -rf $(DEPDIR) @@ -206,6 +206,7 @@ clean-template: -$Drm -rf $(TEMPLATE_DIR) $(LIBAR): $(call GETALLOBJ,$(EXCLUDE_SRC_FROM_LIB)) $(EXTRA_LIB_DEPS) + -$Dmkdir $(BINDIR) -$Drm -f $@ $(call test_output_2,Creating $@ ,$(AR) rcs $@ $^, $(DONE_STRING)) @@ -213,7 +214,7 @@ $(LIBAR): $(call GETALLOBJ,$(EXCLUDE_SRC_FROM_LIB)) $(EXTRA_LIB_DEPS) library: $(LIBAR) .PHONY: template -template: clean-template $(LIBAR) +template:: clean-template $(LIBAR) $Dpros c create-template . $(LIBNAME) $(VERSION) $(foreach file,$(TEMPLATE_FILES) $(LIBAR),--system "$(file)") --target v5 $(CREATE_TEMPLATE_FLAGS) endif @@ -262,7 +263,7 @@ $(foreach asmext,$(ASMEXTS),$(eval $(call asm_rule,$(asmext)))) define c_rule $(BINDIR)/%.$1.o: $(SRCDIR)/%.$1 -$(BINDIR)/%.$1.o: $(SRCDIR)/%.$1 $(DEPDIR)/$(basename $1).d +$(BINDIR)/%.$1.o: $(SRCDIR)/%.$1 $(DEPDIR)/$(basename %).d $(VV)mkdir -p $$(dir $$@) $(MAKEDEPFOLDER) $$(call test_output_2,Compiled $$< ,$(CC) -c $(INCLUDE) -iquote"$(INCDIR)/$$(dir $$*)" $(CFLAGS) $(EXTRA_CFLAGS) $(DEPFLAGS) -o $$@ $$<,$(OK_STRING)) @@ -305,4 +306,4 @@ cxx-sysroot: $(DEPDIR)/%.d: ; .PRECIOUS: $(DEPDIR)/%.d -include $(wildcard $(patsubst $(SRCDIR)/%,$(DEPDIR)/%.d,$(CSRC) $(CXXSRC))) +include $(wildcard $(patsubst $(SRCDIR)/%,$(DEPDIR)/%.d,$(CSRC) $(CXXSRC))) \ No newline at end of file diff --git a/firmware/libc.a b/firmware/libc.a index 51439b9..a2e1cdb 100644 Binary files a/firmware/libc.a and b/firmware/libc.a differ diff --git a/firmware/liblvgl.a b/firmware/liblvgl.a index 81e5f1a..e35a122 100644 Binary files a/firmware/liblvgl.a and b/firmware/liblvgl.a differ diff --git a/firmware/libm.a b/firmware/libm.a index 3d4066d..63c9951 100644 Binary files a/firmware/libm.a and b/firmware/libm.a differ diff --git a/firmware/libpros.a b/firmware/libpros.a index 36de8fa..b316dae 100644 Binary files a/firmware/libpros.a and b/firmware/libpros.a differ diff --git a/firmware/v5-common.ld b/firmware/v5-common.ld index 762a905..7f262fc 100644 --- a/firmware/v5-common.ld +++ b/firmware/v5-common.ld @@ -132,12 +132,6 @@ SECTIONS *(.gcc_except_table) } > RAM -.mmu_tbl (ALIGN(16384)) : { - __mmu_tbl_start = .; - *(.mmu_tbl) - __mmu_tbl_end = .; -} > RAM - .ARM.exidx : { __exidx_start = .; *(.ARM.exidx*) @@ -166,11 +160,9 @@ SECTIONS __fini_array_end = .; } > RAM -.ARM.attributes : { - __ARM.attributes_start = .; - *(.ARM.attributes) - __ARM.attributes_end = .; -} > RAM +/DISCARD/ : { + *(.ARM.attributes*) +} .sdata : { __sdata_start = .; @@ -228,36 +220,7 @@ _SDA2_BASE_ = __sdata2_start + ((__sbss2_end - __sdata2_start) / 2 ); _heap_end = .; HeapLimit = .; } > HEAP - -.stack (NOLOAD) : { - . = ALIGN(16); - _stack_end = .; - . += _STACK_SIZE; - . = ALIGN(16); - _stack = .; - __stack = _stack; - . = ALIGN(16); - _irq_stack_end = .; - . += _IRQ_STACK_SIZE; - . = ALIGN(16); - __irq_stack = .; - _supervisor_stack_end = .; - . += _SUPERVISOR_STACK_SIZE; - . = ALIGN(16); - __supervisor_stack = .; - _abort_stack_end = .; - . += _ABORT_STACK_SIZE; - . = ALIGN(16); - __abort_stack = .; - _fiq_stack_end = .; - . += _FIQ_STACK_SIZE; - . = ALIGN(16); - __fiq_stack = .; - _undef_stack_end = .; - . += _UNDEF_STACK_SIZE; - . = ALIGN(16); - __undef_stack = .; -} > COLD_MEMORY - +/* There are no sections for the stack. This is intentional, since VEXOs sets up a stack before + starting the program, and FreeRTOS task stacks are dynamically allocated. */ _end = .; } diff --git a/include/api.h b/include/api.h index d56915c..5ba908b 100644 --- a/include/api.h +++ b/include/api.h @@ -8,7 +8,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * \copyright Copyright (c) 2017-2023, Purdue University ACM SIGBots. + * \copyright Copyright (c) 2017-2024, Purdue University ACM SIGBots. * All rights reserved. * * This Source Code Form is subject to the terms of the Mozilla Public @@ -39,13 +39,8 @@ #include #endif /* __cplusplus */ -#define PROS_VERSION_MAJOR 4 -#define PROS_VERSION_MINOR 1 -#define PROS_VERSION_PATCH 1 -#define PROS_VERSION_STRING "4.1.1" - - #include "pros/adi.h" +#include "pros/ai_vision.h" #include "pros/colors.h" #include "pros/device.h" #include "pros/distance.h" @@ -65,6 +60,7 @@ #ifdef __cplusplus #include "pros/adi.hpp" +#include "pros/ai_vision.hpp" #include "pros/colors.hpp" #include "pros/device.hpp" #include "pros/distance.hpp" diff --git a/include/devils/2025/pjRobot.hpp b/include/devils/2025/pjRobot.hpp index 6897423..6c39dc1 100644 --- a/include/devils/2025/pjRobot.hpp +++ b/include/devils/2025/pjRobot.hpp @@ -16,64 +16,21 @@ namespace devils { PJRobot() { - imu.calibrate(); - conveyorSensor.setLEDBrightness(100); - conveyor.useSensor(&conveyorSensor); - - odometry.useIMU(&imu); - odometry.setSensorOffsets(verticalSensorOffset, horizontalSensorOffset); - odometry.runAsync(); } void autonomous() override { - // Default State - intakeSystem.setArmPosition(IntakeSystem::BOTTOM_RING); - mogoGrabber.setMogoGrabbed(false); - - // Calibrate IMU - imu.waitUntilCalibrated(); - - // Run Autonomous - bool isBlue = autoOptions.allianceColor == AllianceColor::BLUE_ALLIANCE; - switch (autoOptions.routine.id) - { - case 0: - PJMatchAuto::southAuto(chassis, odometry, intakeSystem, conveyor, mogoGrabber, isBlue, true); - break; - case 1: - PJMatchAuto::southAuto(chassis, odometry, intakeSystem, conveyor, mogoGrabber, isBlue, false); - break; - case 2: - PJSkillsAuto::runSkills(chassis, odometry, intakeSystem, conveyor, mogoGrabber); - break; - } + } void opcontrol() override { // Default State - intakeSystem.setArmPosition(IntakeSystem::INTAKE); - mogoGrabber.setMogoGrabbed(false); - - - // Skills Startup - bool isSkills = autoOptions.routine.id == 2; - bool isBlue = autoOptions.allianceColor == AllianceColor::BLUE_ALLIANCE; - auto opponentRingType = (isSkills || !isBlue) ? RingType::BLUE : RingType::RED; - if (isSkills) - { - imu.waitUntilCalibrated(); - PJSkillsStartAuto::runStart(chassis, odometry, intakeSystem, conveyor, mogoGrabber); - } // Stop autonomous AutoStep::stopAll(); - // Climb - bool isClimbing = false; - // Loop while (true) { @@ -83,20 +40,6 @@ namespace devils double rightX = mainController.get_analog(ANALOG_RIGHT_X) / 127.0; double rightY = mainController.get_analog(ANALOG_RIGHT_Y) / 127.0; - bool lowArmInput = mainController.get_digital(DIGITAL_B); - bool midArmInput = mainController.get_digital(DIGITAL_A) || mainController.get_digital(DIGITAL_Y); - bool highArmInput = mainController.get_digital(DIGITAL_X) || mainController.get_digital(DIGITAL_L1); - bool climbInput = mainController.get_digital_new_press(DIGITAL_UP); - - bool goalRushInput = mainController.get_digital_new_press(DIGITAL_LEFT); - bool pickupInput = mainController.get_digital(DIGITAL_R2); - - bool clawInput = mainController.get_digital_new_press(DIGITAL_R1); - bool mogoInput = mainController.get_digital_new_press(DIGITAL_L2); - bool slowInput = false; // mainController.get_digital(DIGITAL_L1); - - bool colorSortInput = mainController.get_digital(DIGITAL_DOWN) || mainController.get_digital(DIGITAL_RIGHT); - // Curve Joystick Inputs leftY = JoystickCurve::curve(leftY, 3.0, 0.1, 0.15); leftX = JoystickCurve::curve(leftX, 3.0, 0.05, 0.2); @@ -109,81 +52,9 @@ namespace devils // Combine Left and Right X Joystick Inputs double combinedX = JoystickCurve::combine(leftX, rightX); - // Climb - if (climbInput) - isClimbing = !isClimbing; - - // Intake Arm - if (lowArmInput) - intakeSystem.setArmPosition(IntakeSystem::BOTTOM_RING); - else if (midArmInput) - intakeSystem.setArmPosition(IntakeSystem::ALLIANCE_STAKE); - else if (highArmInput) - intakeSystem.setArmPosition(IntakeSystem::NEUTRAL_STAKE); - else if (isClimbing) - intakeSystem.setArmPosition(IntakeSystem::UP); - else - intakeSystem.setArmPosition(IntakeSystem::INTAKE); - intakeSystem.moveArmToPosition(); - intakeSystem.disableSpeedClamp(lowArmInput); - - // Intake Claw - if (clawInput) - { - // Toggle Claw Grabber - bool shouldGrab = !intakeSystem.getClawGrabbed(); - intakeSystem.setClawGrabbed(shouldGrab); - - if (!shouldGrab) - mainController.rumble(".."); - } - - // Mogo Grab - if (mogoInput) - { - // Goal-Rush Mode - if (goalRushSystem.getExtended()) - { - goalRushSystem.setClamped(!goalRushSystem.getClamped()); - if (goalRushSystem.getClamped()) - mainController.rumble("."); - } - - // Rear-Mogo Mode - else - { - mogoGrabber.setMogoGrabbed(!mogoGrabber.getMogoGrabbed()); - if (mogoGrabber.getMogoGrabbed()) - mainController.rumble("."); - } - } - - // Goal Rush - if (goalRushInput) - { - goalRushSystem.setExtended(!goalRushSystem.getExtended()); - if (goalRushSystem.getExtended()) - mainController.rumble("..."); - } - - // Slow Mode - double speedMultiplier = slowInput ? 0.5 : 1.0; - - // Conveyor - if (isSkills && colorSortInput) - conveyor.setRingSorting(RingType::NONE); - else if (isSkills || colorSortInput) - conveyor.setRingSorting(opponentRingType); - else - conveyor.setRingSorting(RingType::NONE); - - conveyor.setMogoGrabbed(mogoGrabber.getMogoGrabbed()); - conveyor.setArmLowered(intakeSystem.getArmPosition() == IntakeSystem::ArmPosition::BOTTOM_RING); // Always allow the conveyor to move - conveyor.moveAutomatic(pickupInput ? 1.0 : rightY); - conveyor.setPaused(false); // Drive normally - chassis.move(leftY * speedMultiplier, combinedX * speedMultiplier); + chassis.move(leftY, combinedX); // Delay to prevent the CPU from being overloaded pros::delay(20); @@ -213,42 +84,17 @@ namespace devils SmartMotorGroup leftMotors = SmartMotorGroup("LeftMotors", {-11, 3, -12, 4, -1}); SmartMotorGroup rightMotors = SmartMotorGroup("RightMotors", {6, -9, 5, -10, 21}); - SmartMotorGroup conveyorMotors = SmartMotorGroup("ConveyorMotors", {-19, 8}); - SmartMotorGroup intakeArmMotors = SmartMotorGroup("IntakeArmMotors", {-17, 18}); - - RotationSensor verticalSensor = RotationSensor("VerticalOdom", 13); - RotationSensor horizontalSensor = RotationSensor("HorizontalOdom", 20); - - OpticalSensor conveyorSensor = OpticalSensor("ConveyorSensor", 16); - InertialSensor imu = InertialSensor("IMU", 15); - - ADIPneumatic mogoPneumatic = ADIPneumatic("MogoPneumatic", 1); - ADIPneumatic intakeClawPneumatic = ADIPneumatic("IntakeClawPneumatic", 2); - ADIPneumatic goalRushDeployPneumatic = ADIPneumatic("GoalRushDeployPneumatic", 3); - ADIPneumatic goalRushClampPneumatic = ADIPneumatic("GoalRushClampPneumatic", 4); - - ADIDigitalInput mogoRushSensor = ADIDigitalInput("MogoSensor", 8); // LED Strips LEDStrip ledStrip = LEDStrip(9); // Subsystems TankChassis chassis = TankChassis(leftMotors, rightMotors); - ConveyorSystem conveyor = ConveyorSystem(conveyorMotors); - MogoGrabSystem mogoGrabber = MogoGrabSystem(mogoPneumatic); - IntakeSystem intakeSystem = IntakeSystem(intakeClawPneumatic, intakeArmMotors); - PerpendicularSensorOdometry odometry = PerpendicularSensorOdometry(verticalSensor, horizontalSensor, DEAD_WHEEL_RADIUS); - GoalRushSystem goalRushSystem = GoalRushSystem(goalRushDeployPneumatic, goalRushClampPneumatic, mogoRushSensor); - SymmetricControl symmetricControl = SymmetricControl(leftMotors, rightMotors); - - // Auto - VBOdom vbOdom = VBOdom("PJ", odometry); + RobotAutoOptions autoOptions = RobotAutoOptions(); std::vector routines = { - {0, "Match (end center)", true}, - {1, "Match (end side)", true}, - {2, "Skills", false}, + {0, "Default", false}, }; // Renderer OptionsRenderer optionsRenderer = OptionsRenderer("PepperJack", routines, &autoOptions); diff --git a/include/devils/2025/robinRobot.hpp b/include/devils/2025/robinRobot.hpp index 7ffb236..47608a2 100644 --- a/include/devils/2025/robinRobot.hpp +++ b/include/devils/2025/robinRobot.hpp @@ -22,6 +22,8 @@ namespace devils odometry.useIMU(&imu); odometry.setTicksPerRevolution(300); odometry.runAsync(); + + t(); } void autonomous() override diff --git a/include/devils/2026/autonomous/autoIntakeStep.hpp b/include/devils/2026/autonomous/autoIntakeStep.hpp new file mode 100644 index 0000000..15e4f33 --- /dev/null +++ b/include/devils/2026/autonomous/autoIntakeStep.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include "../subsystems/intakeSystem.hpp" +#include "../subsystems/StickSystem.hpp" + +namespace devils +{ + class AutoIntakeStep : public AutoStep + { + public: + AutoIntakeStep( + IntakeSystem& intake, + StickSystem& stick, + const float targetSpeed = 1.0) + : intake(intake), + stick(stick), + targetSpeed(targetSpeed) + { + } + + void onUpdate() override + { + intake.runIntake(targetSpeed); + intake.setStickStalled(stick.checkStalled()); + } + + /** + * Sets the target speed of the intake. + * @param speed The target speed of the intake. + */ + void setTargetSpeed(const float speed) + { + targetSpeed = speed; + } + + private: + IntakeSystem& intake; + StickSystem& stick; + float targetSpeed = 1.0; + }; +} diff --git a/include/devils/2026/autonomous/matchAuto.hpp b/include/devils/2026/autonomous/matchAuto.hpp new file mode 100644 index 0000000..9eeb885 --- /dev/null +++ b/include/devils/2026/autonomous/matchAuto.hpp @@ -0,0 +1,204 @@ +#pragma once + +#include "devils/devils.h" +#include "../subsystems/StickSystem.hpp" +#include "../subsystems/intakeSystem.hpp" +#include "../subsystems/tubeSystem.hpp" +#include "../subsystems/wingSystem.hpp" +#include "stickAutoStep.hpp" +#include "autoIntakeStep.hpp" + +namespace devils +{ + class TubeSystem; + class IntakeSystem; + + class MatchAuto + { + public: + static void run(TankChassis& chassis, + OdomSource& odom, + StickSystem& stick, + IntakeSystem& intake, + TubeSystem& tube, + WingSystem& wings, + const bool isRight = false) + { + TrajectoryConstraints::defaultConstraints = TrajectoryConstraints{ + .maxVelocity = 50.0f + }; + auto slowSpeed = TrajectoryConstraints{.maxVelocity = 15.0f}; + TrajectoryConstraints::defaultConstraints = slowSpeed; + + const auto stickAutoStep = std::make_shared(stick); + stickAutoStep->start(); + + const auto intakeAutoStep = std::make_shared(intake, stick); + intakeAutoStep->start(); + + // ===================================== + // WIP Match Auto + // Generated by AutoDevil 0.3.1 + // https://auto.devilbots.org/ + // ===================================== + + auto autoBuilder = AutoBuilder(chassis, odom); + + if (isRight) + { + autoBuilder.setPoseTransformer(std::make_unique()); + autoBuilder.jumpTo({-46, -14, Units::degToRad(0)}); + } + else + autoBuilder.jumpTo({-46, -14, Units::degToRad(180)}); + tube.setTubeRaised(true); + + + // 0. Deploy PJ + /*if (isRight) + { + tube.setTubeRaised(true); + wings.setWingRaised(true); + } */ + + // Drop off at center goal + stick.homeStick(); + //deploy pj + if (isRight) + { + tube.setTubeRaised(true); + //opwn hood later + //stick.setState(StickSystem::State::EXTEND_FOR_PJ_DEPLOY); + } + //drives to center goal, pj faces forwards + if (isRight) + autoBuilder.driveTo({-14.5f, -11, Units::degToRad(45)})->startSync(); + else + autoBuilder.driveTo({-15, -12, Units::degToRad(225)})->startSync(); + stick.setState(StickSystem::State::RETRACTED); + + //score center goal, pj uses intake + if (isRight) + { + intake.setArmsExtended(true); + intakeAutoStep->setTargetSpeed(-.65f); + pros::delay(1000); + } + else + { + tube.setTubeRaised(false); + wings.setWingRaised(false); + stick.setState(StickSystem::State::EXTENDED_FAST); + pros::delay(1000); + } + + // Move to loader + stick.setState(StickSystem::State::RETRACTED); + intake.setArmsExtended(true); + if (isRight) + { + stick.setState(StickSystem::State::EXTENDED_FAST); + pros::delay(500); + tube.setTubeRaised(false); + autoBuilder.driveTo({-19.5, -16, Units::degToRad(45)})->startSync(); + stick.setState(StickSystem::State::RETRACTED); + tube.setTubeRaised(true); + const auto backupFromGoalStep = autoBuilder.driveTo({-50, -45, Units::degToRad(180)}, 40.0f)->start(); + pros::delay(100); + intakeAutoStep->setTargetSpeed(1.0f); + autoBuilder.rotateTo(180)->startSync(); + tube.setTubeRaised(true); + backupFromGoalStep->join(); + } + else + { + const auto backupFromGoalStep = autoBuilder.driveTo({-50, -43, Units::degToRad(180)}, 40.0f)->start(); + pros::delay(100); + intakeAutoStep->setTargetSpeed(1.0f); + backupFromGoalStep->join(); + autoBuilder.rotateTo(180)->startSync(); + tube.setTubeRaised(true); + } + autoBuilder.driveTo({-62, isRight ? -41 : -43.0f, Units::degToRad(180)})->startSync(); + // Wiggle while picking up from loader + //wiggle(chassis, 3.5f, 0.2f); + pros::delay(1500); + + // Move to long goal + autoBuilder.rotateTo(180)->startSync(); + autoBuilder.driveTo({-32, isRight ? -42.0f : -43.0f, Units::degToRad(180)})->startSync(); + scoreInLongGoal(chassis, stick, StickSystem::State::EXTEND_FOR_THREE); + + // Spit blocks + autoBuilder.driveTo({-45, isRight ? -40 : -42.0f, Units::degToRad(180)})->startSync(); + intakeAutoStep->setTargetSpeed(-1.0f); + autoBuilder.rotateTo(300)->startSync(); + stick.setState(StickSystem::State::EXTENDED_FAST); + autoBuilder.rotateTo(180)->startSync(); + stick.setState(StickSystem::State::RETRACTED); + intakeAutoStep->setTargetSpeed(1.0f); + + // Move to loader + autoBuilder.driveTo({-61.5f, isRight ? -41 : -43.0f, Units::degToRad(180)})->startSync(); + //wiggle(chassis, 3.5f, 0.2f); + pros::delay(2000); + + // Score in long goal + autoBuilder.rotateTo(180)->startSync(); + autoBuilder.driveTo({-32, isRight ? -42.0f : -44.0f, Units::degToRad(180)})->startSync(); + scoreInLongGoal(chassis, stick, StickSystem::State::EXTEND_FOR_THREE); + //stick.setState(StickSystem::State::EXTEND_FOR_THREE); + + // ===================================== + // End of WIP Match Auto + // ===================================== + } + + static void wiggle(TankChassis& chassis, const float duration, const float magnitude = 0.15f) + { + auto wiggleTimer = Timer(duration); + wiggleTimer.start(); + + while (!wiggleTimer.getIsFinished()) + { + chassis.move(0.1f, 0.15f, 0); + pros::delay(100); + chassis.move(0.1f, -0.15f, 0); + pros::delay(100); + chassis.stop(); + pros::delay(300); + + if (wiggleTimer.getIsFinished()) + break; + + + chassis.move(0.1f, -magnitude, 0); + pros::delay(200); + chassis.stop(); + pros::delay(300); + chassis.move(0.1f, magnitude, 0); + pros::delay(200); + chassis.stop(); + pros::delay(300); + } + } + + static void scoreInLongGoal( + TankChassis& chassis, + StickSystem& stick, + const StickSystem::State targetState = StickSystem::State::EXTENDED_SLOW) + { + // Apply pressure + chassis.move(-0.1f, 0, 0); + + // Extend stick + stick.setState(targetState); + pros::delay(1000); + stick.setState(StickSystem::State::RETRACTED); + pros::delay(500); + stick.setState(targetState); + pros::delay(1000); + stick.setState(StickSystem::State::RETRACTED); + } + }; +} diff --git a/include/devils/2026/autonomous/skillsAuto.hpp b/include/devils/2026/autonomous/skillsAuto.hpp new file mode 100644 index 0000000..37a850b --- /dev/null +++ b/include/devils/2026/autonomous/skillsAuto.hpp @@ -0,0 +1,229 @@ +#pragma once + +#include "devils/devils.h" +#include "matchAuto.hpp" +#include "../subsystems/wingSystem.hpp" + +namespace devils +{ + class SkillsAuto + { + public: + static void runEmergencyMode(IntakeSystem& intake) { + intake.runIntake(1.0f); + pros::delay(1000); + } + + static void run(TankChassis& chassis, + OdomSource& odom, + StickSystem& stick, + IntakeSystem& intake, + TubeSystem& tube, + WingSystem& wings, + const bool isRight = false) + { + auto defaultSpeed = TrajectoryConstraints::defaultConstraints; + auto slowSpeed = TrajectoryConstraints{.maxVelocity = 8.0f}; + + const auto stickAutoStep = std::make_shared(stick); + stickAutoStep->start(); + + const auto intakeAutoStep = std::make_shared(intake, stick); + intakeAutoStep->start(); + intakeAutoStep->setTargetSpeed(1.0f); + + // ===================================== + // Skills Auto (Purdue Slam & Jam) + // Generated by AutoDevil 0.3.1 + // https://auto.devilbots.org/ + // ===================================== + + auto autoBuilder = AutoBuilder(chassis, odom); + + if (isRight) + autoBuilder.setPoseTransformer(std::make_unique()); + + if (!isRight) + autoBuilder.jumpTo({-44, -14, Units::degToRad(0)}); + else + autoBuilder.jumpTo({-44, -14, Units::degToRad(0)}); + stick.homeStick(); + + // 0. Deploy PJ + if (isRight) + { + tube.setTubeRaised(true); + wings.setWingRaised(true); + } + + // 1. Pickup N Blocks + autoBuilder.driveTo({-6, -16, Units::degToRad(0)})->startSync(); + + tube.setTubeRaised(false); + wings.setWingRaised(false); + + TrajectoryConstraints::defaultConstraints = slowSpeed; + autoBuilder.driveTo({2, -24, Units::degToRad(-90)})->startSync(); + autoBuilder.driveTo({2, -48, Units::degToRad(-90)})->startSync(); + TrajectoryConstraints::defaultConstraints = defaultSpeed; + intake.setArmsExtended(true); + pros::delay(500); + + + // 2. Center Goal + if (!isRight) + { + auto step2 = autoBuilder.driveTo({-11, -22, Units::degToRad(0)})->start(); + pros::delay(500); + tube.setTubeRaised(true); + step2->join(); + + autoBuilder.rotateTo(230)->startSync(); + autoBuilder.driveTo({-10, -18, Units::degToRad(230)})->startSync(); + + tube.setTubeRaised(false); + pros::delay(200); + stick.setState(StickSystem::State::EXTENDED_SLOWER); + pros::delay(1000); + stick.setState(StickSystem::State::RETRACTED); + pros::delay(400); + stick.setState(StickSystem::State::EXTENDED_SLOWER); + pros::delay(1000); + stick.setState(StickSystem::State::RETRACTED); + tube.setTubeRaised(true); + + autoBuilder.driveTo({-48, -48, Units::degToRad(180)}, 40.0f)->startSync(); + } + else + { + auto step2 = autoBuilder.driveTo({-14, -21, Units::degToRad(0)})->start(); + pros::delay(500); + tube.setTubeRaised(true); + step2->join(); + + autoBuilder.rotateTo(47)->startSync(); + autoBuilder.driveTo({-12.5f, -16, Units::degToRad(47)})->startSync(); + intakeAutoStep->setTargetSpeed(-0.9f); + pros::delay(2500); + intakeAutoStep->setTargetSpeed(1.0f); + + autoBuilder.driveTo({-14, -21, Units::degToRad(0)})->startSync(); + autoBuilder.rotateTo(230)->startSync(); + } + + // 3. NW Loader + intake.setArmsExtended(true); + autoBuilder.driveTo({-58.5f, -44, Units::degToRad(180)})->startSync(); + MatchAuto::wiggle(chassis, 4.0f); + + // 4. N Long Goal + autoBuilder.driveTo({-33, isRight ? -62.0f : -61.0f, Units::degToRad(180)}, 40.0f)->startSync(); + autoBuilder.driveTo({39, -60, Units::degToRad(180)})->startSync(); + autoBuilder.driveTo({45, isRight ? -49 : -48.0f, Units::degToRad(360)}, 20.0f)->startSync(); + autoBuilder.driveTo({33, isRight ? -49.5f : -48.0f, Units::degToRad(360)})->startSync(); + scoreInLongGoal(chassis, stick); + + // 5. NE Loader + autoBuilder.driveTo({62, isRight ? -48.5f : -49.0f, Units::degToRad(0)})->startSync(); + MatchAuto::wiggle(chassis, 4.0f); + + // 6. N Long Goal + autoBuilder.driveTo({34, isRight ? -50 : -49.0f, Units::degToRad(0)})->startSync(); + scoreInLongGoal(chassis, stick); + intake.setArmsExtended(false); + + // 7. Park (and clear parking zone) + if (!isRight) + { + autoBuilder.driveTo({56, -42, Units::degToRad(0)})->startSync(); + autoBuilder.driveTo({30, -28, Units::degToRad(0)}, 40.0f)->startSync(); + autoBuilder.driveTo({-40, -38, Units::degToRad(0)}, 40.0f)->startSync(); + autoBuilder.driveTo({-58, -22, Units::degToRad(-90)}, 20.0f)->startSync(); + autoBuilder.driveTo({-59, -18, Units::degToRad(-90)})->startSync(); + + intake.setArmsExtended(false); + tube.setTubeRaised(false); + + // Align to park zone + chassis.move(-0.2f, 0, 0); + pros::delay(1000); + + // Backup for speed + chassis.move(0.2f, 0, 0); + pros::delay(200); + + // Go over initial hump + chassis.move(-0.6f, 0, 0); + pros::delay(800); + + // Slow down for second hump + chassis.move(-0.2f, 0, 0); + pros::delay(900); + chassis.stop(); + } + else + { + autoBuilder.driveTo({56, -58, Units::degToRad(-65)})->startSync(); + autoBuilder.driveTo({66.5f, -17, Units::degToRad(-90)})->startSync(); + + // Align to park zone + chassis.move(-0.2f, 0, 0); + pros::delay(1000); + + // Backup for speed + chassis.move(0.2f, 0, 0); + pros::delay(200); + + // Go over initial hump + chassis.move(-0.6f, 0, 0); + pros::delay(800); + + // Drive over second hump + chassis.move(-0.4f, 0, 0); + pros::delay(1000); + chassis.stop(); + + pros::delay(1000); + autoBuilder.jumpTo({64, 32, Units::degToRad(-90)}); + autoBuilder.driveTo({40, 40, Units::degToRad(45)})->startSync(); + autoBuilder.driveTo({-40, 38, Units::degToRad(0)}, 40.0f)->startSync(); + autoBuilder.driveTo({-60.5f, 20, Units::degToRad(90)}, 20.0f)->startSync(); + autoBuilder.driveTo({-61.5f, 16, Units::degToRad(90)})->startSync(); + + // Align to park zone + chassis.move(-0.2f, 0, 0); + pros::delay(1000); + + // Backup for speed + chassis.move(0.2f, 0, 0); + pros::delay(200); + + // Go over initial hump + chassis.move(-0.6f, 0, 0); + pros::delay(500); + chassis.stop(); + } + + // ===================================== + // End of Skills Auto - Purdue + // ===================================== + } + + static void scoreInLongGoal(TankChassis& chassis, StickSystem& stick) + { + // Apply pressure + chassis.move(-0.1f, 0, 0); + + // Extend stick + stick.setState(StickSystem::State::EXTENDED_SLOW); + pros::delay(1000); + stick.setState(StickSystem::State::RETRACTED); + pros::delay(500); + stick.setState(StickSystem::State::EXTENDED_SLOW); + pros::delay(1000); + + // Retract stick + stick.setState(StickSystem::State::RETRACTED); + } + }; +} diff --git a/include/devils/2026/autonomous/stickAutoStep.hpp b/include/devils/2026/autonomous/stickAutoStep.hpp new file mode 100644 index 0000000..a7f24bb --- /dev/null +++ b/include/devils/2026/autonomous/stickAutoStep.hpp @@ -0,0 +1,22 @@ +#pragma once +#include "../subsystems/StickSystem.hpp" + +namespace devils +{ + class StickAutoStep : public AutoStep + { + public: + StickAutoStep(StickSystem& stickSystem) + : stickSystem(stickSystem) + { + } + + void onUpdate() override + { + stickSystem.moveStick(); + } + + private: + StickSystem& stickSystem; + }; +} \ No newline at end of file diff --git a/include/devils/2026/blazeRobot.hpp b/include/devils/2026/blazeRobot.hpp new file mode 100644 index 0000000..d67bfd1 --- /dev/null +++ b/include/devils/2026/blazeRobot.hpp @@ -0,0 +1,176 @@ +#pragma once + +#include "../devils.h" +#include "./autonomous/matchAuto.hpp" +#include "./autonomous/skillsAuto.hpp" +#include "./subsystems/intakeSystem.hpp" +#include "./subsystems/StickSystem.hpp" +#include "./subsystems/tubeSystem.hpp" +#include "./subsystems/wingSystem.hpp" + +namespace devils +{ + struct BlazeRobot : Robot + { + BlazeRobot() + { + imu.calibrate(); + + odometry->useIMU(&imu); + odometry->setSensorOffsets(VERTICAL_SENSOR_OFFSET, HORIZONTAL_SENSOR_OFFSET); + odometry->start(); + + devilBotsDisplay->start(); + toastDisplay->start(); + + constexpr auto joystickOptions = ControllerAxis::Options{ + .deadzone = 0.1f, // <-- Minimum input to register + .startingValue = 0.15f, // <-- Jumps to 15% to overcome motor friction + .exponent = 3.0f // <-- Cubes the input for finer control at low speeds + }; + + mainController.leftX.setOptions(joystickOptions); + mainController.leftY.setOptions(joystickOptions); + mainController.rightX.setOptions(joystickOptions); + mainController.rightY.setOptions(joystickOptions); + + mainController.up.setMode(ControllerButton::JUST_PRESSED); + mainController.right.setMode(ControllerButton::TOGGLED); + } + + void autonomous() override + { + imu.waitUntilDoneCalibrated(); + //SkillsAuto::run(chassis, *odometry.get(), stick, intake, tube, wings, false); + MatchAuto::run(chassis, *odometry.get(), stick, intake, tube, wings, false); + } + + void opcontrol() override + { + // Home the stick + stick.homeStick(); + + // Stop autonomous + AutoStep::stopAll(); + + bool isDriveReversed = false; + + // Loop + while (true) + { + // Take Controller Inputs + const float leftY = mainController.leftY; + const float leftX = mainController.leftX; + const float rightY = mainController.rightY; + const float rightX = mainController.rightX * 0.5f; + + const bool tubeExtendButton = mainController.l2; // Tube extend/retract + const bool intakeArmExtendButton = mainController.y; + const bool wingExtendButton = mainController.x; // Wing raise/lower + const bool stickFastButton = mainController.r1; + const bool stickSlowButton = mainController.r2; + + const bool driveReverseButton = mainController.b; // Reverse controls + const bool ptoButton = mainController.up; // PTO extend/retract (for testing) + + // Combine Left and Right X Joystick Inputs + const float combinedX = Math::largestMagnitude({leftX, rightX}); + + // Reverse the drive + if (std::abs(leftY) < MAX_ROBOT_SPEED_TO_SWITCH_DIRECTION) + isDriveReversed = driveReverseButton; + const auto driveDirection = isDriveReversed ? -1.0f : 1.0f; + + // Drive the robot with the left joystick + chassis.move(leftY * driveDirection, combinedX * 0.75f, 0); + + // Intake controls + intake.runIntake(rightY); + intake.setArmsExtended(intakeArmExtendButton); + intake.setStickStalled(stick.checkStalled()); + stick.rumbleIfStalled(mainController); + + // Stick pneumatic defaults + if (ptoButton) + stick.setPTOExtended(!stick.getPTOExtended()); + + // For fancy stick controls once testing is done + if (stickFastButton) + stick.setState(StickSystem::State::EXTENDED_FAST); + else if (stickSlowButton) + stick.setState(StickSystem::State::EXTENDED_SLOW); + else + stick.setState(StickSystem::State::RETRACTED); + + stick.moveStick(); + + wings.setWingRaised(tube.getTubeRaised() == wingExtendButton); + + tube.setTubeRaised(tubeExtendButton); + + + // Delay to prevent the CPU from being overloaded + pros::delay(20); + } + } + + void disabled() override + { + // Stop the robot + chassis.stop(); + + // Stop autonomous + AutoStep::stopAll(); + } + + // Constants + static constexpr float DEAD_WHEEL_RADIUS = 1; + static constexpr float MAX_ROBOT_SPEED_TO_SWITCH_DIRECTION = 0.5f; + + Vector2 VERTICAL_SENSOR_OFFSET = Vector2(-1.8, 0); + Vector2 HORIZONTAL_SENSOR_OFFSET = Vector2(0, -2.2); + + // Hardware + SmartMotorGroup leftMotors = SmartMotorGroup("LeftMotors", {10, -9, 8, -7, 6}); + SmartMotorGroup rightMotors = SmartMotorGroup("RightMotors", {-1, 2, -3, 4, -5}); + + SmartMotorGroup stickMotorsRight = SmartMotorGroup("StickMotorsRight", {-20}); + SmartMotorGroup stickMotorsLeft = SmartMotorGroup("StickMotorsLeft", {19}); + + SmartMotorGroup topRoller = SmartMotorGroup("TopIntakeRoller", {16}); + SmartMotorGroup sideRollers = SmartMotorGroup("SideRollers", {-18, 17}); + + ADIPneumatic tubePnematics = ADIPneumatic("TubePneumatics", 'C', false); + ADIPneumatic wingPnematics = ADIPneumatic("WingPneumatics", 'F', false); + ADIPneumaticGroup intakePnematics = ADIPneumaticGroup("IntakePneumatics", {'D', 'E'}, false); + ADIPneumaticGroup ptoPnematics = ADIPneumaticGroup("PTOPneumatics", {'A', 'B'}, false); + + ADIDigitalInput stickHomeSensor = ADIDigitalInput("StickSensor", 'H'); + + RotationSensor verticalSensor = RotationSensor("VerticalOdom", 12); + RotationSensor horizontalSensor = RotationSensor("HorizontalOdom", -11); + InertialSensor imu = InertialSensor("IMU", 13); + + // Subsystems + TankChassis chassis = TankChassis(leftMotors, rightMotors); + IntakeSystem intake = IntakeSystem(intakePnematics, sideRollers, topRoller); + StickSystem stick = StickSystem( + ptoPnematics, + stickMotorsLeft, + stickMotorsRight, + leftMotors, + rightMotors, + stickHomeSensor); + TubeSystem tube = TubeSystem(tubePnematics); + WingSystem wings = WingSystem(wingPnematics); + + std::shared_ptr odometry = std::make_shared( + verticalSensor, + horizontalSensor, + DEAD_WHEEL_RADIUS); + + // Displays + std::shared_ptr devilBotsDisplay = std::make_shared(); + std::shared_ptr toastDisplay = std::make_shared(); + }; +} diff --git a/include/devils/2026/pjRobot.hpp b/include/devils/2026/pjRobot.hpp new file mode 100644 index 0000000..47f0640 --- /dev/null +++ b/include/devils/2026/pjRobot.hpp @@ -0,0 +1,205 @@ +#pragma once + +#include "../devils.h" +#include "./autonomous/matchAuto.hpp" +#include "./autonomous/skillsAuto.hpp" +#include "./subsystems/intakeSystem.hpp" +#include "./subsystems/StickSystem.hpp" +#include "./subsystems/tubeSystem.hpp" +#include "./subsystems/wingSystem.hpp" + +namespace devils +{ + struct PJRobot : Robot + { + //for rising edge detection + bool prevTubeInput = false; + + PJRobot() + { + imu.calibrate(); + + odometry->useIMU(&imu); + odometry->setSensorOffsets(VERTICAL_SENSOR_OFFSET, HORIZONTAL_SENSOR_OFFSET); + odometry->start(); + + toastDisplay->start(); + // devilBotsDisplay->start(); + + constexpr auto joystickOptions = ControllerAxis::Options{ + .deadzone = 0.1f, // <-- Minimum input to register + .startingValue = 0.15f, // <-- Jumps to 15% to overcome motor friction + .exponent = 3.0f // <-- Cubes the input for finer control at low speeds + }; + + mainController.leftX.setOptions(joystickOptions); + mainController.leftY.setOptions(joystickOptions); + mainController.rightX.setOptions(joystickOptions); + mainController.rightY.setOptions(joystickOptions); + + mainController.up.setMode(ControllerButton::JUST_PRESSED); + mainController.right.setMode(ControllerButton::JUST_PRESSED); + } + + void autonomous() override + { + imu.waitUntilDoneCalibrated(); + //SkillsAuto::run(chassis, *odometry.get(), stick, intake, tube, wings, true); + MatchAuto::run(chassis, *odometry.get(), stick, intake, tube, wings, true); + } + + void opcontrol() override + { + // Home the stick + stick.homeStick(); + + // Stop autonomous + AutoStep::stopAll(); + + bool isDriveReversed = false; + + // Loop + while (true) + { + // Take Controller Inputs + const float leftY = mainController.leftY; + const float leftX = mainController.leftX; + const float rightY = mainController.rightY; + const float rightX = mainController.rightX * 0.5f; + + const bool tubeExtendButton = mainController.l2; // Tube extend/retract + const bool intakeArmExtendButton = mainController.y; + const bool wingExtendButton = mainController.x; // Wing raise/lower + const bool stickFastButton = mainController.r1; + const bool stickSlowButton = mainController.r2; + + const bool partnerA = partnerController.a; + const bool partnerB = partnerController.b; + + const bool driveReverseButton = mainController.b; // Reverse controls + const bool ptoButton = mainController.right; // PTO extend/retract (for testing) + + // Reverse the drive + if (std::abs(leftY) < MAX_ROBOT_SPEED_TO_SWITCH_DIRECTION) + isDriveReversed = driveReverseButton; + const auto driveDirection = isDriveReversed ? -1.0f : 1.0f; + + // Drive the robot with the left joystick + chassis.move(leftY * driveDirection, leftX * 0.75f, 0); + + // Intake controls + if (std::abs(rightY) > 0.1) // <-- Stick Deadzone + intake.runIntake(rightY); + else if (partnerA) + intake.runIntake(1.0f); + else if (partnerB) + intake.runIntake(-1.0f); + else + intake.runIntake(rightY); + + intake.setArmsExtended(intakeArmExtendButton); + intake.setStickStalled(stick.checkStalled()); + stick.rumbleIfStalled(mainController); + + // Stick pneumatic defaults + if (ptoButton) + stick.setPTOExtended(!stick.getPTOExtended()); + + // For fancy stick controls once testing is done + if (stickFastButton) + stick.setState(StickSystem::State::EXTENDED_FAST); + else if (stickSlowButton) + stick.setState(StickSystem::State::EXTENDED_SLOW); + else + stick.setState(StickSystem::State::RETRACTED); + + stick.moveStick(); + + wings.setWingRaised(tube.getTubeRaised() == wingExtendButton); + // if (!tube.getTubeRaised()) + // wings.setWingRaised(!wingExtendButton); + // else + // wings.setWingRaised(wingExtendButton); + + // if (tubeExtendButton && !prevTubeInput) + // tube.setTubeRaised(!tube.getTubeRaised()); + // prevTubeInput = tubeExtendButton; + //wings.setWingRaised(wingExtendButton); + if (tubeExtendButton) + tube.setTubeRaised(true); + else + tube.setTubeRaised(false); + + + + // Delay to prevent the CPU from being overloaded + pros::delay(20); + } + } + + void disabled() override + { + // Stop the robot + chassis.stop(); + + // Stop autonomous + AutoStep::stopAll(); + } + + // Constants + static constexpr float DEAD_WHEEL_RADIUS = 1; + static constexpr float MAX_ROBOT_SPEED_TO_SWITCH_DIRECTION = 0.5f; + + Vector2 VERTICAL_SENSOR_OFFSET = Vector2(-1.8, 0); + Vector2 HORIZONTAL_SENSOR_OFFSET = Vector2(0, -2.2); + + // Hardware + SmartMotorGroup leftMotors = SmartMotorGroup("LeftMotors", {10, -9, 8, -7, 6}); + SmartMotorGroup rightMotors = SmartMotorGroup("RightMotors", {-1, 2, -3, 4, -5}); + + SmartMotorGroup stickMotorsRight = SmartMotorGroup("StickMotorsRight", {-20}); + SmartMotorGroup stickMotorsLeft = SmartMotorGroup("StickMotorsLeft", {19}); + + SmartMotorGroup topRoller = SmartMotorGroup("TopIntakeRoller", {16}); + SmartMotorGroup sideRollers = SmartMotorGroup("SideRollers", {-18, 17}); + + ADIPneumatic tubePnematics = ADIPneumatic("TubePneumatics", 'C', false); + ADIPneumatic wingPnematics = ADIPneumatic("WingPneumatics", 'F', false); + ADIPneumaticGroup intakePnematics = ADIPneumaticGroup("IntakePneumatics", {'D', 'E'}, false); + ADIPneumaticGroup ptoPnematics = ADIPneumaticGroup("PTOPneumatics", {'A', 'B'}, false); + + ADIDigitalInput stickHomeSensor = ADIDigitalInput("StickSensor", 'H'); + + RotationSensor verticalSensor = RotationSensor("VerticalOdom", 12); + RotationSensor horizontalSensor = RotationSensor("HorizontalOdom", -11); + InertialSensor imu = InertialSensor("IMU", 13); + + // Subsystems + TankChassis chassis = TankChassis(leftMotors, rightMotors); + IntakeSystem intake = IntakeSystem(intakePnematics, sideRollers, topRoller); + StickSystem stick = StickSystem( + ptoPnematics, + stickMotorsLeft, + stickMotorsRight, + leftMotors, + rightMotors, + stickHomeSensor); + TubeSystem tube = TubeSystem(tubePnematics); + WingSystem wings = WingSystem(wingPnematics); + + std::shared_ptr odometry = std::make_shared( + verticalSensor, + horizontalSensor, + DEAD_WHEEL_RADIUS); + + // Displays + // std::shared_ptr devilBotsDisplay = std::make_shared(); + // AutoPickerDisplay autoPickerDisplay = AutoPickerDisplay( + // "PJ Robot", + // { + // AutoPickerDisplay::Routine{.id = 0, .displayName = "Match Auto", .requiresAllianceColor = false}, + // AutoPickerDisplay::Routine{.id = 1, .displayName = "Skills Auto", .requiresAllianceColor = false} + // }); + std::shared_ptr toastDisplay = std::make_shared(); + }; +} diff --git a/include/devils/2026/robinRobot.hpp b/include/devils/2026/robinRobot.hpp new file mode 100644 index 0000000..42600c4 --- /dev/null +++ b/include/devils/2026/robinRobot.hpp @@ -0,0 +1,100 @@ +#pragma once + +#include "../devils.h" + +namespace devils +{ + /** + * Represents a Robin robot and all of its subsystems. + */ + struct RobinRobot : Robot + { + /** + * Creates a new instance of Blaze. + */ + RobinRobot() + { + // Configure Controller Joysticks + constexpr ControllerAxis::Options joystickOptions = { + .deadzone = 0.1f, // <-- Minimum input to register + .startingValue = 0.15f, // <-- Jumps to 15% to overcome motor friction + .exponent = 3.0f // <-- Cubes the input for finer control at low speeds + }; + mainController.leftX.setOptions(joystickOptions); + mainController.leftY.setOptions(joystickOptions); + mainController.rightX.setOptions(joystickOptions); + mainController.rightY.setOptions(joystickOptions); + + // Configure IMU scaling + imu.setHeadingScale(1.013); + + // Configue odometry options + odometry->useIMU(&imu); + odometry->setTicksPerRevolution(300); + } + + void autonomous() override + { + } + + void opcontrol() override + { + while (true) + { + // Take Controller Inputs + const float leftY = mainController.leftY; + const float leftX = mainController.leftX; + const float rightY = mainController.rightY; + const float rightX = mainController.rightX * 0.5f; + + //stick movement buttons, will replace later with macros to move stick to preset rotations + const bool stickScoreButton = mainController.r1; //temp button to move stick + const bool stickResetButton = mainController.r2; //temp button to move stick + + if (stickScoreButton) stick.move(0.7f); + else if (stickResetButton) stick.move(-0.5f); + else stick.move(0.0f); + + floatingRollers.move(rightY); + + // Combine Left and Right X Joystick Inputs + const float combinedX = Math::largestMagnitude({leftX, rightX}); + + // Drive normally + chassis.move(leftY, combinedX * 0.5f, 0); + + // Delay to prevent the CPU from being overloaded + pros::delay(20); + } + } + + void disabled() override + { + } + + // Hardware + SmartMotorGroup leftMotors = SmartMotorGroup("LeftMotors", {16, -17, 18, -19, 20}); + SmartMotorGroup rightMotors = SmartMotorGroup("RightMotors", {-11, 12, -13, 14, -15}); + SmartMotorGroup floatingRollers = SmartMotorGroup("FloatingRoller", {-6}); + SmartMotorGroup stick = SmartMotorGroup("Stick", {-4}); + InertialSensor imu = InertialSensor("IMU", 1); + + // Subsystems + TankChassis chassis = TankChassis(leftMotors, rightMotors); + + // Auto + std::shared_ptr odometry = std::make_shared(chassis, 1.375, 11); + + // // Auto Options + // RobotAutoOptions autoOptions = RobotAutoOptions(); + // std::vector routines = { + // {0, "Match 1", true}, + // {1, "Match 2", true}, + // {2, "Skills 1", false}, + // {3, "Skills 2", false} + // }; + // + // // Renderer + // OptionsRenderer optionsRenderer = OptionsRenderer("Robin", routines, &autoOptions); + }; +} diff --git a/include/devils/2026/subsystems/StickSystem.hpp b/include/devils/2026/subsystems/StickSystem.hpp new file mode 100644 index 0000000..57a6741 --- /dev/null +++ b/include/devils/2026/subsystems/StickSystem.hpp @@ -0,0 +1,341 @@ +#pragma once + +#include "devils/devils.h" + +namespace devils +{ + /** + * Represents the intake arm and claw system of the robot. + */ + class StickSystem + { + public: + enum State + { + EXTENDED_FAST, + EXTENDED_SLOW, + EXTENDED_SLOWER, + RETRACTED, + EXTEND_FOR_THREE, + EXTEND_FOR_PJ_DEPLOY, + STOP + }; + + StickSystem( + ADIPneumaticGroup& pto, + SmartMotorGroup& leftStickMotors, + SmartMotorGroup& rightStickMotors, + SmartMotorGroup& leftDriveMotors, + SmartMotorGroup& rightDriveMotors, + ADIDigitalInput& stickHomeSensor) + : pto(pto), + leftStickMotors(leftStickMotors), + rightStickMotors(rightStickMotors), + leftDriveMotors(leftDriveMotors), + rightDriveMotors(rightDriveMotors), + stickHomeSensor(stickHomeSensor) + { + leftStickMotors.setPosition(0); + rightStickMotors.setPosition(0); + } + + /** + * Sets whether the PTO is extended or not. + * If the PTO is extended, the stick will follow the drive motors. + * If the PTO is retracted, the stick will move to the position corresponding to the current state. + * @param extended - True to extend the PTO, false to retract it. + */ + void setPTOExtended(const bool extended) const + { + pto.setExtended(extended); + } + + /** + * Sets the current state of the stick system. + * @param state - The state to set the stick system to. + */ + void setState(const State state) + { + currentState = state; + if (currentState == EXTENDED_FAST || + currentState == EXTENDED_SLOW || + currentState == EXTEND_FOR_THREE || + currentState == EXTEND_FOR_PJ_DEPLOY) + { + pto.setExtended(false); + } + } + + /** + * Gets whether the PTO is currently extended or not. + * @return True if the PTO is extended, false otherwise. + */ + bool getPTOExtended() const + { + return pto.getExtended(); + } + + /** + * Moves the stick based on the current state. + * If the PTO is extended, the stick will follow the drive motors. + * Otherwise, it will move to the position corresponding to the current state. + */ + void moveStick() + { + // If the PTO state has changed, start the actuation timer to prevent movement for a short duration + if (pto.getExtended() != wasPTOExtended) + { + ptoActuationTimer.start(); + wasPTOExtended = pto.getExtended(); + homeStick(); + } + if (ptoActuationTimer.getIsRunning()) + { + leftStickMotors.move(0); + rightStickMotors.move(0); + return; + } + + // If the PTO is extended, the stick should follow the drive motors + if (pto.getExtended()) + { + leftStickMotors.move(leftDriveMotors.getLastVoltage()); + rightStickMotors.move(rightDriveMotors.getLastVoltage()); + return; + } + + // Check if we are homed + const auto isStickHomed = getIsStickHomed(); + const auto didHomeTimeout = stickHomingTimer.getIsFinished(); + + if (isStickHoming && (isStickHomed || didHomeTimeout)) + { + leftStickMotors.setPosition(0); + rightStickMotors.setPosition(0); + isStickHoming = false; + } + + // Try to home the stick if we are not homed yet + if (isStickHoming) + { + moveStick(HOMING_SPEED); + return; + } + + // Don't move motor if retracting and homed + /*if (isStickHomed && currentState == RETRACTED) + { + moveStick(0); + return; + } */ + + // Otherwise, go to the current state + auto expectedPosition = getExpectedPosition(); + switch (currentState) + { + case EXTENDED_FAST: + moveToPosition(expectedPosition, FAST_SPEED); + break; + case EXTENDED_SLOW: + moveToPosition(expectedPosition, SLOW_SPEED); + break; + case EXTENDED_SLOWER: + moveToPosition(expectedPosition, SLOWER_SPEED); + break; + case RETRACTED: + moveToPosition(expectedPosition, RETRACTION_SPEED); + break; + case EXTEND_FOR_THREE: + moveToPosition(expectedPosition, FAST_SPEED); + break; + case EXTEND_FOR_PJ_DEPLOY: + moveToPosition(expectedPosition, FAST_SPEED); + break; + case STOP: + moveStick(0); + break; + }; + } + + /** + * Manually moves the stick at a specified speed. The speed should be between -1 and 1, where positive values move the stick up and negative values move the stick down. If the PTO is extended, the stick will not move. + * @param speed - The speed at which to move the stick (from -1 to 1). Positive values move the stick up, negative values move the stick down. The speed is limited to prevent the stick from moving too fast. + */ + void moveStick(const float speed) const + { + if (pto.getExtended()) return; // Don't move stick if PTO is extended + + leftStickMotors.move(speed); + rightStickMotors.move(speed); + } + + /** + * Starts the homing process for the stick. + * @details + * This will cause the stick to move downwards until the stick home sensor is triggered. + * This should be called whenever we want to ensure that we know the position of the stick, + * such as after starting the robot or after changing the PTO state. + */ + void homeStick() + { + stickHomingTimer.start(); + isStickHoming = true; + } + + /** + * Checks if the stick is stalled during retraction. + * This is done by checking the current of the stick motors. + * If the stick is trying to retract but the motor current is above a certain threshold, we can assume it's stalled. + * @return True if the stick is stalled, false otherwise. + */ + bool checkStalled() const + { + // Check for retraction stall + const auto stickMotorCurrent = leftStickMotors.getCurrent(); + if (!stickMotorCurrent.isSuccess()) + return false; + + // If the stick is trying to retract but the motor current is above the stall threshold, we can assume it's stalled + if (stickMotorCurrent > RETRACTION_STALL_CURRENT) + return true; + return false; + } + + void rumbleIfStalled(Controller& mainController) const + { + if (checkStalled()) + mainController.rumble("..."); + } + + /** + * Gets the current position of the stick. + * This is done by checking the position of the stick motor encoders. + * @return The current position of the stick in ticks. If the position cannot be determined, returns an error code. + */ + HWResult getStickPosition() const + { + const auto leftStickPosition = leftStickMotors.getPosition(); + if (leftStickPosition.isSuccess()) + return leftStickPosition.value; + + const auto rightStickPosition = rightStickMotors.getPosition(); + if (rightStickPosition.isSuccess()) + return rightStickPosition.value; + + Logger::warn("Failed to get stick position from both motors"); + return ERROR_UNKNOWN; + } + + + /** + * Gets the expected position of the stick based on the current state. + * This is used for logging and debugging purposes to + * know where we expect the stick to be based on the current state. + * @return The expected position of the stick in ticks based on the current state. If the state is unknown, returns 0.0f. + */ + float getExpectedPosition() + { + switch (currentState) + { + case EXTENDED_FAST: + case EXTENDED_SLOW: + case EXTENDED_SLOWER: + return EXPECTED_POSITION_UP; + case RETRACTED: + return EXPECTED_POSITION_DOWN; + case EXTEND_FOR_THREE: + return EXPECTED_POSITION_EXTEND_FOR_THREE; + case EXTEND_FOR_PJ_DEPLOY: + return EXPECTED_POSITION_UP_DEPLOY; + default: + return 0.0f; + }; + } + + /** + * Moves the stick to a target position using a PID controller. The speed of the movement is limited by the `speed` parameter. If the PTO is extended, the stick will not move. + * @param targetPosition - The target position for the stick to move to (in ticks). This should be set based on the expected positions of the stick when fully extended and fully retracted. + * @param speed - The maximum speed at which the stick should move (from 0 to 1). This limits the output of the PID controller to prevent the stick from moving too fast. + */ + void moveToPosition(const float targetPosition, const float speed) + { + const auto currentPosition = getStickPosition(); + if (!currentPosition.isSuccess()) + return; + + const float error = targetPosition - currentPosition; // Calculate the error + float output = stickPID.update(error); // Calculate the PID output + if (output > speed) + output = speed; // Limit the output to the specified speed + else if (output < -speed) + output = -speed; + + moveStick(output); // Move the stick based on the PID output + } + + /** + * Gets whether the stick is currently homed or not. + * @return True if the stick is homed, false otherwise. + */ + bool getIsStickHomed() + { + const auto stickHomeSensorResult = stickHomeSensor.getValue(); + if (!stickHomeSensorResult.isSuccess()) + { + Logger::warn("Failed to get stick home sensor value"); + return false; + } + + const bool isHomed = stickHomeSensorResult.value; + if (!isHomed) + stickHomedTimer.stop(); + + if (stickHomedTimer.getIsFinished()) + return true; + + if (isHomed && !stickHomedTimer.getIsRunning()) + stickHomedTimer.start(); + + return false; + } + + private: + static constexpr float FAST_SPEED = 0.8f; // % + static constexpr float SLOW_SPEED = 0.5f; // % + static constexpr float SLOWER_SPEED = 0.4f; // % + static constexpr float RETRACTION_SPEED = 0.6f; // % + static constexpr float HOMING_SPEED = 0.6f; // % + + static constexpr float RETRACTION_STALL_CURRENT = 1.7f; // amps + static constexpr float PTO_PAUSE_DURATION = 0.2f; // seconds + + // Expected position of the stick when fully retracted (in ticks) + static constexpr float EXPECTED_POSITION_DOWN = -10.0f; + // Expected position of the stick when fully extended (in ticks) + static constexpr float EXPECTED_POSITION_UP = -830.0f; + // Expected position of the stick when extended for three (in ticks) + static constexpr float EXPECTED_POSITION_EXTEND_FOR_THREE = -450.0f; + // Expected position of the stick when extended for pj's deploy + static constexpr float EXPECTED_POSITION_UP_DEPLOY = -674.0f; + + // Hardware + ADIPneumaticGroup& pto; + SmartMotorGroup& leftStickMotors; + SmartMotorGroup& rightStickMotors; + SmartMotorGroup& leftDriveMotors; + SmartMotorGroup& rightDriveMotors; + ADIDigitalInput& stickHomeSensor; + + PIDController stickPID{0.0035f, 0.0f, 0.0f}; // PID controller for stick position control + + State currentState{RETRACTED}; + Timer ptoActuationTimer = Timer(PTO_PAUSE_DURATION); // Pauses the stick movement while the pto actuates + Timer stickHomedTimer = Timer(0.1f); + Timer stickHomingTimer = Timer(1.0f); + // Timer to hold the home for a brief period to ensure we are actually homed + + bool wasPTOExtended = false; + bool isStickHoming = false; + }; +} + diff --git a/include/devils/2026/subsystems/intakeSystem.hpp b/include/devils/2026/subsystems/intakeSystem.hpp new file mode 100644 index 0000000..39d827b --- /dev/null +++ b/include/devils/2026/subsystems/intakeSystem.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include "devils/devils.h" +#include + +namespace devils +{ + /** + * Represents the intake arm and claw system of the robot. + */ + class IntakeSystem + { + public: + IntakeSystem( + ADIPneumaticGroup& intakeArms, + SmartMotorGroup& sideRollers, + SmartMotorGroup& topRollers) + : intakeArms(intakeArms), + sideRollers(sideRollers), + topRollers(topRollers) + { + } + + /** + * Runs the intake rollers at a specified speed. + * The speed should be between -1 and 1, where positive values run the rollers inwards to intake objects and negative values run the rollers outwards to expel objects. + * The speed is limited to prevent the rollers from running too fast. + * @param speed - The speed to run the intake rollers at, from -1 to 1. + */ + void runIntake(float speed) + { + if (std::abs(speed) < INPUT_DEADZONE && !isStickStalled) + { + sideRollers.move(0); + topRollers.move(IDLE_ROLLER_SPEED); + return; + } + + // Limit the speed to prevent the rollers from running too fast + speed = std::clamp(speed, -1.0f, 1.0f); + + sideRollers.move(speed * SIDE_ROLLER_SPEED); + + if (isStickStalled) + topRollers.move(-IDLE_ROLLER_SPEED); + else + topRollers.move(speed * TOP_ROLLER_SPEED); + } + + /** + * Sets whether the intake arms are extended or not. + * @param extended - True to extend the intake arms, false to retract them. + */ + void setArmsExtended(const bool extended) const + { + intakeArms.setExtended(extended); + } + + /** + * Sets the stick stalled state, which can be used to prevent the intake from running when the stick is stalled during retraction. + * @param isStalled - True if the stick is stalled, false otherwise. + */ + void setStickStalled(const bool isStalled) + { + this->isStickStalled = isStalled; + } + + private: + static constexpr float TOP_ROLLER_SPEED = 0.75f; + static constexpr float SIDE_ROLLER_SPEED = 0.75f; + + static constexpr float IDLE_ROLLER_SPEED = 0.5f; + static constexpr float INPUT_DEADZONE = 0.1f; + + bool isStickStalled = false; + + ADIPneumaticGroup& intakeArms; + SmartMotorGroup& sideRollers; + SmartMotorGroup& topRollers; + }; +} diff --git a/include/devils/2026/subsystems/tubeSystem.hpp b/include/devils/2026/subsystems/tubeSystem.hpp new file mode 100644 index 0000000..d0b9bd1 --- /dev/null +++ b/include/devils/2026/subsystems/tubeSystem.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "devils/devils.h" + +namespace devils +{ + class TubeSystem + { + public: + TubeSystem(ADIPneumatic& tubePneumatics) + : tube(tubePneumatics) + { + } + + /** + * Raises and lowers the tube + * @param extended - True to raise the tube, false to lower it. + */ + void setTubeRaised(const bool extended) const + { + tube.setExtended(extended); + } + + bool getTubeRaised() const + { + return tube.getExtended(); + } + + private: + ADIPneumatic& tube; + }; +} diff --git a/include/devils/2026/subsystems/wingSystem.hpp b/include/devils/2026/subsystems/wingSystem.hpp new file mode 100644 index 0000000..8893485 --- /dev/null +++ b/include/devils/2026/subsystems/wingSystem.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "devils/devils.h" + +namespace devils +{ + class WingSystem + { + public: + WingSystem(ADIPneumatic& wingPneumatics) + :wing(wingPneumatics) + { + } + + /** + * Raises and lowers the wing + * @param extended - True to raise the wing, false to lower it. + */ + void setWingRaised(const bool extended) const + { + wing.setExtended(extended); + } + + private: + ADIPneumatic& wing; + }; +} diff --git a/include/devils/autoSteps/autoBuilder.hpp b/include/devils/autoSteps/autoBuilder.hpp index 4ba9a22..131400f 100644 --- a/include/devils/autoSteps/autoBuilder.hpp +++ b/include/devils/autoSteps/autoBuilder.hpp @@ -3,292 +3,157 @@ #include "../geometry/units.hpp" #include "../odom/odomSource.hpp" #include "../chassis/chassisBase.hpp" -#include "../vexbridge/vbPath.hpp" -#include "./steps/autoJumpToStep.hpp" -#include "./steps/autoDriveStep.hpp" -#include "./steps/autoDriveToStep.hpp" -#include "./steps/autoDriveTimeStep.hpp" -#include "./steps/autoRotateToStep.hpp" -#include "./steps/autoTimeoutStep.hpp" -#include "./steps/autoPauseStep.hpp" -#include "./steps/autoPurePursuitStep.hpp" -#include "./steps/autoBoomerangStep.hpp" -#include "./steps/autoRamseteStep.hpp" -#include "./transformer/poseTransform.h" +#include "../trajectory/trajectoryGenerator.hpp" +#include "../path/splinePath.hpp" +#include "./steps/rotateMotionProfileStep.hpp" +#include "./steps/driveRAMSETEStep.hpp" +#include "../autoSteps/transformer/poseTransformer.hpp" namespace devils { + /** + * A builder for creating closed-loop autonomous routines w/ a chassis and odometry source. + * Used to quickly create autonomous routines with a fluent API. + */ class AutoBuilder { public: /** - * A builder for creating closed-loop autonomous routines w/ a chassis and odometry source. - * Used to quickly create autonomous routines with a fluent API. + * Creates a new autonomous builder with the given chassis and odometry source. * @param chassis The chassis to control * @param odom The odometry source to use */ AutoBuilder( - ChassisBase &chassis, - OdomSource &odom) + ChassisBase& chassis, + OdomSource& odom) : chassis(chassis), odom(odom) { } - void runAfterDelay(uint32_t delay, const std::function &callback) - { - // Create PROS task to run the callback after the delay - pros::Task([delay, callback]() - { - // - pros::delay(delay); - callback(); - // - }); - } - - /** - * Sets the pose of the robot - * @param pose The pose to set the robot to - * @returns A pointer to the created step - */ - AutoStepPtr setPose(Pose pose) - { - // Set the current pose - this->pose = pose; - - // Transform the pose - Pose transformedPose = tryTransformPose(pose); - - // Return a new `AutoJumpToStep` with the given pose - return std::make_shared(odom, transformedPose); - } - - /** - * Sets the pose of the robot - * @param x The x position to set the robot to in inches - * @param y The y position to set the robot to in inches - * @param rotation The rotation to set the robot to in degrees - * @returns A pointer to the created step - */ - AutoStepPtr setPose(double x, double y, double rotation) - { - // Create a new pose - Pose pose = Pose(x, y, Units::degToRad(rotation)); - - // Return a new `AutoJumpToStep` with the given pose - return setPose(pose); - } - /** - * Pauses the autonomous routine for a given duration - * @param duration The duration to pause in milliseconds + * Shorthand to drive to a specific location on the field. + * Uses 'AutoRamseteStep' by default, but can be changed to use other control methods in the future. + * @param pose - The pose to drive to + * @param endingVelocity - The desired velocity at the end of the trajectory in inches per second. Defaults to 0 for a full stop. * @returns A pointer to the created step */ - AutoStepPtr pause(uint32_t duration) - { - velocity = 0.0; - return std::make_shared(duration); - } - - /** - * Drives the robot to a given pose using a bezier curve, trajectory generation, and ramsete control. - * @param x The x position to drive to in inches - * @param y The y position to drive to in inches - * @param rotation The rotation to drive to in degrees - * @param isReversed Whether to drive in reverse or not - * @param finalVelocity The final velocity to drive at in inches per second. Speed is carried over from the previous step. - * @param strength The strength of the bezier curve (inches) - * @param constraints The constraints for the trajectory - * @param options The options for the drive step - * @returns A pointer to the created step - */ - AutoStepPtr driveToTrajectory( - double x, - double y, - double rotation, - bool isReversed = false, - double finalVelocity = 0, - double strength = 10.0, - TrajectoryConstraints constraints = {56, 64}, - AutoRamseteStep::Options options = AutoRamseteStep::Options::defaultOptions) + AutoStepPtr driveTo( + const Pose& pose, + const float endingVelocity = 0.0f) const { - // Create a new pose - Pose targetPose = Pose(x, y, Units::degToRad(rotation)); + const auto transformedPose = tryTransformPose(pose); - // Return a new `AutoRamseteStep` with the given pose - return driveToTrajectoryPose(targetPose, isReversed, finalVelocity, strength, constraints, options); + // TODO: Auto switch between `AutoRamseteStep` and `AutoHolonomicDriveStep` depending on the chassis type (holonomic or not). + const auto trajectory = generateTrajectoryToPose(transformedPose, endingVelocity); + return std::make_shared(chassis, odom, trajectory); } /** - * Drives the robot to a given pose using a bezier curve, trajectory generation, and ramsete control. - * @param pose The pose to drive to - * @param isReversed Whether to drive in reverse or not - * @param finalVelocity The final velocity to drive at in inches per second. Speed is carried over from the previous step. - * @param strength The strength of the bezier curve (inches) - * @param constraints The constraints for the trajectory - * @param options The options for the drive step - * @returns A pointer to the created step - */ - AutoStepPtr driveToTrajectoryPose( - Pose pose, - bool isReversed = false, - double finalVelocity = 0, - double strength = 10.0, - TrajectoryConstraints constraints = {48, 64}, - AutoRamseteStep::Options options = AutoRamseteStep::Options::defaultOptions) - { - // Transform the pose - Pose fromPose = tryTransformPose(this->pose); - Pose toPose = tryTransformPose(pose); - - // Create a new path - SplinePath path = SplinePath::makeArc(fromPose, toPose, strength, isReversed); - VBPath::sync("AutoBuilderPath", path); - - // Flip final velocity if the path is reversed - if (isReversed) - finalVelocity *= -1; - - // Generate Trajectory - auto trajectoryGenerator = TrajectoryGenerator( - constraints, - TrajectoryGenerator::PathInfo{velocity, finalVelocity}); - auto trajectory = trajectoryGenerator.calc(path); - - // Set the current pose - this->pose = pose; - velocity = finalVelocity; - - // Make a new `AutoRamseteStep` with the given trajectory - return std::make_shared(chassis, odom, trajectory, options); - } - - /** - * Drives to a given pose using boomerang control. See `AutoBoomerangStep` for more info. - * @param x The x position to drive to in inches - * @param y The y position to drive to in inches - * @param rotation The rotation to drive to in degrees - * @param timeout The timeout in milliseconds + * Rotates the robot a given amount + * @param distance The distance to rotate in degrees * @param options The options for the drive step * @returns A pointer to the created step */ - AutoStepPtr driveTo( - double x, - double y, - double rotation, - uint32_t timeout = 2000, - AutoDriveToStep::Options options = AutoDriveToStep::Options::defaultOptions) + AutoStepPtr rotate( + const float distance, + const RotateMotionProfileStep::Options& options = RotateMotionProfileStep::Options::defaultOptions) const { - // Create a new pose - Pose targetPose = Pose(x, y, Units::degToRad(rotation)); - - // Return a new `AutoBoomerangStep` with the given pose - return driveTo(targetPose, timeout, options); + const auto currentRotation = odom.getPose().rotation; + const auto targetRotation = currentRotation + Units::degToRad(distance); + // TODO: Transform pose here + + return std::make_shared( + chassis, + odom, + targetRotation, + options); } /** - * Drives to a given pose using boomerang control. See `AutoBoomerangStep` for more info. - * @param pose The pose to drive to - * @param timeout The timeout in milliseconds + * Rotates the robot to a given heading + * @param heading The heading to rotate to in degrees * @param options The options for the drive step * @returns A pointer to the created step */ - AutoStepPtr driveTo( - Pose pose, - uint32_t timeout = 2000, - AutoDriveToStep::Options options = AutoDriveToStep::Options::defaultOptions) + AutoStepPtr rotateTo( + const float heading, + RotateMotionProfileStep::Options options = RotateMotionProfileStep::Options::defaultOptions) const { - // Set the current pose - this->pose = pose; - velocity = 0.0; + const auto transformedPose = tryTransformPose(Pose(0, 0, Units::degToRad(heading))); - // Transform the pose - Pose transformedPose = tryTransformPose(pose); - - // Return a new `AutoBoomerangStep` with the given pose - return std::make_shared(std::make_shared(chassis, odom, transformedPose, options), timeout); + return std::make_shared( + chassis, + odom, + transformedPose.rotation, + options); } /** - * Rotates the robot a given amount - * @param distance The distance to rotate in degrees - * @param timeout The timeout in milliseconds - * @returns A pointer to the created step + * Sets the robot's current pose to a given pose. Useful for resetting odometry or "teleporting" the robot during autonomous. + * @param pose - The pose to set the robot to */ - AutoStepPtr rotate( - double distance, - uint32_t timeout = 2000, - AutoRotateToStep::Options options = AutoRotateToStep::Options::defaultOptions) + void jumpTo(const Pose& pose) const { - // Calculate the new heading (in degrees) - double newHeading = Units::radToDeg(pose.rotation) + distance; - - // Return a new `AutoRotateToStep` with the given heading - return rotateTo(newHeading, timeout, options); + const auto transformedPose = tryTransformPose(pose); + odom.setPose(transformedPose); } /** - * Drives the robot a given distance in closed loop - * @param distance The distance to drive in inches - * @param timeout The timeout in milliseconds - * @param options The options for the drive step + * Assigns a transformer to the auto builder to transform all poses used in the builder. + * @param newTransformer - The new transformer to use. Can be `nullptr` to remove the current transformer. */ - AutoStepPtr driveTrajectory( - double distance, - bool isReversed = false, - double finalVelocity = 0, - double strength = 10.0, - TrajectoryConstraints constraints = {48, 64}, - AutoRamseteStep::Options options = AutoRamseteStep::Options::defaultOptions) + void setPoseTransformer(std::unique_ptr newTransformer) { - // Create a new pose - Pose targetPose = Pose(pose.x + distance * std::cos(pose.rotation), - pose.y + distance * std::sin(pose.rotation), - pose.rotation); - - // Return a new `AutoDriveStep` with the given pose - return driveToTrajectoryPose(targetPose, isReversed, finalVelocity, strength, constraints, options); + transformer = std::move(newTransformer); } + protected: /** - * Rotates the robot to a given heading - * @param heading The heading to rotate to in degrees - * @param timeout The timeout in milliseconds - * @returns A pointer to the created step + * Generates a trajectory to a given pose using the current pose as the starting point. + * @param targetPose - The target pose to generate the trajectory to + * @param endingVelocity - The desired velocity at the end of the trajectory in inches per second. Defaults to 0 for a full stop. + * @return A shared pointer to the generated trajectory */ - AutoStepPtr rotateTo( - double heading, - uint32_t timeout = 2000, - AutoRotateToStep::Options options = AutoRotateToStep::Options::defaultOptions) + std::shared_ptr generateTrajectoryToPose( + const Pose& targetPose, + const float endingVelocity = 0.0f) const { - // Convert & apply the heading to the current pose - pose.rotation = Units::degToRad(heading); - velocity = 0.0; + // Check if the targetPose is behind the currentPose + const auto currentPose = odom.getPose(); + const auto isReversed = targetPose.isBehind(currentPose); + + // Calculate starting velocity based on current velocity and its direction relative to the target pose + const auto currentVelocity = odom.getVelocity(); + const auto deltaPose = targetPose - currentPose; + const auto dotProduct = deltaPose.x * currentVelocity.x + deltaPose.y * currentVelocity.y; + const auto velocityDirection = (dotProduct >= 0) ? 1.0f : -1.0f; + const auto startingVelocity = velocityDirection * currentVelocity.magnitude(); + Logger::info("Starting Velocity : " + std::to_string(startingVelocity) + " in/s"); + + // Generate Path + const auto distance = currentPose.distanceTo(targetPose); + auto path = SplinePath::makeArc( + currentPose, + targetPose, + distance * 0.25f, + isReversed); - // Transform the pose - Pose transformedPose = tryTransformPose(pose); - - // Return a new `AutoRotateToStep` with the given heading - return std::make_shared(std::make_shared(chassis, odom, transformedPose.rotation, options), timeout); - } - - /** - * Uses a pose transformation when building autonomous - * @param transformer - The transformation to apply - */ - void useTransformer(std::unique_ptr transformer) - { - this->transformer = std::move(transformer); + // Generate Trajectory + const auto generator = TrajectoryGenerator({ + .startingVelocity = startingVelocity, + .endingVelocity = endingVelocity + }); + const auto trajectory = generator.calc(path); + return trajectory; } - protected: /** * Tries to transform a pose using the assigned transformer, if any * @param pose - The pose to transform * @returns The transformed pose */ - Pose tryTransformPose(Pose pose) + Pose tryTransformPose(Pose pose) const { // If a transformer is assigned, transform the pose if (transformer) @@ -298,18 +163,10 @@ namespace devils return pose; } - private: - /// @brief The current robot pose (pre-transform) - Pose pose; - - /// @brief The current velocity of the robot in inches per second - double velocity = 0.0; - /// @brief The active transformer used to transform poses - std::unique_ptr transformer = nullptr; + std::unique_ptr transformer = nullptr; - // Input references - ChassisBase &chassis; - OdomSource &odom; + ChassisBase& chassis; + OdomSource& odom; }; -} \ No newline at end of file +} diff --git a/include/devils/autoSteps/autoStep.hpp b/include/devils/autoSteps/autoStep.hpp index 101b6f6..a649af3 100644 --- a/include/devils/autoSteps/autoStep.hpp +++ b/include/devils/autoSteps/autoStep.hpp @@ -1,49 +1,42 @@ #pragma once -#include "../utils/runnable.hpp" -#include "../utils/logger.hpp" -#include +#include "../utils/asyncTask.hpp" #include namespace devils { - struct AutoStep : public Runnable, std::enable_shared_from_this + class AutoStep : public AsyncTask { - void runAsync() override + public: + // Allows inheritance by running `AsyncTask` constructor/destructor + AutoStep() : AsyncTask("AutoStep") { - // Add this step to the active steps list - activeStepsMutex.take(); - activeSteps.push_back(shared_from_this()); - activeStepsMutex.give(); + } - // Run base class run method - Runnable::runAsync(); + AutoStep(const std::string& debugName) : AsyncTask(debugName) + { } + ~AutoStep() override = default; + /** - * Stops all active runnables. + * Stops all running `AutoStep` instance. + * Should be called on disable to ensure all autonomous steps are stopped cleanly. */ static void stopAll() { - // Stop all active steps - for (auto &step : activeSteps) - step->stop(); - - // Clear the active steps list - activeStepsMutex.take(); - activeSteps.clear(); - activeStepsMutex.give(); + const auto allRunningInstances = getAllRunningInstances(); + + // Check each instance to see if it's an AutoStep and stop it if so + for (auto& instance : allRunningInstances) + { + // Use dynamic_pointer_cast to check if the instance is an AutoStep + if (const auto autoStepInstance = std::dynamic_pointer_cast(instance)) + autoStepInstance->stop(); + } } - - private: - static pros::Mutex activeStepsMutex; // Mutex to protect access to the active runnables vector - static std::vector> activeSteps; }; - // Define the static members - pros::Mutex AutoStep::activeStepsMutex; - std::vector> AutoStep::activeSteps; - // Define a shared pointer type for AutoStep typedef std::shared_ptr AutoStepPtr; } diff --git a/include/devils/autoSteps/steps/autoBoomerangStep.hpp b/include/devils/autoSteps/steps/autoBoomerangStep.hpp deleted file mode 100644 index 261772f..0000000 --- a/include/devils/autoSteps/steps/autoBoomerangStep.hpp +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" -#include "../autoStep.hpp" -#include "../../chassis/chassisBase.hpp" -#include "../../odom/odomSource.hpp" -#include "../../path/path.hpp" -#include "autoDriveToStep.hpp" - -namespace devils -{ - /** - * Represents a step to drive to a pose using boomerang control. - * Results in a curved path toward the target pose. - * See: https://area-53-robotics.github.io/Intro-To-Robotics/software/advanced-concepts/boomerang/ - */ - class AutoBoomerangStep : public AutoDriveToStep - { - public: - /** - * Creates a new boomerang step. - * @param chassis The chassis to control. - * @param odomSource The odometry source to use. - * @param pose The target pose to drive to. - */ - AutoBoomerangStep( - ChassisBase &chassis, - OdomSource &odomSource, - Pose targetPose, - Options options = Options::defaultOptions) - : AutoDriveToStep(chassis, odomSource, targetPose, options), - targetPose(targetPose), - odomSource(odomSource) - { - } - - void onStart() override - { - // Start the drive step - AutoDriveToStep::onStart(); - } - - void onUpdate() override - { - // Get the current pose - Pose currentPose = odomSource.getPose(); - - // Get the distance to the target - double distance = currentPose.distanceTo(targetPose); - - // Calculate the dot product - double currentDotTarget = cos(currentPose.rotation) * (targetPose.x - currentPose.x) + - sin(currentPose.rotation) * (targetPose.y - currentPose.y); - - // Drive in reverse if the goal is behind us - if (currentDotTarget < 0) - distance = -distance; - - // Calculate the carrot pose - Pose carrotPose = Pose( - targetPose.x - distance * cos(targetPose.rotation) * LEAD_DISTANCE, - targetPose.y - distance * sin(targetPose.rotation) * LEAD_DISTANCE, - targetPose.rotation); - - // Set the target pose - AutoDriveToStep::targetPose = carrotPose; - - // Update the drive step - AutoDriveToStep::onUpdate(); - } - - void onStop() override - { - // Stop the drive step - AutoDriveToStep::onStop(); - } - - bool checkFinished() override - { - // Check against the target pose, not the carrot pose - AutoDriveToStep::targetPose = targetPose; - - return AutoDriveToStep::checkFinished(); - } - - protected: - static constexpr double LEAD_DISTANCE = 0.7; // % - - Pose targetPose; - OdomSource &odomSource; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoBranchStep.hpp b/include/devils/autoSteps/steps/autoBranchStep.hpp deleted file mode 100644 index cb35a7f..0000000 --- a/include/devils/autoSteps/steps/autoBranchStep.hpp +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -#include "../autoStep.hpp" - -namespace devils -{ - /** - * Base class to branch based on a condition. - * Extend this class to create a new branch step. - */ - class AutoBranchStep : public AutoStep - { - public: - /** - * Branches based on a condition. - * @param trueStep The step to execute if the condition is true. - * @param falseStep The step to execute if the condition is false. - */ - AutoBranchStep(AutoStep *trueStep, AutoStep *falseStep) - : trueStep(trueStep), - falseStep(falseStep) - { - } - - /** - * Gets the condition of the branch step. - * Called once at the start of the step. - * @return The condition of the branch step. - */ - virtual bool getCondition() = 0; - - /** - * Gets the active step based on the condition. - * @return The active step based on the condition. - */ - AutoStep *getActiveStep() - { - return condition ? trueStep : falseStep; - } - - protected: - void onStart() override - { - // Update the condition - condition = getCondition(); - - // Start the correct step - getActiveStep()->onStart(); - } - - void onUpdate() override - { - // Update the correct step - getActiveStep()->onUpdate(); - } - - void onStop() override - { - // Stop the correct step - getActiveStep()->onStop(); - } - - bool checkFinished() override - { - // Check if the correct step is finished - return getActiveStep()->checkFinished(); - } - - private: - bool condition = false; - AutoStep *trueStep; - AutoStep *falseStep; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoDriveStep.hpp b/include/devils/autoSteps/steps/autoDriveStep.hpp deleted file mode 100644 index 3016a99..0000000 --- a/include/devils/autoSteps/steps/autoDriveStep.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once -#include -#include "pros/rtos.hpp" -#include "../autoStep.hpp" -#include "../../utils/math.hpp" -#include "../../odom/odomSource.hpp" -#include "../../chassis/chassisBase.hpp" -#include "autoDriveToStep.hpp" - -namespace devils -{ - /** - * Drives the robot a specific distance w/ odometry feedback. - */ - class AutoDriveStep : public AutoDriveToStep - { - public: - /** - * Drives the robot a specific distance w/ odometry feedback. - * @param chassis The chassis to control. - * @param odomSource The odometry source to use. - * @param distance The distance to drive in inches. - * @param options The options for the drive step. - */ - AutoDriveStep( - ChassisBase &chassis, - OdomSource &odomSource, - double distance, - Options options = Options::defaultOptions) - : AutoDriveToStep(chassis, odomSource, Pose(0, 0, 0), options), - distance(distance) - { - } - - void onStart() override - { - // Calculate Target Pose - Pose startPose = odomSource.getPose(); - this->targetPose = Pose( - startPose.x + distance * std::cos(startPose.rotation), - startPose.y + distance * std::sin(startPose.rotation), - startPose.rotation); - - // Do base step - AutoDriveToStep::onStart(); - } - - protected: - // Drive Step Variables - double distance = 0; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoDriveTimeStep.hpp b/include/devils/autoSteps/steps/autoDriveTimeStep.hpp deleted file mode 100644 index 1062773..0000000 --- a/include/devils/autoSteps/steps/autoDriveTimeStep.hpp +++ /dev/null @@ -1,72 +0,0 @@ -#pragma once -#include -#include "pros/rtos.hpp" -#include "../autoStep.hpp" -#include "../../utils/math.hpp" -#include "../../utils/timer.hpp" -#include "../../chassis/chassisBase.hpp" - -namespace devils -{ - /** - * Drives the robot for a given duration in open loop. - */ - class AutoDriveTimeStep : public AutoStep - { - public: - /** - * Drives the robot for a given duration in open loop. - * @param chassis The chassis to control. - * @param duration Time to drive in milliseconds. - * @param forwardSpeed The forward speed of the robot from -1 to 1. - * @param turnSpeed The turn speed of the robot from -1 to 1. - * @param strafeSpeed The strafe speed of the robot from -1 to 1. - */ - AutoDriveTimeStep( - ChassisBase &chassis, - uint32_t duration, - double forwardSpeed, - double turnSpeed, - double strafeSpeed = 0) - : chassis(chassis), - timer(duration), - forwardSpeed(forwardSpeed), - turnSpeed(turnSpeed), - strafeSpeed(strafeSpeed) - { - } - - protected: - void onStart() override - { - // Start Timer - timer.start(); - } - - void onUpdate() override - { - // Set Speed - chassis.move(forwardSpeed, turnSpeed, strafeSpeed); - } - - void onStop() override - { - // Stop Chassis - chassis.stop(); - } - - bool checkFinished() override - { - return timer.finished(); - } - - // State - Timer timer; - - // Params - ChassisBase &chassis; - double forwardSpeed = 0; - double turnSpeed = 0; - double strafeSpeed = 0; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoJumpToStep.hpp b/include/devils/autoSteps/steps/autoJumpToStep.hpp deleted file mode 100644 index b2c33e8..0000000 --- a/include/devils/autoSteps/steps/autoJumpToStep.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" -#include "../autoStep.hpp" -#include "../../odom/odomSource.hpp" - -namespace devils -{ - /** - * Jumps the odometry state to a given pose. Usually ran at the start of an auto routine. - */ - class AutoJumpToStep : public AutoStep - { - public: - /** - * Jumps the odometry state to a given pose. Usually ran at the start of an auto routine. - * @param chassis The chassis to control. - * @param x The x position to jump to in inches. - * @param y The y position to jump to in inches. - * @param heading The heading to jump to in radians. - */ - AutoJumpToStep(OdomSource &odom, double x, double y, double heading) - : odom(odom), - targetPose(x, y, heading) - { - } - - /** - * Jumps the odometry state to a given pose. Usually ran at the start of an auto routine. - * @param chassis The chassis to control. - * @param pose The pose to jump to. - */ - AutoJumpToStep(OdomSource &odom, Pose pose) - : odom(odom), - targetPose(pose) - { - } - - void onStart() override - { - odom.setPose(targetPose); - } - - bool checkFinished() override - { - return true; - } - - protected: - // Params - OdomSource &odom; - Pose targetPose; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoPauseStep.hpp b/include/devils/autoSteps/steps/autoPauseStep.hpp deleted file mode 100644 index b703560..0000000 --- a/include/devils/autoSteps/steps/autoPauseStep.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" -#include "../autoStep.hpp" -#include "../../utils/timer.hpp" -#include "../../chassis/chassisBase.hpp" - -namespace devils -{ - /** - * Pauses the autonomous routine for a given duration. - */ - class AutoPauseStep : public AutoStep - { - public: - /** - * Pauses the autonomous routine for a given duration. - * @param duration The duration to pause in milliseconds. - */ - AutoPauseStep(uint32_t duration) - : timer(duration) - { - } - - void onStart() override - { - timer.start(); - } - - bool checkFinished() override - { - return timer.finished(); - } - - protected: - Timer timer; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoPurePursuitStep.hpp b/include/devils/autoSteps/steps/autoPurePursuitStep.hpp deleted file mode 100644 index b299c30..0000000 --- a/include/devils/autoSteps/steps/autoPurePursuitStep.hpp +++ /dev/null @@ -1,99 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" -#include "../autoStep.hpp" -#include "../../chassis/chassisBase.hpp" -#include "../../odom/odomSource.hpp" -#include "../../path/path.hpp" -#include "autoDriveToStep.hpp" - -namespace devils -{ - /** - * Represents a step to follow a path using basic pure pursuit. - */ - class AutoPurePursuitStep : public AutoDriveToStep - { - public: - /** - * Creates a new Pure Pursuit step. - * @param chassis The chassis to control. - * @param odomSource The odometry source to use. - * @param path The path to follow. - */ - AutoPurePursuitStep( - ChassisBase &chassis, - OdomSource &odomSource, - Path *path, - Options options = Options::defaultOptions) - : AutoDriveToStep(chassis, odomSource, Pose(0, 0, 0), options), - odomSource(odomSource), - path(path) - { - } - - void onStart() override - { - // Start the drive step - AutoDriveToStep::onStart(); - } - - void onUpdate() override - { - // Get the current pose - Pose currentPose = odomSource.getPose(); - - // Find the closest index on the path - double closestIndex = 0; - double closestDistance = std::numeric_limits::infinity(); - for (double i = 0; i < path->getLength(); i += DELTA_T) - { - // Get the distance to the path at i - Pose pathPose = path->getPoseAt(i); - double distance = currentPose.distanceTo(pathPose); - - // Check if this is the closest point - if (distance < closestDistance) - { - closestDistance = distance; - closestIndex = i; - } - } - - // Get the lookahead index - double lookaheadIndex = closestIndex; - while (lookaheadIndex < path->getLength() && - currentPose.distanceTo(path->getPoseAt(lookaheadIndex)) < LOOKAHEAD_DIST) - { - lookaheadIndex += DELTA_T; - } - - // Set the target pose - Pose lookaheadPose = path->getPoseAt(lookaheadIndex); - AutoDriveToStep::targetPose = lookaheadPose; - - // Update the drive step - AutoDriveToStep::onUpdate(); - } - - void onStop() override - { - // Stop the drive step - AutoDriveToStep::onStop(); - } - - bool checkFinished() override - { - // Check against the target pose, not the lookahead pose - AutoDriveToStep::targetPose = path->getPoseAt(path->getLength() - 1); - - return AutoDriveToStep::checkFinished(); - } - - protected: - static constexpr double DELTA_T = 0.01; // indices - static constexpr double LOOKAHEAD_DIST = 8.0; // inches - - OdomSource &odomSource; - Path *path; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoRotateStep.hpp b/include/devils/autoSteps/steps/autoRotateStep.hpp deleted file mode 100644 index db0d547..0000000 --- a/include/devils/autoSteps/steps/autoRotateStep.hpp +++ /dev/null @@ -1,48 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" -#include "autoRotateToStep.hpp" -#include "../autoStep.hpp" -#include "../../odom/odomSource.hpp" -#include "../../chassis/chassisBase.hpp" -#include "../../utils/math.hpp" - -namespace devils -{ - /** - * Rotates the robot a specific distance along its center of rotation. - */ - class AutoRotateStep : public AutoRotateToStep - { - public: - /** - * Rotates the robot a specific distance along its center of rotation. - * @param chassis The chassis to control. - * @param odomSource The odometry source to use. - * @param distance The distance to rotate in radians. - * @param options The options for the rotational step. - */ - AutoRotateStep( - ChassisBase &chassis, - OdomSource &odomSource, - double distance, - Options options = Options::defaultOptions) - : AutoRotateToStep(chassis, odomSource, distance, options), - distance(distance) - { - } - - void onStart() override - { - // Calculate Target Pose - Pose startPose = odomSource.getPose(); - this->targetAngle = distance + startPose.rotation; - - // Do base step - AutoRotateToStep::onStart(); - } - - protected: - // Drive Step Variables - double distance = 0; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoRotateToStep.hpp b/include/devils/autoSteps/steps/autoRotateToStep.hpp deleted file mode 100644 index 83c1e43..0000000 --- a/include/devils/autoSteps/steps/autoRotateToStep.hpp +++ /dev/null @@ -1,143 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" -#include "../autoStep.hpp" -#include "../../odom/odomSource.hpp" -#include "../../chassis/chassisBase.hpp" -#include "../../utils/math.hpp" -#include "../../controller/pidController.hpp" -#include "../../odom/poseVelocityCalculator.hpp" - -namespace devils -{ - /** - * Rotates the robot to a specific angle along its center of rotation. - */ - class AutoRotateToStep : public AutoStep - { - public: - struct Options - { - /// @brief The PID parameters to snap to an angle. Uses delta radians as the error. - PIDController::Options pidParams = {0.1, 0.0, 0.0}; - - /// @brief The minimum speed in % - double minSpeed = 0.1; - - /// @brief The maximum speed in % - double maxSpeed = 0.6; - - /// @brief The distance to the goal in radians - double goalDist = 0.015; - - /// @brief The maximum goal speed of the robot in rad/s. (Defaults to no limit) - double goalSpeed = std::numeric_limits::max(); - - /// @brief Setting this to false will rotate to the absolute angle instead of the minimum distance. - bool useMinimumDistance = true; - - /// @brief The default options for the rotational step. - static Options defaultOptions; - }; - - /** - * Rotates the robot to a specific angle along its center of rotation. - * @param chassis The chassis to control. - * @param odomSource The odometry source to use. - * @param targetAngle The angle to rotate to in radians. - * @param options The options for the rotational step. - */ - AutoRotateToStep( - ChassisBase &chassis, - OdomSource &odomSource, - double targetAngle, - Options options = Options::defaultOptions) - : chassis(chassis), - odomSource(odomSource), - targetAngle(targetAngle), - options(options), - rotationPID(options.pidParams) - { - } - - Options &getOptions() - { - return options; - } - - void onStart() override - { - // Reset Finished - this->isAtGoal = false; - - // Reset PID - rotationPID.reset(); - } - - void onUpdate() override - { - // Get Current Pose - Pose currentPose = odomSource.getPose(); - double currentVelocity = odomSource.getVelocity().rotation; - - // Calculate distance to start and target - double currentAngle = currentPose.rotation; - double distanceToTarget = angleDiff(targetAngle, currentAngle); - - // Check if we are at the goal - bool isAtGoalPose = fabs(distanceToTarget) < options.goalDist; - bool isAtGoalVelocity = fabs(currentVelocity) < options.goalSpeed; - isAtGoal = isAtGoalPose && isAtGoalVelocity; - - // Calculate Speed - double speed = rotationPID.update(distanceToTarget); - speed = std::clamp(speed, -options.maxSpeed, options.maxSpeed); // Clamp to max speed - speed = std::copysign(std::max(fabs(speed), options.minSpeed), speed); // Clamp to min speed - - // Move Chassis - chassis.move(0, speed); - } - - void onStop() override - { - // Stop Chassis - chassis.stop(); - } - - bool checkFinished() override - { - return isAtGoal; - } - - protected: - // State - bool isAtGoal = false; - - // Params - ChassisBase &chassis; - OdomSource &odomSource; - PIDController rotationPID; - double targetAngle = 0; - Options options; - - private: - static constexpr double POST_DRIVE_DELAY = 50; // ms - - /** - * Gets the angle difference between two angles. - * Uses the minimum distance if the option is enabled. - * Otherwise, returns the difference. - * @param a The first angle. - * @param b The second angle. - * @return The difference between the two angles. - */ - double angleDiff(double a, double b) - { - if (options.useMinimumDistance) - return Math::angleDiff(a, b); - return a - b; - } - }; - - // Initialize Default Options - AutoRotateToStep::Options AutoRotateToStep::Options::defaultOptions = AutoRotateToStep::Options(); -} diff --git a/include/devils/autoSteps/steps/autoRotateTowardStep.hpp b/include/devils/autoSteps/steps/autoRotateTowardStep.hpp deleted file mode 100644 index a446871..0000000 --- a/include/devils/autoSteps/steps/autoRotateTowardStep.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" -#include "autoRotateToStep.hpp" -#include "../autoStep.hpp" -#include "../../odom/odomSource.hpp" -#include "../../chassis/chassisBase.hpp" -#include "../../utils/math.hpp" - -namespace devils -{ - /** - * Rotates the robot toward a specific pose along its center of rotation. - */ - class AutoRotateTowardStep : public AutoRotateToStep - { - public: - /** - * Rotates the robot toward a specific pose along its center of rotation. - * @param chassis The chassis to control. - * @param odomSource The odometry source to use. - * @param targetPose The target pose to rotate toward. - * @param options The options for the rotational step. - */ - AutoRotateTowardStep( - ChassisBase &chassis, - OdomSource &odomSource, - Pose targetPose, - Options options = Options::defaultOptions) - : AutoRotateToStep(chassis, odomSource, 0, options), - targetPose(targetPose) - { - } - - void onStart() override - { - // Calculate Target Pose - Pose startPose = odomSource.getPose(); - this->targetAngle = std::atan2( - targetPose.y - startPose.y, - targetPose.x - startPose.x); - - // Do base step - AutoRotateToStep::onStart(); - } - - protected: - Pose targetPose; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoTimeoutStep.hpp b/include/devils/autoSteps/steps/autoTimeoutStep.hpp deleted file mode 100644 index 6dc4259..0000000 --- a/include/devils/autoSteps/steps/autoTimeoutStep.hpp +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once -#include -#include "pros/rtos.hpp" -#include "../autoStep.hpp" -#include "../../utils/timer.hpp" - -namespace devils -{ - /** - * Represents an autostep that aborts after a given duration. - */ - class AutoTimeoutStep : public AutoStep - { - public: - /** - * Creates a new timeout step. - * @param autoStep The step to execute. - * @param duration The duration of the step in milliseconds. - */ - AutoTimeoutStep( - AutoStepPtr autoStep, - uint32_t duration) - : autoStep(std::move(autoStep)), - timer(duration) - { - } - - void onStart() override - { - // Start Timer - timer.start(); - - // Start Auto Step - if (autoStep) - autoStep->onStart(); - } - - void onUpdate() override - { - // Update Auto Step - if (autoStep) - autoStep->onUpdate(); - } - - void onStop() override - { - // Stop Timer - timer.stop(); - - // Stop Auto Step - if (autoStep) - autoStep->onStop(); - } - - bool checkFinished() override - { - // Check Timer - if (timer.finished()) - return true; - - // Check Auto Step - if (autoStep) - return autoStep->checkFinished(); - - // Otherwise, return true - return true; - } - - protected: - // Params - AutoStepPtr autoStep; - Timer timer; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/steps/autoDriveToStep.hpp b/include/devils/autoSteps/steps/drivePIDStep.hpp similarity index 66% rename from include/devils/autoSteps/steps/autoDriveToStep.hpp rename to include/devils/autoSteps/steps/drivePIDStep.hpp index ea39292..3adc137 100644 --- a/include/devils/autoSteps/steps/autoDriveToStep.hpp +++ b/include/devils/autoSteps/steps/drivePIDStep.hpp @@ -1,12 +1,10 @@ #pragma once -#include -#include "pros/rtos.hpp" + #include "../autoStep.hpp" -#include "../../utils/math.hpp" +#include "../../geometry/math.hpp" #include "../../odom/odomSource.hpp" #include "../../chassis/chassisBase.hpp" #include "../../controller/pidController.hpp" -#include "../../utils/timer.hpp" namespace devils { @@ -14,7 +12,7 @@ namespace devils * Drives the robot linearly to a specific pose. * Rotates the robot to face the target pose, disregarding target rotation. */ - class AutoDriveToStep : public AutoStep + class DrivePIDStep : public AutoStep { public: struct Options @@ -26,19 +24,19 @@ namespace devils PIDController::Options rotationPID = {0.05, 0.0, 0.0}; /// @brief THe minimum speed in % - double minSpeed = 0.0; + float minSpeed = 0.0; /// @brief The maximum speed in % - double maxSpeed = 0.5; + float maxSpeed = 0.5; /// @brief The maximum final distance to the target in inches - double goalDist = 6.0; + float goalDist = 6.0; - /// @brief The maximum final speed of the robot in in/s. (Defaults to no limit) - double goalSpeed = std::numeric_limits::max(); + /// @brief The maximum final speed of the robot in inches/s. (Defaults to no limit) + float goalSpeed = std::numeric_limits::max(); /// @brief The minimum distance from the target to apply rotation. If we are closer than this, we will not rotate to avoid oscillation. - double minDistanceToRotate = 1.0; + float minDistanceToRotate = 1.0; /// @brief The default options for the drive step. static Options defaultOptions; @@ -51,20 +49,21 @@ namespace devils * @param targetPose The target pose to drive to. * @param options The options for the drive step. */ - AutoDriveToStep( - ChassisBase &chassis, - OdomSource &odomSource, - Pose targetPose, - Options options = Options::defaultOptions) + DrivePIDStep( + ChassisBase& chassis, + OdomSource& odomSource, + const Pose& targetPose, + const Options& options = Options::defaultOptions) : chassis(chassis), odomSource(odomSource), - targetPose(targetPose), - options(options), rotationPID(options.rotationPID), - translationPID(options.translationPID) + translationPID(options.translationPID), + targetPose(targetPose), + options(options) { } + protected: void onStart() override { // Reset PID Controllers @@ -74,46 +73,46 @@ namespace devils void onUpdate() override { // Get Current State - Pose currentPose = odomSource.getPose(); - Vector2 currentVelocity = odomSource.getVelocity(); + const Pose currentPose = odomSource.getPose(); // Calculate distance to start and target - double distanceToTarget = currentPose.distanceTo(targetPose); + float distanceToTarget = currentPose.distanceTo(targetPose); // Calculate target angle - double targetAngleRads = std::atan2( + float targetAngleRads = std::atan2( targetPose.y - currentPose.y, targetPose.x - currentPose.x); // Calculate Dot Product - double currentDotTarget = cos(currentPose.rotation) * - (targetPose.x - currentPose.x) + - sin(currentPose.rotation) * - (targetPose.y - currentPose.y); + const float currentDotTarget = + cosf(currentPose.rotation) * + (targetPose.x - currentPose.x) + + sinf(currentPose.rotation) * + (targetPose.y - currentPose.y); // Drive in reverse if the goal is behind us if (currentDotTarget < 0) { distanceToTarget = -distanceToTarget; - targetAngleRads += M_PI; + targetAngleRads += M_PIF; } // Calculate Forward Speed - double speed = getSpeed(distanceToTarget); + const auto speed = getSpeed(distanceToTarget); // Calculate Turn Speed - double turnSpeed = 0; + float turnSpeed = 0; if (std::fabs(distanceToTarget) > options.minDistanceToRotate) { // Difference in angle - double angleDiff = Math::angleDiff(targetAngleRads, currentPose.rotation); + const float angleDiff = Units::diffRad(targetAngleRads, currentPose.rotation); turnSpeed = rotationPID.update(angleDiff); turnSpeed = std::clamp(turnSpeed, -options.maxSpeed, options.maxSpeed); } // Move Chassis - chassis.move(speed, turnSpeed); + chassis.move(speed, turnSpeed, 0.0f); } void onStop() override @@ -125,25 +124,24 @@ namespace devils bool checkFinished() override { // Get Current State - Pose currentPose = odomSource.getPose(); + const Pose currentPose = odomSource.getPose(); // Calculate distance to target pose - double distanceToTarget = currentPose.distanceTo(targetPose); + const float distanceToTarget = currentPose.distanceTo(targetPose); // Check if we reached the goal - return fabs(distanceToTarget) < options.goalDist; + return fabsf(distanceToTarget) < options.goalDist; } - protected: /** * Gets the speed at a given distance to the target. * @param distanceToTarget The distance to the target in inches * @returns The target speed in inches per second */ - virtual double getSpeed(double distanceToTarget) + virtual float getSpeed(const float distanceToTarget) { // Calculate output speed - double outputSpeed = translationPID.update(distanceToTarget); + float outputSpeed = translationPID.update(distanceToTarget); // Apply max speed outputSpeed = std::clamp(outputSpeed, -options.maxSpeed, options.maxSpeed); @@ -156,8 +154,8 @@ namespace devils } // Params - ChassisBase &chassis; - OdomSource &odomSource; + ChassisBase& chassis; + OdomSource& odomSource; PIDController rotationPID; PIDController translationPID; Pose targetPose = Pose(); @@ -165,5 +163,5 @@ namespace devils }; // Define the default options - AutoDriveToStep::Options AutoDriveToStep::Options::defaultOptions = AutoDriveToStep::Options(); -} \ No newline at end of file + DrivePIDStep::Options DrivePIDStep::Options::defaultOptions = DrivePIDStep::Options(); +} diff --git a/include/devils/autoSteps/steps/autoRamseteStep.hpp b/include/devils/autoSteps/steps/driveRAMSETEStep.hpp similarity index 52% rename from include/devils/autoSteps/steps/autoRamseteStep.hpp rename to include/devils/autoSteps/steps/driveRAMSETEStep.hpp index 72a6e9f..6ef18dd 100644 --- a/include/devils/autoSteps/steps/autoRamseteStep.hpp +++ b/include/devils/autoSteps/steps/driveRAMSETEStep.hpp @@ -1,8 +1,10 @@ #pragma once -#include + +#include #include "pros/rtos.hpp" #include "../autoStep.hpp" -#include "../../utils/math.hpp" +#include "../../geometry/units.hpp" +#include "../../geometry/math.hpp" #include "../../odom/odomSource.hpp" #include "../../chassis/chassisBase.hpp" #include "../../trajectory/trajectory.hpp" @@ -10,34 +12,28 @@ namespace devils { /** - * Drives the robot along a trajectory using ramsete control. + * Drives the robot along a trajectory using RAMSETE control. * See: https://file.tavsys.net/control/controls-engineering-in-frc.pdf */ - class AutoRamseteStep : public AutoStep + class DriveRAMSETEStep : public AutoStep { public: struct Options { - /// @brief The minimum speed in % - double minSpeed = 0.0; - - /// @brief The maximum speed in % - double maxSpeed = 1.0; - /// @brief A proportional constant. Must be greater than 0. Larger values will result in more aggressive control. - double proportionGain = 0.01; + float proportionGain = 0.005; /// @brief A damping coefficient. Must be between 0 and 1. Larger values will result in increased damping. - double dampingCoefficient = 0.75; + float dampingCoefficient = 0.75; /// @brief Proportion (P in PID) between translational velocity and motor voltage - double translationP = 0.012; + float translationP = 0.015; /// @brief Proportion (P in PID) between rotational velocity and motor voltage - double rotationP = 0.1; + float rotationP = 0.1; /// @brief Time in seconds of odometry latency. This is the time it takes for the odometry to update after the robot moves. - double sensorLatency = 0.2; + float sensorLatency = 0.1; /// @brief The default options for the drive step. static Options defaultOptions; @@ -50,107 +46,96 @@ namespace devils * @param trajectory The trajectory to follow. * @param options The options for the drive step. */ - AutoRamseteStep( - ChassisBase &chassis, - OdomSource &odomSource, - std::shared_ptr trajectory, - Options options = Options::defaultOptions) - : trajectory(trajectory), - chassis(chassis), + DriveRAMSETEStep( + ChassisBase& chassis, + OdomSource& odomSource, + const std::shared_ptr& trajectory, + const Options& options = Options::defaultOptions) + : chassis(chassis), odomSource(odomSource), + trajectory(trajectory), + internalTimer(trajectory->duration()), options(options) { } + protected: void onStart() override { - // Save the start time - startTime = pros::millis(); + internalTimer.start(); } void onUpdate() override { // Get the current time - double t = pros::millis() - startTime; - t /= 1000.0; // Convert to seconds + const auto t = internalTimer.getElapsedTime(); // Get current setpoint - auto setpoint = trajectory->getStateAt(t); - auto feedbackSetpoint = trajectory->getStateAt(t - options.sensorLatency); + const auto setpoint = trajectory->getStateAt(t); + const auto feedbackSetpoint = trajectory->getStateAt(t - options.sensorLatency); // Get current position - auto currentPosition = odomSource.getPose(); + const auto currentPosition = odomSource.getPose(); // Calculate error to setpoint - auto error = feedbackSetpoint.pose - currentPosition; + const auto error = feedbackSetpoint.pose - currentPosition; // Calculate local error // This is the error relative to the robot's current rotation // such that localError.x is aligned with the front of the robot and // localError.y is aligned with the side of the robot. - auto localError = Pose( - error.x * cos(currentPosition.rotation) + error.y * sin(currentPosition.rotation), - -error.x * sin(currentPosition.rotation) + error.y * cos(currentPosition.rotation), + const auto localError = Pose( + error.x * cosf(currentPosition.rotation) + error.y * sinf(currentPosition.rotation), + -error.x * sinf(currentPosition.rotation) + error.y * cosf(currentPosition.rotation), Units::diffRad(feedbackSetpoint.pose.rotation, currentPosition.rotation)); // Calculate controller gain // k = 2 * zeta * sqrt(w^2 + b * v^2) - double controllerGain = std::pow(setpoint.angularVelocity, 2) + - options.proportionGain * std::pow(setpoint.velocity, 2); + float controllerGain = setpoint.angularVelocity * setpoint.angularVelocity + + options.proportionGain * setpoint.velocity * setpoint.velocity; controllerGain = 2 * options.dampingCoefficient * std::sqrt(controllerGain); // Calculate sinc (sin(x) / x) - double sinc = sin(localError.rotation) / localError.rotation; + float sinc = sinf(localError.rotation) / localError.rotation; if (std::isnan(sinc)) sinc = 1.0; // Handle the case where localError.rotation is 0 // Calculate translation output // v = v_d * cos(e_r) + k * e_x - double translationOutput = setpoint.velocity * cos(localError.rotation) + - controllerGain * localError.x; + float translationOutput = setpoint.velocity * cosf(localError.rotation) + + controllerGain * localError.x; // Calculate rotation output // w = w_d + k * e_r + b * v * sinc(e_r) * e_y - double rotationOutput = setpoint.angularVelocity + - controllerGain * localError.rotation + - options.proportionGain * setpoint.velocity * sinc * localError.y; + float rotationOutput = setpoint.angularVelocity + + controllerGain * localError.rotation + + options.proportionGain * setpoint.velocity * sinc * localError.y; // Multiply by translation and rotation P values translationOutput *= options.translationP; rotationOutput *= options.rotationP; - // Clamp outputs - translationOutput = Math::deadbandClamp(translationOutput, options.minSpeed, options.maxSpeed); - rotationOutput = std::clamp(rotationOutput, -options.maxSpeed, options.maxSpeed); - // Set the chassis output - chassis.move(translationOutput, rotationOutput); + chassis.move(translationOutput, rotationOutput, 0.0f); } void onStop() override { - // Stop the chassis chassis.stop(); } bool checkFinished() override { - // Get the current time - auto t = pros::millis() - startTime; - - // Check if the trajectory is finished - return trajectory->duration() * 1000 <= t; + return internalTimer.getIsFinished(); } - protected: - ChassisBase &chassis; - OdomSource &odomSource; + ChassisBase& chassis; + OdomSource& odomSource; std::shared_ptr trajectory; + Timer internalTimer; Options options; - - uint32_t startTime = 0; }; } // Define the default options -devils::AutoRamseteStep::Options devils::AutoRamseteStep::Options::defaultOptions = devils::AutoRamseteStep::Options(); \ No newline at end of file +devils::DriveRAMSETEStep::Options devils::DriveRAMSETEStep::Options::defaultOptions = Options(); diff --git a/include/devils/autoSteps/steps/rotateMotionProfileStep.hpp b/include/devils/autoSteps/steps/rotateMotionProfileStep.hpp new file mode 100644 index 0000000..ff3a63b --- /dev/null +++ b/include/devils/autoSteps/steps/rotateMotionProfileStep.hpp @@ -0,0 +1,144 @@ +#pragma once + +#include "../autoStep.hpp" +#include "../../odom/odomSource.hpp" +#include "../../chassis/chassisBase.hpp" +#include "../../geometry/math.hpp" +#include "../../controller/pidController.hpp" +#include "devils/controller/profiledPIDController.hpp" + +namespace devils +{ + /** + * Rotates the robot to a specific angle along its center of rotation. + */ + class RotateMotionProfileStep : public AutoStep + { + public: + struct Options + { + /// @brief The PID parameters to snap to an angle. Used to correct drift from the motion profile. + PIDController::Options pidParams = {0.8, 0.0, 0.0}; + + /// @brief The constraints for the motion profile. + TrapezoidMotionProfile::Constraints motionProfileConstraints = {16.0f, 8.0f, 16.0f}; + + /// @brief The feedforward option to apply based on the motion profile. + MotorFeedforward::Options feedforwardOptions = {0.0f, 0.09f, 0.005f}; + + /// @brief The amount of time to add to the remaining time of the motion profile to account for feedback latency + float feedbackLatency = 0.07f; // 70 ms + + /// @brief The starting rotational velocity for the motion profile + float startingVelocity = 0.0f; + + /// @brief The ending rotational velocity for the motion profile + float endingVelocity = 0.0f; + + /** + * If true, the robot will try to rotate to the angle using the fastest path possible. + * For example if the robot is at 10 degrees and the target angle is 350 degrees, the robot will rotate -20 degrees instead of +340 degrees. + * + * If false, the robot will try to match the angle exactly, even if it means taking a longer path. + * For example if the robot is at 10 degrees and the target angle is 350 degrees, the robot will rotate +340 degrees instead of -20 degrees. + */ + bool useMinimumDistance = true; + + /// @brief The default options for the rotational step. + static Options defaultOptions; + }; + + /** + * Rotates the robot to a specific angle along its center of rotation. + * @param chassis The chassis to control. + * @param odomSource The odometry source to use. + * @param targetAngle The angle to rotate to in radians. + * @param options The options for the rotational step. + */ + RotateMotionProfileStep( + ChassisBase& chassis, + OdomSource& odomSource, + const float targetAngle, + const Options& options = Options::defaultOptions) + : chassis(chassis), + odomSource(odomSource), + targetAngle(targetAngle), + pidController(options.pidParams, + options.motionProfileConstraints, + 0), + feedforwardController(options.feedforwardOptions), + options(options) + { + // Set the goal distance for the motion profile based on the target angle and the current angle from odometry. + pidController.setGoal( + getGoalDistance(), + options.startingVelocity, + options.endingVelocity); + + // Set the feedback delay for the PID controller to account for any latency in the odometry or the control loop. + pidController.setFeedbackDelay(options.feedbackLatency); + } + + Options& getOptions() + { + return options; + } + + protected: + void onStart() override + { + pidController.reset(); + startingRotation = odomSource.getPose().rotation; + } + + void onUpdate() override + { + // Get profiled PID output + const auto currentAngle = odomSource.getPose().rotation; + const auto currentAngleRelative = Units::diffRad(currentAngle, startingRotation); + const auto pidOutput = pidController.update(currentAngleRelative); + + // Get feedforward output + const auto setpoint = pidController.getSetpoint(); + const auto feedforwardOutput = feedforwardController.update(setpoint.velocity, setpoint.acceleration); + + // Combine the outputs and apply to the chassis + chassis.move(0.0f, feedforwardOutput + pidOutput, 0.0f); + } + + void onStop() override + { + chassis.stop(); + } + + bool checkFinished() override + { + return pidController.getTimeRemaining() <= 0; + } + + ChassisBase& chassis; + OdomSource& odomSource; + float targetAngle = 0; + float startingRotation = 0; + ProfiledPIDController pidController; + MotorFeedforward feedforwardController; + Options options; + + private: + /** + * Gets the remaining angle to the target angle based on the current angle from odometry and the target angle. + * @return the remaining angle to the target angle in radians. + */ + float getGoalDistance() const + { + const auto currentAngle = odomSource.getPose().rotation; + return options.useMinimumDistance ? + Units::diffRad(targetAngle, currentAngle) : + targetAngle - currentAngle; + } + + }; + + // Initialize Default Options + RotateMotionProfileStep::Options RotateMotionProfileStep::Options::defaultOptions = Options(); +} diff --git a/include/devils/autoSteps/transformer/mirrorTransform.hpp b/include/devils/autoSteps/transformer/mirrorTransform.hpp deleted file mode 100644 index f290480..0000000 --- a/include/devils/autoSteps/transformer/mirrorTransform.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include "poseTransform.h" - -namespace devils -{ - /// @brief Transforms the robot's pose by mirroring it over the x-axis - class MirrorTransformX : public PoseTransform - { - Pose transform(Pose pose) override - { - return Pose( - -pose.x, - pose.y, - M_PI - pose.rotation); - } - }; - - /// @brief Transforms the robot's pose by mirroring it over the y-axis - class MirrorTransformY : public PoseTransform - { - Pose transform(Pose pose) override - { - return Pose( - pose.x, - -pose.y, - -pose.rotation); - } - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/transformer/poseTransform.h b/include/devils/autoSteps/transformer/poseTransform.h deleted file mode 100644 index 82a93a0..0000000 --- a/include/devils/autoSteps/transformer/poseTransform.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#include "../../geometry/pose.hpp" - -namespace devils -{ - /** - * Base class for transforming poses. - * Used to reuse autonomous steps across different alliance orientations. - */ - struct PoseTransform - { - /** - * Transforms the pose to another location - * @param pose - Input Pose - * @returns Output pose - */ - virtual Pose transform(Pose pose) = 0; - }; -} \ No newline at end of file diff --git a/include/devils/autoSteps/transformer/poseTransformer.hpp b/include/devils/autoSteps/transformer/poseTransformer.hpp new file mode 100644 index 0000000..25ed10f --- /dev/null +++ b/include/devils/autoSteps/transformer/poseTransformer.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include "../../geometry/pose.hpp" + +namespace devils +{ + /** + * Base class for transforming poses. + * Used to reuse autonomous steps across different alliance orientations. + */ + struct PoseTransformer + { + virtual ~PoseTransformer() = default; + /** + * Transforms the pose to another location + * @param pose - Input Pose + * @returns Output pose + */ + virtual Pose transform(Pose pose) = 0; + }; + + /// @brief Transforms the robot's pose by mirroring it over the y-axis + class MirrorTransformX : public PoseTransformer + { + Pose transform(const Pose pose) override + { + return Pose( + -pose.x, + pose.y, + M_PI - pose.rotation); + } + }; + + /// @brief Transforms the robot's pose by mirroring it over the x-axis + class MirrorTransformY : public PoseTransformer + { + Pose transform(const Pose pose) override + { + return Pose( + pose.x, + -pose.y, + -pose.rotation); + } + }; +} diff --git a/include/devils/autoSteps/transformer/rotateTransform.hpp b/include/devils/autoSteps/transformer/rotateTransform.hpp deleted file mode 100644 index 5e15a0e..0000000 --- a/include/devils/autoSteps/transformer/rotateTransform.hpp +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - -#include "poseTransform.h" - -namespace devils -{ - /// @brief Transforms the robot's pose by rotating it 180 degrees - class RotateTransform : public PoseTransform - { - Pose transform(Pose pose) override - { - return Pose( - -pose.x, - -pose.y, - M_PI + pose.rotation); - } - }; -} \ No newline at end of file diff --git a/include/devils/chassis/chassisBase.hpp b/include/devils/chassis/chassisBase.hpp index 498b11b..dcfd1a4 100644 --- a/include/devils/chassis/chassisBase.hpp +++ b/include/devils/chassis/chassisBase.hpp @@ -1,5 +1,4 @@ #pragma once -#include namespace devils { @@ -8,20 +7,31 @@ namespace devils */ struct ChassisBase { + virtual ~ChassisBase() = default; + /** * Moves the robot in a direction using voltage. * @param forward The forward speed of the robot from -1 to 1. * @param turn The turn speed of the robot from -1 to 1. * @param strafe The strafe speed of the robot from -1 to 1. */ - virtual void move(double forward, double turn, double strafe = 0) = 0; + virtual void move(float forward, float turn, float strafe) = 0; /** - * Stops the robot. + * Stops the robot. */ virtual void stop() { move(0, 0, 0); }; + + /** + * Checks if the chassis is holonomic, meaning it can strafe in any direction. + * @return True if the chassis is holonomic, false otherwise. + */ + virtual bool isHolonomic() const + { + return false; + } }; -} \ No newline at end of file +} diff --git a/include/devils/chassis/dummyChassis.hpp b/include/devils/chassis/dummyChassis.hpp index 4b39574..79df1d5 100644 --- a/include/devils/chassis/dummyChassis.hpp +++ b/include/devils/chassis/dummyChassis.hpp @@ -1,12 +1,10 @@ #pragma once + #include "chassisBase.hpp" -#include "../hardware/smartMotorGroup.hpp" -#include -#include -#include "../utils/logger.hpp" +#include "../utils/backgroundService.hpp" #include "../odom/odomSource.hpp" #include "../odom/poseVelocityCalculator.hpp" -#include "../utils/runnable.hpp" +#include "../geometry/math.hpp" namespace devils { @@ -15,73 +13,113 @@ namespace devils * This is useful for testing autonomous routines without a physical robot. * Can be used as an OdomSource. */ - class DummyChassis : public ChassisBase, public OdomSource, public Runnable, public PoseVelocityCalculator + class DummyChassis : + public BackgroundService, + public ChassisBase, + public OdomSource, + public PoseVelocityCalculator { public: - DummyChassis() + /** + * Moves the robot in a direction using voltage. + * @param forward The forward speed of the robot from -1 to 1. + * @param turn The turn speed of the robot from -1 to 1. + * @param strafe The strafe speed of the robot from -1 to 1. + */ + void move(float forward, float turn, float strafe) override { - // Run on startup - runAsync(); + forward = std::clamp(forward, -1.0f, 1.0f); + turn = std::clamp(turn, -1.0f, 1.0f); + strafe = std::clamp(strafe, -1.0f, 1.0f); + + lastForward = forward; + lastTurn = turn; + lastStrafe = strafe; } - void stop() override + /** + * Jumps the robot to a specific pose. + * @param pose The pose to set the robot to. + */ + void setPose(const Pose pose) override { - ChassisBase::stop(); + currentPose = pose; } - void move(double forward, double turn, double strafe = 0) override + /** + * Gets the current pose of the robot. + * @return The current pose of the robot. + */ + Pose getPose() override { - forward = std::clamp(forward, -1.0, 1.0); - turn = std::clamp(turn, -1.0, 1.0); - strafe = std::clamp(strafe, -1.0, 1.0); + return currentPose; + } - lastForward = forward; - lastTurn = turn; - lastStrafe = strafe; + /** + * Gets the current velocity of the robot. + * @return Current velocity of the robot. + */ + PoseVelocity getVelocity() override + { + return PoseVelocityCalculator::getVelocity(); + } + + /** + * Checks if the chassis is holonomic, meaning it can strafe in any direction. + * @return True if the chassis is holonomic, false otherwise. + */ + bool isHolonomic() const override + { + return holonomicEnabled; + } + + /** + * Sets whether the chassis is holonomic, meaning it can strafe in any direction. + * @param holonomic True if the chassis should be holonomic, false otherwise. + */ + void setHolonomic(const bool holonomic) + { + holonomicEnabled = holonomic; } + protected: void onUpdate() override { // TODO: Multiply acceleration by delta time + // Get Strafe Input + const float strafeInput = holonomicEnabled ? lastStrafe : 0; + // Calculate Acceleration - currentAcceleration.x += (cos(currentPose.rotation) * lastForward + sin(currentPose.rotation) * lastStrafe) * TRANSLATION_ACCEL; - currentAcceleration.y += (sin(currentPose.rotation) * lastForward + cos(currentPose.rotation) * lastStrafe) * TRANSLATION_ACCEL; + currentAcceleration.x += TRANSLATION_ACCEL * ( + cosf(currentPose.rotation) * lastForward + + sinf(currentPose.rotation) * strafeInput); + + currentAcceleration.y += TRANSLATION_ACCEL * ( + sinf(currentPose.rotation) * lastForward + + cosf(currentPose.rotation) * strafeInput); + currentAcceleration.rotation += lastTurn * ROTATION_ACCEL; currentAcceleration = currentAcceleration * (1 - DRAG); // Update Pose currentPose = currentPose + currentAcceleration; - currentPose.rotation = std::fmod(currentPose.rotation, 2 * M_PI); + currentPose.rotation = fmodf(currentPose.rotation, 2 * M_PIF); // Update Velocity updateVelocity(currentPose); } - void setPose(Pose pose) override - { - currentPose = pose; - } - - Pose getPose() override - { - return currentPose; - } - - PoseVelocity getVelocity() override - { - return PoseVelocityCalculator::getVelocity(); - } - private: - static constexpr double TRANSLATION_ACCEL = 0.8; // in/s^2 - static constexpr double ROTATION_ACCEL = 0.1; // rad/s^2 - static constexpr double DRAG = 0.35; // % + static constexpr float TRANSLATION_ACCEL = 0.8; // in/s^2 + static constexpr float ROTATION_ACCEL = 0.1; // rad/s^2 + static constexpr float DRAG = 0.35; // % - double lastForward = 0; - double lastTurn = 0; - double lastStrafe = 0; + float lastForward = 0; + float lastTurn = 0; + float lastStrafe = 0; Pose currentAcceleration = Pose(); Pose currentPose = Pose(); + bool holonomicEnabled = true; }; -} \ No newline at end of file +} diff --git a/include/devils/chassis/hDriveChassis.hpp b/include/devils/chassis/hDriveChassis.hpp new file mode 100644 index 0000000..79ebf25 --- /dev/null +++ b/include/devils/chassis/hDriveChassis.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include "chassisBase.hpp" +#include "../hardware/smartMotorGroup.hpp" + +namespace devils +{ + /** + * Represents a chassis driven by the differential of two sets of wheels along with a horizontal set of wheels for strafing. + * This chassis is holonomic, allowing for omnidirectional movement and strafing. + */ + class HDriveChassis : public ChassisBase + { + public: + /** + * Creates a new tank chassis. + * @param leftMotors The motor group for the left side of the chassis. + * @param rightMotors The motor group for the right side of the chassis. + * @param horizontalMotors The motor group for the horizontal wheels of the chassis. + */ + HDriveChassis( + SmartMotorGroup& leftMotors, + SmartMotorGroup& rightMotors, + SmartMotorGroup& horizontalMotors) + : leftMotors(leftMotors), + rightMotors(rightMotors), + horizontalMotors(horizontalMotors) + { + // Disable brake mode by default to prevent overheating + leftMotors.setBrakeMode(false); + rightMotors.setBrakeMode(false); + horizontalMotors.setBrakeMode(false); + } + + /** + * Runs the chassis in voltage mode. + * @param forward The forward speed of the robot from -1 to 1. + * @param turn The turn speed of the robot from -1 to 1. + * @param strafe The strafe speed of the robot from -1 to 1. (Ignored for tank chassis) + */ + void move( + const float forward, + const float turn, + const float strafe) override + { + moveTank( + forward + turn, + forward - turn, + strafe); + } + + /** + * Runs the chassis in voltage mode with individual control of the left and right sides. + * @param leftSpeed The speed to run the left side of the chassis from -1 to 1. + * @param rightSpeed The speed to run the right side of the chassis from -1 to 1. + * @param horizontalSpeed The speed to run the horizontal wheels of the chassis from -1 to 1. + */ + void moveTank( + const float leftSpeed, + const float rightSpeed, + const float horizontalSpeed = 0) const + { + leftMotors.move(leftSpeed); + rightMotors.move(rightSpeed); + horizontalMotors.move(horizontalSpeed); + } + + /** + * Forces the chassis to stop. + */ + void stop() override + { + leftMotors.stop(); + rightMotors.stop(); + } + + /** + * Checks if the chassis is holonomic, meaning it can strafe in any direction. + * @return True, since an H-Drive chassis can strafe. + */ + bool isHolonomic() const override + { + return true; + } + + protected: + SmartMotorGroup& leftMotors; + SmartMotorGroup& rightMotors; + SmartMotorGroup& horizontalMotors; + }; +} diff --git a/include/devils/chassis/mecanumChassis.hpp b/include/devils/chassis/mecanumChassis.hpp new file mode 100644 index 0000000..a2b023a --- /dev/null +++ b/include/devils/chassis/mecanumChassis.hpp @@ -0,0 +1,86 @@ +#pragma once + +#include "chassisBase.hpp" +#include "../hardware/smartMotorGroup.hpp" + +namespace devils +{ + /** + * Represents a chassis driven by a set of 4 mecanum wheels, allowing for omnidirectional movement. + */ + class MecanumChassis : public ChassisBase + { + public: + /** + * Creates a new tank chassis. + * @param frontLeftMotors The motor group for the front left wheel of the chassis. + * @param frontRightMotors The motor group for the front right wheel of the chassis + * @param backLeftMotors The motor group for the back left wheel of the chassis + * @param backRightMotors The motor group for the back right wheel of the chassis + */ + MecanumChassis( + SmartMotorGroup& frontLeftMotors, + SmartMotorGroup& frontRightMotors, + SmartMotorGroup& backLeftMotors, + SmartMotorGroup& backRightMotors) + : frontLeftMotors(frontLeftMotors), + frontRightMotors(frontRightMotors), + backLeftMotors(backLeftMotors), + backRightMotors(backRightMotors) + { + // Disable brake mode by default to prevent overheating + frontLeftMotors.setBrakeMode(false); + frontRightMotors.setBrakeMode(false); + backLeftMotors.setBrakeMode(false); + backRightMotors.setBrakeMode(false); + } + + /** + * Runs the chassis in voltage mode. + * @param forward The forward speed of the robot from -1 to 1. + * @param turn The turn speed of the robot from -1 to 1. + * @param strafe The strafe speed of the robot from -1 to 1. + */ + void move( + const float forward, + const float turn, + const float strafe) override + { + const float frontLeftSpeed = forward + turn + strafe; + const float frontRightSpeed = forward - turn - strafe; + const float backLeftSpeed = forward + turn - strafe; + const float backRightSpeed = forward - turn + strafe; + + frontLeftMotors.move(frontLeftSpeed); + frontRightMotors.move(frontRightSpeed); + backLeftMotors.move(backLeftSpeed); + backRightMotors.move(backRightSpeed); + } + + /** + * Forces the chassis to stop. + */ + void stop() override + { + frontLeftMotors.stop(); + frontRightMotors.stop(); + backLeftMotors.stop(); + backRightMotors.stop(); + } + + /** + * Checks if the chassis is holonomic, meaning it can strafe in any direction. + * @return True, since a mecanum chassis can strafe. + */ + bool isHolonomic() const override + { + return true; + } + + protected: + SmartMotorGroup& frontLeftMotors; + SmartMotorGroup& frontRightMotors; + SmartMotorGroup& backLeftMotors; + SmartMotorGroup& backRightMotors; + }; +} diff --git a/include/devils/chassis/swerveChassis.hpp b/include/devils/chassis/swerveChassis.hpp new file mode 100644 index 0000000..1d52f29 --- /dev/null +++ b/include/devils/chassis/swerveChassis.hpp @@ -0,0 +1,104 @@ +#pragma once + +#include "chassisBase.hpp" +#include "../hardware/smartMotorGroup.hpp" +#include "../hardware/hallEffectEncoder.hpp" +#include "../geometry/vector2.hpp" +#include "../controller/pidController.hpp" + +namespace devils +{ + /** + * Represents a chassis driven by four sets of wheels that can rotate independently, allowing for omnidirectional movement and strafing. + * This chassis is holonomic, allowing for omnidirectional movement and strafing. + */ + class SwerveChassis : public ChassisBase + { + public: + /** + * Creates a new swerve chassis. + * @param frontLeft The swerve module for the front left wheel. + * @param frontRight The swerve module for the front right wheel. + * @param backLeft The swerve module for the back left wheel. + * @param backRight The swerve module for the back right wheel. + */ + SwerveChassis( + SwerveModule& frontLeft, + SwerveModule& frontRight, + SwerveModule& backLeft, + SwerveModule& backRight) + : frontLeft(frontLeft), + frontRight(frontRight), + backLeft(backLeft), + backRight(backRight) + { + + } + + /** + * Runs the chassis in voltage mode. + * @param forward The forward speed of the robot from -1 to 1. + * @param turn The turn speed of the robot from -1 to 1. + * @param strafe The strafe speed of the robot from -1 to 1. (Ignored for tank chassis) + */ + void move( + const float forward, + const float turn, + const float strafe) override + { + + } + + /** + * Checks if the chassis is holonomic, meaning it can strafe in any direction. + * @return True, since an H-Drive chassis can strafe. + */ + bool isHolonomic() const override + { + return true; + } + + protected: + SwerveModule& frontLeft; + SwerveModule& frontRight; + SwerveModule& backLeft; + SwerveModule& backRight; + }; + + /** + * Represents a single swerve module, which consists of two motors that can rotate independently to allow for omnidirectional movement and strafing. + * This module is used in the SwerveChassis to control the movement of the robot. + */ + struct SwerveModule + { + SmartMotorGroup& motorA; + SmartMotorGroup& motorB; + HallEffectEncoder& encoder; + + /// @brief The offset of the swerve module from the center of the robot in inches + Vector2 moduleOffset; + + SwerveModule(SmartMotorGroup& motorA, SmartMotorGroup& motorB, HallEffectEncoder& encoder) + : motorA(motorA), motorB(motorB), encoder(encoder) + { + // Disable brake mode by default to prevent overheating + motorA.setBrakeMode(false); + motorB.setBrakeMode(false); + } + + /** + * Moves the swerve module to the specified angle and magnitude. + * @param angle The target angle in degrees. + * @param magnitude The magnitude of the movement from -1 to 1. + */ + void move(float angle, float magnitude) + { + float angleDelta = angleController.update(angle - encoder.getRotation()); + motorA.move(magnitude + angleDelta); + motorB.move(magnitude - angleDelta); + } + + private: + PIDController angleController = PIDController(0.0f, 0.0f, 0.0f); // TODO: Tune these values + }; +} diff --git a/include/devils/chassis/tankChassis.hpp b/include/devils/chassis/tankChassis.hpp index b960294..ef44b6c 100644 --- a/include/devils/chassis/tankChassis.hpp +++ b/include/devils/chassis/tankChassis.hpp @@ -1,9 +1,7 @@ #pragma once -#include -#include + #include "chassisBase.hpp" #include "../hardware/smartMotorGroup.hpp" -#include "../utils/logger.hpp" namespace devils { @@ -15,13 +13,12 @@ namespace devils public: /** * Creates a new tank chassis. - * @param name The name of the chassis (for logging purposes) - * @param leftMotorPorts The ports of the left motors. Negative ports are reversed. - * @param rightMotorPorts The ports of the right motors. Negative ports are reversed. + * @param leftMotors The motor group for the left side of the chassis. + * @param rightMotors The motor group for the right side of the chassis. */ TankChassis( - SmartMotorGroup &leftMotors, - SmartMotorGroup &rightMotors) + SmartMotorGroup& leftMotors, + SmartMotorGroup& rightMotors) : leftMotors(leftMotors), rightMotors(rightMotors) { @@ -34,59 +31,65 @@ namespace devils * Runs the chassis in voltage mode. * @param forward The forward speed of the robot from -1 to 1. * @param turn The turn speed of the robot from -1 to 1. + * @param strafe The strafe speed of the robot from -1 to 1. (Ignored for tank chassis) */ - void move(double forward, double turn, double strafe = 0) override + void move( + const float forward, + const float turn, + const float strafe) override { - double fixedForward = std::clamp(forward, -1.0, 1.0); - double fixedTurn = std::clamp(turn, -1.0, 1.0); - double fixedStrafe = std::clamp(strafe, -1.0, 1.0); - moveTank(forward + turn, forward - turn); } /** * Runs the chassis in voltage mode with individual control of the left and right sides. - * @param left The speed to run the left side of the chassis from -1 to 1. - * @param right The speed to run the right side of the chassis from -1 to 1. + * @param leftSpeed The speed to run the left side of the chassis from -1 to 1. + * @param rightSpeed The speed to run the right side of the chassis from -1 to 1. */ - void moveTank(double left, double right) + void moveTank(const float leftSpeed, const float rightSpeed) const { - double fixedLeft = std::clamp(left, -1.0, 1.0); - double fixedRight = std::clamp(right, -1.0, 1.0); - - leftMotors.moveVoltage(fixedLeft); - rightMotors.moveVoltage(fixedRight); + leftMotors.move(leftSpeed); + rightMotors.move(rightSpeed); } /** - * Gets the left motor group. - * @return The left motor group. + * Forces the chassis to stop. */ - SmartMotorGroup &getLeftMotors() + void stop() override { - return leftMotors; + leftMotors.stop(); + rightMotors.stop(); } /** - * Gets the right motor group. - * @return The right motor group. + * Checks if the chassis is holonomic, meaning it can strafe in any direction. + * @return False, since a tank chassis can't strafe. */ - SmartMotorGroup &getRightMotors() + bool isHolonomic() const override { - return rightMotors; + return true; } - + /** - * Forces the chassis to stop. + * Gets the motor group for the left side of the chassis. + * @return The motor group for the left side of the chassis. */ - void stop() override + SmartMotorGroup& getLeftMotors() const { - leftMotors.stop(); - rightMotors.stop(); + return leftMotors; + } + + /** + * Gets the motor group for the right side of the chassis. + * @return The motor group for the right side of the chassis. + */ + SmartMotorGroup& getRightMotors() const + { + return rightMotors; } - private: - SmartMotorGroup &leftMotors; - SmartMotorGroup &rightMotors; + protected: + SmartMotorGroup& leftMotors; + SmartMotorGroup& rightMotors; }; -} \ No newline at end of file +} diff --git a/include/devils/chassis/xDriveChassis.hpp b/include/devils/chassis/xDriveChassis.hpp new file mode 100644 index 0000000..69d694b --- /dev/null +++ b/include/devils/chassis/xDriveChassis.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include "chassisBase.hpp" +#include "../hardware/smartMotorGroup.hpp" + +namespace devils +{ + /** + * Represents a chassis driven by a set of 4 omni-wheels at each corner forming an X. + * This chassis is holonomic, allowing for omnidirectional movement and strafing. + */ + class XDriveChassis : public ChassisBase + { + public: + /** + * Creates a new X-Drive chassis. + * @param frontLeftMotors The motor group for the front left wheel of the chassis. + * @param frontRightMotors The motor group for the front right wheel of the chassis + * @param backLeftMotors The motor group for the back left wheel of the chassis + * @param backRightMotors The motor group for the back right wheel of the chassis + */ + XDriveChassis( + SmartMotorGroup& frontLeftMotors, + SmartMotorGroup& frontRightMotors, + SmartMotorGroup& backLeftMotors, + SmartMotorGroup& backRightMotors) + : frontLeftMotors(frontLeftMotors), + frontRightMotors(frontRightMotors), + backLeftMotors(backLeftMotors), + backRightMotors(backRightMotors) + { + // Disable brake mode by default to prevent overheating + frontLeftMotors.setBrakeMode(false); + frontRightMotors.setBrakeMode(false); + backLeftMotors.setBrakeMode(false); + backRightMotors.setBrakeMode(false); + } + + /** + * Runs the chassis in voltage mode. + * @param forward The forward speed of the robot from -1 to 1. + * @param turn The turn speed of the robot from -1 to 1. + * @param strafe The strafe speed of the robot from -1 to 1. + */ + void move( + const float forward, + const float turn, + const float strafe) override + { + const float frontLeftSpeed = forward + turn + strafe; + const float frontRightSpeed = -forward + turn + strafe; + const float backLeftSpeed = forward + turn - strafe; + const float backRightSpeed = -forward + turn - strafe; + + frontLeftMotors.move(frontLeftSpeed); + frontRightMotors.move(frontRightSpeed); + backLeftMotors.move(backLeftSpeed); + backRightMotors.move(backRightSpeed); + } + + /** + * Forces the chassis to stop. + */ + void stop() override + { + frontLeftMotors.stop(); + frontRightMotors.stop(); + backLeftMotors.stop(); + backRightMotors.stop(); + } + + /** + * Checks if the chassis is holonomic, meaning it can strafe in any direction. + * @return True, since an X-Drive chassis can strafe. + */ + bool isHolonomic() const override + { + return true; + } + + protected: + SmartMotorGroup& frontLeftMotors; + SmartMotorGroup& frontRightMotors; + SmartMotorGroup& backLeftMotors; + SmartMotorGroup& backRightMotors; + }; +} diff --git a/include/devils/controller/armFeedforward.hpp b/include/devils/controller/armFeedforward.hpp index 9aef3c6..e097876 100644 --- a/include/devils/controller/armFeedforward.hpp +++ b/include/devils/controller/armFeedforward.hpp @@ -14,19 +14,19 @@ namespace devils struct Options { /// @brief Amount of gravitational force applied to the arm. - double gravity = 0.0; + float gravity = 0.0; /// @brief Amount of force required to overcome static friction. - double staticFriction = 0.0; + float staticFriction = 0.0; /// @brief Amount of voltage to apply a given velocity. - double velocityGain = 0.0; + float velocityGain = 0.0; /// @brief Amount of voltage to apply a given acceleration. - double accelerationGain = 0.0; + float accelerationGain = 0.0; }; - ArmFeedforward(Options options) + ArmFeedforward(const Options& options) : options(options) { } @@ -38,10 +38,10 @@ namespace devils * @param acceleration The desired acceleration of the arm. * @return The voltage to apply to the motor. */ - double update( - double position = 0, - double velocity = 0, - double acceleration = 0) + float update( + const float position = 0, + const float velocity = 0, + const float acceleration = 0) const { // V = output voltage // k_g = gravity @@ -51,14 +51,15 @@ namespace devils // pos = position // V = k_g * cos(pos) + k_s * sign(vel) + k_v * vel + k_a * acc - double voltage = 0.0; - voltage += options.gravity * std::cos(position); // Gravity - voltage += options.staticFriction * std::copysign(1, velocity); // Static friction - voltage += options.velocityGain * velocity; // Velocity - voltage += options.accelerationGain * acceleration; // Acceleration + float voltage = 0.0; + voltage += options.gravity * std::cos(position); // Gravity + if (velocity != 0) + voltage += options.staticFriction * std::copysign(1.0f, velocity); // Static friction + voltage += options.velocityGain * velocity; // Velocity + voltage += options.accelerationGain * acceleration; // Acceleration // Clamp the voltage to the range [-1, 1] - voltage = std::clamp(voltage, -1.0, 1.0); + voltage = std::clamp(voltage, -1.0f, 1.0f); return voltage; } @@ -66,4 +67,4 @@ namespace devils private: Options options; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/chainLoop.hpp b/include/devils/controller/chainLoop.hpp similarity index 60% rename from include/devils/hardware/chainLoop.hpp rename to include/devils/controller/chainLoop.hpp index 9c44ced..74f75e0 100644 --- a/include/devils/hardware/chainLoop.hpp +++ b/include/devils/controller/chainLoop.hpp @@ -1,6 +1,6 @@ #pragma once -#include "smartMotorGroup.hpp" -#include "../utils/math.hpp" +#include "../hardware/smartMotorGroup.hpp" +#include "../geometry/math.hpp" namespace devils { @@ -18,10 +18,10 @@ namespace devils * @param length The amount of chain in the loop. * @param startingOffset The offset of the first motor in the loop. */ - ChainLoop(SmartMotorGroup &motorGroup, - int sprocketTeeth, - int length, - double startingOffset = 0) + ChainLoop(SmartMotorGroup& motorGroup, + const int sprocketTeeth, + const int length, + const float startingOffset = 0) : motorGroup(motorGroup), sprocketTeeth(sprocketTeeth), length(length), @@ -32,19 +32,22 @@ namespace devils /** * Gets the position of the loop in chain links. */ - double getPosition() + float getPosition() { // Get the encoder position - double encoderPosition = motorGroup.getPosition(); + const auto encoderPosition = motorGroup.getPosition(); + if (!encoderPosition.isSuccess()) + return lastPosition; // Get the revolutions of the sprocket - double revolutions = encoderPosition / ENCODER_TICKS_PER_REVOLUTION; + const float revolutions = encoderPosition / ENCODER_TICKS_PER_REVOLUTION; // Get the final position in chain links - double position = revolutions * sprocketTeeth + startingOffset; + const float position = revolutions * static_cast(sprocketTeeth) + startingOffset; // Modulo the position to keep it within the length of the chain loop - return Math::signedMod(position, length); + lastPosition = Math::signedMod(position, static_cast(length)); + return lastPosition; } /** @@ -52,13 +55,13 @@ namespace devils * @param targetPosition The target position in chain links. * @return The distance to the target position in chain links. */ - double getDistanceToPosition(double targetPosition) + float getDistanceToPosition(const float targetPosition) { // Get the current position of the loop - double currentPosition = getPosition(); + const float currentPosition = getPosition(); // Calculate the distance to the target position - double distance = Math::signedMod(targetPosition - currentPosition, length); + const float distance = Math::signedMod(targetPosition - currentPosition, static_cast(length)); // Return the distance to the target position return distance; @@ -66,11 +69,13 @@ namespace devils private: /// @brief The amount of encoder ticks per revolution of the conveyor motors. - static constexpr double ENCODER_TICKS_PER_REVOLUTION = 300.0; + static constexpr float ENCODER_TICKS_PER_REVOLUTION = 300.0; - SmartMotorGroup &motorGroup; + SmartMotorGroup& motorGroup; int sprocketTeeth = 0; int length = 0; - double startingOffset = 0; + float startingOffset = 0; + + float lastPosition = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/controller/controllerBase.h b/include/devils/controller/controllerBase.h deleted file mode 100644 index 9cbe5f3..0000000 --- a/include/devils/controller/controllerBase.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include -#include - -namespace devils -{ - /** - * Represents a SISO (Single Input Single Output) controller base. - * Can be extended to provide feedforward and/or feedback control. - */ - struct ControllerBase - { - /** - * Updates the controller with a new input value. - * @param input The new input value. - * @return The output value of the controller. - */ - virtual double update(double input) = 0; - }; -} \ No newline at end of file diff --git a/include/devils/controller/elevatorFeedforward.hpp b/include/devils/controller/elevatorFeedforward.hpp index 97ec4d2..f05c678 100644 --- a/include/devils/controller/elevatorFeedforward.hpp +++ b/include/devils/controller/elevatorFeedforward.hpp @@ -13,19 +13,19 @@ namespace devils struct Options { /// @brief Amount of gravitational force applied to the elevator. - double gravity = 0.0; + float gravity = 0.0; /// @brief Amount of force required to overcome static friction. - double staticFriction = 0.0; + float staticFriction = 0.0; /// @brief Amount of voltage to apply a given velocity. - double velocityGain = 0.0; + float velocityGain = 0.0; /// @brief Amount of voltage to apply a given acceleration. - double accelerationGain = 0.0; + float accelerationGain = 0.0; }; - ElevatorFeedforward(Options options) + ElevatorFeedforward(const Options& options) : options(options) { } @@ -36,9 +36,9 @@ namespace devils * @param acceleration The desired acceleration of the elevator. * @return The voltage to apply to the motor. */ - double update( - double velocity = 0, - double acceleration = 0) + float update( + const float velocity = 0, + const float acceleration = 0) const { // V = output voltage // k_g = gravity @@ -47,14 +47,15 @@ namespace devils // k_a = acceleration gain // V = k_g + k_s * sign(vel) + k_v * vel + k_a * acc - double voltage = 0.0; - voltage += options.gravity; // Gravity - voltage += options.staticFriction * std::copysign(1, velocity); // Static friction - voltage += options.velocityGain * velocity; // Velocity - voltage += options.accelerationGain * acceleration; // Acceleration + float voltage = 0.0; + voltage += options.gravity; // Gravity + if (velocity != 0) + voltage += options.staticFriction * std::copysign(1.0f, velocity); // Static friction + voltage += options.velocityGain * velocity; // Velocity + voltage += options.accelerationGain * acceleration; // Acceleration // Clamp the voltage to the range [-1, 1] - voltage = std::clamp(voltage, -1.0, 1.0); + voltage = std::clamp(voltage, -1.0f, 1.0f); return voltage; } @@ -62,4 +63,4 @@ namespace devils private: Options options; }; -} \ No newline at end of file +} diff --git a/include/devils/controller/motorFeedforward.hpp b/include/devils/controller/motorFeedforward.hpp index d9637eb..1efd0aa 100644 --- a/include/devils/controller/motorFeedforward.hpp +++ b/include/devils/controller/motorFeedforward.hpp @@ -5,8 +5,11 @@ namespace devils { /** - * Represents a feedforward controller for a motor. - * Assumes no gravity or external forces act on the motor (See `ArmFeedforward` or `ElevatorFeedforward` for those cases). + * Represents a feedforward controller for a motor or a group of motors. + * Can be used to calculate the voltage to apply to a motor based on the desired velocity and acceleration of the motor. + * Typically used in conjunction with a `TrapezoidMotionProfile` and/or `ProfiledPIDController` to follow a motion profile with feedforward control. + * + * \note Assumes no gravity or external forces act on the motor (See `ArmFeedforward` or `ElevatorFeedforward` for those cases). */ class MotorFeedforward { @@ -14,16 +17,16 @@ namespace devils struct Options { /// @brief Amount of force required to overcome static friction. - double staticFriction = 0.0; + float staticFriction = 0.0; /// @brief Amount of voltage to apply a given velocity. - double velocityGain = 0.0; + float velocityGain = 0.0; /// @brief Amount of voltage to apply a given acceleration. - double accelerationGain = 0.0; + float accelerationGain = 0.0; }; - MotorFeedforward(Options options) + MotorFeedforward(const Options& options) : options(options) { } @@ -34,9 +37,9 @@ namespace devils * @param acceleration The desired acceleration of the motor. * @return The voltage to apply to the motor. */ - double update( - double velocity = 0, - double acceleration = 0) + float update( + const float velocity = 0, + const float acceleration = 0) const { // V = output voltage // k_s = static friction @@ -44,13 +47,14 @@ namespace devils // k_a = acceleration gain // V = k_g + k_s * sign(vel) + k_v * vel + k_a * acc - double voltage = 0.0; - voltage += options.staticFriction * std::copysign(1, velocity); // Static friction - voltage += options.velocityGain * velocity; // Velocity - voltage += options.accelerationGain * acceleration; // Acceleration + float voltage = 0.0; + if (velocity != 0) + voltage += options.staticFriction * std::copysign(1.0f, velocity); // Static friction + voltage += options.velocityGain * velocity; // Velocity + voltage += options.accelerationGain * acceleration; // Acceleration // Clamp the voltage to the range [-1, 1] - voltage = std::clamp(voltage, -1.0, 1.0); + voltage = std::clamp(voltage, -1.0f, 1.0f); return voltage; } @@ -58,4 +62,4 @@ namespace devils private: Options options; }; -} \ No newline at end of file +} diff --git a/include/devils/controller/pidController.hpp b/include/devils/controller/pidController.hpp index 93008d8..7a31bb1 100644 --- a/include/devils/controller/pidController.hpp +++ b/include/devils/controller/pidController.hpp @@ -1,27 +1,26 @@ #pragma once #include "pros/rtos.hpp" -#include "devils/controller/controllerBase.h" namespace devils { /** * Represents a feedback controller that uses a PID algorithm. */ - class PIDController : public ControllerBase + class PIDController { public: /// @brief The options for the PID controller. struct Options { /// @brief Proportional gain (p * error) - double pGain = 0.0; + float pGain = 0.0; /// @brief Integral gain (i * integral) - double iGain = 0.0; + float iGain = 0.0; /// @brief Derivative gain (d * derivative) - double dGain = 0.0; + float dGain = 0.0; }; /** @@ -31,9 +30,9 @@ namespace devils * @param dGain The derivative gain of the controller (d * derivative) */ PIDController( - double pGain, - double iGain, - double dGain) + const float pGain, + const float iGain, + const float dGain) : pGain(pGain), iGain(iGain), dGain(dGain) @@ -44,7 +43,7 @@ namespace devils * Creates a new PID controller with the given options. * @param options The options for the PID controller. */ - PIDController(Options options) + PIDController(const Options& options) : pGain(options.pGain), iGain(options.iGain), dGain(options.dGain) @@ -68,25 +67,25 @@ namespace devils /** * Updates the PID controller with a new error value. * Should be called every control loop iteration. - * @param currentError The current error value. + * @param error The current error value. * @return The current output value of the PID controller. */ - double update(double currentError) override + float update(const float error) { // Get Delta Time - double dt = pros::millis() - lastUpdateTimestamp; + const auto dt = static_cast(pros::millis() - lastUpdateTimestamp); lastUpdateTimestamp = pros::millis(); // Update Error - this->currentError = currentError; + this->currentError = error; // Update Integral - currentIntegral += currentError * dt; + currentIntegral += error * dt; // Update Derivative if (dt > 0) - currentDerivative = (currentError - lastError) / dt; - lastError = currentError; + currentDerivative = (error - lastError) / dt; + lastError = error; // Return Value return getValue(); @@ -96,29 +95,28 @@ namespace devils * Gets the last output value of the PID controller without updating it. * @return The last output value of the PID controller. */ - double getValue() + float getValue() const { - double p = pGain * currentError; - double i = iGain * currentIntegral; - double d = dGain * currentDerivative; + const float p = pGain * currentError; + const float i = iGain * currentIntegral; + const float d = dGain * currentDerivative; return p + i + d; } private: // Feedback - double currentError = 0; - double currentIntegral = 0; - double currentDerivative = 0; + float currentError = 0; + float currentIntegral = 0; + float currentDerivative = 0; // Last Values - double lastError = 0; - double lastUpdateTimestamp = 0; + float lastError = 0; + uint32_t lastUpdateTimestamp = 0; // PID Variables - double pGain; - double iGain; - double dGain; + float pGain; + float iGain; + float dGain; }; - -} \ No newline at end of file +} diff --git a/include/devils/controller/profiledPIDController.hpp b/include/devils/controller/profiledPIDController.hpp new file mode 100644 index 0000000..4013222 --- /dev/null +++ b/include/devils/controller/profiledPIDController.hpp @@ -0,0 +1,117 @@ +#pragma once + +#include "trapezoidMotionProfile.hpp" +#include "pidController.hpp" +#include "pros/rtos.hpp" + +namespace devils +{ + /** + * Represents a controller for `TrapezoidMotionProfile` that allows you to get the current state of the motion profile over a time period. + * Based on https://docs.wpilib.org/en/stable/docs/software/advanced-controls/controllers/profiled-pidcontroller.html + */ + class ProfiledPIDController + { + public: + /** + * Makes a controller for a new `TrapezoidMotionProfile` based on the given constraints, distance, starting velocity, and ending velocity. + * @param pidOptions - The options for the internal PID controller, which can be used as a feedback controller to correct for any errors in the motion profile. + * @param constraints - The constraints for the motion profile, including max velocity, acceleration, and deceleration. + * @param goalDistance - The total distance to be covered by the motion profile (typically inches or degrees). + * @param startingVelocity - The initial velocity of the motion profile (typically inches/s or degrees/s). + * @param endingVelocity - The final velocity of the motion profile (typically inches/s or degrees/s). + */ + ProfiledPIDController( + const PIDController::Options& pidOptions, + const TrapezoidMotionProfile::Constraints& constraints, + const float goalDistance, + const float startingVelocity = 0, + const float endingVelocity = 0) + : pidController(pidOptions), + motionProfile(constraints, goalDistance, startingVelocity, endingVelocity), + internalTimer(motionProfile.getTotalDuration()) + { + } + + /** + * Resets the internal timer to 0 and restarts the motion profile from the beginning + */ + void reset() + { + pidController.reset(); + internalTimer.start(); + } + + /** + * Calculates the time remaining in the motion profile based on the internal timer. + * @return The time remaining in the motion profile in seconds. Returns 0 if the motion profile is complete. + */ + float getTimeRemaining() const + { + return internalTimer.getTimeRemaining(); + } + + /** + * Gets the total duration of the motion profile. + * @return The total duration of the motion profile in seconds. + */ + float getTotalDuration() const + { + return motionProfile.getTotalDuration(); + } + + /** + * Gets the setpoint which is the target state of the motion profile based on the internal timer. + * @param deltaTime - The amount of time to add to the internal timer to account for feedback latency. This can be used to get a more accurate setpoint for the current time when using the `update` method, which may have some latency between getting the setpoint and applying the output. + * @return The current state of the motion profile based on the internal timer. + */ + TrapezoidMotionProfile::State getSetpoint(const float deltaTime = 0.0f) const + { + const auto t = internalTimer.getElapsedTime() + deltaTime; + return motionProfile.getStateAtTime(t); + } + + /** + * Gets the current error of the motion profile based on the current position and the setpoint. This can be used as feedback to correct for any errors in the motion profile. + * @param currentPosition - The current position of the system (typically inches or degrees). + * @return The current error of the motion profile based on the current position and the setpoint. Returns 0 if the motion profile is complete. + */ + float update(const float currentPosition) + { + const auto setpoint = getSetpoint(-feedbackDelay); + return pidController.update(setpoint.position - currentPosition); + } + + /** + * Sets a new goal distance for the motion profile and resets the internal timer. + * Should be called whenever the goal distance changes to update the motion profile. + * @param newGoal - The new goal distance to be covered by the motion profile (typically inches or degrees). + * @param newStartingVelocity - The new initial velocity of the motion profile (typically inches/s or degrees/s). If not provided, the starting velocity will be 0. + * @param newEndingVelocity - The new final velocity of the motion profile (typically inches/s or degrees/s). If not provided, the ending velocity will be 0. + */ + void setGoal(const float newGoal, + const float newStartingVelocity = 0, + const float newEndingVelocity = 0) + { + motionProfile.recalculate(newGoal, newStartingVelocity, newEndingVelocity); + internalTimer.setDuration(motionProfile.getTotalDuration()); + reset(); + } + + /** + * Adds a delay to the internal timer to account for feedback latency. + * Only used in the `update` method to get a more accurate setpoint for the current time when there may be some latency between getting the setpoint and applying the output. + * @param delay - The amount of time to add to the internal timer to account for feedback latency in seconds. + */ + void setFeedbackDelay(const float delay) + { + feedbackDelay = delay; + } + + protected: + PIDController pidController; + TrapezoidMotionProfile motionProfile; + Timer internalTimer; + float feedbackDelay = 0.02f; // seconds + }; +} diff --git a/include/devils/hardware/symmetricControl.hpp b/include/devils/controller/symmetricController.hpp similarity index 71% rename from include/devils/hardware/symmetricControl.hpp rename to include/devils/controller/symmetricController.hpp index c244103..417e67b 100644 --- a/include/devils/hardware/symmetricControl.hpp +++ b/include/devils/controller/symmetricController.hpp @@ -1,7 +1,6 @@ #pragma once -#include "smartMotorGroup.hpp" -#include "vexbridge/vexBridge.h" +#include "../hardware/smartMotorGroup.hpp" namespace devils { @@ -14,7 +13,8 @@ namespace devils { public: SymmetricControl(SmartMotorGroup &leftMotors, SmartMotorGroup &rightMotors) - : leftMotors(leftMotors), rightMotors(rightMotors) + : leftMotors(leftMotors), + rightMotors(rightMotors) { } @@ -31,7 +31,7 @@ namespace devils * Drives the motors in a symmetric manner. * @param speed The speed to drive the motors at, from -1 to 1. */ - void drive(double speed, double horizontalSpeed) + void drive(float speed, float horizontalSpeed) { // Clamp the speed to the range [-1, 1] speed = std::clamp(speed, -1.0, 1.0); @@ -46,8 +46,8 @@ namespace devils rightOffset += horizontalSpeed * HORIZONTAL_ENCODER_SPEED; // Fetch the encoder values and subtract the offsets - double leftEncoder = leftMotors.getPosition() - leftOffset; - double rightEncoder = rightMotors.getPosition() - rightOffset; + float leftEncoder = leftMotors.getPosition() - leftOffset; + float rightEncoder = rightMotors.getPosition() - rightOffset; // Check which motor is behind bool leftMotorBehind = leftEncoder < rightEncoder; @@ -56,15 +56,15 @@ namespace devils leftMotorBehind = !leftMotorBehind; // Calculate the speed scaling factor - double behindEncoder = leftMotorBehind ? leftEncoder : rightEncoder; - double aheadEncoder = leftMotorBehind ? rightEncoder : leftEncoder; - double deltaEncoder = leftEncoder - rightEncoder; + float behindEncoder = leftMotorBehind ? leftEncoder : rightEncoder; + float aheadEncoder = leftMotorBehind ? rightEncoder : leftEncoder; + float deltaEncoder = leftEncoder - rightEncoder; - double speedScale = 1.0 - (std::abs(deltaEncoder) / ENCODER_MAX_OFFSET); + float speedScale = 1.0 - (std::abs(deltaEncoder) / ENCODER_MAX_OFFSET); speedScale = std::clamp(speedScale, 0.0, 1.0); // Calculate the horizontal speed scaling factor - double horizontalScale = deltaEncoder * + float horizontalScale = deltaEncoder * HORIZONTAL_MULTIPLIER * std::abs(horizontalSpeed); @@ -82,16 +82,16 @@ namespace devils } private: - static constexpr double HORIZONTAL_DEADZONE = 0.5; // deadzone for the joystick - static constexpr double HORIZONTAL_MULTIPLIER = 0.005; // % - static constexpr double HORIZONTAL_ENCODER_SPEED = 2; // ticks per iteration - static constexpr double ENCODER_MAX_OFFSET = 100; // ticks + static constexpr float HORIZONTAL_DEADZONE = 0.5f; // deadzone for the joystick + static constexpr float HORIZONTAL_MULTIPLIER = 0.005f; // % + static constexpr float HORIZONTAL_ENCODER_SPEED = 2.0f; // ticks per iteration + static constexpr float ENCODER_MAX_OFFSET = 100; // ticks static constexpr bool IS_REVERSED = true; // true if the motors are reversed SmartMotorGroup &leftMotors; SmartMotorGroup &rightMotors; - double leftOffset = 0.0; - double rightOffset = 0.0; + float leftOffset = 0.0; + float rightOffset = 0.0; }; } \ No newline at end of file diff --git a/include/devils/controller/trapezoidMotionProfile.hpp b/include/devils/controller/trapezoidMotionProfile.hpp new file mode 100644 index 0000000..f16e4e2 --- /dev/null +++ b/include/devils/controller/trapezoidMotionProfile.hpp @@ -0,0 +1,247 @@ +#pragma once + +#include "../utils/logger.hpp" + +namespace devils +{ + class TrapezoidMotionProfile + { + public: + /// @brief A list of constraints for the motion profile. + struct Constraints + { + /// @brief The maximum velocity of the motion profile (typically inches/s or degrees/s). + float maxVelocity; + + /// @brief The maximum acceleration of the motion profile (typically inches/s^2 or degrees/s^2). + float maxAcceleration; + + /// @brief The maximum deceleration of the motion profile (typically inches/s^2 or degrees/s^2). + float maxDeceleration; + }; + + /// @brief An enum representing the current phase of the motion profile. + enum Phase + { + ACCELERATING, + CRUISING, + DECELERATING, + COMPLETE + }; + + /// @brief A struct representing the state of the motion profile at a given time. + struct State + { + float time; + float position; + float velocity; + float acceleration; + }; + + /** + * Calculates a trapezoidal motion profile based on the given constraints, starting velocity, ending velocity, and distance. + * @param constraints - The constraints for the motion profile, including max velocity, acceleration, and deceleration. + * @param goalDistance - The total distance to be covered by the motion profile (typically inches or degrees). + * @param startingVelocity - The initial velocity of the motion profile (typically inches/s or degrees/s). + * @param endingVelocity - The final velocity of the motion profile (typically inches/s or degrees/s). + */ + TrapezoidMotionProfile( + const Constraints& constraints, + const float goalDistance, + const float startingVelocity = 0, + const float endingVelocity = 0) + : constraints(constraints) + { + recalculate(goalDistance, startingVelocity, endingVelocity); + } + + /** + * Recalculates the motion profile based on the given distance. + * Should be called whenever the distance changes to update the motion profile. + * @param goalDistance - The total distance to be covered by the motion profile (typically inches or degrees). + * @param newStartingVelocity - The new initial velocity of the motion profile (typically inches/s or degrees/s). + * @param newEndingVelocity - The new final velocity of the motion profile (typically inches/s or degrees/s). + */ + void recalculate( + const float goalDistance, + const float newStartingVelocity = 0, + const float newEndingVelocity = 0) + { + // Check if constraints are zero + if (constraints.maxVelocity <= 0 || + constraints.maxAcceleration <= 0 || + constraints.maxDeceleration <= 0) + { + Logger::warn( + "TrapezoidMotionProfile: One or more constraints are zero or negative. Adjusting to default values."); + constraints.maxVelocity = std::max(constraints.maxVelocity, 1.0f); + constraints.maxAcceleration = std::max(constraints.maxAcceleration, 1.0f); + constraints.maxDeceleration = std::max(constraints.maxDeceleration, 1.0f); + } + + startingVelocity = newStartingVelocity; + endingVelocity = newEndingVelocity; + + // Calculate the time to accelerate/decelerate to/from the peak velocity + peakVelocity = constraints.maxVelocity; + accelerationTime = (peakVelocity - startingVelocity) / constraints.maxAcceleration; + decelerationTime = (peakVelocity - endingVelocity) / constraints.maxDeceleration; + + accelerationDistance = + calculateDistance(0, startingVelocity, constraints.maxAcceleration, accelerationTime); + decelerationDistance = calculateDistance(0, peakVelocity, -constraints.maxDeceleration, decelerationTime); + + // Calculate the distance covered during the cruising velocity phase + const auto positiveGoalDistance = std::abs(goalDistance); + cruisingDistance = positiveGoalDistance - accelerationDistance - decelerationDistance; + cruisingTime = cruisingDistance / peakVelocity; + + // Check if this is a triangular profile + if (cruisingDistance < 0) + { + // Recalculate the peak velocity for a triangular profile + peakVelocity = 2 * positiveGoalDistance + + startingVelocity * startingVelocity / constraints.maxAcceleration + + endingVelocity * endingVelocity / constraints.maxDeceleration; + peakVelocity /= 1 / constraints.maxAcceleration + 1 / constraints.maxDeceleration; + peakVelocity = std::sqrt(peakVelocity); + + // Recalculate the times for the triangular profile + accelerationTime = (peakVelocity - startingVelocity) / constraints.maxAcceleration; + decelerationTime = (peakVelocity - endingVelocity) / constraints.maxDeceleration; + + accelerationDistance = calculateDistance(0, startingVelocity, constraints.maxAcceleration, + accelerationTime); + decelerationDistance = calculateDistance(0, peakVelocity, -constraints.maxDeceleration, + decelerationTime); + + cruisingTime = 0; + cruisingDistance = 0; + } + + // Safety check if the calculated times are negative (which can happen if the starting/ending velocities are too high) + if (accelerationTime < 0) + { + Logger::warn("TrapezoidMotionProfile: Negative acceleration time calculated. Adjusting to 0."); + accelerationTime = 0; + peakVelocity = startingVelocity; + } + if (decelerationTime < 0) + { + Logger::warn("TrapezoidMotionProfile: Negative deceleration time calculated. Adjusting to 0."); + decelerationTime = 0; + peakVelocity = endingVelocity; + } + + // Copy the sign of the goal distance to the distances and velocities + const float goalSign = (goalDistance >= 0) ? 1 : -1; + accelerationDistance *= goalSign; + cruisingDistance *= goalSign; + decelerationDistance *= goalSign; + peakVelocity *= goalSign; + acceleration = constraints.maxAcceleration * goalSign; + deceleration = -constraints.maxDeceleration * goalSign; + } + + /** + * Gets the current phase of the motion profile at a given time. + * @param t - The time at which to get the current phase of the motion profile (typically seconds). + * @return The current phase of the motion profile (ACCELERATING, CRUISING, DECELERATING, or COMPLETE). + */ + Phase getPhase(const float t) const + { + if (t < accelerationTime) + return ACCELERATING; + if (t < accelerationTime + cruisingTime) + return CRUISING; + if (t < accelerationTime + cruisingTime + decelerationTime) + return DECELERATING; + return COMPLETE; + } + + /** + * Gets the state of the motion profile at a given time. + * @param t - The time at which to get the state of the motion profile (typically seconds). + * @return The state of the motion profile at the given time, including position, velocity, and acceleration. + */ + State getStateAtTime(const float t) const + { + float localT; + const auto currentPhase = getPhase(t); + State state{t, 0, 0, 0}; + + switch (currentPhase) + { + case ACCELERATING: + state.position = calculateDistance(0, startingVelocity, acceleration, t); + state.velocity = startingVelocity + acceleration * t; + state.acceleration = acceleration; + break; + case CRUISING: + localT = t - accelerationTime; + state.position = calculateDistance(accelerationDistance, peakVelocity, 0, localT); + state.velocity = peakVelocity; + state.acceleration = 0; + break; + case DECELERATING: + localT = t - accelerationTime - cruisingTime; + state.position = calculateDistance(accelerationDistance + cruisingDistance, peakVelocity, deceleration, + localT); + state.velocity = peakVelocity + deceleration * localT; + state.acceleration = deceleration; + break; + case COMPLETE: + state.position = accelerationDistance + cruisingDistance + decelerationDistance; + state.velocity = endingVelocity; + state.acceleration = 0; + break; + } + return state; + } + + /** + * Gets the total duration of the motion profile. + * @return The total duration of the motion profile in seconds. + */ + float getTotalDuration() const + { + return accelerationTime + cruisingTime + decelerationTime; + } + + protected: + /** + * Calculates the distance covered during a segment of motion given an initial velocity, constant acceleration, and time. + * @param initialPosition - The initial position of this segment (typically inches or degrees). + * @param initialVelocity - The initial velocity of this segment (typically inches/s or degrees/s). + * @param acceleration - A constant acceleration of this segment (typically inches/s^2 or degrees/s^2). + * @param time - The duration of this segment (typically seconds). + * @return The distance covered during this segment (typically inches or degrees). + */ + static float calculateDistance( + const float initialPosition, + const float initialVelocity, + const float acceleration, + const float time) + { + return initialPosition + initialVelocity * time + 0.5f * acceleration * time * time; + } + + private: + float startingVelocity; + float endingVelocity; + Constraints constraints; + + float accelerationDistance; + float cruisingDistance; + float decelerationDistance; + + float accelerationTime; + float cruisingTime; + float decelerationTime; + + float acceleration; + float deceleration; + + float peakVelocity; + }; +} diff --git a/include/devils/devils.h b/include/devils/devils.h index c81a479..01ce1a4 100644 --- a/include/devils/devils.h +++ b/include/devils/devils.h @@ -10,21 +10,19 @@ // Chassis #include "chassis/chassisBase.hpp" #include "chassis/tankChassis.hpp" +#include "chassis/mecanumChassis.hpp" +#include "chassis/hDriveChassis.hpp" +#include "chassis/xDriveChassis.hpp" #include "chassis/dummyChassis.hpp" // Hardware -#include "hardware/gps.hpp" #include "hardware/inertialSensor.hpp" -#include "hardware/inertialSensorGroup.hpp" #include "hardware/opticalSensor.hpp" -#include "hardware/legacyVisionSensor.hpp" #include "hardware/adiPneumatic.hpp" #include "hardware/adiDigitalInput.hpp" +#include "hardware/adiAnalogInput.hpp" #include "hardware/adiPneumaticGroup.hpp" -#include "hardware/chainLoop.hpp" -#include "hardware/symmetricControl.hpp" -#include "hardware/led.hpp" -// #include "hardware/devilCV.hpp" +#include "controller/chainLoop.hpp" // Odom #include "odom/odomSource.hpp" @@ -33,20 +31,11 @@ #include "odom/perpendicularSensorOdom.hpp" #include "odom/parallelSensorOdom.hpp" #include "odom/delayedOdom.hpp" -#include "odom/visionTargetOdom.hpp" - -// Pros -#include "api.h" - -// Robot -#include "utils/robot.hpp" - -// robotAutoOptions -#include "utils/robotAutoOptions.hpp" // Utils -#include "utils/joystickCurve.hpp" +#include "utils/robot.hpp" #include "utils/timer.hpp" +#include "utils/stopwatch.hpp" // Path #include "path/path.hpp" @@ -59,35 +48,24 @@ #include "trajectory/trajectoryConstraints.hpp" // Controller -#include "controller/controllerBase.h" #include "controller/pidController.hpp" #include "controller/armFeedforward.hpp" #include "controller/elevatorFeedforward.hpp" #include "controller/motorFeedforward.hpp" +#include "controller/trapezoidMotionProfile.hpp" // AutoSteps #include "autoSteps/autoBuilder.hpp" -#include "autoSteps/steps/autoTimeoutStep.hpp" -#include "autoSteps/steps/autoDriveTimeStep.hpp" -#include "autoSteps/steps/autoDriveToStep.hpp" -#include "autoSteps/steps/autoDriveStep.hpp" -#include "autoSteps/steps/autoRotateStep.hpp" -#include "autoSteps/steps/autoRotateToStep.hpp" -#include "autoSteps/steps/autoPauseStep.hpp" -#include "autoSteps/steps/autoJumpToStep.hpp" -#include "autoSteps/steps/autoBranchStep.hpp" -#include "autoSteps/steps/autoPurePursuitStep.hpp" -#include "autoSteps/steps/autoRamseteStep.hpp" -#include "autoSteps/transformer/mirrorTransform.hpp" +#include "autoSteps/steps/drivePIDStep.hpp" +#include "autoSteps/steps/driveRAMSETEStep.hpp" +#include "autoSteps/steps/rotateMotionProfileStep.hpp" // Display -#include "display/optionsRenderer.hpp" -#include "display/eyesRenderer.hpp" - -// Led -#include "lights/ledStrip.hpp" +#include "display/toastDisplay.hpp" +#include "display/devilbotsDisplay.hpp" +#include "display/autoPickerDisplay.hpp" +// #include "display/eyesRenderer.hpp" // VEXBridge -#include "../vexbridge/vexbridge.h" -#include "vexbridge/vbOdom.hpp" -#include "vexbridge/vbPath.hpp" \ No newline at end of file +// TODO: VEXBridge is currently broken +// #include "../vexbridge/vexbridge.h" diff --git a/include/devils/display/autoPickerDisplay.hpp b/include/devils/display/autoPickerDisplay.hpp new file mode 100644 index 0000000..941a7f5 --- /dev/null +++ b/include/devils/display/autoPickerDisplay.hpp @@ -0,0 +1,352 @@ +#pragma once + +#include +#include +#include "liblvgl/lvgl.h" +#include "displayBase.hpp" + +namespace devils +{ + class AutoPickerDisplay : public DisplayBase + { + public: + /// @brief List of alliance colors to choose from when selecting an autonomous routine. + enum AllianceColor + { + RED_ALLIANCE = 0, + BLUE_ALLIANCE = 1, + NONE_ALLIANCE = 2 + }; + + /// @brief A struct representing an autonomous routine that can be selected by the user. + struct Routine + { + uint8_t id = 0; + std::string displayName = "Routine"; + bool requiresAllianceColor = false; + }; + + /** + * + * @param robotName - The name of the robot to display at the top of the screen + * @param routines - A list of routines to display for the user to select from. The first routine in the list will be selected by default. + */ + AutoPickerDisplay( + std::string robotName, + const std::vector& routines) + : robotName(std::move(robotName)), + routines(routines) + { + root = lv_obj_create(getRootContainer()); + lv_obj_set_size(root, LV_HOR_RES_MAX, LV_VER_RES_MAX); + lv_obj_center(root); + lv_obj_set_style_bg_opa(root, 0, 0); + lv_obj_set_style_border_opa(root, 0, 0); + + createOptionsDisplayContainer(); + } + + ~AutoPickerDisplay() override + { + lv_obj_del(root); + } + + /** + * Gets the currently selected autonomous routine. + * @return The currently selected autonomous routine. + */ + Routine getSelectedRoutine() + { + return selectedRoutine; + } + + /** + * Sets the visibility of the auto picker display. + * @param visible - Whether the auto picker display should be visible or not. + */ + void setVisible(const bool visible) const + { + if (visible) + lv_obj_clear_flag(root, LV_OBJ_FLAG_HIDDEN); + else + lv_obj_add_flag(root, LV_OBJ_FLAG_HIDDEN); + } + + protected: + /// @brief An enum to identify the type of callback in the event system, allowing us to use a single callback function for multiple different events. + enum CallbackType + { + ROUTINE, + ALLIANCE_COLOR, + SCREEN_SAVER_TOGGLE + }; + + /// @brief A struct to hold data for event callbacks, allowing us to pass multiple pieces of information through the lvgl event system. + struct CallbackData + { + AutoPickerDisplay* display; + CallbackType type; + void* data; + }; + + /** + * Static method used as a callback for all events in the auto picker display. The CallbackData struct is used to determine what type of event occurred and to access the relevant data for that event. + * @param e - The event data passed by the LVGL event system when an event occurs. + */ + static void onCallback(lv_event_t* e) + { + const auto data = static_cast(lv_event_get_user_data(e)); + switch (data->type) + { + case ROUTINE: + data->display->handleRoutineChange(e); + break; + case ALLIANCE_COLOR: + data->display->handleAllianceColorChange(e); + break; + case SCREEN_SAVER_TOGGLE: + data->display->handleScreenSaverToggle(e); + break; + } + } + + void createOptionsDisplayContainer() + { + const auto fullscreen_container = lv_obj_create(root); + lv_obj_set_size(fullscreen_container, lv_pct(100), lv_pct(100)); + lv_obj_set_layout(fullscreen_container, LV_LAYOUT_FLEX); + lv_obj_set_flex_flow(fullscreen_container, LV_FLEX_FLOW_COLUMN); + lv_obj_set_flex_align( + fullscreen_container, + LV_FLEX_ALIGN_CENTER, + LV_FLEX_ALIGN_CENTER, + LV_FLEX_ALIGN_CENTER); + + lv_obj_t* title = lv_label_create(fullscreen_container); + lv_label_set_text(title, robotName.c_str()); + lv_obj_set_style_text_align(title, LV_TEXT_ALIGN_CENTER, 0); + lv_obj_set_style_text_color(title, lv_color_make(255, 255, 255), 0); + lv_obj_set_style_text_font(title, &lv_font_montserrat_24, 0); + + auto options_display_container = lv_obj_create(fullscreen_container); + lv_obj_set_style_pad_all(options_display_container, 0, 0); + lv_obj_set_flex_grow(options_display_container, 1); + lv_obj_set_style_border_width(options_display_container, 0, 0); + lv_obj_set_width(options_display_container, lv_pct(100)); + lv_obj_clear_flag(options_display_container, LV_OBJ_FLAG_SCROLLABLE); + lv_obj_set_layout(options_display_container, LV_LAYOUT_FLEX); + lv_obj_set_flex_flow(options_display_container, LV_FLEX_FLOW_ROW); + lv_obj_set_style_pad_row(options_display_container, 0, 0); + + createAllianceColorContainer(options_display_container); + createRoutineContainer(options_display_container); + } + + void createAllianceColorContainer(lv_obj_t* parent) + { + const auto alliance_color_container = lv_obj_create(parent); + lv_obj_set_size(alliance_color_container, lv_pct(30), lv_pct(100)); + lv_obj_set_layout(alliance_color_container, LV_LAYOUT_FLEX); + lv_obj_set_flex_flow(alliance_color_container, LV_FLEX_FLOW_COLUMN); + lv_obj_set_flex_align(alliance_color_container, LV_FLEX_ALIGN_CENTER, LV_FLEX_ALIGN_CENTER, + LV_FLEX_ALIGN_CENTER); + + lv_obj_t* alliance_title = lv_label_create(alliance_color_container); + lv_label_set_text(alliance_title, "Alliance"); + lv_obj_set_style_text_align(alliance_title, LV_TEXT_ALIGN_CENTER, 0); + lv_obj_set_style_text_color(alliance_title, lv_color_make(255, 255, 255), 0); + lv_obj_set_style_text_font(alliance_title, &lv_font_montserrat_16, 0); + + lv_obj_t* alliance_color_button = lv_btn_create(alliance_color_container); + lv_obj_set_size(alliance_color_button, lv_pct(100), 100); + lv_obj_set_flex_grow(alliance_color_button, 1); + lv_obj_set_style_bg_color(alliance_color_button, ALLIANCE_COLOR_MAP.at(selectedAlliance), 0); + lv_obj_add_event_cb( + alliance_color_button, + onCallback, + LV_EVENT_CLICKED, + new CallbackData{this, ALLIANCE_COLOR, nullptr}); + + lv_obj_t* alliance_color_label = lv_label_create(alliance_color_button); + lv_label_set_text(alliance_color_label, ALLIANCE_NAME_MAP.at(selectedAlliance).c_str()); + lv_obj_center(alliance_color_label); + } + + void createRoutineContainer(lv_obj_t* parent) + { + const auto right_container = lv_obj_create(parent); + lv_obj_set_size(right_container, lv_pct(70), lv_pct(100)); + lv_obj_set_layout(right_container, LV_LAYOUT_FLEX); + lv_obj_set_flex_flow(right_container, LV_FLEX_FLOW_ROW); + lv_obj_clear_flag(right_container, LV_OBJ_FLAG_SCROLLABLE); + + const auto routine_container = lv_obj_create(right_container); + lv_obj_set_size(routine_container, lv_pct(70), lv_pct(100)); + lv_obj_set_layout(routine_container, LV_LAYOUT_FLEX); + lv_obj_set_flex_flow(routine_container, LV_FLEX_FLOW_COLUMN); + lv_obj_set_style_border_width(routine_container, 0, 0); + lv_obj_set_style_pad_all(routine_container, 0, 0); + + // title + lv_obj_t* routine_title = lv_label_create(routine_container); + lv_label_set_text(routine_title, "Select Routine"); + lv_obj_set_style_text_align(routine_title, LV_TEXT_ALIGN_CENTER, 0); + lv_obj_set_style_text_color(routine_title, lv_color_make(255, 255, 255), 0); + lv_obj_set_style_text_font(routine_title, &lv_font_montserrat_16, 0); + + for (const auto& routine : routines) + createRoutineButton(routine_container, routine); + + // std::vector routine_names; + // for (const auto& routine : newRoutines) + // routine_names.push_back(routine.displayName); + + // TODO: FIX ME + // const auto radio_group = new RadioGroup(routine_container, routine_names, handleRoutineChange); + // lv_obj_t* radio_group_obj = radio_group->getRadioGroup(); + // lv_obj_set_width(radio_group_obj, lv_pct(100)); + // lv_obj_set_flex_grow(radio_group_obj, 1); + // lv_obj_set_style_pad_all(radio_group_obj, 0, 0); + // lv_obj_set_style_border_width(radio_group_obj, 0, 0); + + const auto screen_saver_toggle_container = lv_obj_create(right_container); + lv_obj_set_size(screen_saver_toggle_container, lv_pct(30), lv_pct(100)); + lv_obj_set_layout(screen_saver_toggle_container, LV_LAYOUT_FLEX); + lv_obj_set_flex_flow(screen_saver_toggle_container, LV_FLEX_FLOW_COLUMN); + lv_obj_set_flex_align(screen_saver_toggle_container, LV_FLEX_ALIGN_END, LV_FLEX_ALIGN_END, + LV_FLEX_ALIGN_END); + lv_obj_set_style_pad_all(screen_saver_toggle_container, 0, 0); + lv_obj_set_style_border_width(screen_saver_toggle_container, 0, 0); + + const auto screen_saver_toggle_button = lv_btn_create(screen_saver_toggle_container); + lv_obj_set_size(screen_saver_toggle_button, lv_pct(100), 50); + const auto screen_saver_toggle_label = lv_label_create(screen_saver_toggle_button); + lv_label_set_text(screen_saver_toggle_label, "Save"); + lv_obj_center(screen_saver_toggle_label); + lv_obj_add_event_cb( + screen_saver_toggle_button, + onCallback, + LV_EVENT_CLICKED, + new CallbackData{this, SCREEN_SAVER_TOGGLE, nullptr}); + } + + void createRoutineButton(lv_obj_t* parent, const Routine& routine) + { + const auto button = lv_checkbox_create(parent); + lv_checkbox_set_text(button, routine.displayName.c_str()); + lv_obj_add_flag(button, LV_OBJ_FLAG_EVENT_BUBBLE); + lv_obj_add_event_cb( + button, + onCallback, + LV_EVENT_VALUE_CHANGED, + new CallbackData{this, ROUTINE, new Routine(routine)}); + } + + void handleRoutineChange(lv_event_t* e) + { + // Deselect other checkboxes + const auto target = lv_event_get_target(e); + const auto parent = lv_obj_get_parent(target); + const auto child_count = lv_obj_get_child_cnt(parent); + for (auto i = 0; i < child_count; i++) + { + const auto child = lv_obj_get_child(parent, i); + if (child != target) + lv_obj_clear_state(child, LV_STATE_CHECKED); + } + + // Ensure the clicked checkbox is selected + lv_obj_add_state(target, LV_STATE_CHECKED); + + // Update selected routine + const auto data = static_cast(lv_event_get_user_data(e)); + selectedRoutine = *static_cast(data->data); + } + + void handleAllianceColorChange(lv_event_t* e) + { + auto* btn = static_cast(lv_event_get_target(e)); + lv_obj_t* label = lv_obj_get_child(btn, 0); + if (label != nullptr) + { + const auto text = lv_label_get_text(label); + const auto currentColor = getCurrentAllianceColor(text); + + const auto nextColor = getNextAllianceColor(currentColor); + lv_obj_set_style_bg_color(btn, ALLIANCE_COLOR_MAP.at(nextColor), 0); + lv_label_set_text(label, ALLIANCE_NAME_MAP.at(nextColor).c_str()); + selectedAlliance = nextColor; + } + } + + void handleScreenSaverToggle(lv_event_t* e) const + { + if (selectedRoutine.requiresAllianceColor && + selectedAlliance == NONE_ALLIANCE) + { + // show error message + lv_obj_t* message_box = lv_msgbox_create( + root, + "Error", + "Selected routine can't be used when Alliance is set to 'None'.", + nullptr, + true); + + // center the message box + // shadow + lv_obj_set_style_shadow_color(message_box, lv_color_make(0, 0, 0), 0); + lv_obj_set_style_shadow_width(message_box, 10, 0); + lv_obj_set_style_shadow_opa(message_box, LV_OPA_COVER, 0); + lv_obj_set_style_shadow_ofs_x(message_box, 4, 0); + lv_obj_set_style_shadow_ofs_y(message_box, 4, 0); + // border + + lv_obj_center(message_box); + return; + } + + // Hide + setVisible(false); + } + + AllianceColor getCurrentAllianceColor(const char* text) const + { + for (const auto& pair : ALLIANCE_NAME_MAP) + { + if (pair.second == text) + return pair.first; + } + return NONE_ALLIANCE; + } + + AllianceColor getNextAllianceColor(const AllianceColor current_color) const + { + auto it = ALLIANCE_COLOR_MAP.find(current_color); + auto next_it = std::next(it); + if (next_it == ALLIANCE_COLOR_MAP.end()) + next_it = ALLIANCE_COLOR_MAP.begin(); + return next_it->first; + } + + private: + const std::map ALLIANCE_COLOR_MAP = { + {RED_ALLIANCE, lv_color_make(255, 0, 0)}, + {BLUE_ALLIANCE, lv_color_make(0, 0, 255)}, + {NONE_ALLIANCE, lv_color_make(124, 124, 124)} + }; + const std::map ALLIANCE_NAME_MAP = { + {RED_ALLIANCE, "Red"}, + {BLUE_ALLIANCE, "Blue"}, + {NONE_ALLIANCE, "None"} + }; + + std::string robotName; + std::vector routines; + + AllianceColor selectedAlliance = NONE_ALLIANCE; + Routine selectedRoutine; + bool isModalOpen = true; + + lv_obj_t* root; + }; +} diff --git a/include/devils/display/buttonGrid.hpp b/include/devils/display/buttonGrid.hpp deleted file mode 100644 index 5a26388..0000000 --- a/include/devils/display/buttonGrid.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#pragma once - -#include "liblvgl/lvgl.h" -#include -#include - -namespace devils -{ - class ButtonGrid - { - public: - struct ButtonData - { - std::string title; - lv_event_cb_t callback; - }; - - ButtonGrid(uint8_t rows, uint8_t cols, std::vector buttonData) - : rows(rows), - cols(cols) - { - root = lv_obj_create(NULL); - lv_scr_load(root); - - grid = lv_obj_create(root); - lv_obj_set_size(grid, 240, 240); - lv_obj_align(grid, LV_ALIGN_CENTER, 0, 0); - lv_obj_set_pos(grid, 0, 0); - lv_obj_set_style_radius(grid, 0, 0); - lv_obj_set_style_border_width(grid, 0, 0); - lv_obj_set_style_bg_color(grid, lv_color_make(3, 36, 53), 0); - lv_obj_set_scrollbar_mode(grid, LV_SCROLLBAR_MODE_OFF); - - buttons.reserve(buttonData.size()); - - uint8_t row = 0; - uint8_t col = 0; - - for (uint16_t i = 0; i < buttonData.size(); i++) - { - // Fetch Data - ButtonData data = buttonData[i]; - - // Create Button - lv_obj_t *button = lv_btn_create(grid); - lv_obj_set_size(button, 60, 60); - lv_obj_align(button, LV_ALIGN_CENTER, 0, 0); - lv_obj_set_pos(button, col * 60, row * 60); - lv_obj_set_style_radius(button, 0, 0); - lv_obj_set_style_border_width(button, 0, 0); - lv_obj_set_style_bg_color(button, lv_color_make(255, 255, 255), 0); - lv_obj_add_event_cb(button, data.callback, LV_EVENT_CLICKED, NULL); - buttons.push_back(button); - - // Create Label - lv_obj_t *label = lv_label_create(button); - lv_label_set_text(label, data.title.c_str()); - lv_obj_align(label, LV_ALIGN_CENTER, 0, 0); - - // Update Row and Column - col++; - if (col >= cols) - { - col = 0; - row++; - } - } - } - ~ButtonGrid() - { - lv_obj_del(root); - } - - private: - lv_obj_t *root; - lv_obj_t *grid; - - std::vector buttons; - - uint8_t rows; - uint8_t cols; - }; -} \ No newline at end of file diff --git a/include/devils/display/components/radio.hpp b/include/devils/display/components/radio.hpp deleted file mode 100644 index 2c15ff3..0000000 --- a/include/devils/display/components/radio.hpp +++ /dev/null @@ -1,113 +0,0 @@ -#pragma once - -#include "liblvgl/lvgl.h" -#include -#include -#include - -// radio button component -static lv_style_t style_radio; -static lv_style_t style_radio_chk; - -void set_styles() -{ - lv_style_init(&style_radio); - lv_style_set_radius(&style_radio, LV_RADIUS_CIRCLE); - - lv_style_init(&style_radio_chk); - lv_style_set_bg_img_src(&style_radio_chk, NULL); -} -namespace devils -{ - class Radio - { - public: - Radio(lv_obj_t *parent, const char *label, lv_event_cb_t event_cb = nullptr, bool group = false) - { - // create radio button - radio = lv_checkbox_create(parent); - - lv_checkbox_set_text(radio, label); - if (event_cb != nullptr) - { - lv_obj_add_event_cb(radio, event_cb, LV_EVENT_VALUE_CHANGED, NULL); - } - if (group) - { - lv_obj_add_flag(radio, LV_OBJ_FLAG_EVENT_BUBBLE); - } - else set_styles(); - lv_obj_add_style(radio, &style_radio, LV_PART_INDICATOR); - lv_obj_add_style(radio, &style_radio_chk, LV_PART_INDICATOR | LV_STATE_CHECKED); - } - - - private: - lv_obj_t *radio; - }; - - class RadioGroup - { - public: - RadioGroup(lv_obj_t *parent, const std::vector &labels, void (*event_cb)(std::string) = nullptr) - { - // create radio group - radio_group = lv_obj_create(parent); - this->event_cb = event_cb; - lv_obj_set_layout(radio_group, LV_LAYOUT_FLEX); - lv_obj_set_flex_flow(radio_group, LV_FLEX_FLOW_COLUMN); - - set_styles(); - - for (const auto &label : labels) - { - auto radio = Radio(radio_group, label.c_str(), nullptr, true); - } - // set the first radio button as checked - lv_obj_t *first_radio = lv_obj_get_child(radio_group, 0); - lv_obj_add_state(first_radio, LV_STATE_CHECKED); - - lv_obj_add_event_cb( - radio_group, - handleCheck, - LV_EVENT_VALUE_CHANGED, - this - ); - }; - - lv_obj_t* getRadioGroup() { return radio_group; } - - private: - lv_obj_t *radio_group; - void (*event_cb)(std::string) = nullptr; - uint32_t selected_index = 0; - - static void handleCheck(lv_event_t *e) { - // get instance of the RadioGroup class - RadioGroup *instance = static_cast(lv_event_get_user_data(e)); - lv_obj_t * cont = lv_event_get_current_target(e); - lv_obj_t * act_cb = lv_event_get_target(e); - lv_obj_t * old_cb = lv_obj_get_child(cont, instance->selected_index); - - if (act_cb == cont) return; - - lv_obj_clear_state(old_cb, LV_STATE_CHECKED); /*Uncheck the previous radio button*/ - lv_obj_add_state(act_cb, LV_STATE_CHECKED); /*Uncheck the current radio button*/ - - instance->selected_index = lv_obj_get_index(act_cb); - - const char *text = lv_checkbox_get_text(act_cb); - - - if (instance->event_cb != nullptr) - { - instance->event_cb(text); // call the event callback with the selected radio button text - } - else - { - // default action: print the selected radio button text - printf("Selected radio button: %s\n", text); - } - } - }; -} diff --git a/include/devils/display/devilbotsDisplay.hpp b/include/devils/display/devilbotsDisplay.hpp new file mode 100644 index 0000000..dfef97d --- /dev/null +++ b/include/devils/display/devilbotsDisplay.hpp @@ -0,0 +1,118 @@ +#pragma once + +#include "displayBase.hpp" +#include "../utils/backgroundService.hpp" + +namespace devils +{ + class DevilBotsDisplay : + public BackgroundService, + public DisplayBase + { + public: + DevilBotsDisplay() + { + fullScreenContainer = lv_obj_create(getRootContainer()); + lv_obj_set_style_bg_color(fullScreenContainer, lv_color_make(3, 36, 53), 0); + lv_obj_set_size(fullScreenContainer, lv_pct(100), lv_pct(100)); + + eyesGroup = lv_obj_create(fullScreenContainer); + lv_obj_set_size(eyesGroup, 340, 200); + lv_obj_align(eyesGroup, LV_ALIGN_CENTER, 0, 0); + lv_obj_set_pos(eyesGroup, 0, 0); + lv_obj_set_style_radius(eyesGroup, 0, 0); + lv_obj_set_style_border_width(eyesGroup, 0, 0); + lv_obj_set_style_bg_color(fullScreenContainer, lv_color_make(3, 36, 53), 0); + lv_obj_set_style_bg_opa(eyesGroup, 0, 0); + lv_obj_set_scrollbar_mode(eyesGroup, LV_SCROLLBAR_MODE_OFF); + + leftEye = makeEye(-100, 0, 120); + rightEye = makeEye(100, 0, 120); + leftEyebrow = makeEyebrow(-100, -85, 40, 130); + rightEyebrow = makeEyebrow(100, -110, -40, 130); + } + + ~DevilBotsDisplay() override + { + lv_obj_del(fullScreenContainer); + } + + protected: + void onUpdate() override + { + static int16_t t = 0; + t++; + + lv_obj_set_pos( + eyesGroup, + std::cos(t * 0.03) * 24, + 0); + } + + /** + * Makes an eye object in LVGL with the given position and size and adds it to the eyes group. + * @param x - The x position of the eye relative to the center of the screen. + * @param y - The y position of the eye relative to the center of the screen. + * @param size - The size of the eye in pixels. The eye will be a square with sides of this length. + * @return The created eye object. + */ + lv_obj_t* makeEye( + const int16_t x, + const int16_t y, + const int16_t size) const + { + // Object + lv_obj_t* eye = lv_obj_create(eyesGroup); + lv_obj_set_size(eye, size, size); + lv_obj_align(eye, LV_ALIGN_CENTER, 0, 0); + lv_obj_set_pos(eye, x, y); + + // Style + lv_obj_set_style_bg_color(eye, lv_color_make(255, 255, 255), 0); + lv_obj_set_style_radius(eye, LV_RADIUS_CIRCLE, 0); + lv_obj_set_style_border_width(eye, 0, 0); + + return eye; + } + + /** + * Makes an eyebrow object in LVGL with the given position, angle, and size and adds it to the eyes group. + * @param x - The x position of the eyebrow relative to the center of the screen. + * @param y - The y position of the eyebrow relative to the center of the screen. + * @param angle - The angle of the eyebrow in degrees. 0 is flat, positive is raised, negative is furrowed. + * @param size - The size of the eyebrow in pixels. The eyebrow will be a square with sides of this length. + * @return The created eyebrow object. + */ + lv_obj_t* makeEyebrow( + const int16_t x, + const int16_t y, + const int16_t angle, + const int16_t size) const + { + // Object + lv_obj_t* eyebrow = lv_obj_create(eyesGroup); + lv_obj_set_size(eyebrow, size, size); + + // Style + lv_obj_set_style_bg_color(eyebrow, lv_color_make(3, 36, 53), 0); + lv_obj_set_style_radius(eyebrow, 0, 0); + lv_obj_set_style_border_width(eyebrow, 0, 0); + lv_obj_set_style_transform_pivot_x(eyebrow, size / 2, 0); + lv_obj_set_style_transform_pivot_y(eyebrow, size, 0); + lv_obj_set_style_transform_angle(eyebrow, angle, 0); + + lv_obj_align(eyebrow, LV_ALIGN_CENTER, 0, 0); + lv_obj_set_pos(eyebrow, x, y); + + return eyebrow; + } + + private: + lv_obj_t* fullScreenContainer; + lv_obj_t* eyesGroup; + lv_obj_t* leftEye; + lv_obj_t* rightEye; + lv_obj_t* leftEyebrow; + lv_obj_t* rightEyebrow; + }; +} diff --git a/include/devils/display/displayBase.hpp b/include/devils/display/displayBase.hpp new file mode 100644 index 0000000..f59bc0a --- /dev/null +++ b/include/devils/display/displayBase.hpp @@ -0,0 +1,34 @@ +#pragma once +#include "liblvgl/core/lv_disp.h" +#include "liblvgl/core/lv_obj.h" + +namespace devils +{ + /** + * Base class for all displays. Provides common functionality for all displays, such as a root container to create display elements in. + */ + class DisplayBase + { + public: + virtual ~DisplayBase() = default; + + protected: + /** + * Gets or creates the root container for the display. All display elements should be created as children of this container. + * @return A pointer to the root container for the display. All display elements should be created as children of this container. + */ + static lv_obj_t* getRootContainer() + { + // Create a single root container for all displays to use. + // This ensures that multiple displays can be used at the same time without interfering with each other. + static lv_obj_t* rootContainer = nullptr; + if (rootContainer == nullptr) + { + rootContainer = lv_obj_create(nullptr); + lv_scr_load(rootContainer); + } + + return rootContainer; + } + }; +} diff --git a/include/devils/display/eyesRenderer.hpp b/include/devils/display/eyesRenderer.hpp deleted file mode 100644 index bddfac4..0000000 --- a/include/devils/display/eyesRenderer.hpp +++ /dev/null @@ -1,132 +0,0 @@ -#pragma once - -#include "liblvgl/lvgl.h" - -namespace devils -{ - class EyesRenderer : Runnable - { - public: - EyesRenderer(lv_obj_t *parent) : Runnable(50) - { - this->parent = parent; - - } - - void onUpdate() override - { - static int16_t t = 0; - t++; - - lv_obj_set_pos( - eyesGroup, - std::cos(t * 0.1) * 20, - 0); - } - - void render() { - fullScreenContainer = lv_obj_create(parent); - lv_obj_set_size(fullScreenContainer, lv_pct(100), lv_pct(100)); - eyesGroup = lv_obj_create(fullScreenContainer); - lv_obj_set_size(eyesGroup, 340, 200); - lv_obj_align(eyesGroup, LV_ALIGN_CENTER, 0, 0); - lv_obj_set_pos(eyesGroup, 0, 0); - lv_obj_set_style_radius(eyesGroup, 0, 0); - lv_obj_set_style_border_width(eyesGroup, 0, 0); - lv_obj_set_style_bg_color(fullScreenContainer, lv_color_make(3, 36, 53), 0); - lv_obj_set_style_bg_opa(eyesGroup, 0, 0); - lv_obj_set_scrollbar_mode(eyesGroup, LV_SCROLLBAR_MODE_OFF); - - - leftEye = makeEye(-100, 0, 120); - rightEye = makeEye(100, 0, 120); - leftEyebrow = makeEyebrow(-100, -85, 0, 130); - rightEyebrow = makeEyebrow(100, -110, 0, 130); - - lv_obj_set_style_bg_color(parent, lv_color_make(3, 36, 53), 0); - - lv_obj_add_event_cb(fullScreenContainer, handleDestroy, LV_EVENT_LONG_PRESSED, this); - lv_obj_add_event_cb(eyesGroup, handleDestroy, LV_EVENT_LONG_PRESSED, this); - lv_obj_add_event_cb(leftEye, handleDestroy, LV_EVENT_LONG_PRESSED, this); - lv_obj_add_event_cb(rightEye, handleDestroy, LV_EVENT_LONG_PRESSED, this); - lv_obj_add_event_cb(leftEyebrow, handleDestroy, LV_EVENT_LONG_PRESSED, this); - lv_obj_add_event_cb(rightEyebrow, handleDestroy, LV_EVENT_LONG_PRESSED, this); - - - - runAsync(); - - } - - void destroy() - { - stop(); - lv_obj_del(fullScreenContainer); - } - - private: - lv_obj_t *makeEye(int16_t x, int16_t y, int16_t size) - { - // Object - lv_obj_t *eye = lv_obj_create(eyesGroup); - lv_obj_set_size(eye, size, size); - lv_obj_align(eye, LV_ALIGN_CENTER, 0, 0); - lv_obj_set_pos(eye, x, y); - - // Style - lv_obj_set_style_bg_color(eye, lv_color_make(255, 255, 255), 0); - lv_obj_set_style_radius(eye, LV_RADIUS_CIRCLE, 0); - lv_obj_set_style_border_width(eye, 0, 0); - - return eye; - } - - lv_obj_t *makeEyebrow(int16_t x, int16_t y, int16_t angle, int16_t size) - { - // Object - lv_obj_t *eyebrow = lv_obj_create(eyesGroup); - lv_obj_set_size(eyebrow, size, size); - - // Style - lv_obj_set_style_bg_color(eyebrow, lv_color_make(3, 36, 53), 0); - lv_obj_set_style_radius(eyebrow, 0, 0); - lv_obj_set_style_border_width(eyebrow, 0, 0); - lv_obj_set_style_transform_pivot_x(eyebrow, size / 2, 0); - lv_obj_set_style_transform_pivot_y(eyebrow, size, 0); - lv_obj_set_style_transform_angle(eyebrow, angle, 0); - - lv_obj_align(eyebrow, LV_ALIGN_CENTER, 0, 0); - lv_obj_set_pos(eyebrow, x, y); - - return eyebrow; - } - - static void handleDestroy(lv_event_t *e) - { - // get target object - EyesRenderer *renderer = static_cast(lv_event_get_user_data(e)); - renderer->destroy(); - - - } - - static void removeObject(lv_obj_t *obj) - { - if (obj != nullptr) - { - lv_obj_del(obj); - obj = nullptr; - } - } - - lv_obj_t *parent; - lv_obj_t *fullScreenContainer; - lv_obj_t *eyesGroup; - lv_obj_t *leftEye; - lv_obj_t *rightEye; - lv_obj_t *leftEyebrow; - lv_obj_t *rightEyebrow; - - }; - -} \ No newline at end of file diff --git a/include/devils/display/optionsRenderer.hpp b/include/devils/display/optionsRenderer.hpp deleted file mode 100644 index da833b9..0000000 --- a/include/devils/display/optionsRenderer.hpp +++ /dev/null @@ -1,240 +0,0 @@ -#pragma once - -#include "liblvgl/lvgl.h" -#include "./components/radio.hpp" -#include "./eyesRenderer.hpp" - - -namespace devils -{ - const std::map color_map = { - {RED_ALLIANCE, lv_color_make(255, 0, 0)}, - {BLUE_ALLIANCE, lv_color_make(0, 0, 255)}, - {NONE_ALLIANCE, lv_color_make(124, 124, 124)} - }; - const std::map color_name_map = { - {RED_ALLIANCE, "Red"}, - {BLUE_ALLIANCE, "Blue"}, - {NONE_ALLIANCE, "None"} - }; - - class OptionsRenderer : Runnable - { - public: - OptionsRenderer(const char* bot_name, const std::vector& routines, RobotAutoOptions *options) : Runnable(50) - { - OptionsRenderer::options = options; - OptionsRenderer::routines = routines; - options->routine = routines[0]; - initializeRoot(); - createOptionsDisplayContainer(bot_name, routines); - } - - ~OptionsRenderer() - { - lv_obj_del(root); - } - - private: - static lv_obj_t *root; - - void initializeRoot() - { - root = lv_obj_create(NULL); - eyesRenderer = new EyesRenderer(root); - lv_scr_load(root); - } - - void createOptionsDisplayContainer(const char* bot_name, const std::vector& routines) - { - auto fullscreen_container = lv_obj_create(root); - lv_obj_set_size(fullscreen_container, lv_pct(100), lv_pct(100)); - lv_obj_set_layout(fullscreen_container, LV_LAYOUT_FLEX); - lv_obj_set_flex_flow(fullscreen_container, LV_FLEX_FLOW_COLUMN); - // center - lv_obj_set_flex_align(fullscreen_container, LV_FLEX_ALIGN_CENTER, LV_FLEX_ALIGN_CENTER, LV_FLEX_ALIGN_CENTER); - - lv_obj_t *title = lv_label_create(fullscreen_container); - lv_label_set_text(title, bot_name); - lv_obj_set_style_text_align(title, LV_TEXT_ALIGN_CENTER, 0); - lv_obj_set_style_text_color(title, lv_color_make(255, 255, 255), 0); - lv_obj_set_style_text_font(title, &lv_font_unscii_16, 0); - - auto options_display_container = lv_obj_create(fullscreen_container); - lv_obj_set_style_pad_all(options_display_container, 0, 0); - lv_obj_set_flex_grow(options_display_container, 1); - lv_obj_set_style_border_width(options_display_container, 0, 0); - lv_obj_set_width(options_display_container, lv_pct(100)); - lv_obj_clear_flag(options_display_container, LV_OBJ_FLAG_SCROLLABLE); - lv_obj_set_layout(options_display_container, LV_LAYOUT_FLEX); - lv_obj_set_flex_flow(options_display_container, LV_FLEX_FLOW_ROW); - lv_obj_set_style_pad_row(options_display_container, 0, 0); - - createAllianceColorContainer(options_display_container); - createRoutineContainer(options_display_container, routines); - } - - void createAllianceColorContainer(lv_obj_t *parent) - { - auto alliance_color_container = lv_obj_create(parent); - lv_obj_set_size(alliance_color_container, lv_pct(30), lv_pct(100)); - lv_obj_set_layout(alliance_color_container, LV_LAYOUT_FLEX); - lv_obj_set_flex_flow(alliance_color_container, LV_FLEX_FLOW_COLUMN); - lv_obj_set_flex_align(alliance_color_container, LV_FLEX_ALIGN_CENTER, LV_FLEX_ALIGN_CENTER, LV_FLEX_ALIGN_CENTER); - - lv_obj_t *alliance_title = lv_label_create(alliance_color_container); - lv_label_set_text(alliance_title, "Alliance"); - lv_obj_set_style_text_align(alliance_title, LV_TEXT_ALIGN_CENTER, 0); - lv_obj_set_style_text_color(alliance_title, lv_color_make(255, 255, 255), 0); - lv_obj_set_style_text_font(alliance_title, &lv_font_montserrat_16, 0); - - lv_obj_t *alliance_color_button = lv_btn_create(alliance_color_container); - lv_obj_set_size(alliance_color_button, lv_pct(100), 100); - lv_obj_set_flex_grow(alliance_color_button, 1); - lv_obj_set_style_bg_color(alliance_color_button, color_map.at(options->allianceColor), 0); - lv_obj_add_event_cb(alliance_color_button, handleAllianceColorChange, LV_EVENT_CLICKED, NULL); - - lv_obj_t *alliance_color_label = lv_label_create(alliance_color_button); - lv_label_set_text(alliance_color_label, color_name_map.at(options->allianceColor).c_str()); - lv_obj_center(alliance_color_label); - } - - void createRoutineContainer(lv_obj_t *parent, const std::vector& routines) - { - auto right_container = lv_obj_create(parent); - lv_obj_set_size(right_container, lv_pct(70), lv_pct(100)); - lv_obj_set_layout(right_container, LV_LAYOUT_FLEX); - lv_obj_set_flex_flow(right_container, LV_FLEX_FLOW_ROW); - lv_obj_clear_flag(right_container, LV_OBJ_FLAG_SCROLLABLE); - - auto routine_container = lv_obj_create(right_container); - lv_obj_set_size(routine_container, lv_pct(70), lv_pct(100)); - lv_obj_set_layout(routine_container, LV_LAYOUT_FLEX); - lv_obj_set_flex_flow(routine_container, LV_FLEX_FLOW_COLUMN); - lv_obj_set_style_border_width(routine_container, 0, 0); - lv_obj_set_style_pad_all(routine_container, 0, 0); - - // title - lv_obj_t *routine_title = lv_label_create(routine_container); - lv_label_set_text(routine_title, "Select Routine"); - lv_obj_set_style_text_align(routine_title, LV_TEXT_ALIGN_CENTER, 0); - lv_obj_set_style_text_color(routine_title, lv_color_make(255, 255, 255), 0); - lv_obj_set_style_text_font(routine_title, &lv_font_montserrat_16, 0); - - std::vector routine_names; - for (const auto& routine : routines) - { - routine_names.push_back(routine.displayName); - } - - RadioGroup *radio_group = new RadioGroup(routine_container, routine_names, handleRoutineChange); - lv_obj_t *radio_group_obj = radio_group->getRadioGroup(); - lv_obj_set_width(radio_group_obj, lv_pct(100)); - lv_obj_set_flex_grow(radio_group_obj, 1); - lv_obj_set_style_pad_all(radio_group_obj, 0, 0); - lv_obj_set_style_border_width(radio_group_obj, 0, 0); - - auto screen_saver_toggle_container = lv_obj_create(right_container); - lv_obj_set_size(screen_saver_toggle_container, lv_pct(30), lv_pct(100)); - lv_obj_set_layout(screen_saver_toggle_container, LV_LAYOUT_FLEX); - lv_obj_set_flex_flow(screen_saver_toggle_container, LV_FLEX_FLOW_COLUMN); - lv_obj_set_flex_align(screen_saver_toggle_container, LV_FLEX_ALIGN_END, LV_FLEX_ALIGN_END, LV_FLEX_ALIGN_END); - lv_obj_set_style_pad_all(screen_saver_toggle_container, 0, 0); - lv_obj_set_style_border_width(screen_saver_toggle_container, 0, 0); - - auto screen_saver_toggle_button = lv_btn_create(screen_saver_toggle_container); - lv_obj_set_size(screen_saver_toggle_button, lv_pct(100), 50); - auto screen_saver_toggle_label = lv_label_create(screen_saver_toggle_button); - lv_label_set_text(screen_saver_toggle_label, "Save"); - lv_obj_center(screen_saver_toggle_label); - lv_obj_add_event_cb(screen_saver_toggle_button, handleScreenSaverToggle, LV_EVENT_CLICKED, NULL); - } - - static void handleAllianceColorChange(lv_event_t *e) - { - lv_obj_t *btn = lv_event_get_target(e); - lv_obj_t *label = lv_obj_get_child(btn, NULL); - if (label != NULL) - { - const char *text = lv_label_get_text(label); - AllianceColor current_color = getCurrentAllianceColor(text); - - auto next_color = getNextAllianceColor(current_color); - lv_obj_set_style_bg_color(btn, color_map.at(next_color), 0); - lv_label_set_text(label, color_name_map.at(next_color).c_str()); - options->allianceColor = next_color; - } - } - - static void handleRoutineChange(std::string selected_routine) - { - Routine new_routine; - for (const auto& routine : routines) - { - if (routine.displayName == selected_routine) - { - new_routine = routine; - break; - } - } - options->routine = new_routine; - } - - static void handleScreenSaverToggle(lv_event_t *e) - { - if (options->routine.requiresAllianceColor && options->allianceColor == NONE_ALLIANCE) - { - // show error message - lv_obj_t *message_box = lv_msgbox_create(root, "Error", "Selected routine can't be used when Alliance is set to 'None'.", {}, true); - // center the message box - // shadow - lv_obj_set_style_shadow_color(message_box, lv_color_make(0, 0, 0), 0); - lv_obj_set_style_shadow_width(message_box, 10, 0); - lv_obj_set_style_shadow_opa(message_box, LV_OPA_COVER, 0); - lv_obj_set_style_shadow_ofs_x(message_box, 4, 0); - lv_obj_set_style_shadow_ofs_y(message_box, 4, 0); - // border - - lv_obj_center(message_box); - return; - } - eyesRenderer->render(); - } - - - - static AllianceColor getCurrentAllianceColor(const char *text) - { - for (const auto& pair : color_name_map) - { - if (pair.second == text) - { - return pair.first; - } - } - return NONE_ALLIANCE; - } - - static AllianceColor getNextAllianceColor(AllianceColor current_color) - { - auto it = color_map.find(current_color); - auto next_it = std::next(it); - if (next_it == color_map.end()) - { - next_it = color_map.begin(); - } - return next_it->first; - } - - static RobotAutoOptions *options; - static std::vector routines; - static EyesRenderer *eyesRenderer; - static bool isScreenSaverEnabled; - }; - - RobotAutoOptions *OptionsRenderer::options = nullptr; - EyesRenderer *OptionsRenderer::eyesRenderer = nullptr; - bool OptionsRenderer::isScreenSaverEnabled = false; - std::vector OptionsRenderer::routines = {}; - lv_obj_t *OptionsRenderer::root = nullptr; -} \ No newline at end of file diff --git a/include/devils/display/toastDisplay.hpp b/include/devils/display/toastDisplay.hpp new file mode 100644 index 0000000..e720479 --- /dev/null +++ b/include/devils/display/toastDisplay.hpp @@ -0,0 +1,161 @@ +#pragma once + +#include "displayBase.hpp" +#include "liblvgl/core/lv_obj.h" +#include "liblvgl/extra/widgets/list/lv_list.h" +#include "../utils/backgroundService.hpp" +#include "../utils/logger.hpp" +#include "pros/rtos.hpp" + +namespace devils +{ + class ToastDisplay : + public BackgroundService, + public DisplayBase + { + public: + ToastDisplay() + { + // Load Toast Container + toastContainer = lv_list_create(getRootContainer()); + lv_obj_set_size(toastContainer, OVERLAY_WIDTH, OVERLAY_HEIGHT); + lv_obj_center(toastContainer); + lv_obj_set_style_bg_opa(toastContainer, 0, 0); + lv_obj_set_style_border_opa(toastContainer, 0, 0); + } + + ~ToastDisplay() override + { + lv_obj_del(toastContainer); + } + + protected: + /// @brief Represents a single error popup being displayed on the screen. + struct ActiveToast + { + uint32_t timestamp; + std::string text; + lv_obj_t* toastObject; + }; + + /** + * Creates a toast object in LVGL with the given text and adds it to the toast container. + * @param logMessage - The log message to create a toast for. The text of the log message will be displayed in the toast. + * @return The created toast object. + */ + lv_obj_t* createToast(const Logger::LogMessage& logMessage) const + { + // Get proper icon and color for the toast based on the log level of the message + const auto toastIcon = getIconFromLogLevel(logMessage.level); + const auto toastColor = getColorFromLogLevel(logMessage.level); + + // Create the toast as a button + const auto button = lv_list_add_btn( + toastContainer, + toastIcon.c_str(), + logMessage.text.c_str()); + lv_obj_set_style_bg_color(button, toastColor, 0); + + return button; + } + + void onUpdate() override + { + // Log any new log messages as toasts + while (!Logger::toastBuffer.empty()) + { + onLogMessage(Logger::toastBuffer.top()); + Logger::toastBuffer.pop(); + } + } + + /** + * Called every time a log message is logged. + * If the log message is an error, it creates a toast for the error message and adds it to the screen. + * @param logMessage - The log message to display as a toast if it's an error message. + */ + void onLogMessage(const Logger::LogMessage& logMessage) + { + // Ignore messages that aren't at least the current log level + if (logMessage.level < logLevel) + return; + + // Check if the error is already being displayed + for (auto& toast : activeToasts) + { + if (toast.text == logMessage.text) + { + // Update the timestamp to keep it on screen longer + toast.timestamp = pros::millis(); + return; + } + } + + // Add the toast to the list of active toasts + activeToasts.push_back({ + pros::millis(), + logMessage.text, + createToast(logMessage) + }); + } + + /** + * Sets the log level for the toast display. Only log messages at least this severe will be displayed as toasts. + * @param newLogLevel - The new log level to set for the toast display. Only log messages at least this severe will be displayed as toasts. + */ + void setLogLevel(const Logger::LogLevel newLogLevel) + { + logLevel = newLogLevel; + } + + /** + * Takes the log level of a message and returns the icon to use for a toast based on the log level of the message. + * @param logLevel - The log level of the message to get the icon for. + * @return The icon to use for a toast based on the log level of the message. Errors are X's, warnings are !'s, etc. + */ + static std::string getIconFromLogLevel(const Logger::LogLevel logLevel) + { + switch (logLevel) + { + case Logger::ERROR: + return LV_SYMBOL_CLOSE; + case Logger::WARN: + return LV_SYMBOL_WARNING; + case Logger::INFO: + return LV_SYMBOL_PLUS; + default: + return LV_SYMBOL_MINUS; + } + } + + /** + * Gets the color to use for a toast based on the log level of the message. + * Errors are red, warnings are yellow, etc. + * @param logLevel - The log level of the message to get the color for. + * @return The color to use for a toast based on the log level of the message. + */ + static lv_color_t getColorFromLogLevel(const Logger::LogLevel logLevel) + { + switch (logLevel) + { + case Logger::ERROR: + return lv_color_hex(0xE94E4E); + case Logger::WARN: + return lv_color_hex(0xE6BA01); + case Logger::INFO: + return lv_color_hex(0x078BEB); + default: + return lv_color_hex(0xA8A8A8); + } + } + + private: + static constexpr int OVERLAY_WIDTH = 440; + static constexpr int OVERLAY_HEIGHT = 200; + + Logger::LogLevel logLevel = Logger::WARN; + lv_obj_t* toastContainer; + + std::vector activeToasts = {}; + }; +}; diff --git a/include/devils/geometry/grid.hpp b/include/devils/geometry/grid.hpp deleted file mode 100644 index 8b74040..0000000 --- a/include/devils/geometry/grid.hpp +++ /dev/null @@ -1,75 +0,0 @@ -#pragma once -#include - -namespace devils -{ - /** - * A struct representing a grid of traversable cells - */ - template - struct Grid - { - /** - * Initializes an empty 0x0 grid - */ - Grid() {} - - /** - * Initializes a grid with a width and height - * @param width - Width of the grid in cells - * @param height - Height of the grid in cells - */ - Grid(int width, int height) : width(width), - height(height), - values(width * height) - { - } - - /** - * Initializes a grid with a default value at every cell - * @param width - Width of the grid in cells - * @param height - Height of the grid in cells - * @param defaultValue - The default value to fill every cell of the grid - */ - Grid(int width, int height, T defaultValue) : width(width), - height(height), - values(width * height) - { - for (int i = 0; i < values.size(); i++) - values[i] = defaultValue; - } - - /// @brief Width of the grid in cells - int width = 0; - - /// @brief Height of the grid in cells - int height = 0; - - /// @brief 2D array of grid cells. Every `width` indices is a new row. - std::vector values; - - /** - * Gets the value of a cell at the given position - * @param x - X position in cells - * @param y - Y position in cells - * @param defaultValue - The default value if dimensions are out of bounds - * @returns The value at the given cell - */ - T getCell(int x, int y, T defaultValue) - { - // Get the index of the vector - int index = height * x + y; - - // Check for OOB - if (x < 0 || x >= width) - return defaultValue; - if (y < 0 || y >= height) - return defaultValue; - if (index < 0 || index >= values.size()) - return defaultValue; - - // Return the cooresponding value - return values.at(index); - } - }; -} \ No newline at end of file diff --git a/include/devils/geometry/lerp.hpp b/include/devils/geometry/lerp.hpp index cc04417..1cf5a89 100644 --- a/include/devils/geometry/lerp.hpp +++ b/include/devils/geometry/lerp.hpp @@ -5,8 +5,13 @@ namespace devils { + /** + * A collection of interpolation utilities. + */ struct Lerp { + Lerp() = delete; + /** * Linearly interpolates a value from a to b. * @param a The minimum value. @@ -14,7 +19,10 @@ namespace devils * @param t The ratio between a and b. Values between 0 and 1. * @return The interpolated value. */ - static double lerp(double a, double b, double t) + static float lerp( + const float a, + const float b, + const float t) { return a + (b - a) * t; } @@ -25,21 +33,20 @@ namespace devils * @param b The maximum value in radians. * @param t The ratio between a and b. Values between 0 and 1. */ - static double rotation(const double a, const double b, const double t) + static float rotation( + const float a, + const float b, + const float t) { - double aMod = std::fmod(a, 2 * M_PI); - double bMod = std::fmod(b, 2 * M_PI); - double diff = std::abs(aMod - bMod); - if (diff > M_PI) + const auto aMod = fmodf(a, 2.0f * M_PIF); + const auto bMod = fmodf(b, 2.0f * M_PIF); + const auto diff = std::abs(aMod - bMod); + if (diff > M_PIF) { if (aMod > bMod) - { - return Units::normalizeRadians(lerp(aMod, bMod + 2 * M_PI, t)); - } - else - { - return Units::normalizeRadians(lerp(aMod + 2 * M_PI, bMod, t)); - } + return Units::normalizeRadians(lerp(aMod, bMod + 2.0f * M_PIF, t)); + + return Units::normalizeRadians(lerp(aMod + 2.0f * M_PIF, bMod, t)); } return Units::normalizeRadians(lerp(aMod, bMod, t)); } @@ -51,12 +58,16 @@ namespace devils * @param t The ratio between a and b. Values between 0 and 1. * @return The interpolated point. */ - static Pose linearPoints(const Pose &a, const Pose &b, const double t) + static Pose linearPoints( + const Pose& a, + const Pose& b, + const float t) { - return Pose( + return { lerp(a.x, b.x, t), lerp(a.y, b.y, t), - rotation(a.rotation, b.rotation, t)); + rotation(a.rotation, b.rotation, t) + }; } /** @@ -67,10 +78,14 @@ namespace devils * @param t The ratio between a and c. Values between 0 and 1. * @return The interpolated point. */ - static Pose quadraticPoints(Pose &a, Pose &b, Pose &c, double t) + static Pose quadraticPoints( + const Pose& a, + const Pose& b, + const Pose& c, + const float t) { - Pose ab = linearPoints(a, b, t); - Pose bc = linearPoints(b, c, t); + const Pose ab = linearPoints(a, b, t); + const Pose bc = linearPoints(b, c, t); return linearPoints(ab, bc, t); } @@ -83,14 +98,16 @@ namespace devils * @param t The ratio between a and d. Values between 0 and 1. * @return The interpolated point. */ - static Pose cubicPoints(Pose &a, Pose &b, Pose &c, Pose &d, double t) + static Pose cubicPoints( + const Pose& a, + const Pose& b, + const Pose& c, + const Pose& d, + const float t) { - Pose abc = quadraticPoints(a, b, c, t); - Pose bcd = quadraticPoints(b, c, d, t); + const Pose abc = quadraticPoints(a, b, c, t); + const Pose bcd = quadraticPoints(b, c, d, t); return linearPoints(abc, bcd, t); } - - private: - Lerp() = delete; }; -} \ No newline at end of file +} diff --git a/include/devils/geometry/math.hpp b/include/devils/geometry/math.hpp new file mode 100644 index 0000000..0d27305 --- /dev/null +++ b/include/devils/geometry/math.hpp @@ -0,0 +1,144 @@ +#pragma once + +#include +#include +#include "vector2.hpp" + +#ifndef M_PIF +#define M_PIF 3.141592653589793238462643383279502884e+00F +#endif + +namespace devils +{ + /** + * Collection of utility math functions. + */ + struct Math + { + // Delete constructor to prevent instantiation + Math() = delete; + + /** + * Modulus function that works with negative numbers. + * For example, -1 % 3 = 2 and -1 % -3 = 1. + * @param a The dividend. + * @param b The divisor. + * @return The remainder. + */ + static float signedMod( + const float a, + const float b) + { + return a - b * std::floor(a / b); + } + + /** + * Clamps a value between a minimum and maximum. + * Allows for a deadband around zero such that negative values are clamped to -min and positive values are clamped to +min. + * @param value The value to clamp. + * @param min The minimum value. + * @param max The maximum value. + * @return The clamped value. + */ + static float deadbandClamp( + const float value, + const float min, + const float max) + { + if (value > 0) + return std::clamp(value, min, max); + if (value < 0) + return std::clamp(value, -max, -min); + + return 0; + } + + /** + * Calculates the end velocity over a specified distance given initial velocity and acceleration. + * @param initialVelocity The initial velocity in units per second. + * @param acceleration Constant acceleration in units per second squared. + * @param distance The distance in units. + */ + static float velocityOverDist( + const float initialVelocity, + const float acceleration, + const float distance) + { + return sqrtf( + initialVelocity * initialVelocity + + 2 * acceleration * distance); + } + + /** + * Calculates the acceleration over a specified distance given initial and final velocities. + * @param initialVelocity The initial velocity in units per second. + * @param finalVelocity The final velocity in units per second. + * @param distance The distance in units. + */ + static float accelOverDist( + const float initialVelocity, + const float finalVelocity, + const float distance) + { + // (v_f^2 - v_i^2) / 2d + return (finalVelocity * finalVelocity - initialVelocity * initialVelocity) / (2 * distance); + } + + /** + * Calculates the distance between b and c along the line defined by a and b. + * @param a The first point on the line. + * @param b The second point on the line. Also, the point to measure from. + * @param c The point to measure to. + * @return The distance between b and c along the line defined by a and b. + */ + static float distanceOnLine( + const Vector2& a, + const Vector2& b, + const Vector2& c) + { + const float numerator = (c.x - a.x) * (b.x - a.x) + (c.y - a.y) * (b.y - a.y); + const float denominator = (b.x - a.x) * (b.x - a.x) + (b.y - a.y) * (b.y - a.y); + const float t = numerator / denominator; + + const float x = b.x - ((1 - t) * a.x + t * b.x); + const float y = b.y - ((1 - t) * a.y + t * b.y); + return sqrtf(x * x + y * y); + } + + /** + * Chooses the value with the largest magnitude from a list of values. + * @param values - A list of values to find the largest magnitude of. + * @return The value with the largest magnitude in the list. + */ + static float largestMagnitude(const std::initializer_list& values) + { + float largest = 0; + for (const auto& value : values) + { + if (std::abs(value) > std::abs(largest)) + largest = value; + } + return largest; + } + + /** + * Chooses the value with the smallest magnitude from a list of values. + * @param values - A list of values to find the smallest magnitude of. + * @return The value with the smallest magnitude in the list. + */ + static float smallestMagnitude(const std::initializer_list& values) + { + // Handle empty list case + if (values.size() == 0) + return 0; + + float smallest = *values.begin(); + for (const auto& value : values) + { + if (std::abs(value) < std::abs(smallest)) + smallest = value; + } + return smallest; + } + }; +} diff --git a/include/devils/geometry/polygon.hpp b/include/devils/geometry/polygon.hpp deleted file mode 100644 index 3abfd82..0000000 --- a/include/devils/geometry/polygon.hpp +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" -#include "../utils/logger.hpp" -#include "pose.hpp" - -namespace devils -{ - /** - * Represents a polygonal area. - */ - struct Polygon - { - /** - * Creates a new polygon. - * @param points The points of the polygon. - */ - Polygon(std::initializer_list points) - : points(points) - { - } - - /** - * Return true if the odometry pose is within the polygon. - * @param pose The odometry pose to check. - * @return True if the odometry pose is within the polygon. - */ - bool contains(Vector2 &pose) - { - int i, j; - bool c = false; - for (i = 0, j = points.size() - 1; i < points.size(); j = i++) - { - if (((points[i].y > pose.y) != (points[j].y > pose.y)) && - (pose.x < (points[j].x - points[i].x) * (pose.y - points[i].y) / (points[j].y - points[i].y) + points[i].x)) - c = !c; - } - return c; - } - - /** - * Gets a random point within the polygon. - * @return A random point within the polygon. - */ - Vector2 getRandomPose() - { - // If the polygon is empty, return an empty pose - if (points.size() == 0) - return Vector2(); - - // Calculate the bounding box of the polygon - double minX = points[0].x; - double maxX = points[0].x; - double minY = points[0].y; - double maxY = points[0].y; - for (auto &point : points) - { - minX = std::min(minX, point.x); - maxX = std::max(maxX, point.x); - minY = std::min(minY, point.y); - maxY = std::max(maxY, point.y); - } - - // Randomize Seed - srand(pros::millis()); - - // Randomly select a point within the bounding box until it is within the polygon - Vector2 pose = Vector2(); - do - { - double rand1 = (rand() % 1000) / 1000.0; - double rand2 = (rand() % 1000) / 1000.0; - double x = minX + (maxX - minX) * rand1; - double y = minY + (maxY - minY) * rand2; - pose = Vector2(x, y); - } while (!contains(pose)); - - return pose; - } - - std::vector points; - }; -} \ No newline at end of file diff --git a/include/devils/geometry/pose.hpp b/include/devils/geometry/pose.hpp index 6470633..530db34 100644 --- a/include/devils/geometry/pose.hpp +++ b/include/devils/geometry/pose.hpp @@ -1,6 +1,6 @@ #pragma once + #include -#include #include "vector2.hpp" namespace devils @@ -8,28 +8,32 @@ namespace devils /** * Represents a pose in 2D space. */ - struct Pose : public Vector2 + struct Pose : Vector2 { /// @brief The x position of the robot in inches - double &x = Vector2::x; + float& x = Vector2::x; /// @brief The y position of the robot in inches - double &y = Vector2::y; + float& y = Vector2::y; /// @brief The rotation of the robot in radians - double rotation = 0; + float rotation = 0; /** * Constructs a pose with all values set to 0 */ - Pose() : rotation(0) {} + Pose() = default; /** * Constructs a pose with the given x and y * @param x The x position of the robot in inches * @param y The y position of the robot in inches */ - Pose(double x, double y) : Vector2(x, y), rotation(0) {} + Pose( + const float x, + const float y) : Vector2(x, y) + { + } /** * Constructs a pose with the given x, y, and rotation @@ -37,25 +41,36 @@ namespace devils * @param y The y position of the robot in inches * @param rotation The rotation of the robot in radians */ - Pose(double x, double y, double rotation) : Vector2(x, y), rotation(rotation) {} + Pose( + const float x, + const float y, + const float rotation + ) : Vector2(x, y), + rotation(rotation) + { + } /** * Constructs a pose by copying another vector. Sets rotation to 0. * @param other The other vector */ - Pose(const Vector2 &other) : Vector2(other.x, other.y), rotation(0) {} + Pose(const Vector2& other) : Vector2(other.x, other.y) + { + } /** * Copy constructor * @param other The other pose */ - Pose(const Pose &other) : Vector2(other.x, other.y), rotation(other.rotation) {} + Pose(const Pose& other) : Vector2(other.x, other.y), rotation(other.rotation) + { + } /** * Constructs a pose by copying another pose * @param other The other pose */ - Pose operator=(const Pose &other) + Pose& operator=(const Pose& other) { x = other.x; y = other.y; @@ -68,7 +83,7 @@ namespace devils * @param other The other pose * @return The sum of the two poses */ - Pose operator+(const Pose &other) + Pose operator+(const Pose& other) const { return {x + other.x, y + other.y, rotation + other.rotation}; } @@ -78,7 +93,7 @@ namespace devils * @param other The other pose * @return The difference of the two poses */ - Pose operator-(const Pose &other) + Pose operator-(const Pose& other) const { return {x - other.x, y - other.y, rotation - other.rotation}; } @@ -88,7 +103,7 @@ namespace devils * @param scalar The scalar to multiply by * @return The pose multiplied by the scalar */ - Pose operator*(const double &scalar) + Pose operator*(const float& scalar) const { return {x * scalar, y * scalar, rotation * scalar}; } @@ -98,7 +113,7 @@ namespace devils * @param other The other pose * @return True if the poses are equal, false otherwise */ - bool operator==(const Pose &other) + bool operator==(const Pose& other) const { return x == other.x && y == other.y && rotation == other.rotation; } @@ -108,7 +123,7 @@ namespace devils * @param other The other pose * @return True if the poses are not equal, false otherwise */ - bool operator!=(const Pose &other) + bool operator!=(const Pose& other) const { return !(*this == other); } @@ -119,7 +134,7 @@ namespace devils */ Pose normalize() const { - double mag = magnitude(); + const float mag = magnitude(); return {x / mag, y / mag, rotation}; } @@ -131,10 +146,41 @@ namespace devils { return "Pose(" + std::to_string(x) + ", " + std::to_string(y) + ", " + std::to_string(rotation) + ")"; } + + /** + * Checks if this pose is behind another pose. + * This is determined by taking the dot product of the current pose's direction vector and the vector from the current pose to the other pose. If the dot product is negative, then the other pose is behind the current pose. + * @param otherPose - The other pose to compare to + * @return True if the other pose is behind this pose, false otherwise + */ + bool isBehind(const Pose& otherPose) const + { + const float dotProduct = cosf(rotation) * (otherPose.x - x) + + sinf(rotation) * (otherPose.y - y); + return dotProduct > 0; + } + + /** + * Calculates the curvature between this pose and another pose. + * Curvature is defined as the change in rotation divided by the distance between the two poses. + * A higher curvature indicates a sharper turn, while a lower curvature indicates a gentler turn. + * Negative curvature indicates a turn to the left, while positive curvature indicates a turn to the right. + * @param otherPose - The other pose to compare to + * @return The curvature between this pose and the other pose (measured in radians per inch) + */ + float curvature(const Pose& otherPose) const + { + const float distance = distanceTo(otherPose); + if (distance == 0) + return 0; // Avoid division by zero + + const float angleDifference = otherPose.rotation - rotation; + return angleDifference / distance; + } }; /** * A list of poses to play in sequence */ typedef std::vector PoseSequence; -} \ No newline at end of file +} diff --git a/include/devils/geometry/units.hpp b/include/devils/geometry/units.hpp index f262b9f..88555a3 100644 --- a/include/devils/geometry/units.hpp +++ b/include/devils/geometry/units.hpp @@ -1,9 +1,7 @@ #pragma once +#include #include - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif +#include "math.hpp" namespace devils { @@ -18,9 +16,9 @@ namespace devils * @param inches The inches to convert. * @return The inches in meters. */ - static inline double inToMeters(double inches) + static float inToMeters(const float inches) { - return inches * 0.0254; + return inches * 0.0254f; } /** @@ -28,97 +26,115 @@ namespace devils * @param meters The meters to convert. * @return The meters in inches. */ - static inline double metersToIn(double meters) + static float metersToIn(const float meters) { - return meters / 0.0254; + return meters / 0.0254f; } /** - * Compares the difference of two radian angles. - * @param radiansA The first angle in radians. - * @param radiansB The second angle in radians. - * @return The difference between the two angles, relative to `radiansB`. Can be [-PI, PI] + * Converts degrees to radians. + * @param degrees The degrees to convert. + * @return The degrees in radians. */ - static double diffRad(double radiansA, double radiansB) + static float degToRad(const float degrees) { - return std::atan2(std::sin(radiansA - radiansB), std::cos(radiansA - radiansB)); + return degrees * (M_PIF / 180.0f); } /** - * Converts degrees to radians. - * @param degrees The degrees to convert. - * @return The degrees in radians. + * Converts radians to degrees. + * @param radians The radians to convert. */ - static inline double degToRad(double degrees) + static float radToDeg(const float radians) { - return degrees * (M_PI / 180.0); + return radians * (180.0f / M_PIF); } - + /** * Converts centidegrees to radians. * @param centidegrees The centidegrees to convert. * @return The centidegrees in radians. */ - static inline double centidegToRad(double centidegrees) + static float centidegToRad(const float centidegrees) { - return centidegrees * (M_PI / 18000.0); + return centidegrees * (M_PIF / 18000.0f); } /** - * Modulus function that also works with negative numbers. - * @param a The number to mod. - * @param b The modulus. - * @return The result of the modulus. Always positive. + * Converts radians to centidegrees. + * @param radians - The radians to convert. + * @return The radians in centidegrees. */ - static double mod(double a, double b) + static float radToCentideg(const float radians) { - return a - std::floor(a / b) * b; + return radians * (18000.0f / M_PIF); } /** - * Converts radians to degrees. - * @param radians The radians to convert. + * Converts centidegrees to degrees. + * @param centidegrees The centidegrees to convert. + * @return The centidegrees in degrees. */ - static inline double radToDeg(double radians) + static float centidegToDeg(const float centidegrees) { - return radians * (180.0 / M_PI); + return centidegrees / 100.0f; } /** - * Normalizes an angle in radians to be between 0 and 2 * PI. - * @param radians The angle in radians to normalize. - * @return The normalized angle in radians. + * Converts degrees to centidegrees. + * @param degrees The degrees to convert. + * @return The degrees in centidegrees. */ - static double normalizeRadians(const double radians) + static float degToCentideg(const float degrees) { - return mod(radians, 2 * M_PI); + return degrees * 100.0f; } - + /** - * Converts inches to a string. - * @param inches The inches to convert. - * @return The string representation of the inches and feet + * Compares the difference of two radian angles. + * @param radiansA The first angle in radians. + * @param radiansB The second angle in radians. + * @return The difference between the two angles, relative to `radiansB`. Can be [-PI, PI] */ - static std::string inToString(int inches) + static float diffRad(const float radiansA, const float radiansB) { - // Handle Negative - bool negative = inches < 0; - if (negative) - inches = -inches; - - // Split Inches and Feet - int feet = inches / 12; - inches = inches % 12; - - // Create String - std::stringstream stream; - if (negative) - stream << "-"; - if (feet > 0) - stream << feet << "' "; - stream << inches << "\""; + return std::atan2( + std::sin(radiansA - radiansB), + std::cos(radiansA - radiansB)); + } - return stream.str(); + /** + * Compares the difference of two degree angles. + * @param degreesA - The first angle in degrees. + * @param degreesB - The second angle in degrees. + * @return The difference between the two angles, relative to `degreesB`. Can be [-180, 180] + */ + static float diffDeg(const float degreesA, const float degreesB) + { + return radToDeg(diffRad( + degToRad(degreesA), + degToRad(degreesB))); + } + + /** + * Modulus function that also works with negative numbers. + * @param a The number to mod. + * @param b The modulus. + * @return The result of the modulus. Always positive. + */ + static float mod(const float a, const float b) + { + return a - std::floor(a / b) * b; + } + + /** + * Normalizes an angle in radians to be between 0 and 2 * PI. + * @param radians The angle in radians to normalize. + * @return The normalized angle in radians. + */ + static float normalizeRadians(const float radians) + { + return mod(radians, 2.0f * M_PIF); } }; -} \ No newline at end of file +} diff --git a/include/devils/geometry/vector2.hpp b/include/devils/geometry/vector2.hpp index 0759434..39c5599 100644 --- a/include/devils/geometry/vector2.hpp +++ b/include/devils/geometry/vector2.hpp @@ -10,45 +10,42 @@ namespace devils struct Vector2 { /// @brief The x position. - double x = 0; + float x = 0; /// @brief The y position. - double y = 0; + float y = 0; /** * Constructs a 3D vector with all values set to 0 */ - Vector2() : x(0), y(0) {} + Vector2() = default; /** * Constructs a vector with the given x, and y * @param x The x position * @param y The y position */ - Vector2(double x, double y) : x(x), y(y) {} + Vector2(const float x, const float y) : x(x), y(y) + { + } /** * Copy constructor * @param other The other vector */ - Vector2(const Vector2 &other) : x(other.x), y(other.y) {} + Vector2(const Vector2& other) = default; /** * Constructs a vector by copying another vector * @param other The other vector */ - Vector2 operator=(const Vector2 &other) - { - x = other.x; - y = other.y; - return *this; - } + Vector2& operator=(const Vector2& other) = default; /** * Adds two vectors together * @param other The other vector * @return The sum of the two vectors */ - Vector2 operator+(const Vector2 &other) + Vector2 operator+(const Vector2& other) const { return {x + other.x, y + other.y}; } @@ -58,7 +55,7 @@ namespace devils * @param other The other vector * @return The difference of the two vectors */ - Vector2 operator-(const Vector2 &other) + Vector2 operator-(const Vector2& other) const { return {x - other.x, y - other.y}; } @@ -68,7 +65,7 @@ namespace devils * @param scalar The scalar to multiply by * @return The vector multiplied by the scalar */ - Vector2 operator*(const double &scalar) + Vector2 operator*(const float& scalar) const { return {x * scalar, y * scalar}; } @@ -78,7 +75,7 @@ namespace devils * @param other The other vector * @return True if the vectors are equal, false otherwise */ - bool operator==(const Vector2 &other) + bool operator==(const Vector2& other) const { return x == other.x && y == other.y; } @@ -88,7 +85,7 @@ namespace devils * @param other The other vector * @return True if the vectors are not equal, false otherwise */ - bool operator!=(const Vector2 &other) + bool operator!=(const Vector2& other) const { return !(*this == other); } @@ -98,7 +95,7 @@ namespace devils * @param other The other vector * @return The dot product of the two vectors */ - double dot(const Vector2 &other) const + float dot(const Vector2& other) const { return x * other.x + y * other.y; } @@ -108,18 +105,25 @@ namespace devils * @param other The other vector * @return The distance between the two vectors */ - double distanceTo(const Vector2 &other) const + float distanceTo(const Vector2& other) const { - return std::sqrt(std::pow(other.x - x, 2) + std::pow(other.y - y, 2)); + const float deltaX = x - other.x; + const float deltaY = y - other.y; + + return std::sqrt( + deltaX * deltaX + + deltaY * deltaY); } /** * Calculates the magnitude of the vector * @return The magnitude of the vector */ - double magnitude() const + float magnitude() const { - return std::sqrt(std::pow(x, 2) + std::pow(y, 2)); + return std::sqrt( + x * x + + y * y); } /** @@ -128,7 +132,7 @@ namespace devils */ Vector2 normalize() const { - double mag = magnitude(); + const float mag = magnitude(); return {x / mag, y / mag}; } @@ -141,4 +145,4 @@ namespace devils return "(" + std::to_string(x) + ", " + std::to_string(y) + ")"; } }; -} \ No newline at end of file +} diff --git a/include/devils/geometry/vector3.hpp b/include/devils/geometry/vector3.hpp index 3234298..39c38a1 100644 --- a/include/devils/geometry/vector3.hpp +++ b/include/devils/geometry/vector3.hpp @@ -1,4 +1,5 @@ #pragma once + #include #include @@ -10,23 +11,25 @@ namespace devils struct Vector3 { /// @brief The x position. - double x = 0; + float x = 0; /// @brief The y position. - double y = 0; + float y = 0; /// @brief The z position. - double z = 0; + float z = 0; /** * Constructs a 3D vector with all values set to 0 */ - Vector3() : x(0), y(0), z(0) {} + Vector3() = default; /** * Constructs a vector with the given x, and y * @param x The x position * @param y The y position */ - Vector3(double x, double y) : x(x), y(y), z(0) {} + Vector3(const float x, const float y) : x(x), y(y) + { + } /** * Constructs a vector with the given x, y, and z @@ -34,32 +37,28 @@ namespace devils * @param y The y position * @param z The z position */ - Vector3(double x, double y, double z) : x(x), y(y), z(z) {} + Vector3(const float x, const float y, const float z) : x(x), y(y), z(z) + { + } /** * Copy constructor * @param other The other vector */ - Vector3(const Vector3 &other) : x(other.x), y(other.y), z(other.z) {} + Vector3(const Vector3& other) = default; /** * Constructs a vector by copying another vector * @param other The other vector */ - Vector3 operator=(const Vector3 &other) - { - x = other.x; - y = other.y; - z = other.z; - return *this; - } + Vector3& operator=(const Vector3& other) = default; /** * Adds two vectors together * @param other The other vector * @return The sum of the two vectors */ - Vector3 operator+(const Vector3 &other) + Vector3 operator+(const Vector3& other) const { return {x + other.x, y + other.y, z + other.z}; } @@ -69,7 +68,7 @@ namespace devils * @param other The other vector * @return The difference of the two vectors */ - Vector3 operator-(const Vector3 &other) + Vector3 operator-(const Vector3& other) const { return {x - other.x, y - other.y, z - other.z}; } @@ -79,7 +78,7 @@ namespace devils * @param scalar The scalar to multiply by * @return The vector multiplied by the scalar */ - Vector3 operator*(const double &scalar) + Vector3 operator*(const float& scalar) const { return {x * scalar, y * scalar, z * scalar}; } @@ -89,7 +88,7 @@ namespace devils * @param other The other vector * @return True if the vectors are equal, false otherwise */ - bool operator==(const Vector3 &other) + bool operator==(const Vector3& other) const { return x == other.x && y == other.y && z == other.z; } @@ -99,7 +98,7 @@ namespace devils * @param other The other vector * @return True if the vectors are not equal, false otherwise */ - bool operator!=(const Vector3 &other) + bool operator!=(const Vector3& other) const { return !(*this == other); } @@ -109,7 +108,7 @@ namespace devils * @param other The other vector * @return The dot product of the two vectors */ - double dot(const Vector3 &other) + float dot(const Vector3& other) const { return x * other.x + y * other.y + z * other.z; } @@ -119,27 +118,37 @@ namespace devils * @param other The other vector * @return The distance between the two vectors */ - double distanceTo(const Vector3 &other) + float distanceTo(const Vector3& other) const { - return std::sqrt(std::pow(other.x - x, 2) + std::pow(other.y - y, 2) + std::pow(other.z - z, 2)); + const float deltaX = other.x - x; + const float deltaY = other.y - y; + const float deltaZ = other.z - z; + + return std::sqrt( + deltaX * deltaX + + deltaY * deltaY + + deltaZ * deltaZ); } /** * Calculates the magnitude of the vector * @return The magnitude of the vector */ - double magnitude() + float magnitude() const { - return std::sqrt(std::pow(x, 2) + std::pow(y, 2) + std::pow(z, 2)); + return std::sqrt( + x * x + + y * y + + z * z); } /** * Normalizes the vector * @return The normalized vector */ - Vector3 normalize() + Vector3 normalize() const { - double mag = magnitude(); + const float mag = magnitude(); return {x / mag, y / mag, z / mag}; } @@ -147,9 +156,9 @@ namespace devils * Prints the vector to a string * @return The vector as a string */ - const std::string toString() + std::string toString() const { return "(" + std::to_string(x) + ", " + std::to_string(y) + ", " + std::to_string(z) + ")"; } }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/adiAnalogInput.hpp b/include/devils/hardware/adiAnalogInput.hpp new file mode 100644 index 0000000..ceefbf6 --- /dev/null +++ b/include/devils/hardware/adiAnalogInput.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include "pros/adi.hpp" +#include "hardwareBase.hpp" +#include + +namespace devils +{ + /** + * Represents an analog (0% - 100%) input from the ADI ports. + */ + class ADIAnalogInput : ADIHardwareBase + { + public: + /** + * Creates a new digital input controlled by an ADI port. + * @param name The name of the input (for logging purposes) + * @param port The ADI port (from 'A' to 'H'). + * @param isInverted True if the input should be inverted, false otherwise. + */ + ADIAnalogInput( + const std::string& name, + const char port, + const bool isInverted = false) + : ADIHardwareBase(name, "ADIAnalogInput", port), + isInverted(isInverted) + { + // ADI Ports need to be configured before they can be used + executeWithErrorCheck(pros::c::adi_port_set_config, port, pros::E_ADI_ANALOG_IN); + } + + /** + * Gets the state of the analog input. + * @return 1.0f if the input is active, 0.0f if it is not. If the input is inverted, the values are flipped. + */ + HWResult getValue() + { + // Read the value from the ADI port + const auto result = executeWithErrorCheck(pros::c::adi_analog_read, port); + if (!result.isSuccess()) return result.status; + + // Invert the value if necessary + auto value = static_cast(result.value) / MAX_ANALOG_VALUE; + if (isInverted) + value = 1.0f - value; + + return value; + } + + private: + constexpr static int32_t MAX_ANALOG_VALUE = 4095; // 12-bit maximum value + + bool isInverted = false; + }; +} diff --git a/include/devils/hardware/adiDigitalInput.hpp b/include/devils/hardware/adiDigitalInput.hpp index f4760e1..c668d8e 100644 --- a/include/devils/hardware/adiDigitalInput.hpp +++ b/include/devils/hardware/adiDigitalInput.hpp @@ -1,46 +1,54 @@ #pragma once + #include "pros/adi.hpp" -#include "../utils/logger.hpp" -#include "structs/hardwareBase.hpp" +#include "hardwareBase.hpp" #include +#include namespace devils { /** * Represents a digital (on/off) input from the ADI ports. */ - class ADIDigitalInput : private HardwareBase + class ADIDigitalInput : ADIHardwareBase { public: /** * Creates a new digital input controlled by an ADI port. * @param name The name of the input (for logging purposes) - * @param port The ADI port of the motor controller (from 1 to 8) + * @param port The ADI port (from 'A' to 'H'). + * @param isInverted True if the input should be inverted, false otherwise. */ - ADIDigitalInput(std::string name, int8_t port) - : HardwareBase(name, "ADIDigitalInput", port), - controller(abs(port)) + ADIDigitalInput( + const std::string& name, + const char port, + const bool isInverted = false) + : ADIHardwareBase(name, "ADIDigitalInput", port), + isInverted(isInverted) { - isInverted = port < 0; - if (errno != 0) - reportFault("ADI port is invalid"); + // ADI Ports need to be configured before they can be used + executeWithErrorCheck(pros::c::adi_port_set_config, port, pros::E_ADI_DIGITAL_IN); } /** * Gets the state of the digital input. * @return True if the input is high, false if the input is low. */ - bool getValue() + HWResult getValue() { - bool value = controller.get_value(); + // Read the value from the ADI port + const auto result = executeWithErrorCheck(pros::c::adi_digital_read, port); + if (!result.isSuccess()) return result.status; + + // Invert the value if necessary + bool value = result.value; if (isInverted) value = !value; + return value; } private: - const pros::adi::DigitalIn controller; - bool isInverted = false; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/adiPneumatic.hpp b/include/devils/hardware/adiPneumatic.hpp index 14185ab..e07ec2f 100644 --- a/include/devils/hardware/adiPneumatic.hpp +++ b/include/devils/hardware/adiPneumatic.hpp @@ -1,7 +1,7 @@ #pragma once + #include "pros/adi.hpp" -#include "../utils/logger.hpp" -#include "structs/hardwareBase.hpp" +#include "hardwareBase.hpp" #include namespace devils @@ -10,40 +10,40 @@ namespace devils * Represents a non-V5 pneumatic valve controlled by ADI ports. * See https://github.com/msoe-vex/pcb-design/tree/main/VEX%20Solenoid%20Driver%20V2%20Complete */ - class ADIPneumatic : private HardwareBase + class ADIPneumatic : ADIHardwareBase { public: /** * Creates a new pneumatic controlled by an ADI port. * @param name The name of the pneumatic (for logging purposes) - * @param port The ADI port of the motor controller (from 1 to 8) + * @param port The ADI port (from 'A' to 'H'). + * @param isInverted True if the pneumatic should be inverted, false otherwise. */ - ADIPneumatic(std::string name, int8_t port) - : HardwareBase(name, "ADIPneumatic", port), - controller(abs(port)) + ADIPneumatic( + const std::string& name, + const char port, + const bool isInverted = false) + : ADIHardwareBase(name, "ADIPneumatic", port), + isInverted(isInverted) { - isInverted = port < 0; - if (errno != 0) - reportFault("ADI port is invalid"); + // ADI Ports need to be configured before they can be used + executeWithErrorCheck(pros::c::adi_port_set_config, port, pros::E_ADI_DIGITAL_OUT); } /** * Sets the state of the pneumatic. - * @param isExtended True to extend the pneumatic, false to retract it. + * @param extended True to extend the pneumatic, false to retract it. */ - void setExtended(bool isExtended) + void setExtended(bool extended) { + this->isExtended = extended; + // Invert the value if the port is inverted if (isInverted) - isExtended = !isExtended; - this->isExtended = isExtended; + extended = !extended; - // Set the value and check for errors - int32_t status = controller.set_value(isExtended); - - // Check for errors - if (status != 1) - reportFault("Set ADI value failed"); + // Write the value to the ADI port + executeWithErrorCheck(pros::c::adi_digital_write, port, extended); } /** @@ -66,15 +66,13 @@ namespace devils * Checks if the pneumatic is extended. * @return True if the pneumatic is extended, false otherwise. */ - bool getExtended() + bool getExtended() const { return isExtended; } private: - const pros::adi::DigitalOut controller; - bool isExtended = false; bool isInverted = false; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/adiPneumaticGroup.hpp b/include/devils/hardware/adiPneumaticGroup.hpp index d5a0bda..a262d6c 100644 --- a/include/devils/hardware/adiPneumaticGroup.hpp +++ b/include/devils/hardware/adiPneumaticGroup.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "adiPneumatic.hpp" namespace devils @@ -16,33 +17,37 @@ namespace devils /** * Creates a new ADI pneumatic group. * @param name The name of the pneumatic group (for logging purposes) - * @param ports The ADI ports of the pneumatics in the group + * @param ports The ADI ports of the pneumatics in the group (from 'A' to 'H'). + * @param isInverted True if the pneumatics should be inverted, false otherwise. */ ADIPneumaticGroup( std::string name, - std::initializer_list ports) - : name(name), - pneumatics() + const std::initializer_list ports, + bool isInverted = false) + : name(std::move(name)) { pneumatics.reserve(ports.size()); - for (int8_t port : ports) - pneumatics.push_back(std::make_shared(getPneumaticName(port), port)); + for (auto port : ports) + pneumatics.push_back(std::make_shared( + getPneumaticName(port), + port, + isInverted)); } /** * Sets the state of all the pneumatics in the group. * @param isExtended True to extend the pneumatics, false to retract them. */ - void setExtended(bool isExtended) + void setExtended(const bool isExtended) const { - for (auto pneumatic : pneumatics) + for (const auto& pneumatic : pneumatics) pneumatic->setExtended(isExtended); } /** * Extends all the pneumatics in the group. */ - void extend() + void extend() const { setExtended(true); } @@ -50,7 +55,7 @@ namespace devils /** * Retracts all the pneumatics in the group. */ - void retract() + void retract() const { setExtended(false); } @@ -59,9 +64,9 @@ namespace devils * Checks if all the pneumatics in the group are extended. * @return True if all the pneumatics in the group are extended, false otherwise. */ - bool getExtended() + bool getExtended() const { - for (auto pneumatic : pneumatics) + for (const auto& pneumatic : pneumatics) if (!pneumatic->getExtended()) return false; return true; @@ -71,7 +76,7 @@ namespace devils * Gets the pneumatics in the motor group. * @return The pneumatics in the motor group. */ - ADIPneumaticList &getPneumatics() + ADIPneumaticList& getPneumatics() { return pneumatics; } @@ -82,12 +87,12 @@ namespace devils * @param port The port of the pneumatic * @return The name of the pneumatic */ - std::string getPneumaticName(uint32_t port) + std::string getPneumaticName(const char port) const { - return name + "_" + std::to_string(port); + return name + "_" + ADIHardwareBase::adiPortToString(port); } const std::string name; ADIPneumaticList pneumatics; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/controller.hpp b/include/devils/hardware/controller.hpp new file mode 100644 index 0000000..4eeb5d7 --- /dev/null +++ b/include/devils/hardware/controller.hpp @@ -0,0 +1,244 @@ +#pragma once + +#include "hardwareBase.hpp" +#include "pros/misc.h" + +namespace devils +{ + struct ControllerAxis + { + struct Options + { + /** + * The minimum joystick input to register movement. + */ + float deadzone = 0.0f; + + /** + * The initial value of the joystick curve + */ + float startingValue = 0.0f; + + /** + * The ending value of the joystick curve + */ + float endingValue = 1.0f; + + /** + * The amount of curve applied to the joystick input. + */ + float exponent = 1.0f; + }; + + /** + * Creates a new Controller Joystick Axis. + * @param port The port of the controller. + * @param axis The axis of the joystick. + */ + ControllerAxis( + const pros::controller_id_e_t port, + const pros::controller_analog_e_t axis) + : port(port), + axis(axis) + { + } + + operator float() const + { + return getValue(); + } + + /** + * Gets the raw value of the joystick axis from -1 to 1. + * @return The raw value of the joystick axis. + */ + float getRawValue() const + { + const auto value = pros::c::controller_get_analog(port, axis); + return static_cast(value) / 127.0f; + } + + /** + * Gets the processed value of the joystick axis from -1 to 1, applying deadzone and curve options. + * @return The processed value of the joystick axis. + */ + float getValue() const + { + const float rawValue = getRawValue(); + + // Apply deadzone + if (std::abs(rawValue) < options.deadzone) + return 0.0f; + + // Apply curve + const float sign = (rawValue > 0) ? 1.0f : -1.0f; + const float normalizedValue = (std::abs(rawValue) - options.deadzone) / (1.0f - options.deadzone); + const float curvedValue = options.startingValue + (options.endingValue - options.startingValue) * std::pow( + normalizedValue, options.exponent); + + return sign * curvedValue; + } + + /** + * Sets the options for the joystick axis. + * @param newOptions The new options to set. + */ + void setOptions(const Options& newOptions) + { + options = newOptions; + } + + private: + Options options; + + pros::controller_id_e_t port; + pros::controller_analog_e_t axis; + }; + + struct ControllerButton + { + enum Mode + { + /// @brief True while the button is being pressed. + PRESSED, + + /// @brief True for one cycle when the button is pressed. + JUST_PRESSED, + + /// @brief Toggles between true and false each time the button is pressed. + TOGGLED + }; + + ControllerButton( + const pros::controller_id_e_t port, + const pros::controller_digital_e_t button) + : port(port), + button(button) + { + } + + operator bool() + { + return getValue(); + } + + /** + * Gets the value of the button based on its mode. + * @return The value of the button. + */ + bool getValue() + { + switch (mode) + { + case PRESSED: + return pros::c::controller_get_digital(port, button) & 1; + case JUST_PRESSED: + return pros::c::controller_get_digital_new_press(port, button) & 1; + case TOGGLED: + if (pros::c::controller_get_digital_new_press(port, button) & 1) + toggledState = !toggledState; + return toggledState; + default: + return false; + } + } + + /** + * Sets the mode of the button. + * @param newMode The new mode to set. + */ + void setMode(const Mode newMode) + { + mode = newMode; + toggledState = false; + } + + private: + pros::controller_id_e_t port; + pros::controller_digital_e_t button; + Mode mode = PRESSED; + bool toggledState = false; + }; + + /** + * Represents a VEX V5 controller. + */ + class Controller : HardwareBase + { + pros::controller_id_e_t port; + public: + Controller(const std::string& name, const pros::controller_id_e_t port) + : HardwareBase(name, "Controller", controllerPortToString(port)), + + a(port, pros::E_CONTROLLER_DIGITAL_A), + b(port, pros::E_CONTROLLER_DIGITAL_B), + x(port, pros::E_CONTROLLER_DIGITAL_X), + y(port, pros::E_CONTROLLER_DIGITAL_Y), + + up(port, pros::E_CONTROLLER_DIGITAL_UP), + down(port, pros::E_CONTROLLER_DIGITAL_DOWN), + left(port, pros::E_CONTROLLER_DIGITAL_LEFT), + right(port, pros::E_CONTROLLER_DIGITAL_RIGHT), + + l1(port, pros::E_CONTROLLER_DIGITAL_L1), + l2(port, pros::E_CONTROLLER_DIGITAL_L2), + r1(port, pros::E_CONTROLLER_DIGITAL_R1), + r2(port, pros::E_CONTROLLER_DIGITAL_R2), + + leftY(port, pros::E_CONTROLLER_ANALOG_LEFT_Y), + leftX(port, pros::E_CONTROLLER_ANALOG_LEFT_X), + rightY(port, pros::E_CONTROLLER_ANALOG_RIGHT_Y), + rightX(port, pros::E_CONTROLLER_ANALOG_RIGHT_X), + port(port) + { + } + + ControllerButton a; + ControllerButton b; + ControllerButton x; + ControllerButton y; + + ControllerButton up; + ControllerButton down; + ControllerButton left; + ControllerButton right; + + ControllerButton l1; + ControllerButton l2; + ControllerButton r1; + ControllerButton r2; + + ControllerAxis leftY; + ControllerAxis leftX; + ControllerAxis rightY; + ControllerAxis rightX; + + private: + static std::string controllerPortToString(const pros::controller_id_e_t port) + { + switch (port) + { + case pros::E_CONTROLLER_MASTER: + return "Master"; + case pros::E_CONTROLLER_PARTNER: + return "Partner"; + default: + return "Unknown"; + } + } + + public: + /** + * Rumbles the controller with a given pattern. + * @param pattern The rumble pattern to use. This should be a string of '.' and '-' characters + */ + void rumble(const std::string& pattern) const + { + pros::c::controller_rumble(port, pattern.c_str()); + } + }; + + + + +} diff --git a/include/devils/hardware/devilCV.hpp b/include/devils/hardware/devilCV.hpp deleted file mode 100644 index dde84e8..0000000 --- a/include/devils/hardware/devilCV.hpp +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once -#include "vexbridge/vexBridge.h" -#include "structs/camera.h" - -namespace devils -{ - /** - * Represents an offboard Raspberry Pi running OpenCV. - * Data is sent over the network using VEX Bridge. - */ - class DevilCV : public ICamera - { - public: - static constexpr double VISION_WIDTH_FOV = 55; // degrees - - /** - * Creates a DevilCV object. - * @param name The name of the camera - */ - DevilCV(std::string name) - { - setCameraName(name); - } - - bool hasTargets() override - { - return hasTarget->get(); - } - - ICamera::VisionObject getClosestTarget() override - { - return ICamera::VisionObject{targetX->get(), targetY->get()}; - } - - /** - * Sets the camera name for the VEX Bridge. - * @param name The name of the camera - */ - void setCameraName(std::string name) - { - targetX = std::make_unique>("vision/color/" + name + "/center_x", 0.0f); - targetY = std::make_unique>("vision/color/" + name + "/center_y", 0.0f); - hasTarget = std::make_unique>("vision/color/" + name + "/has_target", false); - } - - private: - std::unique_ptr> targetX = std::make_unique>(nullptr); - std::unique_ptr> targetY = std::make_unique>(nullptr); - std::unique_ptr> hasTarget = std::make_unique>(nullptr); - }; -} \ No newline at end of file diff --git a/include/devils/hardware/gps.hpp b/include/devils/hardware/gps.hpp deleted file mode 100644 index 0c6482c..0000000 --- a/include/devils/hardware/gps.hpp +++ /dev/null @@ -1,149 +0,0 @@ -#pragma once -#include "pros/gps.hpp" -#include "../utils/logger.hpp" -#include "../geometry/pose.hpp" -#include "../geometry/units.hpp" -#include "../geometry/polygon.hpp" -#include "../odom/odomSource.hpp" -#include "structs/hardwareBase.hpp" - -namespace devils -{ - /** - * Represents a Vex V5 GPS - */ - class GPS : public OdomSource, public Runnable, private HardwareBase - { - public: - /** - * Initializes a Vex V5 GPS as an odometry source. - * Remember to call `GPS::setPosition` to set the initial position before the GPS locks on. - * Also call `GPS::setOffset` to set the GPS's position relative to the center of the robot. - * @param gpsPort The port of the GPS - */ - GPS(std::string name, uint8_t gpsPort) - : HardwareBase(name, "GPS", gpsPort), - gps(gpsPort, 0, 0, 0, 0, 0) - { - if (errno != 0) - reportFault("Invalid port"); - gps.set_data_rate(20); - } - - /** - * Updates the odometry with the latest GPS data - */ - void onUpdate() override - { - double gpsX = gps.get_position_x(); - double gpsY = gps.get_position_y(); - double gpsHeading = gps.get_heading(); - - if (gpsX == PROS_ERR_F || gpsY == PROS_ERR_F || gpsHeading == PROS_ERR_F) - { - reportFault("Get GPS position failed"); - return; - } - - // Convert Units - gpsX = Units::metersToIn(gpsX); - gpsY = -Units::metersToIn(gpsY); - gpsHeading = Units::normalizeRadians(Units::degToRad(gpsHeading) - GPS_ROTATION_OFFSET - rotationalOffset); - - // Check Calibrating - if (isCalibrating()) - return; - - // Check Within Bounds - if (gpsX < -MAX_GPS_X || gpsX > MAX_GPS_X || gpsY < -MAX_GPS_Y || gpsY > MAX_GPS_Y) - { - reportFault("GPS out of bounds"); - return; - } - - // Update Pose - currentPose.x = gpsX; - currentPose.y = gpsY; - currentPose.rotation = gpsHeading; - } - - /** - * Restarts the calibration timer - */ - void reset() - { - calibrationStartTime = -1; - } - - /** - * Gets the current pose of the robot since the last `GPS::update` - * @return The current pose of the robot - */ - Pose getPose() override - { - return currentPose; - } - - /** - * Sets the current pose of the robot. - * This value is overriden when the GPS locks on to the field strip. - * @param pose The pose to set the robot to - * @return The current pose of the robot - */ - void setPose(Pose pose) override - { - currentPose = pose; - - double gpsX = Units::inToMeters(pose.x); - double gpsY = Units::inToMeters(pose.y); - double gpsYaw = Units::degToRad(pose.rotation) + GPS_ROTATION_OFFSET + rotationalOffset; - - int32_t status = gps.set_position( - gpsX, - gpsY, - gpsYaw); - - if (status != 1) - reportFault("Set GPS position failed"); - } - - /** - * Sets the GPS's position relative to the center of the robot. - * @param x The x offset of the GPS from the center of the robot in inches - * @param y The y offset of the GPS from the center of the robot in inches - * @param rotation The rotation offset of the GPS from the center of the robot in radians - */ - void setOffset(double x, double y, double rotation) - { - int32_t result = gps.set_offset( - Units::inToMeters(x), - Units::inToMeters(y)); - rotationalOffset = rotation; - - if (result != 1) - reportFault("Set GPS offset failed"); - } - - /** - * Checks if the GPS is calibrating. GPS takes a few seconds to lock on to the field strip. - * @return Whether the GPS is calibrating - */ - bool isCalibrating() - { - if (calibrationStartTime < 0) - calibrationStartTime = pros::millis(); - return (pros::millis() - calibrationStartTime) < CALIBRATION_TIME; - } - - private: - static constexpr int CALIBRATION_TIME = 7000; // ms - static constexpr double GPS_ROTATION_OFFSET = M_PI * 0.5; // PROS defaults to north as 0 degrees - static constexpr double MAX_GPS_X = 72; - static constexpr double MAX_GPS_Y = 72; - - pros::Gps gps; - Pose currentPose = Pose(0, 0, 0); - double rotationalOffset = 0; - int calibrationStartTime = -1; - }; -} \ No newline at end of file diff --git a/include/devils/hardware/structs/gyro.h b/include/devils/hardware/gyroBase.h similarity index 53% rename from include/devils/hardware/structs/gyro.h rename to include/devils/hardware/gyroBase.h index 4038562..ec07bda 100644 --- a/include/devils/hardware/structs/gyro.h +++ b/include/devils/hardware/gyroBase.h @@ -1,5 +1,7 @@ #pragma once +#include "hardwareBase.hpp" + namespace devils { /** @@ -7,16 +9,24 @@ namespace devils */ struct IGyro { + virtual ~IGyro() = default; + + /** + * Check if the sensor is ready to be used (e.g. finished calibrating). + * @return True if the sensor is ready to be used, false otherwise. + */ + virtual bool getIsReady() = 0; + /** * Gets the current heading of the sensor in radians. * @return The current heading of the sensor in radians. */ - virtual double getHeading() = 0; + virtual HWResult getHeading() = 0; /** * Sets the current heading of the sensor in radians. * @param heading The heading to set the sensor to in radians. */ - virtual void setHeading(double heading) = 0; + virtual void setHeading(float heading) = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/hallEffectEncoder.hpp b/include/devils/hardware/hallEffectEncoder.hpp new file mode 100644 index 0000000..b7f426c --- /dev/null +++ b/include/devils/hardware/hallEffectEncoder.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include "pros/rotation.hpp" +#include "../geometry/units.hpp" +#include "hardwareBase.hpp" +#include + +namespace devils +{ + /** + * Represents a Hall Effect Encoder connected to an I2C port. This sensor can be used to measure the rotation of a shaft or wheel. + * The sensor should be connected to vexbridge + */ + class HallEffectEncoder : public HardwareBase + { + public: + /** + * Creates a new Hall Effect Encoder. + * @param name The name of the sensor (for logging purposes) + */ + HallEffectEncoder(const std::string& name) + : HardwareBase(name, "HallEffectEncoder", "I2C port") + { + + } + + /** + * Gets the current rotation of the encoder in degrees. + * @return The current rotation of the encoder in degrees, or an error code if an error occurred. + */ + HWResult getRotation() const + { + //TODO: Implement this funciton using i2c communication to read the rotation from the sensor. The rotation should be returned in degrees. + reportErrorFromStatus(HWStatus::ERROR_NOT_IMPLEMENTED); + return HWStatus::ERROR_NOT_IMPLEMENTED; + } + }; +} diff --git a/include/devils/hardware/hardwareBase.hpp b/include/devils/hardware/hardwareBase.hpp new file mode 100644 index 0000000..ad382e8 --- /dev/null +++ b/include/devils/hardware/hardwareBase.hpp @@ -0,0 +1,260 @@ +#pragma once +#include +#include +#include "../utils/logger.hpp" +#include "pros/apix.h" + +namespace devils +{ + /** + * The status of a hardware operation. + */ + enum HWStatus + { + // success + SUCCESS, + + // internal + ERROR_NO_SUCCESSFUL_RESULTS, + + // errno + ERROR_INVALID_PORT, + ERROR_INCORRECT_TYPE, + ERROR_CALIBRATING, + ERROR_PORT_IN_USE, + ERROR_ACCESS_DENIED, + ERROR_TOO_MANY_OPEN_FILES, + ERROR_NOT_IMPLEMENTED, + ERROR_OUT_OF_DOMAIN, + ERROR_OUT_OF_BOUNDS, + ERROR_IO, + ERROR_INVALID_ARGUMENT, + ERROR_OUT_OF_MEMORY, + ERROR_RESOURCE_BUSY, + ERROR_UNKNOWN + }; + + /** + * Represents an error related to hardware components. + */ + template + struct HWResult + { + T value; + HWStatus status; + + // Constructor for implicit conversion from T to HWResult + HWResult(const T val) : + value(val), + status(SUCCESS) + { + } + + // Constructor for explicit creation of HWResult with status + HWResult(const HWStatus status) : + value(T{}), + status(status) + { + } + + // Conversion operator for implicit conversion from HWResult to T + operator T() const + { + return value; + } + + /** + * Checks if the result is successful. + * @return True if the result is successful, false otherwise. + */ + bool isSuccess() const + { + return status == SUCCESS; + } + }; + + /** + * Base class for all hardware devices. + */ + class HardwareBase + { + public: + /** + * Base class for all hardware devices. + * @param name The name of the hardware (e.g. "Left Drive Motor") + * @param type The type of the hardware (e.g. SmartMotor) + * @param portName The name of the port the hardware is connected to (e.g. "A") + */ + HardwareBase( + std::string name, + std::string type, + std::string portName) + : name(std::move(name)), + type(std::move(type)), + portName(std::move(portName)) + { + } + + protected: + /** + * Reports an error based on a hardware status code. + * @param status The hardware status code to report an error for. + */ + void reportErrorFromStatus(const HWStatus status) const + { + switch (status) + { + case ERROR_INCORRECT_TYPE: + Logger::error(toString() + " isn't connected"); + break; + case ERROR_INVALID_PORT: + Logger::error(toString() + " isn't connected to a valid port."); + break; + case ERROR_CALIBRATING: + Logger::error(toString() + " is busy calibrating."); + break; + case ERROR_RESOURCE_BUSY: + case ERROR_PORT_IN_USE: + Logger::error(toString() + " is ` use by another process."); + break; + default: + Logger::error(toString() + " encountered an unknown error (code " + std::to_string(status) + ")."); + break; + } + } + + /** + * Executes a function and checks for errors based on errno. + * @tparam ReturnType Return type of the function + * @tparam Func The function type + * @tparam Args The argument types + * @param func The function to execute + * @param args The arguments to pass to the function + * @return The result of the function or an error code if an error occurred. + */ + template + HWResult executeWithErrorCheck(Func func, Args... args) + { + // Always resets errno before calling the function + errno = 0; + + // Call the function with its arguments + ReturnType result = func(args...); + auto statusCode = getStatusCode(); + + // Check errno for errors + if (statusCode != SUCCESS) + { + reportErrorFromStatus(statusCode); + return statusCode; + } + return result; // or handle someValue as needed + } + + /** + * Gets the hardware status code based on the current errno value. + * @return The hardware status code based on the current errno value. + */ + static HWStatus getStatusCode() + { + switch (errno) + { + case 0: + return SUCCESS; + case ENXIO: + case ENOENT: + return ERROR_INVALID_PORT; + case ENODEV: + return ERROR_INCORRECT_TYPE; + case EAGAIN: + return ERROR_CALIBRATING; + case EADDRINUSE: + return ERROR_PORT_IN_USE; + case EACCES: + return ERROR_ACCESS_DENIED; + case ENFILE: + return ERROR_TOO_MANY_OPEN_FILES; + case ENOSYS: + return ERROR_NOT_IMPLEMENTED; + case EDOM: + return ERROR_OUT_OF_DOMAIN; + case EOVERFLOW: + return ERROR_OUT_OF_BOUNDS; + case EIO: + return ERROR_IO; + case EINVAL: + return ERROR_INVALID_ARGUMENT; + case ENOMEM: + return ERROR_OUT_OF_MEMORY; + case EBUSY: + return ERROR_RESOURCE_BUSY; + default: + return ERROR_UNKNOWN; + } + } + + /** + * Returns a string representation of the hardware, including its name and port. + * @return A string representation of the hardware, including its name and port (e.g. "Left Drive Motor (port A)"). + */ + std::string toString() const + { + return name + " (port " + portName + ")"; + } + + std::string name; + std::string type; + std::string portName; + }; + + /** + * Represents a `HardwareBase` that is connected to a specific ADI port on the VEX V5 brain. + */ + class ADIHardwareBase : public HardwareBase + { + public: + ADIHardwareBase(std::string name, std::string type, const char port) + : HardwareBase(std::move(name), std::move(type), adiPortToString(port)), + port(port) + { + } + + /** + * Converts an ADI port id (e.g. 'A', 'B', etc.) to its corresponding string representation (e.g. "A", "B", etc.). + * @param port - The ID of the port (e.g. 'A', 'B', etc.) + * @return A string representation of the ADI port (e.g. "A", "B", etc.). If the port name is invalid, returns "?". + */ + static std::string adiPortToString(const char port) + { + if (port >= 'A' && port <= 'H') + return std::string(1, port); // Convert 'A'-'H' to "A"-"H" + if (port >= 'a' && port <= 'h') + return std::string(1, port - ('a' - 'A')); // Convert 'a'-'h' to "A"-"H" + + // Invalid port name + return "?"; + } + + protected: + char port; + }; + + /** + * Represents a `HardwareBase` that is connected to a specific V5 port on the VEX V5 brain. + */ + class V5HardwareBase : public HardwareBase + { + public: + V5HardwareBase( + std::string name, + std::string type, + const uint8_t port) + : HardwareBase(std::move(name), std::move(type), std::to_string(port)), + port(port) + { + } + + protected: + uint8_t port; + }; +} diff --git a/include/devils/hardware/inertialSensor.hpp b/include/devils/hardware/inertialSensor.hpp index 738f341..048765b 100644 --- a/include/devils/hardware/inertialSensor.hpp +++ b/include/devils/hardware/inertialSensor.hpp @@ -2,13 +2,11 @@ #include "pros/imu.hpp" #include "pros/error.h" -#include "../utils/logger.hpp" -#include "structs/gyro.h" -#include "structs/hardwareBase.hpp" +#include "gyroBase.h" +#include "hardwareBase.hpp" #include "../geometry/units.hpp" #include "../geometry/vector3.hpp" #include "../odom/odomSource.hpp" -#include "../odom/poseVelocityCalculator.hpp" #include namespace devils @@ -16,7 +14,7 @@ namespace devils /** * Represents a V5 inertial measurement unit. */ - class InertialSensor : private HardwareBase, public IGyro + class InertialSensor : V5HardwareBase, public IGyro { public: /** @@ -25,145 +23,172 @@ namespace devils * @param name The name of the IMU (for logging purposes) * @param port The port of the IMU (from 1 to 21) */ - InertialSensor(std::string name, uint8_t port) - : HardwareBase(name, "IMU", port), - imu(port) + InertialSensor( + const std::string& name, + const uint8_t port) + : V5HardwareBase(name, "IMU", port) { - if (errno != 0) - reportFault("Invalid port"); } - InertialSensor(const InertialSensor &) = delete; - InertialSensor &operator=(const InertialSensor &) = delete; + /** + * Sets the current heading of the IMU in radians. + * @param heading The heading to set the IMU to in radians. + */ + void setHeading(const float heading) override + { + const auto rawHeading = getRawHeading(); + if (!rawHeading.isSuccess()) + return; + + headingOffset = heading - rawHeading; + } /** - * Gets the current acceleration of the IMU in inches per second squared. - * @return The current acceleration of the IMU in inches per second squared. + * Scales the heading by a given factor. + * Used to fix consistent heading drift after a set rotation. + * Can be calculated by rotating the robot exactly 360 degrees and doing `2 * PI / getHeading()`. + * @param scale The scale to multiply the heading by. */ - Vector3 getAccel() + void setHeadingScale(const float scale) { - auto accel = imu.get_accel(); - if (accel.x == PROS_ERR_F) - { - reportFault("Get IMU acceleration failed"); - return Vector3(0, 0, 0); - } - return Vector3( - Units::metersToIn(accel.x), - Units::metersToIn(accel.y), - Units::metersToIn(accel.z)); + headingScale = scale; } /** - * Gets the current heading of the IMU in radians, unbounded. - * @return The current heading of the IMU in radians or 0 if the operation failed. + * Calibrates the IMU. Robot should be still during calibration. + * Run `waitUntilCalibrated` to wait until calibration is finished. */ - double getHeading() override + void calibrate() { - errno = 0; - double heading = imu.get_rotation(); - if (heading == PROS_ERR_F) - { - reportFault("Get IMU heading failed"); - return 0; - } + executeWithErrorCheck(pros::c::imu_reset, port); + } - // Apply scale/offset - return Units::degToRad(heading) * headingScale + headingOffset; + /** + * Waits until the IMU is finished calibrating. + * Should be run to avoid movement during calibration. + */ + void waitUntilDoneCalibrated() + { + while (getIsCalibrating()) + pros::delay(20); } /** - * Gets the current pitch of the IMU in radians. - * @return The current pitch of the IMU in radians or 0 if the operation failed. + * Checks if the IMU is currently calibrating. + * @return True if the IMU is currently calibrating, false otherwise. If the operation failed, an error code is returned. */ - double getPitch() + HWResult getIsCalibrating() { - double pitch = imu.get_pitch(); - if (pitch == PROS_ERR_F) - { - reportFault("Get IMU pitch failed"); - return 0; - } - return Units::degToRad(pitch); + const auto result = executeWithErrorCheck(pros::c::imu_get_status, port); + if (!result.isSuccess()) + return result.status; + + return result.value & IMU_STATUS_CALIBRATING; + } + + /** + * Checks if the IMU is ready to be used (e.g. finished calibrating). + * @return True if the IMU is ready to be used, false otherwise. If the operation failed, false is returned. + */ + bool getIsReady() override + { + const auto isCalibrating = getIsCalibrating(); + if (!isCalibrating.isSuccess()) + return false; + return !isCalibrating.value; } /** - * Gets the current roll of the IMU in radians. - * @return The current roll of the IMU in radians or 0 if the operation failed. + * Gets the current acceleration of the IMU in inches per second squared. + * @return The current acceleration of the IMU in inches per second squared. */ - double getRoll() + HWResult getAccel() { - double roll = imu.get_roll(); - if (roll == PROS_ERR_F) - { - reportFault("Get IMU roll failed"); - return 0; - } - return Units::degToRad(roll); + const auto result = executeWithErrorCheck(pros::c::imu_get_accel, port); + if (!result.isSuccess()) + return result.status; + + const auto x = static_cast(result.value.x); + const auto y = static_cast(result.value.y); + const auto z = static_cast(result.value.z); + + if (x == PROS_ERR_F) + return getStatusCode(); + + return Vector3( + Units::metersToIn(x), + Units::metersToIn(y), + Units::metersToIn(z)); } /** - * Gets the current yaw of the IMU in radians. - * @return The current yaw of the IMU in radians or 0 if the operation failed. + * Gets the current heading of the IMU in radians, unscaled and without offset. + * @return The current heading of the IMU in radians or 0 if the operation failed. */ - double getYaw() + HWResult getRawHeading() { - double yaw = imu.get_yaw(); - if (yaw == PROS_ERR_F) - { - reportFault("Get IMU yaw failed"); - return 0; - } - return Units::degToRad(yaw); + const auto result = executeWithErrorCheck(pros::c::imu_get_rotation, port); + if (!result.isSuccess()) + return result.status; + + // Apply scale/offset + return Units::degToRad(static_cast(result.value)); } /** - * Sets the current heading of the IMU in radians. - * @param heading The heading to set the IMU to in radians. + * Gets the current heading of the IMU in radians, unbounded. + * @return The current heading of the IMU in radians or 0 if the operation failed. */ - void setHeading(double heading) override + HWResult getHeading() override { - headingOffset = heading - getHeading(); + const auto rawHeading = getRawHeading(); + if (!rawHeading.isSuccess()) + return rawHeading; + + return rawHeading * headingScale + headingOffset; } /** - * Scales the heading by a given factor. - * Used to fix consistent heading drift after a set rotation. - * Can be calculated by rotating the robot exactly 360 degrees and doing `2 * PI / getHeading()`. - * @param scale The scale to multiply the heading by. + * Gets the current pitch of the IMU in radians. + * @return The current pitch of the IMU in radians or 0 if the operation failed. */ - void setHeadingScale(double scale) + HWResult getPitch() { - headingScale = scale; + const auto result = executeWithErrorCheck(pros::c::imu_get_pitch, port); + if (!result.isSuccess()) + return result.status; + + return Units::degToRad(static_cast(result.value)); } /** - * Calibrates the IMU. Robot should be still during calibration. - * Run `waitUntilCalibrated` to wait until calibration is finished. + * Gets the current roll of the IMU in radians. + * @return The current roll of the IMU in radians or 0 if the operation failed. */ - void calibrate() + HWResult getRoll() { - imu.reset(false); + const auto result = executeWithErrorCheck(pros::c::imu_get_roll, port); + if (!result.isSuccess()) + return result.status; + + return Units::degToRad(static_cast(result.value)); } /** - * Waits until the IMU is finished calibrating. - * Should be ran to avoid movement during calibration. + * Gets the current yaw of the IMU in radians. + * @return The current yaw of the IMU in radians or 0 if the operation failed. */ - void waitUntilCalibrated() + HWResult getYaw() { - while (imu.is_calibrating()) - pros::delay(20); + const auto result = executeWithErrorCheck(pros::c::imu_get_yaw, port); + if (!result.isSuccess()) + return result.status; + + return Units::degToRad(static_cast(result.value)); } private: - double headingScale = 1; - double headingOffset = 0; - bool isCalibrating = false; - bool isErrored = false; - bool isConnected = false; - - pros::IMU imu; - Pose odomPose; + float headingScale = 1; + float headingOffset = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/inertialSensorGroup.hpp b/include/devils/hardware/inertialSensorGroup.hpp deleted file mode 100644 index 7c82f3e..0000000 --- a/include/devils/hardware/inertialSensorGroup.hpp +++ /dev/null @@ -1,95 +0,0 @@ -#pragma once - -#include -#include "structs/gyro.h" -#include "inertialSensor.hpp" -#include "pros/error.h" - -namespace devils -{ - /** - * Represents a set of Inertial Sensors fused together. - */ - class InertialSensorGroup : public IGyro - { - public: - /** - * Creates a new IMU group. - * @param name The name of the IMU group (for logging purposes) - * @param ports The ports of the IMUs in the group (from 1 to 21) - */ - InertialSensorGroup(std::string name, std::initializer_list ports) - : name(name), sensors() - { - sensors.reserve(ports.size()); - for (int8_t port : ports) - sensors.push_back(std::make_shared(getSensorName(port), port)); - } - - /** - * Gets the average heading of all IMUs in the group. - * @return The average heading of all IMUs in the group. - */ - double getHeading() override - { - // Iterate through sensors and get average heading - int sensorCount = 0; - double heading = 0; - for (auto sensor : sensors) - { - double sensorHeading = sensor->getHeading(); - - // Skip sensors that fail to return heading - if (sensorHeading == PROS_ERR_F) - continue; - - heading += sensorHeading; - sensorCount++; - } - - // Log if no sensors returned heading - if (sensorCount == 0) - { - if (LOGGING_ENABLED) - Logger::warn(name + ": no sensors returned heading"); - return 0; - } - - // Return the mean heading - return heading / sensorCount; - } - - /** - * Sets the current heading of all IMUs in the group. - * @param heading The heading to set the IMUs to in radians. - */ - void setHeading(double heading) override - { - for (auto sensor : sensors) - sensor->setHeading(heading); - } - - /** - * Gets the sensors in the inertial sensor group. - * @return The sensors in the inertial sensor group. - */ - std::vector> &getSensors() - { - return sensors; - } - - /** - * Gets the name of each sensor in the inertial sensor group. - */ - std::string getSensorName(int32_t port) - { - return name + "_" + std::to_string(port); - } - - private: - static constexpr bool LOGGING_ENABLED = false; - - const std::string name; - std::vector> sensors; - }; -} \ No newline at end of file diff --git a/include/devils/hardware/led.hpp b/include/devils/hardware/led.hpp deleted file mode 100644 index 340b84a..0000000 --- a/include/devils/hardware/led.hpp +++ /dev/null @@ -1,75 +0,0 @@ -#pragma once -#include "pros/adi.hpp" -#include "../utils/logger.hpp" -#include "structs/hardwareBase.hpp" -#include - -namespace devils -{ - /** - * Represents an LED connected to the ADI ports. - */ - class LED : private HardwareBase - { - public: - /** - * Creates a new LED. - * @param name The name of the LED (for logging purposes) - * @param port The ADI port of the LED (from 1 to 8) - */ - LED(std::string name, uint8_t port) - : HardwareBase(name, "LED", port), - led(port) - { - if (errno != 0) - reportFault("Invalid port"); - } - - /** - * Turns the LED on - */ - void enable() - { - isEnabled = true; - int32_t status = led.set_value(false); - if (status != 1) - reportFault("LED enable failed"); - } - - /** - * Turns the LED off - */ - void disable() - { - isEnabled = false; - int32_t status = led.set_value(true); - if (status != 1) - reportFault("LED disable failed"); - } - - /** - * Sets the LED to a boolean value. True is on, false is off. - * @param enabled Whether or not the LED should be on - */ - void setEnabled(bool enabled) - { - if (enabled) - enable(); - else - disable(); - } - - /** - * Gets the state of the LED. - * @return True if the LED is on, false if it is off. - */ - bool getEnabled() - { - return isEnabled; - } - - private: - bool isEnabled = false; - pros::ADIDigitalOut led; - }; -} \ No newline at end of file diff --git a/include/devils/hardware/legacyVisionSensor.hpp b/include/devils/hardware/legacyVisionSensor.hpp deleted file mode 100644 index dd95c97..0000000 --- a/include/devils/hardware/legacyVisionSensor.hpp +++ /dev/null @@ -1,81 +0,0 @@ -#pragma once -#include "pros/motors.hpp" -#include "pros/vision.hpp" -#include "pros/error.h" -#include "../utils/logger.hpp" -#include "structs/hardwareBase.hpp" -#include "structs/camera.h" -#include "../geometry/units.hpp" -#include - -namespace devils -{ - /** - * Represents a VEX legacy vision sensor object. - */ - class LegacyVisionSensor : private HardwareBase, public ICamera - { - public: - // Thank you James Pearman for these measurements - // https://www.vexforum.com/t/vision-sensor-fov-measurements/62397 - static constexpr int VISION_WIDTH_PX = VISION_FOV_WIDTH; // px - static constexpr int VISION_HEIGHT_PX = VISION_FOV_HEIGHT; // px - static constexpr int VISION_WIDTH_FOV = 61; // degrees - static constexpr int VISION_HEIGHT_FOV = 41; // degrees - - /** - * Creates a vision sensor object. - * @param name The name of the motor (for logging purposes) - * @param port The port of the motor (from 1 to 21) - */ - LegacyVisionSensor(std::string name, uint8_t port) - : HardwareBase(name, "VisionSensor", port), - sensor(port, pros::E_VISION_ZERO_CENTER) - { - if (errno != 0) - reportFault("Invalid port"); - } - - /** - * Sets the vision sensor's LED color. - * Overrides the default LED behavior. - * @param color The color to set the LED to - */ - void setLEDColor(int32_t color) - { - auto status = sensor.set_led(color); - if (status == PROS_ERR) - reportFault("Failed to set LED color"); - } - - /** - * Resets the vision sensor's LED color to the default behavior. - */ - void resetLEDColor() - { - auto status = sensor.clear_led(); - if (status == PROS_ERR) - reportFault("Failed to reset LED color"); - } - - bool hasTargets() override - { - int32_t objectCount = sensor.get_object_count(); - return objectCount > 0; - } - - ICamera::VisionObject getClosestTarget() override - { - // Get the biggest object in view - auto object = sensor.get_by_size(0); - - // Return the object as a VisionObject - return ICamera::VisionObject{ - (double)object.x_middle_coord / VISION_WIDTH_PX, - (double)object.y_middle_coord / VISION_HEIGHT_PX}; - } - - private: - pros::Vision sensor; - }; -} \ No newline at end of file diff --git a/include/devils/hardware/structs/motor.h b/include/devils/hardware/motorBase.h similarity index 58% rename from include/devils/hardware/structs/motor.h rename to include/devils/hardware/motorBase.h index e4262c3..7540779 100644 --- a/include/devils/hardware/structs/motor.h +++ b/include/devils/hardware/motorBase.h @@ -1,7 +1,5 @@ #pragma once -#include "pros/motors.hpp" -#include "../../utils/logger.hpp" -#include +#include "hardwareBase.hpp" namespace devils { @@ -10,11 +8,13 @@ namespace devils */ struct IMotor { + virtual ~IMotor() = default; + /** - * Runs the motor in voltage mode. - * @param voltage The voltage to run the motor at, from -1 to 1. + * Controls the motor speed based on the voltage. + * @param speed The speed to run the motor at, from -1 to 1. */ - virtual void moveVoltage(double voltage) = 0; + virtual void move(float speed) = 0; /** * Stops the motor. @@ -25,6 +25,6 @@ namespace devils * Gets the current position of the motor in encoder ticks. * @return The current position of the motor in encoder ticks. */ - virtual double getPosition() = 0; + virtual HWResult getPosition() = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/opticalSensor.hpp b/include/devils/hardware/opticalSensor.hpp index 5629c8f..eb6b512 100644 --- a/include/devils/hardware/opticalSensor.hpp +++ b/include/devils/hardware/opticalSensor.hpp @@ -1,10 +1,7 @@ #pragma once -#include "pros/imu.hpp" #include "pros/optical.hpp" #include "pros/error.h" -#include "../utils/logger.hpp" -#include "../geometry/units.hpp" -#include "structs/hardwareBase.hpp" +#include "hardwareBase.hpp" #include namespace devils @@ -12,7 +9,7 @@ namespace devils /** * Represents a V5 optical sensor unit. */ - class OpticalSensor : private HardwareBase + class OpticalSensor : V5HardwareBase { public: /** @@ -20,86 +17,87 @@ namespace devils * @param name The name of the Optical Sensor (for logging purposes) * @param port The port of the Optical Sensor (from 1 to 21) */ - OpticalSensor(const std::string name, const uint8_t port) - : HardwareBase(name, "OpticalSensor", port), - sensor(port) + OpticalSensor( + const std::string& name, + const uint8_t port) + : V5HardwareBase(name, "OpticalSensor", port) { - if (errno != 0) - reportFault("Invalid port"); + } + + /** + * Sets the brightness of the LED on the Optical Sensor. + * @param brightness The brightness of the LED from 0 to 1 + */ + void setLEDBrightness(const float brightness) + { + // Scale the brightness from [0, 1] to [0, 255] and clamp it to the valid range + const auto brightnessScaled = std::clamp(brightness, 0.0f, 1.0f) * 255.0f; + + // Convert the brightness to an integer + const auto brightnessInt = static_cast(brightnessScaled); + + // Set the brightness of the LED + executeWithErrorCheck(pros::c::optical_set_led_pwm, port, brightnessInt); } /** * Gets the current proximity of the Optical Sensor. * @return The current proximity of the Optical Sensor as an arbitrary value from 0 to 1 where 1 is the closest. */ - double getProximity() + HWResult getProximity() { - std::int32_t proximity = sensor.get_proximity(); - if (proximity == PROS_ERR) - { - reportFault("Failed to retrieve proximity"); - return 0; - } - return proximity / 255.0; + // Get the proximity from the sensor + const auto result = executeWithErrorCheck(pros::c::optical_get_proximity, port); + if (!result.isSuccess()) + return result.status; + + // Scale the proximity from [0, 255] to [0, 1] + return static_cast(result) / 255.0f; } /** * Gets the current hue of the Optical Sensor. * @return The current hue of the Optical Sensor as a value from 0 to 360. */ - double getHue() + HWResult getHue() { - std::int32_t hue = sensor.get_hue(); - if (hue == PROS_ERR) - { - reportFault("Failed to retrieve hue"); - return 0; - } - return hue; + // Get the hue from the sensor + const auto result = executeWithErrorCheck(pros::c::optical_get_hue, port); + if (!result.isSuccess()) + return result.status; + + // Convert the hue to a float + return static_cast(result); } /** * Gets the current saturation of the Optical Sensor. - * @return The current saturation of the Optical Sensor as an percentage value from 0 to 1. + * @return The current saturation of the Optical Sensor as a percentage value from 0 to 1. */ - double getSaturation() + HWResult getSaturation() { - std::int32_t saturation = sensor.get_saturation(); - if (saturation == PROS_ERR) - { - reportFault("Failed to retrieve saturation"); - return 0; - } - return saturation; + // Get the saturation from the sensor + const auto result = executeWithErrorCheck(pros::c::optical_get_saturation, port); + if (!result.isSuccess()) + return result.status; + + // Convert the saturation to a float + return static_cast(result); } /** * Gets the current brightness of the Optical Sensor. - * @return The current brightness of the Optical Sensor as an percentage value from 0 to 1. + * @return The current brightness of the Optical Sensor as a percentage value from 0 to 1. */ - double getBrightness() + HWResult getBrightness() { - std::int32_t brightness = sensor.get_brightness(); - if (brightness == PROS_ERR) - { - reportFault("Failed to retrieve brightness"); - return 0; - } - return brightness; + // Get the brightness from the sensor + const auto result = executeWithErrorCheck(pros::c::optical_get_brightness, port); + if (!result.isSuccess()) + return result.status; + + // Convert the brightness to a float + return static_cast(result); } - - /** - * Sets the brightness of the LED on the Optical Sensor. - * @param brightness The brightness of the LED from 0 to 100. - */ - void setLEDBrightness(uint8_t brightness) - { - int32_t result = sensor.set_led_pwm(100); - if (result == PROS_ERR) - reportFault("Failed to set LED brightness"); - } - - private: - pros::Optical sensor; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/rotationSensor.hpp b/include/devils/hardware/rotationSensor.hpp index f43830c..ccd9771 100644 --- a/include/devils/hardware/rotationSensor.hpp +++ b/include/devils/hardware/rotationSensor.hpp @@ -1,16 +1,17 @@ #pragma once + #include "pros/rotation.hpp" -#include "../utils/logger.hpp" #include "../geometry/units.hpp" -#include "structs/hardwareBase.hpp" +#include "hardwareBase.hpp" #include namespace devils { /** - * Represents a V5 rotational sensor. + * Represents a VEX V5 rotational sensor. + * https://www.vexrobotics.com/276-6050.html */ - class RotationSensor : private HardwareBase + class RotationSensor : public V5HardwareBase { public: /** @@ -19,66 +20,70 @@ namespace devils * @param port The port of the rotational sensor (from 1 to 21). Negative ports are reversed. */ RotationSensor( - const std::string name, + const std::string& name, const int8_t port) - : HardwareBase(name, "RotationSensor", port), - rotationSensor(port) + : V5HardwareBase(name, "RotationSensor", abs(port)), + isInverted(port < 0) { - rotationSensor.set_position(0); - if (errno != 0) - reportFault("Invalid port"); } /** - * Gets the absolute angle of the sensor in radians. - * @return The absolute angle of the sensor in radians. + * Sets the position of the sensor in degrees. + * @param position The position to set the sensor to. */ - double getAngle() + void setPosition(const float position) { - errno = 0; - double angle = rotationSensor.get_position(); - if (angle == PROS_ERR) - { - reportFault("Get rotation sensor angle failed"); - return 0; - } - return Units::centidegToRad(angle); - } + // Convert the position to centidegrees + auto centidegPosition = static_cast(Units::degToCentideg(position)); - /** - * Gets the velocity of the sensor in radians per second. - * @return The velocity of the sensor in radians per second. - */ - double getVelocity() - { - double velocity = rotationSensor.get_velocity(); - if (velocity == PROS_ERR_F) - { - reportFault("Get rotation sensor velocity failed"); - return 0; - } - return Units::centidegToRad(velocity); + // Invert the position if necessary + if (isInverted) + centidegPosition = -centidegPosition; + + // Write the position to the sensor + executeWithErrorCheck(pros::c::rotation_set_position, port, centidegPosition); } /** - * Checks if the sensor is still connected. - * @return True if the sensor is still connected, false otherwise. + * Gets the absolute angle of the sensor in degrees (rounded to the nearest centidegree) + * @return The absolute angle of the sensor in degrees. */ - bool isConnected() + HWResult getAngle() { - return rotationSensor.is_installed(); + // Get the angle from the sensor + const auto result = executeWithErrorCheck(pros::c::rotation_get_position, port); + if (!result.isSuccess()) + return result.status; + + // Invert the angle if necessary + auto angle = static_cast(result.value); + if (isInverted) + angle = -angle; + + // Convert from centidegrees to degrees and return + return Units::centidegToDeg(angle); } /** - * Sets the position of the sensor in centidegrees. - * @param position The position to set the sensor to. + * Gets the velocity of the sensor in degrees per second. + * @return The velocity of the sensor in degrees per second. */ - void setPosition(uint32_t position) + HWResult getVelocity() { - rotationSensor.set_position(position); + // Get the velocity from the sensor + const auto result = executeWithErrorCheck(pros::c::rotation_get_velocity, port); + if (!result.isSuccess()) + return result.status; + + // Invert the velocity if necessary + auto velocity = static_cast(result.value); + if (isInverted) + velocity = -velocity; + + return Units::centidegToDeg(velocity); } - private: - pros::Rotation rotationSensor; + protected: + bool isInverted = false; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/smartMotor.hpp b/include/devils/hardware/smartMotor.hpp index 6641e01..c8012d1 100644 --- a/include/devils/hardware/smartMotor.hpp +++ b/include/devils/hardware/smartMotor.hpp @@ -1,151 +1,249 @@ #pragma once +#include #include #include -#include "pros/motors.hpp" -#include "pros/error.h" -#include "../utils/logger.hpp" -#include "structs/motor.h" -#include "structs/hardwareBase.hpp" +#include +#include "motorBase.h" +#include "hardwareBase.hpp" namespace devils { /** - * Represents a motor object. All events are logged. + * Represents a single VEX V5 SmartMotor. + * Use `MotorGroup` to control multiple motors as one. */ - class SmartMotor : public IMotor, protected HardwareBase + class SmartMotor : public IMotor, V5HardwareBase { public: /** * Creates a motor object. * @param name The name of the motor (for logging purposes) * @param port The port of the motor (from 1 to 21) - * @throws std::runtime_error if the motor could not be created, likely due to an invalid port. */ - SmartMotor(std::string name, int8_t port) - : HardwareBase(name, "SmartMotor", port), - motor(port) + SmartMotor( + std::string name, + const int8_t port) + : V5HardwareBase(std::move(name), "SmartMotor", abs(port)), + isInverted(port < 0) { - if (errno != 0) - reportFault("Invalid Port"); + } + + /** + * Checks for SmartMotor micro-disconnection by comparing the SmartMotor's clock to the system clock. + * This allows the system to detect if a motor has been disconnected and reconnected before VEX's device timeout occurs. + * Based on https://github.com/LemLib/hardware/issues/5 + * + * @example + * SmartMotor motor("Motor", 1); + * while (true) + * { + * motor.move(1.0); + * motor.checkForMicroDisconnect(); + * pros::delay(100); + * } + */ + void checkForMicroDisconnect() + { + // Get timestamp from the motor + uint32_t motorTimestamp = 0; + executeWithErrorCheck(pros::c::motor_get_raw_position, port, &motorTimestamp); + const auto motorTimeDifference = static_cast(motorTimestamp) - lastMotorTimestamp; + + // Get timestamp from the system + const auto systemTime = pros::c::millis(); + const auto systemTimeDifference = static_cast(systemTime) - lastSystemTimestamp; + + // Compare the time differences + const auto timeDifference = std::abs(static_cast(motorTimeDifference - systemTimeDifference)); + const auto isFirstCheck = lastSystemTimestamp == 0 || lastMotorTimestamp == 0; + if (timeDifference > MICRODISCONNECT_THRESHOLD_MS && !isFirstCheck) + Logger::warn(name + " may have disconnected (" + std::to_string(timeDifference) + "ms out of sync)"); + + lastSystemTimestamp = systemTime; + lastMotorTimestamp = motorTimestamp; + } + + /** + * Checks for SmartMotor overhead by monitoring the motor temperature + * and logging a warning if it exceeds a certain threshold. + */ + void checkForOverheat() + { + // Get the temperature of the motor + const auto isMotorOverTemp = executeWithErrorCheck(pros::c::motor_is_over_temp, port); + if (!isMotorOverTemp.isSuccess()) + return; + + // Check if the motor is overheating and log a warning if it is + if (isMotorOverTemp == 1) + Logger::warn(name + " is overheating"); } /** * Runs the motor in voltage mode. - * @param voltage The voltage to run the motor at, from -1 to 1. - * @throws std::runtime_error if the motor could not be moved. + * @param speed The speed to run the motor at, from -1 to 1. + * @return HWStatus indicating success or failure. */ - void moveVoltage(double voltage) override + void move(const float speed) override { - // Move Motor - int32_t status = motor.move(voltage * 127); - if (status != 1) - reportFault("Move Voltage Failed"); + // Convert -1 to 1 range to -127 to 127 range + auto voltage = std::clamp(static_cast(speed * MAX_VALUE), -MAX_VALUE, MAX_VALUE); + + // Invert voltage if necessary + if (isInverted) + voltage = -voltage; + + // Pass voltage to motor + executeWithErrorCheck(pros::c::motor_move_voltage, port, voltage); + + // Store the last voltage for reference + lastVoltage = speed; + + // Check for overheat + checkForOverheat(); } /** * Stops the motor. - * @throws std::runtime_error if the motor could not be stopped. + * @return HWStatus indicating success or failure. */ void stop() override { - int32_t status = motor.brake(); - if (status != 1) - reportFault("Stop Failed"); + executeWithErrorCheck(pros::c::motor_brake, port); } /** - * Gets the current position of the motor in encoder ticks. + * Sets the position of the motor in encoder ticks. + * @param position The position to set the motor to in encoder ticks. + * @return HWStatus indicating success or failure. + */ + void setPosition(const float position) + { + // Convert position to a double + const auto positionDouble = static_cast(position); + + // Pass position to motor + executeWithErrorCheck(pros::c::motor_set_zero_position, port, positionDouble); + } + + /** + * Sets the brake mode of the motor. * * \note - * 1800 ticks/rev with 36:1 gears (red cartridge), - * 900 ticks/rev with 18:1 gears (green cartridge), - * 300 ticks/rev with 6:1 gears (blue cartridge) + * Brake mode will use the motor's e-brake to stop the motor when `stop()` is called. + * Coast mode will allow the motor to coast to a stop when `stop()` is called. * - * @return The current position of the motor in encoder ticks or `PROS_ERR_F` if the position could not be retrieved. - * @throws std::runtime_error if the position could not be retrieved. + * @param useBrakeMode True to use brake mode, false to coast mode. */ - double getPosition() override + void setBrakeMode(const bool useBrakeMode) { - double position = motor.get_position(); - if (position == PROS_ERR_F) - reportFault("Get Position Failed"); - return position; + // Convert to PROS status + const auto prosBrakeMode = useBrakeMode ? pros::E_MOTOR_BRAKE_BRAKE : pros::E_MOTOR_BRAKE_COAST; + + // Pass brake mode to motor + executeWithErrorCheck(pros::c::motor_set_brake_mode, port, prosBrakeMode); } /** - * Sets the position of the motor in encoder ticks. - * @param position The position to set the motor to in encoder ticks. + * Gets the current position of the motor in encoder ticks. + * + * \note + * 1800 ticks/rev with 36:1 gears (red cartridge), + * 900 ticks/rev with 18:1 gears (green cartridge), + * 300 ticks/rev with 6:1 gears (blue cartridge) + * + * @return The current position of the motor in encoder ticks */ - void setPosition(double position) + HWResult getPosition() override { - motor.set_zero_position(position); + // Get the position from the motor + const auto result = executeWithErrorCheck(pros::c::motor_get_position, port); + if (!result.isSuccess()) + return result.status; + + // Invert the position if necessary + auto position = static_cast(result.value); + if (isInverted) + position = -position; + + return position; } /** * Returns the current speed of the motor in RPM. - * @return The current speed of the motor in RPM or `PROS_ERR_F` if the speed could not be retrieved. + * @return The current speed of the motor in RPM. */ - double getVelocity() + HWResult getVelocity() { - double velocity = motor.get_actual_velocity(); - if (velocity == PROS_ERR_F) - reportFault("Get Velocity Failed"); + // Get the velocity from the motor + const auto result = executeWithErrorCheck(pros::c::motor_get_actual_velocity, port); + if (!result.isSuccess()) + return result.status; + + // Invert the velocity if necessary + auto velocity = static_cast(result.value); + if (isInverted) + velocity = -velocity; + return velocity; } /** - * Gets the current temperature of the motor in celsius. - * @return The current temperature of the motor in celsius or `PROS_ERR_F` if the temperature could not be retrieved. + * Gets the current temperature of the motor in Celsius. + * @return The current temperature of the motor in Celsius. */ - double getTemperature() + HWResult getTemperature() { - double temperature = motor.get_temperature(); - if (temperature == PROS_ERR_F) - reportFault("Get Temperature Failed"); - return temperature; + // Get the temperature from the motor + const auto result = executeWithErrorCheck(pros::c::motor_get_temperature, port); + if (!result.isSuccess()) + return result.status; + + return static_cast(result.value); } /** - * Gets the current current draw of the motor in mA. - * @return The current current draw of the motor in mA or `PROS_ERR` if the current could not be retrieved. + * Gets the current draw of the motor in amps. + * @return The current draw of the motor in amps. */ - double getCurrent() + HWResult getCurrent() { - double current = motor.get_current_draw(); - if (current == PROS_ERR) - reportFault("Get Current Failed"); - return current; + // Get the current from the motor + const auto result = executeWithErrorCheck(pros::c::motor_get_current_draw, port); + if (!result.isSuccess()) + return result.status; + + // Convert from milliamps to amps + return static_cast(result.value) / 1000.0f; } /** - * Sets the brake mode of the motor. - * - * \note - * Brake mode will use the motor's e-brake to stop the motor when `stop()` is called. - * Coast mode will allow the motor to coast to a stop when `stop()` is called. - * - * @param useBrakeMode True to use brake mode, false to coast mode. - * @throws std::runtime_error if the brake mode could not be set. + * Gets the last voltage that was set to the motor, from -1 to 1. + * @return The last voltage that was set to the motor, from -1 to 1. */ - void setBrakeMode(bool useBrakeMode) + float getLastVoltage() const { - int32_t status = motor.set_brake_mode(useBrakeMode ? pros::E_MOTOR_BRAKE_BRAKE : pros::E_MOTOR_BRAKE_COAST); - if (status != 1) - reportFault("Set Brake Mode Failed"); + return lastVoltage; } - private: - // Hardware - pros::Motor motor; - - // Motor State - bool isPortOutOfRange = false; - bool isPortTaken = false; - bool isOverTemp = false; - bool isDriverFault = false; - bool isOverCurrent = false; - bool isDriverOverCurrent = false; - bool isConnected = true; + protected: + /// @brief Motor clock must be this out of sync with system clock to be considered a micro-disconnection + static constexpr int MICRODISCONNECT_THRESHOLD_MS = 100; + + /// @brief Maximum voltage value inputted into the motor (in millivolts) + static constexpr int MAX_VALUE = 12000; + + /// @brief Whether the motor is inverted (i.e. if the port number is negative) + bool isInverted = false; + + /// @brief System's timestamp the last time `checkForMicroDisconnect()` was called + uint32_t lastSystemTimestamp = 0; + + /// @brief Motor's timestamp the last time `checkForMicroDisconnect()` was called + uint32_t lastMotorTimestamp = 0; + + /// @brief The last voltage that was set to the motor, from -1 to 1 + float lastVoltage = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/smartMotorGroup.hpp b/include/devils/hardware/smartMotorGroup.hpp index 5e42a9c..846558e 100644 --- a/include/devils/hardware/smartMotorGroup.hpp +++ b/include/devils/hardware/smartMotorGroup.hpp @@ -1,8 +1,9 @@ #pragma once + #include -#include "structs/motor.h" +#include +#include "motorBase.h" #include "smartMotor.hpp" -#include "pros/error.h" namespace devils { @@ -17,8 +18,10 @@ namespace devils * @param name The name of the motor group (for logging purposes) * @param ports The ports of the motors in the group (from 1 to 21) */ - SmartMotorGroup(std::string name, std::initializer_list ports) - : name(name), motors() + SmartMotorGroup( + std::string name, + const std::initializer_list ports) + : name(std::move(name)) { motors.reserve(ports.size()); for (int8_t port : ports) @@ -29,10 +32,11 @@ namespace devils * Runs all the motors in voltage mode. * @param voltage The voltage to run the motors at, from -1 to 1. */ - void moveVoltage(double voltage) override + void move(const float voltage) override { - for (auto motor : motors) - motor->moveVoltage(voltage); + for (const auto& motor : motors) + motor->move(voltage); + lastVoltage = voltage; } /** @@ -40,25 +44,55 @@ namespace devils */ void stop() override { - for (auto motor : motors) + for (const auto& motor : motors) motor->stop(); } + /** + * Sets the position of all the motors in encoder ticks. + * @param position The position to set all the motors to in encoder ticks. + */ + void setPosition(const float position) const + { + for (const auto& motor : motors) + motor->setPosition(position); + } + + /** + * Gets the motors in the motor group. + * @return The motors in the motor group. + */ + std::vector>& getMotors() + { + return motors; + } + + /** + * Sets the brake mode of all the motors in the group. + * @param useBrakeMode True to use brake mode, false to use coast mode. + * @return HWStatus indicating success or failure. + */ + void setBrakeMode(const bool useBrakeMode) const + { + for (const auto& motor : motors) + motor->setBrakeMode(useBrakeMode); + } + /** * Gets the average position of all the motors in encoder ticks. * @return The average position of all the motors in encoder ticks. */ - double getPosition() override + HWResult getPosition() override { // Iterate through motors and get average position int motorCount = 0; - double position = 0; - for (auto motor : motors) + float position = 0; + for (const auto& motor : motors) { - double motorPosition = motor->getPosition(); + const auto motorPosition = motor->getPosition(); // Skip motors that fail to return position - if (motorPosition == PROS_ERR_F) + if (!motorPosition.isSuccess()) continue; position += motorPosition; @@ -67,42 +101,28 @@ namespace devils // Log if no motors returned position if (motorCount == 0) - { - if (LOGGING_ENABLED) - Logger::warn(name + ": no motors returned position"); - return 0; - } + return ERROR_NO_SUCCESSFUL_RESULTS; // Return the mean position return position / motorCount; } - /** - * Sets the position of all the motors in encoder ticks. - * @param position The position to set all the motors to in encoder ticks. - */ - void setPosition(double position) - { - for (auto motor : motors) - motor->setPosition(position); - } - /** * Returns the average speed of all the motors in RPM. * If 1 or more motors fail to return velocity, they are ignored. * @return The average speed of all the motors in RPM. */ - double getVelocity() + HWResult getVelocity() const { // Iterate through motors and get average speed int motorCount = 0; - double speed = 0; - for (auto motor : motors) + float speed = 0; + for (const auto& motor : motors) { - double motorSpeed = motor->getVelocity(); + const auto motorSpeed = motor->getVelocity(); // Skip motors that fail to return velocity - if (motorSpeed == PROS_ERR_F) + if (!motorSpeed.isSuccess()) continue; speed += motorSpeed; @@ -111,11 +131,7 @@ namespace devils // Log if no motors returned velocity if (motorCount == 0) - { - if (LOGGING_ENABLED) - Logger::warn(name + ": no motors returned velocity"); - return 0; - } + return ERROR_NO_SUCCESSFUL_RESULTS; // Return the mean speed return speed / motorCount; @@ -126,17 +142,17 @@ namespace devils * If 1 or more motors fail to return current, they are ignored. * @return The average current of all the motors in mA. */ - double getCurrent() + HWResult getCurrent() const { // Iterate through motors and get average current int motorCount = 0; - double current = 0; - for (auto motor : motors) + float current = 0; + for (const auto& motor : motors) { - double motorCurrent = motor->getCurrent(); + const auto motorCurrent = motor->getCurrent(); // Skip motors that fail to return current - if (motorCurrent == PROS_ERR) + if (!motorCurrent.isSuccess()) continue; current += motorCurrent; @@ -145,27 +161,27 @@ namespace devils // Log if no motors returned current if (motorCount == 0) - { - if (LOGGING_ENABLED) - Logger::warn(name + ": no motors returned current"); - return 0; - } + return ERROR_NO_SUCCESSFUL_RESULTS; // Return the mean current return current / motorCount; } - double getTemperature() + /** + * Returns the average temperature of all the motors in Celsius. + * @return The average temperature of all the motors in Celsius. + */ + HWResult getTemperature() const { // Iterate through motors and get average temperature int motorCount = 0; - double temperature = 0; - for (auto motor : motors) + float temperature = 0; + for (const auto& motor : motors) { - double motorTemperature = motor->getTemperature(); + const auto motorTemperature = motor->getTemperature(); // Skip motors that fail to return temperature - if (motorTemperature == PROS_ERR_F) + if (!motorTemperature.isSuccess()) continue; temperature += motorTemperature; @@ -174,47 +190,35 @@ namespace devils // Log if no motors returned temperature if (motorCount == 0) - { - if (LOGGING_ENABLED) - Logger::warn(name + ": no motors returned temperature"); - return 0; - } + return ERROR_NO_SUCCESSFUL_RESULTS; // Return the mean temperature return temperature / motorCount; } /** - * Gets the motors in the motor group. - * @return The motors in the motor group. + * Gets the last voltage that was set to the motors in the group. + * @return The last voltage that was set to the motors in the group, from -1 to 1. */ - std::vector> &getMotors() + float getLastVoltage() const { - return motors; + return lastVoltage; } + protected: /** - * Gets the name of each motor in the motor group. + * Gets the motor name for a specific port. + * @param port The port of the motor + * @return The generated motor name */ - std::string getMotorName(int32_t port) + std::string getMotorName(const int32_t port) const { - return name + "_" + std::to_string(port); - } - - /** - * Sets the brake mode of all the motors in the group. - * @param useBrakeMode True to use brake mode, false to use coast mode. - */ - void setBrakeMode(bool useBrakeMode) - { - for (auto motor : motors) - motor->setBrakeMode(useBrakeMode); + return name + "_" + std::to_string(abs(port)); } private: - static constexpr bool LOGGING_ENABLED = false; - const std::string name; std::vector> motors; + float lastVoltage = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/hardware/structs/camera.h b/include/devils/hardware/structs/camera.h deleted file mode 100644 index 2a3a056..0000000 --- a/include/devils/hardware/structs/camera.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include - -namespace devils -{ - /** - * Represents some kind of vision sensor that can measure objects in the field of view. - */ - struct ICamera - { - /// @brief Represents an object detected by the camera. - struct VisionObject - { - /// @brief The x position of the object measured in the camera's frame. Represented as a percentage of the camera's width from -1 to 1. - double x; - - /// @brief The y position of the object measured in the camera's frame. Represented as a percentage of the camera's height from -1 to 1. - double y; - }; - - /** - * Gets the closest vision object - * @returns The closest vision object - */ - virtual VisionObject getClosestTarget() = 0; - - /** - * Returns true if the camera has detected any targets. - * @return True if the camera has detected any targets, false otherwise. - */ - virtual bool hasTargets() = 0; - }; -} \ No newline at end of file diff --git a/include/devils/hardware/structs/hardwareBase.hpp b/include/devils/hardware/structs/hardwareBase.hpp deleted file mode 100644 index 681aae6..0000000 --- a/include/devils/hardware/structs/hardwareBase.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include "../../utils/runnable.hpp" -#include "../../utils/logger.hpp" - -namespace devils -{ - class HardwareBase - { - public: - /** - * Base class for all hardware devices. - * Handles network table serialization and fault reporting. - * @param name The name of the hardware - * @param type The type of the hardware (e.g. SmartMotor) - * @param port The port of the hardware - */ - HardwareBase( - const std::string name, - const std::string type, - const int8_t port) - : name(name) - { - } - - protected: - /** - * Reports a hardware fault. - * Value is sent to the network table and logged. - * @param fault The fault to set - * @param code Error code (optional) - */ - void reportFault(const std::string fault, const int code = 0) - { - // Log to console - if (LOGGING_ENABLED) - Logger::error(name + ": " + fault + " (" + std::to_string(code) + ")"); - } - - std::string name; - - private: - static constexpr bool SERIALIZATION_ENABLED = true; - static constexpr bool LOGGING_ENABLED = false; - }; -} \ No newline at end of file diff --git a/include/devils/lights/ledStrip.hpp b/include/devils/lights/ledStrip.hpp deleted file mode 100644 index 0a01eab..0000000 --- a/include/devils/lights/ledStrip.hpp +++ /dev/null @@ -1,48 +0,0 @@ -#pragma once -#include "pros/adi.hpp" - -#pragma once - -namespace devils -{ - enum StripAction { - PULSE_BLUE = 50, - WAVE_BLUE = 250, - WAVE_GREEN = 450, - PULSE_GREEN = 550, - PULSE_RED = 650, - - }; - - class LEDStrip - { - public: - LEDStrip(int port) : ledStrip(port) - { - this->pulseBlue(); - }; - - void pulseBlue() - { - ledStrip.set_value(PULSE_BLUE); - } - void waveBlue() - { - ledStrip.set_value(WAVE_BLUE); - } - void pulseRed() - { - ledStrip.set_value(PULSE_GREEN); - } - void waveGreen() - { - ledStrip.set_value(WAVE_GREEN); - } - void pulseGreen() - { - ledStrip.set_value(PULSE_RED); - } - private: - pros::ADIMotor ledStrip; - }; -} \ No newline at end of file diff --git a/include/devils/odom/delayedOdom.hpp b/include/devils/odom/delayedOdom.hpp index 9fab157..b2e0984 100644 --- a/include/devils/odom/delayedOdom.hpp +++ b/include/devils/odom/delayedOdom.hpp @@ -1,10 +1,9 @@ #pragma once #include "odomSource.hpp" -#include "../utils/runnable.hpp" +#include "../utils/asyncTask.hpp" #include "pros/rtos.hpp" #include -#include namespace devils { @@ -12,7 +11,7 @@ namespace devils * Delays the output pose of an odometry source by a specified amount of time. * Used for latency compensation in vision-based odometry. */ - class DelayedOdom : public OdomSource, public Runnable + class DelayedOdom : public OdomSource, public AsyncTask { public: /** @@ -21,11 +20,11 @@ namespace devils * @param delay The delay in milliseconds */ DelayedOdom( - OdomSource &baseOdom, - double delay) - : baseOdom(baseOdom), - maxQueueSize(delay / INTERVAL_DELAY), - Runnable(INTERVAL_DELAY) + OdomSource& baseOdom, + const float delay) + : AsyncTask(), + baseOdom(baseOdom), + maxQueueSize(static_cast(delay / INTERVAL_DELAY)) { // Ensure the queue size is at least 1 if (maxQueueSize < 1) @@ -36,24 +35,6 @@ namespace devils maxQueueSize = MAX_QUEUE_SIZE; } - void onUpdate() override - { - // Get the current pose from the base odometry source - auto pose = baseOdom.getPose(); - auto velocity = baseOdom.getVelocity(); - - // Push the pose to the queue - poseQueue.push(pose); - velocityQueue.push(velocity); - - // Check if the queue is too large - while (poseQueue.size() > maxQueueSize) - poseQueue.pop(); - - while (velocityQueue.size() > maxQueueSize) - velocityQueue.pop(); - } - Pose getPose() override { // Check if the queue is empty @@ -74,11 +55,30 @@ namespace devils return velocityQueue.front(); } - void setPose(Pose pose) override + void setPose(const Pose pose) override { baseOdom.setPose(pose); } + protected: + void onUpdate() override + { + // Get the current pose from the base odometry source + const auto pose = baseOdom.getPose(); + const auto velocity = baseOdom.getVelocity(); + + // Push the pose to the queue + poseQueue.push(pose); + velocityQueue.push(velocity); + + // Check if the queue is too large + while (poseQueue.size() > maxQueueSize) + poseQueue.pop(); + + while (velocityQueue.size() > maxQueueSize) + velocityQueue.pop(); + } + private: /// @brief Update interval in milliseconds static constexpr uint32_t INTERVAL_DELAY = 20; @@ -89,7 +89,7 @@ namespace devils std::queue poseQueue; std::queue velocityQueue; - OdomSource &baseOdom; + OdomSource& baseOdom; size_t maxQueueSize; }; -} \ No newline at end of file +} diff --git a/include/devils/odom/differentialWheelOdom.hpp b/include/devils/odom/differentialWheelOdom.hpp index 087cc3a..21fd4b5 100644 --- a/include/devils/odom/differentialWheelOdom.hpp +++ b/include/devils/odom/differentialWheelOdom.hpp @@ -1,14 +1,10 @@ #pragma once -#include "../chassis/tankChassis.hpp" #include "../hardware/rotationSensor.hpp" -#include "../utils/logger.hpp" #include "../geometry/pose.hpp" #include "odomSource.hpp" #include "poseVelocityCalculator.hpp" #include "pros/rtos.hpp" -#include "pros/error.h" #include -#include #define M_PI 3.14159265358979323846 @@ -26,8 +22,8 @@ namespace devils * @param wheelRadius The radius of the wheels in inches. * @param wheelBase The distance between the wheels in inches. */ - DifferentialWheelOdom(const double wheelRadius, - const double wheelBase) + DifferentialWheelOdom(const float wheelRadius, + const float wheelBase) : wheelRadius(wheelRadius), wheelBase(wheelBase) { @@ -46,7 +42,7 @@ namespace devils * Sets the current pose of the robot. * @param pose The pose to set the robot to. */ - void setPose(Pose pose) override + void setPose(const Pose pose) override { currentPose = pose; } @@ -56,29 +52,30 @@ namespace devils * @param leftRotations The left wheel rotations. * @param rightRotations The right wheel rotations. */ - void update(double leftRotations, double rightRotations) + void update( + const float leftRotations, + const float rightRotations) { // Get Delta Time - uint32_t deltaT = lastUpdateTimestamp - pros::millis(); lastUpdateTimestamp = pros::millis(); // Get Distance - double left = leftRotations * 2 * M_PI * wheelRadius; - double right = rightRotations * 2 * M_PI * wheelRadius; + const float left = leftRotations * 2 * M_PIF * wheelRadius; + const float right = rightRotations * 2 * M_PIF * wheelRadius; // Get Delta Distance - double deltaLeft = left - lastLeft; - double deltaRight = right - lastRight; + const float deltaLeft = left - lastLeft; + const float deltaRight = right - lastRight; lastLeft = left; lastRight = right; // Calculate Delta Distance - double deltaDistance = (deltaLeft + deltaRight) / 2; - double deltaRotation = (deltaLeft - deltaRight) / wheelBase; + const float deltaDistance = (deltaLeft + deltaRight) / 2; + const float deltaRotation = (deltaLeft - deltaRight) / wheelBase; // Calculate Delta X and Y - double deltaX = deltaDistance * std::cos(currentPose.rotation + deltaRotation / 2); - double deltaY = deltaDistance * std::sin(currentPose.rotation + deltaRotation / 2); + const float deltaX = deltaDistance * std::cos(currentPose.rotation + deltaRotation / 2); + const float deltaY = deltaDistance * std::sin(currentPose.rotation + deltaRotation / 2); // Update X, Y, and Rotation currentPose.x += deltaX; @@ -86,7 +83,7 @@ namespace devils currentPose.rotation += deltaRotation; // Update Velocity - PoseVelocityCalculator::updateVelocity(currentPose); + updateVelocity(currentPose); } PoseVelocity getVelocity() override @@ -95,13 +92,13 @@ namespace devils } private: - const double wheelRadius; - const double wheelBase; + const float wheelRadius; + const float wheelBase; Pose currentPose = Pose(); uint32_t lastUpdateTimestamp = 0; - double lastLeft = 0; - double lastRight = 0; + float lastLeft = 0; + float lastRight = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/odom/odomSource.hpp b/include/devils/odom/odomSource.hpp index faef002..621f79a 100644 --- a/include/devils/odom/odomSource.hpp +++ b/include/devils/odom/odomSource.hpp @@ -10,6 +10,7 @@ namespace devils */ struct OdomSource { + virtual ~OdomSource() = default; /** * Gets the current pose of the robot * @return The current pose of the robot diff --git a/include/devils/odom/parallelSensorOdom.hpp b/include/devils/odom/parallelSensorOdom.hpp index 4ec0569..14a5837 100644 --- a/include/devils/odom/parallelSensorOdom.hpp +++ b/include/devils/odom/parallelSensorOdom.hpp @@ -1,10 +1,7 @@ #pragma once #include "differentialWheelOdom.hpp" -#include "../utils/runnable.hpp" -#include "poseVelocityCalculator.hpp" - -#define M_PI 3.14159265358979323846 +#include "../utils/asyncTask.hpp" namespace devils { @@ -12,7 +9,7 @@ namespace devils * Represents an odometry system using a set of parallel rotation sensors. * If the sensors are perpendicular, use `PerpendicularSensorOdometry` instead. */ - class ParallelSensorOdometry : public DifferentialWheelOdom, public Runnable + class ParallelSensorOdometry : public DifferentialWheelOdom, public AsyncTask { public: /** @@ -22,34 +19,47 @@ namespace devils * @param wheelRadius The radius of the wheels in inches. * @param wheelBase The distance between the wheels in inches. */ - ParallelSensorOdometry(RotationSensor &leftSensor, - RotationSensor &rightSensor, - const double wheelRadius, - const double wheelBase) - : leftSensor(leftSensor), - rightSensor(rightSensor), - DifferentialWheelOdom(wheelRadius, wheelBase) + ParallelSensorOdometry(RotationSensor& leftSensor, + RotationSensor& rightSensor, + const float wheelRadius, + const float wheelBase) + : DifferentialWheelOdom(wheelRadius, wheelBase), + leftSensor(leftSensor), + rightSensor(rightSensor) { } void onUpdate() override { - double leftPosition = (leftSensor.getAngle() / (2 * M_PI)) / ticksPerRevolution; - double rightPosition = (rightSensor.getAngle() / (2 * M_PI)) / ticksPerRevolution; - DifferentialWheelOdom::update(leftPosition, rightPosition); + // Get sensor angles + const auto leftResult = leftSensor.getAngle(); + const auto rightResult = rightSensor.getAngle(); + + if (!leftResult.isSuccess() || + !rightResult.isSuccess()) + { + Logger::warn("Failed to get sensor angles for ParallelSensorOdometry."); + return; + } + + // Convert angles to wheel positions in revolutions + const float leftPosition = leftResult.value / (2 * M_PIF) / ticksPerRevolution; + const float rightPosition = rightResult.value / (2 * M_PIF) / ticksPerRevolution; + update(leftPosition, rightPosition); } /** * Sets the number of encoder ticks per full revolution of the wheels. + * @param newTicksPerRevolution The new ticks per revolution. */ - void setTicksPerRevolution(double ticksPerRevolution) + void setTicksPerRevolution(const float newTicksPerRevolution) { - this->ticksPerRevolution = ticksPerRevolution; + this->ticksPerRevolution = newTicksPerRevolution; } private: - double ticksPerRevolution = 300.0 * (48.0 / 36.0); // ticks - RotationSensor &leftSensor; - RotationSensor &rightSensor; + float ticksPerRevolution = 300.0 * (48.0 / 36.0); // ticks + RotationSensor& leftSensor; + RotationSensor& rightSensor; }; -} \ No newline at end of file +} diff --git a/include/devils/odom/perpendicularSensorOdom.hpp b/include/devils/odom/perpendicularSensorOdom.hpp index 2745495..8c919e1 100644 --- a/include/devils/odom/perpendicularSensorOdom.hpp +++ b/include/devils/odom/perpendicularSensorOdom.hpp @@ -1,9 +1,9 @@ #pragma once -#include "../utils/runnable.hpp" +#include "../utils/asyncTask.hpp" #include "../odom/odomSource.hpp" #include "../hardware/rotationSensor.hpp" -#include "../hardware/structs/gyro.h" +#include "../hardware/gyroBase.h" #include "poseVelocityCalculator.hpp" namespace devils @@ -12,7 +12,10 @@ namespace devils * Represents an odometry system using a set of perpendicular rotation sensors. * If the sensors are parallel, use `ParallelSensorOdometry` instead. */ - class PerpendicularSensorOdometry : public OdomSource, public Runnable, public PoseVelocityCalculator + class PerpendicularSensorOdometry : + public OdomSource, + public AsyncTask, + public PoseVelocityCalculator { public: /** @@ -22,60 +25,109 @@ namespace devils * @param wheelRadius The radius of the wheels in inches. */ PerpendicularSensorOdometry( - RotationSensor &verticalSensor, - RotationSensor &horizontalSensor, - const double wheelRadius) - : verticalSensor(verticalSensor), - horizontalSensor(horizontalSensor), - wheelRadius(wheelRadius) + RotationSensor& verticalSensor, + RotationSensor& horizontalSensor, + const float wheelRadius) + : wheelRadius(wheelRadius), + verticalSensor(verticalSensor), + horizontalSensor(horizontalSensor) { - lastUpdateTimestamp = pros::millis(); + verticalSensor.setPosition(0); + horizontalSensor.setPosition(0); } /** - * Updates the odometry from vertical and horizontal tracking wheels. + * Gets the current pose of the robot. */ - void onUpdate() override + Pose getPose() override { - // Get Sensor Angles in Degrees - double verticalAngle = verticalSensor.getAngle(); - bool isVerticalSensorError = (errno != 0); + return currentPose; + } - double horizontalAngle = horizontalSensor.getAngle(); - bool isHorizontalSensorError = (errno != 0); + /** + * Sets the current pose of the robot. + * @param pose The pose to set the robot to. + */ + void setPose(const Pose pose) override + { + currentPose = pose; + lastRotation = pose.rotation; - // Get Delta Time - uint32_t deltaT = lastUpdateTimestamp - pros::millis(); - lastUpdateTimestamp = pros::millis(); + if (imu != nullptr) + imu->setHeading(pose.rotation); + } - // Calculate arc length - // Arc Length = r * theta - double vertical = verticalAngle * wheelRadius; - double horizontal = horizontalAngle * wheelRadius; + /** + * Sets the IMU to use for odometry. + * @param newIMU The new IMU to use for odometry. + */ + void useIMU(IGyro* newIMU) + { + this->imu = newIMU; + } + + /** + * Sets the sensor offsets for the odometry system. + * Accounts for the difference in sensor placement on the robot. + * @param newVerticalSensorOffset The offset for the vertical sensor relative to the robot's center of rotation. + * @param newHorizontalSensorOffset The offset for the horizontal sensor relative to the robot's center of rotation. + */ + void setSensorOffsets(Vector2& newVerticalSensorOffset, + Vector2& newHorizontalSensorOffset) + { + this->verticalSensorOffset = &newVerticalSensorOffset; + this->horizontalSensorOffset = &newHorizontalSensorOffset; + } + + PoseVelocity getVelocity() override + { + return PoseVelocityCalculator::getVelocity(); + } + + protected: + /** + * Updates the odometry from vertical and horizontal tracking wheels. + */ + void onUpdate() override + { + // Get Sensor Angles in Degrees + const auto verticalResult = verticalSensor.getAngle(); + const auto horizontalResult = horizontalSensor.getAngle(); // Update IMU // Also calculate the change in rotation - double deltaRotation = 0; - if (imu != nullptr) + float deltaRotation = 0; + if (imu != nullptr && imu->getIsReady()) { - double heading = imu->getHeading(); - if (errno == 0) + const auto headingResult = imu->getHeading(); + if (headingResult.isSuccess()) { - deltaRotation = heading - lastRotation; - lastRotation = heading; - currentPose.rotation = heading; + deltaRotation = headingResult.value - lastRotation; + lastRotation = headingResult.value; + currentPose.rotation = lastRotation; } } + // Check for sensor errors + if (!verticalResult.isSuccess() || + !horizontalResult.isSuccess()) + { + Logger::warn("Failed to get sensor angles for PerpendicularSensorOdometry."); + return; + } + + // Calculate arc length + // rads * theta + const float verticalRevolutions = Units::degToRad(verticalResult.value) * wheelRadius; + const float horizontalRevolutions = Units::degToRad(horizontalResult.value) * wheelRadius; + // Get Delta Distance - double deltaVertical = vertical - lastVertical; - double deltaHorizontal = horizontal - lastHorizontal; + float deltaVertical = verticalRevolutions - lastVertical; + float deltaHorizontal = horizontalRevolutions - lastHorizontal; - // Check for sensor errors - if (!isVerticalSensorError) - lastVertical = vertical; - if (!isHorizontalSensorError) - lastHorizontal = horizontal; + // Update last sensor values + lastVertical = verticalRevolutions; + lastHorizontal = horizontalRevolutions; // Apply Sensor Offsets if (verticalSensorOffset != nullptr && @@ -83,103 +135,49 @@ namespace devils { // Calculate radius of rotation for each sensor // We only care about the x and y components since the sensors are perpendicular to the other axis - double verticalOffsetRadius = verticalSensorOffset->x; - double horizontalOffsetRadius = horizontalSensorOffset->y; + const float verticalOffsetRadius = verticalSensorOffset->x; + const float horizontalOffsetRadius = horizontalSensorOffset->y; // Calculate Arc Length // Arc Length = r * theta - double verticalArcLength = verticalOffsetRadius * deltaRotation; - double horizontalArcLength = horizontalOffsetRadius * deltaRotation; + const float verticalArcLength = verticalOffsetRadius * deltaRotation; + const float horizontalArcLength = horizontalOffsetRadius * deltaRotation; // Subtract Arc Length deltaVertical -= verticalArcLength; deltaHorizontal -= horizontalArcLength; } - // Check for sensor errors - if (isVerticalSensorError) - deltaVertical = 0; - if (isHorizontalSensorError) - deltaHorizontal = 0; - // Calculate trigonometric values - double rotation = currentPose.rotation; - double sin = std::sin(rotation); - double cos = std::cos(rotation); + const float rotation = currentPose.rotation; + const float sin = std::sin(rotation); + const float cos = std::cos(rotation); - double deltaX = deltaVertical * cos + deltaHorizontal * sin; - double deltaY = deltaVertical * sin - deltaHorizontal * cos; + const float deltaX = deltaVertical * cos + deltaHorizontal * sin; + const float deltaY = deltaVertical * sin - deltaHorizontal * cos; // Update Pose currentPose.x += deltaX; currentPose.y += deltaY; // Update Velocity - PoseVelocityCalculator::updateVelocity(currentPose); - } - - /** - * Gets the current pose of the robot. - */ - Pose getPose() override - { - return currentPose; - } - - /** - * Sets the current pose of the robot. - * @param pose The pose to set the robot to. - */ - void setPose(Pose pose) override - { - currentPose = pose; - - if (imu != nullptr) - imu->setHeading(pose.rotation); - } - - /** - * Sets the IMU to use for odometry. - * @param imu The IMU to use for odometry. - */ - void useIMU(IGyro *imu) - { - this->imu = imu; - } - - /** - * Sets the sensor offsets for the odometry system. - * Accounts for the difference in sensor placement on the robot. - * @param verticalSensorOffset The offset for the vertical sensor relative to the robot's center of rotation. - * @param horizontalSensorOffset The offset for the horizontal sensor relative to the robot's center of rotation. - */ - void setSensorOffsets(Vector2 &verticalSensorOffset, - Vector2 &horizontalSensorOffset) - { - this->verticalSensorOffset = &verticalSensorOffset; - this->horizontalSensorOffset = &horizontalSensorOffset; - } - - PoseVelocity getVelocity() override - { - return PoseVelocityCalculator::getVelocity(); + updateVelocity(currentPose); } private: - const double wheelRadius; - RotationSensor &verticalSensor; - RotationSensor &horizontalSensor; + const float wheelRadius; + RotationSensor& verticalSensor; + RotationSensor& horizontalSensor; - IGyro *imu = nullptr; + IGyro* imu = nullptr; Pose currentPose = Pose(); - uint32_t lastUpdateTimestamp = 0; - double lastVertical = 0; - double lastHorizontal = 0; - double lastRotation = 0; + float lastVertical = 0; + float lastHorizontal = 0; + float lastRotation = 0; - Vector2 *verticalSensorOffset = nullptr; - Vector2 *horizontalSensorOffset = nullptr; + Vector2* verticalSensorOffset = nullptr; + Vector2* horizontalSensorOffset = nullptr; }; -} \ No newline at end of file +} diff --git a/include/devils/odom/poseVelocityCalculator.hpp b/include/devils/odom/poseVelocityCalculator.hpp index 5e720f3..67ea36a 100644 --- a/include/devils/odom/poseVelocityCalculator.hpp +++ b/include/devils/odom/poseVelocityCalculator.hpp @@ -8,6 +8,8 @@ namespace devils class PoseVelocityCalculator { public: + virtual ~PoseVelocityCalculator() = default; + /** * Gets current velocity of the robot. * @return The current velocity of the robot as a `PoseVelocity`. @@ -23,11 +25,11 @@ namespace devils * Should be run whenever the current `pose` is updated. * @param pose The current pose of the robot. */ - void updateVelocity(Pose pose) + void updateVelocity(const Pose& pose) { // Calculate Time Delta - uint32_t timestamp = pros::millis(); - double dt = (timestamp - lastTimestamp) / 1000.0; + const auto timestamp = pros::millis(); + const auto dt = static_cast(timestamp - lastTimestamp) / 1000.0f; // Initial update if (lastTimestamp == 0) @@ -38,20 +40,20 @@ namespace devils } // Skip if no time has passed - if (dt <= 0) + if (dt <= MIN_DELTA_TIME) return; // Update Timestamp lastTimestamp = timestamp; // Calculate Linear Velocity - Vector2 positionDelta = pose - lastPose; - double linearVelocityX = positionDelta.x / dt; - double linearVelocityY = positionDelta.y / dt; + const Vector2 positionDelta = pose - lastPose; + const float linearVelocityX = positionDelta.x / dt; + const float linearVelocityY = positionDelta.y / dt; // Calculate Angular Velocity - double angleDelta = pose.rotation - lastPose.rotation; - double angularVelocity = angleDelta / dt; + const float angleDelta = pose.rotation - lastPose.rotation; + const float angularVelocity = angleDelta / dt; // Update Velocity currentVelocity = PoseVelocity(linearVelocityX, linearVelocityY, angularVelocity); @@ -61,6 +63,9 @@ namespace devils } private: + // Minimum time in seconds between velocity updates to prevent noise from small pose changes + static constexpr float MIN_DELTA_TIME = 0.05f; + // Current state PoseVelocity currentVelocity = PoseVelocity(); diff --git a/include/devils/odom/tankChassisOdom.hpp b/include/devils/odom/tankChassisOdom.hpp index 648d91a..9d95942 100644 --- a/include/devils/odom/tankChassisOdom.hpp +++ b/include/devils/odom/tankChassisOdom.hpp @@ -1,8 +1,7 @@ #pragma once #include "differentialWheelOdom.hpp" -#include "../utils/runnable.hpp" -#include "../hardware/structs/gyro.h" -#include "poseVelocityCalculator.hpp" +#include "../utils/backgroundService.hpp" +#include "../hardware/gyroBase.h" #define M_PI 3.14159265358979323846 @@ -11,7 +10,9 @@ namespace devils /** * Represents an odometry system using a tank chassis. */ - class TankChassisOdom : public DifferentialWheelOdom, public Runnable + class TankChassisOdom : + BackgroundService, + public DifferentialWheelOdom { public: /** @@ -21,50 +22,62 @@ namespace devils * @param wheelRadius The radius of the wheels in inches. * @param wheelBase The distance between the wheels in inches. */ - TankChassisOdom(TankChassis &chassis, - const double wheelRadius, - const double wheelBase) - : chassis(chassis), - DifferentialWheelOdom(wheelRadius, wheelBase) + TankChassisOdom(TankChassis& chassis, + const float wheelRadius, + const float wheelBase) + : DifferentialWheelOdom(wheelRadius, wheelBase), + chassis(chassis) { } - void onUpdate() override - { - // Process differential odometry - double leftPosition = chassis.getLeftMotors().getPosition() / ticksPerRevolution; - double rightPosition = chassis.getRightMotors().getPosition() / ticksPerRevolution; - DifferentialWheelOdom::update(leftPosition, rightPosition); - - // Apply IMU - if (imu != nullptr) - { - Pose currentPose = getPose(); - currentPose.rotation = imu->getHeading(); - setPose(currentPose); - } - } - /** * Sets the number of encoder ticks per full revolution of the wheels. + * @param newTicksPerRevolution The new ticks per revolution. */ - void setTicksPerRevolution(double ticksPerRevolution) + void setTicksPerRevolution(const float newTicksPerRevolution) { - this->ticksPerRevolution = ticksPerRevolution; + this->ticksPerRevolution = newTicksPerRevolution; } /** * Sets the IMU to use for odometry. - * @param imu The IMU to use. + * @param newIMU The new IMU to use. */ - void useIMU(IGyro *imu) + void useIMU(IGyro* newIMU) + { + this->imu = newIMU; + } + + protected: + void onUpdate() override { - this->imu = imu; + // Get encoder positions + const auto leftResult = chassis.getLeftMotors().getPosition(); + const auto rightResult = chassis.getRightMotors().getPosition(); + + if (!leftResult.isSuccess() || + !rightResult.isSuccess()) + { + Logger::warn("Failed to get encoder positions for TankChassisOdom."); + return; + } + + // Convert ticks to wheel rotations and update odometry + update(leftResult.value / ticksPerRevolution, + rightResult.value / ticksPerRevolution); + + // Apply IMU + if (imu != nullptr && imu->getIsReady()) + { + Pose currentPose = getPose(); + currentPose.rotation = imu->getHeading(); + setPose(currentPose); + } } private: - double ticksPerRevolution = 300.0 * (48.0 / 36.0); // ticks - TankChassis &chassis; - IGyro *imu = nullptr; + float ticksPerRevolution = 300.0 * (48.0 / 36.0); // ticks + TankChassis& chassis; + IGyro* imu = nullptr; }; -} \ No newline at end of file +} diff --git a/include/devils/odom/visionTargetOdom.hpp b/include/devils/odom/visionTargetOdom.hpp deleted file mode 100644 index 01b348c..0000000 --- a/include/devils/odom/visionTargetOdom.hpp +++ /dev/null @@ -1,131 +0,0 @@ -#pragma once - -#include "odomSource.hpp" -#include "../hardware/structs/camera.h" -#include "../geometry/units.hpp" -#include -#include - -namespace devils -{ - /** - * Offsets the odometry of the robot based on the vision target. - * Corrects for slight deviations in the robot's position by using the vision target as a reference. - */ - class VisionTargetOdom : public OdomSource - { - public: - /** - * Constructs a new vision target with the given expected pose - * @param baseOdom The base odometry source to use - * @param camera The camera to use - * @param cameraFOV The camera's field of view in degrees - * @param expectedPose The expected pose of the vision target. - */ - VisionTargetOdom( - OdomSource &baseOdom, - std::shared_ptr camera, - double cameraFOV, - double latency = 0.0, - Pose expectedPose = Pose(0, 0, 0)) - : baseOdom(baseOdom), - delayedOdom(baseOdom, latency), - camera(camera), - expectedPose(expectedPose), - cameraFOV(Units::degToRad(cameraFOV)) - { - if (!camera) - throw std::invalid_argument("Camera cannot be null"); - } - - /** - * Converts a vision target in the camera's view to an absolute pose. - * @param target The vision target to convert - * @return The estimated pose of the target - */ - Pose visionTargetToPose(const ICamera::VisionObject &target) - { - // Get pose of the robot at the time of the target's detection (delayed odometry) - Pose robotPose = delayedOdom.getPose(); - - // Calculate the angle of the target in degrees - double targetAngle = 0.5 * cameraFOV * target.x + robotPose.rotation; - - // Calculate the offset from the camera to the target - // TODO: Estimate the distance of the target using the vision target's size - Vector2 targetOffset = Vector2( - TARGET_DISTANCE_TO_CAMERA * std::cos(targetAngle), - TARGET_DISTANCE_TO_CAMERA * std::sin(targetAngle)); - - // Append the offset to the robot's pose - return Pose( - robotPose.x + targetOffset.x, - robotPose.y + targetOffset.y, - 0); // <-- Must be 0 to avoid appending rotation to the odometry - } - - Pose getPose() override - { - // Get the current pose of the robot - Pose robotPose = baseOdom.getPose(); - - // Fallback to base odometry if no targets are found - if (!camera->hasTargets()) - return robotPose + lastOffset; - - // Get the pose of the closest target - auto visionTarget = camera->getClosestTarget(); - auto targetPose = visionTargetToPose(visionTarget); - - // Check if the target is too far away - if (targetPose.distanceTo(expectedPose) > MAX_DISTANCE) - return robotPose + lastOffset; - - // Calculate the offset from the expected pose to the target pose - Pose offset = expectedPose - targetPose; - lastOffset = offset; - - // Correct the robot's pose using the offset - return robotPose + offset; - } - - void setPose(Pose pose) override - { - baseOdom.setPose(pose); - } - - PoseVelocity getVelocity() override - { - return baseOdom.getVelocity(); - } - - /** - * Change the expected pose of the vision target. - * Used if using the same vision target in multiple locations. - * @param pose The new expected pose of the robot - */ - void setExpectedPose(Pose pose) - { - expectedPose = pose; - } - - private: - /// @brief The target's maximum distance from the expected pose to be considered valid - static constexpr double MAX_DISTANCE = 6.0; - - /// @brief A vision target's *assumed* distance to the camera in inches - static constexpr double TARGET_DISTANCE_TO_CAMERA = 8.0; - - OdomSource &baseOdom; - DelayedOdom delayedOdom; - std::shared_ptr camera = nullptr; - double cameraFOV; // radians - - /// @brief The last offset applied to the robot's pose - /// @details This is used to correct the robot's pose when no targets are found - Pose lastOffset = Pose(0, 0, 0); - - /// @brief The expected pose of the vision target - Pose expectedPose; - }; -} \ No newline at end of file diff --git a/include/devils/path/linearPath.hpp b/include/devils/path/linearPath.hpp index 591b710..e97c119 100644 --- a/include/devils/path/linearPath.hpp +++ b/include/devils/path/linearPath.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include "path.hpp" #include "../geometry/pose.hpp" @@ -18,11 +19,11 @@ namespace devils * @param poses The list of poses to interpolate between */ LinearPath(std::vector poses) - : poses(poses) + : poses(std::move(poses)) { } - Pose getPoseAt(double index) override + Pose getPoseAt(const float index) override { // Check OOB if (index <= 0) @@ -31,15 +32,16 @@ namespace devils return poses.back(); // Get the two poses to interpolate between - Pose prevPose = poses[(int)index]; - Pose nextPose = poses[(int)index + 1]; + const int intIndex = static_cast(index); + const Pose prevPose = poses[intIndex]; + const Pose nextPose = poses[intIndex + 1]; // Interpolate between the two poses - double t = index - (int)index; + const float t = index - intIndex; return Lerp::linearPoints(prevPose, nextPose, t); } - double getLength() override + float getLength() override { return poses.size(); } @@ -47,4 +49,4 @@ namespace devils private: std::vector poses; }; -} \ No newline at end of file +} diff --git a/include/devils/path/path.hpp b/include/devils/path/path.hpp index f360779..e40e3a0 100644 --- a/include/devils/path/path.hpp +++ b/include/devils/path/path.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include "../geometry/pose.hpp" namespace devils @@ -10,17 +9,19 @@ namespace devils */ struct Path { + virtual ~Path() = default; + /** * Gets an interpolated pose at a given index * @param index The index of the pose * @return An interpolated pose at the given index */ - virtual Pose getPoseAt(double index) = 0; + virtual Pose getPoseAt(float index) = 0; /** * Gets the length of the path * @return The length of the path in indices */ - virtual double getLength() = 0; + virtual float getLength() = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/path/splinePath.hpp b/include/devils/path/splinePath.hpp index 0dd3eb1..fa4e2b9 100644 --- a/include/devils/path/splinePath.hpp +++ b/include/devils/path/splinePath.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include "path.hpp" #include "splinePose.hpp" @@ -19,9 +20,11 @@ namespace devils * @param isReversed True if the path is reversed * @return The spline path */ - SplinePath(std::vector poses, bool isReversed = false) + SplinePath( + std::vector poses, + const bool isReversed = false) : isReversed(isReversed), - poses(poses) + poses(std::move(poses)) { } @@ -33,15 +36,19 @@ namespace devils * @param isReversed True if the path is reversed * @return The spline path */ - static SplinePath makeArc(Pose from, Pose to, double delta = 18.0, bool isReversed = false) + static SplinePath makeArc( + const Pose& from, + const Pose& to, + float delta = 6.0, + bool isReversed = false) { if (isReversed) delta *= -1; std::vector poses; - poses.push_back(SplinePose(from.x, from.y, from.rotation, delta, delta)); - poses.push_back(SplinePose(to.x, to.y, to.rotation, delta, delta)); - return SplinePath(poses, isReversed); + poses.emplace_back(from.x, from.y, from.rotation, delta, delta); + poses.emplace_back(to.x, to.y, to.rotation, delta, delta); + return {poses, isReversed}; } /** @@ -49,25 +56,25 @@ namespace devils * @param index The index to get the pose at. Interpolate between indices. * @return The pose at the index */ - Pose getPoseAt(double index) override + Pose getPoseAt(const float index) override { // Check OOB if (index <= 0) - return poses.front(); + return static_cast(poses.front()); if (index >= poses.size() - 1) - return poses.back(); + return static_cast(poses.back()); // Get the two poses to interpolate between - int prevIndex = (int)index; - int nextIndex = prevIndex + 1; - SplinePose prevPose = poses[prevIndex]; - SplinePose nextPose = poses[nextIndex]; - Pose prevAnchor = prevPose.getExitAnchor(); - Pose nextAnchor = nextPose.getEntryAnchor(); + const int prevIndex = static_cast(index); + const int nextIndex = prevIndex + 1; + const SplinePose prevPose = poses[prevIndex]; + const SplinePose nextPose = poses[nextIndex]; + const Pose prevAnchor = prevPose.getExitAnchor(); + const Pose nextAnchor = nextPose.getEntryAnchor(); // Calculate dt between the two poses // This is the percentage of the way between the two poses - double dt = index - prevIndex; + const float dt = index - static_cast(prevIndex); // Interpolate between the two poses // using cubic interpolation @@ -78,7 +85,7 @@ namespace devils nextPose, dt); - Pose interpolatedPosePrime = Lerp::cubicPoints( + const Pose interpolatedPosePrime = Lerp::cubicPoints( prevPose, prevAnchor, nextAnchor, @@ -87,14 +94,14 @@ namespace devils // Calculate rotation // This is the instantaneous derivative of the pose at the given index - double rotation = std::atan2( + const float rotation = std::atan2( interpolatedPosePrime.y - interpolatedPose.y, interpolatedPosePrime.x - interpolatedPose.x); interpolatedPose.rotation = rotation; // Reverse the rotation if the path is reversed if (isReversed) - interpolatedPose.rotation = Units::normalizeRadians(interpolatedPose.rotation + M_PI); + interpolatedPose.rotation = Units::normalizeRadians(interpolatedPose.rotation + M_PIF); return interpolatedPose; } @@ -103,14 +110,14 @@ namespace devils * Gets the length of the path * @return The length of the path in control points */ - double getLength() override + float getLength() override { - return poses.size(); + return static_cast(poses.size()); } private: /// @brief Step size used to calculate pose rotation - static constexpr double DELTA_INDEX = 0.0001; + static constexpr float DELTA_INDEX = 0.0001; /// @brief True if the path is reversed bool isReversed = false; @@ -118,4 +125,4 @@ namespace devils /// @brief The list of poses to interpolate between std::vector poses; }; -} \ No newline at end of file +} diff --git a/include/devils/path/splinePose.hpp b/include/devils/path/splinePose.hpp index f6f18bd..05fede9 100644 --- a/include/devils/path/splinePose.hpp +++ b/include/devils/path/splinePose.hpp @@ -8,48 +8,52 @@ namespace devils * Represents a pose with additional spline data. * Used in `SplinePath` to define control points. */ - struct SplinePose : public Pose + struct SplinePose : Pose { - SplinePose() : Pose() {} + SplinePose() = default; SplinePose( - double x, - double y, - double rotation, - double entryDelta, - double exitDelta) + const float x, + const float y, + const float rotation, + const float entryDelta, + const float exitDelta) : Pose(x, y, rotation), entryDelta(entryDelta), - exitDelta(exitDelta) {} + exitDelta(exitDelta) + { + } /// @brief Distance of anchor point in inches from the start of the spline - double entryDelta = 0; + float entryDelta = 0; /// @brief Distance of anchor point in inches from the end of the spline - double exitDelta = 0; + float exitDelta = 0; /** * Gets the pose of the entry anchor point of the spline * @return The entry anchor pose */ - Pose getEntryAnchor() + Pose getEntryAnchor() const { - return Pose( - x + std::cos(rotation) * -entryDelta, - y + std::sin(rotation) * -entryDelta, - rotation); + return { + x + cosf(rotation) * -entryDelta, + y + sinf(rotation) * -entryDelta, + rotation + }; } /** * Gets the pose of the exit anchor point of the spline * @return The exit anchor pose */ - Pose getExitAnchor() + Pose getExitAnchor() const { - return Pose( + return { x + std::cos(rotation) * exitDelta, y + std::sin(rotation) * exitDelta, - rotation); + rotation + }; } }; -} \ No newline at end of file +} diff --git a/include/devils/trajectory/trajectory.hpp b/include/devils/trajectory/trajectory.hpp index db1af2a..4cf229a 100644 --- a/include/devils/trajectory/trajectory.hpp +++ b/include/devils/trajectory/trajectory.hpp @@ -13,26 +13,26 @@ namespace devils struct Point { /// @brief The current time in seconds - double t; + float t = 0; /// @brief The current position Pose pose; /// @brief The translation velocity in inches per second - double velocity; + float velocity = 0; /// @brief The angular acceleration in radians per second squared - double angularVelocity; + float angularVelocity = 0; /// @brief The linear acceleration in inches per second squared - double acceleration; + float acceleration = 0; }; /** * Creates a new instance of a trajectory * @param trajectoryPoints A list of generated points along the trajectory */ - Trajectory(std::unique_ptr> trajectoryPoints) + explicit Trajectory(std::unique_ptr> trajectoryPoints) : trajectoryPoints(std::move(trajectoryPoints)) { } @@ -41,7 +41,7 @@ namespace devils * Gets the duration of the trajectory in seconds * @return The duration of the trajectory in seconds */ - double duration() const + float duration() const { return trajectoryPoints->back().t; } @@ -52,7 +52,7 @@ namespace devils * @param t The time in seconds * @return The trajectory state at the given time */ - Point getStateAt(const double t) + Point getStateAt(const float t) const { // Check if the trajectory is empty if (!trajectoryPoints || trajectoryPoints->empty()) @@ -66,16 +66,16 @@ namespace devils for (size_t i = 0; i < trajectoryPoints->size() - 1; i++) { // Get the two states - auto previousState = trajectoryPoints->at(i); - auto nextState = trajectoryPoints->at(i + 1); + const auto previousState = trajectoryPoints->at(i); + const auto nextState = trajectoryPoints->at(i + 1); // Check if the time is between the two states if (t >= previousState.t && t <= nextState.t) { // Calculate the interpolation time - double deltaTime = nextState.t - previousState.t; - double timeBetween = t - previousState.t; - double interpolationTime = timeBetween / deltaTime; + const float deltaTime = nextState.t - previousState.t; + const float timeBetween = t - previousState.t; + const float interpolationTime = timeBetween / deltaTime; // Interpolate between the two states return lerpStates(previousState, nextState, interpolationTime); @@ -94,7 +94,10 @@ namespace devils * @param t The time to interpolate (0 to 1) * @return The interpolated state */ - Point lerpStates(const Point a, const Point b, const double t) const + static Point lerpStates( + const Point& a, + const Point& b, + const float t) { Point state; state.t = t; @@ -108,4 +111,4 @@ namespace devils private: std::unique_ptr> trajectoryPoints; }; -} \ No newline at end of file +} diff --git a/include/devils/trajectory/trajectoryConstraints.hpp b/include/devils/trajectory/trajectoryConstraints.hpp index 5fdc533..8d2ce09 100644 --- a/include/devils/trajectory/trajectoryConstraints.hpp +++ b/include/devils/trajectory/trajectoryConstraints.hpp @@ -9,12 +9,20 @@ namespace devils struct TrajectoryConstraints { /// @brief The maximum velocity of the robot in inches per second - double maxVelocity = 36; // in/s + float maxVelocity = 30.0f; // in/s /// @brief The maximum acceleration of the robot in inches per second squared - double maxAcceleration = 48; // in/s^2 + float maxAcceleration = 48.0f; // in/s^2 /// @brief The maximum deceleration of the robot in inches per second squared - double maxDeceleration = 100; // in/s^2 + float maxDeceleration = 100.0f; // in/s^2 + + /// @brief The maximum rotational velocity of the robot in degrees per second + float rotationalMaxVelocity = 90.0f; // deg/s + + /// @brief The default constraints for trajectory generation + static TrajectoryConstraints defaultConstraints; }; -} \ No newline at end of file +} + +devils::TrajectoryConstraints devils::TrajectoryConstraints::defaultConstraints = {}; diff --git a/include/devils/trajectory/trajectoryGenerator.hpp b/include/devils/trajectory/trajectoryGenerator.hpp index 3dfac94..7b49067 100644 --- a/include/devils/trajectory/trajectoryGenerator.hpp +++ b/include/devils/trajectory/trajectoryGenerator.hpp @@ -2,14 +2,12 @@ #include #include -#include #include -#include #include "trajectory.hpp" #include "trajectoryConstraints.hpp" #include "../path/path.hpp" #include "../geometry/units.hpp" -#include "../utils/math.hpp" +#include "../geometry/math.hpp" namespace devils { @@ -23,10 +21,10 @@ namespace devils struct PathInfo { /// @brief The initial velocity in inches per second - double startingVelocity = 0; + float startingVelocity = 0; /// @brief The final velocity in inches per second - double endingVelocity = 0; + float endingVelocity = 0; }; /** @@ -34,7 +32,9 @@ namespace devils * @param constraints The robot constraints * @param pathInfo The path information */ - TrajectoryGenerator(TrajectoryConstraints constraints, PathInfo pathInfo) + explicit TrajectoryGenerator( + const PathInfo pathInfo, + const TrajectoryConstraints& constraints = TrajectoryConstraints::defaultConstraints) : constraints(constraints), pathInfo(pathInfo) { @@ -52,11 +52,12 @@ namespace devils * @param path The path to generate the trajectory from * @return The generated trajectory */ - std::shared_ptr calc(Path &path) + std::shared_ptr calc(Path& path) const { // The velocities at each step auto points = std::make_unique>(); - points->reserve(path.getLength() / DELTA_INDEX + 1); + auto length = path.getLength() / DELTA_INDEX + 1; + points->reserve(static_cast(length) + 1); // Initial Robot State Trajectory::Point previousPoint = { @@ -64,31 +65,43 @@ namespace devils path.getPoseAt(0), pathInfo.startingVelocity, 0, - constraints.maxAcceleration}; + constraints.maxAcceleration + }; points->push_back(previousPoint); // FORWARD PASS // Iterate through the path and constrain the velocity in the acceleration phase - for (double i = 0; i <= path.getLength() - 1 + DELTA_INDEX; i += DELTA_INDEX) + for (float i = 0; i <= path.getLength() - 1 + DELTA_INDEX; i += DELTA_INDEX) { // Get pose Pose currentPose = path.getPoseAt(i); // Calculate distance - double deltaDistance = currentPose.distanceTo(previousPoint.pose); + const float deltaDistance = currentPose.distanceTo(previousPoint.pose); // Calculate velocity // v_f = sqrt(v_i^2 + 2*a*d) - double velocity = sqrt(pow(previousPoint.velocity, 2) + 2 * constraints.maxAcceleration * deltaDistance); + float velocity = previousPoint.velocity * previousPoint.velocity + + 2 * constraints.maxAcceleration * deltaDistance; + velocity = sqrtf(velocity); - // Clamp velocity + // Clamp velocity to constraints velocity = std::min(velocity, constraints.maxVelocity); + // Clamp velocity to rotational constraints + const float curvature = std::abs(currentPose.curvature(previousPoint.pose)); + if (curvature > 0) + { + const float maxVelocityFromRotation = Units::degToRad(constraints.rotationalMaxVelocity) / + curvature; + velocity = std::min(velocity, maxVelocityFromRotation); + } + // Calculate dot product to determine if the robot is moving forward or backward - double prevDotCurrentPose = cos(previousPoint.pose.rotation) * - (currentPose.x - previousPoint.pose.x) + - sin(previousPoint.pose.rotation) * - (currentPose.y - previousPoint.pose.y); + const float prevDotCurrentPose = cosf(previousPoint.pose.rotation) * + (currentPose.x - previousPoint.pose.x) + + sinf(previousPoint.pose.rotation) * + (currentPose.y - previousPoint.pose.y); // If the dot product is negative, we are moving backwards if (prevDotCurrentPose < 0) @@ -103,7 +116,8 @@ namespace devils currentPose, velocity, 0, - 0}; + 0 + }; points->push_back(point); // Update previous point @@ -115,32 +129,34 @@ namespace devils // REVERSE PASS // Iterate through the path and constrain the velocity in the deceleration phase - for (double i = points->size() - 1; i >= 0; i--) + for (int i = static_cast(points->size()) - 1; i >= 0; i--) { // Grab existing point - auto &point = points->at(i); + auto& point = points->at(i); // Calculate distance - double deltaDistance = point.pose.distanceTo(previousPoint.pose); + const float deltaDistance = point.pose.distanceTo(previousPoint.pose); // Calculate velocity // Note: Uses deceleration instead of acceleration since // we are stepping from the end of the path to the beginning // v_f = sqrt(v_i^2 + 2*a*d) - double velocity = sqrt(pow(previousPoint.velocity, 2) + 2 * constraints.maxDeceleration * deltaDistance); + float velocity = previousPoint.velocity * previousPoint.velocity + + 2 * constraints.maxDeceleration * deltaDistance; + velocity = sqrtf(velocity); // Calculate dot product to determine if the robot is moving forward or backward - double prevDotCurrentPose = cos(point.pose.rotation) * - (previousPoint.pose.x - point.pose.x) + - sin(point.pose.rotation) * - (previousPoint.pose.y - point.pose.y); + const float prevDotCurrentPose = cosf(point.pose.rotation) * + (previousPoint.pose.x - point.pose.x) + + sinf(point.pose.rotation) * + (previousPoint.pose.y - point.pose.y); // If the dot product is negative, we are moving backwards if (prevDotCurrentPose < 0) velocity = -velocity; // Clamp velocity to existing point - velocity = Math::minMagnitude(velocity, point.velocity); + velocity = Math::smallestMagnitude({velocity, point.velocity}); // Update previous point point.velocity = velocity; @@ -152,24 +168,28 @@ namespace devils // FINAL PASS // Calculate the time, acceleration, and angular velocity for each point - for (int i = 1; i < points->size(); i++) + for (size_t i = 1; i < points->size(); i++) { // Get points - auto &point = points->at(i); + auto& point = points->at(i); // Calculate distance - double deltaDistance = point.pose.distanceTo(previousPoint.pose); + const float deltaDistance = point.pose.distanceTo(previousPoint.pose); // Calculate actual acceleration // a = (v_f^2 - v_i^2) / (2 * d) - double acceleration = (pow(point.velocity, 2) - pow(previousPoint.velocity, 2)) / (2 * deltaDistance); + float acceleration = point.velocity * point.velocity - + previousPoint.velocity * previousPoint.velocity; + acceleration /= 2 * deltaDistance; + if (std::isnan(acceleration)) acceleration = 0; // Calculate delta time // 0 = (1/2)at^2 + v_i*t - d // t = (-v_i + sqrt(v_i^2 + 2ad)) / a - double deltaTime = std::pow(previousPoint.velocity, 2) + 2 * acceleration * deltaDistance; + float deltaTime = previousPoint.velocity * previousPoint.velocity; + deltaTime += 2 * acceleration * deltaDistance; deltaTime = (-std::abs(previousPoint.velocity) + std::sqrt(deltaTime)) / acceleration; // If the time is NaN, acceleration is 0 @@ -181,8 +201,12 @@ namespace devils if (std::isnan(deltaTime)) deltaTime = 0; + // Safety check to prevent large jumps in time due to numerical instability + if (deltaTime > 1.0f) + deltaTime = 0.0f; // Cap delta time to prevent large jumps + // Calculate angular velocity - double angularVelocity = Units::diffRad(point.pose.rotation, previousPoint.pose.rotation) / deltaTime; + float angularVelocity = Units::diffRad(point.pose.rotation, previousPoint.pose.rotation) / deltaTime; if (std::isnan(angularVelocity)) angularVelocity = 0; @@ -200,7 +224,7 @@ namespace devils private: /// @brief The step size in path indices - static constexpr double DELTA_INDEX = 0.01; + static constexpr float DELTA_INDEX = 0.01; /// @brief Constraints of trajectory generation TrajectoryConstraints constraints; @@ -208,4 +232,4 @@ namespace devils /// @brief The path information PathInfo pathInfo; }; -} \ No newline at end of file +} diff --git a/include/devils/trajectory/trapezoidMotionProfile.hpp b/include/devils/trajectory/trapezoidMotionProfile.hpp deleted file mode 100644 index ea29f16..0000000 --- a/include/devils/trajectory/trapezoidMotionProfile.hpp +++ /dev/null @@ -1,187 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include "trajectoryConstraints.hpp" - -namespace devils -{ - /** - * Represents a 1-dimensional trapezoidal motion profile - */ - class TrapezoidMotionProfile - { - public: - /// @brief Path parameters - struct PathInfo - { - /// @brief The initial velocity in inches per second - double startingVelocity; - - /// @brief The final velocity in inches per second - double endingVelocity; - - /// @brief The distance to the goal in inches - double goalDistance; - }; - - /// @brief Point along the profile - struct ProfilePoint - { - /// @brief The position in inches - double position; - - /// @brief The velocity in inches per second - double velocity; - }; - - /** - * Creates a new trapezoidal motion profile. - * Make sure to call `calc()` to calculate the profile before using it. - * @param constraints The robot constraints - * @param pathInfo The path information - */ - TrapezoidMotionProfile(TrajectoryConstraints constraints, PathInfo pathInfo) - : constraints(constraints), - pathInfo(pathInfo) - { - } - - /** - * Calculates the motion profile. - * This is a resource-intensive operation and should be called sparingly. - */ - void calc() - { - // Check if we've already calculated the profile - if (accelVelocities.size() > 0 && decelVelocities.size() > 0) - return; - - // Calculate acceleration phase - // This will create the acceleration segment of the S-Curve - accelVelocities = calculateAccelVelocities(pathInfo.startingVelocity); - - // Calculate deceleration phase - // This will create the deceleration segment of the S-Curve - decelVelocities = calculateAccelVelocities(pathInfo.endingVelocity); - } - - double getSpeed(double position) - { - // Get each velocity - double accelVelocity = getVelocityAtPosition(accelVelocities, position); - double decelVelocity = getVelocityAtPosition(decelVelocities, pathInfo.goalDistance - position); - - // Clamp to the minimum of the two curves - return std::min(accelVelocity, decelVelocity); - } - - protected: - /** - * Steps through the list of profile points and returns the velocity at the given position. - * Lerps between the two closest points. - * @param velocities The list of profile points - * @param position The current position in inches - * @return The target velocity in inches per second - */ - double getVelocityAtPosition(std::vector &velocities, double position) - { - // Check if we've calculated the profile - if (velocities.size() == 0) - throw std::runtime_error("No velocities calculated. Call `calc()` first."); - - // Iterate through the list of velocities - // TODO: Replace with binary search - for (size_t i = 1; i < velocities.size(); i++) - { - // Check if we've passed the position - if (velocities[i].position >= position) - { - // Get the two closest points - auto &prev = velocities[i - 1]; - auto &next = velocities[i]; - - // Lerp position - double t = (position - prev.position) / (next.position - prev.position); - t = std::clamp(t, 0.0, 1.0); - - // Lerp velocity - return std::lerp(prev.velocity, next.velocity, t); - } - } - - // Fallback to the last point - return velocities.back().velocity; - } - - /** - * Calculates a list of profile points by accelerating from `startingVelocity` to the goal distance. - * @param startingVelocity The initial velocity in inches per second - * @return The list of profile points sequential order - */ - std::vector calculateAccelVelocities(double startingVelocity) - { - // The velocities at each step - std::vector velocities; - - // Current Robot State - double currentAcceleration = constraints.maxAcceleration; - double currentVelocity = startingVelocity; - double currentPosition = 0; - double currentTime = 0; - - // Step forward from 0 to goal distance - while (currentPosition < pathInfo.goalDistance) - { - // Calculate velocity - currentVelocity += currentAcceleration * DELTA_T; - currentVelocity = std::min(currentVelocity, constraints.maxVelocity); // Constrain velocity - - // Update current distance - currentPosition += currentVelocity * DELTA_T; - - // Update time - currentTime += DELTA_T; - - // Append to velocities - velocities.push_back(ProfilePoint{currentPosition, currentVelocity}); - - // Abort if we reach max velocity - // This saves processing time by ignoring vestigial points - if (currentVelocity >= constraints.maxVelocity) - break; - - // Abort if we exceed max time - // This is a safety check to prevent infinite loops - if (currentTime > MAX_TIME) - throw std::runtime_error("Exceeded max timespan calculating motion profile. Double-check constraints."); - } - - return velocities; - } - - private: - /// @brief The step size in seconds - static constexpr double DELTA_T = 0.1; - - /// @brief The step size in inches - static constexpr double DELTA_POS = 0.1; - - /// @brief The maximum allowed time in seconds - static constexpr double MAX_TIME = 20.0; - - /// @brief The velocities along the acceleration curve - std::vector accelVelocities; - - /// @brief The velocities along the deceleration curve - std::vector decelVelocities; - - /// @brief Constraints of trajectory generation - TrajectoryConstraints constraints; - - /// @brief The path information - PathInfo pathInfo; - }; -} \ No newline at end of file diff --git a/include/devils/utils/asyncTask.hpp b/include/devils/utils/asyncTask.hpp new file mode 100644 index 0000000..dde0eec --- /dev/null +++ b/include/devils/utils/asyncTask.hpp @@ -0,0 +1,253 @@ +#pragma once +#include "pros/rtos.hpp" +#include +#include "logger.hpp" + +namespace devils +{ + /// @brief The current state of an `AsyncTask`. + enum AsyncTaskState + { + // `onStart` is currently executing + STARTING, + + // The task is running and `onUpdate` is being called every 20ms + STARTED, + + // `onStop` is currently executing + STOPPING, + + // The task is not running + STOPPED, + }; + + /** + * Represents an asynchronous object that executes asynchronously from the main program. + * Contains methods for `onStart`, `onUpdate`, `onStop`, and `checkFinished`. + * + * \note + * Any classes derived from `AsyncTask` must be managed by `std::shared_ptr` AND + * publically inherit from `AsyncTask` to properly up-cast when using `shared_from_this()`. + */ + class AsyncTask : public std::enable_shared_from_this + { + public: + AsyncTask() = default; + + AsyncTask(const std::string& debugName) : AsyncTask() + { + setDebugName(debugName); + } + + virtual ~AsyncTask() + { + stop(); + } + + /** + * If the `AsyncTask` is running, it stops it. + * Does nothing if `getState()` is not `STARTED`. + * + * When stopped, `onStop()` is called once and the task is no longer updated. + * Use `start()` to start the task again. + */ + void stop() + { + if (state == STOPPING || state == STOPPED) + return; + + // Set state to STOPPING + stateMutex.take(1000); + state = STOPPING; + + // Call onStop + onStop(); + + // Set the state to STOPPED + state = STOPPED; + stateMutex.give(); + } + + /** + * If the `AsyncTask` is not running, it starts it. + * Does nothing if `getState()` is not `STOPPED`. + * + * When started, `onStart()` is called once, and then `onUpdate()` is called every 20ms by a hidden background task. + * Use `stop()` to stop the task at any time. + * + * @return A shared pointer to the current `AsyncTask` instance. + */ + std::shared_ptr start() + { + if (state == STARTING || state == STARTED) + return shared_from_this(); + + // Set state to STARTING + stateMutex.take(1000); + state = STARTING; + + // Add this task to the all running instances list + allRunningInstancesMutex.take(); + allRunningInstances.push_back(shared_from_this()); + allRunningInstancesMutex.give(); + + // Start the background task if it hasn't been started yet + if (!backgroundTask) + backgroundTask = std::make_unique(prosBackgroundTask, "AsyncTask Background Task"); + + // Call onStart + onStart(); + + // Set the state to STARTED + state = STARTED; + stateMutex.give(); + + return shared_from_this(); + } + + /** + * Runs the `AsyncTask` in the current thread. + * Blocks the current thead until the task is finished. + * @return A shared pointer to the current `AsyncTask` instance. + */ + std::shared_ptr startSync() + { + start(); + return join(); + } + + /** + * Blocks the current thread until the task is fully stopped. + * If the task is already stopped, returns immediately. + * + * @return A shared pointer to the current `AsyncTask` instance. + */ + std::shared_ptr join() + { + while (state != STOPPED) + pros::delay(UPDATE_INTERVAL); + + return shared_from_this(); + } + + /** + * Gets the current state of the task. + * @return The current state of the task. + */ + AsyncTaskState getState() const + { + return state; + } + + /** + * Gets a list of all active `AsyncTask` instances. + * @return A vector of weak pointers to all active `AsyncTask` instances. + */ + static std::vector> getAllRunningInstances() + { + return allRunningInstances; + } + + protected: + /// @brief Called before the task starts. + virtual void onStart() + { + } + + /// @brief Called every 20ms to update the task state. + virtual void onUpdate() + { + }; + + /// @brief Called before the task stops. + virtual void onStop() + { + }; + + /// @brief Called every update to check if the task is finished. If so, `stop()` is called automatically. Default implementation always returns false. + /// @return True if the task is finished, false otherwise. + virtual bool checkFinished() { return false; }; + + /// @brief Gets the debug name of the task. + std::string getDebugName() const + { + return debugName; + } + + /// @brief Sets the debug name of the task. + void setDebugName(const std::string& name) + { + debugName = name; + } + + private: + /** + * The hidden background task that updates all active AsyncTask instances. + * Started automatically when the first AsyncTask is created. + */ + static void prosBackgroundTask() + { + if (!backgroundTask) + return; + + while (true) + { + // Iterate over all tasks and call their onUpdate method if they are running + allRunningInstancesMutex.take(); + for (size_t i = 0; i < allRunningInstances.size(); i++) + { + // Get a shared pointer from the weak pointer + const auto instance = allRunningInstances[i]; + + try + { + // Check if the instance is stopped and clean it from the list + if (instance->getState() == STOPPED) + { + allRunningInstances.erase(allRunningInstances.begin() + i); + i--; + continue; + } + + // Call onUpdate if the instance is started + if (instance->getState() == STARTED) + instance->onUpdate(); + + // Call checkFinished. Stop the instance if it returns true + if (instance->checkFinished()) + instance->stop(); + } + + // Catch any exceptions to prevent the background task from crashing + catch (const std::exception& e) + { + Logger::error("An error occurred in AsyncTask: " + std::string(e.what())); + } + } + allRunningInstancesMutex.give(); + + pros::delay(UPDATE_INTERVAL); + } + } + + static constexpr int UPDATE_INTERVAL = 10; // Default update interval in milliseconds + + // List of all running instances + static pros::Mutex allRunningInstancesMutex; + static std::vector> allRunningInstances; + // <-- Using shared_ptr to ensure instances stay alive while running + + // Background task to update all AsyncTask instances + static std::unique_ptr backgroundTask; + + // State of this current task + pros::Mutex stateMutex; + AsyncTaskState state = STOPPED; + std::string debugName = "AsyncTask"; + }; + + // Define the static members + pros::Mutex AsyncTask::allRunningInstancesMutex; + std::vector> AsyncTask::allRunningInstances; + std::unique_ptr AsyncTask::backgroundTask; +} diff --git a/include/devils/utils/backgroundService.hpp b/include/devils/utils/backgroundService.hpp new file mode 100644 index 0000000..03121b2 --- /dev/null +++ b/include/devils/utils/backgroundService.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include "asyncTask.hpp" + +namespace devils +{ + /** + * Represents an asynchronous background service. + * Like `AsyncTask`, classes derived from `BackgroundService` must be managed by `std::shared_ptr`. + */ + class BackgroundService : public AsyncTask + { + public: + BackgroundService() : AsyncTask("BackgroundService") {} + }; +} diff --git a/include/devils/utils/joystickCurve.hpp b/include/devils/utils/joystickCurve.hpp deleted file mode 100644 index 9431b45..0000000 --- a/include/devils/utils/joystickCurve.hpp +++ /dev/null @@ -1,110 +0,0 @@ -#pragma once -#include -#include - -namespace devils -{ - /** - * Represents various curve values to apply to joystick inputs. - */ - struct JoystickCurve - { - /** - * Applies a deadzone to a joystick input. - * @param deadzone The deadzone of the joystick. - * @param val The value of the joystick. Must be between -1 and 1. - * @return Value between deadzone and 1. Negative values are preserved. - */ - static double deadzone( - double deadzone, - double val) - { - // Check if the value is within the deadzone - double absVal = std::abs(val); - if (absVal < deadzone) - return 0; - if (absVal > 1) - return std::copysign(1, val); - - // Correct the value so it starts at 0 instead of deadzone - double correctedVal = (absVal - deadzone) / (1 - deadzone); - - // Apply the sign back to the value - return std::copysign(correctedVal, val); - } - - /** - * Applies an exponential curve to a joystick input. - * @param val The value of the joystick. Must be between -1 and 1. - * @param power The power of the curve. - */ - static double pow(double val, double power) - { - double absVal = std::abs(val); - double exponent = std::pow(absVal, power); - return std::copysign(exponent, val); - } - - /** - * Lerp that maintains the sign of the value. - * @param min The minimum value. - * @param max The maximum value. - * @param val The value to lerp. - * @return The lerped value. If val is negative, the result will be negative. - */ - static double lerp(double min, double max, double val) - { - double absVal = std::fabs(val); - double lerpVal = std::lerp(min, max, absVal); - return std::copysign(lerpVal, val); - } - - /** - * Applies a curve to a joystick input. - * @param val The value of the joystick. Must be between -1 and 1. - * @param power The power of the curve. - * @param deadzone The deadzone of the joystick. - * @param min The minimum output value - * @param max The maximum output value - */ - static double curve( - double val, - double power, - double deadzone, - double min = 0.0, - double max = 1.0) - { - // Deadzone - double newVal = JoystickCurve::deadzone(deadzone, val); - - // Avoid remaining calculations if the value is 0 - if (newVal == 0) - return 0; - - // Apply curve - newVal = JoystickCurve::pow(newVal, power); - - // Lerp - return JoystickCurve::lerp(min, max, newVal); - } - - /** - * Combines two joystick values. Chooses the value with the highest magnitude. - * @param valueA The first value. - * @param valueB The second value. - * @return The combined value. - */ - static double combine( - double valueA, - double valueB) - { - if (std::fabs(valueA) > std::fabs(valueB)) - return valueA; - return valueB; - } - - private: - // Prevent instantiation - JoystickCurve() = delete; - }; -} \ No newline at end of file diff --git a/include/devils/utils/logger.hpp b/include/devils/utils/logger.hpp index debc8bb..88114ef 100644 --- a/include/devils/utils/logger.hpp +++ b/include/devils/utils/logger.hpp @@ -1,182 +1,156 @@ #pragma once -#include "pros/misc.hpp" -#include "pros/llemu.hpp" #include #include #include -#include -#include +#include +#include + +#include "pros/rtos.hpp" namespace devils { - // TODO: Check to see if this logs to the SD card or if it only logs to the terminal - + // Forward declare `ToastDisplay` to prevent circular dependency + class ToastDisplay; + /** * Represents a global logging utility. */ class Logger { public: - /** - * The log level (INFO, WARN, ERROR, DEBUG). - */ + // Delete constructor to prevent instantiation + Logger() = delete; + + /// @brief The log level (INFO, WARN, ERROR, DEBUG). enum LogLevel { + // Least severe + DEBUG = 0, INFO, WARN, ERROR, - DEBUG + // Most severe }; - - /** - * Checks if the SD card is inserted. - * @return True if the SD card is inserted. - */ - static bool isSDInserted() - { - return pros::usd::is_installed() == 1; - } - - /** - * Initializes the logger. - */ - static void init() + + /// @brief Represents a single log message. + struct LogMessage { - pros::lcd::initialize(); - - // Open Log File - if (LOG_TO_SD) - { - if (!isSDInserted()) - { - warn("Logger: SD card is not installed!"); - return; - } - - // Set up the log file - std::string path = getLogFilePath(); - logFileStream.open(path); - if (logFileStream.is_open()) - info("Logger: Opened log file at " + path); - else - warn("Logger: Failed to open log file at " + path); - } - } - - /** - * Logs a message to the terminal, display, SD card, and network. - * @param message The message to log. - * @param level The log level. - */ - static void log(std::string message, LogLevel level) - { - // Log to terminal - if (LOG_TO_SERIAL) - logToSerial(message); - - // Log to display - if (LOG_TO_DISPLAY) - logToLCD(message); - - // Log to SD card - if (LOG_TO_SD) - logToSD(message); - } + uint32_t timestamp; + LogLevel level; + std::string text; + }; /** * Logs an info message. * @param message The message to log. */ - static void info(std::string message) + static void info(const std::string& message) { - log(message, LogLevel::INFO); + log({pros::millis(), INFO, message}); } /** * Logs a warning message. * @param message The message to log. */ - static void warn(std::string message) + static void warn(const std::string& message) { - log(message, LogLevel::WARN); + log({pros::millis(), WARN, message}); } /** * Logs an error message. * @param message The message to log. */ - static void error(std::string message) + static void error(const std::string& message) { - log(message, LogLevel::ERROR); + log({pros::millis(), ERROR, message}); } /** * Logs a debug message. * @param message The message to log. */ - static void debug(std::string message) + static void debug(const std::string& message) { - log(message, LogLevel::DEBUG); + log({pros::millis(), DEBUG, message}); } - + /** - * Logs a message to the SD card. - * @param message The message to log. + * Logs a message to the terminal and the `ToastDisplay` (if it exists). + * @param logMessage The log message to log. */ - static void logToSD(std::string message) + static void log(const LogMessage& logMessage) { - if (logFileStream.is_open()) - { - logFileStream << message << std::endl; - logFileStream.flush(); - } + printToConsole(logMessage); + displayToast(logMessage); + // TODO: Implement SD Card Logging } - + + protected: /** - * Logs a message to the LCD. - * @param message The message to send. + * Adds a log message to the toast buffer to be displayed as a toast notification on the `ToastDisplay`. + * If the buffer exceeds `MAX_BUFFER_SIZE`, the message is discarded to prevent memory issues. + * @param logMessage - The log message to add to the toast buffer. */ - static void logToLCD(std::string message) + static void displayToast(const LogMessage& logMessage) { - static int line = 0; - pros::lcd::set_text(line, message); - line = (line + 1) % 8; + if (toastBuffer.size() < MAX_BUFFER_SIZE) + toastBuffer.push(logMessage); } - + /** - * Logs a message to the serial port. - * @param message The message to log. + * Logs a message to the serial console. + * Can be accessed by used `pros terminal` on a connected device + * while connected to the VEX V5 brain (or controller) over USB. + * @param logMessage - The log message to print to the console. */ - static void logToSerial(std::string message) + static void printToConsole(const LogMessage& logMessage) { - std::cout << message << std::endl; + // Get the message and level from the log message struct + const auto time = logMessage.timestamp; + const auto message = logMessage.text; + const auto level = logMessage.level; + + // ANSI escape codes (prefix colors) + if (level == INFO) + std::cout << "\033[1;94m"; + else if (level == WARN) + std::cout << "\033[38;5;11m"; + else if (level == ERROR) + std::cout << "\033[38;5;160m"; + else if (level == DEBUG) + std::cout << "\033[1;90m"; + + // Timestamp + const auto milliseconds = time % 1000; + const auto seconds = time / 1000; + const auto minutes = seconds / 60; + std::cout << std::format("{:02}:{:02}.{:03}", minutes, seconds % 60, milliseconds) << " |"; + + // Log level + if (level == INFO) + std::cout << " INFO : "; + else if (level == WARN) + std::cout << " WARN : "; + else if (level == ERROR) + std::cout << " ERROR : "; + else if (level == DEBUG) + std::cout << " DEBUG : "; + + // ANSI escape codes (message colors) + if (level == WARN) + std::cout << "\033[38;5;11m"; + else if (level == ERROR) + std::cout << "\033[38;5;160m"; + + // Message + std::cout << message << "\033[0m" << std::endl; } - private: - /** - * Gets a file path for a new log file. Increments the index until a file that doesn't exist is found. - * @return The file path for a new log file. - */ - static std::string getLogFilePath() - { - int index = 0; - std::string path; - do - { - path = "/usd/log-" + std::to_string(index) + ".txt"; - index++; - info("Logger: Checking for log file at " + path); - } while (std::ifstream(path).good()); - return path; - } - - Logger() = delete; - - inline static const std::string TERMINAL_PATH = "/ser/sout"; - static constexpr bool LOG_TO_DISPLAY = false; - static constexpr bool LOG_TO_SD = false; - static constexpr bool LOG_TO_SERIAL = true; - - inline static std::ofstream logFileStream = std::ofstream(); + /// @brief Represents a buffer of log messages that have been logged but not yet displayed as toasts. + static inline std::stack toastBuffer = {}; + static constexpr int MAX_BUFFER_SIZE = 1024; + friend class ToastDisplay; }; -} \ No newline at end of file +} diff --git a/include/devils/utils/math.hpp b/include/devils/utils/math.hpp deleted file mode 100644 index e4c09e4..0000000 --- a/include/devils/utils/math.hpp +++ /dev/null @@ -1,233 +0,0 @@ -#pragma once - -#include -#include - -namespace devils -{ - struct Math - { - /** - * Interpolates along a trapezoidal profile. Used to minimize acceleration. - * @param distanceToStart The distance to the start of the profile. - * @param distanceToEnd The distance to the end of the profile. - * @param accelDistance The distance to start accelerating. - * @param decelDistance The distance to start decelerating. - * @param minSpeed The minimum speed of the profile. - * @param maxSpeed The maximum speed of the profile. - * @return The speed at the given distance. - */ - static double trapezoidProfile( - double distanceToStart, - double distanceToEnd, - double accelDistance, - double decelDistance, - double minAccelSpeed, - double minDecelSpeed, - double maxSpeed) - { - double accelPercent = std::fabs(std::clamp(distanceToStart / accelDistance, -1.0, 1.0)); // Percent of distance to start - double decelPercent = std::fabs(std::clamp(distanceToEnd / decelDistance, -1.0, 1.0)); // Percent of distance to target - - bool isAccel = accelPercent < decelPercent; // Check if we are accelerating or decelerating - - double speedPercent = isAccel ? accelPercent : decelPercent; // Use the smaller of the two - double minSpeed = isAccel ? minAccelSpeed : minDecelSpeed; // Use the correct min speed - - double speed = std::lerp(minSpeed, maxSpeed, speedPercent); // Lerp between min and max speed - speed *= std::copysign(1.0, distanceToEnd); // Apply direction - return speed; - } - - /** - * Interpolates along a sigmoid (S-Curve) profile. Used to minimize jerk. - * @param distanceToStart The distance to the start of the profile. - * @param distanceToEnd The distance to the end of the profile. - * @param accelDistance The distance to start accelerating. - * @param decelDistance The distance to start decelerating. - * @param minSpeed The minimum speed of the profile. - * @param maxSpeed The maximum speed of the profile. - * @param kCurve The strength of the curve. Must be greater than 1. - * @return The speed at the given distance. - */ - static double sigmoidProfile( - double distanceToStart, - double distanceToEnd, - double accelDistance, - double decelDistance, - double minSpeed, - double maxSpeed, - double kCurve = 2.0) - { - double accelPercent = std::clamp(distanceToStart / accelDistance, -1.0, 1.0); // Percent of distance to start - double decelPercent = std::clamp(distanceToEnd / decelDistance, -1.0, 1.0); // Percent of distance to target - double speedPercent = std::min(fabs(accelPercent), fabs(decelPercent)); // Use the smaller of the two - - double curvedSpeed = sigmoidCurve(speedPercent, kCurve); // Curve the speed - double speed = std::lerp(minSpeed, maxSpeed, curvedSpeed); // Lerp between min and max speed - speed *= std::copysign(1.0, distanceToEnd); // Apply direction - return speed; - } - - /** - * Returns the minimum of two values by magnitude. - * @param valueA The first value. - * @param valueB The second value. - * @return The value with the minimum magnitude. - */ - static double minMagnitude( - double valueA, - double valueB) - { - if (std::abs(valueA) < std::abs(valueB)) - return valueA; - else - return valueB; - } - - /** - * Returns the maximum of two values by magnitude. - * @param valueA The first value. - * @param valueB The second value. - * @return The value with the maximum magnitude. - */ - static double maxMagnitude( - double valueA, - double valueB) - { - if (std::abs(valueA) > std::abs(valueB)) - return valueA; - else - return valueB; - } - - /** - * Interpolates a value along a sigmoid curve. - * @param x The input value. - * @param kCurve The curve factor. Must be greater than 1. - * @return The value along the curve. - */ - static double sigmoidCurve( - double x, - double kCurve) - { - // Avoid division by zero - if (x == 1) - return 1; - - // Calculate the curve - double hyperbolic = std::pow(x / (1 - x), kCurve); - return 1 - 1 / (1 + hyperbolic); - } - - /** - * Modulus function that works with negative numbers. - * For example, -1 % 3 = 2 and -1 % -3 = 1. - * @param a The dividend. - * @param b The divisor. - * @return The remainder. - */ - static double signedMod(double a, double b) - { - return a - b * std::floor(a / b); - } - - /** - * Clamps a value between a minimum and maximum. - * Allows for a deadband around zero such that negative values are clamped to -min and positive values are clamped to +min. - * @param value The value to clamp. - * @param min The minimum value. - * @param max The maximum value. - * @return The clamped value. - */ - static double deadbandClamp( - const double value, - const double min, - const double max) - { - if (value >= 0) - return std::clamp(value, min, max); - else - return std::clamp(value, -max, -min); - } - - /** - * Calculates the difference between two radian angles. - * @param a The first angle in radians. - * @param b The second angle in radians. - * @return The minimum difference between the two angles in radians. - */ - static double angleDiff(double a, double b) - { - double dist = a - b; - dist = signedMod(dist + M_PI, 2 * M_PI) - M_PI; - return dist; - } - - /** - * Calculates the difference between two degree angles. - * @param a The first angle in degrees. - * @param b The second angle in degrees. - * @return The minimum difference between the two angles in degrees. - */ - static double angleDiffDeg(double a, double b) - { - double dist = a - b; - dist = signedMod(dist + 180, 360) - 180; - return dist; - } - - /** - * Calculates the end velocity over a specified distance given initial velocity and acceleration. - * @param initialVelocity The initial velocity in units per second. - * @param acceleration Constant acceleration in units per second squared. - * @param distance The distance in units. - */ - static double velocityOverDist( - double initialVelocity, - double acceleration, - double distance) - { - return std::sqrt( - initialVelocity * initialVelocity + - 2 * acceleration * distance); - } - - /** - * Calculates the acceleration over a specified distance given initial and final velocities. - * @param initialVelocity The initial velocity in units per second. - * @param finalVelocity The final velocity in units per second. - * @param distance The distance in units. - */ - static double accelOverDist( - double initialVelocity, - double finalVelocity, - double distance) - { - return (finalVelocity * finalVelocity - initialVelocity * initialVelocity) / (2 * distance); - } - - /** - * Calculates the distance between b and c along the line defined by a and b. - * @param a The first point on the line. - * @param b The second point on the line. Also the point to measure from. - * @param c The point to measure to. - * @return The distance between b and c along the line defined by a and b. - */ - static double distanceOnLine( - Vector2 a, - Vector2 b, - Vector2 c) - { - double numerator = (c.x - a.x) * (b.x - a.x) + (c.y - a.y) * (b.y - a.y); - double denominator = (b.x - a.x) * (b.x - a.x) + (b.y - a.y) * (b.y - a.y); - double t = numerator / denominator; - - double x = (b.x - ((1 - t) * a.x + t * b.x)); - double y = (b.y - ((1 - t) * a.y + t * b.y)); - double distance = std::sqrt(x * x + y * y); - - return distance; - } - }; -} \ No newline at end of file diff --git a/include/devils/utils/robot.hpp b/include/devils/utils/robot.hpp index 0ab3df9..da5256c 100644 --- a/include/devils/utils/robot.hpp +++ b/include/devils/utils/robot.hpp @@ -1,6 +1,6 @@ #pragma once -#include "pros/misc.hpp" +#include "../hardware/controller.hpp" namespace devils { @@ -11,30 +11,40 @@ namespace devils */ struct Robot { + virtual ~Robot() = default; + /** - * Ran when the FMS is connected. + * Ran when field control is connected. */ - virtual void competition() {} + virtual void competition() + { + } /** - * Ran when the robot is disabled with the FMS connected. + * Ran when the robot is disabled */ - virtual void disabled() {} + virtual void disabled() + { + } /** * Ran at the start of the Autonomous period. */ - virtual void autonomous() {} + virtual void autonomous() + { + } /** * Ran at the start of the Teleoperated period. */ - virtual void opcontrol() {} + virtual void opcontrol() + { + } /// @brief The main game controller. - pros::Controller mainController = pros::Controller(pros::E_CONTROLLER_MASTER); - + Controller mainController = Controller("Main Controller", pros::E_CONTROLLER_MASTER); + /// @brief The partner game controller. - pros::Controller partnerController = pros::Controller(pros::E_CONTROLLER_PARTNER); + Controller partnerController = Controller("Partner Controller", pros::E_CONTROLLER_PARTNER); }; -} \ No newline at end of file +} diff --git a/include/devils/utils/robotAutoOptions.hpp b/include/devils/utils/robotAutoOptions.hpp deleted file mode 100644 index 1e33e87..0000000 --- a/include/devils/utils/robotAutoOptions.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include - -namespace devils -{ - enum AllianceColor - { - RED_ALLIANCE = 0, - BLUE_ALLIANCE = 1, - NONE_ALLIANCE = 2 - }; - - struct Routine{ - uint8_t id; - std::string displayName; - bool requiresAllianceColor = false; - }; - - struct RobotAutoOptions - { - AllianceColor allianceColor = NONE_ALLIANCE; - Routine routine = {0, "None", false}; - }; - - - -} \ No newline at end of file diff --git a/include/devils/utils/runnable.hpp b/include/devils/utils/runnable.hpp deleted file mode 100644 index db7cd7d..0000000 --- a/include/devils/utils/runnable.hpp +++ /dev/null @@ -1,106 +0,0 @@ -#pragma once -#include "pros/rtos.hpp" - -namespace devils -{ - /** - * Represents an asynchronous object that can be run on a separate task. - * Tasks are updated periodically at a given interval. - */ - class Runnable - { - public: - Runnable() = default; - Runnable(int updateInterval) : updateInterval(updateInterval) {} - - /** - * Function that is called when the object starts running. - */ - virtual void onStart() {} - - /** - * Function that is called when the object updates periodically. - */ - virtual void onUpdate() {}; - - /** - * Function that is called when the object stops running. - */ - virtual void onStop() {}; - - /** - * Function that is called to check if the object is finished running. - * @return True if the object is finished running. - */ - virtual bool checkFinished() { return false; } - - /** - * Runs the object asynchronously. - * @return The PROS task that runs the object. - */ - virtual void runAsync() - { - // Stop any existing async tasks - stop(); - - // Start task asynchronously - currentTask = std::make_unique(std::bind(&Runnable::run, this)); - } - - /** - * Stops the object from running. - * Deletes the task and calls onStop. - */ - void stop() - { - if (currentTask) - { - onStop(); - currentTask->remove(); - currentTask = nullptr; - } - } - - /** - * Runs the object synchronously. - */ - void run() - { - // Start Event - onStart(); - - // Loop - while (!checkFinished()) - { - try - { - onUpdate(); - } - catch (const std::exception &e) - { - Logger::error("An error occurred in Runnable: " + std::string(e.what())); - } - pros::delay(updateInterval); - } - - // Stop Event - onStop(); - } - - /** - * Joins the async task. - */ - void joinAsync() - { - if (currentTask) - { - currentTask->join(); - currentTask = nullptr; - } - } - - private: - std::unique_ptr currentTask = nullptr; - int updateInterval = 20; - }; -} \ No newline at end of file diff --git a/include/devils/utils/stopwatch.hpp b/include/devils/utils/stopwatch.hpp new file mode 100644 index 0000000..077199d --- /dev/null +++ b/include/devils/utils/stopwatch.hpp @@ -0,0 +1,68 @@ +#pragma once +#include "pros/rtos.hpp" + +namespace devils +{ + /** + * Represents a stopwatch that can be used to measure elapsed time. + * The stopwatch can be started, stopped, and reset. + * The time can be retrieved in seconds. + */ + class Stopwatch + { + public: + /** + * Stops the stopwatch from ticking. + */ + void stop() + { + stopTime = pros::millis(); + isRunning = false; + } + + /** + * Resets the stopwatch and starts it from 0. + */ + void start() + { + reset(); + startTime = pros::millis(); + isRunning = true; + } + + /** + * Resets the stopwatch without starting it. The time will be reset to 0 and the stopwatch will be stopped. + */ + void reset() + { + startTime = 0; + stopTime = 0; + isRunning = false; + } + + /** + * Gets the time in seconds since the stopwatch was started. + * If the stopwatch is stopped, gets the time between when it was started and stopped. + * @return the time in seconds since the stopwatch was started, or the time between when it was started and stopped if the stopwatch is stopped. + */ + float getTime() const + { + const uint32_t currentTime = isRunning ? pros::millis() : stopTime; + return (currentTime - startTime) / 1000.0f; + } + + /** + * Gets whether the stopwatch is currently running. + * @return True if the stopwatch is currently running, false otherwise. + */ + bool getIsRunning() const + { + return isRunning; + } + + protected: + uint32_t startTime = pros::millis(); + uint32_t stopTime = 0; + bool isRunning = false; + }; +} diff --git a/include/devils/utils/stringUtils.hpp b/include/devils/utils/stringUtils.hpp deleted file mode 100644 index 2be772b..0000000 --- a/include/devils/utils/stringUtils.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once -#include -#include -#include - -namespace devils -{ - /** - * A class containing a varitet of string utilities. - */ - class StringUtils - { - public: - // Private constructor to prevent instantiation - StringUtils() = delete; - - /** - * Splits a string by a delimiter. - * @param str The string to split. - * @param delimiter The delimiter to split by. - */ - static std::vector split(std::string inputText, char delimiter) - { - std::vector result; - std::string buffer = ""; - // Iterate through characters - for (int i = 0; i < inputText.length(); i++) - { - // Flush current string if matches delimiter - if (inputText[i] == delimiter) - { - result.push_back(buffer); - buffer = ""; - } - // Otherwise, add to buffer - else - { - buffer += inputText[i]; - } - } - // Flush last string - result.push_back(buffer); - return result; - } - }; -} \ No newline at end of file diff --git a/include/devils/utils/timer.hpp b/include/devils/utils/timer.hpp index 439650d..256ce9c 100644 --- a/include/devils/utils/timer.hpp +++ b/include/devils/utils/timer.hpp @@ -1,29 +1,35 @@ #pragma once - -#include "pros/rtos.hpp" -#include "devils/utils/logger.hpp" +#include "stopwatch.hpp" namespace devils { + /** + * Represents a timer that can be used to measure elapsed time against a specified duration. + * The timer can be started and stopped, and the elapsed time and time remaining can be retrieved. + * The timer is considered finished when the elapsed time exceeds the specified duration. + */ class Timer { public: /** * Creates a new instance of Timer with a given duration. - * @param duration The duration of the timer in milliseconds. + * By default, the timer is automatically started. + * It can be restarted by calling the `start()` method again. + * @param duration The duration of the timer in seconds. * @return The new instance of Timer. */ - Timer(uint32_t duration) : duration(duration) + explicit Timer(const float duration) : + duration(duration) { } /** * Sets the duration of the timer. - * @param duration The duration of the timer in milliseconds. + * @param newDuration The duration of the timer in seconds. */ - void setDuration(uint32_t duration) + void setDuration(const float newDuration) { - this->duration = duration; + this->duration = newDuration; } /** @@ -31,8 +37,7 @@ namespace devils */ void start() { - this->startTime = pros::millis(); - this->isStarted = true; + stopwatch.start(); } /** @@ -40,39 +45,49 @@ namespace devils */ void stop() { - this->isStarted = false; + stopwatch.stop(); } /** * Gets whether the timer has started. * @return True if the timer has started, false otherwise. */ - bool running() + bool getIsRunning() const { - return isStarted && pros::millis() - startTime < duration; + return stopwatch.getIsRunning() && !getIsFinished(); } /** * Gets whether the timer has finished. * @return True if the timer has finished, false otherwise. */ - bool finished() + bool getIsFinished() const { - return isStarted && pros::millis() - startTime >= duration; + return stopwatch.getIsRunning() && getElapsedTime() >= duration; } + /** + * Gets the elapsed time since the timer was started. + * @return The elapsed time since the timer was started in seconds. + */ + float getElapsedTime() const + { + return stopwatch.getTime(); + } + /** * Gets the time remaining on the timer. - * @return The time remaining on the timer in milliseconds. + * @return The time remaining on the timer in seconds. */ - double timeRemaining() + float getTimeRemaining() const { - return duration - (pros::millis() - startTime); + if (!getIsRunning()) + return 0; + return duration - getElapsedTime(); } - private: - bool isStarted = false; - uint32_t startTime = 0; - uint32_t duration = 0; + protected: + Stopwatch stopwatch; + float duration = 0; }; -} \ No newline at end of file +} diff --git a/include/devils/vexbridge/vbOdom.hpp b/include/devils/vexbridge/vbOdom.hpp deleted file mode 100644 index 871ed3d..0000000 --- a/include/devils/vexbridge/vbOdom.hpp +++ /dev/null @@ -1,67 +0,0 @@ -#pragma once -#include "../odom/odomSource.hpp" -#include "../geometry/units.hpp" -#include "../utils/runnable.hpp" -#include "vexbridge/vbValue.hpp" -#include "vexbridge/vbGroup.hpp" - -using namespace vexbridge; - -namespace devils -{ - /** - * Syncs odometry data to VEXBridge. - */ - class VBOdom : private Runnable - { - public: - /** - * Creates a new odometry syncer. - * @param name The name of the odometry data. - * @param odometry The odometry source to sync. - */ - VBOdom(std::string name, OdomSource &odometry) - : odometry(odometry), - group("_poses/" + name), - xValue(group.addValue("x", 0.0f)), - yValue(group.addValue("y", 0.0f)), - rotation(group.addValue("rotation", 0.0f)), - width(group.addValue("width", 15.0f)), - length(group.addValue("length", 15.0f)), - speed(group.addValue("speed", 0.0f)) - { - this->runAsync(); - } - - /** - * Sets the dimensions of the robot. - * @param width The width of the robot in inches. - * @param length The length of the robot in inches. - */ - void setDimensions(float width, float length) - { - this->width.set(width); - this->length.set(length); - } - - protected: - void onUpdate() override - { - Pose pose = odometry.getPose(); - xValue.set(pose.x); - yValue.set(pose.y); - rotation.set(Units::radToDeg(pose.rotation)); - speed.set(odometry.getVelocity().magnitude()); - } - - private: - VBGroup group; - VBValue xValue; - VBValue yValue; - VBValue rotation; - VBValue width; - VBValue length; - VBValue speed; - OdomSource &odometry; - }; -} \ No newline at end of file diff --git a/include/devils/vexbridge/vbPath.hpp b/include/devils/vexbridge/vbPath.hpp deleted file mode 100644 index 39c1012..0000000 --- a/include/devils/vexbridge/vbPath.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once -#include "../path/path.hpp" -#include "vexbridge/vbValue.hpp" -#include "vexbridge/vbGroup.hpp" - -using namespace vexbridge; - -namespace devils -{ - /** - * Syncs path data to VEXBridge. - */ - class VBPath - { - public: - /** - * Updates the path data on VEXBridge. - * @param name The name of the path. - * @param path The path to sync. - */ - static void sync(std::string name, Path &path) - { - // Create vectors for x and y values - std::vector xValues; - std::vector yValues; - - xValues.reserve(path.getLength() / DELTA_INDEX); - yValues.reserve(path.getLength() / DELTA_INDEX); - - // Iterate over the path and add points - for (double i = 0; i < path.getLength(); i += DELTA_INDEX) - { - Pose pose = path.getPoseAt(i); - - xValues.push_back(pose.x); - yValues.push_back(pose.y); - } - - // Sync the path data - VEXBridge::set("_paths/" + name + "/x", xValues); - VEXBridge::set("_paths/" + name + "/y", yValues); - } - - private: - static constexpr double DELTA_INDEX = 0.01; - }; -} \ No newline at end of file diff --git a/include/incbin/incbin.c b/include/incbin/incbin.c deleted file mode 100644 index fd1e72b..0000000 --- a/include/incbin/incbin.c +++ /dev/null @@ -1,291 +0,0 @@ -#ifdef _MSC_VER -# define _CRT_SECURE_NO_WARNINGS -#endif - -#include -#include -#include -#include -#include - -#ifndef PATH_MAX -# define PATH_MAX 260 -#endif - -#define SEARCH_PATHS_MAX 64 -#define FILE_PATHS_MAX 1024 - -static int fline(char **line, size_t *n, FILE *fp) { - int chr; - char *pos; - if (!line || !n || !fp) - return -1; - if (!*line) - if (!(*line = (char *)malloc((*n=64)))) - return -1; - chr = *n; - pos = *line; - for (;;) { - int c = fgetc(fp); - if (chr < 2) { - *n += (*n > 16) ? *n : 64; - chr = *n + *line - pos; - if (!(*line = (char *)realloc(*line,*n))) - return -1; - pos = *n - chr + *line; - } - if (ferror(fp)) - return -1; - if (c == EOF) { - if (pos == *line) - return -1; - else - break; - } - *pos++ = c; - chr--; - if (c == '\n') - break; - } - *pos = '\0'; - return pos - *line; -} - -static FILE *open_file(const char *name, const char *mode, const char (*searches)[PATH_MAX], int count) { - int i; - for (i = 0; i < count; i++) { - char buffer[FILENAME_MAX + PATH_MAX]; - FILE *fp; -#ifndef _MSC_VER - snprintf(buffer, sizeof(buffer), "%s/%s", searches[i], name); -#else - _snprintf(buffer, sizeof(buffer), "%s/%s", searches[i], name); -#endif - if ((fp = fopen(buffer, mode))) - return fp; - } - return !count ? fopen(name, mode) : NULL; -} - -static int strcicmp(const char *s1, const char *s2) { - const unsigned char *us1 = (const unsigned char *)s1, - *us2 = (const unsigned char *)s2; - while (tolower(*us1) == tolower(*us2)) { - if (*us1++ == '\0') - return 0; - us2++; - } - return tolower(*us1) - tolower(*us2); -} - -/* styles */ -enum { kCamel, kSnake }; -/* identifiers */ -enum { kData, kEnd, kSize }; - -static const char *styled(int style, int ident) { - switch (style) { - case kCamel: - switch (ident) { - case kData: return "Data"; - case kEnd: return "End"; - case kSize: return "Size"; - } - break; - case kSnake: - switch (ident) { - case kData: return "_data"; - case kEnd: return "_end"; - case kSize: return "_size"; - } - break; - } - return ""; -} - -int main(int argc, char **argv) { - int ret = 0, i, paths, files = 0, style = kCamel; - char outfile[FILENAME_MAX] = "data.c"; - char search_paths[SEARCH_PATHS_MAX][PATH_MAX]; - char prefix[FILENAME_MAX] = "g"; - char file_paths[FILE_PATHS_MAX][PATH_MAX]; - FILE *out = NULL; - - argc--; - argv++; - - #define s(IDENT) styled(style, IDENT) - - if (argc == 0) { -usage: - fprintf(stderr, "%s [-help] [-Ipath...] | | [-o output] | [-p prefix]\n", argv[-1]); - fprintf(stderr, " -o - output file [default is \"data.c\"]\n"); - fprintf(stderr, " -p - specify a prefix for symbol names (default is \"g\")\n"); - fprintf(stderr, " -S