├── .clang-format ├── .github ├── dependabot.yml └── workflows │ ├── build.yaml │ ├── cancel-superceded-jobs.yaml │ └── clang-format.yaml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── Makefile ├── README.md ├── libs ├── actuators │ ├── CMakeLists.txt │ ├── include │ │ ├── Actuator.h │ │ ├── BrushlessMotor.h │ │ ├── ContServo.h │ │ ├── DCMotor.h │ │ ├── LimServo.h │ │ └── Servo.h │ └── src │ │ ├── BrushlessMotor.cpp │ │ ├── ContServo.cpp │ │ ├── DCMotor.cpp │ │ └── LimServo.cpp ├── can │ ├── CMakeLists.txt │ ├── include │ │ ├── CANBuffer.h │ │ ├── CANBus.h │ │ ├── CANInterface.h │ │ └── CANMsg.h │ └── src │ │ ├── CANBuffer.cpp │ │ ├── CANBus.cpp │ │ └── CANInterface.cpp ├── controllers │ ├── CMakeLists.txt │ ├── include │ │ ├── ActuatorController.h │ │ ├── ActuatorControllerManager.h │ │ ├── Current.h │ │ ├── OpenLoop.h │ │ ├── Position.h │ │ └── Velocity.h │ └── src │ │ ├── ActuatorController.cpp │ │ ├── ActuatorControllerManager.cpp │ │ ├── Current.cpp │ │ ├── OpenLoop.cpp │ │ ├── Position.cpp │ │ └── Velocity.cpp ├── encoder │ ├── CMakeLists.txt │ ├── include │ │ ├── AEAT6012.h │ │ ├── Encoder.h │ │ ├── MAE3.h │ │ ├── Pololu37D.h │ │ └── Quadrature64CPR.h │ └── src │ │ ├── AEAT6012.cpp │ │ ├── MAE3.cpp │ │ ├── Pololu37D.cpp │ │ └── Quadrature64CPR.cpp ├── gamepad │ ├── CMakeLists.txt │ ├── include │ │ ├── AnalogBusIn.h │ │ ├── AnalogInputGroup.h │ │ ├── DigitalInputGroup.h │ │ └── FrameArbiter.h │ └── src │ │ ├── AnalogBusIn.cpp │ │ ├── AnalogInputGroup.cpp │ │ ├── DigitalInputGroup.cpp │ │ └── FrameArbiter.cpp ├── gpio │ ├── CMakeLists.txt │ ├── include │ │ ├── LimitSwitch.h │ │ ├── PwmIn.h │ │ └── QEI.h │ └── src │ │ ├── LimitSwitch.cpp │ │ ├── PwmIn.cpp │ │ └── QEI.cpp ├── led-matrix │ ├── CMakeLists.txt │ ├── include │ │ └── LEDMatrix.h │ └── src │ │ └── LEDMatrix.cpp ├── neopixel │ ├── CMakeLists.txt │ ├── include │ │ └── Neopixel_Blocking.h │ └── src │ │ └── Neopixel_Blocking.cpp ├── pid │ ├── CMakeLists.txt │ ├── include │ │ └── PID.h │ └── src │ │ └── PID.cpp ├── sensors │ ├── CMakeLists.txt │ ├── include │ │ ├── AdafruitSTEMMA.h │ │ ├── CurrentSensor.h │ │ ├── PollingSensors.h │ │ └── Sensor.h │ └── src │ │ ├── AdafruitSTEMMA.cpp │ │ ├── CurrentSensor.cpp │ │ └── PollingSensors.cpp └── utility │ ├── CMakeLists.txt │ ├── include │ ├── Logger.h │ ├── LookupTable.h │ ├── WatchdogWrapper.h │ └── units.h │ └── src │ ├── Logger.cpp │ └── WatchdogWrapper.cpp ├── mbed-os.lib ├── mbed_app.json ├── misc └── NucleoSWDLabels.png ├── rover-apps ├── arm_2021 │ ├── CMakeLists.txt │ ├── include │ │ ├── AppConfig.h │ │ ├── CANConfig.h │ │ ├── ClawConfig.h │ │ ├── ElbowConfig.h │ │ ├── ShoulderConfig.h │ │ ├── TooltipConfig.h │ │ ├── TurntableConfig.h │ │ └── WristConfig.h │ └── src │ │ └── main.cpp ├── common │ ├── CMakeLists.txt │ ├── include │ │ ├── Module.h │ │ └── WatchdogModule.h │ └── src │ │ ├── WatchdogModule.cpp │ │ └── main.cpp ├── gamepad_2021 │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── gimbal_2021 │ ├── CMakeLists.txt │ ├── include │ │ ├── AppConfig.h │ │ ├── CANConfig.h │ │ ├── PanConfig.h │ │ └── PitchConfig.h │ └── src │ │ └── main.cpp ├── pdb_2021 │ ├── CMakeLists.txt │ ├── include │ │ ├── AppConfig.h │ │ └── PDBMonitoring.h │ └── src │ │ ├── PDBMonitoring.cpp │ │ └── main.cpp └── science_2021 │ ├── CMakeLists.txt │ ├── include │ ├── AppConfig.h │ ├── CANConfig.h │ ├── CentrifugeConfig.h │ ├── CoverConfig.h │ ├── DiggerConfig.h │ └── ElevatorConfig.h │ └── src │ └── main.cpp ├── scripts ├── build_configurations_helper.py ├── local-requirements │ └── requirements.txt ├── requirements.txt └── swo_parser.py ├── supported_build_configurations.yaml ├── targets ├── TARGET_ARM_REV2_2021 │ ├── CMakeLists.txt │ ├── include │ │ ├── PinNames.h │ │ └── uwrt_config_target.h │ └── src │ │ ├── PeripheralPins.c │ │ └── system_clock_override.c ├── TARGET_GAMEPAD_REV1_2021 │ ├── CMakeLists.txt │ ├── include │ │ ├── PinNames.h │ │ └── uwrt_config_target.h │ └── src │ │ ├── PeripheralPins.c │ │ └── system_clock_override.c ├── TARGET_GIMBAL_REV2_2021 │ ├── CMakeLists.txt │ ├── include │ │ ├── PinNames.h │ │ └── uwrt_config_target.h │ └── src │ │ ├── PeripheralPins.c │ │ └── system_clock_override.c ├── TARGET_PDB_REV2_2021 │ ├── CMakeLists.txt │ ├── include │ │ ├── PinNames.h │ │ └── uwrt_config_target.h │ └── src │ │ ├── PeripheralPins.c │ │ └── system_clock_override.c ├── TARGET_SCIENCE_REV2_2021 │ ├── CMakeLists.txt │ ├── include │ │ ├── PinNames.h │ │ └── uwrt_config_target.h │ └── src │ │ ├── PeripheralPins.c │ │ └── system_clock_override.c ├── TARGET_UWRT_NUCLEO │ ├── CMakeLists.txt │ └── include │ │ ├── PinNames.h │ │ └── uwrt_config_target.h └── custom_targets.json ├── test-apps ├── test-actuator-controller │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-adafruitSTEMMA │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-aeat-6012 │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-aeat-8800 │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-blinky │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-blockingneo │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-can │ ├── CMakeLists.txt │ ├── include │ │ └── CANConfig.h │ └── src │ │ ├── main.cpp │ │ └── main_backup.cpp ├── test-led-matrix │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-limit-switch │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-logger │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-lookup-table │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-mae3 │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-pid │ ├── CMakeLists.txt │ ├── control_anti_kickback.png │ ├── control_anti_windup.png │ ├── control_anti_windup_anti_kickback.png │ ├── control_basic.png │ ├── include │ │ └── test_data.h │ ├── matlab_control.png │ └── src │ │ └── main.cpp ├── test-pwm │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-pwmin │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-quadrature64cpr │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp ├── test-stress-can │ ├── CMakeLists.txt │ ├── include │ │ ├── CANConfigArm.h │ │ ├── CANConfigGimbal.h │ │ ├── CANConfigPDB.h │ │ └── CANConfigScience.h │ └── src │ │ └── main.cpp ├── test-units │ ├── CMakeLists.txt │ └── src │ │ └── main.cpp └── test-watchdog │ ├── CMakeLists.txt │ └── src │ └── main.cpp └── toolchain.cmake /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | ColumnLimit: 120 4 | 5 | CommentPragmas: '^ NOLINT' 6 | 7 | AllowShortFunctionsOnASingleLine: Empty 8 | 9 | AlignConsecutiveAssignments: true 10 | AlignConsecutiveMacros: true 11 | --- 12 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: "github-actions" 4 | # Workflow files stored in the 5 | # default location of `.github/workflows` 6 | directory: "/" 7 | schedule: 8 | interval: "weekly" 9 | pull-request-branch-name: 10 | separator: "/" 11 | assignees: 12 | - "jahnavithota2011" 13 | reviewers: 14 | - "jahnavithota2011" 15 | 16 | - package-ecosystem: "gitsubmodule" 17 | directory: "/" 18 | schedule: 19 | interval: "weekly" 20 | pull-request-branch-name: 21 | separator: "/" 22 | assignees: 23 | - "jahnavithota2011" 24 | reviewers: 25 | - "jahnavithota2011" 26 | 27 | - package-ecosystem: "pip" 28 | directory: "/scripts/local-requirements" 29 | schedule: 30 | interval: "weekly" 31 | pull-request-branch-name: 32 | separator: "/" 33 | assignees: 34 | - "jahnavithota2011" 35 | reviewers: 36 | - "jahnavithota2011" 37 | -------------------------------------------------------------------------------- /.github/workflows/build.yaml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | on: 4 | push: 5 | pull_request: 6 | schedule: # Trigger a job on master at midnight UTC every day 7 | - cron: '0 0 * * *' 8 | 9 | defaults: 10 | run: 11 | shell: bash 12 | 13 | env: 14 | ARM_TOOLS_URL: https://developer.arm.com/-/media/Files/downloads/gnu-rm/10-2020q4/gcc-arm-none-eabi-10-2020-q4-major-x86_64-linux.tar.bz2 15 | ARM_TOOLS_DIR: gcc-arm-none-eabi-10-2020-q4-major 16 | 17 | jobs: 18 | setup-build-matrix: 19 | runs-on: ubuntu-20.04 20 | outputs: 21 | matrix: ${{ steps.set-matrix.outputs.matrix }} 22 | 23 | steps: 24 | - name: Checkout repository 25 | uses: actions/checkout@v3 26 | 27 | - name: Set Build Matrix Output 28 | id: set-matrix 29 | run: | # Runs script once first to ensure no silent failures 30 | ./scripts/build_configurations_helper.py print-ci-json 31 | echo "::set-output name=matrix::`./scripts/build_configurations_helper.py print-ci-json`" 32 | 33 | build: 34 | needs: setup-build-matrix 35 | runs-on: ubuntu-20.04 36 | strategy: 37 | matrix: ${{fromJson(needs.setup-build-matrix.outputs.matrix)}} 38 | fail-fast: false 39 | 40 | steps: 41 | - name: Checkout repository 42 | uses: actions/checkout@v3 43 | with: 44 | submodules: 'recursive' 45 | 46 | - name: Set Working Directory as Home 47 | run: echo "HOME=$HOME" >> $GITHUB_ENV 48 | 49 | - name: Update Path with Toolchain 50 | run: echo "${{ env.HOME }}/${{ env.ARM_TOOLS_DIR }}/bin" >> $GITHUB_PATH 51 | 52 | - name: Install Python dependencies 53 | uses: py-actions/py-dependency-install@v3 54 | with: 55 | path: "scripts/requirements.txt" 56 | 57 | - name: Load Cached Arm Toolchain 58 | uses: actions/cache@v3 59 | id: toolchain-cache 60 | with: 61 | path: ${{ env.HOME }}/${{ env.ARM_TOOLS_DIR }} 62 | key: ${{ env.ARM_TOOLS_DIR }} 63 | 64 | - name: Install Arm Toolchain (if not cached) 65 | if: steps.toolchain-cache.outputs.cache-hit != 'true' 66 | run: wget $ARM_TOOLS_URL -O /tmp/arm-tools.tar.gz && tar -xvf /tmp/arm-tools.tar.gz -C ${{ env.HOME }} 67 | 68 | - name: Install CMake & Ninja (auto-caches) 69 | uses: lukka/get-cmake@latest 70 | 71 | - name: Print Toolchain Versions 72 | run: | 73 | echo "Make Version:" 74 | make --version 75 | echo 76 | echo "Mbed Tools Version:" 77 | mbed-tools --version 78 | echo 79 | echo "CMake Version:" 80 | cmake --version 81 | echo 82 | echo "Ninja Version:" 83 | ninja --version 84 | echo 85 | echo "ARM GCC Version:" 86 | arm-none-eabi-gcc --version 87 | 88 | - name: Run Make 89 | env: ${{matrix.env}} 90 | run: | 91 | CLEAN=1; make APP=${{ env.APP }} TARGET=${{ env.TARGET }} 92 | 93 | -------------------------------------------------------------------------------- /.github/workflows/cancel-superceded-jobs.yaml: -------------------------------------------------------------------------------- 1 | name: cancel-superceeded-jobs 2 | 3 | on: [ push, pull_request ] 4 | 5 | jobs: 6 | cancel-superceded-jobs: 7 | runs-on: ubuntu-latest 8 | steps: 9 | - name: Cancel Superceded Jobs 10 | uses: styfle/cancel-workflow-action@0.9.1 11 | with: 12 | workflow_id: build.yaml, clang-format.yaml 13 | access_token: ${{ github.token }} 14 | -------------------------------------------------------------------------------- /.github/workflows/clang-format.yaml: -------------------------------------------------------------------------------- 1 | name: clang-format 2 | 3 | on: 4 | push: 5 | pull_request: 6 | schedule: # Trigger a job on master at midnight UTC every day 7 | - cron: '0 0 * * *' 8 | 9 | jobs: 10 | clang-format: 11 | runs-on: ubuntu-20.04 12 | steps: 13 | - name: Checkout repository 14 | uses: actions/checkout@v3 15 | with: 16 | submodules: 'recursive' 17 | 18 | - name: Clang format Check 19 | uses: DoozyX/clang-format-lint-action@master 20 | with: 21 | exclude: './mbed-os' 22 | clangFormatVersion: 11 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Build output 2 | cmake_build/ 3 | 4 | # MacOS pollution 5 | *.DS_store 6 | 7 | # VS Code pollution 8 | firmware.code-workspace 9 | .vscode 10 | 11 | # Clion 12 | *.idea/ 13 | *cmake-build-* 14 | venv 15 | __pycache__ 16 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "mbed-os"] 2 | path = mbed-os 3 | url = https://github.com/uwrobotics/mbed-os.git 4 | branch = uwrt 5 | 6 | [submodule "uwrt-mars-rover-hw-bridge"] 7 | path = uwrt-mars-rover-hw-bridge 8 | url = https://github.com/uwrobotics/uwrt_mars_rover_hw_bridge.git 9 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Default Target 2 | .DEFAULT_GOAL := build 3 | 4 | # Delete Suffix Rules 5 | .SUFFIXES := 6 | 7 | # Phony Targets 8 | .PHONY := build verify_app_target_tuple_config verify_app_target_tuple_is_specified clean all 9 | 10 | # Options 11 | CLEAN ?= 12 | BUILD_PROFILE ?= develop 13 | 14 | MBED_TOOLS_FLAGS := 15 | ifneq (,$(CLEAN)) 16 | MBED_TOOLS_FLAGS += --clean 17 | endif 18 | ifneq (,$(BUILD_PROFILE)) 19 | MBED_TOOLS_FLAGS += --profile $(BUILD_PROFILE) 20 | endif 21 | 22 | # PATHS 23 | MAKEFILE_DIR := $(patsubst %/,%,$(dir $(abspath $(lastword $(MAKEFILE_LIST))))) 24 | ROVER_APPS_DIR := $(abspath $(MAKEFILE_DIR)/rover-apps) 25 | TEST_ROVER_APPS_DIR := $(abspath $(MAKEFILE_DIR)/test-apps) 26 | TARGETS_DIR := $(abspath $(MAKEFILE_DIR)/targets) 27 | 28 | CUSTOM_TARGETS_JSON := $(abspath $(TARGETS_DIR)/custom_targets.json) 29 | 30 | # APP and TARGET lists 31 | APPS_LIST := $(sort $(patsubst $(ROVER_APPS_DIR)/%/,%, $(wildcard $(ROVER_APPS_DIR)/*/))) 32 | APPS_LIST += $(sort $(patsubst $(TEST_ROVER_APPS_DIR)/%/,%, $(wildcard $(TEST_ROVER_APPS_DIR)/*/))) 33 | 34 | TARGETS_LIST := $(sort $(patsubst $(TARGETS_DIR)/TARGET_%/,%, $(wildcard $(TARGETS_DIR)/TARGET_*/))) 35 | 36 | NUMBER_OF_SUPPORTED_CONFIGS := $(shell python3 scripts/build_configurations_helper.py count-supported-configs) 37 | 38 | # Lowercase Command Args 39 | ifneq (,$(app)) 40 | APP = $(app) 41 | endif 42 | ifneq (,$(target)) 43 | TARGET = $(target) 44 | endif 45 | 46 | verify_app_target_tuple_is_specified: 47 | ifeq (,$(findstring $(APP), $(APPS_LIST))) 48 | 49 | ifeq (,$(APP)) 50 | $(warning APP is not set) 51 | else 52 | $(warning $(APP) is not an app within the $(ROVER_APPS_DIR) folder) 53 | endif 54 | 55 | $(warning Using APP=app_name, select one of the following detected apps:) 56 | $(foreach APP_NAME,$(APPS_LIST),$(warning $(APP_NAME))) 57 | $(error ) 58 | endif 59 | 60 | ifeq (,$(findstring $(TARGET), $(TARGETS_LIST))) 61 | 62 | ifeq (,$(TARGET)) 63 | $(warning TARGET is not set) 64 | else 65 | $(warning $(TARGET) is not a target within the $(TARGETS_DIR) folder) 66 | endif 67 | 68 | $(warning Using TARGET=board_name, select one of the following detected board targets:) 69 | $(foreach TARGET_NAME,$(TARGETS_LIST),$(warning $(TARGET_NAME))) 70 | $(error ) 71 | endif 72 | 73 | verify_app_target_tuple_config: verify_app_target_tuple_is_specified 74 | @python3 scripts/build_configurations_helper.py verify-config --APP=$(APP) --TARGET=$(TARGET) ; \ 75 | if [ $$? -ne 0 ]; \ 76 | then \ 77 | echo "Do you want to try to build anyways? (Y/N)"; \ 78 | read RESPONSE; \ 79 | if !([ "$$RESPONSE" = "Y" ] || [ "$$RESPONSE" = "y" ] || [ "$$RESPONSE" = "YES" ] || [ "$$RESPONSE" = "yes" ]); \ 80 | then \ 81 | false ; \ 82 | fi; \ 83 | fi 84 | 85 | build: verify_app_target_tuple_config 86 | @echo Building $(APP) app for $(TARGET) board with $(BUILD_PROFILE) profile 87 | APP=$(APP); mbed-tools compile --toolchain GCC_ARM --mbed-target $(TARGET) --custom-targets-json $(CUSTOM_TARGETS_JSON) $(MBED_TOOLS_FLAGS) 88 | 89 | all: 90 | @echo Building all $(NUMBER_OF_SUPPORTED_CONFIGS) supported app/target configs 91 | @n=0; \ 92 | success=0; \ 93 | while [ $${n} -lt $(NUMBER_OF_SUPPORTED_CONFIGS) ] && [ $${success} -eq 0 ]; do \ 94 | make $$(python3 scripts/build_configurations_helper.py print-supported-config $${n}); \ 95 | success=$${?}; \ 96 | n=`expr $$n + 1`; \ 97 | done; \ 98 | exit $${success}; 99 | @echo Successfully built all $(NUMBER_OF_SUPPORTED_CONFIGS) supported app/target configs 100 | 101 | clean: 102 | @echo "Deleting all build files" 103 | rm -rf cmake_build 104 | 105 | -------------------------------------------------------------------------------- /libs/actuators/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(Actuator INTERFACE) 2 | target_include_directories(Actuator INTERFACE include) 3 | 4 | add_library(DCMotor STATIC) 5 | target_sources(DCMotor PRIVATE src/DCMotor.cpp) 6 | target_include_directories(DCMotor PUBLIC include) 7 | target_link_libraries(DCMotor 8 | PRIVATE 9 | Actuator 10 | mbed-os 11 | ) 12 | 13 | add_library(BrushlessMotor STATIC) 14 | target_sources(BrushlessMotor PRIVATE src/BrushlessMotor.cpp) 15 | target_include_directories(BrushlessMotor PUBLIC include) 16 | target_link_libraries(BrushlessMotor 17 | PRIVATE 18 | Actuator 19 | mbed-os 20 | ) 21 | 22 | add_library(Servo INTERFACE) 23 | target_include_directories(Servo INTERFACE include) 24 | 25 | add_library(ContServo STATIC) 26 | target_sources(ContServo PRIVATE src/ContServo.cpp) 27 | target_include_directories(ContServo PUBLIC include) 28 | target_link_libraries(ContServo 29 | PRIVATE 30 | Actuator 31 | Servo 32 | mbed-os 33 | ) 34 | 35 | add_library(LimServo STATIC) 36 | target_sources(LimServo PRIVATE src/LimServo.cpp) 37 | target_include_directories(LimServo PUBLIC include) 38 | target_link_libraries(LimServo 39 | PRIVATE 40 | Actuator 41 | Servo 42 | mbed-os 43 | ) 44 | -------------------------------------------------------------------------------- /libs/actuators/include/Actuator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | * The Actuator class is an abstracted class that encompasses 5 | * DC motors, brushless motors, limit servos, and continuous servos. 6 | * What "value" represents depends on the actuator type: 7 | * 8 | * Actuator Type Value 9 | * ------------------------------ 10 | * DC Motor Speed 11 | * Brushless Motor Speed 12 | * Limit Servo Position 13 | * Continuous Servo Speed 14 | */ 15 | 16 | namespace Actuator { 17 | class Actuator { 18 | public: 19 | virtual void setValue(float value) = 0; 20 | virtual ~Actuator() {} 21 | }; 22 | } // namespace Actuator 23 | -------------------------------------------------------------------------------- /libs/actuators/include/BrushlessMotor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Actuator.h" 4 | 5 | namespace Actuator { 6 | class BrushlessMotor final : public Actuator { 7 | public: 8 | typedef struct Config { 9 | PinName pwmPin; 10 | float max_speed; 11 | std::chrono::duration max_pulse; 12 | std::chrono::duration min_pulse; 13 | std::chrono::duration period; 14 | } Config; 15 | BrushlessMotor(PinName pin, float max_speed, std::chrono::duration max_pulse = DEFAULT_MAX, 16 | std::chrono::duration min_pulse = DEFAULT_MIN, 17 | std::chrono::duration period = DEFAULT_PERIOD); 18 | 19 | BrushlessMotor(const Config &config); 20 | 21 | /** Read the maximum speed of the motor 22 | * 23 | * @return The maximum speed of the motor 24 | */ 25 | float getMaxSpeed(void) const; 26 | 27 | /** Set the speed of the motor 28 | * 29 | * @param speed The speed of the motor (can be negative) 30 | */ 31 | void setValue(float speed) override; 32 | 33 | /** Read the current speed of the motor 34 | * 35 | * @return Current speed of motor 36 | */ 37 | float getValue() const override; 38 | 39 | // Override default period 40 | void setPeriod(std::chrono::duration period); 41 | 42 | protected: 43 | // TODO: check these default values 44 | static constexpr int PWM_FREQ = 50; // DEFAULT PWM FREQUENCY 45 | static constexpr auto DEFAULT_MAX = 2ms; // DEFAULT MAX WAVE LENGTH 46 | static constexpr auto DEFAULT_MIN = 1ms; // DEFAULT MIN WAVE LENGTH 47 | static constexpr std::chrono::duration DEFAULT_PERIOD = 1.0s / PWM_FREQ; // DEFAULT PERIOD LENGTH 48 | 49 | PwmOut m_pwm; 50 | float m_abs_max_speed, // deg/sec 51 | m_speed; // deg/sec (from -m_abs_max_speed to +m_abs_max_speed) 52 | }; 53 | } // namespace Actuator 54 | -------------------------------------------------------------------------------- /libs/actuators/include/ContServo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Servo.h" 4 | #include "mbed.h" 5 | 6 | namespace Actuator { 7 | class ContServo final : public Servo { 8 | public: 9 | typedef struct Config { 10 | PinName pwmPin; 11 | float max_speed; 12 | std::chrono::duration max_pulse; 13 | std::chrono::duration min_pulse; 14 | std::chrono::duration period; 15 | } Config; 16 | ContServo(PinName pin, float max_speed, std::chrono::duration max_pulse = DEFAULT_MAX, 17 | std::chrono::duration min_pulse = DEFAULT_MIN, std::chrono::duration period = DEFAULT_PERIOD); 18 | 19 | ContServo(const Config &config); 20 | 21 | /** Read the maximum speed of the servo 22 | * 23 | * @return The maximum speed of the servo 24 | */ 25 | float getMaxSpeed(void) const; 26 | 27 | /** Set the speed of the servo 28 | * 29 | * @param speed The speed of the servo (can be negative) 30 | */ 31 | void setValue(float speed) override; 32 | 33 | private: 34 | PwmOut m_pwm; 35 | float m_abs_max_speed; // deg/sec 36 | }; 37 | } // namespace Actuator 38 | -------------------------------------------------------------------------------- /libs/actuators/include/DCMotor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Actuator.h" 4 | #include "mbed.h" 5 | 6 | namespace Actuator { 7 | class DCMotor final : public Actuator { 8 | public: 9 | typedef struct Config { 10 | PinName pwmPin; 11 | PinName dirPin; 12 | bool inverted; 13 | int freqInHz; 14 | float limit; 15 | } Config; 16 | /** Create a motor control interface 17 | * 18 | * @param pwm A PwmOut pin, driving the H-bridge enable line to control the speed 19 | * @param dir A DigitalOut, set high when the motor should go forward, low when backwards 20 | * @param freqInHz Output PWM frequency, default 20kHz 21 | * @param inverted If true, then forward speed will set dir to 0 instead of 1, otherwise inverse 22 | * @param limit Maximum speed magnitude 23 | */ 24 | DCMotor(PinName pwm, PinName dir, bool inverted = false, int freqInHz = MOTOR_DEFAULT_FREQUENCY_HZ, 25 | float limit = 1.0); 26 | 27 | DCMotor(const Config &config); 28 | 29 | /** Set the speed of the motor 30 | * 31 | * @param dutyCycle The speed of the motor as a normalised value between -1.0 and 1.0 32 | */ 33 | void setValue(float dutyCycle) override; 34 | 35 | private: 36 | static constexpr int MOTOR_DEFAULT_FREQUENCY_HZ = 20000; // TODO: Need to test if 20kHz works 37 | 38 | PwmOut m_pwm; 39 | DigitalOut m_dir; 40 | bool m_inverted; 41 | float m_limit; 42 | }; 43 | } // namespace Actuator 44 | -------------------------------------------------------------------------------- /libs/actuators/include/LimServo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Servo.h" 4 | #include "mbed.h" 5 | 6 | namespace Actuator { 7 | class LimServo final : public Servo { 8 | public: 9 | typedef struct Config { 10 | PinName pwmPin; 11 | float range; 12 | std::chrono::duration max_pulse; 13 | std::chrono::duration min_pulse; 14 | std::chrono::duration period; 15 | } Config; 16 | LimServo(PinName pin, float range, std::chrono::duration max_pulse = DEFAULT_MAX, 17 | std::chrono::duration min_pulse = DEFAULT_MIN, std::chrono::duration period = DEFAULT_PERIOD); 18 | 19 | LimServo(const Config &config); 20 | 21 | /** Read the range of the servo 22 | * 23 | * @return The range of the servo in degrees 24 | */ 25 | float getRange(void) const; 26 | 27 | /** Set the position of the servo 28 | * 29 | * @param position The position of the servo in degrees 30 | */ 31 | void setValue(float position) override; 32 | 33 | private: 34 | static constexpr int DEFAULT_RANGE = 180; // degrees 35 | 36 | PwmOut m_pwm; 37 | float m_abs_range, // degrees 38 | m_pos; // degrees (from -m_abs_range to +m_abs_range) 39 | }; 40 | } // namespace Actuator 41 | -------------------------------------------------------------------------------- /libs/actuators/include/Servo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Actuator.h" 6 | namespace Actuator { 7 | using namespace std::chrono_literals; 8 | 9 | class Servo : public Actuator { 10 | protected: 11 | static constexpr int PWM_FREQ = 50; // DEFAULT PWM FREQUENCY, should work for both length and positional control 12 | static constexpr auto DEFAULT_MAX = 2ms; // DEFAULT MAX WAVE LENGTH 13 | static constexpr auto DEFAULT_MIN = 1ms; // DEFAULT MIN WAVE LENGTH 14 | static constexpr std::chrono::duration DEFAULT_PERIOD = 1.0s / PWM_FREQ; // DEFAULT PERIOD LENGTH 15 | 16 | std::chrono::duration m_max_pulse, // PULSE LENGTH for MAX ANGLE/SPEED 17 | m_min_pulse; // PULSE LENGTH for MIN ANGLE/SPEED 18 | }; 19 | } // namespace Actuator 20 | -------------------------------------------------------------------------------- /libs/actuators/src/BrushlessMotor.cpp: -------------------------------------------------------------------------------- 1 | // TODO -------------------------------------------------------------------------------- /libs/actuators/src/ContServo.cpp: -------------------------------------------------------------------------------- 1 | #include "ContServo.h" 2 | 3 | using namespace Actuator; 4 | 5 | ContServo::ContServo::ContServo(PinName pin, float max_speed, std::chrono::duration max_pulse, 6 | std::chrono::duration min_pulse, std::chrono::duration period) 7 | : m_pwm(pin), m_abs_max_speed(std::abs(max_speed)) { 8 | // Set initial condition of PWM 9 | m_pwm.period(period.count()); 10 | m_pwm = 0.0; 11 | 12 | // Set max and min pulse widths 13 | m_max_pulse = max_pulse; 14 | m_min_pulse = min_pulse; 15 | } 16 | 17 | ContServo::ContServo::ContServo(const Config &config) 18 | : ContServo(config.pwmPin, config.max_speed, config.max_pulse, config.min_pulse, config.period) {} 19 | 20 | float ContServo::ContServo::getMaxSpeed(void) const { 21 | return m_abs_max_speed; 22 | } 23 | 24 | void ContServo::ContServo::setValue(float speed) { 25 | // So now speed is from 0 to 2 * m_abs_max_speed 26 | speed += m_abs_max_speed; 27 | m_pwm.pulsewidth(((m_max_pulse - m_min_pulse) * speed / (m_abs_max_speed * 2) + m_min_pulse).count()); 28 | } 29 | -------------------------------------------------------------------------------- /libs/actuators/src/DCMotor.cpp: -------------------------------------------------------------------------------- 1 | #include "DCMotor.h" 2 | 3 | #include 4 | 5 | using namespace Actuator; 6 | 7 | DCMotor::DCMotor(PinName pwm, PinName dir, bool inverted, int freqInHz, float limit) 8 | : m_pwm(pwm), m_dir(dir), m_inverted(inverted), m_limit(fmin(limit, 1.0)) { 9 | // Set initial condition of PWM 10 | m_pwm.period(1.0 / freqInHz); 11 | m_pwm = 0.0; 12 | 13 | // Initial condition of output enables 14 | m_dir = 0; 15 | } 16 | 17 | DCMotor::DCMotor(const Config &config) 18 | : DCMotor(config.pwmPin, config.dirPin, config.inverted, config.freqInHz, config.limit) {} 19 | 20 | void DCMotor::setValue(float dutyCycle) { 21 | m_dir = ((dutyCycle > 0.0) != m_inverted); 22 | m_pwm = fmin(fabs(dutyCycle), m_limit); 23 | } 24 | -------------------------------------------------------------------------------- /libs/actuators/src/LimServo.cpp: -------------------------------------------------------------------------------- 1 | #include "LimServo.h" 2 | 3 | using namespace Actuator; 4 | 5 | LimServo::LimServo(PinName pin, float range, std::chrono::duration max_pulse, 6 | std::chrono::duration min_pulse, std::chrono::duration period) 7 | : m_pwm(pin), m_abs_range(std::abs(range)), m_pos(0) { 8 | // Set initial condition of PWM 9 | m_pwm.period(period.count()); 10 | m_pwm = 0.0; 11 | 12 | // Set max and min pulse widths 13 | m_max_pulse = max_pulse; 14 | m_min_pulse = min_pulse; 15 | } 16 | 17 | LimServo::LimServo(const Config &config) 18 | : LimServo(config.pwmPin, config.range, config.max_pulse, config.min_pulse, config.period) {} 19 | 20 | float LimServo::getRange(void) const { 21 | return m_abs_range; 22 | } 23 | 24 | void LimServo::setValue(float position) { 25 | int sign = (position >= 0) ? 1 : -1; 26 | m_pos = (std::abs(position) < m_abs_range) ? position : m_abs_range * sign; 27 | // position = -1 * m_abs_range -> pwm m_min_pulse_ms; position = +1 * m_abs_range -> pwm m_max_pulse_ms 28 | // now, if we add m_abs_range to position, position = 0 -> pwm m_min_pulse_ms; position = 2 * m_abs_range -> pwm 29 | // m_max_pulse_ms now we have position as a value from 0 to 2 * m_abs_range, which can be linearly mapped to 30 | // m_min_pulse_ms to m_max_pulse_ms, with ease 31 | position += m_abs_range; 32 | m_pwm.pulsewidth(((m_max_pulse - m_min_pulse) * position / (m_abs_range * 2) + m_min_pulse).count()); 33 | } 34 | -------------------------------------------------------------------------------- /libs/can/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(CANMsg INTERFACE) 2 | target_include_directories(CANMsg INTERFACE include) 3 | target_link_libraries(CANMsg 4 | INTERFACE 5 | uwrt-mars-rover-hw-bridge 6 | LookupTable 7 | mbed-os 8 | ) 9 | 10 | add_library(CANBuffer STATIC) 11 | target_sources(CANBuffer PRIVATE src/CANBuffer.cpp) 12 | target_include_directories(CANBuffer PUBLIC include) 13 | target_link_libraries(CANBuffer 14 | PRIVATE 15 | uwrt-mars-rover-hw-bridge 16 | LookupTable 17 | mbed-os 18 | ) 19 | 20 | add_library(CANBus STATIC) 21 | target_sources(CANBus PRIVATE src/CANBus.cpp) 22 | target_include_directories(CANBus PUBLIC include) 23 | target_link_libraries(CANBus 24 | PRIVATE 25 | uwrt-mars-rover-hw-bridge 26 | mbed-os 27 | ) 28 | 29 | add_library(CANInterface STATIC) 30 | target_sources(CANInterface PRIVATE src/CANInterface.cpp) 31 | target_include_directories(CANInterface PUBLIC include) 32 | target_link_libraries(CANInterface 33 | PUBLIC 34 | mbed-os 35 | mbed-events 36 | PRIVATE 37 | uwrt-mars-rover-hw-bridge 38 | CANBus 39 | CANMsg 40 | ) 41 | -------------------------------------------------------------------------------- /libs/can/include/CANBuffer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "CANMsg.h" 6 | #include "CircularBuffer.h" 7 | 8 | constexpr uint8_t CANBUFFER_SIZE = 8; 9 | 10 | constexpr uint8_t CANBUFFER_FLAG_DATA_READY{1UL << 0}; 11 | constexpr uint8_t CANBUFFER_FLAG_FULL{1UL << 1}; 12 | 13 | class CANBuffer : public CircularBuffer { 14 | public: 15 | enum BufferType { rx, tx }; 16 | 17 | CANBuffer(CAN &CANInterface, BufferType type = rx); 18 | 19 | /** Pop the transaction from the buffer 20 | * 21 | * @param data Data to be popped from the buffer 22 | * @return True if the buffer is not empty and data contains a transaction, false otherwise 23 | */ 24 | bool pop(CANMsg &canMSG); 25 | 26 | uint32_t getFlags(); 27 | 28 | uint32_t waitFlagsAny(uint32_t flags = 0, uint32_t millisec = osWaitForever, bool clear = true); 29 | 30 | uint32_t waitFlagsAll(uint32_t flags = 0, uint32_t millisec = osWaitForever, bool clear = true); 31 | 32 | private: 33 | CAN &r_CANInterface; 34 | CANMsg m_CANMsg; 35 | 36 | EventFlags m_eventFlags; 37 | 38 | void rxIrqHandler(); 39 | 40 | // Remove push from public scope 41 | using CircularBuffer::push; 42 | }; 43 | -------------------------------------------------------------------------------- /libs/can/include/CANBus.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "hw_bridge.h" 4 | #include "mbed.h" 5 | 6 | class CANBus : public CAN { 7 | public: 8 | /* Initialize CAN interface pins 9 | */ 10 | CANBus(PinName rd, PinName td); 11 | /* Initialize CAN interface pins and frequency 12 | */ 13 | CANBus(PinName rd, PinName td, uint32_t hz); 14 | /* Filter out incoming CAN msgs 15 | */ 16 | int setFilter(HWBRIDGE::CANFILTER filter, CANFormat format = CANAny, 17 | uint16_t mask = HWBRIDGE::ROVER_CANID_FILTER_MASK, int handle = 0); 18 | 19 | can_t *getHandle(); 20 | 21 | private: 22 | // remove public access to CAN::filter 23 | using CAN::filter; 24 | }; 25 | -------------------------------------------------------------------------------- /libs/can/include/CANInterface.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CANBus.h" 4 | #include "CANMsg.h" 5 | #include "hw_bridge.h" 6 | #include "mbed.h" 7 | 8 | class CANInterface { 9 | public: 10 | typedef struct { 11 | // CAN bus pins 12 | PinName can1_RX; 13 | PinName can1_TX; 14 | PinName can2_RX; 15 | PinName can2_TX; 16 | 17 | // Message maps and handlers 18 | HWBRIDGE::CANMsgMap *rxMsgMap; 19 | HWBRIDGE::CANMsgMap *txMsgMap; 20 | const CANMsg::CANMsgHandlerMap *rxOneShotMsgHandler; 21 | 22 | // Bus frequency 23 | uint32_t frequency_hz = HWBRIDGE::ROVER_CANBUS_FREQUENCY_HZ; 24 | } Config; 25 | 26 | // Initialize CAN interface 27 | CANInterface(const Config &config); 28 | 29 | // Queue up a one shot message to be sent 30 | bool sendOneShotMessage(CANMsg &msg, Kernel::Clock::duration_u32 timeout); 31 | 32 | // Update a TX CAN signal 33 | bool setTXSignalValue(HWBRIDGE::CANID msgID, HWBRIDGE::CANSIGNAL signalName, HWBRIDGE::CANSignalValue_t signalValue); 34 | 35 | // Read a RX CAN signal 36 | bool getRXSignalValue(HWBRIDGE::CANID msgID, HWBRIDGE::CANSIGNAL signalName, HWBRIDGE::CANSignalValue_t &signalValue); 37 | 38 | // Switch CAN bus 39 | bool switchCANBus(HWBRIDGE::CANBUSID canBusID); 40 | 41 | // Set CAN bus hw filter 42 | bool setFilter(HWBRIDGE::CANFILTER filter, CANFormat format = CANStandard, 43 | uint16_t mask = HWBRIDGE::ROVER_CANID_FILTER_MASK, int handle = 0); 44 | 45 | // For diagnostic purposes 46 | uint32_t getNumStreamedMsgsReceived(void); 47 | uint32_t getNumOneShotMsgsReceived(void); 48 | uint32_t getNumStreamedMsgsSent(void); 49 | uint32_t getNumOneShotMsgsSent(void); 50 | 51 | uint16_t getNumCANRXFaults(void); 52 | uint16_t getNumCANTXFaults(void); 53 | 54 | private: 55 | static constexpr osPriority RX_POSTMAN_THREAD_PRIORITY = osPriorityRealtime; 56 | static constexpr osPriority RX_CLIENT_THREAD_PRIORITY = osPriorityAboveNormal; 57 | static constexpr osPriority TX_PROCESSOR_THREAD_PRIORITY = osPriorityBelowNormal; 58 | static constexpr std::chrono::milliseconds TX_INTERDELAY = 1ms; 59 | static constexpr std::chrono::milliseconds TX_PERIOD = 10ms; 60 | 61 | void rxISR(void); 62 | void rxPostman(void); 63 | void rxClient(void); 64 | void txProcessor(void); 65 | 66 | CANBus m_CANBus1; 67 | CANBus m_CANBus2; 68 | CANBus *m_activeCANBus; 69 | 70 | Thread m_rxPostmanThread; 71 | Thread m_rxClientThread; 72 | Thread m_txProcessorThread; 73 | 74 | Mutex m_rxMutex; 75 | Mutex m_txMutex; 76 | 77 | Mail m_rxMailbox; 78 | Mail m_txMailboxOneShot; 79 | EventQueue m_rxEventQueue; 80 | 81 | HWBRIDGE::CANMsgMap *m_rxMsgMap; 82 | HWBRIDGE::CANMsgMap *m_txMsgMap; 83 | 84 | const CANMsg::CANMsgHandlerMap *m_rxOneShotMsgHandler; 85 | 86 | // For diagnostic purposes 87 | uint32_t m_numStreamedMsgsReceived; 88 | uint32_t m_numOneShotMsgsReceived; 89 | uint32_t m_numStreamedMsgsSent; 90 | uint32_t m_numOneShotMsgsSent; 91 | 92 | uint16_t m_numCANRXFaults; 93 | uint16_t m_numCANTXFaults; 94 | }; 95 | -------------------------------------------------------------------------------- /libs/can/include/CANMsg.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "LookupTable.h" 4 | #include "hw_bridge.h" 5 | #include "mbed.h" 6 | 7 | class CANMsg : public CANMessage { 8 | private: 9 | using CAN_Message::id; 10 | 11 | static mbed_error_status_t defaultCANMsgHandler(void) { 12 | MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_APPLICATION, MBED_ERROR_CODE_INVALID_ARGUMENT), 13 | "Invalid key to CANMsgHandlerMap"); 14 | return MBED_ERROR_CODE_INVALID_ARGUMENT; 15 | } 16 | 17 | public: 18 | using CANMsgHandler = mbed_error_status_t (*)(void); 19 | using CANMsgHandlerMap = Utility::LookupTable; 20 | 21 | template 22 | union CANPayload { 23 | unsigned char data[8]; 24 | T value; 25 | }; 26 | 27 | /** Creates empty CAN message. 28 | */ 29 | CANMsg() : CANMessage() {} 30 | 31 | /** Creates CAN message with specific content. 32 | */ 33 | CANMsg(HWBRIDGE::CANID _id, const char *_data, char _len = 8, CANType _type = CANData, 34 | CANFormat _format = CANStandard) 35 | : CANMessage(static_cast(_id), _data, _len, _type, _format) {} 36 | 37 | /** Creates CAN remote message. 38 | */ 39 | CANMsg(HWBRIDGE::CANID _id, CANFormat _format = CANStandard) : CANMessage(static_cast(_id), _format) {} 40 | 41 | /** Sets the ID for a CAN messages 42 | */ 43 | void setID(const HWBRIDGE::CANID newID) { 44 | this->id = static_cast(newID); 45 | } 46 | 47 | /** Returns the ID of the CAN message 48 | */ 49 | HWBRIDGE::CANID getID() const { 50 | return static_cast(this->id); 51 | } 52 | 53 | /** Clears CAN message content 54 | */ 55 | void clear(void) { 56 | this->len = 0; 57 | this->type = CANData; 58 | this->format = CANStandard; 59 | this->id = 0; 60 | memset(this->data, 0, 8); 61 | }; 62 | 63 | /** Set the payload data 64 | */ 65 | template 66 | void setPayload(const T value) { 67 | MBED_ASSERT(sizeof(T) <= 8); // Make sure input type fits 68 | CANPayload *payload = (CANPayload *)&data; 69 | payload->value = value; 70 | this->len = sizeof(T); 71 | } 72 | 73 | /** Set the payload data with custom length 74 | */ 75 | template 76 | void setPayload(const T value, size_t length) { 77 | setPayload(value); 78 | len = length; 79 | } 80 | 81 | /** Get the payload data 82 | */ 83 | template 84 | void getPayload(T &val) { 85 | MBED_ASSERT(this->len = sizeof(T)); // Make sure output fits 86 | CANPayload *payload = (CANPayload *)&data; 87 | val = payload->value; 88 | } 89 | }; 90 | -------------------------------------------------------------------------------- /libs/can/src/CANBuffer.cpp: -------------------------------------------------------------------------------- 1 | #include "CANBuffer.h" 2 | 3 | CANBuffer::CANBuffer(CAN &CANInterface, BufferType type) : r_CANInterface(CANInterface) { 4 | if (type == rx) { 5 | r_CANInterface.attach(callback(this, &CANBuffer::rxIrqHandler), CAN::RxIrq); 6 | } 7 | } 8 | 9 | bool CANBuffer::pop(CANMsg &canMSG) { 10 | core_util_critical_section_enter(); 11 | 12 | bool data_popped = CircularBuffer::pop(canMSG); 13 | 14 | if (empty()) { 15 | m_eventFlags.clear(CANBUFFER_FLAG_DATA_READY | CANBUFFER_FLAG_FULL); 16 | } 17 | 18 | core_util_critical_section_exit(); 19 | 20 | return data_popped; 21 | } 22 | 23 | void CANBuffer::rxIrqHandler(void) { 24 | if (r_CANInterface.read(m_CANMsg)) { 25 | core_util_critical_section_enter(); 26 | 27 | m_eventFlags.set(CANBUFFER_FLAG_DATA_READY); 28 | push(m_CANMsg); 29 | 30 | if (full()) { 31 | m_eventFlags.set(CANBUFFER_FLAG_FULL); 32 | } 33 | 34 | core_util_critical_section_exit(); 35 | } 36 | } 37 | 38 | uint32_t CANBuffer::getFlags() { 39 | return m_eventFlags.get(); 40 | } 41 | 42 | uint32_t CANBuffer::waitFlagsAny(uint32_t flags, uint32_t millisec, bool clear) { 43 | return m_eventFlags.wait_all(flags, millisec, clear); 44 | } 45 | 46 | uint32_t CANBuffer::waitFlagsAll(uint32_t flags, uint32_t millisec, bool clear) { 47 | return m_eventFlags.wait_any(flags, millisec, clear); 48 | } 49 | -------------------------------------------------------------------------------- /libs/can/src/CANBus.cpp: -------------------------------------------------------------------------------- 1 | #include "CANBus.h" 2 | 3 | CANBus::CANBus(PinName rd, PinName td) : CAN(rd, td) {} 4 | 5 | CANBus::CANBus(PinName rd, PinName td, uint32_t hz) : CAN(rd, td, hz) {} 6 | 7 | int CANBus::setFilter(HWBRIDGE::CANFILTER canFilter, CANFormat format, uint16_t mask, int handle) { 8 | return CAN::filter(static_cast(canFilter), mask, format, handle); 9 | } 10 | 11 | can_t *CANBus::getHandle() { 12 | return &_can; 13 | } 14 | -------------------------------------------------------------------------------- /libs/controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(ActuatorControllerManager STATIC) 2 | target_sources(ActuatorControllerManager PRIVATE src/ActuatorControllerManager.cpp) 3 | target_include_directories(ActuatorControllerManager PUBLIC include) 4 | target_link_libraries(ActuatorControllerManager 5 | PUBLIC 6 | ActuatorController 7 | PRIVATE 8 | LookupTable 9 | uwrt-mars-rover-hw-bridge 10 | mbed-os 11 | ) 12 | 13 | add_library(ActuatorController STATIC) 14 | target_sources(ActuatorController PRIVATE src/ActuatorController.cpp) 15 | target_include_directories(ActuatorController PUBLIC include) 16 | target_link_libraries(ActuatorController 17 | PUBLIC 18 | Actuator 19 | CurrentSensor 20 | Encoder 21 | PID 22 | PRIVATE 23 | mbed-os 24 | ) 25 | 26 | add_library(PositionController STATIC) 27 | target_sources(PositionController PRIVATE src/Position.cpp) 28 | target_include_directories(PositionController PUBLIC include) 29 | target_link_libraries(PositionController 30 | PUBLIC 31 | ActuatorController 32 | PRIVATE 33 | mbed-os 34 | ) 35 | 36 | add_library(VelocityController STATIC) 37 | target_sources(VelocityController PRIVATE src/Velocity.cpp) 38 | target_include_directories(VelocityController PUBLIC include) 39 | target_link_libraries(VelocityController 40 | PRIVATE 41 | ActuatorController 42 | mbed-os 43 | ) 44 | 45 | add_library(CurrentController STATIC) 46 | target_sources(CurrentController PRIVATE src/Current.cpp) 47 | target_include_directories(CurrentController PUBLIC include) 48 | target_link_libraries(CurrentController 49 | PRIVATE 50 | ActuatorController 51 | mbed-os 52 | ) 53 | 54 | add_library(OpenLoopController STATIC) 55 | target_sources(OpenLoopController PRIVATE src/OpenLoop.cpp) 56 | target_include_directories(OpenLoopController PUBLIC include) 57 | target_link_libraries(OpenLoopController 58 | PRIVATE 59 | Actuator 60 | ActuatorController 61 | CurrentSensor 62 | Encoder 63 | mbed-os 64 | ) 65 | -------------------------------------------------------------------------------- /libs/controllers/include/ActuatorController.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include "Actuator.h" 7 | #include "ActuatorController.h" 8 | #include "CurrentSensor.h" 9 | #include "Encoder.h" 10 | #include "PID.h" 11 | 12 | namespace Controller { 13 | class ActuatorController { 14 | public: 15 | // TODO: Enable current sensor checks once Current Sensor Driver exists 16 | ActuatorController(Actuator::Actuator &actuator, Encoder::Encoder &encoder, 17 | const std::optional const> ¤tSensor, 18 | float maxDegPerSec, float maxCurrent, PinName lowerLimit, PinName upperLimit, 19 | bool ignoreDegPerSecChecks = false, bool ignoreCurrentChecks = true, 20 | bool ignoreLimitSwitchChecks = false); 21 | virtual ~ActuatorController() = default; 22 | 23 | /* final functions cannot be overriden by children */ 24 | virtual void setSetPoint(float sp) final; 25 | virtual float getSetPoint() const final; 26 | 27 | virtual void activateCurrentChecks() final; 28 | virtual void deactivateCurrentChecks() final; 29 | 30 | virtual void activateDegPerSecChecks() final; 31 | virtual void deactivateDegPerSecChecks() final; 32 | 33 | virtual void activateLimitSwitchChecks() final; 34 | virtual void deactivateLimitSwitchChecks() final; 35 | 36 | virtual float reportAngleDeg() final; 37 | virtual float reportAngularVelocityDegPerSec() final; 38 | 39 | /* functions to be overriden by children */ 40 | /* TODO: Add finer error reporting (encoder read fail vs current sensor read fail, etc) */ 41 | virtual bool update() = 0; 42 | virtual bool reset() = 0; 43 | virtual std::optional> getPID() = 0; 44 | virtual void stop() = 0; 45 | 46 | protected: 47 | virtual bool shouldStop() final; 48 | 49 | std::atomic m_setpoint = {0}; 50 | 51 | std::atomic m_ignoreDegPerSecChecks; 52 | std::atomic m_ignoreCurrentChecks; 53 | std::atomic m_ignoreLimitSwitchChecks; 54 | 55 | Actuator::Actuator &m_actuator; 56 | Encoder::Encoder &m_encoder; 57 | const std::optional const> &m_currentSensor; 58 | 59 | const float m_maxDegPerSec, m_maxCurrent; 60 | 61 | DigitalIn m_lowerLimit, m_upperLimit; 62 | }; 63 | } // namespace Controller 64 | -------------------------------------------------------------------------------- /libs/controllers/include/ActuatorControllerManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "ActuatorController.h" 6 | #include "LookupTable.h" 7 | #include "hw_bridge.h" 8 | 9 | namespace Controller { 10 | using ControlMap = Utility::LookupTable; 11 | class ActuatorControllerManager { 12 | public: 13 | ActuatorControllerManager(const ControlMap &lut, HWBRIDGE::CONTROL::Mode active); 14 | ActuatorControllerManager() = delete; 15 | ActuatorControllerManager(ActuatorControllerManager &) = delete; 16 | ActuatorControllerManager(ActuatorControllerManager &&) = delete; 17 | 18 | bool switchControlMode(HWBRIDGE::CONTROL::Mode newControlType); 19 | HWBRIDGE::CONTROL::Mode getActiveControlMode() const; 20 | ActuatorController *getActiveController() const; 21 | 22 | private: 23 | const Utility::LookupTable m_lut; 24 | 25 | std::atomic m_mode; 26 | std::atomic m_controller; 27 | }; 28 | } // namespace Controller -------------------------------------------------------------------------------- /libs/controllers/include/Current.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ActuatorController.h" 4 | namespace Controller { 5 | 6 | class Current final : public ActuatorController { 7 | public: 8 | Current(Actuator::Actuator &actuator, Encoder::Encoder &encoder, Sensor::CurrentSensor ¤tSensor, PID::PID &pid, 9 | float maxDegPerSec, float maxCurrent, PinName lowerLimit, PinName upperLimit, 10 | bool ignoreDegPerSecChecks = false, bool ignoreCurrentChecks = true, bool ignoreLimitSwitchChecks = false); 11 | bool update() override; 12 | void stop() override; 13 | bool reset() override; 14 | std::optional> getPID() override; 15 | 16 | private: 17 | PID::PID &m_pid; 18 | }; 19 | } // namespace Controller 20 | -------------------------------------------------------------------------------- /libs/controllers/include/OpenLoop.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ActuatorController.h" 4 | 5 | namespace Controller { 6 | class OpenLoop final : public ActuatorController { 7 | public: 8 | using ActuatorController::ActuatorController; 9 | 10 | void stop() override; 11 | bool reset() override; 12 | bool update() override; 13 | 14 | std::optional> getPID() override; 15 | }; 16 | } // namespace Controller 17 | -------------------------------------------------------------------------------- /libs/controllers/include/Position.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ActuatorController.h" 4 | 5 | namespace Controller { 6 | 7 | class Position final : public ActuatorController { 8 | public: 9 | Position(Actuator::Actuator &actuator, Encoder::Encoder &encoder, 10 | const std::optional const> ¤tSensor, PID::PID &pid, 11 | float maxDegPerSec, float maxCurrent, PinName lowerLimit, PinName upperLimit, 12 | bool ignoreDegPerSecChecks = false, bool ignoreCurrentChecks = true, bool ignoreLimitSwitchChecks = false); 13 | void stop() override; 14 | bool update() override; 15 | bool reset() override; 16 | std::optional> getPID() override; 17 | 18 | private: 19 | PID::PID &m_pid; 20 | }; 21 | } // namespace Controller 22 | -------------------------------------------------------------------------------- /libs/controllers/include/Velocity.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ActuatorController.h" 4 | 5 | namespace Controller { 6 | 7 | class Velocity final : public ActuatorController { 8 | public: 9 | Velocity(Actuator::Actuator &actuator, Encoder::Encoder &encoder, 10 | const std::optional const> ¤tSensor, PID::PID &pid, 11 | float maxDegPerSec, float maxCurrent, PinName lowerLimit, PinName upperLimit, 12 | bool ignoreDegPerSecChecks = false, bool ignoreCurrentChecks = true, bool ignoreLimitSwitchChecks = false); 13 | void stop() override; 14 | bool update() override; 15 | bool reset() override; 16 | std::optional> getPID() override; 17 | 18 | private: 19 | PID::PID &m_pid; 20 | }; 21 | } // namespace Controller 22 | -------------------------------------------------------------------------------- /libs/controllers/src/ActuatorController.cpp: -------------------------------------------------------------------------------- 1 | #include "ActuatorController.h" 2 | 3 | using namespace Controller; 4 | 5 | ActuatorController::ActuatorController( 6 | Actuator::Actuator &actuator, Encoder::Encoder &encoder, 7 | const std::optional const> ¤tSensor, float maxDegPerSec, 8 | float maxCurrent, PinName lowerLimit, PinName upperLimit, bool ignoreDegPerSecChecks, bool ignoreCurrentChecks, 9 | bool ignoreLimitSwitchChecks) 10 | : m_ignoreDegPerSecChecks(ignoreDegPerSecChecks), 11 | m_ignoreCurrentChecks(ignoreCurrentChecks), 12 | m_ignoreLimitSwitchChecks(ignoreLimitSwitchChecks), 13 | m_actuator(actuator), 14 | m_encoder(encoder), 15 | m_currentSensor(currentSensor), 16 | m_maxDegPerSec(maxDegPerSec), 17 | m_maxCurrent(maxCurrent), 18 | m_lowerLimit(lowerLimit), 19 | m_upperLimit(upperLimit) {} 20 | 21 | void ActuatorController::setSetPoint(float sp) { 22 | m_setpoint.store(sp); 23 | } 24 | 25 | float ActuatorController::getSetPoint() const { 26 | return m_setpoint.load(); 27 | } 28 | 29 | void ActuatorController::deactivateCurrentChecks() { 30 | m_ignoreCurrentChecks.store(true); 31 | } 32 | 33 | void ActuatorController::activateCurrentChecks() { 34 | m_ignoreCurrentChecks.store(false); 35 | } 36 | 37 | void ActuatorController::deactivateDegPerSecChecks() { 38 | m_ignoreDegPerSecChecks.store(true); 39 | } 40 | 41 | void ActuatorController::activateDegPerSecChecks() { 42 | m_ignoreDegPerSecChecks.store(false); 43 | } 44 | 45 | void ActuatorController::deactivateLimitSwitchChecks() { 46 | m_ignoreLimitSwitchChecks.store(true); 47 | } 48 | 49 | void ActuatorController::activateLimitSwitchChecks() { 50 | m_ignoreLimitSwitchChecks.store(false); 51 | } 52 | 53 | float ActuatorController::reportAngleDeg() { 54 | return m_encoder.getAngleDeg(); 55 | } 56 | 57 | float ActuatorController::reportAngularVelocityDegPerSec() { 58 | return m_encoder.getAngularVelocityDegPerSec(); 59 | } 60 | 61 | bool ActuatorController::shouldStop() { 62 | // this takes advantage of short-circuiting for faster evaluation 63 | bool shouldStop = 64 | !m_ignoreDegPerSecChecks.load() && std::abs(m_encoder.getAngularVelocityDegPerSec()) > m_maxDegPerSec; 65 | shouldStop = shouldStop || (!m_ignoreCurrentChecks.load() && m_currentSensor.has_value() && 66 | std::abs(m_currentSensor.value().get().read()) > m_maxCurrent); 67 | shouldStop = shouldStop || (!m_ignoreLimitSwitchChecks.load() && m_upperLimit.is_connected() && m_upperLimit.read() && 68 | m_setpoint.load() > 0); 69 | shouldStop = shouldStop || (!m_ignoreLimitSwitchChecks.load() && m_lowerLimit.is_connected() && m_lowerLimit.read() && 70 | m_setpoint.load() < 0); 71 | 72 | return shouldStop; 73 | } 74 | -------------------------------------------------------------------------------- /libs/controllers/src/ActuatorControllerManager.cpp: -------------------------------------------------------------------------------- 1 | #include "ActuatorControllerManager.h" 2 | 3 | using namespace Controller; 4 | 5 | ActuatorControllerManager::ActuatorControllerManager(const ControlMap &lut, HWBRIDGE::CONTROL::Mode active) 6 | : m_lut(lut), m_mode(active), m_controller(nullptr) { 7 | MBED_ASSERT(m_lut.find(active) != m_lut.end()); 8 | m_controller = m_lut.at(m_mode).value(); 9 | } 10 | 11 | bool ActuatorControllerManager::switchControlMode(HWBRIDGE::CONTROL::Mode newControlType) { 12 | m_lut.at(getActiveControlMode()).value()->stop(); 13 | if (auto temp = m_lut.at(newControlType)) { 14 | m_mode.store(newControlType); 15 | m_controller.store(m_lut.at(m_mode).value()); 16 | m_controller.load()->reset(); 17 | return true; 18 | } 19 | return false; 20 | } 21 | 22 | HWBRIDGE::CONTROL::Mode ActuatorControllerManager::getActiveControlMode() const { 23 | return m_mode.load(); 24 | } 25 | 26 | ActuatorController *ActuatorControllerManager::getActiveController() const { 27 | return m_controller.load(); 28 | } -------------------------------------------------------------------------------- /libs/controllers/src/Current.cpp: -------------------------------------------------------------------------------- 1 | #include "Current.h" 2 | 3 | using namespace Controller; 4 | 5 | Current::Current(Actuator::Actuator &actuator, Encoder::Encoder &encoder, Sensor::CurrentSensor ¤tSensor, 6 | PID::PID &pid, float maxDegPerSec, float maxCurrent, PinName lowerLimit, PinName upperLimit, 7 | bool ignoreDegPerSecChecks, bool ignoreCurrentChecks, bool ignoreLimitSwitchChecks) 8 | : ActuatorController(actuator, encoder, currentSensor, maxDegPerSec, maxCurrent, lowerLimit, upperLimit, 9 | ignoreDegPerSecChecks, ignoreCurrentChecks, ignoreLimitSwitchChecks), 10 | m_pid(pid) {} 11 | 12 | bool Current::update() { 13 | /* Proceed even if encoder read fails, but report */ 14 | bool enc_update_success = m_encoder.update(); 15 | bool cs_update_success = m_currentSensor.value().get().update(); 16 | bool stop_required = false; 17 | if (shouldStop()) { 18 | stop(); 19 | stop_required = true; 20 | } else { 21 | m_actuator.setValue(m_pid.compute(m_setpoint.load(), m_currentSensor.value().get().read())); 22 | } 23 | return enc_update_success && cs_update_success && !stop_required; 24 | } 25 | 26 | void Current::stop() { 27 | setSetPoint(0); 28 | m_actuator.setValue(0); 29 | } 30 | 31 | bool Current::reset() { 32 | stop(); 33 | m_pid.reset(); 34 | bool enc_rst_success = m_encoder.reset(); 35 | bool cs_rst_success = m_currentSensor.value().get().reset(); 36 | return enc_rst_success && cs_rst_success; 37 | } 38 | 39 | std::optional> Current::getPID() { 40 | return m_pid; 41 | } 42 | -------------------------------------------------------------------------------- /libs/controllers/src/OpenLoop.cpp: -------------------------------------------------------------------------------- 1 | #include "OpenLoop.h" 2 | 3 | using namespace Controller; 4 | 5 | void OpenLoop::stop() { 6 | setSetPoint(0); 7 | m_actuator.setValue(0); 8 | } 9 | 10 | bool OpenLoop::reset() { 11 | /* TODO: Improve error reporting to differentiate between encoder and cs errors */ 12 | stop(); 13 | bool enc_rst_success = m_encoder.reset(); 14 | bool cs_rst_success = m_currentSensor.has_value() && m_currentSensor.value().get().reset(); 15 | return enc_rst_success && cs_rst_success; 16 | } 17 | 18 | std::optional> OpenLoop::getPID() { 19 | return std::nullopt; 20 | } 21 | 22 | bool OpenLoop::update() { 23 | /* Move the motor even if an encoder read fails, but still report it */ 24 | bool enc_update_success = m_encoder.update(); 25 | bool cs_update_success = true; 26 | bool stop_required = false; 27 | if (m_currentSensor.has_value()) { 28 | cs_update_success = m_currentSensor.value().get().update(); 29 | } 30 | if (shouldStop()) { 31 | stop_required = true; 32 | stop(); 33 | } else { 34 | m_actuator.setValue(m_setpoint.load()); 35 | } 36 | return enc_update_success && cs_update_success && !stop_required; 37 | } 38 | -------------------------------------------------------------------------------- /libs/controllers/src/Position.cpp: -------------------------------------------------------------------------------- 1 | #include "Position.h" 2 | 3 | using namespace Controller; 4 | 5 | Position::Position(Actuator::Actuator &actuator, Encoder::Encoder &encoder, 6 | const std::optional const> ¤tSensor, 7 | PID::PID &pid, float maxDegPerSec, float maxCurrent, PinName lowerLimit, PinName upperLimit, 8 | bool ignoreDegPerSecChecks, bool ignoreCurrentChecks, bool ignoreLimitSwitchChecks) 9 | : ActuatorController(actuator, encoder, currentSensor, maxDegPerSec, maxCurrent, lowerLimit, upperLimit, 10 | ignoreDegPerSecChecks, ignoreCurrentChecks, ignoreLimitSwitchChecks), 11 | m_pid(pid) {} 12 | 13 | bool Position::update() { 14 | /* Proceed even if encoder read fails, but report */ 15 | bool enc_update_success = m_encoder.update(); 16 | bool cs_update_success = true; 17 | bool stop_required = false; 18 | if (m_currentSensor.has_value()) { 19 | cs_update_success = m_currentSensor.value().get().update(); 20 | } 21 | if (shouldStop()) { 22 | stop_required = true; 23 | stop(); 24 | } else { 25 | m_actuator.setValue(m_pid.compute(m_setpoint.load(), m_encoder.getAngleDeg())); 26 | } 27 | return enc_update_success && cs_update_success && !stop_required; 28 | } 29 | 30 | void Position::stop() { 31 | // set sp to current angle 32 | m_setpoint.store(m_encoder.getAngleDeg()); 33 | m_actuator.setValue(0); 34 | } 35 | 36 | bool Position::reset() { 37 | stop(); 38 | m_pid.reset(); 39 | bool enc_rst_success = m_encoder.reset(); 40 | bool cs_rst_success = m_currentSensor.value().get().reset(); 41 | return enc_rst_success && cs_rst_success; 42 | } 43 | 44 | std::optional> Position::getPID() { 45 | return m_pid; 46 | } -------------------------------------------------------------------------------- /libs/controllers/src/Velocity.cpp: -------------------------------------------------------------------------------- 1 | #include "Velocity.h" 2 | 3 | using namespace Controller; 4 | 5 | Velocity::Velocity(Actuator::Actuator &actuator, Encoder::Encoder &encoder, 6 | const std::optional const> ¤tSensor, 7 | PID::PID &pid, float maxDegPerSec, float maxCurrent, PinName lowerLimit, PinName upperLimit, 8 | bool ignoreDegPerSecChecks, bool ignoreCurrentChecks, bool ignoreLimitSwitchChecks) 9 | : ActuatorController(actuator, encoder, currentSensor, maxDegPerSec, maxCurrent, lowerLimit, upperLimit, 10 | ignoreDegPerSecChecks, ignoreCurrentChecks, ignoreLimitSwitchChecks), 11 | m_pid(pid) {} 12 | 13 | bool Velocity::update() { 14 | /* Move the motor even if an encoder read fails, but still report it */ 15 | bool enc_update_success = m_encoder.update(); 16 | bool cs_update_success = true; 17 | bool stop_required = false; 18 | if (m_currentSensor.has_value()) { 19 | cs_update_success = m_currentSensor.value().get().update(); 20 | } 21 | if (shouldStop()) { 22 | stop_required = true; 23 | stop(); 24 | } else { 25 | m_actuator.setValue(m_pid.compute(m_setpoint.load(), m_encoder.getAngularVelocityDegPerSec())); 26 | } 27 | return enc_update_success && cs_update_success && !stop_required; 28 | } 29 | 30 | void Velocity::stop() { 31 | setSetPoint(0); 32 | m_actuator.setValue(0); 33 | } 34 | 35 | bool Velocity::reset() { 36 | stop(); 37 | m_pid.reset(); 38 | bool enc_rst_success = m_encoder.reset(); 39 | bool cs_rst_success = m_currentSensor.value().get().reset(); 40 | return enc_rst_success && cs_rst_success; 41 | } 42 | 43 | std::optional> Velocity::getPID() { 44 | return m_pid; 45 | } -------------------------------------------------------------------------------- /libs/encoder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(Encoder INTERFACE) 2 | target_include_directories(Encoder INTERFACE include) 3 | 4 | add_library(EncoderMAE3 STATIC) 5 | target_sources(EncoderMAE3 PRIVATE src/MAE3.cpp) 6 | target_include_directories(EncoderMAE3 PUBLIC include) 7 | target_link_libraries(EncoderMAE3 8 | PRIVATE 9 | PwmIn 10 | Encoder 11 | mbed-os 12 | ) 13 | 14 | add_library(Quadrature64CPR STATIC) 15 | target_sources(Quadrature64CPR PRIVATE src/Quadrature64CPR.cpp) 16 | target_include_directories(Quadrature64CPR PUBLIC include) 17 | target_link_libraries(Quadrature64CPR 18 | PRIVATE 19 | QEI 20 | Encoder 21 | mbed-os 22 | ) 23 | 24 | 25 | add_library(EncoderAEAT6012 STATIC) 26 | target_sources(EncoderAEAT6012 PRIVATE src/AEAT6012.cpp) 27 | target_include_directories(EncoderAEAT6012 PUBLIC include) 28 | target_link_libraries(EncoderAEAT6012 29 | PUBLIC 30 | mbed-os 31 | PRIVATE 32 | Encoder 33 | ) 34 | -------------------------------------------------------------------------------- /libs/encoder/include/AEAT6012.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Encoder.h" 6 | #include "mbed.h" 7 | 8 | /* 9 | * Broadcom's AEAT-6012 is an absolute magnetic encoder with 12 bit resolution and SSI interface 10 | * Datasheet: https://docs.broadcom.com/doc/AV02-0188EN 11 | */ 12 | 13 | namespace Encoder { 14 | 15 | class AEAT6012 final : public Encoder { 16 | public: 17 | typedef void (*callback_ptr)(void); 18 | 19 | typedef struct { 20 | PinName spi_clk; 21 | PinName spi_miso; 22 | PinName cs; 23 | float offset_deg; 24 | } Config; 25 | 26 | AEAT6012(const Config &config); 27 | 28 | // Trigger a blocking encoder read and store a value for angle and velocity 29 | // returns whether the read was successful 30 | [[nodiscard]] bool update() override; 31 | 32 | // Returns the stored angle 33 | float getAngleDeg() override; 34 | 35 | // Returns the stored speed 36 | float getAngularVelocityDegPerSec() override; 37 | 38 | // Reset encoder values and offset 39 | [[nodiscard]] bool reset(void) override; 40 | 41 | // Asynchronous API for triggering encoder read 42 | // Invokes user callback once read transaction is complete 43 | // Returns true if SPI read successfully started, false if SPI peripheral is busy 44 | bool update(callback_ptr callback); 45 | 46 | private: 47 | static constexpr uint32_t FREQUENCY_HZ = 1000000; // 1MHz (max frequency given by datasheet) 48 | static constexpr float FLOAT_COMPARE_TOLERANCE = 1e-6; 49 | static constexpr float MOVING_AVERAGE_FILTER_WEIGHT = 0.8; 50 | 51 | const char dummy_buffer[2] = {0x00, 0x00}; 52 | char read_buffer[2] = {0x00, 0x00}; 53 | 54 | float m_position_deg; 55 | float m_angular_velocity_deg_per_sec; 56 | uint16_t m_position_raw; 57 | float m_offset_deg; 58 | 59 | SPI m_spi; 60 | DigitalOut m_cs; 61 | Timer m_timer; 62 | Mutex m_mutex; 63 | 64 | // User callback function to be invoked when an encoder read is complete (only for async reads) 65 | callback_ptr m_callback; 66 | 67 | // Clean-up helper callback function for asynchronous read 68 | void privCallback(int event); 69 | 70 | // Trigger a blocking encoder read 71 | bool read(void); 72 | 73 | // Converts raw encoder reading to position in degrees 74 | static float rawToDegrees(uint16_t raw); 75 | }; 76 | } // namespace Encoder 77 | -------------------------------------------------------------------------------- /libs/encoder/include/Encoder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace Encoder { 4 | class Encoder { 5 | public: 6 | virtual ~Encoder(){}; 7 | 8 | [[nodiscard]] virtual bool update() = 0; 9 | [[nodiscard]] virtual bool reset() = 0; 10 | 11 | virtual float getAngleDeg() = 0; 12 | virtual float getAngularVelocityDegPerSec() = 0; 13 | }; 14 | } // namespace Encoder 15 | -------------------------------------------------------------------------------- /libs/encoder/include/MAE3.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Encoder.h" 4 | #include "PwmIn.h" 5 | 6 | namespace Encoder { 7 | class MAE3 final : public Encoder { 8 | public: 9 | typedef struct Config { 10 | PinName pwm; 11 | float offsetDeg; 12 | } Config; 13 | 14 | MAE3(const Config &config); 15 | MAE3(MAE3 &) = delete; 16 | MAE3(MAE3 &&) = delete; 17 | ~MAE3() = default; 18 | 19 | [[nodiscard]] bool update() override; 20 | float getAngleDeg() override; 21 | float getAngularVelocityDegPerSec() override; 22 | [[nodiscard]] bool reset() override; 23 | 24 | private: 25 | GPIO::PwmIn m_pwmIn; 26 | 27 | Mutex m_mutex; 28 | 29 | float m_angle{0}, m_speed{0}; 30 | 31 | static constexpr float m_degreesPerUnit = 360; 32 | float m_zeroOffsetDeg; 33 | }; 34 | } // namespace Encoder 35 | -------------------------------------------------------------------------------- /libs/encoder/include/Pololu37D.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Encoder.h" 4 | #include "QEI.h" 5 | 6 | namespace Encoder { 7 | class Pololu37D final : public Encoder { 8 | public: 9 | typedef struct Config { 10 | PinName ChannelA, ChannelB, Index; 11 | float offsetDeg; 12 | GPIO::QEI::Encoding quadratureEncoding; 13 | } Config; 14 | 15 | Pololu37D(const Config &config); 16 | Pololu37D(Pololu37D &) = delete; 17 | Pololu37D(Pololu37D &&) = delete; 18 | ~Pololu37D() = default; 19 | 20 | float getAngleDeg() override; 21 | float getAngularVelocityDegPerSec() override; 22 | [[nodiscard]] bool reset() override; 23 | [[nodiscard]] bool update() override; 24 | 25 | private: 26 | GPIO::QEI m_QEI; 27 | 28 | Mutex m_mutex; 29 | 30 | float m_angle = 0, m_speed = 0; 31 | 32 | static constexpr float m_degreesPerCount = 360.0 / 64.0; // since encoder is 64 CPR 33 | float m_zeroOffsetDeg; 34 | }; 35 | } // namespace Encoder 36 | -------------------------------------------------------------------------------- /libs/encoder/include/Quadrature64CPR.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Encoder.h" 4 | #include "QEI.h" 5 | 6 | namespace Encoder { 7 | class Quadrature64CPR final : public Encoder { 8 | // link to the Quadrature64CPR encoder: 9 | // https://www.pololu.com/product/2821 10 | public: 11 | typedef struct Config { 12 | PinName ChannelA, ChannelB, Index; 13 | float offsetDeg; 14 | } Config; 15 | 16 | Quadrature64CPR(const Config &config); 17 | Quadrature64CPR(Quadrature64CPR &) = delete; 18 | Quadrature64CPR(Quadrature64CPR &&) = delete; 19 | ~Quadrature64CPR() = default; 20 | 21 | float getAngleDeg() override; 22 | float getAngularVelocityDegPerSec() override; 23 | bool reset() override; 24 | bool update() override; 25 | 26 | private: 27 | // tracks the previous angle in degrees reading to calculate speed 28 | float m_previous_angle_deg; 29 | // holds the current angle in degrees 30 | float m_current_angle_deg; 31 | // holds the measured speed in degrees per second 32 | float m_anglular_velocity_deg_per_sec; 33 | 34 | GPIO::QEI m_QEI; 35 | Timer m_timer; 36 | Mutex m_mutex; 37 | 38 | static constexpr float m_degreesPerCount = 360.0 / 64.0; // since encoder is 64 CPR 39 | float m_zeroOffsetDeg; 40 | }; 41 | } // namespace Encoder 42 | -------------------------------------------------------------------------------- /libs/encoder/src/MAE3.cpp: -------------------------------------------------------------------------------- 1 | #include "MAE3.h" 2 | 3 | #include 4 | 5 | using namespace Encoder; 6 | 7 | MAE3::MAE3(const Config &config) : m_pwmIn(config.pwm), m_zeroOffsetDeg(config.offsetDeg) {} 8 | 9 | bool MAE3::update() { 10 | std::scoped_lock lock(m_mutex); 11 | m_angle = (m_pwmIn.avgDutyCycle() * m_degreesPerUnit) - m_zeroOffsetDeg; 12 | m_speed = m_pwmIn.avgDutyCycleVelocity() * m_degreesPerUnit; 13 | return true; 14 | } 15 | 16 | float MAE3::getAngleDeg() { 17 | std::scoped_lock lock(m_mutex); 18 | return m_angle; 19 | } 20 | 21 | float MAE3::getAngularVelocityDegPerSec() { 22 | std::scoped_lock lock(m_mutex); 23 | return m_speed; 24 | } 25 | 26 | bool MAE3::reset() { 27 | std::scoped_lock lock(m_mutex); 28 | m_zeroOffsetDeg = m_pwmIn.avgDutyCycle() * m_degreesPerUnit; 29 | return true; 30 | } 31 | -------------------------------------------------------------------------------- /libs/encoder/src/Pololu37D.cpp: -------------------------------------------------------------------------------- 1 | #include "Pololu37D.h" 2 | 3 | #include 4 | 5 | using namespace Encoder; 6 | 7 | Pololu37D::Pololu37D(const Config &config) 8 | : m_QEI(config.ChannelA, config.ChannelB, config.Index, config.quadratureEncoding), 9 | m_zeroOffsetDeg(config.offsetDeg) {} 10 | 11 | bool Pololu37D::update() { 12 | std::scoped_lock lock(m_mutex); 13 | m_angle = (m_QEI.getPulses() * m_degreesPerCount) - m_zeroOffsetDeg; 14 | m_speed = m_QEI.getPulseVelocity_PulsesPerSec() * m_degreesPerCount; 15 | return true; 16 | } 17 | float Pololu37D::getAngleDeg() { 18 | std::scoped_lock lock(m_mutex); 19 | return m_angle; 20 | } 21 | 22 | float Pololu37D::getAngularVelocityDegPerSec() { 23 | std::scoped_lock lock(m_mutex); 24 | return m_speed; 25 | } 26 | 27 | bool Pololu37D::reset() { 28 | std::scoped_lock lock(m_mutex); 29 | m_QEI.reset(); 30 | return true; 31 | } 32 | -------------------------------------------------------------------------------- /libs/encoder/src/Quadrature64CPR.cpp: -------------------------------------------------------------------------------- 1 | #include "Quadrature64CPR.h" 2 | 3 | #include 4 | 5 | using namespace Encoder; 6 | 7 | Quadrature64CPR::Quadrature64CPR(const Config &config) 8 | : m_QEI(config.ChannelA, config.ChannelB, config.Index, GPIO::QEI::Encoding::X4_ENCODING), 9 | m_zeroOffsetDeg(config.offsetDeg) {} 10 | 11 | float Quadrature64CPR::getAngleDeg() { 12 | std::scoped_lock lock(m_mutex); 13 | return m_current_angle_deg; 14 | } 15 | 16 | float Quadrature64CPR::getAngularVelocityDegPerSec() { 17 | std::scoped_lock lock(m_mutex); 18 | return m_anglular_velocity_deg_per_sec; 19 | } 20 | 21 | bool Quadrature64CPR::reset() { 22 | std::scoped_lock lock(m_mutex); 23 | update(); 24 | 25 | m_zeroOffsetDeg = m_current_angle_deg; 26 | 27 | m_current_angle_deg = 0.0; 28 | m_previous_angle_deg = 0.0; 29 | 30 | m_anglular_velocity_deg_per_sec = 0.0; 31 | m_QEI.reset(); 32 | 33 | return true; 34 | } 35 | 36 | bool Quadrature64CPR::update() { 37 | // time since last measurement in nanoseconds 38 | m_timer.stop(); 39 | float dt = std::chrono::duration_cast(m_timer.elapsed_time()).count(); 40 | m_timer.reset(); 41 | m_timer.start(); 42 | 43 | m_previous_angle_deg = m_current_angle_deg; 44 | 45 | m_current_angle_deg = (m_QEI.getPulses() * m_degreesPerCount) - m_zeroOffsetDeg; 46 | 47 | if (dt != 0) { 48 | // to estimate speed: delta_angle / delta_time (degrees / sec) 49 | m_anglular_velocity_deg_per_sec = (m_current_angle_deg - m_previous_angle_deg) / dt * 1000000000; 50 | } else { 51 | // to avoid div by zero 52 | m_anglular_velocity_deg_per_sec = 0; 53 | // TODO log this as an error considering dt = 0 54 | } 55 | 56 | return true; 57 | } 58 | -------------------------------------------------------------------------------- /libs/gamepad/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(AnalogBusIn STATIC) 2 | target_sources(AnalogBusIn PRIVATE src/AnalogBusIn.cpp) 3 | target_include_directories(AnalogBusIn PUBLIC include) 4 | target_link_libraries(AnalogBusIn 5 | PUBLIC 6 | mbed-os 7 | ) 8 | 9 | add_library(AnalogInputGroup STATIC) 10 | target_sources(AnalogInputGroup PRIVATE src/AnalogInputGroup.cpp) 11 | target_include_directories(AnalogInputGroup PUBLIC include) 12 | target_link_libraries(AnalogInputGroup 13 | PRIVATE 14 | AnalogBusIn 15 | mbed-os 16 | ) 17 | 18 | add_library(DigitalInputGroup STATIC) 19 | target_sources(DigitalInputGroup PRIVATE src/DigitalInputGroup.cpp) 20 | target_include_directories(DigitalInputGroup PUBLIC include) 21 | target_link_libraries(DigitalInputGroup 22 | PRIVATE 23 | AnalogBusIn 24 | mbed-os 25 | ) 26 | 27 | add_library(FrameArbiter STATIC) 28 | target_sources(FrameArbiter PRIVATE src/FrameArbiter.cpp) 29 | target_include_directories(FrameArbiter PUBLIC include) 30 | target_link_libraries(FrameArbiter 31 | PRIVATE 32 | AnalogInputGroup 33 | DigitalInputGroup 34 | mbed-os 35 | ) 36 | 37 | -------------------------------------------------------------------------------- /libs/gamepad/include/AnalogBusIn.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "mbed.h" 6 | 7 | /* This class is analogous to the BusIn class (which can be found at mbed-os/drivers/BusIn) 8 | that mbed provided, except that we changed maximum number of inputs from 16 to 8 9 | 10 | A analog input bus, used for reading the state of a collection of pins 11 | 12 | @note Synchronization level: Thread safe 13 | */ 14 | class AnalogBusIn { 15 | // have to declare it at front, else won't be able to use it in function parameter 16 | private: 17 | static constexpr uint8_t MAX_INPUTS_NUM = 8; 18 | 19 | public: 20 | /** Create an AnalogBusIn, connected to the specified pins 21 | * @note 22 | * It is only required to specify as many pin variables as is required 23 | * for the bus; the rest will default to NC (not connected) 24 | */ 25 | AnalogBusIn(PinName p0, PinName p1 = NC, PinName p2 = NC, PinName p3 = NC, PinName p4 = NC, PinName p5 = NC, 26 | PinName p6 = NC, PinName p7 = NC); 27 | 28 | /** Create an AnalogBusIn, connected to the specified pins 29 | * 30 | * @param pins An array of pins to connect to bus bit 31 | */ 32 | AnalogBusIn(PinName pins[MAX_INPUTS_NUM]); 33 | 34 | virtual ~AnalogBusIn(); 35 | 36 | /** Read the value of the input bus, represented as an array of floats in the range [0, 1] 37 | * @param values, an array of floats of length 16 that will be filled with the values read 38 | */ 39 | void read(float values[MAX_INPUTS_NUM]); 40 | 41 | /** Read the value of the input bus, represented as an array of unsigned short in the range [0x0, 0xFFFF] 42 | * @param values, an array of unsigned short of length 16 that will be filled with the values read 43 | */ 44 | void read_u16(uint16_t values[MAX_INPUTS_NUM]); 45 | 46 | /** Binary mask of bus pins connected to actual pins (not NC pins) 47 | * If bus pin is in NC state make corresponding bit will be cleared (set to 0), else bit will be set to 1 48 | * 49 | * @returns 50 | * Binary mask of connected pins 51 | */ 52 | int mask() { 53 | // No lock needed since _nc_mask is not modified outside the constructor 54 | return _nc_mask; 55 | } 56 | 57 | /** Access to particular input in random-iterator fashion 58 | * @param index Position of input 59 | */ 60 | AnalogIn &operator[](int index); 61 | 62 | protected: 63 | std::unique_ptr _pin[MAX_INPUTS_NUM]; 64 | 65 | /* Mask of bus's NC pins 66 | * If bit[n] is set to 1 - pin is connected 67 | * if bit[n] is cleared - pin is not connected (NC) 68 | */ 69 | int _nc_mask; 70 | 71 | PlatformMutex _mutex; 72 | 73 | private: 74 | virtual void lock(); 75 | virtual void unlock(); 76 | }; 77 | -------------------------------------------------------------------------------- /libs/gamepad/include/AnalogInputGroup.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "AnalogBusIn.h" 4 | #include "mbed.h" 5 | 6 | /* This class wraps an AnalogBusIn object. It is used for reading a group of analog inputs, 7 | and provides averaging to the read values 8 | */ 9 | 10 | /* Need to test the different averaging algo, trade off with the current implementation is that 11 | SMA is faster and EWMA is takes less memory, and is probably more accurate 12 | */ 13 | enum class AveragingAlgoType { SMA, EWMA }; 14 | 15 | class AnalogInputGroup { 16 | public: 17 | AnalogInputGroup(AnalogBusIn& inputs, uint8_t numPins, AveragingAlgoType averagingAlgoType = AveragingAlgoType::SMA, 18 | uint8_t numSampleToAverage = DEFAULT_NUM_SAMPLE_AVERAGE, float decayFactor = DEFAULT_DECAY_FACTOR); 19 | 20 | virtual ~AnalogInputGroup() = default; 21 | 22 | // some getters 23 | uint8_t getNumInputs() const; 24 | 25 | static constexpr uint8_t MAX_INPUTS_NUM = 8; 26 | 27 | /* Read the analog inputs. This function is intended to be called as often as possible, 28 | ideally in a tight loop 29 | */ 30 | void read(); 31 | 32 | /* 33 | Get the average values of the analog inputs by reference. 34 | This function is overloaded to be able to return the analog input values in different format 35 | 36 | Note that the order of pins in the constructor of AnalogBusIn is the reverse order of the pins in the 37 | byte order. If you have BusIn(a,b,c,d,e,f,g,h) then the order of bits in the byte would be 38 | hgfedcba with a being bit 0, b being bit 1, c being bit 2 and so on. 39 | */ 40 | 41 | // the client should make sure the passed in array is large enough 42 | void getValues(float* values); 43 | void getValues(uint16_t* values); 44 | 45 | /* reset the internal variables to start fresh, called by getValuesAndInvalidReads */ 46 | void reset(); 47 | 48 | protected: 49 | void readSMA(); 50 | void readEWMA(); 51 | void getValuesSMA(float* values); 52 | void getValuesSMA(uint16_t* values); 53 | void getValuesEWMA(float* values); 54 | void getValuesEWMA(uint16_t* values); 55 | 56 | static constexpr uint8_t MAX_NUM_SAMPLE_AVERAGE = 20; 57 | static constexpr uint16_t DEFAULT_NUM_SAMPLE_AVERAGE = 5; 58 | // need test to find what is the best value for this factor 59 | static constexpr float DEFAULT_DECAY_FACTOR = 0.8; 60 | 61 | AnalogBusIn& m_inputs; 62 | uint16_t m_numPins; 63 | 64 | // Exponentially weighted moving average: 65 | // S = ax + (1-a)S 66 | float m_decayFactor = DEFAULT_DECAY_FACTOR; 67 | // we make the array size big enough when declared. This might result in some waste, but this 68 | // allows us to not dynamically allocate memory 69 | uint16_t m_values_EWMA[MAX_INPUTS_NUM] = {0}; 70 | 71 | // Simple moving average: 72 | uint8_t m_num_samples_to_average = DEFAULT_NUM_SAMPLE_AVERAGE; 73 | uint8_t m_num_samples = 0; 74 | 75 | uint16_t m_values_SMA[MAX_INPUTS_NUM][MAX_NUM_SAMPLE_AVERAGE] = {0}; 76 | uint16_t m_values_idx[MAX_INPUTS_NUM] = {0}; 77 | uint32_t m_values_sum[MAX_INPUTS_NUM] = {0}; 78 | 79 | AveragingAlgoType m_averagingAlgoType = AveragingAlgoType::SMA; 80 | }; 81 | -------------------------------------------------------------------------------- /libs/gamepad/src/AnalogBusIn.cpp: -------------------------------------------------------------------------------- 1 | #include "AnalogBusIn.h" 2 | 3 | AnalogBusIn::AnalogBusIn(PinName p0, PinName p1, PinName p2, PinName p3, PinName p4, PinName p5, PinName p6, 4 | PinName p7) { 5 | PinName pins[MAX_INPUTS_NUM] = {p0, p1, p2, p3, p4, p5, p6, p7}; 6 | 7 | // No lock needed in the constructor 8 | _nc_mask = 0; 9 | for (int i = 0; i < MAX_INPUTS_NUM; i++) { 10 | _pin[i] = (pins[i] != NC) ? std::make_unique(pins[i]) : nullptr; 11 | if (pins[i] != NC) { 12 | _nc_mask |= (1 << i); 13 | } 14 | } 15 | } 16 | 17 | AnalogBusIn::AnalogBusIn(PinName pins[MAX_INPUTS_NUM]) { 18 | // No lock needed in the constructor 19 | _nc_mask = 0; 20 | for (int i = 0; i < MAX_INPUTS_NUM; i++) { 21 | _pin[i] = (pins[i] != NC) ? std::make_unique(pins[i]) : nullptr; 22 | if (pins[i] != NC) { 23 | _nc_mask |= (1 << i); 24 | } 25 | } 26 | } 27 | 28 | AnalogBusIn::~AnalogBusIn() { 29 | // No lock needed in the destructor, and since we use smart pointers, no need for delete 30 | } 31 | 32 | void AnalogBusIn::read(float values[MAX_INPUTS_NUM]) { 33 | lock(); 34 | for (int i = 0; i < MAX_INPUTS_NUM; i++) { 35 | if (_pin[i] != 0) { 36 | values[i] = _pin[i]->read(); 37 | } 38 | } 39 | unlock(); 40 | } 41 | 42 | void AnalogBusIn::read_u16(uint16_t values[MAX_INPUTS_NUM]) { 43 | lock(); 44 | for (int i = 0; i < MAX_INPUTS_NUM; i++) { 45 | if (_pin[i] != 0) { 46 | values[i] = _pin[i]->read_u16(); 47 | } 48 | } 49 | unlock(); 50 | } 51 | 52 | void AnalogBusIn::lock() { 53 | _mutex.lock(); 54 | } 55 | 56 | void AnalogBusIn::unlock() { 57 | _mutex.unlock(); 58 | } 59 | 60 | AnalogIn &AnalogBusIn::operator[](int index) { 61 | // No lock needed since _pin is not modified outside the constructor 62 | MBED_ASSERT(index >= 0 && index < MAX_INPUTS_NUM); 63 | MBED_ASSERT(_pin[index]); 64 | return *_pin[index]; 65 | } 66 | -------------------------------------------------------------------------------- /libs/gpio/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_include_directories(Encoder INTERFACE include) 2 | 3 | add_library(PwmIn STATIC) 4 | target_sources(PwmIn PRIVATE src/PwmIn.cpp) 5 | target_include_directories(PwmIn PUBLIC include) 6 | target_link_libraries(PwmIn 7 | PRIVATE 8 | mbed-os 9 | ) 10 | 11 | add_library(QEI STATIC) 12 | target_sources(QEI PRIVATE src/QEI.cpp) 13 | target_include_directories(QEI PUBLIC include) 14 | target_include_directories(QEI PUBLIC Encoder) 15 | target_link_libraries(QEI 16 | PRIVATE 17 | mbed-os 18 | ) 19 | 20 | 21 | add_library(LimitSwitch STATIC) 22 | target_sources(LimitSwitch PRIVATE src/LimitSwitch.cpp) 23 | target_include_directories(LimitSwitch PUBLIC include) 24 | target_link_libraries(LimitSwitch 25 | PRIVATE 26 | mbed-os 27 | ) 28 | -------------------------------------------------------------------------------- /libs/gpio/include/LimitSwitch.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "mbed.h" 3 | namespace GPIO { 4 | 5 | class LimitSwitch { 6 | public: 7 | LimitSwitch(DigitalIn limitPin, bool ActiveHigh = 1); 8 | // Tells you whether the Limit Switch has been pressed or not 9 | bool isPressed(); 10 | 11 | // Overloaded bool operator 12 | operator bool(); 13 | 14 | bool isConnected(); 15 | 16 | private: 17 | // Active High is 1 and Active Low is 0 18 | bool m_ActiveHigh; 19 | DigitalIn m_limitPin; 20 | }; 21 | 22 | } // namespace GPIO -------------------------------------------------------------------------------- /libs/gpio/include/PwmIn.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "mbed.h" 4 | 5 | /** PwmIn class to read PWM inputs 6 | * 7 | * Uses InterruptIn to measure the changes on the input 8 | * and record the time they occur 9 | * 10 | * @note uses InterruptIn, so not available on p19/p20 11 | */ 12 | 13 | namespace GPIO { 14 | 15 | class PwmIn { 16 | public: 17 | /** Create a PwmIn with a specified number of pulses to average 18 | * 19 | * @param pwmSense The pwm input pin (must support InterruptIn) 20 | * @param numSamplesToAverage The number of PWM measurements to sum before averaging 21 | */ 22 | PwmIn(PinName pwmSense, int numSamplesToAverage = DEFAULT_NUM_SAMPLES_TO_AVERAGE); 23 | 24 | /** the default copy construcor is ill formed since it uses Interrupt In 25 | * which is a mbed NonCopyable type 26 | */ 27 | PwmIn(PwmIn&) = delete; 28 | 29 | ~PwmIn(); 30 | 31 | /** Read the current period 32 | * 33 | * @returns the period in seconds 34 | */ 35 | std::chrono::duration period(); 36 | 37 | /** Read the average period 38 | * 39 | * @returns the average period in seconds 40 | */ 41 | std::chrono::duration avgPeriod(); 42 | 43 | /** Read the current pulse width 44 | * 45 | * @returns the pulsewidth in seconds 46 | */ 47 | std::chrono::duration pulseWidth(); 48 | 49 | /** Read the average pulse width 50 | * 51 | * @returns the average pulsewidth in seconds 52 | */ 53 | std::chrono::duration avgPulseWidth(); 54 | 55 | /** Read the current duty cycle 56 | * 57 | * @returns the duty cycle as a percentage, represented between 0.0-1.0 58 | */ 59 | float dutyCycle(); 60 | 61 | /** Read the average duty cycle 62 | * 63 | * @returns the average duty cycle as a percentage, represented between 0.0-1.0 64 | */ 65 | float avgDutyCycle(); 66 | 67 | /** Read the average duty cycle velocity 68 | * 69 | * @returns the average duty cycle velocity as a 0.0-1.0 percentage/second 70 | */ 71 | float avgDutyCycleVelocity(); 72 | 73 | protected: 74 | InterruptIn m_pwmSense; 75 | Timer m_timer; 76 | 77 | std::chrono::duration m_pulseWidth, m_period, m_avgPulseWidth, m_avgPeriod; 78 | float m_avgDutyCycle, m_prevAvgDutyCycle, m_avgDutyCycleVelocity; 79 | 80 | int m_sampleCount; 81 | int m_numSamplesToAverage; 82 | 83 | std::chrono::duration* p_pulseWidthSamples; 84 | std::chrono::duration* p_periodSamples; 85 | 86 | std::chrono::duration m_pulseWidthSampleSum; 87 | std::chrono::duration m_periodSampleSum; 88 | 89 | void rise(); 90 | void fall(); 91 | std::chrono::duration movingAvg(std::chrono::duration* p_samples, 92 | std::chrono::duration* p_sampleSum, 93 | std::chrono::duration newSample, int newIndex); 94 | 95 | static constexpr uint8_t DEFAULT_NUM_SAMPLES_TO_AVERAGE = 12; 96 | }; 97 | 98 | } // namespace GPIO 99 | -------------------------------------------------------------------------------- /libs/gpio/src/LimitSwitch.cpp: -------------------------------------------------------------------------------- 1 | #include "LimitSwitch.h" 2 | 3 | using namespace GPIO; 4 | 5 | LimitSwitch::LimitSwitch(DigitalIn limitPin, bool ActiveHigh) : m_limitPin(limitPin), m_ActiveHigh(ActiveHigh) {} 6 | 7 | bool LimitSwitch::isPressed() { 8 | if (m_limitPin.is_connected()) { 9 | return !m_ActiveHigh ? !m_limitPin.read() : m_limitPin.read(); 10 | } 11 | return false; 12 | } 13 | 14 | LimitSwitch::operator bool() { 15 | return isPressed(); 16 | } 17 | 18 | bool LimitSwitch::isConnected() { 19 | return m_limitPin.is_connected(); 20 | } -------------------------------------------------------------------------------- /libs/gpio/src/PwmIn.cpp: -------------------------------------------------------------------------------- 1 | #include "PwmIn.h" 2 | 3 | using namespace GPIO; 4 | 5 | PwmIn::PwmIn(PinName pwmSense, int numSamplesToAverage) 6 | : m_pwmSense(pwmSense), m_numSamplesToAverage(numSamplesToAverage) { 7 | m_pwmSense.rise(callback(this, &PwmIn::rise)); 8 | m_pwmSense.fall(callback(this, &PwmIn::fall)); 9 | 10 | m_period = 0s; 11 | m_pulseWidth = 0s; 12 | m_periodSampleSum = 0s; 13 | m_pulseWidthSampleSum = 0s; 14 | m_sampleCount = 0; 15 | m_avgDutyCycle = 0; 16 | m_prevAvgDutyCycle = 0; 17 | m_avgDutyCycleVelocity = 0; 18 | 19 | p_periodSamples = new std::chrono::duration[m_numSamplesToAverage](); 20 | p_pulseWidthSamples = new std::chrono::duration[m_numSamplesToAverage](); 21 | 22 | m_timer.start(); 23 | } 24 | 25 | PwmIn::~PwmIn() { 26 | delete[] p_pulseWidthSamples; 27 | delete[] p_periodSamples; 28 | } 29 | 30 | std::chrono::duration PwmIn::period() { 31 | return m_period; 32 | } 33 | 34 | std::chrono::duration PwmIn::avgPeriod() { 35 | return m_avgPeriod; 36 | } 37 | 38 | std::chrono::duration PwmIn::pulseWidth() { 39 | return m_pulseWidth; 40 | } 41 | 42 | std::chrono::duration PwmIn::avgPulseWidth() { 43 | return m_avgPulseWidth; 44 | } 45 | 46 | float PwmIn::dutyCycle() { 47 | return m_pulseWidth / m_period; 48 | } 49 | 50 | float PwmIn::avgDutyCycle() { 51 | return m_avgDutyCycle; 52 | } 53 | 54 | float PwmIn::avgDutyCycleVelocity() { 55 | return m_avgDutyCycleVelocity; 56 | } 57 | 58 | void PwmIn::rise() { 59 | m_period = m_timer.elapsed_time(); 60 | m_timer.reset(); 61 | 62 | m_avgPeriod = PwmIn::movingAvg(p_periodSamples, &m_periodSampleSum, m_period, m_sampleCount); 63 | } 64 | 65 | void PwmIn::fall() { 66 | m_pulseWidth = m_timer.elapsed_time(); 67 | 68 | m_avgPulseWidth = PwmIn::movingAvg(p_pulseWidthSamples, &m_pulseWidthSampleSum, m_pulseWidth, m_sampleCount); 69 | m_avgDutyCycle = m_avgPulseWidth / m_avgPeriod; 70 | m_avgDutyCycleVelocity = (m_avgDutyCycle - m_prevAvgDutyCycle) / m_avgPeriod.count(); 71 | m_prevAvgDutyCycle = m_avgDutyCycle; 72 | 73 | m_sampleCount++; 74 | 75 | if (m_sampleCount >= m_numSamplesToAverage) { 76 | m_sampleCount = 0; 77 | } 78 | } 79 | 80 | std::chrono::duration PwmIn::movingAvg(std::chrono::duration* p_samples, 81 | std::chrono::duration* p_sampleSum, 82 | std::chrono::duration newSample, int newIndex) { 83 | *p_sampleSum -= p_samples[newIndex]; 84 | p_samples[newIndex] = newSample; 85 | *p_sampleSum += p_samples[newIndex]; 86 | 87 | return *p_sampleSum / m_numSamplesToAverage; 88 | } 89 | -------------------------------------------------------------------------------- /libs/led-matrix/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(LEDMatrix STATIC) 2 | target_sources(LEDMatrix PRIVATE src/LEDMatrix.cpp) 3 | target_include_directories(LEDMatrix PUBLIC include) 4 | target_link_libraries(LEDMatrix 5 | PRIVATE 6 | uwrt-mars-rover-hw-bridge 7 | mbed-os 8 | ) -------------------------------------------------------------------------------- /libs/led-matrix/include/LEDMatrix.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "hw_bridge.h" 3 | #include "mbed.h" 4 | 5 | class LEDMatrix { 6 | public: 7 | // Define LED matrix colour channels by the pins it is connected to. 8 | LEDMatrix(PinName R, PinName G, PinName B); 9 | 10 | // Terminate thread and clear lights. 11 | ~LEDMatrix(); 12 | 13 | // Set the state of the LEDs. 14 | void setState(HWBRIDGE::LEDMATRIX::LEDMatrixState state); 15 | 16 | // Periodically flash the colour given by R, G, B on the LED matrix. 17 | void setFlashColor(bool R, bool G, bool B); 18 | 19 | // Set a solid colour given by the R, G, B on the LED matrix. 20 | void setSolidColor(bool R, bool G, bool B); 21 | 22 | private: 23 | DigitalOut m_RChannel; 24 | DigitalOut m_GChannel; 25 | DigitalOut m_BChannel; 26 | 27 | // Flags 28 | constexpr static uint32_t START_FLASH = (1U << 0); // 01 29 | constexpr static uint32_t ENDED_FLASH = (1U << 1); // 10 30 | 31 | volatile bool m_flashing_red, m_flashing_green, m_flashing_blue; 32 | volatile bool m_continue_flashing; 33 | 34 | EventFlags m_event_flags; 35 | Thread m_lightsThread; 36 | static constexpr auto PERIOD_DELAY = 500ms; 37 | 38 | // Thread to take care of flashing colours on the LED matrix by setFlashColor(). 39 | void flashing(); 40 | 41 | // Set the given R, G, B colour channels. 42 | void setColor(bool R, bool G, bool B); 43 | }; 44 | -------------------------------------------------------------------------------- /libs/led-matrix/src/LEDMatrix.cpp: -------------------------------------------------------------------------------- 1 | #include "LEDMatrix.h" 2 | 3 | LEDMatrix::LEDMatrix(PinName R, PinName G, PinName B) 4 | : m_RChannel(R), 5 | m_GChannel(G), 6 | m_BChannel(B), 7 | m_flashing_red(false), 8 | m_flashing_green(false), 9 | m_flashing_blue(false), 10 | m_continue_flashing(false) { 11 | m_lightsThread.start(callback(this, &LEDMatrix::flashing)); 12 | } 13 | 14 | LEDMatrix::~LEDMatrix() { 15 | m_lightsThread.terminate(); 16 | setSolidColor(0, 0, 0); 17 | } 18 | 19 | void LEDMatrix::setState(HWBRIDGE::LEDMATRIX::LEDMatrixState state) { 20 | switch (state) { 21 | case HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_RED: 22 | setSolidColor(1, 0, 0); 23 | break; 24 | case HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_BLUE: 25 | setSolidColor(0, 0, 1); 26 | break; 27 | case HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_GREEN: 28 | setSolidColor(0, 1, 0); 29 | break; 30 | case HWBRIDGE::LEDMATRIX::LEDMatrixState::FLASHING_RED: 31 | setFlashColor(1, 0, 0); 32 | break; 33 | case HWBRIDGE::LEDMATRIX::LEDMatrixState::FLASHING_BLUE: 34 | setFlashColor(0, 0, 1); 35 | break; 36 | case HWBRIDGE::LEDMATRIX::LEDMatrixState::FLASHING_GREEN: 37 | setFlashColor(0, 1, 0); 38 | break; 39 | case HWBRIDGE::LEDMATRIX::LEDMatrixState::OFF: 40 | setSolidColor(0, 0, 0); 41 | break; 42 | } 43 | } 44 | 45 | void LEDMatrix::flashing() { // check https://os.mbed.com/docs/mbed-os/v6.8/apis/thisthread.html 46 | while (true) { 47 | m_event_flags.wait_all(START_FLASH); // yield until flag is set 48 | m_continue_flashing = true; 49 | 50 | while (m_continue_flashing) { 51 | setColor(m_flashing_red, m_flashing_green, m_flashing_blue); 52 | ThisThread::sleep_for(PERIOD_DELAY); 53 | setColor(0, 0, 0); 54 | ThisThread::sleep_for(PERIOD_DELAY); 55 | } 56 | m_event_flags.set(ENDED_FLASH); 57 | } 58 | } 59 | 60 | void LEDMatrix::setFlashColor(bool R, bool G, bool B) { 61 | m_flashing_red = R; 62 | m_flashing_green = G; 63 | m_flashing_blue = B; 64 | 65 | if (!m_continue_flashing) { 66 | m_event_flags.set(START_FLASH); 67 | } 68 | } 69 | 70 | void LEDMatrix::setSolidColor(bool R, bool G, bool B) { 71 | if (m_continue_flashing) { 72 | m_continue_flashing = false; 73 | m_event_flags.wait_all(ENDED_FLASH); // wait for while loop in flashing() to finish 74 | } 75 | setColor(R, G, B); 76 | } 77 | 78 | void LEDMatrix::setColor(bool R, bool G, bool B) { 79 | m_RChannel = R; 80 | m_GChannel = G; 81 | m_BChannel = B; 82 | } 83 | -------------------------------------------------------------------------------- /libs/neopixel/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(Neopixel_Blocking STATIC) 2 | target_sources(Neopixel_Blocking PRIVATE src/Neopixel_Blocking.cpp) 3 | target_include_directories(Neopixel_Blocking PUBLIC include) 4 | target_link_libraries(Neopixel_Blocking PRIVATE mbed-os) 5 | -------------------------------------------------------------------------------- /libs/neopixel/include/Neopixel_Blocking.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "mbed.h" 3 | 4 | /* 5 | Data is written in G -> R -> B 6 | Each byte is a number value from 0-255 of the intensity of that individual LED light 7 | */ 8 | class Neopixel_Blocking { 9 | private: 10 | int m_pixelNum; 11 | int m_on_buffer[8] = {1, 1, 1, 1, 1, 1, 1, 1}; 12 | int m_off_buffer[8] = {0}; 13 | DigitalOut out; 14 | 15 | public: 16 | enum colour { Green, Red, Blue, White, Off }; 17 | Neopixel_Blocking(int numPixels, PinName mtrxPinName); 18 | ~Neopixel_Blocking(); 19 | void pulse_1(); 20 | void pulse_0(); 21 | void writeByte(const int buffer[8]); 22 | void showColour(colour selectedColour); 23 | void blinkPixels(int numflashes, std::chrono::duration delay, colour selectedColour); 24 | void writeAnyRGB(const int colour[3]); 25 | 26 | // basic functions for autonomy 27 | void displayRed(); 28 | void displayBlue(); 29 | void flashGreen(int numFlashes, std::chrono::duration delay); 30 | void shutdown(); 31 | }; 32 | -------------------------------------------------------------------------------- /libs/pid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(PID STATIC) 2 | target_sources(PID PRIVATE src/PID.cpp) 3 | target_include_directories(PID PUBLIC include) 4 | target_link_libraries(PID PRIVATE mbed-os) 5 | -------------------------------------------------------------------------------- /libs/pid/include/PID.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This PID library was inspired by http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ 3 | * Terminology: 4 | * Plant: What you are trying to control 5 | * Process Variable(PV): Measured value of plant. Sensor reading. 6 | * Setpoint: Desired state of the plant 7 | * Deadzone: Value below which error is considered 0 8 | * Gain: Scalar seen in the state space representation of a PID controller 9 | * Path: A path on the state space representation of a PID controller 10 | * Features: 11 | * Thread-safety: This PID library is thread safe. 12 | * WARNING: Mutexes wait for lock indefinetly, since deadlock is currently impossible 13 | * If modifying the code, ensure deadlock remains impossible 14 | * Anti-Windup: See this video: https://www.youtube.com/watch?v=NVLXCwc8HzM&t=571s&ab_channel=MATLAB 15 | * See also Brett Beauregard blog: 16 | * http://brettbeauregard.com/blog/2011/04/improving-the-beginner%e2%80%99s-pid-reset-windup/ 17 | * Anti-Derivative-Kickback: Avoid jerkiness, by differentiating on PV. See Brett Beauregard blog: 18 | * http://brettbeauregard.com/blog/2011/04/improving-the-beginner%e2%80%99s-pid-derivative-kick/ 19 | * On-The-Fly Tuning: Allows gains to be changed with reduced joltiness. Done by multiplying the gain into the error 20 | * accumulator: 21 | * http://brettbeauregard.com/blog/2011/04/improving-the-beginner%e2%80%99s-pid-tuning-changes/ 22 | * 23 | */ 24 | #pragma once 25 | 26 | #include "mbed.h" 27 | 28 | namespace PID { 29 | typedef struct Config { 30 | float proportionalGain, integralGain, derivativeGain; 31 | float lowerBound, upperBound; 32 | float deadzone; 33 | bool antiKickback = true; 34 | bool antiWindup = true; 35 | } Config; 36 | 37 | class PID { 38 | public: 39 | PID(const Config &config); 40 | PID(PID &) = delete; 41 | PID(PID &&) = delete; 42 | ~PID() = default; 43 | 44 | void updateProportionalGain(float p); 45 | void updateIntegralGain(float i); 46 | void updateDerivativeGain(float d); 47 | void updateDeadzone(float deadzone); 48 | 49 | float reportProportionalGain() const; 50 | float reportIntegralGain() const; 51 | float reportDerivativeGain() const; 52 | float reportDeadzone() const; 53 | 54 | void reset(); 55 | 56 | float compute(float setPoint, float processVariable, float ff = 0); // takes ~15us to run 57 | 58 | private: 59 | mutable Mutex m_mutex; 60 | Timer m_timer; 61 | float m_PGain, m_IGain, m_DGain; 62 | const float m_lowerBound, m_upperBound; 63 | float m_deadzone; 64 | float m_IAccumulator{0}; 65 | float m_pastError{0}, m_pastPV{0}; 66 | const bool m_antiKickback, m_antiWindup; 67 | float computePPath(float error); 68 | float computeDPath(float deltaNumerator, float dt); 69 | }; 70 | } // namespace PID 71 | -------------------------------------------------------------------------------- /libs/pid/src/PID.cpp: -------------------------------------------------------------------------------- 1 | #include "PID.h" 2 | 3 | #include 4 | #include 5 | 6 | using namespace PID; 7 | using namespace std::chrono_literals; 8 | 9 | PID::PID::PID(const Config &config) 10 | : m_PGain(config.proportionalGain), 11 | m_IGain(config.integralGain), 12 | m_DGain(config.derivativeGain), 13 | m_lowerBound(config.lowerBound), 14 | m_upperBound(config.upperBound), 15 | m_deadzone(config.deadzone), 16 | m_antiKickback(config.antiKickback), 17 | m_antiWindup(config.antiWindup) {} 18 | 19 | void PID::PID::updateProportionalGain(float p) { 20 | std::scoped_lock lock(m_mutex); 21 | m_PGain = p; 22 | } 23 | 24 | void PID::PID::updateIntegralGain(float i) { 25 | std::scoped_lock lock(m_mutex); 26 | m_IGain = i; 27 | } 28 | 29 | void PID::PID::updateDerivativeGain(float d) { 30 | std::scoped_lock lock(m_mutex); 31 | m_DGain = d; 32 | } 33 | 34 | void PID::PID::updateDeadzone(float deadzone) { 35 | std::scoped_lock lock(m_mutex); 36 | m_deadzone = deadzone; 37 | } 38 | 39 | float PID::PID::reportProportionalGain() const { 40 | std::scoped_lock lock(m_mutex); 41 | return m_PGain; 42 | } 43 | 44 | float PID::PID::reportIntegralGain() const { 45 | std::scoped_lock lock(m_mutex); 46 | return m_IGain; 47 | } 48 | 49 | float PID::PID::reportDerivativeGain() const { 50 | std::scoped_lock lock(m_mutex); 51 | return m_DGain; 52 | } 53 | 54 | float PID::PID::reportDeadzone() const { 55 | std::scoped_lock lock(m_mutex); 56 | return m_deadzone; 57 | } 58 | 59 | void PID::PID::reset() { 60 | std::scoped_lock lock(m_mutex); 61 | m_IAccumulator = 0; 62 | m_pastError = 0; 63 | m_pastPV = 0; 64 | m_timer.stop(); 65 | m_timer.reset(); 66 | } 67 | 68 | float PID::PID::computePPath(float error) { 69 | // no mutex lock needed since inside compute() only 70 | return error * m_PGain; 71 | } 72 | 73 | float PID::PID::computeDPath(float deltaNumerator, float dt) { 74 | // no mutex lock needed since inside compute() only 75 | float derivativePath = 0; 76 | if (dt != 0) { 77 | derivativePath = m_DGain * deltaNumerator / dt; 78 | } 79 | return derivativePath; 80 | } 81 | 82 | float PID::PID::compute(float setPoint, float processVariable, float ff) { 83 | std::scoped_lock lock(m_mutex); 84 | float error = setPoint - processVariable; 85 | if (std::abs(error) < m_deadzone) { 86 | error = 0; 87 | } 88 | 89 | m_timer.stop(); 90 | float dt = chrono::duration_cast>(m_timer.elapsed_time()).count(); // seconds 91 | float paths = ff; 92 | paths += computePPath(error); 93 | m_IAccumulator += error * dt * m_IGain; 94 | paths += m_IAccumulator; 95 | paths += computeDPath(m_antiKickback ? processVariable - m_pastPV : error - m_pastError, dt); 96 | 97 | if (m_antiWindup) { 98 | if (paths > m_upperBound) { 99 | m_IAccumulator -= paths - m_upperBound; 100 | paths = m_upperBound; 101 | } else if (paths < m_lowerBound) { 102 | m_IAccumulator += m_lowerBound - paths; 103 | paths = m_lowerBound; 104 | } 105 | } else { 106 | paths = std::clamp(paths, m_lowerBound, m_upperBound); // clamp without affecting accumulator 107 | } 108 | 109 | m_pastError = error; 110 | m_pastPV = processVariable; 111 | m_timer.reset(); 112 | m_timer.start(); 113 | return paths; 114 | } 115 | -------------------------------------------------------------------------------- /libs/sensors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(Sensor INTERFACE) 2 | target_include_directories(Sensor INTERFACE include) 3 | target_link_libraries(Sensor 4 | INTERFACE 5 | mbed-os) 6 | 7 | add_library(AdafruitSTEMMA STATIC) 8 | target_sources(AdafruitSTEMMA PRIVATE src/AdafruitSTEMMA.cpp) 9 | target_include_directories(AdafruitSTEMMA PUBLIC include) 10 | target_link_libraries(AdafruitSTEMMA 11 | PRIVATE 12 | Sensor 13 | mbed-os 14 | ) 15 | 16 | add_library(CurrentSensor STATIC) 17 | target_sources(CurrentSensor PRIVATE src/CurrentSensor.cpp) 18 | target_include_directories(CurrentSensor PUBLIC include) 19 | target_link_libraries(CurrentSensor 20 | PRIVATE 21 | Sensor 22 | mbed-os 23 | ) 24 | add_library(PollingSensors STATIC) 25 | target_sources(PollingSensors PRIVATE src/PollingSensors.cpp) 26 | target_include_directories(PollingSensors PUBLIC include) 27 | target_link_libraries(PollingSensors 28 | PRIVATE 29 | Sensor 30 | mbed-os 31 | Logger 32 | ) 33 | 34 | -------------------------------------------------------------------------------- /libs/sensors/include/AdafruitSTEMMA.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Sensor.h" 4 | #include "mbed.h" 5 | 6 | namespace Sensor { 7 | class AdafruitSTEMMA final : public Sensor { 8 | public: 9 | typedef struct Config { 10 | PinName sda, scl; 11 | } Config; 12 | 13 | AdafruitSTEMMA(PinName sda, PinName scl); 14 | AdafruitSTEMMA(const Config &config); 15 | AdafruitSTEMMA(AdafruitSTEMMA &) = delete; 16 | AdafruitSTEMMA(AdafruitSTEMMA &&) = delete; 17 | ~AdafruitSTEMMA() = default; 18 | 19 | /* returns internal humidity value */ 20 | float read() override; 21 | 22 | /* returns internal temperature value */ 23 | float alternateRead() override; 24 | 25 | [[nodiscard]] bool reset() override; 26 | 27 | // reads the HW ID and checks that it is correct 28 | bool getStatus() const override; 29 | 30 | /* Updates internal humidity and temperature variables after issuing reads */ 31 | [[nodiscard]] bool update() override; 32 | 33 | private: 34 | mutable I2C m_i2c; 35 | mutable Mutex m_mutex; 36 | 37 | bool updateHumidity(); 38 | bool updateTemperature(); 39 | 40 | float m_humidity = 0, m_temperature = 0; 41 | 42 | static constexpr int Sensor_I2C_Address = 43 | 0x36 << 1; // MBED I2C uses 8 bit addressing, so addresses are left shifted by 1 (may need to be shifted by 2) 44 | 45 | static constexpr int Sensor_Status_Base = 0x00; // Base address registers for different modules 46 | static constexpr int Sensor_Moisture_Base = 0x0F; 47 | 48 | static constexpr int Sensor_Status_HW_ID = 0x01; // Function address register for the sensor's HW ID 49 | static constexpr int Sensor_HW_ID_Code = 0x55; // Expected value for sensor HW ID 50 | 51 | static constexpr int Sensor_Moisture_Function = 0x10; // Function address registers for various modules 52 | static constexpr int Sensor_Temp_Function = 0x04; 53 | static constexpr int Sensor_Status_Reset = 0x7F; 54 | }; 55 | } // namespace Sensor 56 | -------------------------------------------------------------------------------- /libs/sensors/include/CurrentSensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Sensor.h" 4 | #include "mbed.h" 5 | 6 | namespace Sensor { 7 | // TODO: rename current sensor class to technical name of sensor (ie. AdafruitSTEMMA) 8 | class CurrentSensor final : Sensor { 9 | public: 10 | CurrentSensor(PinName clk, PinName miso, PinName cs = NC); 11 | float read() override; 12 | float alternateRead() override; 13 | bool getStatus() const override; 14 | [[nodiscard]] bool reset() override; 15 | [[nodiscard]] bool update() override; 16 | }; 17 | } // namespace Sensor 18 | -------------------------------------------------------------------------------- /libs/sensors/include/PollingSensors.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Sensor.h" 4 | #include "mbed.h" 5 | 6 | namespace Sensor { 7 | class PollingSensors final : Sensor { 8 | public: 9 | PollingSensors(PinName moisture_in, PinName co2_in); 10 | 11 | float read() override {} 12 | 13 | float alternateRead() override {} 14 | 15 | bool getStatus() const override {} 16 | 17 | [[nodiscard]] bool reset() override {} 18 | 19 | [[nodiscard]] bool update() override {} 20 | 21 | /* Functions specific to sensor modules*/ 22 | 23 | /* This function returns Moisture sensor value 24 | * 0-300 - dry 25 | * 300-700 - humid 26 | * 700-900 - wet 27 | * Returns -1 for fault 28 | * */ 29 | float moisture_monitoring(); 30 | 31 | /* This function returns CO2 sensor value in ppm\ 32 | * Returns -1 for fault and 0 for preheating 33 | * */ 34 | float C02_monitoring(); 35 | 36 | private: 37 | /*Pins configuration for moisture sensing*/ 38 | AnalogIn m_moisture_in_adc; 39 | 40 | /*Pins configuration for CO2 sensing*/ 41 | AnalogIn m_CO2_in_adc; 42 | }; 43 | } // namespace Sensor -------------------------------------------------------------------------------- /libs/sensors/include/Sensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace Sensor { 4 | class Sensor { 5 | public: 6 | virtual float read() = 0; 7 | virtual float alternateRead() = 0; 8 | virtual bool getStatus() const = 0; 9 | [[nodiscard]] virtual bool reset() = 0; 10 | [[nodiscard]] virtual bool update() = 0; 11 | virtual ~Sensor(){}; 12 | }; 13 | } // namespace Sensor 14 | -------------------------------------------------------------------------------- /libs/sensors/src/AdafruitSTEMMA.cpp: -------------------------------------------------------------------------------- 1 | #include "AdafruitSTEMMA.h" 2 | 3 | #include 4 | 5 | using namespace Sensor; 6 | using namespace std::chrono_literals; 7 | 8 | AdafruitSTEMMA::AdafruitSTEMMA(PinName sda, PinName scl) : m_i2c(sda, scl) {} 9 | 10 | AdafruitSTEMMA::AdafruitSTEMMA(const Config &config) : m_i2c(config.sda, config.scl) {} 11 | 12 | bool AdafruitSTEMMA::reset() { 13 | std::scoped_lock lock(m_mutex); 14 | char cmd[3]; 15 | cmd[0] = Sensor_Status_Base; // initialize registers for clearing sensor memory 16 | cmd[1] = Sensor_Status_Reset; 17 | cmd[2] = 0xFF; 18 | 19 | m_i2c.write(Sensor_I2C_Address, cmd, 3); // set all registers on sensor to default values 20 | 21 | return true; 22 | } 23 | 24 | bool AdafruitSTEMMA::getStatus() const { 25 | std::scoped_lock lock(m_mutex); 26 | char cmd[2]; 27 | cmd[0] = Sensor_Status_Base; 28 | cmd[1] = Sensor_Status_HW_ID; 29 | 30 | char check[1]; 31 | 32 | m_i2c.write(Sensor_I2C_Address, cmd, 2); // initialize registers for checking device ID 33 | ThisThread::sleep_for(125ms); 34 | m_i2c.read(Sensor_I2C_Address, check, 1); // read device ID 35 | 36 | return (check[0] == Sensor_HW_ID_Code); // compare received HW ID Code to correct one 37 | } 38 | 39 | bool AdafruitSTEMMA::update() { 40 | std::scoped_lock lock(m_mutex); 41 | bool humidity_read_success = updateHumidity(); 42 | bool temp_read_success = updateTemperature(); 43 | return humidity_read_success && temp_read_success; 44 | } 45 | 46 | float AdafruitSTEMMA::read() { 47 | std::scoped_lock lock(m_mutex); 48 | return m_humidity; 49 | } 50 | 51 | float AdafruitSTEMMA::alternateRead() { 52 | std::scoped_lock lock(m_mutex); 53 | return m_temperature; 54 | } 55 | 56 | // read moisture reading of device 57 | bool AdafruitSTEMMA::updateHumidity() { 58 | if (!(getStatus())) { // checks if device is initialized, returns false if there is an issue 59 | return false; 60 | } 61 | 62 | char cmd[2]; 63 | cmd[0] = Sensor_Moisture_Base; 64 | cmd[1] = Sensor_Moisture_Function; 65 | 66 | char buf[2]; 67 | 68 | float sensorReading = 65535; 69 | 70 | uint8_t counter = 2; // initialize counter to break out of loop if reading isn't working (prevent infinite looping) 71 | 72 | do { 73 | ThisThread::sleep_for(1ms); 74 | m_i2c.write(Sensor_I2C_Address, cmd, 2); // initialize registers for reading moisture 75 | ThisThread::sleep_for(1s); 76 | m_i2c.read(Sensor_I2C_Address, buf, 2); // read moisture 77 | 78 | sensorReading = (static_cast(buf[0]) << 8 | buf[1]); // concatenate bytes together 79 | 80 | counter--; 81 | } while (sensorReading == 65535 && counter != 0); // repeat until value has been measured, or until loop has run 10 82 | // times (breaks out regardless of if read works or not) 83 | bool success = sensorReading != 65535; 84 | if (success) { 85 | m_humidity = sensorReading; 86 | } 87 | return success; 88 | } 89 | 90 | // read temperature of device 91 | bool AdafruitSTEMMA::updateTemperature() { 92 | if (!(getStatus())) { // checks if device is initialized, returns false if there is an issue 93 | return false; 94 | } 95 | 96 | char cmd[2]; 97 | cmd[0] = Sensor_Status_Base; 98 | cmd[1] = Sensor_Temp_Function; 99 | 100 | char buf[4]; 101 | 102 | m_i2c.write(Sensor_I2C_Address, cmd, 2); // initialize registers for reading temperature 103 | ThisThread::sleep_for(1s); 104 | m_i2c.read(Sensor_I2C_Address, buf, 4); // read temp 105 | 106 | float sensorReading = (static_cast(buf[0]) << 24) | 107 | (static_cast(buf[1]) << 16) | // concatenate bytes together 108 | (static_cast(buf[2]) << 8) | static_cast(buf[3]); 109 | sensorReading = sensorReading * (1.0 / (1UL << 16)); 110 | 111 | m_temperature = sensorReading; 112 | 113 | return true; 114 | } 115 | -------------------------------------------------------------------------------- /libs/sensors/src/CurrentSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "CurrentSensor.h" 2 | 3 | using namespace Sensor; 4 | 5 | CurrentSensor::CurrentSensor(PinName clk, PinName miso, PinName cs) { 6 | MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_APPLICATION, MBED_ERROR_CODE_UNSUPPORTED), 7 | "CURRENT SENSOR CLASS IS UNIMPLEMENTED"); // todo 8 | } 9 | 10 | float CurrentSensor::read() { 11 | MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_APPLICATION, MBED_ERROR_CODE_UNSUPPORTED), 12 | "CURRENT SENSOR CLASS IS UNIMPLEMENTED"); // todo 13 | return -1; 14 | } 15 | 16 | float CurrentSensor::alternateRead() { 17 | MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_APPLICATION, MBED_ERROR_CODE_UNSUPPORTED), 18 | "CURRENT SENSOR CLASS IS UNIMPLEMENTED"); // todo 19 | return -1; 20 | } 21 | 22 | bool CurrentSensor::getStatus() const { 23 | MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_APPLICATION, MBED_ERROR_CODE_UNSUPPORTED), 24 | "CURRENT SENSOR CLASS IS UNIMPLEMENTED"); // todo 25 | return false; 26 | } 27 | 28 | bool CurrentSensor::reset() { 29 | MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_APPLICATION, MBED_ERROR_CODE_UNSUPPORTED), 30 | "CURRENT SENSOR CLASS IS UNIMPLEMENTED"); // todo 31 | return false; 32 | } 33 | 34 | bool CurrentSensor::update() { 35 | MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_APPLICATION, MBED_ERROR_CODE_UNSUPPORTED), 36 | "CURRENT SENSOR CLASS IS UNIMPLEMENTED"); // todo 37 | return false; 38 | } -------------------------------------------------------------------------------- /libs/sensors/src/PollingSensors.cpp: -------------------------------------------------------------------------------- 1 | #include "PollingSensors.h" 2 | 3 | #include "Logger.h" 4 | 5 | using namespace Sensor; 6 | 7 | PollingSensors::PollingSensors(PinName moisture_in, PinName co2_in) 8 | : m_moisture_in_adc(moisture_in), m_CO2_in_adc(co2_in) {} 9 | 10 | /*https://www.dfrobot.com/product-599.html*/ 11 | float PollingSensors::moisture_monitoring() { 12 | float moisture_reading = (m_moisture_in_adc.read_voltage() * 950) / 4.2; 13 | 14 | if (moisture_reading < 0 && moisture_reading > 950) { 15 | Utility::logger << "!!! MOISTURE FAULT:" << moisture_reading << "\n"; 16 | return -1; 17 | } 18 | return moisture_reading; 19 | } 20 | 21 | /*https://www.dfrobot.com/product-1549.html*/ 22 | float PollingSensors::C02_monitoring() { 23 | float CO2_reading = m_CO2_in_adc.read_voltage() * 1000; 24 | 25 | if (CO2_reading <= 0) { 26 | Utility::logger << "!!! CO2 FAULT:" << CO2_reading << "\n"; 27 | return -1; 28 | } else if (CO2_reading < 400) { 29 | Utility::logger << "!!! CO2 PREHEATING:" << CO2_reading << "\n"; 30 | return 0; 31 | } 32 | float concentration = ((CO2_reading - 400) * 50.0) / 16.0; 33 | return concentration; 34 | } 35 | -------------------------------------------------------------------------------- /libs/utility/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(Logger STATIC) 2 | target_sources(Logger PRIVATE src/Logger.cpp) 3 | target_include_directories(Logger PUBLIC include) 4 | target_link_libraries(Logger PUBLIC mbed-os) 5 | 6 | add_library(WatchdogWrapper STATIC) 7 | target_sources(WatchdogWrapper PRIVATE src/WatchdogWrapper.cpp) 8 | target_include_directories(WatchdogWrapper PUBLIC include) 9 | target_link_libraries(WatchdogWrapper PUBLIC Logger mbed-os) 10 | 11 | add_library(LookupTable INTERFACE) 12 | target_include_directories(LookupTable INTERFACE include) 13 | 14 | add_library(Units INTERFACE) 15 | target_include_directories(Units INTERFACE include) 16 | -------------------------------------------------------------------------------- /libs/utility/include/Logger.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "mbed.h" 7 | #include "uwrt_config_target.h" 8 | 9 | namespace Utility { 10 | 11 | class Logger; 12 | extern Logger logger; 13 | 14 | class Logger : public FileHandle { 15 | public: 16 | Logger(); 17 | 18 | template 19 | Logger& operator<<(T out) { 20 | std::scoped_lock lock(m_mutex); 21 | 22 | std::stringstream ss; 23 | ss << out; 24 | 25 | write(static_cast(ss.str().c_str()), ss.str().length()); 26 | 27 | return *this; 28 | } 29 | 30 | // FileHandle overrides 31 | ssize_t read(void* buffer, size_t size) override; 32 | ssize_t write(const void* buffer, size_t size) override; 33 | off_t seek(off_t offset, int whence = SEEK_SET) override; 34 | int close() override; 35 | off_t size() override; 36 | 37 | private: 38 | static Mutex m_mutex; 39 | }; 40 | 41 | } // namespace Utility 42 | -------------------------------------------------------------------------------- /libs/utility/include/LookupTable.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This is a non-mutable data-type based off a std::unordered_map 3 | * See test-lookup-table for usage 4 | */ 5 | 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | 11 | namespace Utility { 12 | namespace internal { 13 | struct UnusedDefaultValue {}; 14 | } // namespace internal 15 | template , typename KeyEqual = std::equal_to, 17 | typename Allocator = std::allocator>> 18 | class LookupTable { 19 | private: 20 | const std::unordered_map m_map; 21 | 22 | public: 23 | using key_type = Key; 24 | using mapped_type = Value; 25 | using value_type = std::pair; 26 | using size_type = size_t; 27 | using difference_type = std::ptrdiff_t; 28 | using hasher = Hash; 29 | using key_equal = KeyEqual; 30 | using allocator_type = Allocator; 31 | using reference = value_type &; 32 | using const_reference = const value_type &; 33 | using pointer = typename std::allocator_traits::pointer; 34 | using const_pointer = typename std::allocator_traits::const_pointer; 35 | using const_iterator = typename std::unordered_map::const_iterator; 36 | 37 | LookupTable() = delete; 38 | 39 | // range based constructor 40 | template 41 | LookupTable(InputIterator first, InputIterator last) : m_map(first, last) {} 42 | // copy constructor and assignment 43 | LookupTable(const LookupTable &) = default; 44 | LookupTable &operator=(const LookupTable &) = default; 45 | // move constructor and assignment 46 | LookupTable(LookupTable &&) = default; 47 | LookupTable &operator=(LookupTable &&) = default; 48 | // initializer list constructor 49 | LookupTable(std::initializer_list init, size_type bucket_count = 0, const Hash &hash = Hash(), 50 | const key_equal &equal = key_equal(), const Allocator &alloc = Allocator()) 51 | : m_map(init, bucket_count, hash, equal, alloc) {} 52 | 53 | const_iterator begin() const { 54 | return m_map.begin(); 55 | } 56 | 57 | const_iterator end() const { 58 | return m_map.end(); 59 | } 60 | 61 | std::size_t size() const { 62 | return m_map.size(); 63 | } 64 | std::size_t max_size() const { 65 | return m_map.max_size(); 66 | } 67 | 68 | bool empty() const { 69 | return m_map.empty(); 70 | } 71 | 72 | bool contains(const Key &key) const { 73 | return m_map.contains(key); 74 | } 75 | 76 | const_iterator find(const Key &key) const { 77 | return m_map.find(key); 78 | } 79 | 80 | template 81 | typename std::enable_if::value, V>::type at(const Key &key) const { 82 | bool value_exists = m_map.find(key) != m_map.end(); 83 | return value_exists ? m_map.at(key) : defaultValue; 84 | } 85 | template 86 | typename std::enable_if::value, std::optional>::type at( 87 | const Key &key) const { 88 | bool value_exists = m_map.find(key) != m_map.end(); 89 | return value_exists ? std::optional{m_map.at(key)} : std::nullopt; 90 | } 91 | 92 | template 93 | typename std::enable_if::value, V>::type operator[](const Key &key) const { 94 | return at(key); 95 | } 96 | template 97 | typename std::enable_if::value, std::optional>::type operator[]( 98 | const Key &key) const { 99 | return at(key); 100 | } 101 | }; 102 | } // namespace Utility 103 | -------------------------------------------------------------------------------- /libs/utility/include/WatchdogWrapper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "mbed.h" 3 | 4 | namespace Utility { 5 | 6 | class WatchdogWrapper { 7 | public: 8 | static void startWatchdog(std::chrono::milliseconds countdown_ms); 9 | static void logResetReason(void); 10 | static void petWatchdog(void); 11 | }; 12 | } // namespace Utility -------------------------------------------------------------------------------- /libs/utility/src/Logger.cpp: -------------------------------------------------------------------------------- 1 | #include "Logger.h" 2 | 3 | #include "hal/itm_api.h" 4 | #include "hal/serial_api.h" 5 | 6 | extern serial_t stdio_uart; 7 | 8 | namespace mbed { 9 | // Redirect stdin, stdout, and stderr to Logger 10 | FileHandle *mbed_target_override_console(int) { 11 | static Utility::Logger logger; 12 | return &logger; 13 | } 14 | } // namespace mbed 15 | 16 | namespace Utility { 17 | 18 | Logger logger; 19 | Mutex Logger::m_mutex; 20 | 21 | Logger::Logger() { 22 | if constexpr (FeatureFlags::UART_LOGGING) { 23 | serial_init(&stdio_uart, STDIO_UART_TX, STDIO_UART_RX); 24 | } 25 | } 26 | 27 | ssize_t Logger::write(const void *buffer, size_t size) { 28 | std::scoped_lock lock(m_mutex); 29 | 30 | if constexpr (FeatureFlags::UART_LOGGING) { 31 | const char *c = static_cast(buffer); 32 | for (size_t i = 0; i < size; i++) { 33 | serial_putc(&stdio_uart, *c++); 34 | } 35 | } 36 | 37 | if constexpr (FeatureFlags::SWO_LOGGING) { 38 | const char *c = static_cast(buffer); 39 | for (size_t i = 0; i < size; i++) { 40 | ITM_SendChar(*c++); 41 | } 42 | } 43 | 44 | return size; 45 | } 46 | 47 | ssize_t Logger::read(void *buffer, size_t size) { 48 | // Reading is not supported by this file handle 49 | return -EBADF; 50 | } 51 | 52 | off_t Logger::seek(off_t offset, int whence) { 53 | // Seeking is not support by this file handler 54 | return -ESPIPE; 55 | } 56 | 57 | off_t Logger::size() { 58 | // Size is not defined for this file handle 59 | return -EINVAL; 60 | } 61 | 62 | int Logger::close() { 63 | return 0; 64 | } 65 | 66 | } // namespace Utility 67 | -------------------------------------------------------------------------------- /libs/utility/src/WatchdogWrapper.cpp: -------------------------------------------------------------------------------- 1 | #include "WatchdogWrapper.h" 2 | 3 | #include "Logger.h" 4 | #include "ResetReason.h" 5 | #include "mbed.h" 6 | 7 | namespace Utility { 8 | 9 | void WatchdogWrapper::startWatchdog(std::chrono::milliseconds countdown_ms) { 10 | uint32_t countdown_uint32 = countdown_ms.count(); 11 | Watchdog &watchdog = Watchdog::get_instance(); 12 | watchdog.start(countdown_uint32); 13 | } 14 | 15 | void WatchdogWrapper::logResetReason(void) { 16 | const reset_reason_t reason = ResetReason::get(); 17 | if (reason == RESET_REASON_WATCHDOG) { 18 | time_t seconds = time(NULL); 19 | // Ideally we would want an accurate time and date for the timestamp, but without a reference point 20 | // the best we can do is the time since the microcontroller was powered on 21 | Utility::logger << "Reset Reason: Watchog Timer - Timestamp: " << seconds << " seconds since power on\r\n"; 22 | } 23 | } 24 | 25 | void WatchdogWrapper::petWatchdog(void) { 26 | Watchdog::get_instance().kick(); 27 | } 28 | } // namespace Utility 29 | -------------------------------------------------------------------------------- /mbed-os.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uwrobotics/MarsRoverFirmware/d513dd11758f9dfc5831bb1b7503cb9aecd69256/mbed-os.lib -------------------------------------------------------------------------------- /mbed_app.json: -------------------------------------------------------------------------------- 1 | { 2 | "target_overrides": { 3 | "*": { 4 | "platform.default-serial-baud-rate": 115200, 5 | "platform.stdio-baud-rate": 115200, 6 | "platform.stdio-buffered-serial": true, 7 | "target.default-adc-vref": "3.3f" 8 | } 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /misc/NucleoSWDLabels.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uwrobotics/MarsRoverFirmware/d513dd11758f9dfc5831bb1b7503cb9aecd69256/misc/NucleoSWDLabels.png -------------------------------------------------------------------------------- /rover-apps/arm_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(arm_2021) 2 | target_sources(arm_2021 PRIVATE ${ROVER_APPS_DIR}/common/src/main.cpp) 3 | target_include_directories(arm_2021 PUBLIC include ${ROVER_APPS_DIR}/common/include) 4 | target_link_libraries(arm_2021 5 | PRIVATE 6 | #Control 7 | OpenLoopController 8 | PositionController 9 | VelocityController 10 | CurrentController 11 | ActuatorControllerManager 12 | #Motors 13 | LimServo 14 | DCMotor 15 | #Encoders 16 | EncoderAEAT6012 17 | EncoderMAE3 18 | Quadrature64CPR 19 | #CAN 20 | CANInterface 21 | CANMsg 22 | #Sensor 23 | CurrentSensor 24 | #common-modules 25 | WatchdogModule 26 | #Other 27 | uwrt-mars-rover-hw-bridge 28 | Logger 29 | #Mbed 30 | mbed-os 31 | mbed-events 32 | ) 33 | mbed_set_post_build(arm_2021) 34 | -------------------------------------------------------------------------------- /rover-apps/arm_2021/include/AppConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Module.h" 6 | #include "WatchdogModule.h" 7 | 8 | WatchdogModule arm_watchdog; 9 | 10 | std::vector gModules = { 11 | &arm_watchdog, 12 | }; 13 | -------------------------------------------------------------------------------- /rover-apps/arm_2021/include/ClawConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "ActuatorControllerManager.h" 6 | #include "Current.h" 7 | #include "DCMotor.h" 8 | #include "LookupTable.h" 9 | #include "OpenLoop.h" 10 | #include "PID.h" 11 | #include "Position.h" 12 | #include "Quadrature64CPR.h" 13 | #include "Velocity.h" 14 | #include "hw_bridge.h" 15 | 16 | namespace Claw { 17 | 18 | namespace Internal { 19 | 20 | static Encoder::Quadrature64CPR encoder({ENC_QUAD_CLAW_A, ENC_QUAD_CLAW_B, NC, 0}); 21 | 22 | static Actuator::DCMotor motor(MTR_PWM_TRNTBL, MTR_DIR_TRNTBL, false); 23 | 24 | // static Sensor::CurrentSensor currentSensor(CLAW_CRNT_SNS_SPI_CLK, CLAW_CRNT_SNS_SPI_MISO, CLAW_CRNT_SNS_SPI_CS); 25 | // TODO: Add once current sensor driver works 26 | 27 | static PID::PID velPID({1, 0, 0, -1, 1, 0, false, false}); 28 | static PID::PID posPID({1, 0, 0, -1, 1, 0, false, false}); 29 | static PID::PID curPID({1, 0, 0, -1, 1, 0, false, false}); 30 | 31 | constexpr float POLOLUMAXCURRENT = 3; 32 | constexpr float POLOLUMADEGPERSEC = 33 | std::numeric_limits::infinity(); // TODO: figure out MAXDEGPERSEC of motors (1680?); 34 | 35 | static Controller::Velocity vel(motor, encoder, std::nullopt, velPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, 36 | LIM_CLAW_OPEN, NC); 37 | static Controller::Position pos(motor, encoder, std::nullopt, posPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, 38 | LIM_CLAW_OPEN, NC); 39 | // static Controller::Current cur(motor, encoder, std::nullopt, curPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, 40 | // LIM_CLAW_OPEN, 41 | // NC); 42 | static Controller::OpenLoop open(motor, encoder, std::nullopt, POLOLUMADEGPERSEC, POLOLUMAXCURRENT, LIM_CLAW_OPEN, NC); 43 | 44 | static const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, 45 | {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, 46 | // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, 47 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; 48 | } // namespace Internal 49 | 50 | static Controller::ActuatorControllerManager manager(Internal::lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 51 | 52 | } // namespace Claw 53 | -------------------------------------------------------------------------------- /rover-apps/arm_2021/include/ElbowConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "AEAT6012.h" 6 | #include "ActuatorControllerManager.h" 7 | #include "Current.h" 8 | #include "DCMotor.h" 9 | #include "LookupTable.h" 10 | #include "OpenLoop.h" 11 | #include "PID.h" 12 | #include "Position.h" 13 | #include "Velocity.h" 14 | 15 | namespace Elbow { 16 | 17 | namespace Internal { 18 | 19 | static Encoder::AEAT6012 encoder({ELBW_ENC_SPI_CLK, ELBW_ENC_SPI_MISO, NC, 0}); 20 | 21 | static Actuator::DCMotor motor(MTR_PWM_ELBW, MTR_DIR_ELBW, false); 22 | 23 | // static Sensor::CurrentSensor currentSensor(ELBW_CRNT_SNS_SPI_CLK, ELBW_CRNT_SNS_SPI_MISO, ELBW_CRNT_SNS_SPI_CS); 24 | // TODO: Add once current sensor driver works 25 | 26 | static PID::PID velPID({1, 0, 0, -1, 1, 0, false, false}); 27 | static PID::PID posPID({1, 0, 0, -1, 1, 0, false, false}); 28 | static PID::PID curPID({1, 0, 0, -1, 1, 0, false, false}); 29 | 30 | constexpr float ASSUNMAXCURRENT = 25.263; 31 | constexpr float ASSUNMAXDEGPERSEC = 32 | std::numeric_limits::infinity(); // TODO: figure out MAXDEGPERSEC of motors (35580?); 33 | 34 | static Controller::Velocity vel(motor, encoder, std::nullopt, velPID, ASSUNMAXDEGPERSEC, ASSUNMAXCURRENT, LIM_ELBW_DN, 35 | LIM_ELBW_UP); 36 | static Controller::Position pos(motor, encoder, std::nullopt, posPID, ASSUNMAXDEGPERSEC, ASSUNMAXCURRENT, LIM_ELBW_DN, 37 | LIM_ELBW_UP); 38 | // static Controller::Current cur(motor, encoder, std::nullopt, curPID, ASSUNMAXDEGPERSEC, ASSUNMAXCURRENT, LIM_ELBW_DN, 39 | // LIM_ELBW_UP); 40 | static Controller::OpenLoop open(motor, encoder, std::nullopt, ASSUNMAXDEGPERSEC, ASSUNMAXCURRENT, LIM_ELBW_DN, 41 | LIM_ELBW_UP); 42 | 43 | static const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, 44 | {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, 45 | // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, 46 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; 47 | } // namespace Internal 48 | 49 | static Controller::ActuatorControllerManager manager(Internal::lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 50 | 51 | } // namespace Elbow 52 | -------------------------------------------------------------------------------- /rover-apps/arm_2021/include/ShoulderConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "AEAT6012.h" 6 | #include "ActuatorControllerManager.h" 7 | #include "Current.h" 8 | #include "DCMotor.h" 9 | #include "LookupTable.h" 10 | #include "OpenLoop.h" 11 | #include "PID.h" 12 | #include "Position.h" 13 | #include "Velocity.h" 14 | 15 | namespace Shoulder { 16 | 17 | namespace Internal { 18 | 19 | static Encoder::AEAT6012 encoder({SHLDR_ENC_SPI_CLK, SHLDR_ENC_SPI_MISO, SHLDR_ENC_SPI_CS, 0}); 20 | 21 | static Actuator::DCMotor motor(MTR_PWM_SHLDR, MTR_DIR_SHLDR, false); 22 | 23 | // static Sensor::CurrentSensor currentSensor(ELBW_CRNT_SNS_SPI_CLK, ELBW_CRNT_SNS_SPI_MISO, ELBW_CRNT_SNS_SPI_CS); 24 | // TODO: Add once current sensor driver works 25 | 26 | static PID::PID velPID({1, 0, 0, -1, 1, 0, false, false}); 27 | static PID::PID posPID({1, 0, 0, -1, 1, 0, false, false}); 28 | static PID::PID curPID({1, 0, 0, -1, 1, 0, false, false}); 29 | 30 | constexpr uint8_t PA04MAXCURRENT = 6; 31 | constexpr float PA04MAXDEGPERSEC = std::numeric_limits::infinity(); // TODO: figure out MAXDEGPERSEC of motors 32 | 33 | static Controller::Velocity vel(motor, encoder, std::nullopt, velPID, PA04MAXDEGPERSEC, PA04MAXCURRENT, LIM_SHLDR_DN, 34 | LIM_SHLDR_UP); 35 | static Controller::Position pos(motor, encoder, std::nullopt, posPID, PA04MAXDEGPERSEC, PA04MAXCURRENT, LIM_SHLDR_DN, 36 | LIM_SHLDR_UP); 37 | // static Controller::Current cur(motor, encoder, std::nullopt, curPID, PA04MAXDEGPERSEC, PA04MAXCURRENT, LIM_SHLDR_DN, 38 | // LIM_SHLDR_UP); 39 | static Controller::OpenLoop open(motor, encoder, std::nullopt, PA04MAXDEGPERSEC, PA04MAXCURRENT, LIM_SHLDR_DN, 40 | LIM_SHLDR_UP); 41 | 42 | static const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, 43 | {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, 44 | //{HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, 45 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; 46 | } // namespace Internal 47 | 48 | static Controller::ActuatorControllerManager manager(Internal::lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 49 | 50 | } // namespace Shoulder 51 | -------------------------------------------------------------------------------- /rover-apps/arm_2021/include/TooltipConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "LimServo.h" 4 | 5 | namespace Tooltip { 6 | static Actuator::LimServo clawTooltipServo(SRVO_PWM_TOOLTIP, 180.0, 2ms, 1ms); 7 | } // namespace Tooltip 8 | -------------------------------------------------------------------------------- /rover-apps/arm_2021/include/TurntableConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "AEAT6012.h" 4 | #include "ActuatorControllerManager.h" 5 | #include "Current.h" 6 | #include "DCMotor.h" 7 | #include "LookupTable.h" 8 | #include "OpenLoop.h" 9 | #include "PID.h" 10 | #include "Position.h" 11 | #include "Velocity.h" 12 | #include "hw_bridge.h" 13 | 14 | namespace Turntable { 15 | 16 | namespace Internal { 17 | 18 | static Encoder::AEAT6012 encoder({TRNTBL_ENC_SPI_CLK, TRNTBL_ENC_SPI_MISO, NC, 0}); 19 | 20 | static Actuator::DCMotor motor(MTR_PWM_TRNTBL, MTR_DIR_TRNTBL, false); 21 | 22 | // static Sensor::CurrentSensor currentSensor(TRNTBL_CRNT_SNS_SPI_CLK, TRNTBL_CRNT_SNS_SPI_MISO, 23 | // TRNTBL_CRNT_SNS_SPI_CS); 24 | // TODO: Add once current sensor driver works 25 | 26 | static PID::PID velPID({1, 0, 0, -1, 1, 0, false, false}); 27 | static PID::PID posPID({1, 0, 0, -1, 1, 0, false, false}); 28 | static PID::PID curPID({1, 0, 0, -1, 1, 0, false, false}); 29 | 30 | constexpr uint8_t MAXCURRENT = 53; 31 | constexpr float MAXDEGPERSEC = 32 | std::numeric_limits::infinity(); // TODO: figure out MAXDEGPERSEC of motors (79080?) 33 | 34 | static Controller::Velocity vel(motor, encoder, std::nullopt, velPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, 35 | LIM_TRNTBL_RHS); 36 | static Controller::Position pos(motor, encoder, std::nullopt, posPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, 37 | LIM_TRNTBL_RHS); 38 | // static Controller::Current cur(motor, encoder, std::nullopt, curPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, 39 | // LIM_TRNTBL_RHS); 40 | static Controller::OpenLoop open(motor, encoder, std::nullopt, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS, 41 | LIM_TRNTBL_RHS); 42 | 43 | static const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, 44 | {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, 45 | // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur}, 46 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; 47 | } // namespace Internal 48 | 49 | static Controller::ActuatorControllerManager manager(Internal::lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 50 | 51 | } // namespace Turntable 52 | -------------------------------------------------------------------------------- /rover-apps/arm_2021/include/WristConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "ActuatorControllerManager.h" 6 | #include "Current.h" 7 | #include "DCMotor.h" 8 | #include "LookupTable.h" 9 | #include "OpenLoop.h" 10 | #include "PID.h" 11 | #include "Position.h" 12 | #include "Quadrature64CPR.h" 13 | #include "Velocity.h" 14 | #include "hw_bridge.h" 15 | 16 | namespace Wrist { 17 | 18 | namespace Internal { 19 | 20 | static Encoder::Quadrature64CPR leftEncoder({ENC_QUAD_WRST_LHS_A, ENC_QUAD_WRST_LHS_B, NC, 0}); 21 | static Encoder::Quadrature64CPR rightEncoder({ENC_QUAD_WRST_RHS_A, ENC_QUAD_WRST_RHS_B, NC, 0}); 22 | 23 | static Actuator::DCMotor leftMotor(MTR_PWM_WRST_LHS, MTR_DIR_WRST_LHS, false); 24 | static Actuator::DCMotor rightMotor(MTR_PWM_WRST_RHS, MTR_DIR_WRST_RHS, false); 25 | 26 | /* TODO: Can we use wrist limit switches if motors are individually addressable */ 27 | 28 | // static Sensor::CurrentSensor leftCurrentSensor(WRSTL_CRNT_SNS_SPI_CLK, WRSTL_CRNT_SNS_SPI_MISO, 29 | // WRSTL_CRNT_SNS_SPI_CS), 30 | // rightCurrentSensor(WRSTR_CRNT_SNS_SPI_CLK, WRSTR_CRNT_SNS_SPI_MISO, WRSTR_CRNT_SNS_SPI_CS); 31 | // TODO: Add once current sensor driver works 32 | 33 | static PID::PID leftVelPID({1, 0, 0, -1, 1, 0, false, false}); 34 | static PID::PID leftPosPID({1, 0, 0, -1, 1, 0, false, false}); 35 | static PID::PID leftCurPID({1, 0, 0, -1, 1, 0, false, false}); 36 | 37 | static PID::PID rightVelPID({1, 0, 0, -1, 1, 0, false, false}); 38 | static PID::PID rightPosPID({1, 0, 0, -1, 1, 0, false, false}); 39 | static PID::PID rightCurPID({1, 0, 0, -1, 1, 0, false, false}); 40 | 41 | constexpr uint8_t POLOLUMAXCURRENT = 3; 42 | constexpr float POLOLUMAXDEGPERSEC = 43 | std::numeric_limits::infinity(); // TODO: figure out MAXDEGPERSEC of motors (1680?) 44 | 45 | static Controller::Velocity leftVel(leftMotor, leftEncoder, std::nullopt, leftVelPID, POLOLUMAXDEGPERSEC, 46 | POLOLUMAXCURRENT, NC, NC); 47 | static Controller::Position leftPos(leftMotor, leftEncoder, std::nullopt, leftPosPID, POLOLUMAXDEGPERSEC, 48 | POLOLUMAXCURRENT, NC, NC); 49 | // static Controller::Current leftCur(leftMotor, leftEncoder, std::nullopt, leftCurPID, POLOLUMAXDEGPERSEC, 50 | // POLOLUMAXCURRENT, NC, NC); 51 | static Controller::OpenLoop leftOpen(leftMotor, leftEncoder, std::nullopt, POLOLUMAXDEGPERSEC, POLOLUMAXCURRENT, NC, 52 | NC); 53 | 54 | static Controller::Velocity rightVel(rightMotor, rightEncoder, std::nullopt, rightVelPID, POLOLUMAXDEGPERSEC, 55 | POLOLUMAXCURRENT, NC, NC); 56 | static Controller::Position rightPos(rightMotor, rightEncoder, std::nullopt, rightPosPID, POLOLUMAXDEGPERSEC, 57 | POLOLUMAXCURRENT, NC, NC); 58 | // static Controller::Current rightCur(rightMotor, rightEncoder, std::nullopt, rightCurPID, POLOLUMAXDEGPERSEC, 59 | // POLOLUMAXCURRENT, NC, NC); 60 | static Controller::OpenLoop rightOpen(rightMotor, rightEncoder, std::nullopt, POLOLUMAXDEGPERSEC, POLOLUMAXCURRENT, NC, 61 | NC); 62 | 63 | static const Controller::ControlMap leftLut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &leftVel}, 64 | {HWBRIDGE::CONTROL::Mode::POSITION, &leftPos}, 65 | // {HWBRIDGE::CONTROL::Mode::CURRENT, &leftCur}, 66 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &leftOpen}}; 67 | static const Controller::ControlMap rightLut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &rightVel}, 68 | {HWBRIDGE::CONTROL::Mode::POSITION, &rightPos}, 69 | // {HWBRIDGE::CONTROL::Mode::CURRENT, &rightCur}, 70 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &rightOpen}}; 71 | } // namespace Internal 72 | 73 | static Controller::ActuatorControllerManager leftManager(Internal::leftLut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 74 | static Controller::ActuatorControllerManager rightManager(Internal::rightLut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 75 | 76 | } // namespace Wrist 77 | -------------------------------------------------------------------------------- /rover-apps/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(WatchdogModule STATIC) 2 | target_sources(WatchdogModule PRIVATE src/WatchdogModule.cpp) 3 | target_include_directories(WatchdogModule PUBLIC include) 4 | target_link_libraries(WatchdogModule 5 | PRIVATE 6 | WatchdogWrapper 7 | mbed-os 8 | ) -------------------------------------------------------------------------------- /rover-apps/common/include/Module.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | class Module { 4 | public: 5 | virtual void periodic_10s(void) = 0; 6 | virtual void periodic_1s(void) = 0; 7 | virtual void periodic_100ms(void) = 0; 8 | virtual void periodic_10ms(void) = 0; 9 | virtual void periodic_1ms(void) = 0; 10 | }; 11 | -------------------------------------------------------------------------------- /rover-apps/common/include/WatchdogModule.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Module.h" 4 | #include "mbed.h" 5 | 6 | class WatchdogModule final : public Module { 7 | public: 8 | /* Initiates the watchdog with a countdown 9 | * @param countdown - max timeout of the watchdog 10 | * */ 11 | WatchdogModule(); 12 | 13 | // Periodic function to kick the watchdog and restart its timer every 1s 14 | void periodic_1s(void) override; 15 | 16 | void periodic_10s(void) override {} 17 | void periodic_100ms(void) override {} 18 | void periodic_10ms(void) override {} 19 | void periodic_1ms(void) override {} 20 | 21 | private: 22 | static const std::chrono::milliseconds WATCHDOG_DEFAULT_COUNTDOWN; 23 | }; 24 | -------------------------------------------------------------------------------- /rover-apps/common/src/WatchdogModule.cpp: -------------------------------------------------------------------------------- 1 | #include "WatchdogModule.h" 2 | 3 | #include "Module.h" 4 | #include "WatchdogWrapper.h" 5 | #include "mbed.h" 6 | 7 | const std::chrono::milliseconds WatchdogModule::WATCHDOG_DEFAULT_COUNTDOWN = 5000ms; 8 | 9 | WatchdogModule::WatchdogModule() { 10 | Utility::WatchdogWrapper::logResetReason(); 11 | Utility::WatchdogWrapper::startWatchdog(WATCHDOG_DEFAULT_COUNTDOWN); 12 | } 13 | 14 | void WatchdogModule::periodic_1s(void) { 15 | Utility::WatchdogWrapper::petWatchdog(); 16 | } 17 | -------------------------------------------------------------------------------- /rover-apps/common/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "AppConfig.h" 2 | #include "Logger.h" 3 | #include "mbed.h" 4 | 5 | Thread periodic_10s_thread(osPriorityHigh); 6 | Thread periodic_1s_thread(osPriorityHigh2); 7 | Thread periodic_100ms_thread(osPriorityHigh4); 8 | Thread periodic_10ms_thread(osPriorityHigh6); 9 | Thread periodic_1ms_thread(osPriorityRealtime); 10 | 11 | void periodic_10s(void) { 12 | auto startTime = Kernel::Clock::now(); 13 | for (Module* module : gModules) { 14 | module->periodic_10s(); 15 | } 16 | 17 | auto nextStartTime = startTime + 10s; 18 | if (Kernel::Clock::now() > nextStartTime) { 19 | Utility::logger << "Periodic 10s task failed to hit the deadline!\n"; 20 | } 21 | ThisThread::sleep_until(nextStartTime); 22 | } 23 | 24 | void periodic_1s(void) { 25 | auto startTime = Kernel::Clock::now(); 26 | for (Module* module : gModules) { 27 | module->periodic_1s(); 28 | } 29 | 30 | auto nextStartTime = startTime + 1s; 31 | if (Kernel::Clock::now() > nextStartTime) { 32 | Utility::logger << "Periodic 1s task failed to hit the deadline!\n"; 33 | } 34 | ThisThread::sleep_until(nextStartTime); 35 | } 36 | 37 | void periodic_100ms(void) { 38 | auto startTime = Kernel::Clock::now(); 39 | for (Module* module : gModules) { 40 | module->periodic_100ms(); 41 | } 42 | 43 | auto nextStartTime = startTime + 100ms; 44 | if (Kernel::Clock::now() > nextStartTime) { 45 | Utility::logger << "Periodic 100ms task failed to hit the deadline!\n"; 46 | } 47 | ThisThread::sleep_until(nextStartTime); 48 | } 49 | 50 | void periodic_10ms(void) { 51 | auto startTime = Kernel::Clock::now(); 52 | for (Module* module : gModules) { 53 | module->periodic_10ms(); 54 | } 55 | 56 | auto nextStartTime = startTime + 10ms; 57 | if (Kernel::Clock::now() > nextStartTime) { 58 | Utility::logger << "Periodic 10ms task failed to hit the deadline!\n"; 59 | } 60 | ThisThread::sleep_until(nextStartTime); 61 | } 62 | 63 | void periodic_1ms(void) { 64 | auto startTime = Kernel::Clock::now(); 65 | for (Module* module : gModules) { 66 | module->periodic_1ms(); 67 | } 68 | 69 | auto nextStartTime = startTime + 1ms; 70 | if (Kernel::Clock::now() > nextStartTime) { 71 | Utility::logger << "Periodic 1ms task failed to hit the deadline!\n"; 72 | } 73 | ThisThread::sleep_until(nextStartTime); 74 | } 75 | 76 | int main() { 77 | periodic_1ms_thread.start(periodic_1ms); 78 | periodic_10ms_thread.start(periodic_10ms); 79 | periodic_100ms_thread.start(periodic_100ms); 80 | periodic_1s_thread.start(periodic_1s); 81 | periodic_10s_thread.start(periodic_10s); 82 | 83 | while (true) { 84 | } 85 | } 86 | -------------------------------------------------------------------------------- /rover-apps/gamepad_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(gamepad_2021) 2 | target_sources(gamepad_2021 PRIVATE src/main.cpp) 3 | target_link_libraries(gamepad_2021 4 | PRIVATE 5 | AnalogBusIn 6 | AnalogInputGroup 7 | DigitalInputGroup 8 | FrameArbiter 9 | Logger 10 | ) 11 | mbed_set_post_build(gamepad_2021) 12 | -------------------------------------------------------------------------------- /rover-apps/gimbal_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(gimbal_2021) 2 | target_sources(gimbal_2021 PRIVATE ${ROVER_APPS_DIR}/common/src/main.cpp) 3 | target_include_directories(gimbal_2021 PUBLIC include ${ROVER_APPS_DIR}/common/include) 4 | target_link_libraries(gimbal_2021 5 | PRIVATE 6 | #Control 7 | OpenLoopController 8 | PositionController 9 | VelocityController 10 | CurrentController 11 | ActuatorControllerManager 12 | #Motors 13 | LimServo 14 | ContServo 15 | #Encoders 16 | EncoderAEAT6012 17 | #CAN 18 | CANInterface 19 | CANMsg 20 | #Sensor 21 | CurrentSensor 22 | #common-modules 23 | WatchdogModule 24 | #Other 25 | Logger 26 | uwrt-mars-rover-hw-bridge 27 | mbed-events 28 | ) 29 | mbed_set_post_build(gimbal_2021) 30 | -------------------------------------------------------------------------------- /rover-apps/gimbal_2021/include/AppConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Module.h" 6 | #include "WatchdogModule.h" 7 | 8 | WatchdogModule gimbal_watchdog; 9 | 10 | std::vector gModules = { 11 | &gimbal_watchdog, 12 | }; 13 | -------------------------------------------------------------------------------- /rover-apps/gimbal_2021/include/CANConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CANInterface.h" 4 | #include "CANMsg.h" 5 | #include "hw_bridge.h" 6 | 7 | static void sendACK(HWBRIDGE::GIMBAL_ACK_VALUES ackValue); 8 | static mbed_error_status_t gimbalSetControlMode(void); 9 | static mbed_error_status_t gimbalSetJointPIDParams(void); 10 | static mbed_error_status_t commonSwitchCANBus(void); 11 | 12 | namespace CANConfig { 13 | 14 | using namespace HWBRIDGE; 15 | 16 | static CANMsgMap rxMsgMap = { 17 | // Streamed messages 18 | {CANID::GIMBAL_SET_JOINT_POSITION, 19 | { 20 | {CANSIGNAL::GIMBAL_SET_PAN_POSITION, 0}, 21 | {CANSIGNAL::GIMBAL_SET_PITCH_POSITION, 0}, 22 | {CANSIGNAL::GIMBAL_SET_ROLL_POSITION, 0}, 23 | }}, 24 | {CANID::GIMBAL_SET_JOINT_ANGULAR_VELOCITY, 25 | { 26 | {CANSIGNAL::GIMBAL_SET_PAN_ANGULAR_VELOCITY, 0}, 27 | }}, 28 | 29 | // One-shot messages 30 | {CANID::GIMBAL_SET_CONTROL_MODE, 31 | { 32 | {CANSIGNAL::GIMBAL_PAN_CONTROL_MODE, (CANSignalValue_t)GIMBAL_PAN_CONTROL_MODE_VALUES::SNA}, 33 | }}, 34 | {CANID::GIMBAL_SET_JOINT_PID_PARAMS, 35 | { 36 | {CANSIGNAL::GIMBAL_JOINT_PIDID, (CANSignalValue_t)GIMBAL_JOINT_PIDID_VALUES::SNA}, 37 | {CANSIGNAL::GIMBAL_JOINT_PID_PROPORTIONAL_GAIN, 38 | (CANSignalValue_t)GIMBAL_JOINT_PID_PROPORTIONAL_GAIN_VALUES::SNA}, 39 | {CANSIGNAL::GIMBAL_JOINT_PID_INTEGRAL_GAIN, (CANSignalValue_t)GIMBAL_JOINT_PID_INTEGRAL_GAIN_VALUES::SNA}, 40 | {CANSIGNAL::GIMBAL_JOINT_PID_DERIVATIVE_GAIN, (CANSignalValue_t)GIMBAL_JOINT_PID_DERIVATIVE_GAIN_VALUES::SNA}, 41 | {CANSIGNAL::GIMBAL_JOINT_PID_DEADZONE, (CANSignalValue_t)GIMBAL_JOINT_PID_DEADZONE_VALUES::SNA}, 42 | }}, 43 | {CANID::COMMON_SWITCH_CAN_BUS, 44 | { 45 | {CANSIGNAL::COMMON_CAN_BUS_ID, (CANSignalValue_t)COMMON_CAN_BUS_ID_VALUES::SNA}, 46 | }}, 47 | }; 48 | 49 | static CANMsgMap txMsgMap = { 50 | {CANID::GIMBAL_REPORT_JOINT_DATA, 51 | { 52 | {CANSIGNAL::GIMBAL_REPORT_PAN_POSITION, 0}, 53 | {CANSIGNAL::GIMBAL_REPORT_PAN_ANGULAR_VELOCITY, 0}, 54 | }}, 55 | {CANID::GIMBAL_REPORT_FAULTS, 56 | { 57 | {CANSIGNAL::GIMBAL_PAN_ENCODER_STATE, (CANSignalValue_t)GIMBAL_PAN_ENCODER_STATE_VALUES::SNA}, 58 | {CANSIGNAL::GIMBAL_NUM_CANRX_FAULTS, 0}, 59 | {CANSIGNAL::GIMBAL_NUM_CANTX_FAULTS, 0}, 60 | }}, 61 | {CANID::GIMBAL_REPORT_DIAGNOSTICS, 62 | { 63 | {CANSIGNAL::GIMBAL_REPORT_NUM_STREAMED_MSGS_RECEIVED, 0}, 64 | {CANSIGNAL::GIMBAL_REPORT_NUM_ONE_SHOT_MSGS_RECEIVED, 0}, 65 | }}, 66 | }; 67 | 68 | const static CANMsg::CANMsgHandlerMap rxOneShotMsgHandler = { 69 | {CANID::GIMBAL_SET_CONTROL_MODE, &gimbalSetControlMode}, 70 | {CANID::GIMBAL_SET_JOINT_PID_PARAMS, &gimbalSetJointPIDParams}, 71 | {CANID::COMMON_SWITCH_CAN_BUS, &commonSwitchCANBus}, 72 | }; 73 | 74 | CANInterface::Config config = { 75 | // CAN bus pins 76 | .can1_RX = CAN1_RX, 77 | .can1_TX = CAN1_TX, 78 | .can2_RX = CAN2_RX, 79 | .can2_TX = CAN2_TX, 80 | 81 | // Message maps and handlers 82 | .rxMsgMap = &rxMsgMap, 83 | .txMsgMap = &txMsgMap, 84 | .rxOneShotMsgHandler = &rxOneShotMsgHandler, 85 | }; 86 | 87 | } // namespace CANConfig 88 | -------------------------------------------------------------------------------- /rover-apps/gimbal_2021/include/PanConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "AEAT6012.h" 4 | #include "ActuatorControllerManager.h" 5 | #include "ContServo.h" 6 | #include "OpenLoop.h" 7 | #include "Position.h" 8 | #include "Velocity.h" 9 | 10 | namespace Pan { 11 | 12 | /* 13 | • Pan: HS-1425 CR (https://www.robotshop.com/en/hitec-hsr-1425cr-continuous-rotation-servo.html) 14 | */ 15 | 16 | namespace Internal { 17 | 18 | constexpr auto MAX_DEG_PER_SEC = 267; 19 | constexpr auto MAX_CURRENT = 0.64; 20 | constexpr auto MAX_PULSE = 2.1ms, MIN_PULSE = 0.9ms; 21 | 22 | static PID::PID pid({1, 0, 0, -MAX_DEG_PER_SEC, MAX_DEG_PER_SEC, 0, false, false}); 23 | 24 | static Actuator::ContServo servo(SRVO_PWM_YAW, MAX_DEG_PER_SEC, MAX_PULSE, MIN_PULSE); 25 | static Encoder::AEAT6012 encoder({PAN_ENC_SPI_SCK, PAN_ENC_SPI_MISO, PAN_ENC_SPI_CS, 0}); 26 | 27 | static Controller::Position pos(servo, encoder, std::nullopt, pid, MAX_DEG_PER_SEC, MAX_CURRENT, NC, NC); 28 | static Controller::Velocity vel(servo, encoder, std::nullopt, pid, MAX_DEG_PER_SEC, MAX_CURRENT, NC, NC); 29 | static Controller::OpenLoop open(servo, encoder, std::nullopt, MAX_DEG_PER_SEC, MAX_CURRENT, NC, NC); 30 | 31 | static const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::POSITION, &pos}, 32 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}, 33 | {HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}}; 34 | 35 | } // namespace Internal 36 | static Controller::ActuatorControllerManager manager(Internal::lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 37 | } // namespace Pan 38 | -------------------------------------------------------------------------------- /rover-apps/gimbal_2021/include/PitchConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "LimServo.h" 4 | 5 | namespace Pitch { 6 | 7 | /* 8 | • Pitch: HS-422 (https://www.robotshop.com/en/hitec-hs-422-servo-motor.html) 9 | */ 10 | 11 | namespace Internal { 12 | constexpr auto RANGE = 195; 13 | constexpr auto MAX_PULSE = 2.5ms, MIN_PULSE = 0.5ms; 14 | } // namespace Internal 15 | 16 | static Actuator::LimServo pitchServo(SRVO_PWM_PITCH, Internal::RANGE, Internal::MAX_PULSE, Internal::MIN_PULSE); 17 | } // namespace Pitch 18 | -------------------------------------------------------------------------------- /rover-apps/pdb_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(PDB_Monitoring STATIC) 2 | target_sources(PDB_Monitoring PRIVATE src/PDBMonitoring.cpp) 3 | target_include_directories(PDB_Monitoring PUBLIC 4 | include 5 | ${ROVER_APPS_DIR}/common/include 6 | ) 7 | target_link_libraries(PDB_Monitoring 8 | PRIVATE 9 | Logger 10 | mbed-os 11 | ) 12 | 13 | add_executable(pdb_2021) 14 | target_sources(pdb_2021 PRIVATE ${ROVER_APPS_DIR}/common/src/main.cpp) 15 | target_include_directories(pdb_2021 PUBLIC include ${ROVER_APPS_DIR}/common/include) 16 | target_link_libraries(pdb_2021 17 | PRIVATE 18 | CANBus 19 | CANMsg 20 | Logger 21 | uwrt-mars-rover-hw-bridge 22 | #Modules 23 | PDB_Monitoring 24 | ) 25 | mbed_set_post_build(pdb_2021) 26 | -------------------------------------------------------------------------------- /rover-apps/pdb_2021/include/AppConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Module.h" 6 | #include "PDBMonitoring.h" 7 | 8 | PDBMonitoring PDB_monitoring; 9 | 10 | std::vector gModules = { 11 | &PDB_monitoring, 12 | }; 13 | -------------------------------------------------------------------------------- /rover-apps/pdb_2021/include/PDBMonitoring.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Module.h" 4 | #include "mbed.h" 5 | 6 | /*This PDB module is for load, rail and temperature monitoring.*/ 7 | 8 | class PDBMonitoring final : public Module { 9 | public: 10 | /* Sets the Load DIAG_EN pins */ 11 | PDBMonitoring(); 12 | 13 | void periodic_1s(void) override; 14 | void periodic_10s(void) override {} 15 | void periodic_100ms(void) override {} 16 | void periodic_10ms(void) override {} 17 | void periodic_1ms(void) override; 18 | 19 | private: 20 | void load_monitoring(); 21 | void rail_monitoring(); 22 | void temperature_monitoring(); 23 | 24 | static const float PDB_VBAT_RAIL_NOMINAL_VOLTAGE; 25 | static const float PDB_VBAT_RAIL_MIN_THRESHOLD; 26 | static const float PDB_VBAT_RAIL_MAX_THRESHOLD; 27 | 28 | static const float PDB_24V_RAIL_NOMINAL_VOLTAGE; 29 | static const float PDB_24V_RAIL_MIN_THRESHOLD; 30 | static const float PDB_24V_RAIL_MAX_THRESHOLD; 31 | 32 | static const float PDB_17V_RAIL_NOMINAL_VOLTAGE; 33 | static const float PDB_17V_RAIL_MIN_THRESHOLD; 34 | static const float PDB_17V_RAIL_MAX_THRESHOLD; 35 | 36 | static const float PDB_5V_RAIL_NOMINAL_VOLTAGE; 37 | static const float PDB_5V_RAIL_MIN_THRESHOLD; 38 | static const float PDB_5V_RAIL_MAX_THRESHOLD; 39 | 40 | static const float PDB_TEMPERATURE_MIN_THRESHOLD; 41 | static const float PDB_TEMPERATURE_MAX_THRESHOLD; 42 | 43 | static const bool PDB_5V_LOAD1_DIAG_EN; 44 | static const bool PDB_5V_LOAD2_DIAG_EN; 45 | static const bool PDB_5V_LOAD3_DIAG_EN; 46 | static const bool PDB_5V_LOAD4_DIAG_EN; 47 | static const bool PDB_5V_LOAD5_DIAG_EN; 48 | static const bool PDB_17V_LOAD_DIAG_EN; 49 | 50 | /* Pins configuration for Load Monitoring */ 51 | DigitalOut load1_5V_diag_en; 52 | DigitalIn load1_5V_fault_n; 53 | 54 | DigitalOut load2_5V_diag_en; 55 | DigitalIn load2_5V_fault_n; 56 | 57 | DigitalOut load3_5V_diag_en; 58 | DigitalIn load3_5V_fault_n; 59 | 60 | DigitalOut load4_5V_diag_en; 61 | DigitalIn load4_5V_fault_n; 62 | 63 | DigitalOut load5_5V_diag_en; 64 | DigitalIn load5_5V_fault_n; 65 | 66 | DigitalOut load_17V_diag_en; 67 | DigitalIn load_17V_fault_n; 68 | 69 | /* Pins configuration for Rail Monitoring */ 70 | AnalogIn railBattery; 71 | AnalogIn rail5V; 72 | AnalogIn rail17V; 73 | AnalogIn rail24V; 74 | DigitalIn rail24V_pgood_n; 75 | 76 | /* Pins configuration for Temperature Monitoring */ 77 | AnalogIn temperatureADC; 78 | }; -------------------------------------------------------------------------------- /rover-apps/pdb_2021/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "CANBus.h" 4 | #include "CANMsg.h" 5 | #include "Logger.h" 6 | #include "hw_bridge.h" 7 | 8 | AnalogIn railBattery(RAIL_BATTERY_ANLG_IN), rail5V(RAIL_5V_ANLG_IN), rail17V(RAIL_17V_ANLG_IN), 9 | rail24V(RAIL_24V_ANLG_IN); // add voltage range (as percentage) to hw bridge also allocate can id for reporting if 10 | // outside range 11 | 12 | CANBus can1(CAN1_RX, CAN1_TX, HWBRIDGE::ROVER_CANBUS_FREQUENCY_HZ); 13 | 14 | void rxCANProcessor(); 15 | void txCANProcessor(); 16 | 17 | // simple switch statement that calls a different function based on contents of CAN msg 18 | static mbed_error_status_t setLEDMatrix(void); 19 | 20 | const static CANMsg::CANMsgHandlerMap canHandlerMap = {{HWBRIDGE::CANID::PDB_SET_LED_MATRIX, &setLEDMatrix}}; 21 | 22 | int main() { 23 | Thread rxCANProcessorThread(osPriorityAboveNormal); 24 | Thread txCANProcessorThread(osPriorityBelowNormal); 25 | while (true) { 26 | // if (std::abs((railBattery.read() - 3) * 100 / 3) < some percent defined in hw bridge) {send to xavier} 27 | // repeat for each digital in 28 | } 29 | } 30 | 31 | void rxCANProcessor() { 32 | CANMsg rxMsg; 33 | while (true) { 34 | if (can1.read(rxMsg)) { 35 | canHandlerMap.at(rxMsg.getID())(); 36 | } 37 | ThisThread::sleep_for(2ms); 38 | } 39 | } 40 | 41 | void txCANProcessor() { 42 | CANMsg txMsg; 43 | while (true) { 44 | // stream ultrasonic data 45 | ThisThread::sleep_for(2ms); // TODO: define the sleep rate in the bridge 46 | } 47 | } 48 | 49 | static mbed_error_status_t setLEDMatrix(void) { 50 | return MBED_ERROR_INVALID_ARGUMENT; 51 | } 52 | -------------------------------------------------------------------------------- /rover-apps/science_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(science_2021) 2 | target_sources(science_2021 PRIVATE ${ROVER_APPS_DIR}/common/src/main.cpp) 3 | target_include_directories(science_2021 PUBLIC include ${ROVER_APPS_DIR}/common/include) 4 | target_link_libraries(science_2021 5 | PRIVATE 6 | #Control 7 | OpenLoopController 8 | PositionController 9 | VelocityController 10 | CurrentController 11 | ActuatorControllerManager 12 | #Motors 13 | LimServo 14 | DCMotor 15 | #Encoders 16 | EncoderAEAT6012 17 | EncoderMAE3 18 | Quadrature64CPR 19 | #CAN 20 | CANInterface 21 | CANMsg 22 | #Sensor 23 | CurrentSensor 24 | AdafruitSTEMMA 25 | #common-modules 26 | WatchdogModule 27 | #Other 28 | uwrt-mars-rover-hw-bridge 29 | Logger 30 | mbed-events 31 | ) 32 | mbed_set_post_build(science_2021) 33 | 34 | add_library(ScienceConfig INTERFACE) 35 | target_include_directories(ScienceConfig INTERFACE include) 36 | -------------------------------------------------------------------------------- /rover-apps/science_2021/include/AppConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Module.h" 6 | #include "WatchdogModule.h" 7 | 8 | WatchdogModule science_watchdog; 9 | 10 | std::vector gModules = { 11 | &science_watchdog, 12 | }; 13 | -------------------------------------------------------------------------------- /rover-apps/science_2021/include/CANConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CANInterface.h" 4 | #include "CANMsg.h" 5 | #include "hw_bridge.h" 6 | 7 | static void sendACK(HWBRIDGE::SCIENCE_ACK_VALUES ackValue); 8 | static mbed_error_status_t scienceSetControlMode(void); 9 | static mbed_error_status_t scienceSetJointPIDParams(void); 10 | static mbed_error_status_t commonSwitchCANBus(void); 11 | 12 | namespace CANConfig { 13 | 14 | using namespace HWBRIDGE; 15 | 16 | static CANMsgMap rxMsgMap = { 17 | // Streamed messages 18 | {CANID::SCIENCE_SET_JOINT_POSITION, 19 | { 20 | {CANSIGNAL::SCIENCE_SET_GENEVA_POSITION, 0}, 21 | {CANSIGNAL::SCIENCE_SET_ELEVATOR_POSITION, 0}, 22 | {CANSIGNAL::SCIENCE_SET_COVER_POSITION, 0}, 23 | {CANSIGNAL::SCIENCE_SET_SHOVEL_POSITION, 0}, 24 | }}, 25 | {CANID::SCIENCE_SET_JOINT_ANGULAR_VELOCITY, 26 | { 27 | {CANSIGNAL::SCIENCE_SET_GENEVA_ANGULAR_VELOCITY, 0}, 28 | }}, 29 | 30 | // One-shot messages 31 | {CANID::SCIENCE_SET_CONTROL_MODE, 32 | { 33 | {CANSIGNAL::SCIENCE_GENEVA_CONTROL_MODE, (CANSignalValue_t)SCIENCE_GENEVA_CONTROL_MODE_VALUES::SNA}, 34 | {CANSIGNAL::SCIENCE_ELEVATOR_CONTROL_MODE, (CANSignalValue_t)SCIENCE_ELEVATOR_CONTROL_MODE_VALUES::SNA}, 35 | }}, 36 | {CANID::SCIENCE_SET_JOINT_PID_PARAMS, 37 | { 38 | {CANSIGNAL::SCIENCE_JOINT_PIDID, (CANSignalValue_t)SCIENCE_JOINT_PIDID_VALUES::SNA}, 39 | {CANSIGNAL::SCIENCE_JOINT_PID_PROPORTIONAL_GAIN, 40 | (CANSignalValue_t)SCIENCE_JOINT_PID_PROPORTIONAL_GAIN_VALUES::SNA}, 41 | {CANSIGNAL::SCIENCE_JOINT_PID_INTEGRAL_GAIN, (CANSignalValue_t)SCIENCE_JOINT_PID_INTEGRAL_GAIN_VALUES::SNA}, 42 | {CANSIGNAL::SCIENCE_JOINT_PID_DERIVATIVE_GAIN, 43 | (CANSignalValue_t)SCIENCE_JOINT_PID_DERIVATIVE_GAIN_VALUES::SNA}, 44 | {CANSIGNAL::SCIENCE_JOINT_PID_DEADZONE, (CANSignalValue_t)SCIENCE_JOINT_PID_DEADZONE_VALUES::SNA}, 45 | }}, 46 | {CANID::COMMON_SWITCH_CAN_BUS, 47 | { 48 | {CANSIGNAL::COMMON_CAN_BUS_ID, (CANSignalValue_t)COMMON_CAN_BUS_ID_VALUES::SNA}, 49 | }}, 50 | }; 51 | 52 | static CANMsgMap txMsgMap = { 53 | {CANID::SCIENCE_REPORT_JOINT_DATA, 54 | { 55 | {CANSIGNAL::SCIENCE_REPORT_GENEVA_POSITION, 0}, 56 | {CANSIGNAL::SCIENCE_REPORT_ELEVATOR_POSITION, 0}, 57 | {CANSIGNAL::SCIENCE_REPORT_GENEVA_ANGULAR_VELOCITY, 0}, 58 | {CANSIGNAL::SCIENCE_REPORT_ELEVATOR_ANGULAR_VELOCITY, 0}, 59 | }}, 60 | {CANID::SCIENCE_REPORT_SENSOR_DATA, 61 | { 62 | {CANSIGNAL::SCIENCE_MOISTURE_DATA, 0}, 63 | {CANSIGNAL::SCIENCE_TEMPERATURE_DATA, 0}, 64 | }}, 65 | {CANID::SCIENCE_REPORT_FAULTS, 66 | { 67 | {CANSIGNAL::SCIENCE_GENEVA_ENCODER_STATE, (CANSignalValue_t)SCIENCE_GENEVA_ENCODER_STATE_VALUES::SNA}, 68 | {CANSIGNAL::SCIENCE_ELEVATOR_ENCODER_STATE, (CANSignalValue_t)SCIENCE_ELEVATOR_ENCODER_STATE_VALUES::SNA}, 69 | {CANSIGNAL::SCIENCE_MOISTURE_SENSOR_STATE, (CANSignalValue_t)SCIENCE_MOISTURE_SENSOR_STATE_VALUES::SNA}, 70 | {CANSIGNAL::SCIENCE_NUM_CANRX_FAULTS, 0}, 71 | {CANSIGNAL::SCIENCE_NUM_CANTX_FAULTS, 0}, 72 | }}, 73 | {CANID::SCIENCE_REPORT_DIAGNOSTICS, 74 | { 75 | {CANSIGNAL::SCIENCE_REPORT_NUM_STREAMED_MSGS_RECEIVED, 0}, 76 | {CANSIGNAL::SCIENCE_REPORT_NUM_ONE_SHOT_MSGS_RECEIVED, 0}, 77 | }}, 78 | }; 79 | 80 | const static CANMsg::CANMsgHandlerMap rxOneShotMsgHandler = { 81 | {CANID::SCIENCE_SET_CONTROL_MODE, &scienceSetControlMode}, 82 | {CANID::SCIENCE_SET_JOINT_PID_PARAMS, &scienceSetJointPIDParams}, 83 | {CANID::COMMON_SWITCH_CAN_BUS, &commonSwitchCANBus}, 84 | }; 85 | 86 | CANInterface::Config config = { 87 | // CAN bus pins 88 | .can1_RX = CAN1_RX, 89 | .can1_TX = CAN1_TX, 90 | .can2_RX = CAN2_RX, 91 | .can2_TX = CAN2_TX, 92 | 93 | // Message maps and handlers 94 | .rxMsgMap = &rxMsgMap, 95 | .txMsgMap = &txMsgMap, 96 | .rxOneShotMsgHandler = &rxOneShotMsgHandler, 97 | }; 98 | 99 | } // namespace CANConfig 100 | -------------------------------------------------------------------------------- /rover-apps/science_2021/include/CentrifugeConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "AEAT6012.h" 6 | #include "ActuatorControllerManager.h" 7 | #include "DCMotor.h" 8 | #include "LookupTable.h" 9 | #include "OpenLoop.h" 10 | #include "Position.h" 11 | #include "Velocity.h" 12 | #include "hw_bridge.h" 13 | 14 | namespace Centrifuge { 15 | 16 | namespace Internal { 17 | static Actuator::DCMotor motor(MTR_PWM_CENTFGE, MTR_DIR_CENTFGE, false); 18 | 19 | // clk, miso, cs 20 | static Encoder::AEAT6012 encoder({ENC_SCK_CENTFGE, ENC_MISO_CENTFGE, ENC_CS_CENTFGE, 0}); 21 | 22 | static PID::PID velPID({1, 0, 0, -1, 1, 0, false, false}); 23 | static PID::PID posPID({1, 0, 0, -1, 1, 0, false, false}); 24 | 25 | constexpr float MAX_DEG_PER_SEC = 601; // 10.5 RAD/s 26 | constexpr float MAX_CURRENT = std::numeric_limits::infinity(); // since no current sensor 27 | 28 | static Controller::Position pos(motor, encoder, std::nullopt, posPID, MAX_DEG_PER_SEC, MAX_CURRENT, LIM_SW_CENTFGE_DN, 29 | LIM_SW_CENTFGE_UP); 30 | static Controller::OpenLoop open(motor, encoder, std::nullopt, MAX_DEG_PER_SEC, MAX_CURRENT, LIM_SW_CENTFGE_DN, 31 | LIM_SW_CENTFGE_UP); 32 | 33 | static const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::POSITION, &pos}, 34 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; 35 | } // namespace Internal 36 | 37 | static Controller::ActuatorControllerManager manager(Internal::lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 38 | 39 | } // namespace Centrifuge 40 | -------------------------------------------------------------------------------- /rover-apps/science_2021/include/CoverConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "LimServo.h" 4 | 5 | namespace Cover { 6 | 7 | namespace Internal { 8 | constexpr auto RANGE = 90; // 90 DEGREES - @Mathieu 9 | constexpr auto MAX_PULSE = 2.2ms, MIN_PULSE = 0.8ms; 10 | } // namespace Internal 11 | 12 | static Actuator::LimServo servo(SRVO_PWM_LID, Internal::RANGE, Internal::MAX_PULSE, Internal::MIN_PULSE); 13 | 14 | } // namespace Cover 15 | -------------------------------------------------------------------------------- /rover-apps/science_2021/include/DiggerConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "LimServo.h" 4 | 5 | namespace Digger { 6 | 7 | namespace Internal { 8 | constexpr auto RANGE = 120; // 120 DEGREES See digger solidworks simulation 9 | constexpr auto MAX_PULSE = 2.2ms, MIN_PULSE = 0.8ms; 10 | } // namespace Internal 11 | 12 | static Actuator::LimServo servo(SRVO_PWM_SHVL, Internal::RANGE, Internal::MAX_PULSE, Internal::MIN_PULSE); 13 | 14 | } // namespace Digger 15 | -------------------------------------------------------------------------------- /rover-apps/science_2021/include/ElevatorConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "ActuatorControllerManager.h" 6 | #include "DCMotor.h" 7 | #include "LookupTable.h" 8 | #include "OpenLoop.h" 9 | #include "Position.h" 10 | #include "Quadrature64CPR.h" 11 | #include "Velocity.h" 12 | #include "hw_bridge.h" 13 | 14 | namespace Elevator { 15 | 16 | namespace Internal { 17 | static Actuator::DCMotor motor(MTR_PWM_ELVTR, MTR_DIR_ELVTR, false); 18 | 19 | // channela, channelb, index, offset deg, qei encoding 20 | static Encoder::Quadrature64CPR encoder({ENC_A_LIFT, ENC_B_LIFT, NC, 0}); 21 | 22 | static PID::PID velPID({1, 0, 0, -1, 1, 0, false, false}); 23 | static PID::PID posPID({1, 0, 0, -1, 1, 0, false, false}); 24 | 25 | constexpr float MAX_DEG_PER_SEC = 1197; // 20.9 RAD/s 26 | constexpr float MAX_CURRENT = std::numeric_limits::infinity(); // since no current sensor 27 | 28 | static Controller::Velocity vel(motor, encoder, std::nullopt, velPID, MAX_DEG_PER_SEC, MAX_CURRENT, LIM_SW_ELVTR_DN, 29 | LIM_SW_ELVTR_UP); 30 | static Controller::Position pos(motor, encoder, std::nullopt, posPID, MAX_DEG_PER_SEC, MAX_CURRENT, LIM_SW_ELVTR_DN, 31 | LIM_SW_ELVTR_UP); 32 | static Controller::OpenLoop open(motor, encoder, std::nullopt, MAX_DEG_PER_SEC, MAX_CURRENT, LIM_SW_ELVTR_DN, 33 | LIM_SW_ELVTR_UP); 34 | 35 | static const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel}, 36 | {HWBRIDGE::CONTROL::Mode::POSITION, &pos}, 37 | {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}}; 38 | } // namespace Internal 39 | 40 | static Controller::ActuatorControllerManager manager(Internal::lut, HWBRIDGE::CONTROL::Mode::OPEN_LOOP); 41 | 42 | } // namespace Elevator 43 | -------------------------------------------------------------------------------- /scripts/local-requirements/requirements.txt: -------------------------------------------------------------------------------- 1 | PyYAML~=6.0 2 | mbed-tools~=7.53.0 3 | cantools~=37.0 4 | matplotlib~=3.4 5 | -------------------------------------------------------------------------------- /scripts/requirements.txt: -------------------------------------------------------------------------------- 1 | -r ../mbed-os/tools/cmake/requirements.txt 2 | -r ./local-requirements/requirements.txt 3 | -------------------------------------------------------------------------------- /supported_build_configurations.yaml: -------------------------------------------------------------------------------- 1 | # Commented boards should be supported, but currently are not 2 | 3 | # Rover Apps 4 | arm_2021: 5 | - ARM_REV2_2021 6 | 7 | gamepad_2021: 8 | - GAMEPAD_REV1_2021 9 | 10 | gimbal_2021: 11 | - GIMBAL_REV2_2021 12 | 13 | pdb_2021: 14 | - PDB_REV2_2021 15 | 16 | science_2021: 17 | - SCIENCE_REV2_2021 18 | 19 | # Test Apps 20 | test-aeat-6012: 21 | - UWRT_NUCLEO 22 | 23 | test-aeat-8800: 24 | - UWRT_NUCLEO 25 | 26 | test-actuator-controller: 27 | - ARM_REV2_2021 28 | - GIMBAL_REV2_2021 29 | - SCIENCE_REV2_2021 30 | - UWRT_NUCLEO 31 | 32 | test-blinky: 33 | - ARM_REV2_2021 34 | - GAMEPAD_REV1_2021 35 | - GIMBAL_REV2_2021 36 | - PDB_REV2_2021 37 | - SCIENCE_REV2_2021 38 | - UWRT_NUCLEO 39 | 40 | test-blockingneo: 41 | - UWRT_NUCLEO 42 | 43 | test-can: 44 | - ARM_REV2_2021 45 | - GIMBAL_REV2_2021 46 | - PDB_REV2_2021 47 | - SCIENCE_REV2_2021 48 | - UWRT_NUCLEO 49 | 50 | test-mae3: 51 | # - GIMBAL_REV2_2021 52 | # - SCIENCE_REV2_2021 53 | - UWRT_NUCLEO 54 | 55 | test-led-matrix: 56 | - UWRT_NUCLEO 57 | 58 | test-limit-switch: 59 | - UWRT_NUCLEO 60 | 61 | test-logger: 62 | - GIMBAL_REV2_2021 63 | - PDB_REV2_2021 64 | - SCIENCE_REV2_2021 65 | - UWRT_NUCLEO 66 | 67 | test-lookup-table: 68 | - UWRT_NUCLEO 69 | 70 | test-adafruitSTEMMA: 71 | - SCIENCE_REV2_2021 72 | # - UWRT_NUCLEO 73 | 74 | test-units: 75 | - UWRT_NUCLEO 76 | 77 | test-pid: 78 | # - ARM_REV2_2021 79 | # - GIMBAL_REV2_2021 80 | # - SCIENCE_REV2_2021 81 | - UWRT_NUCLEO 82 | 83 | test-pwm: 84 | - ARM_REV2_2021 85 | - GIMBAL_REV2_2021 86 | - PDB_REV2_2021 87 | - SCIENCE_REV2_2021 88 | - UWRT_NUCLEO 89 | 90 | test-pwmin: 91 | - ARM_REV2_2021 92 | # - GAMEPAD_REV1_2021 93 | # - GIMBAL_REV2_2021 94 | # - SCIENCE_REV2_2021 95 | - UWRT_NUCLEO 96 | 97 | 98 | test-quadrature64cpr: 99 | - UWRT_NUCLEO 100 | 101 | test-watchdog: 102 | - UWRT_NUCLEO 103 | 104 | test-stress-can: 105 | - ARM_REV2_2021 106 | - GIMBAL_REV2_2021 107 | - PDB_REV2_2021 108 | - SCIENCE_REV2_2021 109 | - UWRT_NUCLEO -------------------------------------------------------------------------------- /targets/TARGET_ARM_REV2_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(mbed-arm-rev2-2021 INTERFACE) 2 | 3 | target_sources(mbed-arm-rev2-2021 4 | INTERFACE 5 | src/system_clock_override.c 6 | src/PeripheralPins.c 7 | ) 8 | 9 | target_include_directories(mbed-arm-rev2-2021 10 | INTERFACE 11 | include 12 | ) 13 | 14 | target_link_libraries(mbed-arm-rev2-2021 INTERFACE mbed-stm32f446xe) 15 | target_compile_options(mbed-arm-rev2-2021 INTERFACE -w) # Disable all warnings from mbed target code 16 | 17 | -------------------------------------------------------------------------------- /targets/TARGET_ARM_REV2_2021/include/uwrt_config_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace FeatureFlags { 4 | constexpr bool UART_LOGGING = false; 5 | constexpr bool SWO_LOGGING = false; 6 | } // namespace FeatureFlags 7 | -------------------------------------------------------------------------------- /targets/TARGET_GAMEPAD_REV1_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(mbed-gamepad-rev1-2021 INTERFACE) 2 | 3 | target_sources(mbed-gamepad-rev1-2021 4 | INTERFACE 5 | src/system_clock_override.c 6 | src/PeripheralPins.c 7 | ) 8 | 9 | target_include_directories(mbed-gamepad-rev1-2021 10 | INTERFACE 11 | include 12 | ) 13 | 14 | target_link_libraries(mbed-gamepad-rev1-2021 INTERFACE mbed-stm32f446xe) 15 | target_compile_options(mbed-gamepad-rev1-2021 INTERFACE -w) # Disable all warnings from mbed target code 16 | 17 | -------------------------------------------------------------------------------- /targets/TARGET_GAMEPAD_REV1_2021/include/uwrt_config_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace FeatureFlags { 4 | constexpr bool UART_LOGGING = true; 5 | constexpr bool SWO_LOGGING = false; 6 | } // namespace FeatureFlags 7 | -------------------------------------------------------------------------------- /targets/TARGET_GIMBAL_REV2_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(mbed-gimbal-rev2-2021 INTERFACE) 2 | 3 | target_sources(mbed-gimbal-rev2-2021 4 | INTERFACE 5 | src/system_clock_override.c 6 | src/PeripheralPins.c 7 | ) 8 | 9 | target_include_directories(mbed-gimbal-rev2-2021 10 | INTERFACE 11 | include 12 | ) 13 | 14 | target_link_libraries(mbed-gimbal-rev2-2021 INTERFACE mbed-stm32f446xe) 15 | target_compile_options(mbed-gimbal-rev2-2021 INTERFACE -w) # Disable all warnings from mbed target code 16 | 17 | -------------------------------------------------------------------------------- /targets/TARGET_GIMBAL_REV2_2021/include/uwrt_config_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace FeatureFlags { 4 | constexpr bool UART_LOGGING = false; 5 | constexpr bool SWO_LOGGING = true; 6 | } // namespace FeatureFlags 7 | -------------------------------------------------------------------------------- /targets/TARGET_PDB_REV2_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(mbed-pdb-rev2-2021 INTERFACE) 2 | 3 | target_sources(mbed-pdb-rev2-2021 4 | INTERFACE 5 | src/system_clock_override.c 6 | src/PeripheralPins.c 7 | ) 8 | 9 | target_include_directories(mbed-pdb-rev2-2021 10 | INTERFACE 11 | include 12 | ) 13 | 14 | target_link_libraries(mbed-pdb-rev2-2021 INTERFACE mbed-stm32f446xe) 15 | target_compile_options(mbed-pdb-rev2-2021 INTERFACE -w) # Disable all warnings from mbed target code 16 | -------------------------------------------------------------------------------- /targets/TARGET_PDB_REV2_2021/include/uwrt_config_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace FeatureFlags { 4 | constexpr bool UART_LOGGING = false; 5 | constexpr bool SWO_LOGGING = true; 6 | } // namespace FeatureFlags 7 | -------------------------------------------------------------------------------- /targets/TARGET_SCIENCE_REV2_2021/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(mbed-science-rev2-2021 INTERFACE) 2 | 3 | target_sources(mbed-science-rev2-2021 4 | INTERFACE 5 | src/system_clock_override.c 6 | src/PeripheralPins.c 7 | ) 8 | 9 | target_include_directories(mbed-science-rev2-2021 10 | INTERFACE 11 | include 12 | ) 13 | 14 | target_link_libraries(mbed-science-rev2-2021 INTERFACE mbed-stm32f446xe) 15 | target_compile_options(mbed-science-rev2-2021 INTERFACE -w) # Disable all warnings from mbed target code 16 | 17 | -------------------------------------------------------------------------------- /targets/TARGET_SCIENCE_REV2_2021/include/uwrt_config_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace FeatureFlags { 4 | constexpr bool UART_LOGGING = false; 5 | constexpr bool SWO_LOGGING = true; 6 | } // namespace FeatureFlags 7 | -------------------------------------------------------------------------------- /targets/TARGET_UWRT_NUCLEO/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(mbed-uwrt-nucleo INTERFACE) 2 | 3 | target_include_directories(mbed-uwrt-nucleo 4 | INTERFACE 5 | include 6 | ) 7 | 8 | target_link_libraries(mbed-uwrt-nucleo INTERFACE mbed-nucleo-f446re) 9 | target_compile_options(mbed-uwrt-nucleo INTERFACE -w) # Disable all warnings from mbed target code 10 | -------------------------------------------------------------------------------- /targets/TARGET_UWRT_NUCLEO/include/uwrt_config_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace FeatureFlags { 4 | constexpr bool UART_LOGGING = true; 5 | constexpr bool SWO_LOGGING = false; 6 | } // namespace FeatureFlags 7 | -------------------------------------------------------------------------------- /targets/custom_targets.json: -------------------------------------------------------------------------------- 1 | { 2 | "ARM_REV2_2021": { 3 | "inherits": [ 4 | "UWRT_STM32F446xE" 5 | ], 6 | "overrides": { 7 | "clock_source": "USE_PLL_HSE_XTAL|USE_PLL_HSI", 8 | "hse_value": "25000000U" 9 | } 10 | }, 11 | "GAMEPAD_REV1_2021": { 12 | "inherits": [ 13 | "UWRT_STM32F446xE" 14 | ], 15 | "overrides": { 16 | "clock_source": "USE_PLL_HSE_XTAL|USE_PLL_HSI", 17 | "hse_value": "25000000U" 18 | } 19 | }, 20 | "GIMBAL_REV2_2021": { 21 | "inherits": [ 22 | "UWRT_STM32F446xE" 23 | ], 24 | "overrides": { 25 | "clock_source": "USE_PLL_HSE_XTAL|USE_PLL_HSI", 26 | "hse_value": "25000000U" 27 | } 28 | }, 29 | "PDB_REV2_2021": { 30 | "inherits": [ 31 | "UWRT_STM32F446xE" 32 | ], 33 | "overrides": { 34 | "clock_source": "USE_PLL_HSE_XTAL|USE_PLL_HSI", 35 | "hse_value": "25000000U" 36 | } 37 | }, 38 | "SCIENCE_REV2_2021": { 39 | "inherits": [ 40 | "UWRT_STM32F446xE" 41 | ], 42 | "overrides": { 43 | "clock_source": "USE_PLL_HSE_XTAL|USE_PLL_HSI", 44 | "hse_value": "25000000U" 45 | } 46 | }, 47 | "UWRT_STM32F446xE": { 48 | "inherits": [ 49 | "MCU_STM32F446xE" 50 | ], 51 | "config": { 52 | "hse_value": { 53 | "help": "HSE default value is 8MHz in stm32f4xx_hal_conf.h", 54 | "value": "8000000U", 55 | "macro_name": "HSE_VALUE" 56 | } 57 | }, 58 | "public": false 59 | }, 60 | "UWRT_NUCLEO": { 61 | "inherits": [ 62 | "NUCLEO_F446RE" 63 | ] 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /test-apps/test-actuator-controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-actuator-controller) 2 | target_sources(test-actuator-controller PRIVATE src/main.cpp) 3 | target_link_libraries(test-actuator-controller PRIVATE mbed-os) 4 | mbed_set_post_build(test-actuator-controller) 5 | -------------------------------------------------------------------------------- /test-apps/test-actuator-controller/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* mbed Microcontroller Library 2 | * Copyright (c) 2018 ARM Limited 3 | * SPDX-License-Identifier: Apache-2.0 4 | */ 5 | 6 | #include "mbed.h" 7 | 8 | /* TODO: create test driver */ 9 | DigitalOut led1(LED1); 10 | 11 | // main() runs in its own thread in the OS 12 | int main() { 13 | while (true) { 14 | // Blink LED and wait 0.5 seconds 15 | led1 = !led1; 16 | ThisThread::sleep_for(500ms); 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /test-apps/test-adafruitSTEMMA/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-adafruitSTEMMA) 2 | target_sources(test-adafruitSTEMMA PRIVATE src/main.cpp) 3 | target_include_directories(test-adafruitSTEMMA PUBLIC include) 4 | target_link_libraries(test-adafruitSTEMMA 5 | PRIVATE 6 | AdafruitSTEMMA 7 | Logger 8 | uwrt-mars-rover-hw-bridge 9 | ) 10 | mbed_set_post_build(test-adafruitSTEMMA) 11 | -------------------------------------------------------------------------------- /test-apps/test-adafruitSTEMMA/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "AdafruitSTEMMA.h" 2 | #include "Logger.h" 3 | #include "hw_bridge.h" 4 | 5 | DigitalOut led1(LED1); 6 | 7 | int main() { 8 | led1 = 0; // start with LED off 9 | 10 | Sensor::AdafruitSTEMMA sensor(TEMP_MOIST_I2C_SDA, TEMP_MOIST_I2C_SCL); 11 | 12 | while (1) { 13 | MBED_ASSERT(sensor.reset()); 14 | 15 | ThisThread::sleep_for(500ms); 16 | 17 | led1 = (sensor.getStatus()); // turn on LED if the sensor's HW_ID code matches the known value 18 | 19 | printf("\r\nChecking Device ID...\r\n"); // read device HW_ID, reading of 85 is expected 20 | 21 | ThisThread::sleep_for(100ms); 22 | 23 | printf("\r\nReading Moisture...\r\n"); // read moisture from sensor, reading of 65534 indicates 24 | // unsuccessful initialization 25 | MBED_ASSERT(sensor.update()); 26 | printf("Moisture: %f \r\n", sensor.read()); 27 | 28 | ThisThread::sleep_for(100ms); 29 | 30 | printf("\r\nReading Temperature...\r\n"); // read temperature from sensor, reading of -273.0 31 | // indicates unsuccessful initialization 32 | printf("Temperature: %f \r\n", sensor.alternateRead()); 33 | 34 | printf("\r\n-----------------------------\r\n"); 35 | 36 | ThisThread::sleep_for(100ms); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /test-apps/test-aeat-6012/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-aeat-6012) 2 | target_sources(test-aeat-6012 PRIVATE src/main.cpp) 3 | target_link_libraries(test-aeat-6012 4 | PRIVATE 5 | EncoderAEAT6012 6 | Logger 7 | ) 8 | mbed_set_post_build(test-aeat-6012) 9 | -------------------------------------------------------------------------------- /test-apps/test-aeat-6012/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "AEAT6012.h" 2 | #include "Logger.h" 3 | 4 | Encoder::AEAT6012 encoder({SPI_SCK, SPI_MISO, SPI_CS, 0}); 5 | 6 | Mutex print_mutex; 7 | 8 | void print(const std::string &str); // thread safe print 9 | 10 | constexpr auto PERIOD = 100ms; 11 | constexpr bool USE_BLOCKING_TEST = true; 12 | 13 | /* Synchronous testing */ 14 | void Updater(); 15 | void Reader(); 16 | 17 | /* Async testing */ 18 | void callback(); 19 | 20 | int main() { 21 | Thread updater_thread(osPriorityNormal), reader_thread(osPriorityNormal); 22 | 23 | updater_thread.start(&Updater); 24 | reader_thread.start(&Reader); 25 | 26 | updater_thread.join(); 27 | reader_thread.join(); 28 | } 29 | 30 | void Updater() { 31 | while (true) { 32 | if constexpr (USE_BLOCKING_TEST) { 33 | Timer timer; 34 | timer.start(); 35 | MBED_ASSERT(encoder.update()); 36 | timer.stop(); 37 | std::string str = "Time taken to update encoder: " + std::to_string(timer.elapsed_time().count()) + "us\r\n"; 38 | print(str); 39 | } else { 40 | MBED_ASSERT(encoder.update(nullptr)); // doesnt make sense to time this. 41 | } 42 | ThisThread::sleep_for(PERIOD); 43 | } 44 | } 45 | 46 | void Reader() { 47 | while (true) { 48 | std::string str = "Angle: " + std::to_string(encoder.getAngleDeg()) + 49 | ", Angular Velocity: " + std::to_string(encoder.getAngularVelocityDegPerSec()) + "\r\n"; 50 | print(str); 51 | ThisThread::sleep_for(PERIOD); 52 | } 53 | } 54 | 55 | void print(const std::string &str) { 56 | std::unique_lock lock(print_mutex); 57 | printf("%s", str.c_str()); 58 | } 59 | -------------------------------------------------------------------------------- /test-apps/test-aeat-8800/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-aeat-8800) 2 | target_sources(test-aeat-8800 PRIVATE src/main.cpp) 3 | target_link_libraries(test-aeat-8800 PRIVATE Logger) 4 | mbed_set_post_build(test-aeat-8800) 5 | -------------------------------------------------------------------------------- /test-apps/test-aeat-8800/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Logger.h" 2 | #include "mbed.h" 3 | 4 | SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK); // mosi, miso, sclk (PA_7, PA_6, PA_5) 5 | DigitalOut cs(SPI_CS); // cs (PB_6) 6 | 7 | int main() { 8 | uint16_t upper, lower = 0x0000; 9 | 10 | // Select SSI protocol by setting CS high 11 | cs = 1; 12 | ThisThread::sleep_for(3ms); 13 | 14 | // Setup the spi for 8 bit data, high steady state clock, 15 | // second edge capture, with a 1MHz clock rate 16 | spi.format(16, 3); 17 | spi.frequency(1000000); 18 | 19 | while (true) { 20 | // Read 21 | upper = spi.write(0x8000); 22 | lower = spi.write(0x0000); 23 | 24 | upper = (upper << 1) | (lower >> 15); 25 | // lower = (lower << 1) & 0xF000; 26 | 27 | printf("Data = 0x%04X %d \t Diag = 0x%04X\t%d\r\n", upper, upper, lower, lower); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /test-apps/test-blinky/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-blinky) 2 | target_sources(test-blinky PRIVATE src/main.cpp) 3 | target_link_libraries(test-blinky 4 | PRIVATE 5 | Logger 6 | mbed-os 7 | ) 8 | mbed_set_post_build(test-blinky) 9 | -------------------------------------------------------------------------------- /test-apps/test-blinky/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* mbed Microcontroller Library 2 | * Copyright (c) 2019 ARM Limited 3 | * SPDX-License-Identifier: Apache-2.0 4 | */ 5 | 6 | #include "Logger.h" 7 | #include "mbed.h" 8 | 9 | // Blinking rate in millisecond 10 | constexpr auto BLINKING_RATE = 500ms; 11 | 12 | int main() { 13 | // Initialise the digital pin LED1 as an output 14 | DigitalOut led(LED1); 15 | 16 | while (true) { 17 | led = !led; 18 | printf("Current LED State is %s\r\n", led ? "ON" : "OFF"); 19 | ThisThread::sleep_for(BLINKING_RATE); 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /test-apps/test-blockingneo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-blockingneo) 2 | target_sources(test-blockingneo PRIVATE src/main.cpp) 3 | target_link_libraries(test-blockingneo 4 | PRIVATE 5 | mbed-os 6 | Neopixel_Blocking 7 | uwrt-mars-rover-hw-bridge 8 | ) 9 | mbed_set_post_build(test-blockingneo) 10 | -------------------------------------------------------------------------------- /test-apps/test-blockingneo/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Neopixel_Blocking.h" 2 | #include "hw_bridge.h" 3 | #include "mbed.h" 4 | 5 | int main() { 6 | Neopixel_Blocking pixels(16, LED1); 7 | 8 | while (1) { 9 | pixels.displayRed(); 10 | ThisThread::sleep_for(1ms); 11 | pixels.displayBlue(); 12 | ThisThread::sleep_for(1ms); 13 | pixels.blinkPixels(2, 1s, pixels.Green); 14 | } 15 | 16 | return 1; 17 | } 18 | -------------------------------------------------------------------------------- /test-apps/test-can/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-can) 2 | target_sources(test-can PRIVATE src/main.cpp) 3 | target_include_directories(test-can PUBLIC include) 4 | target_link_libraries(test-can 5 | PRIVATE 6 | CANInterface 7 | CANBus 8 | CANMsg 9 | Logger 10 | mbed-os 11 | uwrt-mars-rover-hw-bridge 12 | ) 13 | mbed_set_post_build(test-can) 14 | -------------------------------------------------------------------------------- /test-apps/test-can/include/CANConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CANInterface.h" 4 | #include "CANMsg.h" 5 | #include "hw_bridge.h" 6 | 7 | static mbed_error_status_t handle_test_msg_one_shot(void); 8 | 9 | namespace CANConfig { 10 | 11 | using namespace HWBRIDGE; 12 | 13 | static CANMsgMap rxMsgMap = { 14 | // Msg 1 15 | {CANID::COMMON_DEBUG_MESSAGE1, 16 | { 17 | {CANSIGNAL::COMMON_DEBUG_SIGNAL1, 0}, 18 | }}, 19 | 20 | // Msg 2 21 | {CANID::COMMON_DEBUG_MESSAGE3, 22 | { 23 | {CANSIGNAL::COMMON_DEBUG_SIGNAL3, 0}, 24 | }}, 25 | }; 26 | 27 | static CANMsgMap txMsgMap = { 28 | // Msg 1 29 | {CANID::COMMON_DEBUG_MESSAGE2, 30 | { 31 | {CANSIGNAL::COMMON_DEBUG_SIGNAL2, 0}, 32 | }}, 33 | 34 | // Msg 2 35 | }; 36 | 37 | const static CANMsg::CANMsgHandlerMap rxOneShotMsgHandler = { 38 | {CANID::COMMON_DEBUG_MESSAGE3, &handle_test_msg_one_shot}, 39 | }; 40 | 41 | CANInterface::Config config = { 42 | // CAN bus pins 43 | .can1_RX = CAN1_RX, 44 | .can1_TX = CAN1_TX, 45 | .can2_RX = CAN2_RX, 46 | .can2_TX = CAN2_TX, 47 | 48 | // Message maps and handlers 49 | .rxMsgMap = &rxMsgMap, 50 | .txMsgMap = &txMsgMap, 51 | .rxOneShotMsgHandler = &rxOneShotMsgHandler, 52 | }; 53 | 54 | } // namespace CANConfig 55 | -------------------------------------------------------------------------------- /test-apps/test-can/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "CANConfig.h" 2 | #include "CANInterface.h" 3 | #include "Logger.h" 4 | #include "hw_bridge.h" 5 | 6 | CANInterface can(CANConfig::config); 7 | 8 | int main() { 9 | Utility::logger << ""; // Band-aid fix for logger bug (issue #328) 10 | printf("CAN test\r\n"); 11 | 12 | can.setFilter(HWBRIDGE::CANFILTER::COMMON_FILTER, CANStandard, HWBRIDGE::ROVER_CANID_FILTER_MASK); 13 | 14 | HWBRIDGE::CANSignalValue_t rxSignal1Value = 0; 15 | HWBRIDGE::CANSignalValue_t txSignal1Value = 0; 16 | 17 | while (true) { 18 | // Read RX signal 19 | can.getRXSignalValue(HWBRIDGE::CANID::COMMON_DEBUG_MESSAGE1, HWBRIDGE::CANSIGNAL::COMMON_DEBUG_SIGNAL1, 20 | rxSignal1Value); 21 | printf("RX debug signal value: %lu\r\n", static_cast(rxSignal1Value)); 22 | 23 | // Update TX signal 24 | can.setTXSignalValue(HWBRIDGE::CANID::COMMON_DEBUG_MESSAGE2, HWBRIDGE::CANSIGNAL::COMMON_DEBUG_SIGNAL2, 25 | txSignal1Value); 26 | printf("TX test signal 2 value: %u\r\n", static_cast(txSignal1Value)); 27 | txSignal1Value = (static_cast(txSignal1Value) + 1) % 8; 28 | 29 | ThisThread::sleep_for(1000ms); 30 | } 31 | } 32 | 33 | // Receive a one-shot message (COMMON_DEBUG_MESSAGE3) and send a one-shot reply back 34 | mbed_error_status_t handle_test_msg_one_shot(void) { 35 | CANMsg msgACK; 36 | 37 | // Initialize msg struct 38 | struct uwrt_mars_rover_can_common_debug_message3_t msgStruct = { 39 | .common_debug_signal3 = static_cast(HWBRIDGE::COMMON_DEBUG_SIGNAL3_VALUES::DEBUG_VALUE_0), 40 | }; 41 | 42 | // Pack msg struct contents into raw bytes 43 | HWBRIDGE::CANMsgData_t msgData; 44 | uwrt_mars_rover_can_common_debug_message3_pack(msgData.raw, &msgStruct, 1); 45 | 46 | // Prepare CANMsg 47 | msgACK.setID(HWBRIDGE::CANID::COMMON_DEBUG_MESSAGE3); 48 | msgACK.setPayload(msgData, UWRT_MARS_ROVER_CAN_COMMON_DEBUG_MESSAGE3_LENGTH); 49 | 50 | // Send a one shot back 51 | can.sendOneShotMessage(msgACK, 1ms); 52 | 53 | printf("One-shot reply sent\r\n"); 54 | 55 | return MBED_SUCCESS; 56 | } 57 | -------------------------------------------------------------------------------- /test-apps/test-can/src/main_backup.cpp: -------------------------------------------------------------------------------- 1 | #include "CANBus.h" 2 | #include "CANMsg.h" 3 | #include "Logger.h" 4 | #include "hw_bridge.h" 5 | #include "mbed.h" 6 | 7 | CANBus can(CAN1_RX, CAN1_TX, HWBRIDGE::ROVERCONFIG::ROVER_CANBUS_FREQUENCY); 8 | CANMsg rxMsg; 9 | CANMsg txMsg; 10 | DigitalOut ledTX(LED1); 11 | DigitalOut ledRX(LED2); 12 | Timer timer; 13 | uint8_t counter = 0; 14 | 15 | constexpr uint16_t TX_ID = 0x100; 16 | constexpr uint16_t RX_ID = 0x101; 17 | 18 | /** 19 | * @brief Prints CAN msg to PC's serial terminal 20 | * @note 21 | * @param CANMsg to print 22 | * @retval 23 | */ 24 | void printMsg(CANMsg& msg) { 25 | printf(" ID = 0x%.3x\r\n", static_cast(msg.getID())); 26 | printf(" Type = %d\r\n", msg.type); 27 | printf(" Format = %d\r\n", msg.format); 28 | printf(" Length = %d\r\n", msg.len); 29 | printf(" Data ="); 30 | for (int i = 0; i < msg.len; i++) printf(" 0x%.2X", msg.data[i]); 31 | printf("\r\n"); 32 | } 33 | 34 | int main(void) { 35 | ledTX = 0; // set transmit LED off 36 | ledRX = 0; // set recieve LED off 37 | timer.start(); // start timer 38 | printf("CAN_Hello\r\n"); 39 | 40 | while (1) { 41 | if (timer.elapsed_time() >= 1s) { // check for timeout 42 | timer.reset(); // reset timer 43 | counter++; // increment counter 44 | txMsg.clear(); // clear Tx message storage 45 | txMsg.setID(static_cast(TX_ID)); // set ID 46 | txMsg << counter; // copy counter value to CAN msg payload 47 | if (can.write(txMsg)) { // transmit message 48 | printf("-------------------------------------\r\n"); 49 | printf("CAN message sent\r\n"); 50 | printMsg(txMsg); 51 | printf(" counter = %d\r\n", counter); 52 | ledTX = !ledTX; 53 | } else 54 | printf("Transmission error\r\n"); 55 | } 56 | 57 | if (can.read(rxMsg)) { 58 | ledRX = !ledRX; // turn the LED on 59 | printf("-------------------------------------\r\n"); 60 | printf("CAN message received\r\n"); 61 | printMsg(rxMsg); 62 | 63 | // Filtering performed by software: 64 | if (rxMsg.getID() == static_cast(RX_ID)) { 65 | rxMsg >> counter; // extract data from the received CAN message 66 | printf(" counter = %d\r\n", counter); 67 | timer.start(); // transmission lag 68 | } 69 | } 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /test-apps/test-led-matrix/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-led-matrix) 2 | target_sources(test-led-matrix PRIVATE src/main.cpp) 3 | target_link_libraries(test-led-matrix 4 | PRIVATE 5 | LEDMatrix 6 | uwrt-mars-rover-hw-bridge 7 | mbed-os 8 | ) 9 | mbed_set_post_build(test-led-matrix) 10 | -------------------------------------------------------------------------------- /test-apps/test-led-matrix/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "LEDMatrix.h" 2 | #include "hw_bridge.h" 3 | #include "mbed.h" 4 | 5 | int main() { 6 | printf("####### STARTING LED MATRIX TEST #######\r\n"); 7 | 8 | LEDMatrix blinky(D8, D9, D10); 9 | 10 | while (true) { 11 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_RED); 12 | ThisThread::sleep_for(5s); 13 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_BLUE); 14 | ThisThread::sleep_for(5s); 15 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_GREEN); 16 | ThisThread::sleep_for(5s); 17 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::OFF); 18 | ThisThread::sleep_for(5s); 19 | 20 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::FLASHING_RED); 21 | ThisThread::sleep_for(5s); 22 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::FLASHING_BLUE); 23 | ThisThread::sleep_for(5s); 24 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::FLASHING_GREEN); 25 | ThisThread::sleep_for(5s); 26 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::OFF); 27 | ThisThread::sleep_for(5s); 28 | 29 | blinky.setFlashColor(1, 0, 0); // red 30 | ThisThread::sleep_for(5s); 31 | blinky.setFlashColor(0, 1, 0); // green 32 | ThisThread::sleep_for(5s); 33 | blinky.setFlashColor(0, 0, 1); // blue 34 | ThisThread::sleep_for(5s); 35 | 36 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::OFF); 37 | ThisThread::sleep_for(5s); 38 | blinky.setSolidColor(1, 0, 0); // red 39 | ThisThread::sleep_for(5s); 40 | blinky.setSolidColor(0, 1, 0); // green 41 | ThisThread::sleep_for(5s); 42 | blinky.setSolidColor(0, 0, 1); // blue 43 | ThisThread::sleep_for(5s); 44 | 45 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::OFF); 46 | ThisThread::sleep_for(5s); 47 | blinky.setFlashColor(1, 0, 1); // purple 48 | ThisThread::sleep_for(5s); 49 | blinky.setFlashColor(1, 1, 0); // brownish 50 | ThisThread::sleep_for(5s); 51 | blinky.setFlashColor(0, 1, 1); // blue-green 52 | ThisThread::sleep_for(5s); 53 | 54 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::OFF); 55 | ThisThread::sleep_for(5s); 56 | blinky.setSolidColor(1, 0, 1); // purple 57 | ThisThread::sleep_for(5s); 58 | blinky.setSolidColor(1, 1, 0); // brownish 59 | ThisThread::sleep_for(5s); 60 | blinky.setSolidColor(0, 1, 1); // blue-green 61 | ThisThread::sleep_for(5s); 62 | 63 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_RED); 64 | ThisThread::sleep_for(5s); 65 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::FLASHING_GREEN); 66 | ThisThread::sleep_for(5s); 67 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_BLUE); 68 | ThisThread::sleep_for(5s); 69 | 70 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::FLASHING_GREEN); 71 | ThisThread::sleep_for(5s); 72 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_BLUE); 73 | ThisThread::sleep_for(5s); 74 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::SOLID_RED); 75 | ThisThread::sleep_for(5s); 76 | blinky.setState(HWBRIDGE::LEDMATRIX::LEDMatrixState::OFF); 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /test-apps/test-limit-switch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-limit-switch) 2 | target_sources(test-limit-switch PRIVATE src/main.cpp) 3 | target_link_libraries(test-limit-switch PRIVATE LimitSwitch Logger) 4 | mbed_set_post_build(test-limit-switch) 5 | -------------------------------------------------------------------------------- /test-apps/test-limit-switch/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "LimitSwitch.h" 2 | #include "PinNames.h" 3 | #include "mbed.h" 4 | 5 | GPIO::LimitSwitch hSwitch(PA_0); 6 | GPIO::LimitSwitch lSwitch(PA_0, 0); 7 | GPIO::LimitSwitch ncSwitch(NC); 8 | 9 | int main() { 10 | // Check to see if limit switch is pressed 11 | while (true) { 12 | // check for active high and active low case 13 | if (hSwitch.isPressed()) { 14 | printf("Active high limit switch is pressed \n"); 15 | } 16 | 17 | if (lSwitch.isPressed()) { 18 | printf("Active low limit switch is pressed \n"); 19 | } 20 | // test for NC limit switch 21 | if (!ncSwitch.isConnected()) { 22 | printf("Pin is not connected \n"); 23 | } 24 | } 25 | } -------------------------------------------------------------------------------- /test-apps/test-logger/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-logger) 2 | target_sources(test-logger PRIVATE src/main.cpp) 3 | target_link_libraries(test-logger PRIVATE Logger) 4 | mbed_set_post_build(test-logger) 5 | -------------------------------------------------------------------------------- /test-apps/test-logger/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Logger.h" 2 | #include "mbed.h" 3 | 4 | int main() { 5 | printf("Logger %s%c\r\n", "Test", '!'); 6 | 7 | Utility::logger << "Long: " << ((long)-58748397) << "\r\n"; 8 | Utility::logger << "Unsigned long: " << ((unsigned long)123478421) << "\r\n"; 9 | Utility::logger << "Bool: " << true << "\r\n"; 10 | Utility::logger << "Short: " << ((short)-4) << "\r\n"; 11 | Utility::logger << "Unsigned short: " << ((unsigned short)4) << "\r\n"; 12 | Utility::logger << "Int: " << ((int)-4235) << "\r\n"; 13 | Utility::logger << "Unsigned int: " << ((unsigned int)12434) << "\r\n"; 14 | Utility::logger << "Long long: " << ((long long)8765456787654) << "\r\n"; 15 | Utility::logger << "Double: " << ((double)234.56324546) << "\r\n"; 16 | Utility::logger << "Float: " << ((float)-431.6543) << "\r\n"; 17 | Utility::logger << "Long double: " << ((long double)2345453.98765) << "\r\n"; 18 | Utility::logger << "Char: " << 'a' << "\r\n"; 19 | Utility::logger << "String: " 20 | << "hello logger" 21 | << "\r\n"; 22 | 23 | // Test mbed error log 24 | MBED_ASSERT(false); 25 | 26 | while (true) 27 | ; 28 | } 29 | -------------------------------------------------------------------------------- /test-apps/test-lookup-table/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-lookup-table) 2 | target_sources(test-lookup-table PRIVATE src/main.cpp) 3 | target_link_libraries(test-lookup-table 4 | PRIVATE 5 | LookupTable 6 | CANMsg 7 | Logger 8 | ) 9 | mbed_set_post_build(test-lookup-table) 10 | -------------------------------------------------------------------------------- /test-apps/test-lookup-table/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "CANMsg.h" 4 | #include "Logger.h" 5 | #include "LookupTable.h" 6 | #include "hw_bridge.h" 7 | #include "mbed.h" 8 | 9 | mbed_error_status_t trivial1(void); 10 | mbed_error_status_t trivial2(void); 11 | 12 | int main() { 13 | printf("\r\n LookupTable Test App Started \r\n"); 14 | printf("\r\n####### HOW TO ITERATE #######\r\n"); 15 | printf("Note: Won't be in order when iterating since based off hash-map. "); 16 | printf("What's important is that key-value pairs match\r\n"); 17 | const Utility::LookupTable week = { 18 | {"Monday", 1}, {"Tuesday", 2}, {"Wednesday", 3}, {"Thursday", 4}, {"Friday", 5}, {"Saturday", 6}, {"Sunday", 7}}; 19 | // iterators: 20 | for (auto it = week.begin(); it != week.end(); it++) { 21 | printf("%s %d\r\n", it->first.c_str(), it->second); 22 | } 23 | printf("\r\n"); 24 | // range-based for loop 25 | for (const auto& i : week) { 26 | printf("%s %d\r\n", i.first.c_str(), i.second); 27 | } 28 | printf("\r\n"); 29 | // structured bindings 30 | for (const auto& [key, value] : week) { 31 | printf("%s %d\r\n", key.c_str(), value); 32 | } 33 | 34 | printf("\r\n####### HOW TO INDEX #######\r\n"); 35 | // No default case 36 | const Utility::LookupTable a = {{"Jan", 1}, {"Feb", 2}}; 37 | printf("Existing Key: %d\r\n", a["Jan"].value_or(-1)); 38 | printf("Existing Key: %d\r\n", a["Feb"].value_or(-1)); 39 | printf("Non-Existing Key: %d\r\n", a["Doesnt Exist"].value_or(-1)); 40 | printf("\r\n"); 41 | 42 | // Default case: Note necessary cast around the default 43 | const Utility::LookupTable(42)> b = {{0, 100}, {1, 101}}; 44 | printf("Existing Key: %d\r\n", b[0]); 45 | printf("Existing Key: %d\r\n", b[1]); 46 | printf("Non-Existing Key: %d\r\n", b[2]); 47 | printf("\r\n"); 48 | 49 | // Function pointer example 50 | const CANMsg::CANMsgHandlerMap c = {{HWBRIDGE::CANID::TPDO1, trivial1}, {HWBRIDGE::CANID::TPDO2, trivial2}}; 51 | printf("Existing Key: \r\n"); 52 | c[HWBRIDGE::CANID::TPDO1](); 53 | printf("Existing Key: \r\n"); 54 | c[HWBRIDGE::CANID::TPDO2](); 55 | printf("Non-Existing Key \r\nNOTE THE TERMINATION OF THE PROGRAM: \r\n"); 56 | c[HWBRIDGE::CANID::TPDO3](); 57 | 58 | while (true) { 59 | } 60 | } 61 | 62 | mbed_error_status_t trivial1(void) { 63 | printf("Hello from trivial1\r\n"); 64 | return MBED_SUCCESS; 65 | } 66 | mbed_error_status_t trivial2(void) { 67 | printf("Hello from trivial2\r\n"); 68 | return MBED_SUCCESS; 69 | } 70 | -------------------------------------------------------------------------------- /test-apps/test-mae3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-mae3) 2 | target_sources(test-mae3 PRIVATE src/main.cpp) 3 | target_link_libraries(test-mae3 4 | PRIVATE 5 | EncoderMAE3 6 | Logger 7 | PwmIn 8 | ) 9 | mbed_set_post_build(test-mae3) 10 | -------------------------------------------------------------------------------- /test-apps/test-mae3/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Logger.h" 2 | #include "MAE3.h" 3 | 4 | Encoder::MAE3 encoder({NC, 0}); 5 | 6 | Mutex mutex; 7 | 8 | constexpr auto PERIOD = 100ms; 9 | 10 | void Updater(); 11 | void Reader(); 12 | void print(const std::string &str); // thread safe print 13 | 14 | int main() { 15 | Thread updater_thread(osPriorityNormal), reader_thread(osPriorityNormal); 16 | 17 | updater_thread.start(&Updater); 18 | reader_thread.start(&Reader); 19 | 20 | updater_thread.join(); 21 | reader_thread.join(); 22 | } 23 | 24 | void Updater() { 25 | while (true) { 26 | Timer timer; 27 | timer.start(); 28 | MBED_ASSERT(encoder.update()); 29 | timer.stop(); 30 | std::string str; 31 | str = "Time taken to update encoder: " + std::to_string(timer.elapsed_time().count()) + "us\r\n"; 32 | print(str); 33 | ThisThread::sleep_for(PERIOD); 34 | } 35 | } 36 | 37 | void Reader() { 38 | while (true) { 39 | std::string str; 40 | str = "Angle: " + std::to_string(encoder.getAngleDeg()) + 41 | ", Angular Velocity: " + std::to_string(encoder.getAngularVelocityDegPerSec()) + "\r\n"; 42 | print(str); 43 | ThisThread::sleep_for(PERIOD); 44 | } 45 | } 46 | 47 | void print(const std::string &str) { 48 | std::unique_lock lock(mutex); 49 | printf("%s", str.c_str()); 50 | } 51 | -------------------------------------------------------------------------------- /test-apps/test-pid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-pid) 2 | target_sources(test-pid PRIVATE src/main.cpp) 3 | target_include_directories(test-pid PUBLIC include) 4 | target_link_libraries(test-pid PRIVATE PID Logger) 5 | mbed_set_post_build(test-pid) 6 | -------------------------------------------------------------------------------- /test-apps/test-pid/control_anti_kickback.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uwrobotics/MarsRoverFirmware/d513dd11758f9dfc5831bb1b7503cb9aecd69256/test-apps/test-pid/control_anti_kickback.png -------------------------------------------------------------------------------- /test-apps/test-pid/control_anti_windup.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uwrobotics/MarsRoverFirmware/d513dd11758f9dfc5831bb1b7503cb9aecd69256/test-apps/test-pid/control_anti_windup.png -------------------------------------------------------------------------------- /test-apps/test-pid/control_anti_windup_anti_kickback.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uwrobotics/MarsRoverFirmware/d513dd11758f9dfc5831bb1b7503cb9aecd69256/test-apps/test-pid/control_anti_windup_anti_kickback.png -------------------------------------------------------------------------------- /test-apps/test-pid/control_basic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uwrobotics/MarsRoverFirmware/d513dd11758f9dfc5831bb1b7503cb9aecd69256/test-apps/test-pid/control_basic.png -------------------------------------------------------------------------------- /test-apps/test-pid/matlab_control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uwrobotics/MarsRoverFirmware/d513dd11758f9dfc5831bb1b7503cb9aecd69256/test-apps/test-pid/matlab_control.png -------------------------------------------------------------------------------- /test-apps/test-pid/src/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This app tests the functionality of our PID library using the values stored in test_data.h 3 | * NOTE: This test-app is purely computational, 4 | * no motors or encoders are required 5 | * The signal output is plotted in the test-pid folder. 6 | */ 7 | #include 8 | #include 9 | 10 | #include "Logger.h" 11 | #include "PID.h" 12 | #include "test_data.h" 13 | 14 | constexpr float KP = 2, KI = 103, KD = 1; 15 | constexpr float min_rpm = -std::numeric_limits::max(), 16 | max_rpm = std::numeric_limits::max(); // no saturation 17 | constexpr float deadzone = 0; 18 | constexpr auto pid_period = 1ms; 19 | constexpr bool anti_kickback = false, anti_windup = false; 20 | 21 | constexpr float expected_avg_error = 1.6801f; 22 | constexpr auto expected_avg_compute_time = 15us; 23 | 24 | int main() { 25 | printf("##################### PID TEST APP STARTED #####################\r\n"); 26 | PID::Config config = {KP, KI, KD, min_rpm, max_rpm, deadzone, anti_kickback, anti_windup}; 27 | PID::PID controller(config); 28 | Timer timer; 29 | auto total_compute_time = 0us; 30 | float total_error = 0; 31 | for (std::size_t i = 0; i < control.size(); i++) { 32 | if (i % 1000 == 0) { 33 | printf("Completed %zu /50001 iterations\r\n", i); 34 | } 35 | timer.reset(); 36 | timer.start(); 37 | float out = controller.compute(setpoint.at(i), feedback.at(i)); 38 | timer.stop(); 39 | total_error += std::abs(control.at(i) - out); 40 | total_compute_time += timer.elapsed_time(); 41 | MBED_ASSERT(pid_period > timer.elapsed_time()); 42 | wait_us((pid_period - timer.elapsed_time()).count()); // account for compute time 43 | } 44 | printf("TEST RESULTS\r\n"); 45 | float average_error = total_error / control.size(); 46 | auto average_compute_time = total_compute_time / control.size(); 47 | printf("Average difference between Matlab control signal and our control signal: %.5f\r\n", average_error); 48 | printf("Average time for a single call to the compute function: %llu us\r\n", average_compute_time.count()); 49 | if (average_error > expected_avg_error) { 50 | printf("WARNING: Changes made to PID library have increased average error\r\n"); 51 | } 52 | if (average_compute_time > expected_avg_compute_time) { 53 | printf("WARNING: Changes made to PID library have increased execution time of compute function\r\n"); 54 | } 55 | while (true) 56 | ; 57 | } 58 | -------------------------------------------------------------------------------- /test-apps/test-pwm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-pwm) 2 | target_sources(test-pwm PRIVATE src/main.cpp) 3 | target_link_libraries(test-pwm PRIVATE mbed-os) 4 | mbed_set_post_build(test-pwm) 5 | -------------------------------------------------------------------------------- /test-apps/test-pwm/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "mbed.h" 2 | 3 | PwmOut pwmLED(LED1); 4 | 5 | constexpr auto k_period = 1ms; 6 | constexpr auto interval = 50ms; 7 | constexpr float k_step = 0.02; 8 | 9 | int main() { 10 | pwmLED.period(std::chrono::duration_cast>(k_period).count()); 11 | 12 | float duty = 0.0f; 13 | int inverter = 1; 14 | 15 | while (true) { 16 | if (duty > 1.0f) { 17 | inverter = -1; 18 | } else if (duty < 0.0f) { 19 | inverter = +1; 20 | } 21 | 22 | duty += inverter * k_step; 23 | 24 | pwmLED.write(duty); 25 | 26 | ThisThread::sleep_for(interval); 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /test-apps/test-pwmin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-pwmin) 2 | target_sources(test-pwmin PRIVATE src/main.cpp) 3 | target_link_libraries(test-pwmin PRIVATE PwmIn Logger) 4 | mbed_set_post_build(test-pwmin) 5 | -------------------------------------------------------------------------------- /test-apps/test-pwmin/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Logger.h" 2 | #include "PwmIn.h" 3 | 4 | DigitalOut led(LED1); 5 | 6 | // Wire the output PWM signal to the PWM input 7 | // or connect external PWM input to pwmIn pin 8 | PwmOut pwmOut(NC); 9 | GPIO::PwmIn pwmIn(NC, 50); 10 | 11 | Timer timer; 12 | Timer printTimer; 13 | 14 | int main() { 15 | float inverter = +1; 16 | auto period = 2ms; // Equivalent to 0.5kHz frequency 17 | float duty = 0.1; 18 | 19 | // Specify PWM period 20 | pwmOut.period(std::chrono::duration_cast>(period).count()); 21 | 22 | // START THE TIMER 23 | timer.start(); 24 | printTimer.start(); 25 | 26 | while (true) { 27 | led = !led; 28 | 29 | // Sweep the output duty cycle at 2%/second (7.2 deg / sec) 30 | if (timer.elapsed_time() >= 5ms) { 31 | // Set the duty cycle on the pins 32 | duty += 0.0001 * inverter; 33 | pwmOut.write(duty); 34 | 35 | if (duty <= 0.1) { 36 | inverter = 1.0; 37 | } else if (duty >= 0.9) { 38 | inverter = 0.0; // Hold at 0.9 duty cycle for 5 seconds 39 | if (timer.elapsed_time() >= 5s) { 40 | inverter = -1.0; 41 | timer.reset(); 42 | } 43 | } else { 44 | timer.reset(); 45 | } 46 | } 47 | 48 | if (printTimer.elapsed_time() >= 50ms) { 49 | printTimer.reset(); 50 | printf( 51 | "Avg PW: %+f, \tAvg Prd: %+f, \tRaw Duty: %+f, \tAvg Duty: %+f, \tAvg Duty Velo: %+f, \tAvg Ang Velo: " 52 | "%+f\r\n", 53 | pwmIn.avgPulseWidth().count(), pwmIn.avgPeriod().count(), pwmIn.dutyCycle(), pwmIn.avgDutyCycle(), 54 | pwmIn.avgDutyCycleVelocity(), pwmIn.avgDutyCycleVelocity() * 360.0); 55 | } 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /test-apps/test-quadrature64cpr/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-quadrature64cpr) 2 | target_sources(test-quadrature64cpr PRIVATE src/main.cpp) 3 | target_link_libraries(test-quadrature64cpr PRIVATE Quadrature64CPR QEI Logger) 4 | mbed_set_post_build(test-quadrature64cpr) 5 | 6 | -------------------------------------------------------------------------------- /test-apps/test-quadrature64cpr/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Logger.h" 2 | #include "Quadrature64CPR.h" 3 | 4 | // first pin is yellow wire 5 | // second pin is white wire 6 | constexpr Encoder::Quadrature64CPR::Config config = {PA_5, PA_6, NC, 0}; 7 | Encoder::Quadrature64CPR encoder(config); 8 | 9 | // Synchronous testing 10 | void Updater(); 11 | void Reader(); 12 | Timer timer; 13 | 14 | constexpr auto PERIOD = 500ms; 15 | 16 | int main() { 17 | Thread updater_thread(osPriorityNormal), reader_thread(osPriorityNormal); 18 | 19 | updater_thread.start(&Updater); 20 | reader_thread.start(&Reader); 21 | 22 | updater_thread.join(); 23 | reader_thread.join(); 24 | } 25 | 26 | void Updater() { 27 | while (true) { 28 | timer.reset(); 29 | timer.start(); 30 | MBED_ASSERT(encoder.update()); 31 | timer.stop(); 32 | std::string str = "Time taken to update encoder: " + std::to_string(timer.elapsed_time().count()) + "us\r\n"; 33 | printf("%s", str.c_str()); 34 | ThisThread::sleep_for(PERIOD); 35 | } 36 | } 37 | 38 | void Reader() { 39 | while (true) { 40 | std::string str = "Angle: " + std::to_string(encoder.getAngleDeg()) + 41 | ", Angular Velocity: " + std::to_string(encoder.getAngularVelocityDegPerSec()) + "\r\n"; 42 | printf("%s", str.c_str()); 43 | ThisThread::sleep_for(PERIOD); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /test-apps/test-stress-can/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-stress-can) 2 | target_sources(test-stress-can PRIVATE src/main.cpp) 3 | target_include_directories(test-stress-can PUBLIC include) 4 | target_link_libraries(test-stress-can 5 | PRIVATE 6 | CANInterface 7 | CANMsg 8 | uwrt-mars-rover-hw-bridge 9 | mbed-os 10 | Logger 11 | ) 12 | mbed_set_post_build(test-stress-can) 13 | -------------------------------------------------------------------------------- /test-apps/test-stress-can/include/CANConfigGimbal.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CANInterface.h" 4 | #include "CANMsg.h" 5 | #include "hw_bridge.h" 6 | 7 | const HWBRIDGE::CANFILTER targetCANIDFilter = HWBRIDGE::CANFILTER::GIMBAL_RX_FILTER; 8 | const HWBRIDGE::CANID targetReportDiagnosticsCANID = HWBRIDGE::CANID::GIMBAL_REPORT_DIAGNOSTICS; 9 | const HWBRIDGE::CANSIGNAL targetReportNumStreamedSignal = HWBRIDGE::CANSIGNAL::GIMBAL_REPORT_NUM_STREAMED_MSGS_RECEIVED; 10 | const HWBRIDGE::CANSIGNAL targetReportNumOneShotsSignal = HWBRIDGE::CANSIGNAL::GIMBAL_REPORT_NUM_ONE_SHOT_MSGS_RECEIVED; 11 | const HWBRIDGE::CANID targetReportFaultsCANID = HWBRIDGE::CANID::GIMBAL_REPORT_FAULTS; 12 | const HWBRIDGE::CANSIGNAL targetNumCANRXFaultsSignal = HWBRIDGE::CANSIGNAL::GIMBAL_NUM_CANRX_FAULTS; 13 | const HWBRIDGE::CANSIGNAL targetNumCANTXFaultsSignal = HWBRIDGE::CANSIGNAL::GIMBAL_NUM_CANTX_FAULTS; 14 | 15 | static mbed_error_status_t oneShotHandler(void); 16 | static mbed_error_status_t switchCANBus(void); 17 | 18 | namespace CANConfig { 19 | 20 | using namespace HWBRIDGE; 21 | 22 | static CANMsgMap rxMsgMap = { 23 | // Streamed messages 24 | {CANID::GIMBAL_SET_JOINT_POSITION, 25 | { 26 | {CANSIGNAL::GIMBAL_SET_PAN_POSITION, 0}, 27 | {CANSIGNAL::GIMBAL_SET_PITCH_POSITION, 0}, 28 | {CANSIGNAL::GIMBAL_SET_ROLL_POSITION, 0}, 29 | }}, 30 | {CANID::GIMBAL_SET_JOINT_ANGULAR_VELOCITY, 31 | { 32 | {CANSIGNAL::GIMBAL_SET_PAN_ANGULAR_VELOCITY, 0}, 33 | }}, 34 | 35 | // One-shot messages 36 | {CANID::GIMBAL_SET_CONTROL_MODE, 37 | { 38 | {CANSIGNAL::GIMBAL_PAN_CONTROL_MODE, (CANSignalValue_t)GIMBAL_PAN_CONTROL_MODE_VALUES::SNA}, 39 | }}, 40 | {CANID::GIMBAL_SET_JOINT_PID_PARAMS, 41 | { 42 | {CANSIGNAL::GIMBAL_JOINT_PIDID, (CANSignalValue_t)GIMBAL_JOINT_PIDID_VALUES::SNA}, 43 | {CANSIGNAL::GIMBAL_JOINT_PID_PROPORTIONAL_GAIN, 44 | (CANSignalValue_t)GIMBAL_JOINT_PID_PROPORTIONAL_GAIN_VALUES::SNA}, 45 | {CANSIGNAL::GIMBAL_JOINT_PID_INTEGRAL_GAIN, (CANSignalValue_t)GIMBAL_JOINT_PID_INTEGRAL_GAIN_VALUES::SNA}, 46 | {CANSIGNAL::GIMBAL_JOINT_PID_DERIVATIVE_GAIN, (CANSignalValue_t)GIMBAL_JOINT_PID_DERIVATIVE_GAIN_VALUES::SNA}, 47 | {CANSIGNAL::GIMBAL_JOINT_PID_DEADZONE, (CANSignalValue_t)GIMBAL_JOINT_PID_DEADZONE_VALUES::SNA}, 48 | }}, 49 | {CANID::COMMON_SWITCH_CAN_BUS, 50 | { 51 | {CANSIGNAL::COMMON_CAN_BUS_ID, (CANSignalValue_t)COMMON_CAN_BUS_ID_VALUES::SNA}, 52 | }}, 53 | }; 54 | 55 | static CANMsgMap txMsgMap = { 56 | {CANID::GIMBAL_REPORT_JOINT_DATA, 57 | { 58 | {CANSIGNAL::GIMBAL_REPORT_PAN_POSITION, 0}, 59 | {CANSIGNAL::GIMBAL_REPORT_PAN_ANGULAR_VELOCITY, 0}, 60 | }}, 61 | {CANID::GIMBAL_REPORT_FAULTS, 62 | { 63 | {CANSIGNAL::GIMBAL_PAN_ENCODER_STATE, (CANSignalValue_t)GIMBAL_PAN_ENCODER_STATE_VALUES::SNA}, 64 | {CANSIGNAL::GIMBAL_NUM_CANRX_FAULTS, 0}, 65 | {CANSIGNAL::GIMBAL_NUM_CANTX_FAULTS, 0}, 66 | }}, 67 | {CANID::GIMBAL_REPORT_DIAGNOSTICS, 68 | { 69 | {CANSIGNAL::GIMBAL_REPORT_NUM_STREAMED_MSGS_RECEIVED, 0}, 70 | {CANSIGNAL::GIMBAL_REPORT_NUM_ONE_SHOT_MSGS_RECEIVED, 0}, 71 | }}, 72 | }; 73 | 74 | const static CANMsg::CANMsgHandlerMap rxOneShotMsgHandler = { 75 | {CANID::GIMBAL_SET_CONTROL_MODE, &oneShotHandler}, 76 | {CANID::GIMBAL_SET_JOINT_PID_PARAMS, &oneShotHandler}, 77 | {CANID::COMMON_SWITCH_CAN_BUS, &switchCANBus}, 78 | }; 79 | 80 | CANInterface::Config config = { 81 | // CAN bus pins 82 | .can1_RX = CAN1_RX, 83 | .can1_TX = CAN1_TX, 84 | .can2_RX = CAN2_RX, 85 | .can2_TX = CAN2_TX, 86 | 87 | // Message maps and handlers 88 | .rxMsgMap = &rxMsgMap, 89 | .txMsgMap = &txMsgMap, 90 | .rxOneShotMsgHandler = &rxOneShotMsgHandler, 91 | }; 92 | 93 | } // namespace CANConfig 94 | -------------------------------------------------------------------------------- /test-apps/test-stress-can/include/CANConfigPDB.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CANInterface.h" 4 | #include "CANMsg.h" 5 | #include "hw_bridge.h" 6 | 7 | const HWBRIDGE::CANFILTER targetCANIDFilter = HWBRIDGE::CANFILTER::PDB_RX_FILTER; 8 | const HWBRIDGE::CANID targetReportDiagnosticsCANID = HWBRIDGE::CANID::PDB_REPORT_DIAGNOSTICS; 9 | const HWBRIDGE::CANSIGNAL targetReportNumStreamedSignal = HWBRIDGE::CANSIGNAL::PDB_REPORT_NUM_STREAMED_MSGS_RECEIVED; 10 | const HWBRIDGE::CANSIGNAL targetReportNumOneShotsSignal = HWBRIDGE::CANSIGNAL::PDB_REPORT_NUM_ONE_SHOT_MSGS_RECEIVED; 11 | const HWBRIDGE::CANID targetReportFaultsCANID = HWBRIDGE::CANID::PDB_REPORT_FAULTS; 12 | const HWBRIDGE::CANSIGNAL targetNumCANRXFaultsSignal = HWBRIDGE::CANSIGNAL::PDB_NUM_CANRX_FAULTS; 13 | const HWBRIDGE::CANSIGNAL targetNumCANTXFaultsSignal = HWBRIDGE::CANSIGNAL::PDB_NUM_CANTX_FAULTS; 14 | 15 | static mbed_error_status_t oneShotHandler(void); 16 | static mbed_error_status_t switchCANBus(void); 17 | 18 | namespace CANConfig { 19 | 20 | using namespace HWBRIDGE; 21 | 22 | static CANMsgMap rxMsgMap = { 23 | // One-shot messages 24 | {CANID::PDB_SET_LED_MATRIX, 25 | { 26 | {CANSIGNAL::PDB_LED_MATRIX_STATE, (CANSignalValue_t)PDB_LED_MATRIX_STATE_VALUES::SNA}, 27 | }}, 28 | {CANID::COMMON_SWITCH_CAN_BUS, 29 | { 30 | {CANSIGNAL::COMMON_CAN_BUS_ID, (CANSignalValue_t)COMMON_CAN_BUS_ID_VALUES::SNA}, 31 | }}, 32 | }; 33 | 34 | static CANMsgMap txMsgMap = { 35 | {CANID::PDB_REPORT_SENSOR_DATA, 36 | { 37 | {CANSIGNAL::PDB_ULTRASONIC_SENSOR1_DATA, 0}, 38 | {CANSIGNAL::PDB_ULTRASONIC_SENSOR2_DATA, 0}, 39 | {CANSIGNAL::PDB_ULTRASONIC_SENSOR3_DATA, 0}, 40 | {CANSIGNAL::PDB_ULTRASONIC_SENSOR4_DATA, 0}, 41 | }}, 42 | {CANID::PDB_REPORT_FAULTS, 43 | { 44 | {CANSIGNAL::PDB_JETSON_STATUS, (CANSignalValue_t)PDB_JETSON_STATUS_VALUES::SNA}, 45 | {CANSIGNAL::PDB_ARM_POWER_STATUS, (CANSignalValue_t)PDB_ARM_POWER_STATUS_VALUES::SNA}, 46 | {CANSIGNAL::PDB_SCIENCE_POWER_STATUS, (CANSignalValue_t)PDB_SCIENCE_POWER_STATUS_VALUES::SNA}, 47 | {CANSIGNAL::PDB_GIMBAL_POWER_STATUS, (CANSignalValue_t)PDB_GIMBAL_POWER_STATUS_VALUES::SNA}, 48 | {CANSIGNAL::PDB_VBAT_RAIL_STATUS, (CANSignalValue_t)PDB_VBAT_RAIL_STATUS_VALUES::SNA}, 49 | {CANSIGNAL::PDB_17_V_RAIL_STATUS, (CANSignalValue_t)PDB_17_V_RAIL_STATUS_VALUES::SNA}, 50 | {CANSIGNAL::PDB_5_V_RAIL_STATUS, (CANSignalValue_t)PDB_5_V_RAIL_STATUS_VALUES::SNA}, 51 | {CANSIGNAL::PDB_NUM_CANRX_FAULTS, 0}, 52 | {CANSIGNAL::PDB_NUM_CANTX_FAULTS, 0}, 53 | }}, 54 | {CANID::PDB_REPORT_DIAGNOSTICS, 55 | { 56 | {CANSIGNAL::PDB_REPORT_NUM_STREAMED_MSGS_RECEIVED, 0}, 57 | {CANSIGNAL::PDB_REPORT_NUM_ONE_SHOT_MSGS_RECEIVED, 0}, 58 | }}, 59 | }; 60 | 61 | const static CANMsg::CANMsgHandlerMap rxOneShotMsgHandler = { 62 | {CANID::PDB_SET_LED_MATRIX, &oneShotHandler}, 63 | {CANID::COMMON_SWITCH_CAN_BUS, &switchCANBus}, 64 | }; 65 | 66 | CANInterface::Config config = { 67 | // CAN bus pins 68 | .can1_RX = CAN1_RX, 69 | .can1_TX = CAN1_TX, 70 | .can2_RX = CAN2_RX, 71 | .can2_TX = CAN2_TX, 72 | 73 | // Message maps and handlers 74 | .rxMsgMap = &rxMsgMap, 75 | .txMsgMap = &txMsgMap, 76 | .rxOneShotMsgHandler = &rxOneShotMsgHandler, 77 | }; 78 | 79 | } // namespace CANConfig 80 | -------------------------------------------------------------------------------- /test-apps/test-stress-can/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "CANConfigArm.h" // Include the header for the board you want to simulate 2 | #include "Logger.h" 3 | 4 | CANInterface can(CANConfig::config); 5 | 6 | int main() { 7 | Utility::logger << ""; // Band-aid fix for logger bug (issue #328) 8 | 9 | printf("\r\n\r\n"); 10 | printf("STRESS CAN TEST STARTED\r\n"); 11 | printf("=======================\r\n"); 12 | 13 | // Set CAN filters 14 | can.setFilter(targetCANIDFilter, CANStandard, HWBRIDGE::ROVER_CANID_FILTER_MASK, 0); 15 | can.setFilter(HWBRIDGE::CANFILTER::COMMON_FILTER, CANStandard, HWBRIDGE::ROVER_CANID_FILTER_MASK, 1); 16 | 17 | while (true) { 18 | // Report CAN diagnostics 19 | uint32_t numStreamedMsgsReceived = can.getNumStreamedMsgsReceived(); 20 | uint32_t numOneShotMsgsReceived = can.getNumOneShotMsgsReceived(); 21 | uint32_t numStreamedMsgsSent = can.getNumStreamedMsgsSent(); 22 | uint32_t numOneShotMsgsSent = can.getNumOneShotMsgsSent(); 23 | 24 | can.setTXSignalValue(targetReportDiagnosticsCANID, targetReportNumStreamedSignal, numStreamedMsgsReceived); 25 | can.setTXSignalValue(targetReportDiagnosticsCANID, targetReportNumOneShotsSignal, numOneShotMsgsReceived); 26 | 27 | printf("Streamed CAN messages received: %lu\r\n", numStreamedMsgsReceived); 28 | printf("One-shot CAN messages received: %lu\r\n", numOneShotMsgsReceived); 29 | printf("==========\r\n"); 30 | printf("Streamed CAN messages sent: %lu\r\n", numStreamedMsgsSent); 31 | printf("One-shot CAN messages sent: %lu\r\n\r\n\r\n", numOneShotMsgsSent); 32 | 33 | // Report CAN faults 34 | uint16_t numCANRXFaults = can.getNumCANRXFaults(); 35 | uint16_t numCANTXFaults = can.getNumCANTXFaults(); 36 | 37 | can.setTXSignalValue(targetReportFaultsCANID, targetNumCANRXFaultsSignal, numCANRXFaults); 38 | can.setTXSignalValue(targetReportFaultsCANID, targetNumCANTXFaultsSignal, numCANTXFaults); 39 | 40 | printf("CAN RX faults: %u\r\n", numCANRXFaults); 41 | printf("CAN TX faults: %u\r\n\r\n", numCANTXFaults); 42 | 43 | ThisThread::sleep_for(100ms); 44 | } 45 | } 46 | 47 | static mbed_error_status_t oneShotHandler(void) { 48 | return MBED_SUCCESS; 49 | } 50 | 51 | static mbed_error_status_t switchCANBus(void) { 52 | HWBRIDGE::CANSignalValue_t canBusID; 53 | 54 | bool success = 55 | can.getRXSignalValue(HWBRIDGE::CANID::COMMON_SWITCH_CAN_BUS, HWBRIDGE::CANSIGNAL::COMMON_CAN_BUS_ID, canBusID) && 56 | can.switchCANBus((HWBRIDGE::CANBUSID)canBusID); 57 | 58 | return success ? MBED_SUCCESS : MBED_ERROR_CODE_FAILED_OPERATION; 59 | } 60 | -------------------------------------------------------------------------------- /test-apps/test-units/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-units) 2 | target_sources(test-units PRIVATE src/main.cpp) 3 | target_link_libraries(test-units PRIVATE Units Logger) 4 | mbed_set_post_build(test-units) 5 | -------------------------------------------------------------------------------- /test-apps/test-units/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "Logger.h" 4 | #include "units.h" 5 | 6 | // These are various namespaces defined that make units esaier to work with 7 | using namespace units::length; 8 | using namespace units::force; 9 | using namespace units::current; 10 | using namespace units::angular_velocity; 11 | using namespace units::literals; 12 | using namespace units::angle; 13 | using namespace units::time; 14 | using namespace units::current; 15 | using namespace units::impedance; 16 | using namespace units::voltage; 17 | using namespace units::mass; 18 | 19 | int main() { 20 | while (true) { 21 | // Testing angular velocity in degrees per second and angle in degrees 22 | 23 | degrees_per_second_t speed = 24 | degree_t(5) / second_t(1.0); // creates an angular velocity type object with velocity of 5 degrees/second 25 | 26 | // Multiplying 5 degrees/sec by 5 seconds yields 25 degrees 27 | // Not explicitly declaring it as degree_t would assume it as radians, which is the SI unit 28 | degree_t degreesMoved = speed * second_t(5); 29 | 30 | // Printing both the angular velocity and the degrees subtended: 31 | std::cout << speed << std::endl; 32 | std::cout << degreesMoved << std::endl; 33 | 34 | radian_t angleInRad = radian_t(2 * M_PI); // Create angle object in radians 35 | degree_t angleInDeg = degree_t(angleInRad); // Can be cast to degrees 36 | 37 | std::cout << angleInRad << std::endl; // prints 6.28319 rad 38 | std::cout << angleInDeg << std::endl; // prints 360 degrees 39 | 40 | // Testing current in Amperes 41 | 42 | ampere_t currentIn = ampere_t(3); // Creating 3A current 43 | ohm_t resistance = ohm_t(4); // resistance through the load 44 | auto voltageThrough = currentIn * resistance; // automatically casted to voltage since V = IR 45 | 46 | std::cout << voltageThrough << std::endl; // prints a voltage object 47 | 48 | // Testing force in Newtons 49 | 50 | newton_t forceIn = newton_t(5); // Create a 5 newton force 51 | kilogram_t massOfObject = kilogram_t(10); // applied on a 10 kg object 52 | auto accelerationIn = forceIn / massOfObject; // automatically casted to m/s^2 (acceleration) 53 | std::cout << accelerationIn << std::endl; 54 | 55 | ThisThread::sleep_for(1s); 56 | } 57 | } -------------------------------------------------------------------------------- /test-apps/test-watchdog/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test-watchdog) 2 | target_sources(test-watchdog PRIVATE src/main.cpp) 3 | target_link_libraries(test-watchdog PRIVATE PwmIn WatchdogWrapper) 4 | mbed_set_post_build(test-watchdog) 5 | -------------------------------------------------------------------------------- /test-apps/test-watchdog/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "WatchdogWrapper.h" 2 | #include "mbed.h" 3 | 4 | Thread pet_thread; 5 | std::chrono::milliseconds countdown_ms = 1000ms; 6 | std::chrono::milliseconds pet_ms = 200ms; 7 | 8 | void pet_dog_task(std::chrono::milliseconds *pet_ms) { 9 | while (1) { 10 | Utility::WatchdogWrapper::petWatchdog(); 11 | ThisThread::sleep_for(*pet_ms); 12 | } 13 | } 14 | 15 | int main() { 16 | Utility::WatchdogWrapper::logResetReason(); 17 | Utility::WatchdogWrapper::startWatchdog(countdown_ms); 18 | pet_thread.start(callback(pet_dog_task, &pet_ms)); 19 | ThisThread::sleep_for(2000ms); 20 | MBED_ASSERT(false); 21 | } 22 | -------------------------------------------------------------------------------- /toolchain.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_SYSTEM_NAME Generic) 2 | set(CMAKE_SYSTEM_PROCESSOR arm) 3 | 4 | set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) 5 | set(CMAKE_C_COMPILER arm-none-eabi-gcc) 6 | set(CMAKE_CXX_COMPILER arm-none-eabi-g++) 7 | set(CMAKE_AS arm-none-eabi-as) 8 | set(CMAKE_AR arm-none-eabi-ar) 9 | set(CMAKE_CPP arm-none-eabi-cpp) 10 | set(CMAKE_LINKER arm-none-eabi-ld) 11 | set(CMAKE_NM arm-none-eabi-nm) 12 | set(CMAKE_OBJCOPY arm-none-eabi-objcopy) 13 | set(CMAKE_OBJDUMP arm-none-eabi-objdump) 14 | set(CMAKE_READELF arm-none-eabi-readelf) 15 | set(CMAKE_SIZE arm-none-eabi-size) 16 | set(CMAKE_STRINGS arm-none-eabi-strings) 17 | set(CMAKE_STRIP arm-none-eabi-strip) 18 | 19 | # Required to pass CMAKE toolchain functionality check 20 | set(CMAKE_TRY_COMPILE_TARGET_TYPE "STATIC_LIBRARY") 21 | --------------------------------------------------------------------------------