diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md index 3698408105..1aced54d42 100644 --- a/docs/useful-robot-commands.md +++ b/docs/useful-robot-commands.md @@ -12,7 +12,7 @@ - [Robot Diagnostics](#robot-diagnostics) - [For Just Diagnostics](#for-just-diagnostics) - [For AI + Diagnostics](#for-ai--diagnostics) - - [Robot Auto Test](#robot-auto-test) + - [STSPIN Motor Controller Test](#stspin-motor-controller-test) - [On Robot Commands](#on-robot-commands) - [Systemd Services](#systemd-services) - [Debugging Uart](#debugging-uart) @@ -76,9 +76,10 @@ This will stop the current Systemd services, replace and restart them. Binaries This will trigger motor calibration meaning the wheels may spin. Please elevate the robot so the wheels are not touching the ground for proper calibration. ```bash -bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot -- --playbook deploy_robot_software.yml --hosts --ssh_pass +bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:motor_board= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ``` +* is the type of motor driver board on the robot (either `STSPIN` or `TRINAMIC`) * is the IP address of the robot * is the password of the `robot` user account @@ -140,13 +141,13 @@ From Software/src network_interface can be found with `ifconfig` commonly `wlp59s0` for wifi. -## Robot Auto Test -Runs the robot auto test fixture on a robot through Ansible, which tests the motor board and power board SPI and UART transfer respectively. +## STSPIN Motor Controller Test +Deploys the STSPIN Motor Controller Test binary onto a robot through Ansible. From Software/src: ```bash -bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot -- --playbook robot_auto_test_playbook.yml --hosts --ssh_pass +bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:motor_board=STSPIN -- --playbook deploy_stspin_motor_controller_test.yml --hosts --ssh_pass ``` * replace the \ with the actual ip address of the Raspberry Pi for the ssh connection. diff --git a/src/MODULE.bazel b/src/MODULE.bazel index 323fd7de00..c3ba4db1bc 100644 --- a/src/MODULE.bazel +++ b/src/MODULE.bazel @@ -210,6 +210,13 @@ http_archive( url = "https://github.com/analogdevicesinc/TMC-API/archive/0cd695fab6d43ceb121af4b8608e5d92b14e1ce9.tar.gz", ) +http_archive( + name = "cppcrc", + build_file = "@//extlibs:cppcrc.BUILD", + strip_prefix = "cppcrc-6360eee1c8966d32b2552ce156b135ec6a3235f1", + url = "https://github.com/DarrenLevine/cppcrc/archive/6360eee1c8966d32b2552ce156b135ec6a3235f1.zip", +) + ############################################## # Register our Toolchains ############################################## diff --git a/src/extlibs/cppcrc.BUILD b/src/extlibs/cppcrc.BUILD new file mode 100644 index 0000000000..264d46bf5e --- /dev/null +++ b/src/extlibs/cppcrc.BUILD @@ -0,0 +1,9 @@ +# Description: +# A very small, fast, header-only, C++ library for generating CRCs +# https://github.com/DarrenLevine/cppcrc/tree/main + +cc_library( + name = "cppcrc", + hdrs = ["cppcrc.h"], + visibility = ["//visibility:public"], +) diff --git a/src/proto/robot_status_msg.proto b/src/proto/robot_status_msg.proto index ac153b4f85..3277ebe515 100644 --- a/src/proto/robot_status_msg.proto +++ b/src/proto/robot_status_msg.proto @@ -81,6 +81,7 @@ enum ErrorCode enum MotorFault { + /*********** TMC Faults ************/ // Refer to Trinamic 6100 datasheet for precise definitions of motor faults. // https://trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC6100_datasheet_Rev1.00.pdf // (Section 5.1, p22-23) @@ -99,6 +100,20 @@ enum MotorFault PHASE_W_SHORT_COUNTER_DETECTED = 12; PHASE_W_SHORT_TO_GND_DETECTED = 13; PHASE_W_SHORT_TO_VS_DETECTED = 14; + + /*********** STSPIN Faults ************/ + NO_FAULT = 15; + DURATION = 16; + OVER_VOLT = 17; + UNDER_VOLT = 18; + OVER_TEMP = 19; + START_UP = 20; + SPEED_FDBK = 21; + OVER_CURR = 22; + SW_ERROR = 23; + SAMPLE_FAULT = 24; + OVERCURR_SW = 25; + DP_FAULT = 26; } message DriveUnit @@ -113,13 +128,15 @@ message DriveUnit /* Data from all four drive units and the dribbler */ message MotorStatus { - DriveUnit front_left = 1; - DriveUnit front_right = 2; - DriveUnit back_left = 3; - DriveUnit back_right = 4; - DribblerStatus dribbler = 5; - Vector local_velocity = 6; - AngularVelocity angular_velocity = 7; + DriveUnit front_left = 1; + DriveUnit front_right = 2; + DriveUnit back_left = 3; + DriveUnit back_right = 4; + DribblerStatus dribbler = 5; + Vector local_velocity = 6; + AngularVelocity angular_velocity = 7; + Vector target_local_velocity = 8; + AngularVelocity target_angular_velocity = 9; } /* Data about the network connection with the robots, including network-derived values */ diff --git a/src/software/constants.h b/src/software/constants.h index d1592e54b1..4e549fa4cf 100644 --- a/src/software/constants.h +++ b/src/software/constants.h @@ -53,7 +53,7 @@ const std::string ROBOT_KICK_EXP_COEFF_CONFIG_KEY = "kick_coeff"; const std::string ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY = "chip_pulse_width"; const std::string SSL_VISION_ADDRESS = "224.5.23.2"; -static constexpr unsigned int SSL_VISION_PORT = 10020; +static constexpr unsigned int SSL_VISION_PORT = 10006; const std::string SSL_REFEREE_ADDRESS = "224.5.23.1"; static constexpr unsigned int SSL_REFEREE_PORT = 10003; diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index d28b7ff4eb..5ee03cb54c 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -2,6 +2,29 @@ load("@bazel_skylib//rules:common_settings.bzl", "string_flag") package(default_visibility = ["//visibility:public"]) +config_setting( + name = "build_trinamic", + flag_values = { + ":motor_board": "TRINAMIC", + }, +) + +config_setting( + name = "build_stspin", + flag_values = { + ":motor_board": "STSPIN", + }, +) + +string_flag( + name = "motor_board", + build_setting_default = "TRINAMIC", + values = [ + "TRINAMIC", + "STSPIN", + ], +) + cc_library( name = "primitive_executor", srcs = ["primitive_executor.cpp"], @@ -19,6 +42,15 @@ cc_library( ], ) +cc_library( + name = "spi_utils", + srcs = ["spi_utils.cpp"], + hdrs = ["spi_utils.h"], + deps = [ + "//software/logger", + ], +) + cc_library( name = "thunderloop", srcs = ["thunderloop.cpp"], @@ -37,24 +69,6 @@ cc_library( ], ) -cc_library( - name = "gpio", - srcs = [ - "gpio_char_dev.cpp", - "gpio_sysfs.cpp", - ], - hdrs = [ - "gpio.h", - "gpio_char_dev.h", - "gpio_sysfs.h", - ], - deps = [ - "//software/logger", - "//software/logger:network_logger", - "//software/util/make_enum", - ], -) - cc_binary( name = "thunderloop_main", srcs = ["thunderloop_main.cpp"], diff --git a/src/software/embedded/ansible/BUILD b/src/software/embedded/ansible/BUILD index 1225195e86..2417b4b99e 100644 --- a/src/software/embedded/ansible/BUILD +++ b/src/software/embedded/ansible/BUILD @@ -23,8 +23,8 @@ py_binary( "//software/embedded:thunderloop_main", "//software/embedded/linux_configs/pi:pi_files", "//software/embedded/linux_configs/systemd:systemd_files", + "//software/embedded/motor_controller:stspin_motor_controller_test", "//software/embedded/robot_diagnostics_cli:robot_diagnostics_cli_tar", - "//software/embedded/services:robot_auto_test", "//software/power:powerloop_tar", ], deps = [ diff --git a/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml b/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml new file mode 100644 index 0000000000..54676dd0fd --- /dev/null +++ b/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml @@ -0,0 +1,42 @@ +--- +- name: Deploy stspin_motor_controller_test binary to robot + hosts: THUNDERBOTS_HOSTS + + tasks: + - name: Log Robot ID + ansible.builtin.debug: + msg: "[Robot ID = {{ inventory_hostname }}]" + tags: always + + - name: Deploy stspin_motor_controller_test binary + block: + - name: Stop Services + become: true + become_method: ansible.builtin.sudo + ansible.builtin.systemd: + name: "thunderloop.service" + masked: false + daemon_reload: true + state: stopped + tags: always + + - name: Delete stspin_motor_controller_test binary under ~/thunderbots_binaries + ansible.builtin.file: + state: absent + path: ~/thunderbots_binaries/stspin_motor_controller_test + become_method: ansible.builtin.sudo + become: true + register: result + + - name: Log where Ansible is searching + ansible.builtin.debug: + msg: "{{ ansible_search_path }}" + tags: always + + - name: Sync Binary + ansible.posix.synchronize: + src: ../../motor_controller/stspin_motor_controller_test + dest: ~/thunderbots_binaries/ + recursive: true + copy_links: true + tags: always diff --git a/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml b/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml deleted file mode 100644 index 5e337bacf4..0000000000 --- a/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml +++ /dev/null @@ -1,66 +0,0 @@ ---- -- name: Robot Auto Test Playbook - hosts: THUNDERBOTS_HOSTS - - tasks: - - name: Log Robot ID - ansible.builtin.debug: - msg: "[Robot ID = {{ inventory_hostname }}]" - tags: always - - - name: Start Robot Auto Test Process - block: - - name: Stop Services - become: true - become_method: ansible.builtin.sudo - ansible.builtin.systemd: - name: "thunderloop.service" - masked: false - daemon_reload: true - state: stopped - tags: always - - - name: Delete robot_auto_test binary under ~/thunderbots_binaries - ansible.builtin.file: - state: absent - path: ~/thunderbots_binaries/robot_auto_test - become_method: ansible.builtin.sudo - become: true - register: result - - - name: Log where Ansible is searching - ansible.builtin.debug: - msg: "{{ ansible_search_path }}" - tags: always - - - name: Sync Binary - ansible.posix.synchronize: - src: ../../services/robot_auto_test - dest: ~/thunderbots_binaries/ - recursive: true - copy_links: true - tags: always - - - name: Run Binary - ansible.builtin.command: "./robot_auto_test" - args: - chdir: /home/robot/thunderbots_binaries - become_method: ansible.builtin.sudo - become: true - register: robot_auto_test_output - changed_when: true - ignore_errors: true - - - name: Print Binary Output - ansible.builtin.debug: - var: robot_auto_test_output.stdout - - - name: Start Services - become: true - become_method: ansible.builtin.sudo - ansible.builtin.systemd: - name: "thunderloop.service" - masked: false - daemon_reload: true - state: started - tags: always diff --git a/src/software/embedded/constants/BUILD b/src/software/embedded/constants/BUILD index 1d6c07337c..1f3467963e 100644 --- a/src/software/embedded/constants/BUILD +++ b/src/software/embedded/constants/BUILD @@ -1,12 +1,5 @@ package(default_visibility = ["//visibility:public"]) -cc_library( - name = "constants", - hdrs = [ - "constants.h", - ], -) - py_library( name = "py_constants", srcs = [":py_constants.py"], diff --git a/src/software/embedded/constants/constants.h b/src/software/embedded/constants/constants.h deleted file mode 100644 index 3a811a5d7c..0000000000 --- a/src/software/embedded/constants/constants.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -// Constants for the Raspberry Pi - -constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = 16; -constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO = 19; -constexpr int MOTOR_DRIVER_RESET_GPIO = 12; -constexpr int DRIVER_CONTROL_ENABLE_GPIO = 22; - -// Path to the CPU thermal zone temperature file -constexpr const char CPU_TEMP_FILE_PATH[] = "/sys/class/thermal/thermal_zone0/temp"; diff --git a/src/software/embedded/gpio/BUILD b/src/software/embedded/gpio/BUILD new file mode 100644 index 0000000000..a09031ccf2 --- /dev/null +++ b/src/software/embedded/gpio/BUILD @@ -0,0 +1,18 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "gpio", + srcs = [ + "gpio.cpp", + "gpio_char_dev.cpp", + ], + hdrs = [ + "gpio.h", + "gpio_char_dev.h", + ], + deps = [ + "//software/logger", + "//software/logger:network_logger", + "//software/util/make_enum", + ], +) diff --git a/src/software/embedded/gpio/gpio.cpp b/src/software/embedded/gpio/gpio.cpp new file mode 100644 index 0000000000..3fd57d4179 --- /dev/null +++ b/src/software/embedded/gpio/gpio.cpp @@ -0,0 +1,21 @@ +#include "software/embedded/gpio/gpio.h" + +#include +#include + +bool Gpio::pollValue(GpioState state, std::chrono::milliseconds timeout_ms) +{ + const auto start_time = std::chrono::system_clock::now(); + while (getValue() != state) + { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + + const auto current_time = std::chrono::system_clock::now(); + const auto elapsed_time = current_time - start_time; + if (elapsed_time > timeout_ms) + { + return false; + } + } + return true; +} diff --git a/src/software/embedded/gpio.h b/src/software/embedded/gpio/gpio.h similarity index 51% rename from src/software/embedded/gpio.h rename to src/software/embedded/gpio/gpio.h index c07e15c239..da60b19c50 100644 --- a/src/software/embedded/gpio.h +++ b/src/software/embedded/gpio/gpio.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "software/util/make_enum/make_enum.hpp" MAKE_ENUM(GpioState, LOW, HIGH); @@ -22,4 +24,15 @@ class Gpio * Get the current state of the gpio */ virtual GpioState getValue() = 0; + + /** + * Polls the GPIO until it reaches the requested state or a timeout occurs + * + * @param state The GPIO state to wait for + * @param timeout_ms Maximum wait duration in milliseconds + * + * @return true if the GPIO reached the requested state within the timeout, + * false if timed out before the GPIO reached the requested state + */ + bool pollValue(GpioState state, std::chrono::milliseconds timeout_ms); }; diff --git a/src/software/embedded/gpio_char_dev.cpp b/src/software/embedded/gpio/gpio_char_dev.cpp similarity index 97% rename from src/software/embedded/gpio_char_dev.cpp rename to src/software/embedded/gpio/gpio_char_dev.cpp index 772a0f6f65..1ddf9b4f51 100644 --- a/src/software/embedded/gpio_char_dev.cpp +++ b/src/software/embedded/gpio/gpio_char_dev.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/gpio_char_dev.h" +#include "software/embedded/gpio/gpio_char_dev.h" #include diff --git a/src/software/embedded/gpio_char_dev.h b/src/software/embedded/gpio/gpio_char_dev.h similarity index 92% rename from src/software/embedded/gpio_char_dev.h rename to src/software/embedded/gpio/gpio_char_dev.h index 87e69d3141..0b013c0171 100644 --- a/src/software/embedded/gpio_char_dev.h +++ b/src/software/embedded/gpio/gpio_char_dev.h @@ -4,7 +4,7 @@ #include -#include "software/embedded/gpio.h" +#include "software/embedded/gpio/gpio.h" /** * GPIO with the character device interface @@ -24,7 +24,7 @@ class GpioCharDev : public Gpio * @param char_dev_path The path to the gpio character device */ GpioCharDev(uint32_t gpio_number, GpioDirection direction, GpioState state, - std::string char_dev_path = "/dev/gpiochip0"); + std::string char_dev_path = "/dev/gpiochip4"); /** * Destructor diff --git a/src/software/embedded/gpio_sysfs.cpp b/src/software/embedded/gpio_sysfs.cpp deleted file mode 100644 index 233f11d912..0000000000 --- a/src/software/embedded/gpio_sysfs.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include "software/embedded/gpio_sysfs.h" - -#include -#include -#include - -#include -#include -#include - -#include "software/logger/logger.h" - -GpioSysfs::GpioSysfs(const std::string& gpio_number, GpioDirection direction, - GpioState initial_state) -{ - // Setup the provided GPIO pin - gpio_number_ = gpio_number; - - auto export_gpio_fs = std::ofstream("/sys/class/gpio/export"); - export_gpio_fs << gpio_number; - export_gpio_fs.close(); - - auto set_direction_fs = - std::ofstream("/sys/class/gpio/gpio" + gpio_number + "/direction"); - - switch (direction) - { - case GpioDirection::OUTPUT: - { - set_direction_fs << "out"; - break; - } - case GpioDirection::INPUT: - { - set_direction_fs << "in"; - break; - } - } - - set_direction_fs.close(); - setValue(initial_state); - - LOG(DEBUG) << "GPIO " << gpio_number_ << " online"; -} - -void GpioSysfs::setValue(GpioState state) -{ - std::ofstream gpio_fs("/sys/class/gpio/gpio" + gpio_number_ + "/value"); - - CHECK(gpio_fs.is_open()) << "Could not set GPIO pin"; - - switch (state) - { - case GpioState::HIGH: - { - gpio_fs << "1"; - break; - } - case GpioState::LOW: - { - gpio_fs << "0"; - break; - } - } - - if (gpio_fs.is_open()) - { - gpio_fs.close(); - } -} - -GpioState GpioSysfs::getValue() -{ - std::ifstream gpio_fs("/sys/class/gpio/gpio" + gpio_number_ + "/value"); - std::string level; - - CHECK(gpio_fs.is_open()) << "Could not read GPIO pin"; - std::getline(gpio_fs, level); - - if (level.compare("0") == 0) - { - return GpioState::LOW; - } - else - { - return GpioState::HIGH; - } -} diff --git a/src/software/embedded/gpio_sysfs.h b/src/software/embedded/gpio_sysfs.h deleted file mode 100644 index 8d8501cd04..0000000000 --- a/src/software/embedded/gpio_sysfs.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "software/embedded/gpio.h" - -/** - * GPIO with the sysfs interface - * - * Deprecated in Linux kernels 4.8 and later but still available on some systems. - */ -class GpioSysfs : public Gpio -{ - public: - /* - * GPIO Sysfs Wrapper - * - * See https://www.kernel.org/doc/Documentation/gpio/sysfs.txt - * - * @param gpio The gpio to setup - * @param direction The direction to configure this gpio in - * @param initial_state The initial GpioState of the pin - */ - GpioSysfs(const std::string& gpio_number, GpioDirection direction, - GpioState initial_state); - - /** - * Set the value to the provided state - * - * @param state The state - */ - void setValue(GpioState state) override; - - /** - * Get the current state of the gpio - */ - GpioState getValue() override; - - private: - std::string gpio_number_; -}; diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD new file mode 100644 index 0000000000..26518edbfd --- /dev/null +++ b/src/software/embedded/motor_controller/BUILD @@ -0,0 +1,73 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "motor_index", + hdrs = ["motor_index.h"], + deps = [ + "//software/util/make_enum", + ], +) + +cc_library( + name = "motor_board", + hdrs = ["motor_board.h"], + deps = [ + "//software/util/make_enum", + ], +) + +cc_library( + name = "motor_controller", + srcs = [ + "stspin_motor_controller.cpp", + "tmc_motor_controller.cpp", + ], + hdrs = [ + "motor_controller.h", + "stspin_motor_controller.h", + "tmc_motor_controller.h", + ], + deps = [ + ":motor_board", + ":motor_fault_indicator", + ":motor_index", + ":stspin_types", + "//proto/message_translation:tbots_protobuf", + "//software/embedded:spi_utils", + "//software/embedded/gpio", + "//software/logger", + "//software/physics:euclidean_to_wheel", + "@cppcrc", + "@trinamic", + ], +) + +cc_library( + name = "motor_fault_indicator", + srcs = ["motor_fault_indicator.cpp"], + hdrs = ["motor_fault_indicator.h"], + deps = [ + ":motor_board", + "//proto:tbots_cc_proto", + ], +) + +cc_library( + name = "stspin_types", + hdrs = ["stspin_types.h"], +) + +cc_binary( + name = "stspin_motor_controller_test", + srcs = ["stspin_motor_controller_test.cpp"], + linkopts = [ + "-lpthread", + "-lrt", + ], + deps = [ + ":motor_controller", + "//software/logger", + "//software/logger:network_logger", + "@boost//:program_options", + ], +) diff --git a/src/software/embedded/motor_controller/motor_board.h b/src/software/embedded/motor_controller/motor_board.h new file mode 100644 index 0000000000..5076a78064 --- /dev/null +++ b/src/software/embedded/motor_controller/motor_board.h @@ -0,0 +1,11 @@ +#pragma once + +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(MotorBoard, TRINAMIC, STSPIN); + +#ifdef TRINAMIC_MOTOR_BOARD +constexpr MotorBoard MOTOR_BOARD = MotorBoard::TRINAMIC; +#elif STSPIN_MOTOR_BOARD +constexpr MotorBoard MOTOR_BOARD = MotorBoard::STSPIN; +#endif diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h new file mode 100644 index 0000000000..5682a2791e --- /dev/null +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -0,0 +1,57 @@ +#pragma once + +#include "software/embedded/motor_controller/motor_fault_indicator.h" +#include "software/embedded/motor_controller/motor_index.h" +#include "software/physics/euclidean_to_wheel.h" + +/** + * Abstract interface for motor controllers. + * + * Implementations provide hardware-specific setup, fault reporting, + * and velocity control for the robot's motors. + */ +class MotorController +{ + public: + virtual ~MotorController() = default; + + /** + * Initializes the motor controller hardware and any internal state. + */ + virtual void setup() = 0; + + /** + * Resets the underlying motor controller hardware. + */ + virtual void reset() = 0; + + /** + * Returns the current fault state for the given motor. + * + * @param motor the motor to inspect + * @return a MotorFaultIndicator indicating the faults for that motor + */ + virtual const MotorFaultIndicator& checkFaults(MotorIndex motor) = 0; + + /** + * Reads the current velocity and writes a new target velocity for a motor. + * + * @param motor the motor to command + * @param target_velocity the desired target velocity for the motor in mechanical RPM + * @return the current measured velocity of the motor in mechanical RPM + */ + virtual int readThenWriteVelocity(MotorIndex motor, int target_velocity) = 0; + + /** + * Updates the motor controller with the target local velocity of the robot + * in Euclidean space. + * + * @param target_euclidean_velocity the target local velocity of the robot + */ + virtual void updateEuclideanVelocity(EuclideanSpace_t target_euclidean_velocity) = 0; + + /** + * Immediately disables all motors. + */ + virtual void immediatelyDisable() = 0; +}; diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.cpp b/src/software/embedded/motor_controller/motor_fault_indicator.cpp new file mode 100644 index 0000000000..366710838b --- /dev/null +++ b/src/software/embedded/motor_controller/motor_fault_indicator.cpp @@ -0,0 +1,15 @@ +#include "software/embedded/motor_controller/motor_fault_indicator.h" + +MotorFaultIndicator::MotorFaultIndicator() : drive_enabled(true) {} + +MotorFaultIndicator::MotorFaultIndicator( + const bool drive_enabled, + const std::unordered_set& motor_faults) + : drive_enabled(drive_enabled), faults(motor_faults) +{ +} + +bool MotorFaultIndicator::requiresReset() const +{ + return !drive_enabled || faults.find(TbotsProto::MotorFault::RESET) != faults.end(); +} diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.h b/src/software/embedded/motor_controller/motor_fault_indicator.h new file mode 100644 index 0000000000..9e5419b787 --- /dev/null +++ b/src/software/embedded/motor_controller/motor_fault_indicator.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include "proto/robot_status_msg.pb.h" + +/** + * Holds motor fault information for a particular motor and whether any fault has + * caused the motor to be disabled. + */ +struct MotorFaultIndicator +{ + bool drive_enabled; + std::unordered_set faults; + + /** + * Construct a default indicator of no faults and running motors. + */ + MotorFaultIndicator(); + + /** + * Construct an indicator with faults and whether the motor is enabled. + * + * @param drive_enabled true if the motor is enabled, false if disabled due to a + * motor fault + * @param motor_faults a set of faults associated with this motor + */ + MotorFaultIndicator(bool drive_enabled, + const std::unordered_set& motor_faults); + + /** + * Checks if the motor requires a reset due to being disabled or + * having a reset fault. + * + * @return true if the motor requires a reset, false otherwise + */ + bool requiresReset() const; +}; diff --git a/src/software/embedded/motor_controller/motor_index.h b/src/software/embedded/motor_controller/motor_index.h new file mode 100644 index 0000000000..4a58a63d6a --- /dev/null +++ b/src/software/embedded/motor_controller/motor_index.h @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(MotorIndex, FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT, DRIBBLER); + +constexpr std::array driveMotors() +{ + return { + MotorIndex::FRONT_LEFT, + MotorIndex::FRONT_RIGHT, + MotorIndex::BACK_LEFT, + MotorIndex::BACK_RIGHT, + }; +} diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp new file mode 100644 index 0000000000..7f7164c407 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -0,0 +1,462 @@ +#include "software/embedded/motor_controller/stspin_motor_controller.h" + +#include + +#include +#include +#include + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wconversion" +#include "cppcrc.h" +#pragma GCC diagnostic pop + +#include "proto/message_translation/tbots_protobuf.h" +#include "software/embedded/gpio/gpio_char_dev.h" +#include "software/embedded/motor_controller/stspin_types.h" +#include "software/embedded/spi_utils.h" +#include "software/logger/logger.h" + +// AUTOSAR variant of CRC-8 +// (https://reveng.sourceforge.io/crc-catalogue/all.htm#crc.cat.crc-8-autosar) +using Crc8Autosar = crc_utils::crc; + +StSpinMotorController::StSpinMotorController( + const robot_constants::RobotConstants& robot_constants) + : robot_constants_(robot_constants), + reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, + GpioState::HIGH)) +{ + for (const MotorIndex motor : driveMotors()) + { + openSpiFileDescriptor(motor); + } +} + +void StSpinMotorController::setup() +{ + reset(); + + for (const MotorIndex motor : reflective_enum::values()) + { + motor_status_[motor] = MotorStatus(); + motor_status_[motor].enabled = true; + } + + for (const MotorIndex motor : driveMotors()) + { + sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = SPEED_PID_PROPORTIONAL_GAIN, + .ki = SPEED_PID_INTEGRAL_GAIN}); + } +} + +void StSpinMotorController::reset() +{ + reset_gpio_->setValue(GpioState::LOW); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + reset_gpio_->setValue(GpioState::HIGH); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); +} + +const MotorFaultIndicator& StSpinMotorController::checkFaults(const MotorIndex motor) +{ + return motor_status_.at(motor).faults; +} + +void StSpinMotorController::updateFaults(const MotorIndex motor, + const uint16_t fault_flags) +{ + MotorStatus& motor_status = motor_status_.at(motor); + + if (motor_status.fault_flags == fault_flags) + { + // No change in faults + return; + } + + motor_status.fault_flags = fault_flags; + MotorFaultIndicator& motor_faults = motor_status.faults; + motor_faults.drive_enabled = true; + motor_faults.faults.clear(); + + if (fault_flags == 0) + { + // No faults + return; + } + + std::ostringstream oss; + oss << "======= Faults For Motor " << motor << "=======\n"; + + if (fault_flags & static_cast(StSpinFaultCode::DURATION)) + { + oss << "DURATION: FOC rate too high\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::DURATION); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::OVER_VOLT)) + { + oss << "OVER_VOLT: Over voltage\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_VOLT); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::UNDER_VOLT)) + { + oss << "UNDER_VOLT: Under voltage\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::UNDER_VOLT); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::OVER_TEMP)) + { + oss << "OVER_TEMP: Over temperature\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_TEMP); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::START_UP)) + { + oss << "START_UP: Start up failed\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::START_UP); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::SPEED_FDBK)) + { + oss << "SPEED_FDBK: Speed feedback fault\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::SPEED_FDBK); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::OVER_CURR)) + { + oss << "OVER_CURR: Over current\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_CURR); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::SW_ERROR)) + { + oss << "SW_ERROR: Software error\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::SW_ERROR); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::SAMPLE_FAULT)) + { + oss << "SAMPLE_FAULT: Sample fault for testing purposes\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::SAMPLE_FAULT); + } + + if (fault_flags & static_cast(StSpinFaultCode::OVERCURR_SW)) + { + oss << "OVERCURR_SW: Software over current\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::OVERCURR_SW); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::DP_FAULT)) + { + oss << "DP_FAULT: Driver protection fault\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::DP_FAULT); + motor_faults.drive_enabled = false; + } + + LOG(WARNING) << oss.str(); +} + +int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, + const int target_velocity) +{ + if (motor == MotorIndex::DRIBBLER) + { + return 0; + } + + const auto outgoing_frame = SetTargetSpeedFrame{ + .motor_enabled = motor_status_.at(motor).enabled, + .motor_target_speed_rpm = static_cast(target_velocity), + }; + + sendAndReceiveFrame(motor, outgoing_frame); + + return motor_status_.at(motor).speed; +} + +void StSpinMotorController::updateEuclideanVelocity( + EuclideanSpace_t target_euclidean_velocity) +{ + const Vector local_velocity(target_euclidean_velocity[1], + -target_euclidean_velocity[0]); + + if (local_velocity.length() <= 0.01) + { + sendAndReceiveFrame( + MotorIndex::FRONT_LEFT, + SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveFrame( + MotorIndex::FRONT_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveFrame( + MotorIndex::BACK_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveFrame( + MotorIndex::BACK_LEFT, + SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + return; + } + + const Angle direction = local_velocity.orientation(); + + const Angle front_wheel_angle = + Angle::fromDegrees(robot_constants_.front_wheel_angle_deg); + const Angle back_wheel_angle = + Angle::fromDegrees(robot_constants_.back_wheel_angle_deg); + + const int16_t front_left_ks = static_cast( + MAX_SPEED_FEED_FORWARD_STATIC_GAIN * + std::abs((direction - Angle::quarter() + front_wheel_angle).sin())); + const int16_t front_right_ks = static_cast( + MAX_SPEED_FEED_FORWARD_STATIC_GAIN * + std::abs((direction + Angle::quarter() - front_wheel_angle).sin())); + const int16_t back_right_ks = static_cast( + MAX_SPEED_FEED_FORWARD_STATIC_GAIN * + std::abs((direction + Angle::quarter() + back_wheel_angle).sin())); + const int16_t back_left_ks = static_cast( + MAX_SPEED_FEED_FORWARD_STATIC_GAIN * + std::abs((direction - Angle::quarter() - back_wheel_angle).sin())); + + sendAndReceiveFrame(MotorIndex::FRONT_LEFT, + SetSpeedFeedForwardKsFrame{.ks = front_left_ks}); + sendAndReceiveFrame(MotorIndex::FRONT_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = front_right_ks}); + sendAndReceiveFrame(MotorIndex::BACK_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = back_right_ks}); + sendAndReceiveFrame(MotorIndex::BACK_LEFT, + SetSpeedFeedForwardKsFrame{.ks = back_left_ks}); +} + +void StSpinMotorController::immediatelyDisable() +{ + for (const MotorIndex motor : reflective_enum::values()) + { + motor_status_[motor].enabled = false; + readThenWriteVelocity(motor, 0); + } +} + +void StSpinMotorController::openSpiFileDescriptor(const MotorIndex motor) +{ + spi_fds_[motor] = open(SPI_PATHS.at(motor), O_RDWR); + CHECK(spi_fds_[motor] >= 0) + << "can't open device: " << motor << "error: " << strerror(errno); + + int ret = ioctl(spi_fds_[motor], SPI_IOC_WR_MODE32, &SPI_MODE); + CHECK(ret != -1) << "can't set spi mode for: " << motor + << "error: " << strerror(errno); + + ret = ioctl(spi_fds_[motor], SPI_IOC_WR_BITS_PER_WORD, &SPI_BITS); + CHECK(ret != -1) << "can't set bits_per_word for: " << motor + << "error: " << strerror(errno); + + ret = ioctl(spi_fds_[motor], SPI_IOC_WR_MAX_SPEED_HZ, &MAX_SPI_SPEED_HZ); + CHECK(ret != -1) << "can't set spi max speed hz for: " << motor + << "error: " << strerror(errno); +} + +void StSpinMotorController::sendAndReceiveFrame(const MotorIndex motor, + const OutgoingFrame& outgoing_frame) +{ + std::array tx{}; + std::array rx{}; + + populateTx(outgoing_frame, tx); + + spiTransfer(spi_fds_[motor], tx.data(), rx.data(), FRAME_LEN, SPI_SPEED_HZ); + + motor_status_[motor].frame_count++; + + // Frame integrity check + const uint8_t rx_crc = Crc8Autosar::calc(rx.data(), FRAME_LEN - 1); + if (rx[5] != rx_crc) + { + // Log RX buffer and expected vs. actual CRC + std::ostringstream oss; + oss << std::hex << std::uppercase << std::setfill('0') << "Expected CRC 0x" + << std::setw(2) << static_cast(rx[5]) << " but got 0x" << std::setw(2) + << static_cast(rx_crc) << ". RX: "; + for (const uint8_t byte : rx) + { + oss << "0x" << std::setw(2) << static_cast(byte) << " "; + } + + LOG(WARNING) << "Frame #" << motor_status_[motor].frame_count + << " received from motor " << motor << " failed integrity check. " + << oss.str(); + return; + } + + processRx(motor, rx); +} + +void StSpinMotorController::populateTx(const OutgoingFrame& outgoing_frame, + std::array& tx) +{ + std::visit( + [&](TFrame&& frame) + { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::NO_OP); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_TARGET_SPEED); + tx[1] = static_cast(frame.motor_enabled); + tx[2] = static_cast(0xFF & (frame.motor_target_speed_rpm >> 8)); + tx[3] = static_cast(0xFF & frame.motor_target_speed_rpm); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_TARGET_TORQUE); + tx[1] = static_cast(frame.motor_enabled); + tx[2] = static_cast(0xFF & (frame.motor_target_torque >> 8)); + tx[3] = static_cast(0xFF & frame.motor_target_torque); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_RESPONSE_TYPE); + tx[1] = static_cast(frame.response_type); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_PID_TORQUE_KP_KI); + tx[1] = static_cast(0xFF & (frame.kp >> 8)); + tx[2] = static_cast(0xFF & frame.kp); + tx[3] = static_cast(0xFF & (frame.ki >> 8)); + tx[4] = static_cast(0xFF & frame.ki); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_PID_FLUX_KP_KI); + tx[1] = static_cast(0xFF & (frame.kp >> 8)); + tx[2] = static_cast(0xFF & frame.kp); + tx[3] = static_cast(0xFF & (frame.ki >> 8)); + tx[4] = static_cast(0xFF & frame.ki); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_PID_SPEED_KP_KI); + tx[1] = static_cast(0xFF & (frame.kp >> 8)); + tx[2] = static_cast(0xFF & frame.kp); + tx[3] = static_cast(0xFF & (frame.ki >> 8)); + tx[4] = static_cast(0xFF & frame.ki); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_SPEED_FEED_FORWARD_KA_KV); + tx[1] = static_cast(0xFF & (frame.ka >> 8)); + tx[2] = static_cast(0xFF & frame.ka); + tx[3] = static_cast(0xFF & (frame.kv >> 8)); + tx[4] = static_cast(0xFF & frame.kv); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_SPEED_FEED_FORWARD_KS); + tx[1] = static_cast(0xFF & (frame.ks >> 8)); + tx[2] = static_cast(0xFF & frame.ks); + } + }, + outgoing_frame); + + tx[5] = Crc8Autosar::calc(tx.data(), FRAME_LEN - 1); +} + +void StSpinMotorController::processRx(const MotorIndex motor, + const std::array& rx) +{ + switch (static_cast(rx[0])) + { + case StSpinResponseType::SPEED_AND_FAULTS: + { + motor_status_[motor].speed = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + const uint16_t fault_flags = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + updateFaults(motor, fault_flags); + break; + } + case StSpinResponseType::IQ_AND_ID: + { + motor_status_[motor].iq = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].id = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::VQ_AND_VD: + { + motor_status_[motor].vq = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].vd = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::PHASE_CURRENT_AND_VOLTAGE: + { + motor_status_[motor].phase_current = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].phase_voltage = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::IQ_AND_IQ_REF: + { + motor_status_[motor].iq = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].iq_ref = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::ID_AND_ID_REF: + { + motor_status_[motor].id = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].id_ref = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::SPEED_AND_SPEED_REF: + { + motor_status_[motor].speed = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].speed_ref = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + } +} + +void StSpinMotorController::sendMotorStatusToPlotJuggler(const MotorIndex motor) +{ + for (const StSpinResponseType response_type : + {StSpinResponseType::SPEED_AND_SPEED_REF, StSpinResponseType::IQ_AND_IQ_REF, + StSpinResponseType::ID_AND_ID_REF, StSpinResponseType::SPEED_AND_FAULTS}) + { + sendAndReceiveFrame(motor, SetResponseTypeFrame{response_type}); + } + + const MotorStatus& motor_status = motor_status_.at(motor); + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"speed_" + motor, motor_status.speed}, + {"speed_ref_" + motor, motor_status.speed_ref}, + {"iq_" + motor, motor_status.iq}, + {"iq_ref_" + motor, motor_status.iq_ref}, + {"id_" + motor, motor_status.id}, + {"id_ref_" + motor, motor_status.id_ref}, + }); +} diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h new file mode 100644 index 0000000000..8402db8080 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -0,0 +1,143 @@ +#pragma once + +#include "software/embedded/gpio/gpio.h" +#include "software/embedded/motor_controller/motor_controller.h" +#include "software/embedded/motor_controller/motor_fault_indicator.h" +#include "software/embedded/motor_controller/motor_index.h" +#include "software/embedded/motor_controller/stspin_types.h" + +/** + * Motor controller for controlling our 6th generation STSPIN motor drivers. + * + * We communicate with our MDv6 boards over SPI in full-duplex mode using a custom + * frame-based protocol. This protocol is documented in the MDv6 firmware repo, which + * can be found at https://github.com/UBC-Thunderbots/MDv6_Firmware + */ +class StSpinMotorController : public MotorController +{ + public: + explicit StSpinMotorController( + const robot_constants::RobotConstants& robot_constants); + + void setup() override; + + void reset() override; + + const MotorFaultIndicator& checkFaults(MotorIndex motor) override; + + int readThenWriteVelocity(MotorIndex motor, int target_velocity) override; + + void updateEuclideanVelocity(EuclideanSpace_t target_euclidean_velocity) override; + + void immediatelyDisable() override; + + private: + using OutgoingFrame = + std::variant; + + // Length of frame (in number of bytes) + static constexpr unsigned int FRAME_LEN = 6; + + // clang-format off + static const inline std::unordered_map SPI_PATHS = { + {MotorIndex::FRONT_LEFT, "/dev/spidev0.2"}, + {MotorIndex::FRONT_RIGHT, "/dev/spidev0.4"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.3"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.0"}, + }; + // clang-format on + + static constexpr uint32_t SPI_SPEED_HZ = 100000; + static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; + static constexpr uint8_t SPI_BITS = 8; + static constexpr uint32_t SPI_MODE = 0; + + static constexpr int RESET_GPIO_PIN = 12; + + static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 700; + static constexpr int SPEED_PID_INTEGRAL_GAIN = 30; + + static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750; + static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 300; + + robot_constants::RobotConstants robot_constants_; + + // SPI file descriptors + std::unordered_map spi_fds_; + + std::unique_ptr reset_gpio_; + + struct MotorStatus + { + unsigned int frame_count; + bool enabled; + MotorFaultIndicator faults; + uint16_t fault_flags; + int16_t speed; + int16_t speed_ref; + int16_t iq; + int16_t iq_ref; + int16_t id; + int16_t id_ref; + int16_t vq; + int16_t vd; + int16_t phase_current; + int16_t phase_voltage; + }; + + std::unordered_map motor_status_; + + /** + * Opens a SPI file descriptor for the given motor. + * + * @param motor the motor to open a SPI file descriptor for + */ + void openSpiFileDescriptor(MotorIndex motor); + + /** + * Transmits a frame to the given motor and receives a frame back over SPI. + * + * @param motor the motor to send the frame to + * @param outgoing_frame the outgoing frame to send to the motor + */ + void sendAndReceiveFrame(MotorIndex motor, const OutgoingFrame& outgoing_frame); + + /** + * Populates the transmit buffer with the data from an outgoing frame. + * + * @param outgoing_frame the outgoing frame to populate the transmit buffer with + * @param tx the transmit buffer to populate with the outgoing frame's data + */ + void populateTx(const OutgoingFrame& outgoing_frame, + std::array& tx); + + /** + * Processes a frame received from the given motor, caching any motor status + * the frame provides. + * + * @param motor the motor that the received frame corresponds to + * @param rx the receive buffer with the received frame to process + */ + void processRx(MotorIndex motor, const std::array& rx); + + /** + * Records that the given faults were raised for a given motor. + * + * @param motor the motor to update the faults of + * @param fault_flags the faults for the motor + */ + void updateFaults(MotorIndex motor, uint16_t fault_flags); + + /** + * Sends the target and measured speed and current (Iq and Id) for the + * specified motor to PlotJuggler. + * + * @param motor the motor to plot the status of + */ + void sendMotorStatusToPlotJuggler(MotorIndex motor); + + friend class StSpinMotorControllerTest; +}; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp new file mode 100644 index 0000000000..1f54b929f5 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -0,0 +1,282 @@ +#include "software/embedded/motor_controller/stspin_motor_controller.h" + +#include +#include +#include +#include +#include + +#include "software/logger/logger.h" +#include "software/logger/network_logger.h" + +// Flag to indicate when a stop has been requested (e.g. via Ctrl+C) +static volatile std::sig_atomic_t g_stop_requested = false; + +class StSpinMotorControllerTest +{ + public: + struct CommandLineArgs + { + struct PidOption + { + std::optional kp; + std::optional ki; + }; + + bool help = false; + std::string mode = "speed"; + int setpoint_duration_ms = 2000; + std::vector setpoints; + std::unordered_set enabled_motors; + PidOption torque_pid; + PidOption flux_pid; + PidOption speed_pid; + }; + + explicit StSpinMotorControllerTest(CommandLineArgs args) : args_(std::move(args)) {} + + void runMotorTest() const + { + LOG(INFO) << "Running Motor Test"; + + std::stringstream ss; + for (const MotorIndex motor : args_.enabled_motors) + { + ss << motor << " "; + } + LOG(INFO) << "Enabled motors: " << ss.str(); + + const auto motor_controller = std::make_unique( + robot_constants::createRobotConstants()); + motor_controller->setup(); + + LOG(INFO) << "Motor controller setup complete"; + + for (const MotorIndex motor : args_.enabled_motors) + { + if (args_.torque_pid.kp && args_.torque_pid.ki) + { + motor_controller->sendAndReceiveFrame( + motor, SetPidTorqueKpKiFrame{.kp = args_.torque_pid.kp.value(), + .ki = args_.torque_pid.ki.value()}); + } + + if (args_.flux_pid.kp && args_.flux_pid.ki) + { + motor_controller->sendAndReceiveFrame( + motor, SetPidFluxKpKiFrame{.kp = args_.flux_pid.kp.value(), + .ki = args_.flux_pid.ki.value()}); + } + + if (args_.speed_pid.kp && args_.speed_pid.ki) + { + motor_controller->sendAndReceiveFrame( + motor, SetPidSpeedKpKiFrame{.kp = args_.speed_pid.kp.value(), + .ki = args_.speed_pid.ki.value()}); + } + } + + LOG(INFO) << "PID configuration complete"; + + LOG(INFO) << "Running " << args_.mode << " profile"; + + for (const int16_t setpoint : args_.setpoints) + { + if (g_stop_requested) + { + break; + } + + StSpinMotorController::OutgoingFrame command_frame; + if (args_.mode == "speed") + { + command_frame = SetTargetSpeedFrame{ + .motor_enabled = true, + .motor_target_speed_rpm = setpoint, + }; + } + else if (args_.mode == "torque") + { + command_frame = SetTargetTorqueFrame{ + .motor_enabled = true, + .motor_target_torque = setpoint, + }; + } + + for (const MotorIndex motor : args_.enabled_motors) + { + motor_controller->sendAndReceiveFrame(motor, command_frame); + } + + auto start_time = std::chrono::steady_clock::now(); + auto duration = std::chrono::milliseconds(args_.setpoint_duration_ms); + while (std::chrono::steady_clock::now() - start_time < duration) + { + if (g_stop_requested) + { + break; + } + + for (const MotorIndex motor : args_.enabled_motors) + { + motor_controller->sendMotorStatusToPlotJuggler(motor); + motor_controller->checkFaults(motor); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } + + motor_controller->immediatelyDisable(); + + LOG(INFO) << "Motor Test Complete"; + } + + private: + CommandLineArgs args_; +}; + +static std::vector parseSetpoints(const std::string& setpoints_str) +{ + std::vector setpoints; + std::stringstream ss(setpoints_str); + std::string item; + + while (std::getline(ss, item, ',')) + { + if (!item.empty()) + { + setpoints.push_back(static_cast(std::stoi(item))); + } + } + + return setpoints; +} + +static std::unordered_set parseEnabledMotors( + const std::string& enabled_motors_str) +{ + if (enabled_motors_str == "all") + { + constexpr auto all_motors = driveMotors(); + return {all_motors.begin(), all_motors.end()}; + } + + std::unordered_set enabled_motors; + std::stringstream ss(enabled_motors_str); + std::string item; + + while (std::getline(ss, item, ',')) + { + if (item == "fl") + { + enabled_motors.insert(MotorIndex::FRONT_LEFT); + } + else if (item == "fr") + { + enabled_motors.insert(MotorIndex::FRONT_RIGHT); + } + else if (item == "bl") + { + enabled_motors.insert(MotorIndex::BACK_LEFT); + } + else if (item == "br") + { + enabled_motors.insert(MotorIndex::BACK_RIGHT); + } + } + + return enabled_motors; +} + +int main(int argc, char** argv) +{ + NetworkLoggerSingleton::initializeLogger(0, true); + + StSpinMotorControllerTest::CommandLineArgs args; + + boost::program_options::options_description desc{"Options"}; + // clang-format off + desc.add_options() + ("help,h", boost::program_options::bool_switch(&args.help), "Show help") + ("mode", boost::program_options::value(&args.mode), + "Control mode: speed | torque") + ("setpoints", boost::program_options::value(), + "Comma-separated setpoints (required)") + ("setpoint_duration", boost::program_options::value(&args.setpoint_duration_ms), + "Duration to hold each setpoint in milliseconds") + ("enabled_motors", boost::program_options::value(), + "Motors to enable: 'all' (default) or comma-separated list (e.g. 'fl,fr,bl,br')") + ("torque_kp", boost::program_options::value(), "Torque PID Kp") + ("torque_ki", boost::program_options::value(), "Torque PID Ki") + ("flux_kp", boost::program_options::value(), "Flux PID Kp") + ("flux_ki", boost::program_options::value(), "Flux PID Ki") + ("speed_kp", boost::program_options::value(), "Speed PID Kp") + ("speed_ki", boost::program_options::value(), "Speed PID Ki"); + // clang-format on + + boost::program_options::variables_map vm; + boost::program_options::store(parse_command_line(argc, argv, desc), vm); + boost::program_options::notify(vm); + + if (args.help) + { + std::cout << desc << std::endl; + return 0; + } + + if (!vm.count("setpoints")) + { + std::cerr << "Error: setpoints not set" << std::endl; + return 1; + } + + if (vm.count("enabled_motors")) + { + args.enabled_motors = + parseEnabledMotors(vm.at("enabled_motors").as()); + } + else + { + args.enabled_motors = parseEnabledMotors("all"); + } + + args.setpoints = parseSetpoints(vm.at("setpoints").as()); + + if (vm.count("torque_kp")) + { + args.torque_pid.kp = vm.at("torque_kp").as(); + } + + if (vm.count("torque_ki")) + { + args.torque_pid.ki = vm.at("torque_ki").as(); + } + + if (vm.count("flux_kp")) + { + args.flux_pid.kp = vm.at("flux_kp").as(); + } + + if (vm.count("flux_ki")) + { + args.flux_pid.ki = vm.at("flux_ki").as(); + } + + if (vm.count("speed_kp")) + { + args.speed_pid.kp = vm.at("speed_kp").as(); + } + + if (vm.count("speed_ki")) + { + args.speed_pid.ki = vm.at("speed_ki").as(); + } + + std::signal(SIGINT, [](int) { g_stop_requested = true; }); + + StSpinMotorControllerTest stspin_motor_controller_test(args); + stspin_motor_controller_test.runMotorTest(); + + return 0; +} diff --git a/src/software/embedded/motor_controller/stspin_types.h b/src/software/embedded/motor_controller/stspin_types.h new file mode 100644 index 0000000000..7b1ba2e3c8 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -0,0 +1,93 @@ +#pragma once + +#include + +enum class StSpinOpcode +{ + NO_OP = 0x00, + SET_TARGET_SPEED = 0x01, + SET_TARGET_TORQUE = 0x02, + SET_RESPONSE_TYPE = 0x03, + SET_PID_TORQUE_KP_KI = 0x04, + SET_PID_FLUX_KP_KI = 0x05, + SET_PID_SPEED_KP_KI = 0x06, + SET_SPEED_FEED_FORWARD_KA_KV = 0x07, + SET_SPEED_FEED_FORWARD_KS = 0x08, +}; + +enum class StSpinResponseType +{ + SPEED_AND_FAULTS = 0x01, + IQ_AND_ID = 0x02, + VQ_AND_VD = 0x03, + PHASE_CURRENT_AND_VOLTAGE = 0x04, + IQ_AND_IQ_REF = 0x05, + ID_AND_ID_REF = 0x06, + SPEED_AND_SPEED_REF = 0x07, +}; + +enum class StSpinFaultCode +{ + NO_FAULT = 0x0000, + DURATION = 0x0001, + OVER_VOLT = 0x0002, + UNDER_VOLT = 0x0004, + OVER_TEMP = 0x0008, + START_UP = 0x0010, + SPEED_FDBK = 0x0020, + OVER_CURR = 0x0040, + SW_ERROR = 0x0080, + SAMPLE_FAULT = 0x0100, + OVERCURR_SW = 0x0200, + DP_FAULT = 0x0400 +}; + +struct NoOpFrame +{ +}; + +struct SetTargetSpeedFrame +{ + bool motor_enabled; + int16_t motor_target_speed_rpm; +}; + +struct SetTargetTorqueFrame +{ + bool motor_enabled; + int16_t motor_target_torque; +}; + +struct SetResponseTypeFrame +{ + StSpinResponseType response_type; +}; + +struct SetPidTorqueKpKiFrame +{ + int16_t kp; + int16_t ki; +}; + +struct SetPidFluxKpKiFrame +{ + int16_t kp; + int16_t ki; +}; + +struct SetPidSpeedKpKiFrame +{ + int16_t kp; + int16_t ki; +}; + +struct SetSpeedFeedForwardKaKvFrame +{ + int16_t ka; + int16_t kv; +}; + +struct SetSpeedFeedForwardKsFrame +{ + int16_t ks; +}; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp new file mode 100644 index 0000000000..468919fd4c --- /dev/null +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -0,0 +1,748 @@ +#include "software/embedded/motor_controller/tmc_motor_controller.h" + +#include "shared/constants.h" +#include "software/embedded/gpio/gpio_char_dev.h" +#include "software/embedded/spi_utils.h" +#include "software/logger/logger.h" + +extern "C" +{ +#include "tmc/ic/TMC4671/TMC4671.h" +#include "tmc/ic/TMC4671/TMC4671_Register.h" +#include "tmc/ic/TMC4671/TMC4671_Variants.h" +#include "tmc/ic/TMC6100/TMC6100.h" +} + +#include + +#include +#include + +extern "C" +{ + // We need a static pointer here, because trinamic externs the following two + // SPI binding functions that we need to interface with their API. + // + // The motor service exclusively calls the trinamic API which triggers these + // functions. The motor service will set this variable in the constructor. + static TmcMotorController* g_motor_controller = nullptr; + + uint8_t tmc4671_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) + { + return g_motor_controller->tmc4671ReadWriteByte(motor, data, last_transfer); + } + + uint8_t tmc6100_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) + { + return g_motor_controller->tmc6100ReadWriteByte(motor, data, last_transfer); + } +} + +TmcMotorController::TmcMotorController() + : spi_demux_select_0_(std::make_unique( + SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO, GpioDirection::OUTPUT, GpioState::LOW)), + spi_demux_select_1_(std::make_unique( + SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO, GpioDirection::OUTPUT, GpioState::LOW)), + driver_control_enable_gpio_(std::make_unique( + DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), + reset_gpio_(std::make_unique(MOTOR_DRIVER_RESET_GPIO, + GpioDirection::OUTPUT, GpioState::HIGH)) +{ + openSpiFileDescriptor(MotorIndex::FRONT_LEFT); + openSpiFileDescriptor(MotorIndex::FRONT_RIGHT); + openSpiFileDescriptor(MotorIndex::BACK_LEFT); + openSpiFileDescriptor(MotorIndex::BACK_RIGHT); + openSpiFileDescriptor(MotorIndex::DRIBBLER); + + // Make this instance available to the static functions above + g_motor_controller = this; +} + +int TmcMotorController::readThenWriteVelocity(const MotorIndex motor, + const int target_velocity) +{ + if (motor == MotorIndex::DRIBBLER) + { + const int velocity_erpm = readThenWriteValue( + motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, + target_velocity * DRIBBLER_MOTOR_ELECTRICAL_RPM_PER_MECHANICAL_RPM); + return velocity_erpm * DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + } + else + { + const int velocity_erpm = readThenWriteValue( + motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, + target_velocity * DRIVE_MOTOR_ELECTRICAL_RPM_PER_MECHANICAL_RPM); + return velocity_erpm * DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + } +} + +void TmcMotorController::updateEuclideanVelocity( + EuclideanSpace_t target_euclidean_velocity) +{ +} + + +void TmcMotorController::writeToDriverOrDieTrying(const uint8_t motor, + const uint8_t address, + const int32_t value) +{ + int num_retries_left = NUM_RETRIES_SPI; + int read_value = 0; + + // The SPI lines have a lot of noise, and sometimes a transfer will fail + // randomly. So we retry a few times before giving up. + while (num_retries_left > 0) + { + tmc6100_writeInt(motor, address, value); + read_value = tmc6100_readInt(motor, address); + if (read_value == value) + { + return; + } + LOG(DEBUG) << "SPI Transfer to Driver Failed, retrying..."; + num_retries_left--; + } + + // If we get here, we have failed to write to the driver. We reset + // the chip to clear any bad values we just wrote and crash so everything stops. + reset_gpio_->setValue(GpioState::LOW); + CHECK(read_value == value) << "Couldn't write " << value + << " to the TMC6100 at address " << address + << " at address " << static_cast(address) + << " on motor " << static_cast(motor) + << " received: " << read_value; +} + +void TmcMotorController::writeToControllerOrDieTrying(const MotorIndex motor, + const uint8_t address, + const int32_t value) +{ + int num_retries_left = NUM_RETRIES_SPI; + int read_value = 0; + + // The SPI lines have a lot of noise, and sometimes a transfer will fail + // randomly. So we retry a few times before giving up. + while (num_retries_left > 0) + { + tmc4671_writeInt(CHIP_SELECTS.at(motor), address, value); + read_value = tmc4671_readInt(CHIP_SELECTS.at(motor), address); + if (read_value == value) + { + return; + } + LOG(DEBUG) << "SPI Transfer to Controller Failed, retrying..."; + num_retries_left--; + } +} + +void TmcMotorController::setup() +{ + reset_gpio_->setValue(GpioState::LOW); + usleep(MICROSECONDS_PER_MILLISECOND * 100); + + reset_gpio_->setValue(GpioState::HIGH); + usleep(MICROSECONDS_PER_MILLISECOND * 100); + + for (const MotorIndex motor : reflective_enum::values()) + { + motor_faults_[motor] = MotorFaultIndicator(); + num_motor_fault_checks_[motor] = 0; + } + + for (const MotorIndex motor : reflective_enum::values()) + { + LOG(INFO) << "Clearing RESET for " << motor; + tmc6100_writeInt(CHIP_SELECTS.at(motor), TMC6100_GSTAT, 0x00000001); + encoder_calibrated_[motor] = false; + } + + // Drive Motor Setup + for (const MotorIndex motor : driveMotors()) + { + setupDriveMotor(motor); + } + + // Dribbler Motor Setup + startDriver(MotorIndex::DRIBBLER); + checkFaults(MotorIndex::DRIBBLER); + startController(MotorIndex::DRIBBLER, true); + tmc4671_setTargetVelocity(DRIBBLER_MOTOR_CHIP_SELECT, 0); + + checkEncoderConnections(); + + // calibrate the encoders + for (const MotorIndex motor : driveMotors()) + { + startEncoderCalibration(motor); + } + + sleep(1); + + for (const MotorIndex motor : driveMotors()) + { + endEncoderCalibration(motor); + } + + auto motors = driveMotors(); + bool has_encoders_calibrated = + std::accumulate(motors.begin(), motors.end(), false, + [&](const bool& acc, const MotorIndex motor) + { return acc || encoder_calibrated_[motor]; }); + CHECK(has_encoders_calibrated) + << "Running without encoder calibration can cause serious harm, exiting"; +} + +void TmcMotorController::configurePWM(const MotorIndex motor) +{ + LOG(INFO) << "Configuring PWM for motor " << static_cast(motor); + // Please read the header file and the datasheet for more info + writeToControllerOrDieTrying(motor, TMC4671_PWM_POLARITIES, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_PWM_MAXCNT, 0x00000F9F); + writeToControllerOrDieTrying(motor, TMC4671_PWM_BBM_H_BBM_L, 0x00002828); + writeToControllerOrDieTrying(motor, TMC4671_PWM_SV_CHOP, 0x00000107); +} + +void TmcMotorController::configureDrivePI(const MotorIndex motor) +{ + LOG(INFO) << "Configuring Drive PI for motor " << static_cast(motor); + // Please read the header file and the datasheet for more info + // These values were calibrated using the TMC-IDE + writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 67109376); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 67109376); + writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_P_VELOCITY_I, 52428800); + + // Explicitly disable the position controller + writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); + + writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 2500); + writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 1000); + + writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 45000); + + tmc4671_switchToMotionMode(CHIP_SELECTS.at(motor), TMC4671_MOTION_MODE_VELOCITY); +} + +void TmcMotorController::configureDribblerPI(const MotorIndex motor) +{ + LOG(INFO) << "Configuring Dribbler PI for motor " << static_cast(motor); + // Please read the header file and the datasheet for more info + // These values were calibrated using the TMC-IDE + writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 39337600); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 39333600); + writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_P_VELOCITY_I, 2621448); + + // Explicitly disable the position controller + writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); + + writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); + // TODO (#2677) support MAX_FORCE mode. This value can go up to 4.8 amps but we set it + // to 2 for now (sufficient for INDEFINITE mode). + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 4000); + writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 40000); + writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 15000); +} + +void TmcMotorController::configureADC(const MotorIndex motor) +{ + LOG(INFO) << "Configuring ADC for motor " << static_cast(motor); + // ADC configuration + writeToControllerOrDieTrying(motor, TMC4671_ADC_I_SELECT, 0x18000100); + writeToControllerOrDieTrying(motor, TMC4671_dsADC_MDEC_B_MDEC_A, 0x014E014E); + + // These values have been calibrated for the TI INA240 current sense amplifier. + // The scaling is also set to work with both the drive and dribbler motors. + // + // If you wish to use the TMC4671+TMC6100-BOB you can use the following values, + // that work for the AD8418 current sense amplifier + // + // TMC4671_ADC_I0_SCALE_OFFSET = 0x010081DD + // TMC4671_ADC_I1_SCALE_OFFSET = 0x0100818E + // + writeToControllerOrDieTrying(motor, TMC4671_ADC_I0_SCALE_OFFSET, 0x000981DD); + writeToControllerOrDieTrying(motor, TMC4671_ADC_I1_SCALE_OFFSET, 0x0009818E); +} + +void TmcMotorController::configureEncoder(const MotorIndex motor) +{ + LOG(INFO) << "Configuring Encoder for motor " << static_cast(motor); + // ABN encoder settings + writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_MODE, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PPR, 0x00001000); +} + +void TmcMotorController::configureHall(const MotorIndex motor) +{ + LOG(INFO) << "Configuring Hall for motor " << static_cast(motor); + // Digital hall settings + writeToControllerOrDieTrying(motor, TMC4671_HALL_MODE, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_HALL_PHI_E_PHI_M_OFFSET, 0x00000000); + + // Feedback selection + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_HALL); + writeToControllerOrDieTrying(motor, TMC4671_VELOCITY_SELECTION, + TMC4671_VELOCITY_PHI_E_HAL); +} + +const MotorFaultIndicator& TmcMotorController::checkFaults(const MotorIndex motor) +{ + num_motor_fault_checks_[motor]++; + + // To reduce the number of SPI transactions, we only check for faults every + // MOTOR_FAULT_CHECK_INTERVAL calls and return the cached faults otherwise. + if (num_motor_fault_checks_.at(motor) % MOTOR_FAULT_CHECK_INTERVAL == 0) + { + return motor_faults_.at(motor); + } + + MotorFaultIndicator& motor_faults = motor_faults_.at(motor); + motor_faults.faults.clear(); + + const int gstat = tmc6100_readInt(CHIP_SELECTS.at(motor), TMC6100_GSTAT); + const std::bitset<32> gstat_bitset(gstat); + + if (gstat_bitset.any()) + { + LOG(WARNING) << "======= Faults For Motor " << motor << "======="; + } + + if (gstat_bitset[0]) + { + LOG(WARNING) + << "Indicates that the IC has been reset. All registers have been cleared to reset values." + << "Attention: DRV_EN must be high to allow clearing reset"; + motor_faults.faults.insert(TbotsProto::MotorFault::RESET); + } + + if (gstat_bitset[1]) + { + LOG(WARNING) + << "drv_otpw : Indicates, that the driver temperature has exceeded overtemperature prewarning-level." + << "No action is taken. This flag is latched."; + motor_faults.faults.insert( + TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE_PREWARNING); + } + + if (gstat_bitset[2]) + { + LOG(WARNING) + << "drv_ot: Indicates, that the driver has been shut down due to overtemperature." + << "This flag can only be cleared when the temperature is below the limit again." + << "It is latched for information."; + motor_faults.faults.insert(TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE); + } + + if (gstat_bitset[3]) + { + LOG(WARNING) << "uv_cp: Indicates an undervoltage on the charge pump." + << "The driver is disabled during undervoltage." + << "This flag is latched for information."; + motor_faults.faults.insert(TbotsProto::MotorFault::UNDERVOLTAGE_CHARGEPUMP); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[4]) + { + LOG(WARNING) << "shortdet_u: Short to GND detected on phase U." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert( + TbotsProto::MotorFault::PHASE_U_SHORT_COUNTER_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[5]) + { + LOG(WARNING) << "s2gu: Short to GND detected on phase U." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_GND_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[6]) + { + LOG(WARNING) << "s2vsu: Short to VS detected on phase U." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_VS_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[8]) + { + LOG(WARNING) << "shortdet_v: V short counter has triggered at least once."; + motor_faults.faults.insert( + TbotsProto::MotorFault::PHASE_V_SHORT_COUNTER_DETECTED); + } + + if (gstat_bitset[9]) + { + LOG(WARNING) << "s2gv: Short to GND detected on phase V." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_GND_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[10]) + { + LOG(WARNING) << "s2vsv: Short to VS detected on phase V." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_VS_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[12]) + { + LOG(WARNING) << "shortdet_w: short counter has triggered at least once."; + motor_faults.faults.insert( + TbotsProto::MotorFault::PHASE_W_SHORT_COUNTER_DETECTED); + } + + if (gstat_bitset[13]) + { + LOG(WARNING) << "s2gw: Short to GND detected on phase W." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_GND_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[14]) + { + LOG(WARNING) << "s2vsw: Short to VS detected on phase W." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_VS_DETECTED); + motor_faults.drive_enabled = false; + } + + return motor_faults; +} + +int TmcMotorController::readThenWriteValue(const MotorIndex motor, + const uint8_t read_addr, + const uint8_t write_addr, const int write_data) +{ + spi_demux_select_0_->setValue(GpioState::HIGH); + spi_demux_select_1_->setValue(GpioState::LOW); + + // ensure tx_ and rx_ are cleared + memset(read_tx_, 0, 5); + memset(write_tx_, 0, 5); + memset(read_rx_, 0, 5); + + // Trinamic transactions looks like this: + // + - - - + - - - + - - - + - - - + - - - + + // | ADDR | DATA | + // + - - - + - - - + - - - + - - - + - - - + + // 0 1 2 3 4 + // Also it is in BIG Endian, therefore MSB is leftmost bit of 0. + // For a write, MSB must be 1, for read, MSB must be 0 + // https://github.com/trinamic/TMC-API/blob/master/tmc/ic/TMC4671/TMC4671.c + read_tx_[0] = read_addr & 0x7f; + write_tx_[0] = write_addr | 0x80; + + // Convert from little endian to big endian + for (int i = 3; i >= 0; i--) + { + uint8_t byte_to_copy = (uint8_t)(0xff & (write_data >> 8 * i)); + write_tx_[4 - i] = byte_to_copy; + } + + readThenWriteSpiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], read_tx_, + write_tx_, read_rx_, TMC_CMD_MSG_SIZE, TMC_CMD_MSG_SIZE, + TMC4671_SPI_SPEED); + + int32_t value = read_rx_[0]; + for (int i = 1; i < 5; i++) + { + value <<= 8; + value |= read_rx_[i]; + } + return value; +} + +void TmcMotorController::openSpiFileDescriptor(const MotorIndex motor_index) +{ + file_descriptors_[CHIP_SELECTS.at(motor_index)] = + open(SPI_PATHS.at(motor_index), O_RDWR); + CHECK(file_descriptors_[CHIP_SELECTS.at(motor_index)] >= 0) + << "can't open device: " << motor_index << "error: " << strerror(errno); + + int ret = ioctl(file_descriptors_[CHIP_SELECTS.at(motor_index)], SPI_IOC_WR_MODE32, + &SPI_MODE); + CHECK(ret != -1) << "can't set spi mode for: " << motor_index + << "error: " << strerror(errno); + + ret = ioctl(file_descriptors_[CHIP_SELECTS.at(motor_index)], SPI_IOC_WR_BITS_PER_WORD, + &SPI_BITS); + CHECK(ret != -1) << "can't set bits_per_word for: " << motor_index + << "error: " << strerror(errno); + + ret = ioctl(file_descriptors_[CHIP_SELECTS.at(motor_index)], SPI_IOC_WR_MAX_SPEED_HZ, + &MAX_SPI_SPEED_HZ); + CHECK(ret != -1) << "can't set spi max speed hz for: " << motor_index + << "error: " << strerror(errno); +} + +void TmcMotorController::setupDriveMotor(const MotorIndex motor) +{ + startDriver(motor); + checkFaults(motor); + // Start all the controllers as drive motor controllers + startController(motor, false); + tmc4671_setTargetVelocity(CHIP_SELECTS.at(motor), 0); +} + +void TmcMotorController::startDriver(const MotorIndex motor) +{ + uint8_t motor_cs = CHIP_SELECTS.at(motor); + + // Set the drive strength to 0, the weakest it can go as recommended + // by the TMC4671-TMC6100-BOB datasheet. + int32_t current_drive_conf = tmc6100_readInt(motor_cs, TMC6100_DRV_CONF); + writeToDriverOrDieTrying(motor_cs, TMC6100_DRV_CONF, + current_drive_conf & (~TMC6100_DRVSTRENGTH_MASK)); + writeToDriverOrDieTrying(motor_cs, TMC6100_GCONF, 0x40); + + // All default but updated SHORTFILTER to 2us to avoid false positive shorts + // detection. + writeToDriverOrDieTrying(motor_cs, TMC6100_SHORT_CONF, 0x13020606); + + LOG(DEBUG) << "Driver " << motor << " accepted conf"; +} + +void TmcMotorController::startController(const MotorIndex motor, const bool dribbler) +{ + // Read the chip ID to validate the SPI connection + tmc4671_writeInt(CHIP_SELECTS.at(motor), TMC4671_CHIPINFO_ADDR, 0x000000000); + int chip_id = tmc4671_readInt(CHIP_SELECTS.at(motor), TMC4671_CHIPINFO_DATA); + + CHECK(0x34363731 == chip_id) + << "The TMC4671 of motor " << motor << " is not responding"; + + LOG(DEBUG) << "Controller " << motor << " online, responded with: " << chip_id; + + // Configure common controller params + configurePWM(motor); + configureADC(motor); + + if (dribbler) + { + // Configure to brushless DC motor with 1 pole pair + writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, + 0x00030000 + DRIBBLER_MOTOR_NUM_POLE_PAIRS); + configureHall(motor); + + configureDribblerPI(motor); + } + else + { + // Configure to brushless DC motor with 8 pole pairs + writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, + 0x00030000 + DRIVE_MOTOR_NUM_POLE_PAIRS); + configureEncoder(motor); + } +} + +uint8_t TmcMotorController::tmc4671ReadWriteByte(const uint8_t motor, const uint8_t data, + const uint8_t last_transfer) +{ + spi_demux_select_0_->setValue(GpioState::HIGH); + spi_demux_select_1_->setValue(GpioState::LOW); + return readWriteByte(motor, data, last_transfer, TMC4671_SPI_SPEED); +} + +uint8_t TmcMotorController::tmc6100ReadWriteByte(const uint8_t motor, const uint8_t data, + const uint8_t last_transfer) +{ + spi_demux_select_0_->setValue(GpioState::LOW); + spi_demux_select_1_->setValue(GpioState::HIGH); + return readWriteByte(motor, data, last_transfer, TMC6100_SPI_SPEED); +} + +uint8_t TmcMotorController::readWriteByte(const uint8_t motor, const uint8_t data, + const uint8_t last_transfer, + const uint32_t spi_speed) +{ + uint8_t ret_byte = 0; + + if (!transfer_started_) + { + memset(tx_, 0, sizeof(tx_)); + memset(rx_, 0, sizeof(rx_)); + position_ = 0; + + if (data & TMC_WRITE_BIT) + { + // If the transfer started and its a write operation, + // set the appropriate flags. + currently_reading_ = false; + currently_writing_ = true; + } + else + { + // The first byte should contain the address on a read operation. + // Trigger a transfer (1 byte) and buffer the response (4 bytes) + tx_[position_] = data; + spiTransfer(file_descriptors_[motor], tx_, rx_, 5, spi_speed); + + currently_reading_ = true; + currently_writing_ = false; + } + + transfer_started_ = true; + } + + if (currently_writing_) + { + // Buffer the data to send out when last_transfer is true. + tx_[position_++] = data; + } + + if (currently_reading_) + { + // If we are reading, we just need to return the buffered data + // byte by byte. + ret_byte = rx_[position_++]; + } + + if (currently_writing_ && last_transfer) + { + // we have all the bytes for this transfer, lets trigger the transfer and + // reset state + spiTransfer(file_descriptors_[motor], tx_, rx_, 5, spi_speed); + transfer_started_ = false; + } + + if (currently_reading_ && last_transfer) + { + // when reading, if last transfer is true, we just need to reset state + transfer_started_ = false; + } + + return ret_byte; +} + +void TmcMotorController::checkEncoderConnections() +{ + LOG(INFO) << "Starting encoder connection check!"; + + std::unordered_map calibrated_motors; + std::unordered_map initial_velocities; + + for (const MotorIndex motor : driveMotors()) + { + calibrated_motors[motor] = false; + + // read back current velocity + initial_velocities[motor] = + tmc4671_readInt(CHIP_SELECTS.at(motor), TMC4671_ABN_DECODER_COUNT); + + // open loop mode can be used without an encoder, set open loop phi positive + // direction + writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_MODE, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, + TMC4671_PHI_E_OPEN_LOOP); + writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_ACCELERATION, 0x0000003C); + + // represents effective voltage applied to the motors (% voltage) + writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000799); + + // uq_ud_ext mode + writeToControllerOrDieTrying(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); + + // 10 RPM + writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x0000000A); + } + + for (int num_iterations = 0; + num_iterations < 10 && + std::any_of(calibrated_motors.begin(), calibrated_motors.end(), + [](std::pair calibration_status_pair) + { return !calibration_status_pair.second; }); + ++num_iterations) + { + for (const MotorIndex motor : driveMotors()) + { + if (calibrated_motors[motor]) + { + continue; + } + // now read back the velocity + int read_back_velocity = + tmc4671_readInt(CHIP_SELECTS.at(motor), TMC4671_ABN_DECODER_COUNT); + LOG(INFO) << motor << " read back: " << read_back_velocity + << " and initially read: " << initial_velocities[motor]; + + if (read_back_velocity != initial_velocities[motor]) + { + calibrated_motors[motor] = true; + } + } + + // sleep for 100 milliseconds + usleep(MICROSECONDS_PER_MILLISECOND * 100); + } + + bool calibrated = true; + for (const MotorIndex motor : driveMotors()) + { + if (!calibrated_motors[motor]) + { + calibrated = false; + LOG(WARNING) << "Encoder calibration check failure. " << motor + << " did not change as expected"; + } + } + if (!calibrated) + { + LOG(FATAL) + << "Encoder calibration check failure. Not all encoders responded as expected"; + } + + // stop all motors, reset back to velocity control mode + for (const MotorIndex motor : driveMotors()) + { + writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x00000000); + tmc4671_switchToMotionMode(CHIP_SELECTS.at(motor), TMC4671_MOTION_MODE_VELOCITY); + } + + LOG(INFO) << "All encoders appear to be connected!"; +} + +void TmcMotorController::reset() +{ + reset_gpio_->setValue(GpioState::LOW); +} + +void TmcMotorController::startEncoderCalibration(const MotorIndex motor) +{ + LOG(WARNING) << "Calibrating the encoder, ensure the robot is lifted off the ground"; + + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 0x000003E8); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 0x01000100); + writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 0x01000100); + + writeToControllerOrDieTrying(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); + writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PHI_E_PHI_M_OFFSET, + 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, 0x00000001); + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_EXT, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000FFF); +} + +void TmcMotorController::endEncoderCalibration(const MotorIndex motor) +{ + LOG(WARNING) << "Calibrating the encoder, wheels may move"; + + writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_COUNT, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_ABN); + + encoder_calibrated_[motor] = true; + + configureDrivePI(motor); +} + +void TmcMotorController::immediatelyDisable() +{ + driver_control_enable_gpio_->setValue(GpioState::LOW); +} diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h new file mode 100644 index 0000000000..9715a505ee --- /dev/null +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -0,0 +1,298 @@ +#pragma once + +#include "software/embedded/gpio/gpio.h" +#include "software/embedded/motor_controller/motor_controller.h" +#include "software/embedded/motor_controller/motor_fault_indicator.h" +#include "software/embedded/motor_controller/motor_index.h" + +/** + * Motor controller for interfacing with our 5th generation Trinamic motor + * controllers and drivers. + * + * TMC4671 is the controller and TMC6100 is the driver. We use the TMC4671's SPI + * interface to configure the controller and driver, read/set motor velocities, etc. + */ +class TmcMotorController : public MotorController +{ + public: + TmcMotorController(); + + void setup() override; + + void reset() override; + + const MotorFaultIndicator& checkFaults(MotorIndex motor) override; + + void immediatelyDisable() override; + + int readThenWriteVelocity(MotorIndex motor, int target_velocity) override; + + void updateEuclideanVelocity(EuclideanSpace_t target_euclidean_velocity) override; + + /** + * Trinamic API binding, sets spi_demux_select_0|1 pins appropriately and + * calls readWriteByte. + * + * Both the TMC4671 (the controller) and the TMC6100 (the driver) respect + * the same SPI interface. So when we bind the API, we can use the same + * readWriteByte function, provided that the chip select pin is turning on + * the right chip. + * + * Each TMC4671 controller, TMC6100 driver and encoder group have their chip + * selects coming in from a demux (see diagram below). The demux is controlled + * by two bits {spi_demux_select_0, spi_demux_select_1}. If the bits are + * 10 the TMC4671 is selected, when the select bits are 01 the TMC6100 is + * selected and when they are 11 the encoder is selected. 00 disconnects all + * 3 chips. + * + * + * FRONT LEFT MOTOR + * CONTROLLER + DRIVER + ENCODER + * + * ┌───────┐ ┌───────────────┐ + * │ │ │ │ + * │ 2:4 │ 10 │ ┌─────────┐ │ + * │ ├────────┼──►TMC4671 │ │ B0 + * FRONT_LEFT_CS │ DEMUX │ │ └─────────┘ │ + * ───────────────► │ │ │ + * │ │ 01 │ ┌─────────┐ │ + * │ ├────────┼──►TMC6100 │ │ B1 + * │ │ │ └─────────┘ │ + * │ │ │ │ + * │ │ 11 │ ┌─────────┐ │ + * │ ├────────┼──►ENCODER │ │ B2 + * │ │ │ └─────────┘ │ + * └───▲───┘ │ │ + * │ └───────────────┘ + * │ + * spi_demux_sel_0 & 1 + * + * + * @param motor Which motor to talk to (in our case, the chip select) + * @param data The data to send + * @param last_transfer The last transfer of uint8_t data for this transaction. + * @return A byte read from the trinamic chip + */ + uint8_t tmc4671ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); + uint8_t tmc6100ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); + + private: + /** + * Opens SPI file descriptor + * + * @param motor_index The index of the motor to open + */ + void openSpiFileDescriptor(MotorIndex motor_name); + + /** + * A function which is written in the same style as the rest of the Trinamic API. + * This will trigger two SPI transactions back to back, reading a value and then + * writing a value for a specific motor + * + * @param motor The motor we want to read & write from + * @param read_addr the address of the register to read + * @param write_addr the address of the register to write + * @param write_data the data to write + * @return the value read from the trinamic controller + */ + int readThenWriteValue(MotorIndex motor, uint8_t read_addr, uint8_t write_addr, + int write_data); + + /** + * A lot of initialization parameters are necessary to function. Even if + * there is a single bit error, we can risk frying the motor driver or + * controller. + * + * The following functions can be used to setup initialization params + * that _must_ be set to continue. A failed call will crash the program + * + * @param motor Which motor to talk to (in our case, the chip select) + * @param address The address to send data to + * @param value The value to write + * + */ + void writeToControllerOrDieTrying(MotorIndex motor, uint8_t address, int32_t value); + void writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value); + + /** + * Sets up motor as drive motor controllers + * + * @param motor drive motor number + */ + void setupDriveMotor(MotorIndex motor); + + /** + * Calls the configuration functions below in the right sequence + * + * @param motor The motor setup the driver/controller for + * @param dribbler If true, configures the motor to be a dribbler + */ + void startDriver(MotorIndex motor); + void startController(MotorIndex motor, bool dribbler); + + /** + * Configuration settings + * + * These values were determined by reading the datasheets and user manual + * here: https://www.trinamic.com/support/eval-kits/details/tmc4671-tmc6100-bob/ + * + * If you are planning to change these settings, I highly recommend that you + * plug the motor + encoder pair in the TMC-IDE and use the TMC4671 EVAL + * with the TMC6100 EVAL to get the motor spinning. + * + * Then using the exported registers as a baseline, you can use the + * runOpenLoopCalibrationRoutine and plot the generated csvs. These csvs capture the + * data for encoder calibration and adc configuration, the two most important steps + * for the motor to work. Page 143 (title Setup Guidelines) of the TMC4671 is very + * useful. + * + * @param motor The motor to configure (the same value as the chip select) + */ + void configurePWM(MotorIndex motor); + void configureDribblerPI(MotorIndex motor); + void configureDrivePI(MotorIndex motor); + void configureADC(MotorIndex motor); + void configureEncoder(MotorIndex motor); + void configureHall(MotorIndex motor); + + /** + * Trinamic API Binding function + * + * @param motor Which motor to talk to (in our case, the chip select) + * @param data The data to send + * @param last_transfer The last transfer of uint8_t data for this transaction. + * @param spi_speed The speed to run spi at + * + * @return A byte read from the trinamic chip + */ + uint8_t readWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer, + uint32_t spi_speed); + + /* + * For FOC to work, the controller needs to know the electical angle of the rotor + * relative to the mechanical angle of the rotor. In an incremental-encoder-only + * setup, we can energize the motor coils so that the rotor locks itself along + * one of its pole-pairs, allowing us to reset the encoder. + * + * WARNING: Do not try to spin the motor without initializing the encoder! + * The motor can overheat if the TMC4671 doesn't auto shut-off. + * + * There are some safety checks to ensure that the encoder is + * initialized, do not tamper with them. You have been warned. + * + * @param motor The motor to initialize the encoder for + */ + void startEncoderCalibration(MotorIndex motor); + void endEncoderCalibration(MotorIndex motor); + + /** + * Spin each drive motor in open loop mode to check if the encoder + * is responding as expected. Allows us to do a basic test of whether + * the encoder is physically connected to the motor board. + * + * Leaves the motors connected in MOTION_MODE_VELOCITY + */ + void checkEncoderConnections(); + + // Select between driver and controller gpio + std::unique_ptr spi_demux_select_0_; + std::unique_ptr spi_demux_select_1_; + + // Enable driver gpio + std::unique_ptr driver_control_enable_gpio_; + std::unique_ptr reset_gpio_; + + // Transfer Buffers for spiTransfer + uint8_t tx_[5] = {}; + uint8_t rx_[5] = {}; + + // Transfer Buffers for readThenWriteSpiTransfer + uint8_t write_tx_[5] = {}; + uint8_t read_tx_[5] = {}; + uint8_t read_rx_[5] = {}; + + // Transfer State + bool transfer_started_ = false; + bool currently_writing_ = false; + bool currently_reading_ = false; + uint8_t position_ = 0; + + // Tracks whether each motor's encoder has been calibrated + std::unordered_map encoder_calibrated_; + + // Cached faults for each motor + std::unordered_map motor_faults_; + + // Number of times we've polled each motor for faults, + // used to determine when to actually check for faults again + std::unordered_map num_motor_fault_checks_; + + // The interval at which to check for motor faults, in number of polls. + // We avoid checking faults at every poll to reduce the number of SPI transactions. + static constexpr int MOTOR_FAULT_CHECK_INTERVAL = 5; + + // SPI Chip Selects + static constexpr uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT = 0; + static constexpr uint8_t BACK_LEFT_MOTOR_CHIP_SELECT = 1; + static constexpr uint8_t BACK_RIGHT_MOTOR_CHIP_SELECT = 2; + static constexpr uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3; + static constexpr uint8_t DRIBBLER_MOTOR_CHIP_SELECT = 4; + + // SPI Trinamic Motor Driver Paths + static const inline std::unordered_map SPI_PATHS = { + {MotorIndex::FRONT_LEFT, "/dev/spidev0.0"}, + {MotorIndex::FRONT_RIGHT, "/dev/spidev0.3"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.1"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.2"}, + {MotorIndex::DRIBBLER, "/dev/spidev0.4"}, + }; + + static const inline std::unordered_map CHIP_SELECTS = { + {MotorIndex::FRONT_LEFT, FRONT_LEFT_MOTOR_CHIP_SELECT}, + {MotorIndex::FRONT_RIGHT, FRONT_RIGHT_MOTOR_CHIP_SELECT}, + {MotorIndex::BACK_LEFT, BACK_LEFT_MOTOR_CHIP_SELECT}, + {MotorIndex::BACK_RIGHT, BACK_RIGHT_MOTOR_CHIP_SELECT}, + {MotorIndex::DRIBBLER, DRIBBLER_MOTOR_CHIP_SELECT}, + }; + + // SPI Configs + static constexpr uint32_t TMC6100_SPI_SPEED = 1000000; // 1 Mhz + static constexpr uint32_t TMC4671_SPI_SPEED = 1000000; // 1 Mhz + static constexpr uint32_t MAX_SPI_SPEED_HZ = 2000000; // 2 Mhz + static constexpr uint8_t SPI_BITS = 8; + static constexpr uint32_t SPI_MODE = 0x3u; + + // SPI File Descriptors mapping from Chip Select -> File Descriptor + std::array()> file_descriptors_; + + // Number of times that Thunderloop will try to write the configuration to the driver + // before giving up + static constexpr int NUM_RETRIES_SPI = 3; + + // Trinamics communicate with 5 byte messages + static constexpr uint32_t TMC_CMD_MSG_SIZE = 5; + + static constexpr int DRIVE_MOTOR_NUM_POLE_PAIRS = 8; + static constexpr int DRIBBLER_MOTOR_NUM_POLE_PAIRS = 1; + + static constexpr double WHEEL_ROTATIONS_PER_MOTOR_ROTATION = 17.0 / 60.0; + + // All Trinamic RPMs are Electrical RPMs (eRPM), which represents the speed at which + // the rotating magnetic field inside the motor moves (as opposed to mechanical RPM + // which indicates how fast the motor shaft is rotating). + // + // RPM = eRPM / # of pole pairs + static constexpr double DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM = + 1.0 / DRIVE_MOTOR_NUM_POLE_PAIRS * WHEEL_ROTATIONS_PER_MOTOR_ROTATION; + static constexpr double DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM = + 1.0 / DRIBBLER_MOTOR_NUM_POLE_PAIRS; + static constexpr double DRIVE_MOTOR_ELECTRICAL_RPM_PER_MECHANICAL_RPM = + 1.0 / DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + static constexpr double DRIBBLER_MOTOR_ELECTRICAL_RPM_PER_MECHANICAL_RPM = + 1.0 / DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + + static constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = 16; + static constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO = 19; + static constexpr int MOTOR_DRIVER_RESET_GPIO = 12; + static constexpr int DRIVER_CONTROL_ENABLE_GPIO = 22; +}; diff --git a/src/software/embedded/robot_diagnostics_cli/requirements_lock.txt b/src/software/embedded/robot_diagnostics_cli/requirements_lock.txt index ac94619ecf..e69de29bb2 100644 --- a/src/software/embedded/robot_diagnostics_cli/requirements_lock.txt +++ b/src/software/embedded/robot_diagnostics_cli/requirements_lock.txt @@ -1,134 +0,0 @@ -# -# This file is autogenerated by pip-compile with Python 3.12 -# by the following command: -# -# bazel run //software/embedded/robot_diagnostics_cli:requirements.update -# -click==8.1.7 \ - --hash=sha256:ae74fb96c20a0277a1d615f1e4d73c8414f5a98db8b799a7931d1582f3390c28 \ - --hash=sha256:ca9853ad459e787e2192211578cc907e7594e294c7ccc834310722b41b9ca6de - # via - # click-shell - # typer - # typer-shell -click-shell==2.1 \ - --hash=sha256:2d971a2e50eb7ad387cf0ce79ba4b844e66e0580784e2efe2df58b50a2f047f0 \ - --hash=sha256:ce0c91faae284c41a39bec966f928791ad4a45763755445f1fe2041fd091aa37 - # via typer-shell -inquirerpy==0.3.4 \ - --hash=sha256:89d2ada0111f337483cb41ae31073108b2ec1e618a49d7110b0d7ade89fc197e \ - --hash=sha256:c65fdfbac1fa00e3ee4fb10679f4d3ed7a012abf4833910e63c295827fe2a7d4 - # via -r software/embedded/robot_diagnostics_cli/requirements.in -markdown-it-py==3.0.0 \ - --hash=sha256:355216845c60bd96232cd8d8c40e8f9765cc86f46880e43a8fd22dc1a1a8cab1 \ - --hash=sha256:e3f60a94fa066dc52ec76661e37c851cb232d92f9886b15cb560aaada2df8feb - # via rich -mdurl==0.1.2 \ - --hash=sha256:84008a41e51615a49fc9966191ff91509e3c40b939176e643fd50a5c2196b8f8 \ - --hash=sha256:bb413d29f5eea38f31dd4754dd7377d4465116fb207585f97bf925588687c1ba - # via markdown-it-py -pfzy==0.3.4 \ - --hash=sha256:5f50d5b2b3207fa72e7ec0ef08372ef652685470974a107d0d4999fc5a903a96 \ - --hash=sha256:717ea765dd10b63618e7298b2d98efd819e0b30cd5905c9707223dceeb94b3f1 - # via inquirerpy -prompt-toolkit==3.0.48 \ - --hash=sha256:d6623ab0477a80df74e646bdbc93621143f5caf104206aa29294d53de1a03d90 \ - --hash=sha256:f49a827f90062e411f1ce1f854f2aedb3c23353244f8108b89283587397ac10e - # via inquirerpy -protobuf==6.31.1 \ - --hash=sha256:0414e3aa5a5f3ff423828e1e6a6e907d6c65c1d5b7e6e975793d5590bdeecc16 \ - --hash=sha256:426f59d2964864a1a366254fa703b8632dcec0790d8862d30034d8245e1cd447 \ - --hash=sha256:4ee898bf66f7a8b0bd21bce523814e6fbd8c6add948045ce958b73af7e8878c6 \ - --hash=sha256:6f1227473dc43d44ed644425268eb7c2e488ae245d51c6866d19fe158e207402 \ - --hash=sha256:720a6c7e6b77288b85063569baae8536671b39f15cc22037ec7045658d80489e \ - --hash=sha256:7fa17d5a29c2e04b7d90e5e32388b8bfd0e7107cd8e616feef7ed3fa6bdab5c9 \ - --hash=sha256:8764cf4587791e7564051b35524b72844f845ad0bb011704c3736cce762d8fe9 \ - --hash=sha256:a40fc12b84c154884d7d4c4ebd675d5b3b5283e155f324049ae396b95ddebc39 \ - --hash=sha256:d8cac4c982f0b957a4dc73a80e2ea24fab08e679c0de9deb835f4a12d69aca9a - # via -r software/embedded/robot_diagnostics_cli/requirements.in -pygments==2.18.0 \ - --hash=sha256:786ff802f32e91311bff3889f6e9a86e81505fe99f2735bb6d60ae0c5004f199 \ - --hash=sha256:b8e6aca0523f3ab76fee51799c488e38782ac06eafcf95e7ba832985c8e7b13a - # via rich -pyyaml==6.0.2 \ - --hash=sha256:01179a4a8559ab5de078078f37e5c1a30d76bb88519906844fd7bdea1b7729ff \ - --hash=sha256:0833f8694549e586547b576dcfaba4a6b55b9e96098b36cdc7ebefe667dfed48 \ - --hash=sha256:0a9a2848a5b7feac301353437eb7d5957887edbf81d56e903999a75a3d743086 \ - --hash=sha256:0b69e4ce7a131fe56b7e4d770c67429700908fc0752af059838b1cfb41960e4e \ - --hash=sha256:0ffe8360bab4910ef1b9e87fb812d8bc0a308b0d0eef8c8f44e0254ab3b07133 \ - --hash=sha256:11d8f3dd2b9c1207dcaf2ee0bbbfd5991f571186ec9cc78427ba5bd32afae4b5 \ - --hash=sha256:17e311b6c678207928d649faa7cb0d7b4c26a0ba73d41e99c4fff6b6c3276484 \ - --hash=sha256:1e2120ef853f59c7419231f3bf4e7021f1b936f6ebd222406c3b60212205d2ee \ - --hash=sha256:1f71ea527786de97d1a0cc0eacd1defc0985dcf6b3f17bb77dcfc8c34bec4dc5 \ - --hash=sha256:23502f431948090f597378482b4812b0caae32c22213aecf3b55325e049a6c68 \ - --hash=sha256:24471b829b3bf607e04e88d79542a9d48bb037c2267d7927a874e6c205ca7e9a \ - --hash=sha256:29717114e51c84ddfba879543fb232a6ed60086602313ca38cce623c1d62cfbf \ - --hash=sha256:2e99c6826ffa974fe6e27cdb5ed0021786b03fc98e5ee3c5bfe1fd5015f42b99 \ - --hash=sha256:39693e1f8320ae4f43943590b49779ffb98acb81f788220ea932a6b6c51004d8 \ - --hash=sha256:3ad2a3decf9aaba3d29c8f537ac4b243e36bef957511b4766cb0057d32b0be85 \ - --hash=sha256:3b1fdb9dc17f5a7677423d508ab4f243a726dea51fa5e70992e59a7411c89d19 \ - --hash=sha256:41e4e3953a79407c794916fa277a82531dd93aad34e29c2a514c2c0c5fe971cc \ - --hash=sha256:43fa96a3ca0d6b1812e01ced1044a003533c47f6ee8aca31724f78e93ccc089a \ - --hash=sha256:50187695423ffe49e2deacb8cd10510bc361faac997de9efef88badc3bb9e2d1 \ - --hash=sha256:5ac9328ec4831237bec75defaf839f7d4564be1e6b25ac710bd1a96321cc8317 \ - --hash=sha256:5d225db5a45f21e78dd9358e58a98702a0302f2659a3c6cd320564b75b86f47c \ - --hash=sha256:6395c297d42274772abc367baaa79683958044e5d3835486c16da75d2a694631 \ - --hash=sha256:688ba32a1cffef67fd2e9398a2efebaea461578b0923624778664cc1c914db5d \ - --hash=sha256:68ccc6023a3400877818152ad9a1033e3db8625d899c72eacb5a668902e4d652 \ - --hash=sha256:70b189594dbe54f75ab3a1acec5f1e3faa7e8cf2f1e08d9b561cb41b845f69d5 \ - --hash=sha256:797b4f722ffa07cc8d62053e4cff1486fa6dc094105d13fea7b1de7d8bf71c9e \ - --hash=sha256:7c36280e6fb8385e520936c3cb3b8042851904eba0e58d277dca80a5cfed590b \ - --hash=sha256:7e7401d0de89a9a855c839bc697c079a4af81cf878373abd7dc625847d25cbd8 \ - --hash=sha256:80bab7bfc629882493af4aa31a4cfa43a4c57c83813253626916b8c7ada83476 \ - --hash=sha256:82d09873e40955485746739bcb8b4586983670466c23382c19cffecbf1fd8706 \ - --hash=sha256:8388ee1976c416731879ac16da0aff3f63b286ffdd57cdeb95f3f2e085687563 \ - --hash=sha256:8824b5a04a04a047e72eea5cec3bc266db09e35de6bdfe34c9436ac5ee27d237 \ - --hash=sha256:8b9c7197f7cb2738065c481a0461e50ad02f18c78cd75775628afb4d7137fb3b \ - --hash=sha256:9056c1ecd25795207ad294bcf39f2db3d845767be0ea6e6a34d856f006006083 \ - --hash=sha256:936d68689298c36b53b29f23c6dbb74de12b4ac12ca6cfe0e047bedceea56180 \ - --hash=sha256:9b22676e8097e9e22e36d6b7bda33190d0d400f345f23d4065d48f4ca7ae0425 \ - --hash=sha256:a4d3091415f010369ae4ed1fc6b79def9416358877534caf6a0fdd2146c87a3e \ - --hash=sha256:a8786accb172bd8afb8be14490a16625cbc387036876ab6ba70912730faf8e1f \ - --hash=sha256:a9f8c2e67970f13b16084e04f134610fd1d374bf477b17ec1599185cf611d725 \ - --hash=sha256:bc2fa7c6b47d6bc618dd7fb02ef6fdedb1090ec036abab80d4681424b84c1183 \ - --hash=sha256:c70c95198c015b85feafc136515252a261a84561b7b1d51e3384e0655ddf25ab \ - --hash=sha256:cc1c1159b3d456576af7a3e4d1ba7e6924cb39de8f67111c735f6fc832082774 \ - --hash=sha256:ce826d6ef20b1bc864f0a68340c8b3287705cae2f8b4b1d932177dcc76721725 \ - --hash=sha256:d584d9ec91ad65861cc08d42e834324ef890a082e591037abe114850ff7bbc3e \ - --hash=sha256:d7fded462629cfa4b685c5416b949ebad6cec74af5e2d42905d41e257e0869f5 \ - --hash=sha256:d84a1718ee396f54f3a086ea0a66d8e552b2ab2017ef8b420e92edbc841c352d \ - --hash=sha256:d8e03406cac8513435335dbab54c0d385e4a49e4945d2909a581c83647ca0290 \ - --hash=sha256:e10ce637b18caea04431ce14fabcf5c64a1c61ec9c56b071a4b7ca131ca52d44 \ - --hash=sha256:ec031d5d2feb36d1d1a24380e4db6d43695f3748343d99434e6f5f9156aaa2ed \ - --hash=sha256:ef6107725bd54b262d6dedcc2af448a266975032bc85ef0172c5f059da6325b4 \ - --hash=sha256:efdca5630322a10774e8e98e1af481aad470dd62c3170801852d752aa7a783ba \ - --hash=sha256:f753120cb8181e736c57ef7636e83f31b9c0d1722c516f7e86cf15b7aa57ff12 \ - --hash=sha256:ff3824dc5261f50c9b0dfb3be22b4567a6f938ccce4587b38952d85fd9e9afe4 - # via typer-shell -rich==13.9.2 \ - --hash=sha256:51a2c62057461aaf7152b4d611168f93a9fc73068f8ded2790f29fe2b5366d0c \ - --hash=sha256:8c82a3d3f8dcfe9e734771313e606b39d8247bb6b826e196f4914b333b743cf1 - # via - # -r software/embedded/robot_diagnostics_cli/requirements.in - # typer - # typer-shell -shellingham==1.5.4 \ - --hash=sha256:7ecfff8f2fd72616f7481040475a65b2bf8af90a56c89140852d1120324e8686 \ - --hash=sha256:8dbca0739d487e5bd35ab3ca4b36e11c4078f3a234bfce294b0a0291363404de - # via typer -typer==0.12.5 \ - --hash=sha256:62fe4e471711b147e3365034133904df3e235698399bc4de2b36c8579298d52b \ - --hash=sha256:f592f089bedcc8ec1b974125d64851029c3b1af145f04aca64d69410f0c9b722 - # via typer-shell -typer-shell==0.1.12 \ - --hash=sha256:63b0b6b46b24c906d027b410bc521eeabc893f72c970318a7dd7034e95186caa \ - --hash=sha256:dc7742649e6dcdd77cc3ab909cbac23438a2e3ad6cf847cadc35bb0937b4965c - # via -r software/embedded/robot_diagnostics_cli/requirements.in -typing-extensions==4.12.2 \ - --hash=sha256:04e5ca0351e0f3f85c6853954072df659d0d13fac324d0072316b67d7794700d \ - --hash=sha256:1a7ead55c7e559dd4dee8856e3a88b41225abfe1ce8df57b7c13915fe121ffb8 - # via typer -wcwidth==0.2.13 \ - --hash=sha256:3da69048e4540d84af32131829ff948f1e022c1c6bdb8d6102117aac784f6859 \ - --hash=sha256:72ea0c06399eb286d978fdedb6923a9eb47e1c486ce63e9b4e64fc18303972b5 - # via prompt-toolkit diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index f763e4f8dc..528b1eedc9 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -4,16 +4,22 @@ cc_library( name = "motor", srcs = ["motor.cpp"], hdrs = ["motor.h"], + defines = + select({ + "//software/embedded:build_trinamic": ["TRINAMIC_MOTOR_BOARD"], + "//software/embedded:build_stspin": ["STSPIN_MOTOR_BOARD"], + }), deps = [ "//proto:tbots_cc_proto", "//shared:robot_constants", - "//software/embedded:gpio", - "//software/embedded/constants", + "//software/embedded/gpio", + "//software/embedded/motor_controller", + "//software/embedded/motor_controller:motor_board", + "//software/embedded/motor_controller:motor_fault_indicator", "//software/logger", "//software/physics:euclidean_to_wheel", "//software/util/scoped_timespec_timer", "@eigen", - "@trinamic", ], ) @@ -30,22 +36,3 @@ cc_library( "@boost//:filesystem", ], ) - -cc_binary( - name = "robot_auto_test", - srcs = ["robot_auto_test.cpp"], - linkopts = [ - "-lpthread", - "-lrt", - ], - deps = [ - ":motor", - ":power", - "//proto/message_translation:tbots_geometry", - "//proto/primitive:primitive_msg_factory", - "//shared:robot_constants", - "//software/embedded:primitive_executor", - "//software/logger", - "@trinamic", - ], -) diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index e42ab8f3f3..0c6515f5cf 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -1,373 +1,79 @@ #include "software/embedded/services/motor.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // Needed for mlockall() -#include // needed for getrusage -#include // needed for getrusage -#include // needed for sysconf(int name); - -#include - #include "proto/tbots_software_msgs.pb.h" +#include "software/embedded/motor_controller/motor_board.h" +#include "software/embedded/motor_controller/stspin_motor_controller.h" +#include "software/embedded/motor_controller/tmc_motor_controller.h" #include "software/logger/logger.h" -#include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" - -extern "C" -{ -#include "tmc/ic/TMC4671/TMC4671.h" -#include "tmc/ic/TMC4671/TMC4671_Register.h" -#include "tmc/ic/TMC4671/TMC4671_Variants.h" -#include "tmc/ic/TMC6100/TMC6100.h" -} - -// SPI Configs -static const uint32_t MAX_SPI_SPEED_HZ = 2000000; // 2 Mhz -static const uint32_t TMC6100_SPI_SPEED = 1000000; // 1 Mhz -static const uint32_t TMC4671_SPI_SPEED = 1000000; // 1 Mhz -static const uint8_t SPI_BITS = 8; -static const uint32_t SPI_MODE = 0x3u; -static const uint32_t NUM_RETRIES_SPI = 3; -static const uint32_t TMC_CMD_MSG_SIZE = 5; - -static double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; -static int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; - - -extern "C" -{ - // We need a static pointer here, because trinamic externs the following two - // SPI binding functions that we need to interface with their API. - // - // The motor service exclusively calls the trinamic API which triggers these - // functions. The motor service will set this variable in the constructor. - static MotorService* g_motor_service = NULL; - uint8_t tmc4671_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) - { - return g_motor_service->tmc4671ReadWriteByte(motor, data, last_transfer); - } - - uint8_t tmc6100_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) - { - return g_motor_service->tmc6100ReadWriteByte(motor, data, last_transfer); - } -} - -MotorService::MotorService(const robot_constants::RobotConstants& robot_constants, - int control_loop_frequency_hz) - : spi_demux_select_0_(setupGpio(SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO, - GpioDirection::OUTPUT, GpioState::LOW)), - spi_demux_select_1_(setupGpio(SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO, - GpioDirection::OUTPUT, GpioState::LOW)), - driver_control_enable_gpio_( - setupGpio(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), - reset_gpio_( - setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), - robot_constants_(robot_constants), +MotorService::MotorService(const robot_constants::RobotConstants& robot_constants) + : robot_constants_(robot_constants), + motor_controller_(setupMotorController()), euclidean_to_four_wheel_(robot_constants), - motor_fault_detector_(0), - dribbler_ramp_rpm_(0), - tracked_motor_fault_start_time_(std::nullopt), + dribbler_target_rpm_(0), + drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60), num_tracked_motor_resets_(0) { - motorServiceInit(robot_constants, control_loop_frequency_hz); } -void MotorService::motorServiceInit( - const robot_constants::RobotConstants& robot_constants, int control_loop_frequency_hz) -{ - int ret = 0; - - /** - * Opens SPI File Descriptor - * - * @param motor_name The name of the motor the spi path is connected to - * @param chip_select Which chip select to use - */ -#define OPEN_SPI_FILE_DESCRIPTOR(motor_name, chip_select) \ - \ - file_descriptors_[chip_select] = open(SPI_PATHS[chip_select], O_RDWR); \ - CHECK(file_descriptors_[chip_select] >= 0) \ - << "can't open device: " << #motor_name << "error: " << strerror(errno); \ - \ - ret = ioctl(file_descriptors_[chip_select], SPI_IOC_WR_MODE32, &SPI_MODE); \ - CHECK(ret != -1) << "can't set spi mode for: " << #motor_name \ - << "error: " << strerror(errno); \ - \ - ret = ioctl(file_descriptors_[chip_select], SPI_IOC_WR_BITS_PER_WORD, &SPI_BITS); \ - CHECK(ret != -1) << "can't set bits_per_word for: " << #motor_name \ - << "error: " << strerror(errno); \ - \ - ret = ioctl(file_descriptors_[chip_select], SPI_IOC_WR_MAX_SPEED_HZ, \ - &MAX_SPI_SPEED_HZ); \ - CHECK(ret != -1) << "can't set spi max speed hz for: " << #motor_name \ - << "error: " << strerror(errno); - - OPEN_SPI_FILE_DESCRIPTOR(front_left, FRONT_LEFT_MOTOR_CHIP_SELECT) - OPEN_SPI_FILE_DESCRIPTOR(front_right, FRONT_RIGHT_MOTOR_CHIP_SELECT) - OPEN_SPI_FILE_DESCRIPTOR(back_left, BACK_LEFT_MOTOR_CHIP_SELECT) - OPEN_SPI_FILE_DESCRIPTOR(back_right, BACK_RIGHT_MOTOR_CHIP_SELECT) - OPEN_SPI_FILE_DESCRIPTOR(dribbler, DRIBBLER_MOTOR_CHIP_SELECT) - - // Make this instance available to the static functions above - g_motor_service = this; -} - -MotorService::~MotorService() {} - void MotorService::setup() { - const auto now = std::chrono::system_clock::now(); - long int total_duration_since_last_fault_s = 0; - if (tracked_motor_fault_start_time_.has_value()) - { - total_duration_since_last_fault_s = - std::chrono::duration_cast( - now - tracked_motor_fault_start_time_.value()) - .count(); - } - - if (tracked_motor_fault_start_time_.has_value() && - total_duration_since_last_fault_s < MOTOR_FAULT_TIME_THRESHOLD_S) - { - num_tracked_motor_resets_++; - } - else - { - tracked_motor_fault_start_time_ = std::make_optional(now); - num_tracked_motor_resets_ = 1; - } - - - if (tracked_motor_fault_start_time_.has_value() && - num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) - { - LOG(FATAL) << "In the last " << total_duration_since_last_fault_s - << "s, the motor board has reset " << num_tracked_motor_resets_ - << " times. Thunderloop crashing for safety."; - } - - prev_wheel_velocities_ = {0.0, 0.0, 0.0, 0.0}; - - // Clear faults by resetting all the chips on the motor board - reset_gpio_->setValue(GpioState::LOW); - usleep(MICROSECONDS_PER_MILLISECOND * 100); - - reset_gpio_->setValue(GpioState::HIGH); - usleep(MICROSECONDS_PER_MILLISECOND * 100); - - for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) - { - LOG(INFO) << "Clearing RESET for " << MOTOR_NAMES[motor]; - tmc6100_writeInt(motor, TMC6100_GSTAT, 0x00000001); - cached_motor_faults_[motor] = MotorFaultIndicator(); - encoder_calibrated_[motor] = false; - } - - // Drive Motor Setup - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; motor++) - { - setUpDriveMotor(motor); - } + prev_wheel_velocities_ = WheelSpace_t::Zero(); + target_wheel_velocities_ = WheelSpace_t::Zero(); + dribbler_target_rpm_ = 0; - // Dribbler Motor Setup - startDriver(DRIBBLER_MOTOR_CHIP_SELECT); - checkDriverFault(DRIBBLER_MOTOR_CHIP_SELECT); - startController(DRIBBLER_MOTOR_CHIP_SELECT, true); - tmc4671_setTargetVelocity(DRIBBLER_MOTOR_CHIP_SELECT, 0); - checkEncoderConnections(); - - // calibrate the encoders - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; motor++) - { - startEncoderCalibration(motor); - } - - sleep(1); - - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; motor++) - { - endEncoderCalibration(motor); - } - - is_initialized_ = true; + motor_controller_->setup(); } -void MotorService::setUpDriveMotor(uint8_t motor) +void MotorService::reset() { - startDriver(motor); - checkDriverFault(motor); - // Start all the controllers as drive motor controllers - startController(motor, false); - tmc4671_setTargetVelocity(motor, 0); + motor_controller_->reset(); } -MotorService::MotorFaultIndicator MotorService::checkDriverFault(uint8_t motor) +std::unique_ptr MotorService::setupMotorController() { - bool drive_enabled = true; - std::unordered_set motor_faults; - - int gstat = tmc6100_readInt(motor, TMC6100_GSTAT); - std::bitset<32> gstat_bitset(gstat); - - if (gstat_bitset.any()) - { - LOG(WARNING) << "======= Faults For Motor " << std::to_string(motor) << "======="; - } - - if (gstat_bitset[0]) - { - LOG(WARNING) - << "Indicates that the IC has been reset. All registers have been cleared to reset values." - << "Attention: DRV_EN must be high to allow clearing reset"; - motor_faults.insert(TbotsProto::MotorFault::RESET); - } - - if (gstat_bitset[1]) - { - LOG(WARNING) - << "drv_otpw : Indicates, that the driver temperature has exceeded overtemperature prewarning-level." - << "No action is taken. This flag is latched."; - motor_faults.insert(TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE_PREWARNING); - } - - if (gstat_bitset[2]) - { - LOG(WARNING) - << "drv_ot: Indicates, that the driver has been shut down due to overtemperature." - << "This flag can only be cleared when the temperature is below the limit again." - << "It is latched for information."; - motor_faults.insert(TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE); - } - - if (gstat_bitset[3]) - { - LOG(WARNING) << "uv_cp: Indicates an undervoltage on the charge pump." - << "The driver is disabled during undervoltage." - << "This flag is latched for information."; - motor_faults.insert(TbotsProto::MotorFault::UNDERVOLTAGE_CHARGEPUMP); - drive_enabled = false; - } - - if (gstat_bitset[4]) - { - LOG(WARNING) << "shortdet_u: Short to GND detected on phase U." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_COUNTER_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[5]) - { - LOG(WARNING) << "s2gu: Short to GND detected on phase U." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_GND_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[6]) - { - LOG(WARNING) << "s2vsu: Short to VS detected on phase U." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_VS_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[8]) - { - LOG(WARNING) << "shortdet_v: V short counter has triggered at least once."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_COUNTER_DETECTED); - } - - if (gstat_bitset[9]) - { - LOG(WARNING) << "s2gv: Short to GND detected on phase V." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_GND_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[10]) - { - LOG(WARNING) << "s2vsv: Short to VS detected on phase V." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_VS_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[12]) - { - LOG(WARNING) << "shortdet_w: short counter has triggered at least once."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_COUNTER_DETECTED); - } - - if (gstat_bitset[13]) + if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { - LOG(WARNING) << "s2gw: Short to GND detected on phase W." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_GND_DETECTED); - drive_enabled = false; + return std::make_unique(); } - - if (gstat_bitset[14]) + else { - LOG(WARNING) << "s2vsw: Short to VS detected on phase W." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_VS_DETECTED); - drive_enabled = false; + return std::make_unique(robot_constants_); } - - return MotorFaultIndicator(drive_enabled, motor_faults); } -TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_velocity_mps, - double front_right_velocity_mps, - double back_left_velocity_mps, - double back_right_velocity_mps, - double dribbler_rpm) +TbotsProto::MotorStatus MotorService::createMotorStatus( + const WheelSpace_t& current_wheel_velocities, const double dribbler_rpm) const { TbotsProto::MotorStatus motor_status; - cached_motor_faults_[motor_fault_detector_] = checkDriverFault(motor_fault_detector_); - - for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) + for (const MotorIndex motor : reflective_enum::values()) { - if (motor != DRIBBLER_MOTOR_CHIP_SELECT) + const MotorFaultIndicator& motor_faults = motor_controller_->checkFaults(motor); + + if (motor != MotorIndex::DRIBBLER) { TbotsProto::DriveUnit drive_status; - drive_status.set_enabled(cached_motor_faults_[motor].drive_enabled); + drive_status.set_enabled(motor_faults.drive_enabled); - for (const TbotsProto::MotorFault& fault : - cached_motor_faults_[motor].motor_faults) + for (const TbotsProto::MotorFault& fault : motor_faults.faults) { drive_status.add_motor_faults(fault); } - if (motor == FRONT_LEFT_MOTOR_CHIP_SELECT) + if (motor == MotorIndex::FRONT_LEFT) { *(motor_status.mutable_front_left()) = drive_status; } - if (motor == FRONT_RIGHT_MOTOR_CHIP_SELECT) + if (motor == MotorIndex::FRONT_RIGHT) { *(motor_status.mutable_front_right()) = drive_status; } - if (motor == BACK_LEFT_MOTOR_CHIP_SELECT) + if (motor == MotorIndex::BACK_LEFT) { *(motor_status.mutable_back_left()) = drive_status; } - if (motor == BACK_RIGHT_MOTOR_CHIP_SELECT) + if (motor == MotorIndex::BACK_RIGHT) { *(motor_status.mutable_back_right()) = drive_status; } @@ -376,9 +82,9 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci { TbotsProto::DribblerStatus dribbler_status; dribbler_status.set_dribbler_rpm(static_cast(dribbler_rpm)); - dribbler_status.set_enabled(cached_motor_faults_[motor].drive_enabled); - for (const TbotsProto::MotorFault& fault : - cached_motor_faults_[motor].motor_faults) + dribbler_status.set_enabled(motor_faults.drive_enabled); + + for (const TbotsProto::MotorFault& fault : motor_faults.faults) { dribbler_status.add_motor_faults(fault); } @@ -388,124 +94,81 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci } motor_status.mutable_front_left()->set_wheel_velocity( - static_cast(front_left_velocity_mps)); + static_cast(current_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX])); motor_status.mutable_front_right()->set_wheel_velocity( - static_cast(front_right_velocity_mps)); + static_cast(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX])); motor_status.mutable_back_left()->set_wheel_velocity( - static_cast(back_left_velocity_mps)); + static_cast(current_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX])); motor_status.mutable_back_right()->set_wheel_velocity( - static_cast(back_right_velocity_mps)); - - motor_fault_detector_ = - static_cast((motor_fault_detector_ + 1) % NUM_MOTORS); + static_cast(current_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX])); return motor_status; } -TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor, - double time_elapsed_since_last_poll_s) +TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor_control, + const double time_elapsed_since_last_poll_s) { - bool encoders_calibrated = (encoder_calibrated_[FRONT_LEFT_MOTOR_CHIP_SELECT] || - encoder_calibrated_[FRONT_RIGHT_MOTOR_CHIP_SELECT] || - encoder_calibrated_[BACK_LEFT_MOTOR_CHIP_SELECT] || - encoder_calibrated_[BACK_RIGHT_MOTOR_CHIP_SELECT]); - - if (!encoders_calibrated) + if (anyMotorRequiresReset()) { - is_initialized_ = false; + LOG(INFO) << "Resetting motors due to fault indicators requiring reset"; + trackMotorReset(); + setup(); } - // checks if any motor has reset, sends a log message if so - for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) - { - if (requiresMotorReinit(motor)) - { - LOG(DEBUG) << "RESET DETECTED FOR MOTOR: " << MOTOR_NAMES[motor]; - is_initialized_ = false; - } - } + const Eigen::Vector4 target_wheel_rpms = + (target_wheel_velocities_ / drive_motor_mps_per_rpm_).cast(); - if (!is_initialized_) - { - LOG(INFO) << "MotorService re-initializing"; - setup(); - } + const Eigen::Vector4 current_wheel_rpms = { + motor_controller_->readThenWriteVelocity( + MotorIndex::FRONT_RIGHT, target_wheel_rpms[FRONT_RIGHT_WHEEL_SPACE_INDEX]), + motor_controller_->readThenWriteVelocity( + MotorIndex::FRONT_LEFT, target_wheel_rpms[FRONT_LEFT_WHEEL_SPACE_INDEX]), + motor_controller_->readThenWriteVelocity( + MotorIndex::BACK_LEFT, target_wheel_rpms[BACK_LEFT_WHEEL_SPACE_INDEX]), + motor_controller_->readThenWriteVelocity( + MotorIndex::BACK_RIGHT, target_wheel_rpms[BACK_RIGHT_WHEEL_SPACE_INDEX]), + }; - CHECK(encoder_calibrated_[FRONT_LEFT_MOTOR_CHIP_SELECT] && - encoder_calibrated_[FRONT_RIGHT_MOTOR_CHIP_SELECT] && - encoder_calibrated_[BACK_LEFT_MOTOR_CHIP_SELECT] && - encoder_calibrated_[BACK_RIGHT_MOTOR_CHIP_SELECT]) - << "Running without encoder calibration can cause serious harm, exiting"; + const double dribbler_rpm = motor_controller_->readThenWriteVelocity( + MotorIndex::DRIBBLER, dribbler_target_rpm_); - // Get current wheel electical RPM (don't account for pole pairs). We will use these - // for robot status feedback We assume the motors have ramped to the expected RPM from - // the previous iteration. - double front_right_velocity = - static_cast(tmc4671ReadThenWriteValue( - FRONT_RIGHT_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, front_right_target_rpm)) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; - double front_left_velocity = - static_cast(tmc4671ReadThenWriteValue( - FRONT_LEFT_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, front_left_target_rpm)) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; - double back_right_velocity = - static_cast(tmc4671ReadThenWriteValue( - BACK_RIGHT_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, back_right_target_rpm)) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; - double back_left_velocity = - static_cast(tmc4671ReadThenWriteValue( - BACK_LEFT_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, back_left_target_rpm)) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; - double dribbler_rpm = static_cast( - tmc4671ReadThenWriteValue(DRIBBLER_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, dribbler_ramp_rpm_)); + const WheelSpace_t current_wheel_velocities = + current_wheel_rpms.cast() * drive_motor_mps_per_rpm_; - // Construct a MotorStatus object with the current velocities and dribbler rpm TbotsProto::MotorStatus motor_status = - updateMotorStatus(front_left_velocity, front_right_velocity, back_left_velocity, - back_right_velocity, dribbler_rpm); + createMotorStatus(current_wheel_velocities, dribbler_rpm); - // This order needs to match euclidean_to_four_wheel converters order - // We also want to work in the meters per second space rather than electrical RPMs - WheelSpace_t current_wheel_velocities = {front_right_velocity, front_left_velocity, - back_left_velocity, back_right_velocity}; - - // Run-away protection if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_->setValue(GpioState::LOW); + motor_controller_->immediatelyDisable(); LOG(FATAL) << "Front right motor runaway"; } else if (std::abs(current_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[FRONT_LEFT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_->setValue(GpioState::LOW); + motor_controller_->immediatelyDisable(); LOG(FATAL) << "Front left motor runaway"; } else if (std::abs(current_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[BACK_LEFT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_->setValue(GpioState::LOW); + motor_controller_->immediatelyDisable(); LOG(FATAL) << "Back left motor runaway"; } else if (std::abs(current_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[BACK_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_->setValue(GpioState::LOW); + motor_controller_->immediatelyDisable(); LOG(FATAL) << "Back right motor runaway"; } // Convert to Euclidean velocity_delta - EuclideanSpace_t current_euclidean_velocity = + const EuclideanSpace_t current_euclidean_velocity = euclidean_to_four_wheel_.getEuclideanVelocity(current_wheel_velocities); motor_status.mutable_local_velocity()->set_x_component_meters( @@ -515,685 +178,118 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor motor_status.mutable_angular_velocity()->set_radians_per_second( current_euclidean_velocity[2]); - WheelSpace_t target_wheel_velocities = WheelSpace_t::Zero(); - // Get target wheel velocities from the primitive - if (motor.has_direct_per_wheel_control()) + if (motor_control.has_direct_per_wheel_control()) { - TbotsProto::MotorControl_DirectPerWheelControl direct_per_wheel = - motor.direct_per_wheel_control(); - target_wheel_velocities = { + const auto& direct_per_wheel = motor_control.direct_per_wheel_control(); + + target_wheel_velocities_ = { direct_per_wheel.front_right_wheel_velocity(), direct_per_wheel.front_left_wheel_velocity(), direct_per_wheel.back_left_wheel_velocity(), direct_per_wheel.back_right_wheel_velocity(), }; } - else if (motor.has_direct_velocity_control()) + else if (motor_control.has_direct_velocity_control()) { - TbotsProto::MotorControl_DirectVelocityControl direct_velocity = - motor.direct_velocity_control(); - EuclideanSpace_t target_euclidean_velocity = { + const auto& direct_velocity = motor_control.direct_velocity_control(); + + const EuclideanSpace_t target_euclidean_velocity = { -direct_velocity.velocity().y_component_meters(), direct_velocity.velocity().x_component_meters(), direct_velocity.angular_velocity().radians_per_second()}; - target_wheel_velocities = + motor_controller_->updateEuclideanVelocity(target_euclidean_velocity); + + target_wheel_velocities_ = euclidean_to_four_wheel_.getWheelVelocity(target_euclidean_velocity); } - // ramp the target velocities to keep acceleration compared to current velocities + // Ramp the target velocities to keep acceleration compared to current velocities // within safe bounds - target_wheel_velocities = euclidean_to_four_wheel_.rampWheelVelocity( - prev_wheel_velocities_, target_wheel_velocities, time_elapsed_since_last_poll_s); + target_wheel_velocities_ = euclidean_to_four_wheel_.rampWheelVelocity( + prev_wheel_velocities_, target_wheel_velocities_, time_elapsed_since_last_poll_s); - prev_wheel_velocities_ = target_wheel_velocities; + const EuclideanSpace_t target_euclidean_velocity = + euclidean_to_four_wheel_.getEuclideanVelocity(target_wheel_velocities_); - // Calculate speeds accounting for acceleration - front_right_target_rpm = - static_cast(target_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); - front_left_target_rpm = - static_cast(target_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); - back_left_target_rpm = - static_cast(target_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); - back_right_target_rpm = - static_cast(target_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); + motor_status.mutable_target_local_velocity()->set_x_component_meters( + target_euclidean_velocity[1]); + motor_status.mutable_target_local_velocity()->set_y_component_meters( + -target_euclidean_velocity[0]); + motor_status.mutable_target_angular_velocity()->set_radians_per_second( + target_euclidean_velocity[2]); + + prev_wheel_velocities_ = target_wheel_velocities_; // Get target dribbler rpm from the primitive int target_dribbler_rpm; - if (motor.drive_control_case() == + if (motor_control.drive_control_case() == TbotsProto::MotorControl::DriveControlCase::DRIVE_CONTROL_NOT_SET) { target_dribbler_rpm = 0; } else { - target_dribbler_rpm = motor.dribbler_speed_rpm(); + target_dribbler_rpm = motor_control.dribbler_speed_rpm(); } // Ramp the dribbler velocity // Clamp the max acceleration int max_dribbler_delta_rpm = static_cast( DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 * time_elapsed_since_last_poll_s); - int delta_rpm = std::clamp(target_dribbler_rpm - dribbler_ramp_rpm_, + int delta_rpm = std::clamp(target_dribbler_rpm - dribbler_target_rpm_, -max_dribbler_delta_rpm, max_dribbler_delta_rpm); - dribbler_ramp_rpm_ += delta_rpm; + dribbler_target_rpm_ += delta_rpm; // Clamp to the max rpm - int max_dribbler_rpm = - std::abs(static_cast(robot_constants_.max_force_dribbler_speed_rpm)); - dribbler_ramp_rpm_ = - std::clamp(dribbler_ramp_rpm_, -max_dribbler_rpm, max_dribbler_rpm); + int max_dribbler_rpm = std::abs(robot_constants_.max_force_dribbler_speed_rpm); + dribbler_target_rpm_ = + std::clamp(dribbler_target_rpm_, -max_dribbler_rpm, max_dribbler_rpm); - motor_status.mutable_dribbler()->set_dribbler_rpm(float(dribbler_ramp_rpm_)); + motor_status.mutable_dribbler()->set_dribbler_rpm( + static_cast(dribbler_target_rpm_)); return motor_status; } -bool MotorService::requiresMotorReinit(uint8_t motor) -{ - auto reset_search = - cached_motor_faults_[motor].motor_faults.find(TbotsProto::MotorFault::RESET); - - return !cached_motor_faults_[motor].drive_enabled || - (reset_search != cached_motor_faults_[motor].motor_faults.end()); -} - -void MotorService::spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, - uint32_t spi_speed) +void MotorService::trackMotorReset() { - int ret; - - struct spi_ioc_transfer tr[1]; - memset(tr, 0, sizeof(tr)); - - tr[0].tx_buf = (unsigned long)tx_; - tr[0].rx_buf = (unsigned long)rx_; - tr[0].len = len; - tr[0].delay_usecs = 0; - tr[0].speed_hz = spi_speed; - tr[0].bits_per_word = 8; - - ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); - - CHECK(ret >= 1) << "SPI Transfer to motor failed, not safe to proceed: errno " - << strerror(errno); -} - - -void MotorService::readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, - const uint8_t* write_tx, - const uint8_t* read_rx, uint32_t spi_speed) -{ - uint8_t write_rx[5] = {0}; - - struct spi_ioc_transfer tr[2]; - memset(tr, 0, sizeof(tr)); - - tr[0].tx_buf = (unsigned long)read_tx; - tr[0].rx_buf = (unsigned long)read_rx; - tr[0].len = TMC_CMD_MSG_SIZE; - tr[0].delay_usecs = 0; - tr[0].speed_hz = spi_speed; - tr[0].bits_per_word = 8; - tr[0].cs_change = 0; - tr[1].tx_buf = (unsigned long)write_tx; - tr[1].rx_buf = (unsigned long)write_rx; - tr[1].len = TMC_CMD_MSG_SIZE; - tr[1].delay_usecs = 0; - tr[1].speed_hz = spi_speed; - tr[1].bits_per_word = 8; - tr[1].cs_change = 0; - - int ret1 = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); - int ret2 = ioctl(fd, SPI_IOC_MESSAGE(1), &tr[1]); + const auto now = std::chrono::steady_clock::now(); - CHECK(ret1 >= 1 && ret2 >= 1) - << "SPI Transfer to motor failed, not safe to proceed: errno " << strerror(errno); -} - - -// Both the TMC4671 (the controller) and the TMC6100 (the driver) respect -// the same SPI interface. So when we bind the API, we can use the same -// readWriteByte function, provided that the chip select pin is turning on -// the right chip. -// -// Each TMC4671 controller, TMC6100 driver and encoder group have their chip -// selects coming in from a demux (see diagram below). The demux is controlled -// by two bits {spi_demux_select_0, spi_demux_select_1}. If the bits are -// 10 the TMC4671 is selected, when the select bits are 01 the TMC6100 is -// selected and when they are 11 the encoder is selected. 00 disconnects all -// 3 chips. -// -// -// FRONT LEFT MOTOR -// CONTROLLER + DRIVER + ENCODER -// -// ┌───────┐ ┌───────────────┐ -// │ │ │ │ -// │ 2:4 │ 10 │ ┌─────────┐ │ -// │ ├────────┼──►TMC4671 │ │ B0 -// FRONT_LEFT_CS │ DEMUX │ │ └─────────┘ │ -// ───────────────► │ │ │ -// │ │ 01 │ ┌─────────┐ │ -// │ ├────────┼──►TMC6100 │ │ B1 -// │ │ │ └─────────┘ │ -// │ │ │ │ -// │ │ 11 │ ┌─────────┐ │ -// │ ├────────┼──►ENCODER │ │ B2 -// │ │ │ └─────────┘ │ -// └───▲───┘ │ │ -// │ └───────────────┘ -// │ -// spi_demux_sel_0 & 1 -// -uint8_t MotorService::tmc4671ReadWriteByte(uint8_t motor, uint8_t data, - uint8_t last_transfer) -{ - spi_demux_select_0_->setValue(GpioState::HIGH); - spi_demux_select_1_->setValue(GpioState::LOW); - return readWriteByte(motor, data, last_transfer, TMC4671_SPI_SPEED); -} - -int32_t MotorService::tmc4671ReadThenWriteValue(uint8_t motor, uint8_t read_addr, - uint8_t write_addr, int32_t write_data) -{ - spi_demux_select_0_->setValue(GpioState::HIGH); - spi_demux_select_1_->setValue(GpioState::LOW); - // ensure tx_ and rx_ are cleared - memset(read_tx_, 0, 5); - memset(write_tx_, 0, 5); - memset(read_rx_, 0, 5); - - // Trinamic transactions looks like this: - // + - - - + - - - + - - - + - - - + - - - + - // | ADDR | DATA | - // + - - - + - - - + - - - + - - - + - - - + - // 0 1 2 3 4 - // Also it is in BIG Endian, therefore MSB is leftmost bit of 0. - // For a write, MSB must be 1, for read, MSB must be 0 - // https://github.com/trinamic/TMC-API/blob/master/tmc/ic/TMC4671/TMC4671.c - read_tx_[0] = read_addr & 0x7f; - write_tx_[0] = write_addr | 0x80; - - // Convert from little endian to big endian - for (int i = 3; i >= 0; i--) + if (num_tracked_motor_resets_ == 0) { - uint8_t byte_to_copy = (uint8_t)(0xff & (write_data >> 8 * i)); - write_tx_[4 - i] = byte_to_copy; - } - - readThenWriteSpiTransfer(file_descriptors_[motor], read_tx_, write_tx_, read_rx_, - TMC4671_SPI_SPEED); - - int32_t value = read_rx_[0]; - for (int i = 1; i < 5; i++) - { - value <<= 8; - value |= read_rx_[i]; - } - return value; -} - -uint8_t MotorService::tmc6100ReadWriteByte(uint8_t motor, uint8_t data, - uint8_t last_transfer) -{ - spi_demux_select_0_->setValue(GpioState::LOW); - spi_demux_select_1_->setValue(GpioState::HIGH); - return readWriteByte(motor, data, last_transfer, TMC6100_SPI_SPEED); -} - -uint8_t MotorService::readWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer, - uint32_t spi_speed) -{ - uint8_t ret_byte = 0; - - if (!transfer_started_) - { - memset(tx_, 0, sizeof(tx_)); - memset(rx_, 0, sizeof(rx_)); - position_ = 0; - - if (data & TMC_WRITE_BIT) - { - // If the transfer started and its a write operation, - // set the appropriate flags. - currently_reading_ = false; - currently_writing_ = true; - } - else - { - // The first byte should contain the address on a read operation. - // Trigger a transfer (1 byte) and buffer the response (4 bytes) - tx_[position_] = data; - spiTransfer(file_descriptors_[motor], tx_, rx_, 5, spi_speed); - - currently_reading_ = true; - currently_writing_ = false; - } - - transfer_started_ = true; - } - - if (currently_writing_) - { - // Buffer the data to send out when last_transfer is true. - tx_[position_++] = data; - } - - if (currently_reading_) - { - // If we are reading, we just need to return the buffered data - // byte by byte. - ret_byte = rx_[position_++]; - } - - if (currently_writing_ && last_transfer) - { - // we have all the bytes for this transfer, lets trigger the transfer and - // reset state - spiTransfer(file_descriptors_[motor], tx_, rx_, 5, spi_speed); - transfer_started_ = false; - } - - if (currently_reading_ && last_transfer) - { - // when reading, if last transfer is true, we just need to reset state - transfer_started_ = false; - } - - return ret_byte; -} - -void MotorService::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value) -{ - int num_retires_left = NUM_RETRIES_SPI; - int read_value = 0; - - // The SPI lines have a lot of noise, and sometimes a transfer will fail - // randomly. So we retry a few times before giving up. - while (num_retires_left > 0) - { - tmc6100_writeInt(motor, address, value); - read_value = tmc6100_readInt(motor, address); - if (read_value == value) - { - return; - } - LOG(DEBUG) << "SPI Transfer to Driver Failed, retrying..."; - num_retires_left--; - } - - // If we get here, we have failed to write to the driver. We reset - // the chip to clear any bad values we just wrote and crash so everything stops. - reset_gpio_->setValue(GpioState::LOW); - CHECK(read_value == value) << "Couldn't write " << value - << " to the TMC6100 at address " << address - << " at address " << static_cast(address) - << " on motor " << static_cast(motor) - << " received: " << read_value; -} - -void MotorService::writeIntToTMC4671(uint8_t motor, uint8_t address, int32_t value) -{ - tmc4671_writeInt(motor, address, value); -} - -int MotorService::readIntFromTMC4671(uint8_t motor, uint8_t address) -{ - return tmc4671_readInt(motor, address); -} - -void MotorService::writeToControllerOrDieTrying(uint8_t motor, uint8_t address, - int32_t value) -{ - int num_retires_left = NUM_RETRIES_SPI; - int read_value = 0; - - // The SPI lines have a lot of noise, and sometimes a transfer will fail - // randomly. So we retry a few times before giving up. - while (num_retires_left > 0) - { - tmc4671_writeInt(motor, address, value); - read_value = tmc4671_readInt(motor, address); - if (read_value == value) - { - return; - } - LOG(DEBUG) << "SPI Transfer to Controller Failed, retrying..."; - num_retires_left--; - } - - // If we get here, we have failed to write to the controller. We reset - // the chip to clear any bad values we just wrote and crash so everything stops. - reset_gpio_->setValue(GpioState::LOW); - CHECK(read_value == value) << "Couldn't write " << value - << " to the TMC4671 at address " << address - << " at address " << static_cast(address) - << " on motor " << static_cast(motor) - << " received: " << read_value; -} - -void MotorService::configurePWM(uint8_t motor) -{ - LOG(INFO) << "Configuring PWM for motor " << static_cast(motor); - // Please read the header file and the datasheet for more info - writeToControllerOrDieTrying(motor, TMC4671_PWM_POLARITIES, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_PWM_MAXCNT, 0x00000F9F); - writeToControllerOrDieTrying(motor, TMC4671_PWM_BBM_H_BBM_L, 0x00002828); - writeToControllerOrDieTrying(motor, TMC4671_PWM_SV_CHOP, 0x00000107); -} - -void MotorService::configureDrivePI(uint8_t motor) -{ - LOG(INFO) << "Configuring Drive PI for motor " << static_cast(motor); - // Please read the header file and the datasheet for more info - // These values were calibrated using the TMC-IDE - writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 67109376); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 67109376); - writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_P_VELOCITY_I, 52428800); - - // Explicitly disable the position controller - writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); - - writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 2500); - writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 1000); - - writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 45000); - - tmc4671_switchToMotionMode(motor, TMC4671_MOTION_MODE_VELOCITY); -} - -void MotorService::configureDribblerPI(uint8_t motor) -{ - LOG(INFO) << "Configuring Dribbler PI for motor " << static_cast(motor); - // Please read the header file and the datasheet for more info - // These values were calibrated using the TMC-IDE - writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 39337600); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 39333600); - writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_P_VELOCITY_I, 2621448); - - // Explicitly disable the position controller - writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); - - writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); - // TODO (#2677) support MAX_FORCE mode. This value can go up to 4.8 amps but we set it - // to 2 for now (sufficient for INDEFINITE mode). - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 4000); - writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 40000); - writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 15000); -} - -void MotorService::configureADC(uint8_t motor) -{ - LOG(INFO) << "Configuring ADC for motor " << static_cast(motor); - // ADC configuration - writeToControllerOrDieTrying(motor, TMC4671_ADC_I_SELECT, 0x18000100); - writeToControllerOrDieTrying(motor, TMC4671_dsADC_MDEC_B_MDEC_A, 0x014E014E); - - // These values have been calibrated for the TI INA240 current sense amplifier. - // The scaling is also set to work with both the drive and dribbler motors. - // - // If you wish to use the TMC4671+TMC6100-BOB you can use the following values, - // that work for the AD8418 current sense amplifier - // - // TMC4671_ADC_I0_SCALE_OFFSET = 0x010081DD - // TMC4671_ADC_I1_SCALE_OFFSET = 0x0100818E - // - writeToControllerOrDieTrying(motor, TMC4671_ADC_I0_SCALE_OFFSET, 0x000981DD); - writeToControllerOrDieTrying(motor, TMC4671_ADC_I1_SCALE_OFFSET, 0x0009818E); -} - -void MotorService::configureEncoder(uint8_t motor) -{ - LOG(INFO) << "Configuring Encoder for motor " << static_cast(motor); - // ABN encoder settings - writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_MODE, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PPR, 0x00001000); -} - -void MotorService::configureHall(uint8_t motor) -{ - LOG(INFO) << "Configuring Hall for motor " << static_cast(motor); - // Digital hall settings - writeToControllerOrDieTrying(motor, TMC4671_HALL_MODE, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_HALL_PHI_E_PHI_M_OFFSET, 0x00000000); - - // Feedback selection - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_HALL); - writeToControllerOrDieTrying(motor, TMC4671_VELOCITY_SELECTION, - TMC4671_VELOCITY_PHI_E_HAL); -} - -void MotorService::startEncoderCalibration(uint8_t motor) -{ - LOG(WARNING) << "Calibrating the encoder, ensure the robot is lifted off the ground"; - - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 0x000003E8); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 0x01000100); - writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 0x01000100); - - writeToControllerOrDieTrying(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); - writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PHI_E_PHI_M_OFFSET, - 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, 0x00000001); - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_EXT, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000FFF); -} - -void MotorService::endEncoderCalibration(uint8_t motor) -{ - LOG(WARNING) << "Calibrating the encoder, wheels may move"; - - writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_COUNT, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_ABN); - - encoder_calibrated_[motor] = true; - - configureDrivePI(motor); -} - -void MotorService::runOpenLoopCalibrationRoutine(uint8_t motor, size_t num_samples) -{ - // Some limits - tmc4671_writeInt(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 0x000003E8); - tmc4671_writeInt(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 0x01000100); - tmc4671_writeInt(motor, TMC4671_PID_FLUX_P_FLUX_I, 0x01000100); - - // Open loop settings - tmc4671_writeInt(motor, TMC4671_OPENLOOP_MODE, 0x00000000); - tmc4671_writeInt(motor, TMC4671_OPENLOOP_ACCELERATION, 0x0000003C); - tmc4671_writeInt(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0xFFFFFFFB); - - // Feedback selection - tmc4671_writeInt(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_OPEN_LOOP); - tmc4671_writeInt(motor, TMC4671_UQ_UD_EXT, 0x00000799); - - // Switch to open loop velocity mode - tmc4671_writeInt(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); - - // Rotate right - tmc4671_writeInt(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x0000004A); - - // Setup CSVs - LOG(CSV, "encoder_calibration_" + std::to_string(motor) + ".csv") - << "actual_encoder,estimated_phi\n"; - LOG(CSV, "phase_currents_and_voltages_" + std::to_string(motor) + ".csv") - << "adc_iv,adc_ux,adc_wy,pwm_iv,pwm_ux,pwm_wy\n"; - - // Take samples of the useful registers - for (size_t num_sample = 0; num_sample < num_samples; num_sample++) - { - int estimated_phi = tmc4671_readInt(motor, TMC4671_OPENLOOP_PHI); - int actual_encoder = tmc4671_readRegister16BitValue( - motor, TMC4671_ABN_DECODER_PHI_E_PHI_M, BIT_16_TO_31); - - LOG(CSV, "encoder_calibration_" + std::to_string(motor) + ".csv") - << actual_encoder << "," << estimated_phi << "\n"; - - int16_t adc_v = - tmc4671_readRegister16BitValue(motor, TMC4671_ADC_IV, BIT_0_TO_15); - int16_t adc_u = - tmc4671_readRegister16BitValue(motor, TMC4671_ADC_IWY_IUX, BIT_0_TO_15); - int16_t adc_w = - tmc4671_readRegister16BitValue(motor, TMC4671_ADC_IWY_IUX, BIT_16_TO_31); - - tmc4671_writeInt(motor, TMC4671_INTERIM_ADDR, INTERIM_ADDR_PWM_UV); - int16_t pwm_v = - tmc4671_readRegister16BitValue(motor, TMC4671_INTERIM_DATA, BIT_0_TO_15); - - tmc4671_writeInt(motor, TMC4671_INTERIM_ADDR, INTERIM_ADDR_PWM_WY_UX); - int16_t pwm_u = - tmc4671_readRegister16BitValue(motor, TMC4671_INTERIM_DATA, BIT_0_TO_15); - int16_t pwm_w = - tmc4671_readRegister16BitValue(motor, TMC4671_INTERIM_DATA, BIT_16_TO_31); - - LOG(CSV, "phase_currents_and_voltages_" + std::to_string(motor) + ".csv") - << adc_v << "," << adc_u << "," << adc_w << "," << pwm_v << "," << pwm_u - << "," << pwm_w << "\n"; + tracked_motor_reset_start_time_ = now; + num_tracked_motor_resets_ = 1; + return; } - // Stop open loop rotation - tmc4671_writeInt(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x00000000); -} - -void MotorService::startDriver(uint8_t motor) -{ - // Set the drive strength to 0, the weakest it can go as recommended - // by the TMC4671-TMC6100-BOB datasheet. - int32_t current_drive_conf = tmc6100_readInt(motor, TMC6100_DRV_CONF); - writeToDriverOrDieTrying(motor, TMC6100_DRV_CONF, - current_drive_conf & (~TMC6100_DRVSTRENGTH_MASK)); - writeToDriverOrDieTrying(motor, TMC6100_GCONF, 0x40); - - // All default but updated SHORTFILTER to 2us to avoid false positive shorts - // detection. - writeToDriverOrDieTrying(motor, TMC6100_SHORT_CONF, 0x13020606); - - LOG(DEBUG) << "Driver " << std::to_string(motor) << " accepted conf"; -} - -void MotorService::startController(uint8_t motor, bool dribbler) -{ - // Read the chip ID to validate the SPI connection - tmc4671_writeInt(motor, TMC4671_CHIPINFO_ADDR, 0x000000000); - int chip_id = tmc4671_readInt(motor, TMC4671_CHIPINFO_DATA); + const auto elapsed_s = std::chrono::duration_cast( + now - tracked_motor_reset_start_time_) + .count(); - CHECK(0x34363731 == chip_id) << "The TMC4671 of motor " - << static_cast(motor) << " is not responding"; - - LOG(DEBUG) << "Controller " << std::to_string(motor) - << " online, responded with: " << chip_id; - - // Configure common controller params - configurePWM(motor); - configureADC(motor); - - if (dribbler) + if (elapsed_s < MOTOR_RESET_TIME_THRESHOLD_S) { - // Configure to brushless DC motor with 1 pole pair - writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, 0x00030001); - configureHall(motor); - - configureDribblerPI(motor); + num_tracked_motor_resets_++; } else { - // Configure to brushless DC motor with 8 pole pairs - writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, 0x00030008); - configureEncoder(motor); - } -} - -void MotorService::checkEncoderConnections() -{ - LOG(INFO) << "Starting encoder connection check!"; - - std::vector calibrated_motors(NUM_DRIVE_MOTORS, false); - std::vector initial_velocities(NUM_DRIVE_MOTORS, 0); - - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) - { - // read back current velocity - initial_velocities[motor] = tmc4671_readInt(motor, TMC4671_ABN_DECODER_COUNT); - - // open loop mode can be used without an encoder, set open loop phi positive - // direction - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_MODE, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, - TMC4671_PHI_E_OPEN_LOOP); - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_ACCELERATION, 0x0000003C); - - // represents effective voltage applied to the motors (% voltage) - writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000799); - - // uq_ud_ext mode - writeToControllerOrDieTrying(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); - - // 10 RPM - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x0000000A); - } - - for (int num_iterations = 0; - num_iterations < 10 && - std::any_of(calibrated_motors.begin(), calibrated_motors.end(), - [](bool calibration_status) { return !calibration_status; }); - ++num_iterations) - { - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) - { - if (calibrated_motors[motor]) - { - continue; - } - // now read back the velocity - int read_back_velocity = tmc4671_readInt(motor, TMC4671_ABN_DECODER_COUNT); - LOG(INFO) << MOTOR_NAMES[motor] << " read back: " << read_back_velocity - << " and initially read: " << initial_velocities[motor]; - - if (read_back_velocity != initial_velocities[motor]) - { - calibrated_motors[motor] = true; - } - } - - // sleep for 100 milliseconds - usleep(MICROSECONDS_PER_MILLISECOND * 100); - } - - bool calibrated = true; - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) - { - if (!calibrated_motors[motor]) - { - calibrated = false; - LOG(WARNING) << "Encoder calibration check failure. " << MOTOR_NAMES[motor] - << " did not change as expected"; - } - } - if (!calibrated) - { - LOG(FATAL) - << "Encoder calibration check failure. Not all encoders responded as expected"; + tracked_motor_reset_start_time_ = now; + num_tracked_motor_resets_ = 1; } - // stop all motors, reset back to velocity control mode - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) + if (num_tracked_motor_resets_ > MOTOR_RESET_THRESHOLD_COUNT) { - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x00000000); - tmc4671_switchToMotionMode(motor, TMC4671_MOTION_MODE_VELOCITY); + LOG(FATAL) << "Motors reset too frequently (" << num_tracked_motor_resets_ + << " times in " << elapsed_s + << " seconds). Thunderloop crashing for safety."; } - - LOG(INFO) << "All encoders appear to be connected!"; } -void MotorService::resetMotorBoard() +bool MotorService::anyMotorRequiresReset() const { - reset_gpio_->setValue(GpioState::LOW); + return std::any_of(reflective_enum::values().begin(), + reflective_enum::values().end(), + [&](const MotorIndex motor) + { return motor_controller_->checkFaults(motor).requiresReset(); }); } diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 3efbb40799..445fb03ff5 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -1,22 +1,16 @@ #pragma once -#include #include -#include -#include #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" -#include "shared/constants.h" #include "shared/robot_constants.h" -#include "software/embedded/constants/constants.h" -#include "software/embedded/gpio.h" -#include "software/embedded/gpio_char_dev.h" -#include "software/embedded/gpio_sysfs.h" +#include "software/embedded/gpio/gpio.h" +#include "software/embedded/motor_controller/motor_controller.h" #include "software/physics/euclidean_to_wheel.h" /** - * A service that interacts with the motor. + * A service that interacts with the motors. * * It is responsible for: * - Converting Euclidean velocities to wheel velocities @@ -26,417 +20,88 @@ class MotorService { public: - // SPI Chip Selects - static const uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT = 0; - static const uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3; - static const uint8_t BACK_LEFT_MOTOR_CHIP_SELECT = 1; - static const uint8_t BACK_RIGHT_MOTOR_CHIP_SELECT = 2; - /** - * Service that interacts with the motor board. - * Opens all the required ports and maintains them until destroyed. + * Service that interacts with the motors. * * @param robot_constants The robot constants - * @param control_loop_frequency_hz The frequency the main loop will call poll at */ - MotorService(const robot_constants::RobotConstants& robot_constants, - int control_loop_frequency_hz); - - virtual ~MotorService(); + explicit MotorService(const robot_constants::RobotConstants& robot_constants); /** - * When the motor service is polled with a DirectControlPrimitive msg, - * call the appropriate trinamic api function to spin the appropriate motor. + * Polls the motor service to execute the given motor control command and + * return the current motor status. * - * @param motor The motor msg to unpack and execute on the motors - * @param time_elapsed_since_last_poll_s The time since last poll was called in - * seconds - * @returns MotorStatus The status of all the drive units + * @param motor_control The motor control message to unpack and execute + * @param time_elapsed_since_last_poll_s The time since the last poll in seconds + * @return The status of all the motors */ TbotsProto::MotorStatus poll(const TbotsProto::MotorControl& motor_control, double time_elapsed_since_last_poll_s); - /** - * Trinamic API binding, sets spi_demux_select_0|1 pins - * appropriately and calls readWriteByte. See C++ implementation file for more info - * - * @param motor Which motor to talk to (in our case, the chip select) - * @param data The data to send - * @param last_transfer The last transfer of uint8_t data for this transaction. - * @return A byte read from the trinamic chip - */ - uint8_t tmc4671ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); - uint8_t tmc6100ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); - - /* - * For FOC to work, the controller needs to know the electical angle of the rotor - * relative to the mechanical angle of the rotor. In an incremental-encoder-only - * setup, we can energize the motor coils so that the rotor locks itself along - * one of its pole-pairs, allowing us to reset the encoder. - * - * WARNING: Do not try to spin the motor without initializing the encoder! - * The motor can overheat if the TMC4671 doesn't auto shut-off. - * - * There are some safety checks to ensure that the encoder is - * initialized, do not tamper with them. You have been warned. - * - * @param motor The motor to initialize the encoder for - */ - void startEncoderCalibration(uint8_t motor); - void endEncoderCalibration(uint8_t motor); - - /** - * Spin the motor in openloop mode (safe to run before encoder initialization) - * - * Captures TMC4671_OPENLOOP_PHI and TMC4671_ABN_DECODER_PHI_E and stores it - * in encoder_calibration.csv - * - * Captures adc_iv, adc_ux, adc_wy and pwm_iv, pwm_ux, pwm_wy. - * - * WARNING: Make sure adc_iv is in phase with pwm_iv, adc_ux is in phase - * with pwm_ux, and adc_wy is in phase with pwm_wy. - * - * If you _dont_ do this, you can risk burning the motor. - * - * @param motor The motor to spin in openloop mode - * @param num_samples The number of samples to take on each - */ - void runOpenLoopCalibrationRoutine(uint8_t motor, size_t num_samples); - - /** - * Reset the motor board by toggling the reset GPIO appropriately. Effectively stops - * the motors from moving. - */ - void resetMotorBoard(); - /** * Clears previous faults, configures the motor and checks encoder connections. */ void setup(); /** - * Holds motor fault information for a particular motor and whether any fault has - * caused the motor to be disabled. - */ - struct MotorFaultIndicator - { - bool drive_enabled; - std::unordered_set motor_faults; - - /** - * Construct a default indicator of no faults and running motors. - */ - MotorFaultIndicator() : drive_enabled(true), motor_faults() {} - - /** - * Construct an indicator with faults and whether the motor is enabled. - * - * @param drive_enabled true if the motor is enabled, false if disabled due to a - * motor fault - * @param motor_faults a set of faults associated with this motor - */ - MotorFaultIndicator(bool drive_enabled, - std::unordered_set& motor_faults) - : drive_enabled(drive_enabled), motor_faults(motor_faults) - { - } - }; - - /** - * Log the driver fault in a human readable log msg - * - * @param motor The motor to log the status for - * - * @return a struct containing the motor faults and whether the motor was disabled due - * to the fault + * Resets the motors, effectively stopping them from moving. */ - struct MotorFaultIndicator checkDriverFault(uint8_t motor); - - /** - * Sets up motor as drive motor controllers - * - * @param motor drive motor number - */ - void setUpDriveMotor(uint8_t motor); - - /** - * Used for testing purposes: - * - * Wrapper function that writes int to the TMC4671 - * @param motor drive motor number - * @param address motor address - * @param value write value - */ - void writeIntToTMC4671(uint8_t motor, uint8_t address, int32_t value); - - /** - * Used for testing purposes: - * - * Wrapper function that reads int from the TMC4671 - * @param motor drive motor number - * @param address motor address - * @return read value - */ - int readIntFromTMC4671(uint8_t motor, uint8_t address); + void reset(); private: /** - * Initializes Motor Service - * - * @param robot_constants robot constants for motor service - * @param control_loop_frequency_hz control loop frequency in Hertz - */ - void motorServiceInit(const robot_constants::RobotConstants& robot_constants, - int control_loop_frequency_hz); - - /** - * Calls the configuration functions below in the right sequence - * - * @param motor The motor setup the driver/controller for - * @param dribbler If true, configures the motor to be a dribbler - */ - void startDriver(uint8_t motor); - void startController(uint8_t motor, bool dribbler); - - /** - * Configuration settings - * - * These values were determined by reading the datasheets and user manual - * here: https://www.trinamic.com/support/eval-kits/details/tmc4671-tmc6100-bob/ - * - * If you are planning to change these settings, I highly recommend that you - * plug the motor + encoder pair in the TMC-IDE and use the TMC4671 EVAL - * with the TMC6100 EVAL to get the motor spinning. - * - * Then using the exported registers as a baseline, you can use the - * runOpenLoopCalibrationRoutine and plot the generated csvs. These csvs capture the - * data for encoder calibration and adc configuration, the two most important steps - * for the motor to work. Page 143 (title Setup Guidelines) of the TMC4671 is very - * useful. - * - * @param motor The motor to configure (the same value as the chip select) - */ - void configurePWM(uint8_t motor); - void configureDribblerPI(uint8_t motor); - void configureDrivePI(uint8_t motor); - void configureADC(uint8_t motor); - void configureEncoder(uint8_t motor); - void configureHall(uint8_t motor); - - /** - * A lot of initialization parameters are necessary to function. Even if - * there is a single bit error, we can risk frying the motor driver or - * controller. - * - * The following functions can be used to setup initialization params - * that _must_ be set to continue. A failed call will crash the program - * - * @param motor Which motor to talk to (in our case, the chip select) - * @param address The address to send data to - * @param value The value to write - * - */ - void writeToControllerOrDieTrying(uint8_t motor, uint8_t address, int32_t value); - void writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value); - - /** - * Trigger an SPI transfer over an open SPI connection - * - * @param fd The SPI File Descriptor to transfer data over - * @param tx The tx buffer, data to send out - * @param rx The rx buffer, will be updated with data from the full-duplex transfer - * @param len The length of the tx and rx buffer - * @param spi_speed The speed to run spi at - * - */ - void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, - uint32_t spi_speed); - - /** - * Performs two back to back SPI transactions, first a read and then a write. - * NOTE: read_tx and write_tx must both be in BIG ENDIAN, as required by the - * Trinamic controller - * - * @param fd the SPI file descriptor to transfer data over - * @param read_tx pointer to the buffer containing the address for reading - * @param write_tx pointer to the buffer containing the address + data for write - * @param read_rx the buffer our read response will be placed in - * @param spi_speed the speed to run spi at - */ - void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, uint8_t const* write_tx, - uint8_t const* read_rx, uint32_t spi_speed); - - /** - * A function which is written in the same style as the rest of the Trinamic API. - * This will trigger two SPI transactions back to back, reading a value and then - * writing a value for a specific motor - * - * @param motor The motor we want to read & write from - * @param read_addr the address of the register to read - * @param write_addr the address of the register to write - * @param write_data the data to write - * @return the value read from the trinamic controller - */ - int32_t tmc4671ReadThenWriteValue(uint8_t motor, uint8_t read_addr, - uint8_t write_addr, int32_t write_data); - - /** - * Trinamic API Binding function + * Creates a motor controller based on the motor board type specified at compile time. * - * @param motor Which motor to talk to (in our case, the chip select) - * @param data The data to send - * @param last_transfer The last transfer of uint8_t data for this transaction. - * @param spi_speed The speed to run spi at - * - * @return A byte read from the trinamic chip + * @return the instantiated motor controller implementation corresponding to the + * motor board type specified at compile time */ - uint8_t readWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer, - uint32_t spi_speed); - + std::unique_ptr setupMotorController(); /** - * Spin each motor of the NUM_DRIVE_MOTORS in open loop mode to check if the encoder - * is responding as expected. Allows us to do a basic test of whether the encoder is - * physically connected to the motor board. + * Return a MotorStatus proto filled with motor velocities and faults. * - * Leaves the motors connected in MOTION_MODE_VELOCITY - */ - void checkEncoderConnections(); - - /** - * Return a MotorStatus proto filled with motor faults. Some values of the proto are - * previously cached velocities due to SPI transaction costs. - * - * @param front_left_wheel_velocity_mps the front left motor's velocity in - * mechanical MPS - * @param front_right_velocity_mps the front right motor's velocity in - * mechanical MPS - * @param back_left velocity_mps the back left motor's velocity in - * mechanical MPS - * @param back_right_velocity_mps the back right motor's velocity in - * mechanical MPS - * @param dribbler_rpm the dribbler's rotations per minute + * @param current_wheel_velocities the current wheel velocities in m/s + * @param dribbler_rpm the dribbler motor's rotations per minute * * @return a MotorStatus proto with the velocity of each motor as well as their fault * statuses (some faults may be cached) */ - TbotsProto::MotorStatus updateMotorStatus(double front_left_velocity_mps, - double front_right_velocity_mps, - double back_left_velocity_mps, - double back_right_velocity_mps, - double dribbler_rpm); + TbotsProto::MotorStatus createMotorStatus( + const WheelSpace_t& current_wheel_velocities, double dribbler_rpm) const; /** - * Helper function to setup a GPIO pin. Selects the appropriate GPIO implementation - * based on the host platform. - * - * @tparam T The representation of the GPIO number - * @param gpio_number The GPIO number (this is typically different from the hardware - * pin number) - * @param direction The direction of the GPIO pin (input or output) - * @param initial_state The initial state of the GPIO pin (high or low) + * Tracks that a motor reset occurred just now. + * We count the number of resets within a certain time threshold, and if + * this count exceeds a certain level, we crash Thunderloop for safety. */ - template - static std::unique_ptr setupGpio(const T& gpio_number, GpioDirection direction, - GpioState initial_state); + void trackMotorReset(); /** - * Returns true if we've detected a RESET in our cached motor faults indicators or if - * we have a fault that disables drive. + * Checks if any motor requires a reset due to being disabled or having a + * reset fault. * - * @param motor chip select to check for RESETs - * - * @return true if the motor has returned a cached RESET fault, false otherwise + * @return true if any motor requires a reset, false otherwise */ - bool requiresMotorReinit(uint8_t motor); - - // All trinamic RPMS are electrical RPMS, they don't factor in the number of pole - // pairs of the drive motor. - // - // TODO (#2720): compute from robot constants (this was computed by hand and is - // accurate) - static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 0.000111; - static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = - 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; - - // to check if the motors have been calibrated - bool is_initialized_ = false; - - // Select between driver and controller gpio - std::unique_ptr spi_demux_select_0_; - std::unique_ptr spi_demux_select_1_; - - // Enable driver gpio - std::unique_ptr driver_control_enable_gpio_; - std::unique_ptr reset_gpio_; - - // Transfer Buffers for spiTransfer - uint8_t tx_[5] = {0}; - uint8_t rx_[5] = {0}; - - // Transfer Buffers for readThenWriteSpiTransfer - uint8_t write_tx_[5] = {0}; - uint8_t read_tx_[5] = {0}; - uint8_t read_rx_[5] = {0}; - - // Transfer State - bool transfer_started_ = false; - bool currently_writing_ = false; - bool currently_reading_ = false; - uint8_t position_ = 0; - - // SPI File Descriptors - std::unordered_map file_descriptors_; + bool anyMotorRequiresReset() const; robot_constants::RobotConstants robot_constants_; - // Drive Motors + std::unique_ptr motor_controller_; + EuclideanToWheel euclidean_to_four_wheel_; - std::unordered_map encoder_calibrated_; - std::unordered_map cached_motor_faults_; - // Previous wheel velocities WheelSpace_t prev_wheel_velocities_; + WheelSpace_t target_wheel_velocities_; - int front_left_target_rpm = 0; - int front_right_target_rpm = 0; - int back_left_target_rpm = 0; - int back_right_target_rpm = 0; - - // the motor cs id to check for motor faults - uint8_t motor_fault_detector_; - - static const int NUM_CALIBRATION_ATTEMPTS = 10; + int dribbler_target_rpm_; - int dribbler_ramp_rpm_; + double drive_motor_mps_per_rpm_; - std::optional> - tracked_motor_fault_start_time_; + std::chrono::time_point tracked_motor_reset_start_time_; int num_tracked_motor_resets_; - static const uint8_t DRIBBLER_MOTOR_CHIP_SELECT = 4; - - static const uint8_t NUM_DRIVE_MOTORS = 4; - static const uint8_t NUM_MOTORS = NUM_DRIVE_MOTORS + 1; - - static const int MOTOR_FAULT_TIME_THRESHOLD_S = 60; - static const int MOTOR_FAULT_THRESHOLD_COUNT = 3; - - // SPI Trinamic Motor Driver Paths (indexed with chip select above) - static constexpr const char* SPI_PATHS[] = {"/dev/spidev0.0", "/dev/spidev0.1", - "/dev/spidev0.2", "/dev/spidev0.3", - "/dev/spidev0.4"}; - - // Motor names (indexed with chip select above) - static constexpr const char* MOTOR_NAMES[] = {"front_left", "back_left", "back_right", - "front_right", "dribbler"}; + static constexpr int MOTOR_RESET_TIME_THRESHOLD_S = 60; + static constexpr int MOTOR_RESET_THRESHOLD_COUNT = 3; + static constexpr double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; + static constexpr int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; }; - -template -std::unique_ptr MotorService::setupGpio(const T& gpio_number, - GpioDirection direction, - GpioState initial_state) -{ - return std::make_unique(gpio_number, direction, initial_state, - "/dev/gpiochip4"); -} diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp deleted file mode 100644 index cc8abc541f..0000000000 --- a/src/software/embedded/services/robot_auto_test.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include "proto/message_translation/tbots_geometry.h" -#include "proto/primitive/primitive_msg_factory.h" -#include "shared/constants.h" -#include "shared/robot_constants.h" -#include "software/embedded/primitive_executor.h" -#include "software/embedded/services/motor.h" -#include "software/embedded/services/power.h" -#include "software/logger/network_logger.h" -#include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" - -extern "C" -{ -#include "tmc/ic/TMC4671/TMC4671.h" -#include "tmc/ic/TMC4671/TMC4671_Variants.h" -#include "tmc/ic/TMC6100/TMC6100.h" -} - -std::unique_ptr motor_service_; -std::unique_ptr power_service_; -robot_constants::RobotConstants robot_constants_; -int read_value; - -// SPI Chip Selects - -static const uint8_t CHIP_SELECT[] = {motor_service_->FRONT_LEFT_MOTOR_CHIP_SELECT, - motor_service_->FRONT_RIGHT_MOTOR_CHIP_SELECT, - motor_service_->BACK_LEFT_MOTOR_CHIP_SELECT, - motor_service_->BACK_RIGHT_MOTOR_CHIP_SELECT}; - -constexpr int ASCII_4671_IN_HEXADECIMAL = 0x34363731; -constexpr double THRESHOLD = 0.0001; -constexpr int DELAY_NS = 10000; -std::string runtime_dir = "/tmp/tbots/yellow_test"; - -int main(int argc, char** argv) -{ - LoggerSingleton::initializeLogger(runtime_dir, nullptr, false); - - LOG(INFO) << "Running on the Raspberry Pi!"; - - motor_service_ = std::make_unique( - robot_constants::createRobotConstants(), THUNDERLOOP_HZ); - - // Testing Motor board SPI transfer - for (uint8_t chip_select : CHIP_SELECT) - { - LOG(INFO) << "Checking motor: " << int(chip_select); - - // Check driver fault - if (!motor_service_->checkDriverFault(chip_select).drive_enabled) - { - LOG(WARNING) << "Detected Motor Fault"; - } - - motor_service_->writeIntToTMC4671(chip_select, TMC4671_CHIPINFO_ADDR, - 0x000000000); - read_value = - motor_service_->readIntFromTMC4671(chip_select, TMC4671_CHIPINFO_DATA); - - // Check if CHIPINFO_DATA returns 0x34363731 - if (read_value == ASCII_4671_IN_HEXADECIMAL) - { - LOG(INFO) << "SPI Transfer is successful"; - } - else - { - LOG(WARNING) << "SPI Transfer not successful"; - } - } - - motor_service_->resetMotorBoard(); - - // Testing Power board UART transfer - try - { - power_service_ = std::make_unique(); - - usleep(DELAY_NS); - - TbotsProto::PowerStatus power_status = - power_service_->poll(TbotsProto::PowerControl(), 0, 0, 0); - power_status = power_service_->poll(TbotsProto::PowerControl(), 0, 0, 0); - - if (abs(power_status.battery_voltage()) < THRESHOLD) - { - LOG(WARNING) << "Battery voltage is zero"; - } - else if (abs(power_status.capacitor_voltage()) < THRESHOLD) - { - LOG(WARNING) << "Capacitor voltage is zero"; - } - else if (abs(power_status.current_draw()) < THRESHOLD) - { - LOG(WARNING) << "Current draw is zero"; - } - else if (power_status.sequence_num() == 0) - { - LOG(WARNING) << "Sequence number is zero"; - } - } - catch (std::runtime_error& e) - { - LOG(WARNING) << "Unable to communicate with the power board"; - } - - return 0; -} diff --git a/src/software/embedded/spi_utils.cpp b/src/software/embedded/spi_utils.cpp new file mode 100644 index 0000000000..207f02e90b --- /dev/null +++ b/src/software/embedded/spi_utils.cpp @@ -0,0 +1,59 @@ +#include "software/embedded/spi_utils.h" + +#include +#include +#include + +#include "software/logger/logger.h" + +void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, + uint32_t spi_speed) +{ + int ret; + + struct spi_ioc_transfer tr[1]; + memset(tr, 0, sizeof(tr)); + + tr[0].tx_buf = (unsigned long)tx; + tr[0].rx_buf = (unsigned long)rx; + tr[0].len = len; + tr[0].delay_usecs = 0; + tr[0].speed_hz = spi_speed; + tr[0].bits_per_word = 8; + + ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); + + CHECK(ret >= 1) << "SPI Transfer to motor failed, not safe to proceed: errno " + << strerror(errno); +} + +void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* write_tx, + const uint8_t* read_rx, const uint32_t read_len, + const uint32_t write_len, uint32_t spi_speed) +{ + uint8_t write_rx[5] = {0}; + + struct spi_ioc_transfer tr[2]; + memset(tr, 0, sizeof(tr)); + + tr[0].tx_buf = (unsigned long)read_tx; + tr[0].rx_buf = (unsigned long)read_rx; + tr[0].len = read_len; + tr[0].delay_usecs = 0; + tr[0].speed_hz = spi_speed; + tr[0].bits_per_word = 8; + tr[0].cs_change = 0; + tr[1].tx_buf = (unsigned long)write_tx; + tr[1].rx_buf = (unsigned long)write_rx; + tr[1].len = write_len; + tr[1].delay_usecs = 0; + tr[1].speed_hz = spi_speed; + tr[1].bits_per_word = 8; + tr[1].cs_change = 0; + + int ret1 = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); + int ret2 = ioctl(fd, SPI_IOC_MESSAGE(1), &tr[1]); + + CHECK(ret1 >= 1 && ret2 >= 1) + << "SPI Transfer to motor failed, not safe to proceed: errno " << strerror(errno); +} diff --git a/src/software/embedded/spi_utils.h b/src/software/embedded/spi_utils.h new file mode 100644 index 0000000000..098e6e3e22 --- /dev/null +++ b/src/software/embedded/spi_utils.h @@ -0,0 +1,35 @@ +#pragma once + +#include + +#include + +/** + * Trigger an SPI transfer over an open SPI connection + * + * NOTE: tx is expected to be in BIG ENDIAN + * + * @param fd the SPI file descriptor to transfer data over + * @param tx the tx buffer, data to send out + * @param rx the rx buffer, will be updated with data from the full-duplex transfer + * @param len the length of the tx and rx buffer + * @param spi_speed the speed to run spi at in Hz + * + */ +void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, + uint32_t spi_speed); + +/** + * Performs two back to back SPI transactions, first a read and then a write. + * + * NOTE: read_tx and write_tx are both expected to be in BIG ENDIAN + * + * @param fd the SPI file descriptor to transfer data over + * @param read_tx pointer to the buffer containing the address for reading + * @param write_tx pointer to the buffer containing the address + data for write + * @param read_rx the buffer our read response will be placed in + * @param spi_speed the speed to run spi at in Hz + */ +void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* write_tx, + const uint8_t* read_rx, const uint32_t read_len, + const uint32_t write_len, uint32_t spi_speed); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index b8a440a683..b7b300a393 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -8,6 +8,7 @@ #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" +#include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" #include "software/embedded/services/motor.h" #include "software/logger/logger.h" @@ -41,7 +42,7 @@ extern "C" { if (g_motor_service) { - g_motor_service->resetMotorBoard(); + g_motor_service->reset(); } // by now g3log may have died due to the termination signal, so it isn't reliable @@ -69,7 +70,6 @@ extern "C" Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz) - // TODO (#2495): Set the friendly team colour : toml_config_client_(std::make_unique(TOML_CONFIG_FILE_PATH)), motor_status_(std::nullopt), robot_constants_(robot_constants), @@ -114,11 +114,11 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Network Service initialized! Next initializing Power Service"; - power_service_ = std::make_unique(); + // power_service_ = std::make_unique(); LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; - motor_service_ = std::make_unique(robot_constants, loop_hz); + motor_service_ = std::make_unique(robot_constants); g_motor_service = motor_service_.get(); motor_service_->setup(); LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; @@ -279,7 +279,7 @@ void Thunderloop::runLoop() getMilliseconds(poll_time)); // Power Service: execute the power control command - power_status_ = pollPowerService(poll_time); + // power_status_ = pollPowerService(poll_time); thunderloop_status_.set_power_service_poll_time_ms( getMilliseconds(poll_time)); diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index cf70a12b68..6d12cd4c0d 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -164,6 +164,9 @@ class Thunderloop const std::string PATH_TO_RINGBUFFER_LOG = "/usr/bin/dmesg"; std::ifstream log_file = std::ifstream(PATH_TO_RINGBUFFER_LOG); + + // Path to the CPU thermal zone temperature file + const std::string CPU_TEMP_FILE_PATH = "/sys/class/thermal/thermal_zone0/temp"; }; /* diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index ebf0480ac3..9535e727d1 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -43,6 +43,7 @@ def __init__( blue_full_system_proto_unix_io, yellow_full_system_proto_unix_io, gamecontroller, + robot_communication, publish_validation_protos=True, is_yellow_friendly=False, ): @@ -53,6 +54,7 @@ def __init__( :param blue_full_system_proto_unix_io: The blue full system proto unix io to use :param yellow_full_system_proto_unix_io: The yellow full system proto unix io to use :param gamecontroller: The gamecontroller context managed instance + :param robot_communication: The robot communication instance :param publish_validation_protos: whether to publish validation protos :param: is_yellow_friendly: if yellow is the friendly team """ @@ -66,25 +68,45 @@ def __init__( ) self.publish_validation_protos = publish_validation_protos self.is_yellow_friendly = is_yellow_friendly + self.robot_communication = robot_communication logger.info("determining robots on field") # survey field for available robot ids - try: - world = self.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) - self.initial_world = world - self.friendly_robot_ids_field = [ - robot.id for robot in world.friendly_team.team_robots - ] - - logger.info(f"friendly team ids {self.friendly_robot_ids_field}") - - if len(self.friendly_robot_ids_field) == 0: - raise Exception("no friendly robots found on field") + survey_start_time = time.time() + self.friendly_robot_ids_field = [] + while time.time() - survey_start_time < WORLD_BUFFER_TIMEOUT: + try: + world = self.world_buffer.get(block=True, timeout=0.1) + self.initial_world = world + self.friendly_robot_ids_field = [ + robot.id for robot in world.friendly_team.team_robots + ] + + if len(self.friendly_robot_ids_field) > 0: + logger.info(f"friendly team ids {self.friendly_robot_ids_field}") + break + except queue.Empty: + continue + + if len(self.friendly_robot_ids_field) == 0: + raise Exception("no friendly robots found on field within timeout") + + def wait_for_estop_play(self): + """Blocks until the estop is in the PLAY state""" + if self.robot_communication.estop_is_playing: + return + + logger.info("\x1b[33m" + "Waiting for Estop to be in PLAY state..." + "\x1b[0m") + while not self.robot_communication.estop_is_playing: + # We must process events if Thunderscope is running to keep it responsive + if self.thunderscope: + from pyqtgraph.Qt import QtWidgets - except queue.Empty: - raise Exception( - f"No Worlds were received with in {WORLD_BUFFER_TIMEOUT} seconds. Please make sure atleast 1 robot and 1 ball is present on the field." - ) + QtWidgets.QApplication.processEvents() + time.sleep(0.1) + logger.info( + "\x1b[32m" + "Estop is in PLAY state. Proceeding with test." + "\x1b[0m" + ) @override def send_gamecontroller_command( @@ -99,7 +121,7 @@ def send_gamecontroller_command( :param team: The team which the command as attributed to :param final_ball_placement_point: The ball placement point """ - self.gamecontroller.send_ci_input( + self.gamecontroller.send_gc_command( gc_command=gc_command, team=team, final_ball_placement_point=final_ball_placement_point, @@ -124,14 +146,19 @@ def run_test( def stop_test(delay): time.sleep(delay) - if self.thunderscope: - self.thunderscope.close() + # We no longer close thunderscope here, because a test might call run_test multiple times. + # Thunderscope will be closed when the fixture is torn down. def __runner(): time.sleep(LAUNCH_DELAY_S) test_end_time = time.time() + test_timeout_s + # Keep track if we started with any eventually validations + has_eventually_validations = any( + len(seq) > 0 for seq in eventually_validation_sequence_set + ) + while time.time() < test_end_time: while True: try: @@ -172,9 +199,15 @@ def __runner(): # Check that all always validations are always valid validation.check_validation(always_validation_proto_set) - # Check that all eventually validations are eventually valid + # Break if eventually validation passes + if has_eventually_validations and all( + len(seq) == 0 for seq in eventually_validation_sequence_set + ): + break + validation.check_validation(eventually_validation_proto_set) - stop_test(TEST_END_DELAY) + + stop_test(delay=PROCESS_BUFFER_DELAY_S) def excepthook(args): """This function is _critical_ for show_thunderscope to work. @@ -189,14 +222,22 @@ def excepthook(args): threading.excepthook = excepthook + # If visualization is enabled, we need to be careful. + # Thunderscope.show() is blocking. if self.thunderscope: run_test_thread = threading.Thread(target=__runner, daemon=True) run_test_thread.start() - self.thunderscope.show() + + # Only call show if the window is not already open. + # If it IS open, it means we are ALREADY in the Qt event loop, + # which can only happen if we are running this run_test in a background thread. + if not self.thunderscope.is_open(): + self.thunderscope.show() + run_test_thread.join() if self.last_exception: - pytest.fail(str(ex.last_exception)) + pytest.fail(str(self.last_exception)) else: __runner() @@ -322,6 +363,13 @@ def load_command_line_arguments(): help="Disables checking for estop plugged in (ONLY USE FOR LOCAL TESTING)", ) + parser.add_argument( + "--no_visualization", + action="store_true", + default=False, + help="Disables the Thunderscope GUI", + ) + return parser.parse_args() @@ -388,15 +436,17 @@ def field_test_runner(): simulator_proto_unix_io=simulator_proto_unix_io, ) # Inject the proto unix ios into thunderscope and start the test - tscope = Thunderscope( - configure_field_test_view( - simulator_proto_unix_io=simulator_proto_unix_io, - blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, - yellow_is_friendly=args.run_yellow, - ), - layout_path=None, - ) + tscope = None + if not args.no_visualization: + tscope = Thunderscope( + configure_field_test_view( + simulator_proto_unix_io=simulator_proto_unix_io, + blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, + yellow_is_friendly=args.run_yellow, + ), + layout_path=None, + ) # Set control mode for all robots to AI so that packets are sent to the robots for robot_id in range(MAX_ROBOT_IDS_PER_SIDE): @@ -407,9 +457,10 @@ def field_test_runner(): # connect the keyboard estop toggle to the key event if needed if estop_mode == EstopMode.KEYBOARD_ESTOP: - tscope.keyboard_estop_shortcut.activated.connect( - rc_friendly.toggle_keyboard_estop - ) + if tscope: + tscope.keyboard_estop_shortcut.activated.connect( + rc_friendly.toggle_keyboard_estop + ) # we call this method to enable estop automatically when a field test starts rc_friendly.toggle_keyboard_estop() logger.warning( @@ -425,6 +476,7 @@ def field_test_runner(): yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, gamecontroller=gamecontroller, thunderscope=tscope, + robot_communication=rc_friendly, is_yellow_friendly=args.run_yellow, ) diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index 56c31df60b..2aaa6fa7e7 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -3,68 +3,10 @@ from software.simulated_tests.simulated_test_fixture import * from software.logger.logger import create_logger -import math logger = create_logger(__name__) - - -# TODO 2908: Support running this test in both simulator or field mode -# this test can be run either in simulation or on the field -# @pytest.mark.parametrize( -# "robot_x_destination, robot_y_destination", -# [(-2.0, -1), (-2.0, 1.0), (0.0, 1.0), (0.0, -1.0)], -# ) -# def test_basic_movement(simulated_test_runner): -# -# robot_starting_x = 0 -# robot_starting_y = 0 -# ROBOT_ID = 0 -# angle = 0 -# robot_x_destination = -2 -# robot_y_destination = -1 -# rob_pos_p = Point(x_meters=robot_x_destination, y_meters=robot_y_destination) -# -# move_tactic = MoveTactic() -# move_tactic.destination.CopyFrom(rob_pos_p) -# move_tactic.dribbler_mode = DribblerMode.OFF -# move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) -# move_tactic.ball_collision_type = BallCollisionType.AVOID -# move_tactic.auto_chip_or_kick.CopyFrom(AutoChipOrKick(autokick_speed_m_per_s=0.0)) -# move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT -# move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE -# -# # setup world state -# initial_worldstate = create_world_state( -# yellow_robot_locations=[], -# blue_robot_locations=[tbots_cpp.Point(robot_starting_x, robot_starting_y)], -# ball_location=tbots_cpp.Point(1, 1), -# ball_velocity=tbots_cpp.Point(0, 0), -# ) -# simulated_test_runner.set_world_state(initial_worldstate) -# -# # Setup Tactic -# params = AssignedTacticPlayControlParams() -# -# params.assigned_tactics[ROBOT_ID].move.CopyFrom(move_tactic) -# -# # Eventually Validation -# eventually_validation_sequence_set = [ -# [ -# RobotEventuallyEntersRegion( -# regions=[ -# tbots_cpp.Circle( -# tbots_cpp.Point(robot_x_destination, robot_y_destination), 0.2 -# ) -# ] -# ), -# ] -# ] -# simulated_test_runner.set_tactics(params, True) -# -# simulated_test_runner.run_test( -# eventually_validation_sequence_set=eventually_validation_sequence_set, -# test_timeout_s=5, -# ) +import math +import threading # this test can only be run on the field @@ -88,38 +30,60 @@ def test_basic_rotation(field_test_runner): robot = world.friendly_team.team_robots[0] rob_pos_p = robot.current_state.global_position - logger.info("staying in pos {rob_pos_p}") - - for angle in test_angles: - move_tactic = MoveTactic() - move_tactic.destination.CopyFrom(rob_pos_p) - move_tactic.dribbler_mode = DribblerMode.OFF - move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) - move_tactic.ball_collision_type = BallCollisionType.AVOID - move_tactic.auto_chip_or_kick.CopyFrom( - AutoChipOrKick(autokick_speed_m_per_s=0.0) - ) - move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT - move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE - # Setup Tactic - field_test_runner.set_tactics( - blue_tactics={id: move_tactic}, yellow_tactics=None - ) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=5, + def execute_test(): + # Wait for the user to flip the estop to PLAY + field_test_runner.wait_for_estop_play() + + # Force start the game automatically + field_test_runner.gamecontroller.send_gc_command( + proto.ssl_gc_state_pb2.Command.FORCE_START, + proto.ssl_gc_common_pb2.Team.UNKNOWN, + final_ball_placement_point=None, ) + + for angle in test_angles: + print(f"Rotating to {angle} degrees") + move_tactic = MoveTactic() + move_tactic.destination.CopyFrom(rob_pos_p) + move_tactic.dribbler_mode = DribblerMode.OFF + move_tactic.final_orientation.CopyFrom( + Angle(radians=angle * math.pi / 180.0) + ) + move_tactic.ball_collision_type = BallCollisionType.AVOID + move_tactic.auto_chip_or_kick.CopyFrom( + AutoChipOrKick(autokick_speed_m_per_s=0.0) + ) + move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT + move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE + + # Setup Tactic + field_test_runner.set_tactics( + blue_tactics={id: move_tactic}, yellow_tactics=None + ) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=5, + ) + + # validate by eye + logger.info(f"robot set to {angle} orientation") + time.sleep(2) + # Send a halt tactic after the test finishes field_test_runner.set_tactics( blue_tactics={id: HaltTactic()}, yellow_tactics=None ) - # validate by eye - logger.info(f"robot set to {angle} orientation") + test_thread = threading.Thread(target=execute_test, daemon=True) + test_thread.start() - time.sleep(2) + # If thunderscope is enabled, show it in the main thread (blocking) + if field_test_runner.thunderscope: + field_test_runner.thunderscope.show() + + test_thread.join() def test_one_robots_square(field_test_runner): @@ -138,10 +102,10 @@ def test_one_robots_square(field_test_runner): id = world.friendly_team.team_robots[0].id print(f"Running test on robot {id}") - point1 = Point(x_meters=-0.3, y_meters=0.6) - point2 = Point(x_meters=-0.3, y_meters=-0.6) - point3 = Point(x_meters=-1.5, y_meters=-0.6) - point4 = Point(x_meters=-1.5, y_meters=0.6) + point1 = Point(x_meters=0.4, y_meters=0.4) + point2 = Point(x_meters=0.4, y_meters=-0.4) + point3 = Point(x_meters=-0.4, y_meters=-0.4) + point4 = Point(x_meters=-0.4, y_meters=0.4) tactic_0 = MoveTactic( destination=point1, @@ -179,25 +143,48 @@ def test_one_robots_square(field_test_runner): max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, ) - tactics = [tactic_0, tactic_1, tactic_2, tactic_3] - for tactic in tactics: - print(f"Going to {tactic.destination}") + tactics = [tactic_0, tactic_1, tactic_2, tactic_3, tactic_0] - field_test_runner.set_tactics( - blue_tactics={ - id: tactic, - }, - yellow_tactics=None, + def execute_test(): + # Wait for the user to flip the estop to PLAY + field_test_runner.wait_for_estop_play() + + # Force start the game automatically + field_test_runner.gamecontroller.send_gc_command( + proto.ssl_gc_state_pb2.Command.FORCE_START, + proto.ssl_gc_common_pb2.Team.UNKNOWN, + final_ball_placement_point=None, ) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=4, + + for tactic in tactics: + print(f"Going to {tactic.destination}") + + field_test_runner.set_tactics( + blue_tactics={ + id: tactic, + }, + yellow_tactics=None, + ) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=8, + ) + + # Send a halt tactic after the test finishes + field_test_runner.set_tactics( + blue_tactics={id: HaltTactic()}, yellow_tactics=None ) - # Send a halt tactic after the test finishes - field_test_runner.set_tactics(blue_tactics={id: HaltTactic()}, yellow_tactics=None) + test_thread = threading.Thread(target=execute_test, daemon=True) + test_thread.start() + + # If thunderscope is enabled, show it in the main thread (blocking) + if field_test_runner.thunderscope: + field_test_runner.thunderscope.show() + + test_thread.join() if __name__ == "__main__": diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index 518050305f..4620e14393 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -13,16 +13,16 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(robot_id, enable_log_merging), &NetworkSink::sendToNetwork); - // Sink for outputting logs to the terminal - auto colour_cout_sink_handle = logWorker->addSink( - std::make_unique(true), &ColouredCoutSink::displayColouredLog); + logWorker->addSink(std::make_unique(true), + &ColouredCoutSink::displayColouredLog); - auto csv_sink_handle = - logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); + logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); - // Sink for PlotJuggler plotting - auto plotjuggler_handle = logWorker->addSink(std::make_unique(), - &PlotJugglerSink::sendToPlotJuggler); + logWorker->addSink(std::make_unique("tbotswifi5"), + &PlotJugglerSink::sendToPlotJuggler); + + g3::only_change_at_initialization::addLogLevel(CSV); + g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); g3::initializeLogging(logWorker.get()); } @@ -31,7 +31,7 @@ void NetworkLoggerSingleton::initializeLogger(RobotId robot_id, bool enable_log_ { if (!instance) { - NetworkLoggerSingleton::instance = std::shared_ptr( + instance = std::shared_ptr( new NetworkLoggerSingleton(robot_id, enable_log_merging)); } } diff --git a/src/software/thunderscope/gl/layers/gl_movement_field_test_layer.py b/src/software/thunderscope/gl/layers/gl_movement_field_test_layer.py index 8fb312bae6..e06d625e3f 100644 --- a/src/software/thunderscope/gl/layers/gl_movement_field_test_layer.py +++ b/src/software/thunderscope/gl/layers/gl_movement_field_test_layer.py @@ -31,6 +31,11 @@ def __init__( self.cached_team: tbots_cpp.Team = None self.is_selected = False + # State for drag-to-orient movement + self.is_dragging_to_orient = False + self.target_point = None + self.current_orientation = -math.pi / 2 + def select_closest_robot(self, point): """Find the closest robot to a point @@ -54,9 +59,9 @@ def select_closest_robot(self, point): @override def mouse_in_scene_pressed(self, event: MouseInSceneEvent) -> None: - """Move to the point clicked. + """Handle mouse press events. If Shift+Alt+Control is pressed, clicking selects a robot based on the closest point in scene. - If Shift+Alt is pressed, clicking moves a robot to the point in scene. + If Shift+Alt is pressed, clicking starts a drag-to-orient movement sequence. :param event: The event """ @@ -71,13 +76,20 @@ def mouse_in_scene_pressed(self, event: MouseInSceneEvent) -> None: # Shift+Alt+Control is pressed self.select_closest_robot(point) else: - # Shift+Alt is pressed - self.move_to_point(point) + # Shift+Alt is pressed - start drag-to-orient sequence + if not self.is_selected: + logger.warning("No robot selected to be moved") + return + + self.target_point = point + self.current_orientation = -math.pi / 2 + self.is_dragging_to_orient = True - def move_to_point(self, point): - """Move to a point + def move_to_point(self, point, orientation: float = None): + """Move to a point with an optional orientation :param point: the point we are commanding the robot to move to + :param orientation: the final orientation in radians (defaults to -π/2) """ # whether the selected_robot_index would cause index out of range issues if not self.is_selected: @@ -86,11 +98,14 @@ def move_to_point(self, point): robot_id = self.selected_robot_id + if orientation is None: + orientation = -math.pi / 2 + point = Point(x_meters=point.x(), y_meters=point.y()) move_tactic = MoveTactic( destination=point, dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), + final_orientation=Angle(radians=orientation), ball_collision_type=BallCollisionType.AVOID, auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, @@ -102,6 +117,45 @@ def move_to_point(self, point): self.fullsystem_io.send_proto(AssignedTacticPlayControlParams, assign_tactic) + @override + def mouse_in_scene_dragged(self, event: MouseInSceneEvent) -> None: + """Handle mouse drag events to update orientation. + + :param event: The event + """ + if not self.visible(): + return + + if not self.is_dragging_to_orient or self.target_point is None: + return + + # Calculate the angle from the target point to the current mouse position + dx = event.point_in_scene.x() - self.target_point.x() + dy = event.point_in_scene.y() - self.target_point.y() + + # Calculate angle using atan2 (y, x) + self.current_orientation = math.atan2(dy, dx) + + @override + def mouse_in_scene_released(self, event: MouseInSceneEvent) -> None: + """Handle mouse release events to finalize movement with the calculated orientation. + + :param event: The event + """ + if not self.visible(): + return + + if not self.is_dragging_to_orient or self.target_point is None: + return + + # Move to the target point with the calculated orientation + self.move_to_point(self.target_point, self.current_orientation) + + # Reset drag-to-orient state + self.is_dragging_to_orient = False + self.target_point = None + self.current_orientation = -math.pi / 2 + @override def refresh_graphics(self): """Updating the world cache""" diff --git a/src/software/util/make_enum/make_enum.hpp b/src/software/util/make_enum/make_enum.hpp index 8b26be8bc2..eeccb9f970 100644 --- a/src/software/util/make_enum/make_enum.hpp +++ b/src/software/util/make_enum/make_enum.hpp @@ -50,22 +50,27 @@ return enum_size_##name; \ } \ template <> \ - constexpr auto reflective_enum::values() \ + constexpr auto& reflective_enum::values() \ { \ return enum_values_##name; \ } \ template <> \ - constexpr auto reflective_enum::valueNames() \ + constexpr auto& reflective_enum::valueNames() \ { \ return enum_value_names_##name; \ } \ inline std::ostream& operator<<(std::ostream& os, name value) \ { \ - /* This index lookup relies on the assumption that the enum does not manually */ \ - /* specify any values. If it did, the underlying integer of the given value */ \ - /* may be out of range of the vector of strings */ \ - os << reflective_enum::valueNames().at(static_cast(value)); \ + os << reflective_enum::nameOf(value); \ return os; \ + } \ + inline std::string operator+(const std::string& lhs, name rhs) \ + { \ + return lhs + std::string(reflective_enum::nameOf(rhs)); \ + } \ + inline std::string operator+(name lhs, const std::string& rhs) \ + { \ + return std::string(reflective_enum::nameOf(lhs)) + rhs; \ } namespace reflective_enum @@ -93,7 +98,7 @@ constexpr size_t size(); * @return an array with the values of the enum */ template -constexpr auto values(); +constexpr auto& values(); /** * Returns an array contain the string representations of each value @@ -102,7 +107,23 @@ constexpr auto values(); * @return an array with the names of the values in the enum */ template -constexpr auto valueNames(); +constexpr auto& valueNames(); + +/** + * Returns the string representation of the given enum value. + * + * @param value the enum value to get the name of + * + * @return the string representation of the enum value + */ +template +constexpr std::string_view nameOf(const E value) +{ + // This index lookup relies on the assumption that the enum does not manually + // specify any values. If it did, the underlying integer of the given value + // may be out of range of the vector of strings. + return reflective_enum::valueNames().at(static_cast(value)); +} /** * Returns the enum value with the given string representation. @@ -112,7 +133,7 @@ constexpr auto valueNames(); * @return the enum value */ template -constexpr E fromName(const std::string value_name) +constexpr E fromName(const std::string_view value_name) { constexpr size_t enum_size = size(); constexpr auto enum_value_names = valueNames(); @@ -123,6 +144,6 @@ constexpr E fromName(const std::string value_name) return static_cast(i); } } - throw std::invalid_argument(value_name + " cannot be converted to enum value"); + throw std::invalid_argument("Name cannot be converted to enum value"); } } // namespace reflective_enum