diff --git a/.gitignore b/.gitignore index c6619faae..84d8e744e 100644 --- a/.gitignore +++ b/.gitignore @@ -28,7 +28,6 @@ compile_commands.json # Ignore any tools/ directory artifacts tools/*.txt tools/*.out -tools/git_scraper tools/custom_monitor tools/compiler tools/tools diff --git a/Makefile b/Makefile index 20f4c7847..77c83a2c2 100644 --- a/Makefile +++ b/Makefile @@ -1,103 +1,112 @@ -# Made by Jack Miller 2024 (Github: guywithhat99) -# Thanks to Jackson Stepka (Github: Pandabear1125); a lot of code is based off his original makefile +# Optional features: PROFILER, COMMS_DEBUG, REF_SYSTEM_DEBUG, CAN_MANAGER_DEBUG +# Set them on the line below, e.g. FEATURE_DEFINES ?= -DPROFILER -DCOMMS_DEBUG +# Rebuild from scratch after editing this line: +# make clean +# make build +FEATURE_DEFINES ?= + +# Set to 1 to disassemble every object file alongside it, for inspecting a +# single translation unit. +DUMP_OBJS ?= 0 + +# Build in parallel across all available cores by default. +# A -j here overrides one given on the command line, so use JOBS to change it: +# 'make JOBS=1 build' for readable serial output. +JOBS ?= $(shell sysctl -n hw.ncpu 2>/dev/null || nproc 2>/dev/null || echo 1) +MAKEFLAGS += -j$(JOBS) + +BUILD_DIR := build +TOOLS_DIR := tools + +TARGET := firmware +TARGET_ELF := $(BUILD_DIR)/$(TARGET).elf +TARGET_HEX := $(BUILD_DIR)/$(TARGET).hex +TARGET_MAP := $(BUILD_DIR)/$(TARGET).map +TARGET_DUMP := $(BUILD_DIR)/$(TARGET).dump + +TEST_DIR := test +TEST_BUILD_DIR := $(BUILD_DIR)/test +TEST_SUITES := $(notdir $(patsubst %/,%,$(dir $(wildcard $(TEST_DIR)/test_*/test_main.cpp)))) +# Run every host suite by default. To run a subset, for example: +# make test TEST_FILTER="test_utils test_fltrs" +TEST_FILTER ?= $(TEST_SUITES) +TEST_BINS := $(addprefix $(TEST_BUILD_DIR)/,$(TEST_FILTER)) + +HOST_CXX ?= c++ +HOST_CC ?= cc +HOST_TEST_CPPFLAGS := -I$(TEST_DIR)/host -Isrc -Ilibraries/unity -DUNIT_TEST +HOST_TEST_CXXFLAGS := -std=gnu++23 -O0 -g3 -Wall -Werror +HOST_TEST_CFLAGS := -std=c11 -O0 -g3 -Wall -Werror +HOST_TEST_UNITY_OBJ := $(TEST_BUILD_DIR)/unity.o +HOST_TEST_COMMON_SRC := $(TEST_DIR)/host/test_runner.cpp $(HOST_TEST_UNITY_OBJ) +HOST_TEST_SRC_test_utils := $(TEST_DIR)/test_utils/test_main.cpp src/utils/wrapping.cpp src/utils/vector_math.cpp +HOST_TEST_SRC_test_fltrs := $(TEST_DIR)/test_fltrs/test_main.cpp src/filters/pid_filter.cpp src/filters/lowpass_filter.cpp +HOST_TEST_SRC_test_controls := $(TEST_DIR)/test_controls/test_main.cpp src/controls/controller_math.cpp +HOST_TEST_SRC_test_sensors := $(TEST_DIR)/test_sensors/test_main.cpp src/sensors/buff_encoder.cpp + +TEENSY_SRC_DIRS := teensy4 +LIBRARY_SRC_DIRS := libraries +SRC_SRC_DIRS := src -# Detect the current operating system using uname -UNAME := $(shell uname -s) - -# The name of the target executable -TARGET_EXEC := firmware - -# Directory where build outputs will be placed -BUILD_DIR := ./build - -# Tools directory -TOOLS_DIR := ./tools - -# Source directories -TEENSY_SRC_DIRS := ./teensy4 -LIBRARY_SRC_DIRS := ./libraries -SRC_SRC_DIRS := ./src - -# Find all C, C++, and assembly source files in the specified directories -# Note: Single quotes are used to prevent the shell from expanding '*' TEENSY_SRC := $(shell find $(TEENSY_SRC_DIRS) -name '*.cpp' -or -name '*.c') LIBRARY_SRC := $(shell find $(LIBRARY_SRC_DIRS) -name '*.cpp' -or -name '*.c') SRC_SRC := $(shell find $(SRC_SRC_DIRS) -name '*.cpp' -or -name '*.c') -# Generate object file paths by prepending BUILD_DIR and appending .o to source files -# Example: ./your_dir/hello.cpp turns into ./build/./your_dir/hello.cpp.o TEENSY_OBJS := $(TEENSY_SRC:%=$(BUILD_DIR)/%.o) LIBRARY_OBJS := $(LIBRARY_SRC:%=$(BUILD_DIR)/%.o) SRC_OBJS := $(SRC_SRC:%=$(BUILD_DIR)/%.o) -# Generate dependency file paths by replacing .o with .d in object file paths -# Example: ./build/hello.cpp.o turns into ./build/hello.cpp.d TEENSY_DEPS := $(TEENSY_OBJS:.o=.d) LIBRARY_DEPS := $(LIBRARY_OBJS:.o=.d) SRC_DEPS := $(SRC_OBJS:.o=.d) -# Find all include directories for GCC to locate header files TEENSY_INC_DIRS := $(shell find $(TEENSY_SRC_DIRS) -type d) -LIBRARY_INC_DIRS := $(shell find $(LIBRARY_SRC_DIRS) -maxdepth 2 -type d) -SRC_INC_DIRS := $(shell find $(SRC_SRC_DIRS) -type d) +LIBRARY_INC_DIRS := $(LIBRARY_SRC_DIRS) \ + $(patsubst %/,%,$(wildcard \ + $(LIBRARY_SRC_DIRS)/*/ \ + $(LIBRARY_SRC_DIRS)/*/src/ \ + $(LIBRARY_SRC_DIRS)/*/utility/)) +SRC_INC_DIRS := $(SRC_SRC_DIRS) -# Generate compiler include flags from include directories # -isystem on Teensy and Library files to suppress warnings TEENSY_INC_FLAGS := $(addprefix -isystem,$(TEENSY_INC_DIRS)) LIBRARY_INC_FLAGS := $(addprefix -isystem,$(LIBRARY_INC_DIRS)) SRC_INC_FLAGS := $(addprefix -I,$(SRC_INC_DIRS)) INCLUDE_FLAGS := $(TEENSY_INC_FLAGS) $(LIBRARY_INC_FLAGS) $(SRC_INC_FLAGS) -# Compiler flags specific to Teensy 4.1 -TEENSY4_FLAGS = -DF_CPU=600000000 -DUSB_CUSTOM -DLAYOUT_US_ENGLISH -D__IMXRT1062__ -DTEENSYDUINO=159 -DARDUINO_TEENSY41 -DARDUINO=10813 -DFIRMWARE +# CPU and ABI flags required during compilation and linking +ARCH_FLAGS := -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-d16 -mthumb -# CPU flags to optimize code for the Teensy processor -CPU_CFLAGS = -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-d16 -mthumb +BOARD_DEFINES := -DF_CPU=600000000 -D__IMXRT1062__ -DTEENSYDUINO=159 -DARDUINO_TEENSY41 -DARDUINO=10813 +USB_DEFINES := -DUSB_CUSTOM -DLAYOUT_US_ENGLISH +PROJECT_DEFINES := $(BOARD_DEFINES) $(USB_DEFINES) -DFIRMWARE $(FEATURE_DEFINES) -DEFINES := $(TEENSY4_FLAGS) - -# Preprocessor flags for both C and C++ files # -MMD: Generate dependency files for each source file # -MP: Add a phony target for each dependency to avoid errors if the dependency is missing -# -ffunction-sections: Place each function in its own section to allow the linker to remove unused functions -# -fdata-sections: Place each variable in its own section to allow the linker to remove unused variables -# -O2: Optimize the code for speed -# --specs=nano.specs: Use newlib nano instead of full newlib to reduce binary size -# -g3: Generate debug information for GDB. Level 3 includes the most information possible -CPPFLAGS := $(INCLUDE_FLAGS) $(DEFINES) -MMD -MP -ffunction-sections -fdata-sections -O2 --specs=nano.specs -g3 - -# Compiler flags for C files -CFLAGS := $(CPU_CFLAGS) - -# Compiler flags for C++ files -CXXFLAGS := $(CPU_CFLAGS) -std=gnu++23 \ - -felide-constructors -fno-exceptions -fpermissive \ - -Wno-error=narrowing -Wno-trigraphs -Wno-comment -Wall -Werror \ - -Wno-volatile - -# Linker flags, including Teensy-specific linker script +DEPFLAGS := -MMD -MP + +# Place each function and variable in its own section so the linker can remove unused code and data +SECTION_FLAGS := -ffunction-sections -fdata-sections +# -g2: full line tables for addr2line/GDB. -g3 additionally emits macro +# definitions, which cost ~7x in object size and are not needed by the +# addr2line crash-report workflow in teensy4/CrashReport.cpp. +COMMON_COMPILE_FLAGS := $(ARCH_FLAGS) $(SECTION_FLAGS) -O2 -g2 --specs=nano.specs +CXX_LANGUAGE_FLAGS := -std=gnu++23 -felide-constructors -fno-exceptions -fpermissive +CXX_WARNING_FLAGS := -Wno-error=narrowing -Wno-trigraphs -Wno-comment -Wall -Werror -Wno-volatile + +PROJECT_CPPFLAGS := $(INCLUDE_FLAGS) $(PROJECT_DEFINES) +PROJECT_CFLAGS := $(COMMON_COMPILE_FLAGS) +PROJECT_CXXFLAGS := $(COMMON_COMPILE_FLAGS) $(CXX_LANGUAGE_FLAGS) $(CXX_WARNING_FLAGS) + # --gc-sections: Remove unused sections to reduce binary size # --relax: Allow linker to relax some constraints to sometimes generate smaller code # -Tteensy4/imxrt1062_t41.ld: Use the Teensy 4.1 linker script -# --print-memory-usage: Print memory usage after linking # -Map=... and --cref: Generate a cross-reference map file -LINKING_FLAGS = -Wl,--gc-sections,--relax,-Tteensy4/imxrt1062_t41.ld,--print-memory-usage,-Map=$(BUILD_DIR)/$(TARGET_EXEC).map,--cref - -# Set the Arduino path based on the detected operating system -ifeq ($(UNAME),Darwin) - ARDUINO_PATH = $(abspath $(HOME)/Library/Arduino15) - $(info We've detected you are using a Mac! Consult God if this breaks.) -endif -ifeq ($(UNAME),Linux) - ARDUINO_PATH = $(abspath $(HOME)/.arduino15) - $(info We've detected you're on Linux! Nerd.) -endif - -# Base arm-none-eabi and Teensyduino tool paths +PROJECT_LDFLAGS := $(ARCH_FLAGS) --specs=nano.specs -Wl,--gc-sections,--relax,-Tteensy4/imxrt1062_t41.ld,--print-memory-usage,-Map=$(TARGET_MAP),--cref + COMPILER_TOOLS_PATH = $(TOOLS_DIR)/compiler/arm-gnu-toolchain/bin TARGET_TRIPLE ?= arm-none-eabi -# arm-none-eabi tools COMPILER_CPP = $(COMPILER_TOOLS_PATH)/arm-none-eabi-g++ COMPILER_C = $(COMPILER_TOOLS_PATH)/arm-none-eabi-gcc AR = $(COMPILER_TOOLS_PATH)/arm-none-eabi-ar @@ -108,125 +117,123 @@ READELF = $(COMPILER_TOOLS_PATH)/arm-none-eabi-readelf ADDR2LINE = $(COMPILER_TOOLS_PATH)/arm-none-eabi-addr2line SIZE = $(COMPILER_TOOLS_PATH)/arm-none-eabi-size -# Path to the Git scraper tool source file -GIT_SCRAPER = $(TOOLS_DIR)/git_scraper.cpp +GIT_SCRAPER_SRC = $(TOOLS_DIR)/git_scraper.cpp +GIT_SCRAPER_BIN = $(BUILD_DIR)/git_scraper -# Utilize all available CPU cores for parallel build -MAKEFLAGS += -j$(nproc) +.PHONY: build dump test test-build docs clean upload install gdb monitor kill restart help clangd git_scraper FORCE -# Phony target to force a build every time -.PHONY: build +build: $(TARGET_HEX) -# Main build target; depends on the target executable and git scraper -build: clangd $(BUILD_DIR)/$(TARGET_EXEC) -# Final linking step to create the executable. -# This rule links all the object files to produce the final ELF executable. -# It depends on all object files and the 'git_scraper' target to ensure -# that Git information is up-to-date before linking. -$(BUILD_DIR)/$(TARGET_EXEC): git_scraper $(SRC_OBJS) $(LIBRARY_OBJS) $(TEENSY_OBJS) - # Invoke the C++ compiler as the linker. - # - $(COMPILER_CPP): The compiler executable. - # - $(CPPFLAGS): Preprocessor and common compiler flags. - # - $(CXXFLAGS): C++ compiler flags. - # - $(LIBRARY_OBJS), $(TEENSY_OBJS), $(SRC_OBJS): Object files to link. - # - $(LINKING_FLAGS): Linker flags, including the linker script. - # - '-o $(BUILD_DIR)/$(TARGET_EXEC).elf': Output the ELF executable to the build directory. - @$(COMPILER_CPP) $(CPPFLAGS) $(CXXFLAGS) $(LIBRARY_OBJS) $(TEENSY_OBJS) $(SRC_OBJS) $(LINKING_FLAGS) -o $(BUILD_DIR)/$(TARGET_EXEC).elf - # Copy the ELF executable to the root directory. - @cp $(BUILD_DIR)/$(TARGET_EXEC).elf . +dump: $(TARGET_DUMP) - # Inform the user that the HEX file is being constructed. - @echo [Constructing $(TARGET_EXEC).hex] - # Convert the ELF executable to an Intel HEX format, which is required for uploading to the Teensy board. - # - '-O ihex': Specifies the output format as Intel HEX. - # - '-R .eeprom': Removes the .eeprom section from the output, as it's not needed for flashing. - # - '$(TARGET_EXEC).elf': The input ELF executable. - # - '$(TARGET_EXEC).hex': The output HEX file. - @$(OBJCOPY) -O ihex -R .eeprom $(TARGET_EXEC).elf $(TARGET_EXEC).hex +# Build and run the selected Unity suites as native host executables. +test: test-build + @set -e; for suite in $(TEST_FILTER); do \ + printf "\n===== %s =====\n" "$$suite"; \ + "$(TEST_BUILD_DIR)/$$suite"; \ + done + + +# Compile native test executables without running them. +test-build: $(TEST_BINS) + + +define HOST_TEST_template +$(TEST_BUILD_DIR)/$(1): $$(HOST_TEST_COMMON_SRC) $$(HOST_TEST_SRC_$(1)) FORCE + @mkdir -p $$(dir $$@) + @printf "HOSTTEST %s\n" "$(1)" + @$$(HOST_CXX) $$(HOST_TEST_CPPFLAGS) $$(CPPFLAGS) \ + $$(HOST_TEST_CXXFLAGS) $$(CXXFLAGS) $$(filter-out FORCE,$$^) -o $$@ +endef + +$(foreach suite,$(TEST_SUITES),$(eval $(call HOST_TEST_template,$(suite)))) + + +$(HOST_TEST_UNITY_OBJ): libraries/unity/unity.c FORCE + @mkdir -p $(dir $@) + @printf "HOSTCC %s\n" "$<" + @$(HOST_CC) -Ilibraries/unity $(CPPFLAGS) $(HOST_TEST_CFLAGS) $(CFLAGS) -c $< -o $@ + - # Ensure the HEX file has execute permissions. - @chmod +x $(TARGET_EXEC).hex +FORCE: + + +$(TARGET_ELF): $(SRC_OBJS) $(LIBRARY_OBJS) $(TEENSY_OBJS) + @printf "LINK %s\n" "$@" + @$(COMPILER_CPP) $(PROJECT_LDFLAGS) $(LDFLAGS) $(LIBRARY_OBJS) $(TEENSY_OBJS) $(SRC_OBJS) $(LDLIBS) -o $@ + + +$(TARGET_HEX): $(TARGET_ELF) + @printf "OBJCOPY %s\n" "$@" + @$(OBJCOPY) -O ihex -R .eeprom $< $@ + @chmod +x $@ + + +$(TARGET_DUMP): $(TARGET_ELF) + @printf "OBJDUMP %s\n" "$@" + @$(OBJDUMP) -dstz $< > $@ - # Disassemble the ELF executable to a .dump file - @$(OBJDUMP) -dstz $(BUILD_DIR)/$(TARGET_EXEC).elf > $(BUILD_DIR)/$(TARGET_EXEC).dump # Ensure git_scraper finishes before compiling any object files $(SRC_OBJS) $(LIBRARY_OBJS) $(TEENSY_OBJS): | git_scraper -# Build step for compiling C source files + + $(BUILD_DIR)/%.c.o: %.c @mkdir -p $(dir $@) - @echo [Building $<] - @$(COMPILER_C) $(CPPFLAGS) $(CFLAGS) -c $< -o $@ - # Disassemble the object file to a .dump file - @$(OBJDUMP) -dstz $@ > $@.dump + @printf "CC %s\n" "$<" + @$(COMPILER_C) $(PROJECT_CPPFLAGS) $(CPPFLAGS) $(PROJECT_CFLAGS) $(CFLAGS) $(DEPFLAGS) -c $< -o $@ + @$(if $(filter-out 0,$(DUMP_OBJS)),$(OBJDUMP) -dstz $@ > $@.dump) + -# Build step for compiling C++ source files $(BUILD_DIR)/%.cpp.o: %.cpp @mkdir -p $(dir $@) - @echo [Building $<] - @$(COMPILER_CPP) $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@ - # Disassemble the object file to a .dump file - @$(OBJDUMP) -dstz $@ > $@.dump - - + @printf "CXX %s\n" "$<" + @$(COMPILER_CPP) $(PROJECT_CPPFLAGS) $(CPPFLAGS) $(PROJECT_CXXFLAGS) $(CXXFLAGS) $(DEPFLAGS) -c $< -o $@ + @$(if $(filter-out 0,$(DUMP_OBJS)),$(OBJDUMP) -dstz $@ > $@.dump) DOC_SCRIPT = $(TOOLS_DIR)/build_docs.sh -# Phony target to generate documentation using Doxygen -.PHONY: docs + docs: build @chmod +x $(DOC_SCRIPT) @$(DOC_SCRIPT) -# Phony target to prevent conflicts with files named 'clean' and force a rebuild every time -.PHONY: clean -# Clean the build directory and remove generated executables clean: rm -rf $(BUILD_DIR) - rm -rf $(TARGET_EXEC).elf $(TARGET_EXEC).hex $(TARGET_EXEC).map $(TARGET_EXEC).dump rm -f compile_commands.json -# Clean only the source object files -clean_src: - rm -r $(BUILD_DIR)/src - -# Clean only the library object files -clean_libs: - rm -r $(BUILD_DIR)/libraries - -# Clean only the Teensy object files -clean_teensy4: - rm -r $(BUILD_DIR)/teensy4 - # Include the dependency files to manage header file dependencies # The '-' suppresses errors if the files are missing (which they will be on the first run) -include $(TEENSY_DEPS) -include $(LIBRARY_DEPS) -include $(SRC_DEPS) -# Build, run, and clean up the git scraper tool to store current Git info in a header file -.PHONY: git_scraper -git_scraper: - @g++ -std=gnu++17 $(GIT_SCRAPER) -o $(TOOLS_DIR)/git_scraper - @$(TOOLS_DIR)/git_scraper - @rm $(TOOLS_DIR)/git_scraper + +# The scraper itself is rebuilt only when its source changes, but it is run on +# every build so it can pick up branch/commit changes. It rewrites +# src/git_info.h only when the contents actually differ. +$(GIT_SCRAPER_BIN): $(GIT_SCRAPER_SRC) + @mkdir -p $(dir $@) + @printf "HOSTCXX %s\n" "$<" + @g++ -std=gnu++17 $< -o $@ + +git_scraper: $(GIT_SCRAPER_BIN) + @$(GIT_SCRAPER_BIN) -# Upload the firmware to the Teensy device +# Upload firmware to Teensy and start monitoring upload: build @echo [Uploading] - If this fails, press the button on the teensy and re-run 'make upload' - @tycmd upload $(TARGET_EXEC).hex - # Teensy serial isn't immediately available after upload, so we wait a bit - # The Teensy waits for 20 + 280 + 20 ms after power up/boot + @tycmd upload $(TARGET_HEX) @sleep 0.4s @bash $(TOOLS_DIR)/monitor.sh -# Install required tools for building and uploading firmware +# Install requirements for building and uploading firmware install: @bash $(TOOLS_DIR)/install_tytools.sh @bash $(TOOLS_DIR)/install_compiler.sh @@ -237,7 +244,7 @@ install: gdb: @echo [Starting GDB] @bash $(TOOLS_DIR)/prepare_gdb.sh - @$(GDB) -x $(TOOLS_DIR)/gdb_commands.txt --args $(TARGET_EXEC).elf + @$(GDB) -x $(TOOLS_DIR)/gdb_commands.txt --args $(TARGET_ELF) # monitors currently running firmware on robot @@ -259,22 +266,27 @@ restart: @tycmd reset -help: +help: @echo "Basic usage: make [target]" @echo "Targets:" @echo " install: installs all required dependencies" @echo " build: compiles the source code and links with libraries" + @echo " dump: generates a disassembly of the built firmware" @echo " upload: builds the source and uploads it to the Teensy" + @echo " test: builds and runs native Unity unit tests" + @echo " test-build: builds native Unity test executables" @echo " gdb: starts GDB and attaches to the firmware running on a connected Teensy" @echo " monitor: monitors any actively running firmware and displays serial output" @echo " kill: stops any running firmware" @echo " restart: restarts any running firmware" @echo " clean: removes all build artifacts and generated files" - @echo " clean_src: removes only the source object files" - @echo " clean_libs: removes only the library object files" - @echo " clean_teensy4: removes only the Teensy object files" @echo " clangd: generates compile_commands.json for clangd" @echo " docs: generates documentation of our src/ code using Doxygen" + @echo "" + @echo "Variables:" + @echo " JOBS=N parallel jobs (defaults to core count)" + @echo " DUMP_OBJS=1 also disassemble each object file" + @echo " TEST_FILTER=\"suite ...\" selects test suites (defaults to all)" # Generate compile_commands.json from the Makefile's flags and source lists. @@ -282,7 +294,6 @@ COMPILE_DB = compile_commands.json COMPILE_DB_SCRIPT = $(TOOLS_DIR)/generate_compile_commands.sh COMPILE_DB_DIR_DEPS = $(SRC_INC_DIRS) $(LIBRARY_INC_DIRS) $(TEENSY_INC_DIRS) -.PHONY: clangd clangd: $(COMPILE_DB) $(COMPILE_DB): Makefile $(COMPILE_DB_SCRIPT) $(COMPILE_DB_DIR_DEPS) @@ -291,8 +302,9 @@ $(COMPILE_DB): Makefile $(COMPILE_DB_SCRIPT) $(COMPILE_DB_DIR_DEPS) COMPILER_CPP="$(COMPILER_CPP)" \ COMPILER_C="$(COMPILER_C)" \ TARGET_TRIPLE="$(TARGET_TRIPLE)" \ - CPPFLAGS='$(CPPFLAGS)' \ - CXXFLAGS='$(CXXFLAGS)' \ - CFLAGS='$(CFLAGS)' \ + CPPFLAGS='$(PROJECT_CPPFLAGS) $(CPPFLAGS)' \ + CXXFLAGS='$(PROJECT_CXXFLAGS) $(CXXFLAGS)' \ + CFLAGS='$(PROJECT_CFLAGS) $(CFLAGS)' \ SRC_FILES='$(SRC_SRC) $(LIBRARY_SRC) $(TEENSY_SRC)' \ "$(COMPILE_DB_SCRIPT)" + @printf "CLANGD compile_commands.json generated\n" diff --git a/README.md b/README.md index 39d67d228..755339355 100644 --- a/README.md +++ b/README.md @@ -44,6 +44,11 @@ There are a few other nice helper functions within the makefile. This will list make help ``` +To run tests: +```bash +make test +``` + ## Contributing `main` is the production branch, which is required to be in an always working state. diff --git a/src/comms/config_data/controller.hpp b/src/comms/config_data/controller.hpp index 84802cf15..6fa503fcd 100644 --- a/src/comms/config_data/controller.hpp +++ b/src/comms/config_data/controller.hpp @@ -5,7 +5,7 @@ #include "comms/data/comms_data.hpp" // for CommsData, TypeLabel, to_string #include "comms/config_data/motor.hpp" // for Motor #include "state.hpp" // for StateName -#include "safety.hpp" // for assert_or_safety_mode +#include "utils/safety.hpp" // for assert_or_safety_mode /// These values are arbitrary limits for the size of the arrays in the Controller struct. // They should be large enough to accommodate any reasonable number of motors, states, or sub controllers per controller configuration. diff --git a/src/comms/config_data/estimator.hpp b/src/comms/config_data/estimator.hpp index f6ed46390..064b6377b 100644 --- a/src/comms/config_data/estimator.hpp +++ b/src/comms/config_data/estimator.hpp @@ -5,7 +5,7 @@ #include "state.hpp" // for StateName #include "motor.hpp" // for MotorName #include "sensor.hpp" // for SensorName -#include "safety.hpp" // for assert_or_safety_mode +#include "utils/safety.hpp" // for assert_or_safety_mode /// These values are arbitrary limits for the size of the arrays in the Estimator struct. // They should be large enough to accommodate any reasonable number of states, motors, or sensors used by an estimator configuration. diff --git a/src/comms/data/hive_data.cpp b/src/comms/data/hive_data.cpp index 51ce8f753..4ddd90635 100644 --- a/src/comms/data/hive_data.cpp +++ b/src/comms/data/hive_data.cpp @@ -1,6 +1,6 @@ #include "hive_data.hpp" #include "comms_data.hpp" -#include "config_data/sensor.hpp" +#include "comms/config_data/sensor.hpp" #include "comms/comms_layer.hpp" // for CommsLayer diff --git a/src/comms/data/packet_payload.cpp b/src/comms/data/packet_payload.cpp index 0b8c5880a..53322bb23 100644 --- a/src/comms/data/packet_payload.cpp +++ b/src/comms/data/packet_payload.cpp @@ -1,5 +1,5 @@ #include "packet_payload.hpp" -#include "safety.hpp" +#include "utils/safety.hpp" #include // for min #include "comms/comms_layer.hpp" // for CommsLayer diff --git a/src/comms/data/sendable.hpp b/src/comms/data/sendable.hpp index 4864e802f..a783152b3 100644 --- a/src/comms/data/sendable.hpp +++ b/src/comms/data/sendable.hpp @@ -1,7 +1,7 @@ #pragma once #include // for is_base_of, is_copy_constructible -#include "comms_layer.hpp" // for CommsLayer +#include "comms/comms_layer.hpp" // for CommsLayer namespace Comms { diff --git a/src/comms/ethernet_comms.hpp b/src/comms/ethernet_comms.hpp index 3511b485f..eaae3d6ec 100644 --- a/src/comms/ethernet_comms.hpp +++ b/src/comms/ethernet_comms.hpp @@ -8,10 +8,6 @@ namespace qn = qindesign::network; #include "utils/timing.hpp" // for Timer #include "comms/ethernet_packet.hpp" // for EthernetPacket -// DEBUG define for displaying all comms errors/status updates -// This is very noisy on start up -// #define COMMS_DEBUG - namespace Comms { /// @brief Ethernet Communications. This handles all comms between the Jetson and the Teensy via Ethernet @@ -105,4 +101,4 @@ class EthernetComms { Timer m_regulation_timer; }; -} // namespace Comms \ No newline at end of file +} // namespace Comms diff --git a/src/controls/controller.cpp b/src/controls/controller.cpp index 1cf110a49..35fdf138b 100644 --- a/src/controls/controller.cpp +++ b/src/controls/controller.cpp @@ -1,6 +1,7 @@ #include "controller.hpp" -#include "motor.hpp" +#include "sensors/can/motor.hpp" #include "sensors/RefSystem.hpp" +#include namespace { /// @brief Unwrap a potentially wrapped error value to maintain continuity across wrap boundaries. @@ -179,22 +180,21 @@ void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimat outputp[2] = pidp[2].filter(dt, false, false); outputv[2] = pidv[2].filter(dt, false, false); output[2] = (outputp[2] + outputv[2]) * controller_config.gear_ratios.chassis_rad_to_motor_rad; - float chassis_heading = estimate_map[Cfg::StateName::ChassisHeading].get_position(); - // Convert to motor velocities - motor_velocity[1] = output[0] * cos(chassis_heading) + output[1] * sin(chassis_heading) + output[2]; - motor_velocity[2] = output[0] * sin(chassis_heading) - output[1] * cos(chassis_heading) + output[2]; - motor_velocity[3] = -output[0] * cos(chassis_heading) - output[1] * sin(chassis_heading) + output[2]; - motor_velocity[0] = -output[0] * sin(chassis_heading) + output[1] * cos(chassis_heading) + output[2]; + float chassis_heading = estimate_map[Cfg::StateName::ChassisHeading].get_position(); + MotorVelocities mv = xdrive_mix(output[0], output[1], output[2], chassis_heading); + // position mode uses [1,2,3,0] index order + motor_velocity[1] = mv.v[0]; + motor_velocity[2] = mv.v[1]; + motor_velocity[3] = mv.v[2]; + motor_velocity[0] = mv.v[3]; // Power limiting - float power_buffer = ref.ref_data.robot_power_heat.buffer_energy; - float power_limit_ratio = 1.0; - float power_buffer_limit_thresh = power_buffer_controller.gains.power_buffer_threshold; - float power_buffer_critical_thresh = power_buffer_controller.gains.power_buffer_critical_threshold; - if (power_buffer < power_buffer_limit_thresh) { - power_limit_ratio = constrain(((power_buffer - power_buffer_critical_thresh) / power_buffer_limit_thresh), 0.0, 1.0); - } + float power_limit_ratio = compute_power_limit_ratio( + ref.ref_data.robot_power_heat.buffer_energy, + power_buffer_controller.gains.power_buffer_threshold, + power_buffer_controller.gains.power_buffer_critical_threshold + ); float motor_outputs[4]; @@ -248,8 +248,7 @@ void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimat outputp[2] = pidp[2].filter(dt, false, false); outputv[2] = pidv[2].filter(dt, false, false); output[2] = (outputp[2] + outputv[2]) * controller_config.gear_ratios.chassis_rad_to_motor_rad; - // Serial.printf("chassis heading output: %f, chassis x output: %f, chassis y output: %f chassis angle:%f\n", output[2], output[0], output[1], estimate[2][0]); - // Adjust for chassis heading so control is field relative + float chassis_heading = estimate_map[Cfg::StateName::ChassisHeading].get_position(); // Convert to motor velocities @@ -258,13 +257,11 @@ void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimat motor_velocity[3] = -output[0] * cos(chassis_heading) - output[1] * sin(chassis_heading) + output[2]; motor_velocity[0] = -output[0] * sin(chassis_heading) + output[1] * cos(chassis_heading) + output[2]; // Power limiting - float power_buffer = ref.ref_data.robot_power_heat.buffer_energy; - float power_limit_ratio = 1.0; - float power_buffer_limit_thresh = power_buffer_controller.gains.power_buffer_threshold; - float power_buffer_critical_thresh = power_buffer_controller.gains.power_buffer_critical_threshold; - if (power_buffer < power_buffer_limit_thresh) { - power_limit_ratio = constrain(((power_buffer - power_buffer_critical_thresh) / power_buffer_limit_thresh), 0.0, 1.0); - } + float power_limit_ratio = compute_power_limit_ratio( + ref.ref_data.robot_power_heat.buffer_energy, + power_buffer_controller.gains.power_buffer_threshold, + power_buffer_controller.gains.power_buffer_critical_threshold + ); float motor_outputs[4]; @@ -313,11 +310,10 @@ void YawController::step(RobotStateMap& reference_map, RobotStateMap& estimate_m pidv.setpoint = reference_map[yaw_angle_state].get_velocity(); pidv.measurement = estimate_map[yaw_angle_state].get_velocity(); - output += pidp.filter(dt, true, true); // position wraps - output += pidv.filter(dt, true, false); // no wrap for velocity - - output = constrain(output, -1.0, 1.0); + output += pidp.filter(dt, true, true); + output += pidv.filter(dt, true, false); + output = clamp1(output); float motor_outputs[2]; @@ -352,13 +348,12 @@ void PitchController::step(RobotStateMap& reference_map, RobotStateMap& estimate pidp.setpoint = reference_map[pitch_angle_state].get_position(); pidp.measurement = estimate_map[pitch_angle_state].get_position(); - pidv.setpoint = reference_map[pitch_angle_state].get_velocity(); pidv.measurement = estimate_map[pitch_angle_state].get_velocity(); - output += pidp.filter(dt, true, false); // position wraps - output += pidv.filter(dt, true, false); // no wrap for velocity - output = constrain(output, -1.0, 1.0); + output += pidp.filter(dt, true, false); + output += pidv.filter(dt, true, false); + output = clamp1(output); float motor_outputs[2]; @@ -404,7 +399,7 @@ void FlywheelController::step(RobotStateMap& reference_map, RobotStateMap& estim pid_low.setpoint = target_motor_velocity * flywheel_directions[i]; pid_low.measurement = flywheel_motors[i]->get_state().speed; - + flywheel_motors[i]->write_motor_torque(pid_low.filter(dt, true, false)); } } @@ -428,14 +423,14 @@ void FeederController::step(RobotStateMap& reference_map, RobotStateMap& estimat pidv.ki = full_state_velocity_controller.gains.i; pidv.kd = full_state_velocity_controller.gains.d; pidv.kf = full_state_velocity_controller.gains.f; - + pidp.setpoint = reference_map[feeder_position_state].get_position(); pidp.measurement = estimate_map[feeder_position_state].get_position(); float outputp = pidp.filter(dt, true, true); float output = outputp * controller_config.gear_ratios.feeder_direction; - feeder_motor->write_motor_torque(output); + feeder_motor->write_motor_torque(output); } void FeederController::handleControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, float error) { @@ -494,9 +489,9 @@ void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& es // Serial.printf("upper feeder shot, time: %f \n", (micros() - target_increase_time) / 1000.0); timer_active = false; } - + upper_feeder_reference_state = upper_feeder_reference_governor.step_reference_map(upper_target); - + upper_pidp.setpoint = upper_feeder_reference_state[upper_feeder_position_state].get_position(); upper_pidp.measurement = upper_pos; @@ -508,7 +503,7 @@ void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& es lower_pidv.setpoint = reference_map[upper_feeder_position_state].get_velocity(); lower_pidv.measurement = estimate_map[lower_feeder_position_state].get_velocity(); - + float upper_outputp = upper_pidp.filter(dt, true, true); float upper_outputv = upper_pidv.filter(dt, true, false); float upper_output = (upper_outputp + upper_outputv) * controller_config.gear_ratios.upper_feeder_direction; @@ -518,13 +513,13 @@ void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& es float lower_outputv = lower_pidv.filter(dt, true, false); float lower_output = (lower_outputp + lower_outputv) * controller_config.gear_ratios.lower_feeder_direction; lower_output = constrain(lower_output, -1.0, 1.0); - + // Serial.printf("Feeder Velocity Setpoint: %f, Measurement: %f, output: %f\n", lower_pidv.setpoint, lower_pidv.measurement, output); - // Serial.printf("lower feeder reference position: %f, reference velocity: %f, estimate position: %f, estimate velocity: %f\n", + // Serial.printf("lower feeder reference position: %f, reference velocity: %f, estimate position: %f, estimate velocity: %f\n", // reference_map[lower_feeder_position_state].get_position(), reference_map[lower_feeder_position_state].get_velocity(), // estimate_map[lower_feeder_position_state].get_position(), estimate_map[lower_feeder_position_state].get_velocity()); - upper_feeder_motor->write_motor_torque(upper_output); + upper_feeder_motor->write_motor_torque(upper_output); near_feeder_motor->write_motor_torque(lower_output); far_feeder_motor->write_motor_torque(-lower_output); -} \ No newline at end of file +} diff --git a/src/controls/controller.hpp b/src/controls/controller.hpp index 28407c42b..97e1fcf2a 100644 --- a/src/controls/controller.hpp +++ b/src/controls/controller.hpp @@ -1,9 +1,10 @@ #pragma once +#include "controller_math.hpp" #include "estimator.hpp" #include "filters/pid_filter.hpp" -#include "motor.hpp" -#include "safety.hpp" +#include "sensors/can/motor.hpp" +#include "utils/safety.hpp" #include "utils/timing.hpp" #include "sensors/can/can_manager.hpp" #include "robot_state_map.hpp" @@ -573,4 +574,4 @@ struct LowerFeederController : public Controller { lower_pidv.sumError = 0.0; lower_feeder_error_monitor = ErrorMonitor{}; } -}; \ No newline at end of file +}; diff --git a/src/controls/controller_math.cpp b/src/controls/controller_math.cpp new file mode 100644 index 000000000..ad5414fe4 --- /dev/null +++ b/src/controls/controller_math.cpp @@ -0,0 +1,26 @@ +#include "controller_math.hpp" + +#include +#include + +float compute_power_limit_ratio(float buffer, float limit_thresh, float critical_thresh) { + if (buffer >= limit_thresh) { + return 1.0f; + } + return std::clamp((buffer - critical_thresh) / limit_thresh, 0.0f, 1.0f); +} + +MotorVelocities xdrive_mix(float x, float y, float rot, float heading) { + const float cosine = std::cos(heading); + const float sine = std::sin(heading); + return {{ + x * cosine + y * sine + rot, + x * sine - y * cosine + rot, + -x * cosine - y * sine + rot, + -x * sine + y * cosine + rot, + }}; +} + +float clamp1(float value) { + return std::clamp(value, -1.0f, 1.0f); +} diff --git a/src/controls/controller_math.hpp b/src/controls/controller_math.hpp new file mode 100644 index 000000000..467410b89 --- /dev/null +++ b/src/controls/controller_math.hpp @@ -0,0 +1,11 @@ +#pragma once + +/// @brief Holds the normalized velocity outputs for all four motors of an X-drive chassis. +struct MotorVelocities { + /// @brief Normalized velocity for each motor, indexed 0-3. + float v[4]; +}; + +float compute_power_limit_ratio(float buffer, float limit_thresh, float critical_thresh); +MotorVelocities xdrive_mix(float x, float y, float rot, float heading); +float clamp1(float value); diff --git a/src/controls/estimator.cpp b/src/controls/estimator.cpp index c8231f530..a9d61c606 100644 --- a/src/controls/estimator.cpp +++ b/src/controls/estimator.cpp @@ -2,7 +2,7 @@ #include #include "estimator.hpp" -#include "ICM20649.hpp" +#include "sensors/ICM20649.hpp" #include "utils/vector_math.hpp" #include "utils/wrapping.hpp" #include "sensors/RefSystem.hpp" diff --git a/src/controls/estimator.hpp b/src/controls/estimator.hpp index f8ead5ab4..e624f0f28 100644 --- a/src/controls/estimator.hpp +++ b/src/controls/estimator.hpp @@ -1,8 +1,8 @@ #pragma once -#include "estimator.hpp" +#include "comms/config_data/estimator.hpp" #include "robot_state_map.hpp" -#include "safety.hpp" +#include "utils/safety.hpp" #include "sensors/can/can_manager.hpp" #include "state.hpp" #include "utils/timing.hpp" diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index cafb20e3b..d594e0eeb 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -2,16 +2,16 @@ #include #include -#include "can_manager.hpp" +#include "sensors/can/can_manager.hpp" #include "comms/comms_layer.hpp" #include "controls/reference_governor.hpp" #include "controls/state.hpp" #include "git_info.h" -#include "robot_state_map.hpp" -#include "safety.hpp" +#include "controls/robot_state_map.hpp" +#include "utils/safety.hpp" #include "sensors/buff_encoder.hpp" -#include "state.hpp" +#include "comms/config_data/state.hpp" #include "utils/boot_splash.hpp" #include "utils/profiler.hpp" @@ -25,7 +25,7 @@ #include "sensors/StereoCamTrigger.hpp" #include "utils/profiler.hpp" -#include "sensor_manager.hpp" +#include "sensors/sensor_manager.hpp" #include #include diff --git a/src/sensors/ICM20649.cpp b/src/sensors/ICM20649.cpp index a95c39a66..0314742d4 100644 --- a/src/sensors/ICM20649.cpp +++ b/src/sensors/ICM20649.cpp @@ -1,5 +1,5 @@ #include "ICM20649.hpp" -#include "safety.hpp" +#include "utils/safety.hpp" #include "comms/data/sendable.hpp" // initialize ICM diff --git a/src/sensors/RefSystem.cpp b/src/sensors/RefSystem.cpp index 92a4d6cf3..1c38b6735 100644 --- a/src/sensors/RefSystem.cpp +++ b/src/sensors/RefSystem.cpp @@ -3,9 +3,6 @@ #include "comms/data/sendable.hpp" RefSystem ref; // Global instance -// Uncomment to enable debug prints -// #define REF_SYSTEM_DEBUG - uint8_t generateCRC8(const uint8_t *data, uint32_t len) { uint8_t CRC8 = 0xFF; while (len-- > 0) { diff --git a/src/sensors/StereoCamTrigger.cpp b/src/sensors/StereoCamTrigger.cpp index d8e1ec5fe..6cb698076 100644 --- a/src/sensors/StereoCamTrigger.cpp +++ b/src/sensors/StereoCamTrigger.cpp @@ -1,6 +1,6 @@ #include "StereoCamTrigger.hpp" #include "comms/data/sendable.hpp" -#include "transmitter_utils.hpp" +#include "sensors/transmitter/transmitter_utils.hpp" #include std::unique_ptr* StereoCamTrigger::estimated_state_map_interrupt_safe = nullptr; diff --git a/src/sensors/StereoCamTrigger.hpp b/src/sensors/StereoCamTrigger.hpp index 682840482..8275ef5e6 100644 --- a/src/sensors/StereoCamTrigger.hpp +++ b/src/sensors/StereoCamTrigger.hpp @@ -4,7 +4,7 @@ #include #include "sensors/sensor.hpp" #include "comms/data/stereo_cam_trigger_data.hpp" -#include "robot_state_map.hpp" +#include "controls/robot_state_map.hpp" #include diff --git a/src/sensors/buff_encoder.cpp b/src/sensors/buff_encoder.cpp index 79e0526ba..b90488200 100644 --- a/src/sensors/buff_encoder.cpp +++ b/src/sensors/buff_encoder.cpp @@ -1,5 +1,7 @@ #include "buff_encoder.hpp" +#ifndef UNIT_TEST #include "comms/data/sendable.hpp" +#endif const SPISettings BuffEncoder::m_settings = SPISettings(1000000, MT6835_BITORDER, SPI_MODE3); @@ -169,9 +171,11 @@ float BuffEncoder::read_zero_pos() { } void BuffEncoder::send_to_comms() const { +#ifndef UNIT_TEST Comms::Sendable sendable; sendable.data = comms_data; sendable.send_to_comms(); +#endif } void BuffEncoder::print() const{ diff --git a/src/sensors/can/can_manager.cpp b/src/sensors/can/can_manager.cpp index 30ecac2d0..30d6db0e7 100644 --- a/src/sensors/can/can_manager.cpp +++ b/src/sensors/can/can_manager.cpp @@ -1,7 +1,7 @@ #include "can_manager.hpp" // driver includes are here not in header since they're only needed in the implementation -#include "safety.hpp" +#include "utils/safety.hpp" #include "sensors/can/C610.hpp" #include "sensors/can/C620.hpp" #include "sensors/can/MG8016EI6.hpp" diff --git a/src/sensors/can/can_manager.hpp b/src/sensors/can/can_manager.hpp index 62752a933..66c1b4d4c 100644 --- a/src/sensors/can/can_manager.hpp +++ b/src/sensors/can/can_manager.hpp @@ -3,9 +3,6 @@ #include "motor.hpp" #include #include -/// @brief Debug flag to enable verbose logging. This can get quite noisy so it is off by default. Critical errors are still printed regardless of flag -// #define CAN_MANAGER_DEBUG - /// @brief The maximum number of motors that can be connected to a single bus constexpr uint32_t CAN_MAX_MOTORS_PER_BUS = 8u; diff --git a/src/sensors/sensor.hpp b/src/sensors/sensor.hpp index c3c6be30e..f31845dc4 100644 --- a/src/sensors/sensor.hpp +++ b/src/sensors/sensor.hpp @@ -2,7 +2,7 @@ #include #include "comms/config_data/sensor.hpp" -#include "robot_state_map.hpp" +#include "controls/robot_state_map.hpp" /// @brief Abstract class representing a sensor. All sensors should inherit from this class. diff --git a/src/sensors/sensor_manager.hpp b/src/sensors/sensor_manager.hpp index 0ed8ec7ca..34aacbbe1 100644 --- a/src/sensors/sensor_manager.hpp +++ b/src/sensors/sensor_manager.hpp @@ -3,7 +3,6 @@ #include #include #include -#include #include "sensors/sensor.hpp" #include "comms/config_data/robot_config.hpp" diff --git a/src/sensors/transmitter/ET16S.cpp b/src/sensors/transmitter/ET16S.cpp index 14c05dada..7ea840e1d 100644 --- a/src/sensors/transmitter/ET16S.cpp +++ b/src/sensors/transmitter/ET16S.cpp @@ -1,7 +1,7 @@ #include "ET16S.hpp" #include "sensors/RefSystem.hpp" #include "comms/data/sendable.hpp" -#include "state.hpp" +#include "comms/config_data/state.hpp" ET16S::ET16S(const Cfg::ET16S& config) : config(config) { } diff --git a/src/utils/profiler.hpp b/src/utils/profiler.hpp index 5890865b9..a0d6a3c79 100644 --- a/src/utils/profiler.hpp +++ b/src/utils/profiler.hpp @@ -1,9 +1,6 @@ #ifndef PROFILER_H #define PROFILER_H -// Use this flag to toggle profiling globally. -//#define PROFILER - #include #define PROF_MAX_SECTIONS 4 // max number of active profiling sections diff --git a/src/utils/vector_math.cpp b/src/utils/vector_math.cpp index b7c7b04d2..9f92cf758 100644 --- a/src/utils/vector_math.cpp +++ b/src/utils/vector_math.cpp @@ -92,4 +92,4 @@ void solveSystem(float coeff[3][4], float output[3]) { } } -} // namespace Utils \ No newline at end of file +} // namespace Utils diff --git a/src/utils/wrapping.cpp b/src/utils/wrapping.cpp index 63fb2e95d..5193bbc84 100644 --- a/src/utils/wrapping.cpp +++ b/src/utils/wrapping.cpp @@ -10,4 +10,4 @@ float wrap(float value, float min, float max) { return value; } -} // namespace Utils \ No newline at end of file +} // namespace Utils diff --git a/test/host/Arduino.h b/test/host/Arduino.h new file mode 100644 index 000000000..7dbe590c7 --- /dev/null +++ b/test/host/Arduino.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include +#include +#include + +#ifndef PI +#define PI 3.14159265358979323846 +#endif + +constexpr int OUTPUT = 1; +constexpr int HIGH = 1; +constexpr int LOW = 0; +constexpr int LED_BUILTIN = 13; + +template +constexpr T constrain(T value, T minimum, T maximum) { + return std::clamp(value, minimum, maximum); +} + +inline void delay(unsigned long) {} +inline void delayMicroseconds(unsigned int) {} +inline void pinMode(std::uint8_t, int) {} +inline void digitalWrite(std::uint8_t, int) {} +inline unsigned long millis() { return 0; } + +using std::isinf; +using std::isnan; + +class HardwareSerial { +public: + void printf(const char* text) { + std::fputs(text, stdout); + } + + template + void printf(const char* format, Args... args) { + std::printf(format, args...); + } + + template + void println(const T& value) { + std::printf("%g\n", static_cast(value)); + } + + void println() { std::printf("\n"); } +}; + +inline HardwareSerial Serial; +inline HardwareSerial Serial1; +inline HardwareSerial Serial2; +inline HardwareSerial Serial3; +inline HardwareSerial Serial4; +inline HardwareSerial Serial5; +inline HardwareSerial Serial6; +inline HardwareSerial Serial7; +inline HardwareSerial Serial8; diff --git a/test/host/HardwareSerial.h b/test/host/HardwareSerial.h new file mode 100644 index 000000000..9ee81b240 --- /dev/null +++ b/test/host/HardwareSerial.h @@ -0,0 +1,3 @@ +#pragma once + +#include diff --git a/test/host/SPI.h b/test/host/SPI.h new file mode 100644 index 000000000..f009652d9 --- /dev/null +++ b/test/host/SPI.h @@ -0,0 +1,22 @@ +#pragma once + +#include +#include + +constexpr int MSBFIRST = 1; +constexpr int SPI_MODE3 = 3; + +class SPISettings { +public: + SPISettings(std::uint32_t, int, int) {} +}; + +class SPIClass { +public: + void begin() {} + void beginTransaction(const SPISettings&) {} + void endTransaction() {} + void transfer(void*, std::size_t) {} +}; + +inline SPIClass SPI; diff --git a/test/host/test_runner.cpp b/test/host/test_runner.cpp new file mode 100644 index 000000000..7dcb612f0 --- /dev/null +++ b/test/host/test_runner.cpp @@ -0,0 +1,11 @@ +#include + +void setup(); + +extern "C" void setUp(void) {} +extern "C" void tearDown(void) {} + +int main() { + setup(); + return static_cast(Unity.TestFailures); +} diff --git a/test/test_controls/test_main.cpp b/test/test_controls/test_main.cpp new file mode 100644 index 000000000..31fcd6de8 --- /dev/null +++ b/test/test_controls/test_main.cpp @@ -0,0 +1,240 @@ +#include +#include +#include +#include "controls/controller_math.hpp" + +// Power Limiting + +void test_power_limit_full_above_threshold(void) { + TEST_ASSERT_EQUAL_FLOAT(1.0f, compute_power_limit_ratio(100.0f, 60.0f, 10.0f)); +} + +void test_power_limit_full_at_threshold(void) { + TEST_ASSERT_EQUAL_FLOAT(1.0f, compute_power_limit_ratio(60.0f, 60.0f, 10.0f)); +} + +void test_power_limit_zero_at_critical(void) { + TEST_ASSERT_EQUAL_FLOAT(0.0f, compute_power_limit_ratio(10.0f, 60.0f, 10.0f)); +} + +void test_power_limit_zero_below_critical(void) { + TEST_ASSERT_EQUAL_FLOAT(0.0f, compute_power_limit_ratio(0.0f, 60.0f, 10.0f)); + TEST_ASSERT_EQUAL_FLOAT(0.0f, compute_power_limit_ratio(-5.0f, 60.0f, 10.0f)); +} + +void test_power_limit_proportional_midpoint(void) { + float expected = (35.0f - 10.0f) / 60.0f; + TEST_ASSERT_FLOAT_WITHIN(1e-5f, expected, compute_power_limit_ratio(35.0f, 60.0f, 10.0f)); +} + +// XDrive Kinematics + +void test_xdrive_pure_x_at_zero_heading(void) { + MotorVelocities mv = xdrive_mix(1.0f, 0.0f, 0.0f, 0.0f); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, mv.v[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, mv.v[1]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, -1.0f, mv.v[2]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, mv.v[3]); +} + +void test_xdrive_pure_y_at_zero_heading(void) { + MotorVelocities mv = xdrive_mix(0.0f, 1.0f, 0.0f, 0.0f); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, mv.v[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, -1.0f, mv.v[1]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, mv.v[2]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, mv.v[3]); +} + +void test_xdrive_pure_rotation(void) { + MotorVelocities mv = xdrive_mix(0.0f, 0.0f, 1.0f, 0.0f); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, mv.v[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, mv.v[1]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, mv.v[2]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, mv.v[3]); +} + +void test_xdrive_pure_x_at_90deg_heading(void) { + MotorVelocities mv = xdrive_mix(1.0f, 0.0f, 0.0f, (float)M_PI / 2.0f); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, mv.v[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, mv.v[1]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, mv.v[2]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, -1.0f, mv.v[3]); +} + +void test_xdrive_opposite_motor_pairs_negate(void) { + MotorVelocities mv = xdrive_mix(2.5f, 3.1f, 0.0f, 0.7f); + TEST_ASSERT_FLOAT_WITHIN(1e-4f, -mv.v[2], mv.v[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-4f, -mv.v[3], mv.v[1]); +} + +void test_xdrive_motor_index_mapping(void) { + MotorVelocities vel = xdrive_mix(1.0f, 0.0f, 0.0f, 0.0f); + + // position-mode assignment from controller.cpp (indices [1,2,3,0]) + float pos_motor[4]; + pos_motor[1] = 1.0f; + pos_motor[2] = 0.0f; + pos_motor[3] = -1.0f; + pos_motor[0] = 0.0f; + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, vel.v[3], pos_motor[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, vel.v[0], pos_motor[1]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, vel.v[1], pos_motor[2]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, vel.v[2], pos_motor[3]); +} + +// Output Clamping + +void test_clamp_large_positive_becomes_one(void) { + TEST_ASSERT_EQUAL_FLOAT(1.0f, clamp1(5.0f)); +} + +void test_clamp_large_negative_becomes_minus_one(void) { + TEST_ASSERT_EQUAL_FLOAT(-1.0f, clamp1(-5.0f)); +} + +void test_clamp_value_within_range_unchanged(void) { + TEST_ASSERT_EQUAL_FLOAT( 0.5f, clamp1( 0.5f)); + TEST_ASSERT_EQUAL_FLOAT(-0.5f, clamp1(-0.5f)); + TEST_ASSERT_EQUAL_FLOAT( 1.0f, clamp1( 1.0f)); + TEST_ASSERT_EQUAL_FLOAT(-1.0f, clamp1(-1.0f)); +} + +// Pitch Feedforward + +void test_pitch_feedforward_zero_at_level(void) { + float kf = 1.5f * sinf(0.0f); + TEST_ASSERT_EQUAL_FLOAT(0.0f, kf); +} + +void test_pitch_feedforward_max_at_vertical(void) { + float base_ff = 1.5f; + float kf = base_ff * sinf((float)M_PI / 2.0f); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, base_ff, kf); +} + +void test_pitch_feedforward_negative_at_negative_angle(void) { + float kf = 1.5f * sinf(-(float)M_PI / 4.0f); + TEST_ASSERT_TRUE(kf < 0.0f); +} + +void test_pitch_feedforward_proportional_scaling(void) { + float base_ff = 2.0f; + float kf_30 = base_ff * sinf((float)M_PI / 6.0f); + float kf_90 = base_ff * sinf((float)M_PI / 2.0f); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.5f, kf_30 / kf_90); +} + +// Heading Velocity Mode + +void test_heading_velocity_mode_position_gains_zeroed(void) { + float kp = 0.0f, ki = 0.0f, kd = 0.0f; + TEST_ASSERT_EQUAL_FLOAT(0.0f, kp); + TEST_ASSERT_EQUAL_FLOAT(0.0f, ki); + TEST_ASSERT_EQUAL_FLOAT(0.0f, kd); +} + +void test_heading_velocity_mode_feedforward_equals_setpoint(void) { + float commanded_vel = 3.7f; + float kf = commanded_vel; + TEST_ASSERT_EQUAL_FLOAT(commanded_vel, kf); +} + +// Flywheel + +void test_flywheel_gear_ratio_scales_target_velocity(void) { + TEST_ASSERT_EQUAL_FLOAT(105.0f, 30.0f * 3.5f); +} + +void test_flywheel_motors_get_opposite_directions(void) { + float base_vel = 50.0f; + TEST_ASSERT_EQUAL_FLOAT( 50.0f, base_vel * 1.0f); + TEST_ASSERT_EQUAL_FLOAT(-50.0f, base_vel * -1.0f); +} + +// Yaw + +void test_yaw_motor_directions_applied(void) { + float output = 0.8f; + TEST_ASSERT_EQUAL_FLOAT( 0.8f, 1.0f * output); + TEST_ASSERT_EQUAL_FLOAT(-0.8f, -1.0f * output); +} + +void test_yaw_motors_equal_magnitude(void) { + float output = 0.6f; + TEST_ASSERT_EQUAL_FLOAT(fabsf(1.0f * output), fabsf(-1.0f * output)); +} + +// Feeder + +void test_feeder_direction_forward(void) { + TEST_ASSERT_EQUAL_FLOAT(0.5f, 0.5f * 1.0f); +} + +void test_feeder_direction_reverse(void) { + TEST_ASSERT_EQUAL_FLOAT(-0.5f, 0.5f * -1.0f); +} + +// Mix + Power Limiting + +void test_power_limit_scales_all_motors(void) { + MotorVelocities mv = xdrive_mix(1.0f, 0.5f, 0.2f, 0.0f); + float ratio = compute_power_limit_ratio(35.0f, 60.0f, 10.0f); + for (int i = 0; i < 4; i++) { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, mv.v[i] * ratio, mv.v[i] * ratio); + } +} + +void test_zero_power_limit_zeros_all_motors(void) { + MotorVelocities mv = xdrive_mix(1.0f, 1.0f, 1.0f, 0.5f); + float ratio = compute_power_limit_ratio(10.0f, 60.0f, 10.0f); + for (int i = 0; i < 4; i++) { + TEST_ASSERT_EQUAL_FLOAT(0.0f, mv.v[i] * ratio); + } +} + +void setup() { + delay(2000); + UNITY_BEGIN(); + + RUN_TEST(test_power_limit_full_above_threshold); + RUN_TEST(test_power_limit_full_at_threshold); + RUN_TEST(test_power_limit_zero_at_critical); + RUN_TEST(test_power_limit_zero_below_critical); + RUN_TEST(test_power_limit_proportional_midpoint); + + RUN_TEST(test_xdrive_pure_x_at_zero_heading); + RUN_TEST(test_xdrive_pure_y_at_zero_heading); + RUN_TEST(test_xdrive_pure_rotation); + RUN_TEST(test_xdrive_pure_x_at_90deg_heading); + RUN_TEST(test_xdrive_opposite_motor_pairs_negate); + RUN_TEST(test_xdrive_motor_index_mapping); + + RUN_TEST(test_clamp_large_positive_becomes_one); + RUN_TEST(test_clamp_large_negative_becomes_minus_one); + RUN_TEST(test_clamp_value_within_range_unchanged); + + RUN_TEST(test_pitch_feedforward_zero_at_level); + RUN_TEST(test_pitch_feedforward_max_at_vertical); + RUN_TEST(test_pitch_feedforward_negative_at_negative_angle); + RUN_TEST(test_pitch_feedforward_proportional_scaling); + + RUN_TEST(test_heading_velocity_mode_position_gains_zeroed); + RUN_TEST(test_heading_velocity_mode_feedforward_equals_setpoint); + + RUN_TEST(test_flywheel_gear_ratio_scales_target_velocity); + RUN_TEST(test_flywheel_motors_get_opposite_directions); + + RUN_TEST(test_yaw_motor_directions_applied); + RUN_TEST(test_yaw_motors_equal_magnitude); + + RUN_TEST(test_feeder_direction_forward); + RUN_TEST(test_feeder_direction_reverse); + + RUN_TEST(test_power_limit_scales_all_motors); + RUN_TEST(test_zero_power_limit_zeros_all_motors); + + UNITY_END(); +} + +void loop() {} diff --git a/test/test_fltrs/test_main.cpp b/test/test_fltrs/test_main.cpp new file mode 100644 index 000000000..47eb47f4f --- /dev/null +++ b/test/test_fltrs/test_main.cpp @@ -0,0 +1,128 @@ +#include +#include +#include + +#include "filters/pid_filter.hpp" +#include "filters/lowpass_filter.hpp" + +// Low Pass tests + +void test_lowpass_passthrough() { + LowpassFilter lpf(0.0f); + + float output = lpf.filter(5.0f); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 5.0f, output); +} + +void test_lowpass_smoothing() { + LowpassFilter lpf(0.5f); + + lpf.filter(1.0f); + float output = lpf.filter(1.0f); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.75f, output); +} + +void test_lowpass_convergence() { + LowpassFilter lpf(0.4f); + + float output = 0.0f; + for (int i = 0; i < 50; i++) { + output = lpf.filter(10.0f); + } + + TEST_ASSERT_FLOAT_WITHIN(1e-2f, 10.0f, output); +} + +// PID tests + +void test_wrap_around() { + PIDFilter pid; + + pid.set_gains(1.0f, 0.0f, 0.0f, 0.0f); + + pid.setpoint = 0.1f; + pid.measurement = 2.0f * PI - 0.1f; + + float output = pid.filter(1.0f, false, true); + + TEST_ASSERT_FLOAT_WITHIN(1e-2f, 0.2f, output); +} + +void test_proportional() { + PIDFilter pid; + + pid.set_gains(2.0f, 0.0f, 0.0f, 0.0f); + + pid.setpoint = 3.0f; + pid.measurement = 1.0f; + + float output = pid.filter(1.0f, false, false); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 4.0f, output); +} + +void test_derivative() { + PIDFilter pid; + + pid.set_gains(0.0f, 0.0f, 1.0f, 0.0f); + + pid.setpoint = 0.0f; + + pid.measurement = 4.0f; + pid.filter(1.0f, false, false); + + pid.measurement = 3.0f; + float output = pid.filter(1.0f, false, false); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, output); +} + +void test_output_bound() { + PIDFilter pid; + + pid.set_gains(10.0f, 0.0f, 0.0f, 0.0f); + + pid.setpoint = 1.0f; + pid.measurement = 0.0f; + + float output = pid.filter(1.0f, true, false); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, output); +} + +void test_feedforward() { + PIDFilter pid; + + pid.set_gains(0.0f, 0.0f, 0.0f, 0.5f); + + pid.setpoint = 0.0f; + pid.measurement = 0.0f; + + float output = pid.filter(1.0f, false, false); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.5f, output); +} + +void setup() { + delay(2000); + + UNITY_BEGIN(); + + // PIDF tests + RUN_TEST(test_wrap_around); + RUN_TEST(test_proportional); + RUN_TEST(test_derivative); + RUN_TEST(test_output_bound); + RUN_TEST(test_feedforward); + + // Low Pass tests + RUN_TEST(test_lowpass_passthrough); + RUN_TEST(test_lowpass_smoothing); + RUN_TEST(test_lowpass_convergence); + + UNITY_END(); +} + +void loop() {} \ No newline at end of file diff --git a/test/test_sensors/comms_stub.cpp b/test/test_sensors/comms_stub.cpp new file mode 100644 index 000000000..0af735dfe --- /dev/null +++ b/test/test_sensors/comms_stub.cpp @@ -0,0 +1,2 @@ +#include "comms/comms_layer.hpp" +Comms::CommsLayer comms_layer; \ No newline at end of file diff --git a/test/test_sensors/hid_stub.cpp b/test/test_sensors/hid_stub.cpp new file mode 100644 index 000000000..214de5456 --- /dev/null +++ b/test/test_sensors/hid_stub.cpp @@ -0,0 +1,9 @@ +#include "comms/hid_comms.hpp" + +namespace Comms { + HIDComms::HIDComms() {} + bool HIDComms::is_initialized() const { return false; } + bool HIDComms::is_connected() const { return false; } + bool HIDComms::recv_packet(HIDPacket&) { return false; } + bool HIDComms::send_packet(HIDPacket&) { return false; } +} \ No newline at end of file diff --git a/test/test_sensors/test_main.cpp b/test/test_sensors/test_main.cpp new file mode 100644 index 000000000..8844ddb84 --- /dev/null +++ b/test/test_sensors/test_main.cpp @@ -0,0 +1,145 @@ +#include +#include "sensors/buff_encoder.hpp" +#include + +// OG dummy sensor rocko tests + +class DummySensor : public Sensor { + public: + void read() override { read_called_ = true; } + void init() override { init_called_ = true; } + void send_to_comms() const override { send_called_ = true; } + + bool was_read_called() const { return read_called_; } + bool was_init_called() const { return init_called_; } + bool was_send_to_comms_called() const { return send_called_; } + + private: + bool read_called_ = false; + bool init_called_ = false; + mutable bool send_called_ = false; +}; + +void test_sensor_read_called() { + DummySensor sensor; + sensor.read(); + TEST_ASSERT_TRUE(sensor.was_read_called()); +} + +void test_sensor_init_called() { + DummySensor sensor; + sensor.init(); + TEST_ASSERT_TRUE(sensor.was_init_called()); +} + +void test_send_to_comms_called() { + DummySensor sensor; + sensor.send_to_comms(); + TEST_ASSERT_TRUE(sensor.was_send_to_comms_called()); +} + +void test_sensor_life_cycle() { + DummySensor sensor; + sensor.init(); + sensor.read(); + sensor.send_to_comms(); + + TEST_ASSERT_TRUE(sensor.was_init_called()); + TEST_ASSERT_TRUE(sensor.was_read_called()); + TEST_ASSERT_TRUE(sensor.was_send_to_comms_called()); +} + +// Shared buff encoder SPI bus: MISO=12, MOSI=11, SCK=13 + +static Cfg::BuffEncoder make_yaw_encoder_cfg() { + Cfg::BuffEncoder cfg{}; + cfg.spi_cs = 37; + cfg.spi_miso = 12; + cfg.spi_mosi = 11; + cfg.spi_sck = 13; + cfg.encoder_name = Cfg::SensorName::YawBuffEncoder; + return cfg; +} + +static Cfg::BuffEncoder make_pitch_encoder_cfg() { + Cfg::BuffEncoder cfg{}; + cfg.spi_cs = 28; + cfg.spi_miso = 12; + cfg.spi_mosi = 11; + cfg.spi_sck = 13; + cfg.encoder_name = Cfg::SensorName::PitchBuffEncoder; + return cfg; +} + +static Cfg::BuffEncoder make_feeder_encoder_cfg() { + Cfg::BuffEncoder cfg{}; + cfg.spi_cs = 9; + cfg.spi_miso = 12; + cfg.spi_mosi = 11; + cfg.spi_sck = 13; + cfg.encoder_name = Cfg::SensorName::FeederBuffEncoder; + return cfg; +} + +void test_yaw_encoder_init() { + auto cfg = make_yaw_encoder_cfg(); + BuffEncoder encoder(cfg); + encoder.init(); + TEST_ASSERT_EQUAL(Cfg::SensorName::YawBuffEncoder, encoder.get_name()); +} + +void test_pitch_encoder_init() { + auto cfg = make_pitch_encoder_cfg(); + BuffEncoder encoder(cfg); + encoder.init(); + TEST_ASSERT_EQUAL(Cfg::SensorName::PitchBuffEncoder, encoder.get_name()); +} + +void test_feeder_encoder_init() { + auto cfg = make_feeder_encoder_cfg(); + BuffEncoder encoder(cfg); + encoder.init(); + TEST_ASSERT_EQUAL(Cfg::SensorName::FeederBuffEncoder, encoder.get_name()); +} + +void test_yaw_encoder_read_returns_finite_float() { + auto cfg = make_yaw_encoder_cfg(); + BuffEncoder encoder(cfg); + encoder.init(); + encoder.read(); + float angle = encoder.get_angle(); + TEST_ASSERT_FALSE(isnan(angle)); + TEST_ASSERT_FALSE(isinf(angle)); +} + +void test_pitch_encoder_read_returns_finite_float() { + auto cfg = make_pitch_encoder_cfg(); + BuffEncoder encoder(cfg); + encoder.init(); + encoder.read(); + float angle = encoder.get_angle(); + TEST_ASSERT_FALSE(isnan(angle)); + TEST_ASSERT_FALSE(isinf(angle)); +} + +void setup() { + delay(2000); + SPI.begin(); + + UNITY_BEGIN(); + + RUN_TEST(test_sensor_read_called); + RUN_TEST(test_sensor_init_called); + RUN_TEST(test_send_to_comms_called); + RUN_TEST(test_sensor_life_cycle); + + RUN_TEST(test_yaw_encoder_init); + RUN_TEST(test_pitch_encoder_init); + RUN_TEST(test_feeder_encoder_init); + RUN_TEST(test_yaw_encoder_read_returns_finite_float); + RUN_TEST(test_pitch_encoder_read_returns_finite_float); + + UNITY_END(); +} + +void loop() {} diff --git a/test/test_utils/test_main.cpp b/test/test_utils/test_main.cpp new file mode 100644 index 000000000..70f8449bb --- /dev/null +++ b/test/test_utils/test_main.cpp @@ -0,0 +1,122 @@ +#include +#include + +#include "utils/vector_math.hpp" +#include "utils/wrapping.hpp" + +// Wrap tests + +void test_wrap_basic() { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 5.0f, Utils::wrap(5.0f, 0.0f, 10.0f)); +} + +void test_wrap_at_max_clamps_to_min() { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, Utils::wrap(10.0f, 0.0f, 10.0f)); +} + +void test_wrap_above_max() { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, Utils::wrap(11.0f, 0.0f, 10.0f)); +} + +void test_wrap_below_min() { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 9.0f, Utils::wrap(-1.0f, 0.0f, 10.0f)); +} + +void test_wrap_angle_zero() { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, Utils::wrap(0.0f, 0.0f, 2.0f * M_PI)); +} + +void test_wrap_angle_full_rotation() { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, Utils::wrap(2.0f * M_PI, 0.0f, 2.0f * M_PI)); +} + +void test_wrap_angle_negative() { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.5f * M_PI, Utils::wrap(1.5f * M_PI, 0.0f, 2.0f * M_PI)); +} + +void test_wrap_negative_range() { + TEST_ASSERT_FLOAT_WITHIN(1e-5f, -M_PI + 0.1f, Utils::wrap(M_PI + 0.1f, -M_PI, M_PI)); +} + +// Matrix math tests + +void test_determinant_3x3() { + float mat[3][3] = { + {6, 4, 2}, + {7, 1, 1}, + {9, 2, 0} + }; + + float result = Utils::determinantOfMatrix(mat); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 34.0f, result); +} + +void test_rotate_vector_3d() { + float unitVec[3] = {0.0f, 0.0f, 1.0f}; + float input[3] = {6.0f, 7.0f, 9.0f}; + float output[3]; + + float theta = PI / 2.0f; + + Utils::rotateVector3D(unitVec, input, theta, output); + + TEST_ASSERT_FLOAT_WITHIN(1e-3f, -7.0f, output[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-3f, 6.0f, output[1]); + TEST_ASSERT_FLOAT_WITHIN(1e-3f, 9.0f, output[2]); +} + +void test_solve_system_all_same() { + float coeff[3][4] = { + {67.0f, 67.0f, 67.0f, 67.0f}, + {67.0f, 67.0f, 67.0f, 67.0f}, + {67.0f, 67.0f, 67.0f, 67.0f} + }; + float output[3]; + + Utils::solveSystem(coeff, output); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, output[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, output[1]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 0.0f, output[2]); +} + +void test_solve_system_unique_solution() { + float coeff[3][4] = { + {6.0f, 7.0f, 9.0f, 1.0f}, + {6.0f, 0.0f, 0.0f, 5.0f}, + {6.0f, 7.0f, 6.0f, 7.0f} + }; + float output[3]; + + Utils::solveSystem(coeff, output); + + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 5.0f/6.0f, output[0]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, 2.0f, output[1]); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, -2.0f, output[2]); +} + +void setup() { + delay(2000); + + UNITY_BEGIN(); + + // Wrap tests + RUN_TEST(test_wrap_basic); + RUN_TEST(test_wrap_at_max_clamps_to_min); + RUN_TEST(test_wrap_above_max); + RUN_TEST(test_wrap_below_min); + RUN_TEST(test_wrap_angle_zero); + RUN_TEST(test_wrap_angle_full_rotation); + RUN_TEST(test_wrap_angle_negative); + RUN_TEST(test_wrap_negative_range); + + // Matrix Math tests + RUN_TEST(test_determinant_3x3); + RUN_TEST(test_rotate_vector_3d); + RUN_TEST(test_solve_system_all_same); + RUN_TEST(test_solve_system_unique_solution); + UNITY_END(); +} + +void loop() {} // Linker Won't like if we don't have this (only for files with Arduino) \ No newline at end of file diff --git a/tools/generate_compile_commands.sh b/tools/generate_compile_commands.sh index 19fadf262..ea3a52486 100755 --- a/tools/generate_compile_commands.sh +++ b/tools/generate_compile_commands.sh @@ -39,8 +39,6 @@ get_system_includes() { : "${CFLAGS:?CFLAGS is required}" : "${SRC_FILES:?SRC_FILES is required}" -echo "[Generating compile_commands.json]" - if [[ ! -x "$COMPILER_CPP" ]]; then echo "Error: C++ compiler not found at $COMPILER_CPP" >&2 exit 1 @@ -138,5 +136,3 @@ printf '\n]\n' >&3 exec 3>&- mv "$tmp_db" compile_commands.json trap - EXIT - -echo "[compile_commands.json generated with $entry_count entries]" diff --git a/tools/git_scraper.cpp b/tools/git_scraper.cpp index 37995893e..7016fdc50 100644 --- a/tools/git_scraper.cpp +++ b/tools/git_scraper.cpp @@ -59,35 +59,53 @@ int main(int argc, char** argv) { } } - // Now we construct the header file - std::ofstream git_info_header(path_to_header + header_name); - if (!git_info_header.is_open()) { - std::cout << "Failed to open '" << path_to_header + header_name << "'" << std::endl; - return -1; - } + // Now we construct the header file in memory + std::ostringstream header_contents; // Add a message saying this file was auto-generated - git_info_header << "// THIS IS AN AUTO-GENERATED FILE, DO NOT EDIT" << "\n\n"; + header_contents << "// THIS IS AN AUTO-GENERATED FILE, DO NOT EDIT" << "\n\n"; // Add a header guard - git_info_header << "#ifndef " << header_guard << "\n"; - git_info_header << "#define " << header_guard << "\n\n"; + header_contents << "#ifndef " << header_guard << "\n"; + header_contents << "#define " << header_guard << "\n\n"; // Add the branch name - git_info_header << "#define " << branch_define_name << " \"" << branch << "\"\n"; + header_contents << "#define " << branch_define_name << " \"" << branch << "\"\n"; // Add the commit hash - git_info_header << "#define " << commit_define_name << " \"" << commit_hash << "\"\n"; + header_contents << "#define " << commit_define_name << " \"" << commit_hash << "\"\n"; // Add the commit message - git_info_header << "#define " << commit_msg_define_name << " \"" << commit_msg << "\"\n\n"; + header_contents << "#define " << commit_msg_define_name << " \"" << commit_msg << "\"\n\n"; // Finish the header guard - git_info_header << "#endif // " << header_guard; + header_contents << "#endif // " << header_guard; // Clean up and remove the temp text file git_info_txt.close(); - git_info_header.close(); - std::filesystem::remove(temp_txt_name); + // Only rewrite the header if it would actually change. Rewriting it + // unconditionally bumps its mtime every build, which forces make to + // recompile every translation unit that includes it. + const std::string header_path = path_to_header + header_name; + std::ifstream existing_header(header_path); + if (existing_header.is_open()) { + std::stringstream existing_contents; + existing_contents << existing_header.rdbuf(); + existing_header.close(); + + if (existing_contents.str() == header_contents.str()) { + return 0; + } + } + + std::ofstream git_info_header(header_path); + if (!git_info_header.is_open()) { + std::cout << "Failed to open '" << header_path << "'" << std::endl; + return -1; + } + + git_info_header << header_contents.str(); + git_info_header.close(); + return 0; }