From 7ff486b9aa204e1024e443916e13c006884c7576 Mon Sep 17 00:00:00 2001 From: arun Date: Wed, 22 Nov 2023 22:16:08 -0800 Subject: [PATCH 01/71] project vimrc for FZF --- src/.vimrc | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/.vimrc diff --git a/src/.vimrc b/src/.vimrc new file mode 100644 index 0000000000..692a0bb37f --- /dev/null +++ b/src/.vimrc @@ -0,0 +1 @@ +let $FZF_DEFAULT_COMMAND='find . \( -name .vscode -o -name .git -o -name bazel-bin -o -name bazel-out -o -name bazel-src -o -name bazel-testlogs -o -name cc-toolchain \) -prune -o -print' From e2258b1d075915abb1acc77027b8c9528f903a11 Mon Sep 17 00:00:00 2001 From: arun Date: Sat, 25 Nov 2023 08:41:37 -0800 Subject: [PATCH 02/71] filter out .swp files --- src/.vimrc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/.vimrc b/src/.vimrc index 692a0bb37f..6e1fa11c06 100644 --- a/src/.vimrc +++ b/src/.vimrc @@ -1 +1 @@ -let $FZF_DEFAULT_COMMAND='find . \( -name .vscode -o -name .git -o -name bazel-bin -o -name bazel-out -o -name bazel-src -o -name bazel-testlogs -o -name cc-toolchain \) -prune -o -print' +let $FZF_DEFAULT_COMMAND='find . \( -name .vscode -o -name .git -o -name bazel-bin -o -name bazel-out -o -name bazel-src -o -name bazel-testlogs -o -name cc-toolchain -o -name "*.swp" \) -prune -o -print' From 60d72152e2c8ce514b32d17c84fb3998f1372bae Mon Sep 17 00:00:00 2001 From: Arun Date: Sat, 8 Feb 2025 16:54:48 -0800 Subject: [PATCH 03/71] crosstool-ng config --- src/cc_toolchain/crosstool-ng/.config | 932 ++++++++++++++++++++++++++ 1 file changed, 932 insertions(+) create mode 100644 src/cc_toolchain/crosstool-ng/.config diff --git a/src/cc_toolchain/crosstool-ng/.config b/src/cc_toolchain/crosstool-ng/.config new file mode 100644 index 0000000000..d319dd851c --- /dev/null +++ b/src/cc_toolchain/crosstool-ng/.config @@ -0,0 +1,932 @@ +# +# Automatically generated file; DO NOT EDIT. +# crosstool-NG 1.26.0 Configuration +# +CT_CONFIGURE_has_static_link=y +CT_CONFIGURE_has_cxx11=y +CT_CONFIGURE_has_wget=y +CT_CONFIGURE_has_curl=y +CT_CONFIGURE_has_rsync=y +CT_CONFIGURE_has_make_3_81_or_newer=y +CT_CONFIGURE_has_make_4_0_or_newer=y +CT_CONFIGURE_has_libtool_2_4_or_newer=y +CT_CONFIGURE_has_libtoolize_2_4_or_newer=y +CT_CONFIGURE_has_autoconf_2_65_or_newer=y +CT_CONFIGURE_has_autoreconf_2_65_or_newer=y +CT_CONFIGURE_has_automake_1_15_or_newer=y +CT_CONFIGURE_has_gnu_m4_1_4_12_or_newer=y +CT_CONFIGURE_has_python_3_4_or_newer=y +CT_CONFIGURE_has_bison_2_7_or_newer=y +CT_CONFIGURE_has_bison_3_0_4_or_newer=y +CT_CONFIGURE_has_python=y +CT_CONFIGURE_has_git=y +CT_CONFIGURE_has_md5sum=y +CT_CONFIGURE_has_sha1sum=y +CT_CONFIGURE_has_sha256sum=y +CT_CONFIGURE_has_sha512sum=y +CT_CONFIGURE_has_install_with_strip_program=y +CT_VERSION="1.26.0" +CT_VCHECK="" +CT_CONFIG_VERSION_ENV="4" +CT_CONFIG_VERSION_CURRENT="4" +CT_CONFIG_VERSION="4" +CT_MODULES=y + +# +# Paths and misc options +# + +# +# crosstool-NG behavior +# +# CT_OBSOLETE is not set +# CT_EXPERIMENTAL is not set +# CT_DEBUG_CT is not set + +# +# Paths +# +CT_LOCAL_TARBALLS_DIR="${HOME}/src" +CT_SAVE_TARBALLS=y +# CT_TARBALLS_BUILDROOT_LAYOUT is not set +CT_WORK_DIR="${CT_TOP_DIR}/.build" +CT_BUILD_TOP_DIR="${CT_WORK_DIR:-${CT_TOP_DIR}/.build}/${CT_HOST:+HOST-${CT_HOST}/}${CT_TARGET}" +CT_BUILD_DIR="${CT_BUILD_TOP_DIR}/build" +CT_PREFIX_DIR="${CT_PREFIX:-${HOME}/x-tools}/${CT_HOST:+HOST-${CT_HOST}/}${CT_TARGET}" +CT_RM_RF_PREFIX_DIR=y +CT_REMOVE_DOCS=y +CT_INSTALL_LICENSES=y +CT_PREFIX_DIR_RO=y +CT_STRIP_HOST_TOOLCHAIN_EXECUTABLES=y +# CT_STRIP_TARGET_TOOLCHAIN_EXECUTABLES is not set + +# +# Downloading +# +CT_DOWNLOAD_AGENT_WGET=y +# CT_DOWNLOAD_AGENT_CURL is not set +# CT_DOWNLOAD_AGENT_NONE is not set +# CT_FORBID_DOWNLOAD is not set +# CT_FORCE_DOWNLOAD is not set +CT_CONNECT_TIMEOUT=10 +CT_DOWNLOAD_WGET_OPTIONS="--passive-ftp --tries=3 -nc --progress=dot:binary" +# CT_ONLY_DOWNLOAD is not set +# CT_USE_MIRROR is not set +CT_VERIFY_DOWNLOAD_DIGEST=y +CT_VERIFY_DOWNLOAD_DIGEST_SHA512=y +# CT_VERIFY_DOWNLOAD_DIGEST_SHA256 is not set +# CT_VERIFY_DOWNLOAD_DIGEST_SHA1 is not set +# CT_VERIFY_DOWNLOAD_DIGEST_MD5 is not set +CT_VERIFY_DOWNLOAD_DIGEST_ALG="sha512" +# CT_VERIFY_DOWNLOAD_SIGNATURE is not set + +# +# Extracting +# +# CT_FORCE_EXTRACT is not set +CT_OVERRIDE_CONFIG_GUESS_SUB=y +# CT_ONLY_EXTRACT is not set +CT_PATCH_BUNDLED=y +# CT_PATCH_BUNDLED_LOCAL is not set +CT_PATCH_ORDER="bundled" + +# +# Build behavior +# +CT_PARALLEL_JOBS=0 +CT_LOAD="" +CT_USE_PIPES=y +CT_EXTRA_CFLAGS_FOR_BUILD="" +CT_EXTRA_CXXFLAGS_FOR_BUILD="" +CT_EXTRA_LDFLAGS_FOR_BUILD="" +CT_EXTRA_CFLAGS_FOR_HOST="" +CT_EXTRA_LDFLAGS_FOR_HOST="" +# CT_CONFIG_SHELL_SH is not set +# CT_CONFIG_SHELL_ASH is not set +CT_CONFIG_SHELL_BASH=y +# CT_CONFIG_SHELL_CUSTOM is not set +CT_CONFIG_SHELL="${bash}" + +# +# Logging +# +# CT_LOG_ERROR is not set +# CT_LOG_WARN is not set +# CT_LOG_INFO is not set +CT_LOG_EXTRA=y +# CT_LOG_ALL is not set +# CT_LOG_DEBUG is not set +CT_LOG_LEVEL_MAX="EXTRA" +# CT_LOG_SEE_TOOLS_WARN is not set +CT_LOG_PROGRESS_BAR=y +CT_LOG_TO_FILE=y +CT_LOG_FILE_COMPRESS=y +# end of Paths and misc options + +# +# Target options +# +# CT_ARCH_ALPHA is not set +# CT_ARCH_ARC is not set +CT_ARCH_ARM=y +# CT_ARCH_AVR is not set +# CT_ARCH_BPF is not set +# CT_ARCH_M68K is not set +# CT_ARCH_MIPS is not set +# CT_ARCH_NIOS2 is not set +# CT_ARCH_POWERPC is not set +# CT_ARCH_PRU is not set +# CT_ARCH_S390 is not set +# CT_ARCH_SH is not set +# CT_ARCH_SPARC is not set +# CT_ARCH_X86 is not set +# CT_ARCH_XTENSA is not set +CT_ARCH="arm" +CT_ARCH_CHOICE_KSYM="ARM" +CT_ARCH_CPU="" +CT_ARCH_TUNE="" +CT_ARCH_ARM_SHOW=y + +# +# Options for arm +# +CT_ARCH_ARM_PKG_KSYM="" +CT_ALL_ARCH_CHOICES="ALPHA ARC ARM AVR BPF C6X LOONGARCH M68K MICROBLAZE MIPS MOXIE MSP430 NIOS2 POWERPC PRU RISCV S390 SH SPARC X86 XTENSA" +CT_ARCH_SUFFIX="" +# CT_OMIT_TARGET_VENDOR is not set + +# +# Generic target options +# +# CT_MULTILIB is not set +CT_DEMULTILIB=y +CT_ARCH_SUPPORTS_BOTH_MMU=y +CT_ARCH_DEFAULT_HAS_MMU=y +CT_ARCH_USE_MMU=y +CT_ARCH_SUPPORTS_FLAT_FORMAT=y +CT_ARCH_SUPPORTS_LIBSANITIZER=y +CT_ARCH_SUPPORTS_EITHER_ENDIAN=y +CT_ARCH_DEFAULT_LE=y +# CT_ARCH_BE is not set +CT_ARCH_LE=y +CT_ARCH_ENDIAN="little" +CT_ARCH_SUPPORTS_32=y +CT_ARCH_SUPPORTS_64=y +CT_ARCH_DEFAULT_32=y +CT_ARCH_BITNESS=64 +# CT_ARCH_32 is not set +CT_ARCH_64=y + +# +# Target optimisations +# +CT_ARCH_SUPPORTS_WITH_ARCH=y +CT_ARCH_SUPPORTS_WITH_CPU=y +CT_ARCH_SUPPORTS_WITH_TUNE=y +CT_ARCH_EXCLUSIVE_WITH_CPU=y +CT_ARCH_ARCH="" +CT_TARGET_CFLAGS="" +CT_TARGET_LDFLAGS="" +# end of Target options + +# +# Toolchain options +# + +# +# General toolchain options +# +CT_FORCE_SYSROOT=y +CT_USE_SYSROOT=y +CT_SYSROOT_NAME="sysroot" +CT_SYSROOT_DIR_PREFIX="" +CT_WANTS_STATIC_LINK=y +CT_WANTS_STATIC_LINK_CXX=y +# CT_STATIC_TOOLCHAIN is not set +CT_SHOW_CT_VERSION=y +CT_TOOLCHAIN_PKGVERSION="" +CT_TOOLCHAIN_BUGURL="" + +# +# Tuple completion and aliasing +# +CT_TARGET_VENDOR="tbots" +CT_TARGET_ALIAS_SED_EXPR="" +CT_TARGET_ALIAS="" + +# +# Toolchain type +# +CT_CROSS=y +# CT_CANADIAN is not set +CT_TOOLCHAIN_TYPE="cross" + +# +# Build system +# +CT_BUILD="" +CT_BUILD_PREFIX="" +CT_BUILD_SUFFIX="" + +# +# Misc options +# +# CT_TOOLCHAIN_ENABLE_NLS is not set +# end of Toolchain options + +# +# Operating System +# +CT_KERNEL_SUPPORTS_SHARED_LIBS=y +# CT_KERNEL_BARE_METAL is not set +CT_KERNEL_LINUX=y +CT_KERNEL="linux" +CT_KERNEL_CHOICE_KSYM="LINUX" +CT_KERNEL_LINUX_SHOW=y + +# +# Options for linux +# +CT_KERNEL_LINUX_PKG_KSYM="LINUX" +CT_LINUX_DIR_NAME="linux" +CT_LINUX_USE_WWW_KERNEL_ORG=y +# CT_LINUX_USE_ORACLE is not set +CT_LINUX_USE="LINUX" +CT_LINUX_PKG_NAME="linux" +CT_LINUX_SRC_RELEASE=y +# CT_LINUX_SRC_DEVEL is not set +CT_LINUX_PATCH_ORDER="global" +# CT_LINUX_V_6_4 is not set +# CT_LINUX_V_6_3 is not set +# CT_LINUX_V_6_2 is not set +# CT_LINUX_V_6_1 is not set +# CT_LINUX_V_6_0 is not set +# CT_LINUX_V_5_19 is not set +# CT_LINUX_V_5_18 is not set +# CT_LINUX_V_5_17 is not set +# CT_LINUX_V_5_16 is not set +# CT_LINUX_V_5_15 is not set +# CT_LINUX_V_5_14 is not set +# CT_LINUX_V_5_13 is not set +# CT_LINUX_V_5_12 is not set +# CT_LINUX_V_5_11 is not set +# CT_LINUX_V_5_10 is not set +# CT_LINUX_V_5_9 is not set +# CT_LINUX_V_5_8 is not set +# CT_LINUX_V_5_7 is not set +# CT_LINUX_V_5_4 is not set +# CT_LINUX_V_5_3 is not set +# CT_LINUX_V_5_2 is not set +# CT_LINUX_V_5_1 is not set +# CT_LINUX_V_5_0 is not set +# CT_LINUX_V_4_20 is not set +# CT_LINUX_V_4_19 is not set +# CT_LINUX_V_4_18 is not set +# CT_LINUX_V_4_17 is not set +# CT_LINUX_V_4_16 is not set +# CT_LINUX_V_4_15 is not set +# CT_LINUX_V_4_14 is not set +# CT_LINUX_V_4_13 is not set +# CT_LINUX_V_4_12 is not set +# CT_LINUX_V_4_11 is not set +# CT_LINUX_V_4_10 is not set +CT_LINUX_V_4_9=y +# CT_LINUX_V_4_4 is not set +# CT_LINUX_V_4_1 is not set +# CT_LINUX_V_3_16 is not set +# CT_LINUX_V_3_13 is not set +# CT_LINUX_V_3_12 is not set +# CT_LINUX_V_3_10 is not set +CT_LINUX_VERSION="4.9.335" +CT_LINUX_MIRRORS="$(CT_Mirrors kernel.org linux ${CT_LINUX_VERSION})" +CT_LINUX_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_LINUX_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_LINUX_ARCHIVE_FORMATS=".tar.xz .tar.gz" +CT_LINUX_SIGNATURE_FORMAT="unpacked/.sign" +CT_LINUX_5_19_or_older=y +CT_LINUX_older_than_5_19=y +CT_LINUX_5_12_or_older=y +CT_LINUX_older_than_5_12=y +CT_LINUX_5_5_or_older=y +CT_LINUX_older_than_5_5=y +CT_LINUX_5_3_or_older=y +CT_LINUX_older_than_5_3=y +CT_LINUX_later_than_4_8=y +CT_LINUX_4_8_or_later=y +CT_LINUX_later_than_3_7=y +CT_LINUX_3_7_or_later=y +CT_LINUX_REQUIRE_3_7_or_later=y +CT_LINUX_later_than_3_2=y +CT_LINUX_3_2_or_later=y +CT_LINUX_REQUIRE_3_2_or_later=y +CT_KERNEL_DEP_RSYNC=y +CT_KERNEL_LINUX_VERBOSITY_0=y +# CT_KERNEL_LINUX_VERBOSITY_1 is not set +# CT_KERNEL_LINUX_VERBOSITY_2 is not set +CT_KERNEL_LINUX_VERBOSE_LEVEL=0 +CT_KERNEL_LINUX_INSTALL_CHECK=y +CT_ALL_KERNEL_CHOICES="BARE_METAL LINUX WINDOWS" + +# +# Common kernel options +# +CT_SHARED_LIBS=y +# end of Operating System + +# +# Binary utilities +# +CT_ARCH_BINFMT_ELF=y +CT_BINUTILS_BINUTILS=y +CT_BINUTILS="binutils" +CT_BINUTILS_CHOICE_KSYM="BINUTILS" +CT_BINUTILS_BINUTILS_SHOW=y + +# +# Options for binutils +# +CT_BINUTILS_BINUTILS_PKG_KSYM="BINUTILS" +CT_BINUTILS_DIR_NAME="binutils" +CT_BINUTILS_USE_GNU=y +# CT_BINUTILS_USE_ORACLE is not set +CT_BINUTILS_USE="BINUTILS" +CT_BINUTILS_PKG_NAME="binutils" +CT_BINUTILS_SRC_RELEASE=y +# CT_BINUTILS_SRC_DEVEL is not set +CT_BINUTILS_PATCH_ORDER="global" +# CT_BINUTILS_V_2_40 is not set +# CT_BINUTILS_V_2_39 is not set +# CT_BINUTILS_V_2_38 is not set +# CT_BINUTILS_V_2_37 is not set +# CT_BINUTILS_V_2_36 is not set +# CT_BINUTILS_V_2_35 is not set +# CT_BINUTILS_V_2_34 is not set +# CT_BINUTILS_V_2_33 is not set +# CT_BINUTILS_V_2_32 is not set +# CT_BINUTILS_V_2_31 is not set +# CT_BINUTILS_V_2_30 is not set +# CT_BINUTILS_V_2_29 is not set +# CT_BINUTILS_V_2_28 is not set +CT_BINUTILS_V_2_27=y +# CT_BINUTILS_V_2_26 is not set +CT_BINUTILS_VERSION="2.27" +CT_BINUTILS_MIRRORS="$(CT_Mirrors GNU binutils) $(CT_Mirrors sourceware binutils/releases)" +CT_BINUTILS_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_BINUTILS_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_BINUTILS_ARCHIVE_FORMATS=".tar.bz2 .tar.gz" +CT_BINUTILS_SIGNATURE_FORMAT="packed/.sig" +CT_BINUTILS_2_39_or_older=y +CT_BINUTILS_older_than_2_39=y +CT_BINUTILS_2_30_or_older=y +CT_BINUTILS_older_than_2_30=y +CT_BINUTILS_2_27_or_later=y +CT_BINUTILS_2_27_or_older=y +CT_BINUTILS_later_than_2_26=y +CT_BINUTILS_2_26_or_later=y + +# +# GNU binutils +# +CT_BINUTILS_GOLD_SUPPORTS_ARCH=y +CT_BINUTILS_GOLD_SUPPORT=y +CT_BINUTILS_FORCE_LD_BFD_DEFAULT=y +# CT_BINUTILS_LINKER_LD is not set +CT_BINUTILS_LINKER_LD_GOLD=y +CT_BINUTILS_GOLD_INSTALLED=y +CT_BINUTILS_GOLD_THREADS=y +CT_BINUTILS_LINKER_BOTH=y +CT_BINUTILS_LINKERS_LIST="ld,gold" +CT_BINUTILS_LD_WRAPPER=y +CT_BINUTILS_LINKER_DEFAULT="bfd" +CT_BINUTILS_PLUGINS=y +CT_BINUTILS_RELRO=m +CT_BINUTILS_DETERMINISTIC_ARCHIVES=y +CT_BINUTILS_EXTRA_CONFIG_ARRAY="" +# CT_BINUTILS_FOR_TARGET is not set +CT_ALL_BINUTILS_CHOICES="BINUTILS" +# end of Binary utilities + +# +# C-library +# +CT_LIBC_GLIBC=y +# CT_LIBC_UCLIBC_NG is not set +CT_LIBC="glibc" +CT_LIBC_CHOICE_KSYM="GLIBC" +CT_LIBC_GLIBC_SHOW=y + +# +# Options for glibc +# +CT_LIBC_GLIBC_PKG_KSYM="GLIBC" +CT_GLIBC_DIR_NAME="glibc" +CT_GLIBC_USE_GNU=y +# CT_GLIBC_USE_ORACLE is not set +CT_GLIBC_USE="GLIBC" +CT_GLIBC_PKG_NAME="glibc" +CT_GLIBC_SRC_RELEASE=y +# CT_GLIBC_SRC_DEVEL is not set +CT_GLIBC_PATCH_ORDER="global" +# CT_GLIBC_V_2_38 is not set +# CT_GLIBC_V_2_37 is not set +# CT_GLIBC_V_2_36 is not set +# CT_GLIBC_V_2_35 is not set +# CT_GLIBC_V_2_34 is not set +# CT_GLIBC_V_2_33 is not set +# CT_GLIBC_V_2_32 is not set +# CT_GLIBC_V_2_31 is not set +# CT_GLIBC_V_2_30 is not set +# CT_GLIBC_V_2_29 is not set +# CT_GLIBC_V_2_28 is not set +CT_GLIBC_V_2_27=y +# CT_GLIBC_V_2_26 is not set +# CT_GLIBC_V_2_25 is not set +# CT_GLIBC_V_2_24 is not set +# CT_GLIBC_V_2_23 is not set +# CT_GLIBC_V_2_19 is not set +# CT_GLIBC_V_2_17 is not set +CT_GLIBC_VERSION="2.27" +CT_GLIBC_MIRRORS="$(CT_Mirrors GNU glibc)" +CT_GLIBC_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_GLIBC_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_GLIBC_ARCHIVE_FORMATS=".tar.xz .tar.bz2 .tar.gz" +CT_GLIBC_SIGNATURE_FORMAT="packed/.sig" +CT_GLIBC_2_38_or_older=y +CT_GLIBC_older_than_2_38=y +CT_GLIBC_2_37_or_older=y +CT_GLIBC_older_than_2_37=y +CT_GLIBC_2_36_or_older=y +CT_GLIBC_older_than_2_36=y +CT_GLIBC_2_34_or_older=y +CT_GLIBC_older_than_2_34=y +CT_GLIBC_2_32_or_older=y +CT_GLIBC_older_than_2_32=y +CT_GLIBC_2_31_or_older=y +CT_GLIBC_older_than_2_31=y +CT_GLIBC_2_30_or_older=y +CT_GLIBC_older_than_2_30=y +CT_GLIBC_2_29_or_older=y +CT_GLIBC_older_than_2_29=y +CT_GLIBC_2_28_or_older=y +CT_GLIBC_older_than_2_28=y +CT_GLIBC_2_27_or_later=y +CT_GLIBC_2_27_or_older=y +CT_GLIBC_later_than_2_26=y +CT_GLIBC_2_26_or_later=y +CT_GLIBC_later_than_2_25=y +CT_GLIBC_2_25_or_later=y +CT_GLIBC_later_than_2_24=y +CT_GLIBC_2_24_or_later=y +CT_GLIBC_later_than_2_23=y +CT_GLIBC_2_23_or_later=y +CT_GLIBC_later_than_2_20=y +CT_GLIBC_2_20_or_later=y +CT_GLIBC_later_than_2_17=y +CT_GLIBC_2_17_or_later=y +CT_GLIBC_later_than_2_14=y +CT_GLIBC_2_14_or_later=y +CT_GLIBC_DEP_KERNEL_HEADERS_VERSION=y +CT_GLIBC_DEP_BINUTILS=y +CT_GLIBC_DEP_GCC=y +CT_GLIBC_DEP_PYTHON=y +CT_GLIBC_SPARC_ALLOW_V7=y +CT_THREADS="nptl" +CT_GLIBC_BUILD_SSP=y +CT_GLIBC_HAS_LIBIDN_ADDON=y +# CT_GLIBC_USE_LIBIDN_ADDON is not set +CT_GLIBC_NO_SPARC_V8=y +CT_GLIBC_HAS_OBSOLETE_RPC=y +CT_GLIBC_EXTRA_CONFIG_ARRAY="" +CT_GLIBC_CONFIGPARMS="" +CT_GLIBC_ENABLE_DEBUG=y +CT_GLIBC_EXTRA_CFLAGS="-gdwarf-4 -U_FORTIFY_SOURCE -fcommon" +CT_GLIBC_ENABLE_OBSOLETE_RPC=y +# CT_GLIBC_DISABLE_VERSIONING is not set +CT_GLIBC_OLDEST_ABI="" +CT_GLIBC_FORCE_UNWIND=y +# CT_GLIBC_LOCALES is not set +# CT_GLIBC_KERNEL_VERSION_NONE is not set +CT_GLIBC_KERNEL_VERSION_AS_HEADERS=y +# CT_GLIBC_KERNEL_VERSION_CHOSEN is not set +CT_GLIBC_MIN_KERNEL="4.9.335" +CT_GLIBC_SSP_DEFAULT=y +# CT_GLIBC_SSP_NO is not set +# CT_GLIBC_SSP_YES is not set +# CT_GLIBC_SSP_ALL is not set +# CT_GLIBC_SSP_STRONG is not set +# CT_GLIBC_ENABLE_COMMON_FLAG is not set +CT_ALL_LIBC_CHOICES="AVR_LIBC GLIBC MINGW_W64 MOXIEBOX MUSL NEWLIB NONE PICOLIBC UCLIBC_NG" +CT_LIBC_SUPPORT_THREADS_ANY=y +CT_LIBC_SUPPORT_THREADS_NATIVE=y + +# +# Common C library options +# +CT_THREADS_NATIVE=y +# CT_CREATE_LDSO_CONF is not set +CT_LIBC_XLDD=y +# end of C-library + +# +# C compiler +# +CT_CC_CORE_NEEDED=y +CT_CC_SUPPORT_CXX=y +CT_CC_SUPPORT_FORTRAN=y +CT_CC_SUPPORT_ADA=y +CT_CC_SUPPORT_D=y +CT_CC_SUPPORT_JIT=y +CT_CC_SUPPORT_OBJC=y +CT_CC_SUPPORT_OBJCXX=y +CT_CC_SUPPORT_GOLANG=y +CT_CC_GCC=y +CT_CC="gcc" +CT_CC_CHOICE_KSYM="GCC" +CT_CC_GCC_SHOW=y + +# +# Options for gcc +# +CT_CC_GCC_PKG_KSYM="GCC" +CT_GCC_DIR_NAME="gcc" +CT_GCC_USE_GNU=y +# CT_GCC_USE_ORACLE is not set +CT_GCC_USE="GCC" +CT_GCC_PKG_NAME="gcc" +CT_GCC_SRC_RELEASE=y +# CT_GCC_SRC_DEVEL is not set +CT_GCC_PATCH_ORDER="global" +CT_GCC_V_13=y +# CT_GCC_V_12 is not set +# CT_GCC_V_11 is not set +# CT_GCC_V_10 is not set +# CT_GCC_V_9 is not set +# CT_GCC_V_8 is not set +# CT_GCC_V_7 is not set +# CT_GCC_V_6 is not set +# CT_GCC_V_5 is not set +# CT_GCC_V_4_9 is not set +CT_GCC_VERSION="13.2.0" +CT_GCC_MIRRORS="$(CT_Mirrors GNU gcc/gcc-${CT_GCC_VERSION}) $(CT_Mirrors sourceware gcc/releases/gcc-${CT_GCC_VERSION})" +CT_GCC_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_GCC_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_GCC_ARCHIVE_FORMATS=".tar.xz .tar.gz" +CT_GCC_SIGNATURE_FORMAT="" +CT_GCC_later_than_13=y +CT_GCC_13_or_later=y +CT_GCC_later_than_12=y +CT_GCC_12_or_later=y +CT_GCC_later_than_11=y +CT_GCC_11_or_later=y +CT_GCC_later_than_10=y +CT_GCC_10_or_later=y +CT_GCC_later_than_9=y +CT_GCC_9_or_later=y +CT_GCC_later_than_8=y +CT_GCC_8_or_later=y +CT_GCC_later_than_7=y +CT_GCC_7_or_later=y +CT_GCC_later_than_6=y +CT_GCC_6_or_later=y +CT_GCC_later_than_5=y +CT_GCC_5_or_later=y +CT_GCC_later_than_4_9=y +CT_GCC_4_9_or_later=y +CT_GCC_REQUIRE_4_9_or_later=y +CT_CC_GCC_ENABLE_PLUGINS=y +CT_CC_GCC_GOLD=y +CT_CC_GCC_HAS_LIBMPX=y +CT_CC_GCC_ENABLE_CXX_FLAGS="" +CT_CC_GCC_CORE_EXTRA_CONFIG_ARRAY="" +CT_CC_GCC_EXTRA_CONFIG_ARRAY="" +CT_CC_GCC_STATIC_LIBSTDCXX=y +# CT_CC_GCC_SYSTEM_ZLIB is not set +CT_CC_GCC_CONFIG_TLS=m + +# +# Optimisation features +# +CT_CC_GCC_USE_GRAPHITE=y +CT_CC_GCC_USE_LTO=y +CT_CC_GCC_LTO_ZSTD=m + +# +# Settings for libraries running on target +# +# CT_CC_GCC_ENABLE_DEFAULT_PIE is not set +CT_CC_GCC_ENABLE_TARGET_OPTSPACE=y +# CT_CC_GCC_LIBMUDFLAP is not set +# CT_CC_GCC_LIBGOMP is not set +# CT_CC_GCC_LIBSSP is not set +# CT_CC_GCC_LIBQUADMATH is not set +# CT_CC_GCC_LIBSANITIZER is not set +CT_CC_GCC_LIBSTDCXX_VERBOSE=m + +# +# Misc. obscure options. +# +CT_CC_CXA_ATEXIT=y +CT_CC_GCC_TM_CLONE_REGISTRY=m +# CT_CC_GCC_DISABLE_PCH is not set +CT_CC_GCC_SJLJ_EXCEPTIONS=m +CT_CC_GCC_LDBL_128=m +# CT_CC_GCC_BUILD_ID is not set +CT_CC_GCC_LNK_HASH_STYLE_DEFAULT=y +# CT_CC_GCC_LNK_HASH_STYLE_SYSV is not set +# CT_CC_GCC_LNK_HASH_STYLE_GNU is not set +# CT_CC_GCC_LNK_HASH_STYLE_BOTH is not set +CT_CC_GCC_LNK_HASH_STYLE="" +CT_CC_GCC_DEC_FLOATS_AUTO=y +# CT_CC_GCC_DEC_FLOATS_BID is not set +# CT_CC_GCC_DEC_FLOATS_DPD is not set +# CT_CC_GCC_DEC_FLOATS_NO is not set +CT_CC_GCC_DEC_FLOATS="" +CT_ALL_CC_CHOICES="GCC" + +# +# Additional supported languages: +# +CT_CC_LANG_CXX=y +# CT_CC_LANG_FORTRAN is not set +# end of C compiler + +# +# Debug facilities +# +# CT_DEBUG_DUMA is not set +CT_DEBUG_GDB=y +CT_DEBUG_GDB_PKG_KSYM="GDB" +CT_GDB_DIR_NAME="gdb" +CT_GDB_PKG_NAME="gdb" +CT_GDB_SRC_RELEASE=y +# CT_GDB_SRC_DEVEL is not set +CT_GDB_PATCH_ORDER="global" +CT_GDB_V_13=y +# CT_GDB_V_12 is not set +# CT_GDB_V_11 is not set +# CT_GDB_V_10 is not set +# CT_GDB_V_9 is not set +# CT_GDB_V_8_3 is not set +CT_GDB_VERSION="13.2" +CT_GDB_MIRRORS="$(CT_Mirrors GNU gdb) $(CT_Mirrors sourceware gdb/releases)" +CT_GDB_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_GDB_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_GDB_ARCHIVE_FORMATS=".tar.xz .tar.gz" +CT_GDB_SIGNATURE_FORMAT="" +CT_GDB_later_than_13=y +CT_GDB_13_or_later=y +CT_GDB_later_than_12=y +CT_GDB_12_or_later=y +CT_GDB_later_than_11=y +CT_GDB_11_or_later=y +CT_GDB_later_than_10=y +CT_GDB_10_or_later=y +CT_GDB_later_than_8_3=y +CT_GDB_8_3_or_later=y +CT_GDB_CROSS=y +# CT_GDB_CROSS_STATIC is not set +# CT_GDB_CROSS_SIM is not set +CT_GDB_CROSS_PYTHON=y +CT_GDB_CROSS_PYTHON_BINARY="" +CT_GDB_CROSS_EXTRA_CONFIG_ARRAY="" +# CT_GDB_NATIVE is not set +CT_GDB_GDBSERVER=y +# CT_GDB_NATIVE_BUILD_IPA_LIB is not set +# CT_GDB_NATIVE_STATIC_LIBSTDCXX is not set +CT_GDB_GDBSERVER_TOPLEVEL=y +# CT_DEBUG_LTRACE is not set +# CT_DEBUG_STRACE is not set +CT_ALL_DEBUG_CHOICES="DUMA GDB LTRACE STRACE" +# end of Debug facilities + +# +# Companion libraries +# +# CT_COMPLIBS_CHECK is not set +# CT_COMP_LIBS_CLOOG is not set +CT_COMP_LIBS_EXPAT=y +CT_COMP_LIBS_EXPAT_PKG_KSYM="EXPAT" +CT_EXPAT_DIR_NAME="expat" +CT_EXPAT_PKG_NAME="expat" +CT_EXPAT_SRC_RELEASE=y +# CT_EXPAT_SRC_DEVEL is not set +CT_EXPAT_PATCH_ORDER="global" +CT_EXPAT_V_2_5=y +CT_EXPAT_VERSION="2.5.0" +CT_EXPAT_MIRRORS="http://downloads.sourceforge.net/project/expat/expat/${CT_EXPAT_VERSION} https://github.com/libexpat/libexpat/releases/download/R_${CT_EXPAT_VERSION//./_}" +CT_EXPAT_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_EXPAT_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_EXPAT_ARCHIVE_FORMATS=".tar.xz .tar.lz .tar.bz2 .tar.gz" +CT_EXPAT_SIGNATURE_FORMAT="" +CT_COMP_LIBS_GETTEXT=y +CT_COMP_LIBS_GETTEXT_PKG_KSYM="GETTEXT" +CT_GETTEXT_DIR_NAME="gettext" +CT_GETTEXT_PKG_NAME="gettext" +CT_GETTEXT_SRC_RELEASE=y +# CT_GETTEXT_SRC_DEVEL is not set +CT_GETTEXT_PATCH_ORDER="global" +CT_GETTEXT_V_0_21=y +# CT_GETTEXT_V_0_20_1 is not set +# CT_GETTEXT_V_0_19_8_1 is not set +CT_GETTEXT_VERSION="0.21" +CT_GETTEXT_MIRRORS="$(CT_Mirrors GNU gettext)" +CT_GETTEXT_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_GETTEXT_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_GETTEXT_ARCHIVE_FORMATS=".tar.xz .tar.gz" +CT_GETTEXT_SIGNATURE_FORMAT="packed/.sig" +CT_GETTEXT_0_21_or_later=y +CT_GETTEXT_0_21_or_older=y +CT_GETTEXT_INCOMPATIBLE_WITH_UCLIBC_NG=y + +# +# This version of gettext is not compatible with uClibc-NG. Select +# + +# +# a different version if uClibc-NG is used on the target or (in a +# + +# +# Canadian cross build) on the host. +# +CT_COMP_LIBS_GMP=y +CT_COMP_LIBS_GMP_PKG_KSYM="GMP" +CT_GMP_DIR_NAME="gmp" +CT_GMP_PKG_NAME="gmp" +CT_GMP_SRC_RELEASE=y +# CT_GMP_SRC_DEVEL is not set +CT_GMP_PATCH_ORDER="global" +CT_GMP_V_6_2=y +# CT_GMP_V_6_1 is not set +CT_GMP_VERSION="6.2.1" +CT_GMP_MIRRORS="https://gmplib.org/download/gmp https://gmplib.org/download/gmp/archive $(CT_Mirrors GNU gmp)" +CT_GMP_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_GMP_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_GMP_ARCHIVE_FORMATS=".tar.xz .tar.lz .tar.bz2" +CT_GMP_SIGNATURE_FORMAT="packed/.sig" +CT_COMP_LIBS_ISL=y +CT_COMP_LIBS_ISL_PKG_KSYM="ISL" +CT_ISL_DIR_NAME="isl" +CT_ISL_PKG_NAME="isl" +CT_ISL_SRC_RELEASE=y +# CT_ISL_SRC_DEVEL is not set +CT_ISL_PATCH_ORDER="global" +CT_ISL_V_0_26=y +# CT_ISL_V_0_25 is not set +# CT_ISL_V_0_24 is not set +# CT_ISL_V_0_23 is not set +# CT_ISL_V_0_22 is not set +# CT_ISL_V_0_21 is not set +# CT_ISL_V_0_20 is not set +# CT_ISL_V_0_19 is not set +# CT_ISL_V_0_18 is not set +# CT_ISL_V_0_17 is not set +# CT_ISL_V_0_16 is not set +# CT_ISL_V_0_15 is not set +CT_ISL_VERSION="0.26" +CT_ISL_MIRRORS="https://libisl.sourceforge.io" +CT_ISL_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_ISL_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_ISL_ARCHIVE_FORMATS=".tar.xz .tar.bz2 .tar.gz" +CT_ISL_SIGNATURE_FORMAT="" +CT_ISL_later_than_0_18=y +CT_ISL_0_18_or_later=y +CT_ISL_later_than_0_15=y +CT_ISL_0_15_or_later=y +# CT_COMP_LIBS_LIBELF is not set +CT_COMP_LIBS_LIBICONV=y +CT_COMP_LIBS_LIBICONV_PKG_KSYM="LIBICONV" +CT_LIBICONV_DIR_NAME="libiconv" +CT_LIBICONV_PKG_NAME="libiconv" +CT_LIBICONV_SRC_RELEASE=y +# CT_LIBICONV_SRC_DEVEL is not set +CT_LIBICONV_PATCH_ORDER="global" +CT_LIBICONV_V_1_16=y +# CT_LIBICONV_V_1_15 is not set +CT_LIBICONV_VERSION="1.16" +CT_LIBICONV_MIRRORS="$(CT_Mirrors GNU libiconv)" +CT_LIBICONV_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_LIBICONV_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_LIBICONV_ARCHIVE_FORMATS=".tar.gz" +CT_LIBICONV_SIGNATURE_FORMAT="packed/.sig" +CT_COMP_LIBS_MPC=y +CT_COMP_LIBS_MPC_PKG_KSYM="MPC" +CT_MPC_DIR_NAME="mpc" +CT_MPC_PKG_NAME="mpc" +CT_MPC_SRC_RELEASE=y +# CT_MPC_SRC_DEVEL is not set +CT_MPC_PATCH_ORDER="global" +CT_MPC_V_1_2=y +CT_MPC_VERSION="1.2.1" +CT_MPC_MIRRORS="https://www.multiprecision.org/downloads $(CT_Mirrors GNU mpc)" +CT_MPC_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_MPC_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_MPC_ARCHIVE_FORMATS=".tar.gz" +CT_MPC_SIGNATURE_FORMAT="packed/.sig" +CT_MPC_later_than_1_1_0=y +CT_MPC_1_1_0_or_later=y +CT_COMP_LIBS_MPFR=y +CT_COMP_LIBS_MPFR_PKG_KSYM="MPFR" +CT_MPFR_DIR_NAME="mpfr" +CT_MPFR_PKG_NAME="mpfr" +CT_MPFR_SRC_RELEASE=y +# CT_MPFR_SRC_DEVEL is not set +CT_MPFR_PATCH_ORDER="global" +CT_MPFR_V_4_2=y +CT_MPFR_VERSION="4.2.1" +CT_MPFR_MIRRORS="https://www.mpfr.org/mpfr-${CT_MPFR_VERSION} $(CT_Mirrors GNU mpfr)" +CT_MPFR_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_MPFR_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_MPFR_ARCHIVE_FORMATS=".tar.xz .tar.bz2 .tar.gz .zip" +CT_MPFR_SIGNATURE_FORMAT="packed/.asc" +CT_MPFR_later_than_4_0_0=y +CT_MPFR_4_0_0_or_later=y +CT_COMP_LIBS_NCURSES=y +CT_COMP_LIBS_NCURSES_PKG_KSYM="NCURSES" +CT_NCURSES_DIR_NAME="ncurses" +CT_NCURSES_PKG_NAME="ncurses" +CT_NCURSES_SRC_RELEASE=y +# CT_NCURSES_SRC_DEVEL is not set +CT_NCURSES_PATCH_ORDER="global" +CT_NCURSES_V_6_4=y +# CT_NCURSES_V_6_2 is not set +# CT_NCURSES_V_6_1 is not set +# CT_NCURSES_V_6_0 is not set +CT_NCURSES_VERSION="6.4" +CT_NCURSES_MIRRORS="https://invisible-mirror.net/archives/ncurses $(CT_Mirrors GNU ncurses)" +CT_NCURSES_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_NCURSES_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_NCURSES_ARCHIVE_FORMATS=".tar.gz" +CT_NCURSES_SIGNATURE_FORMAT="packed/.sig" +CT_NCURSES_NEW_ABI=y +CT_NCURSES_HOST_CONFIG_ARGS="" +CT_NCURSES_HOST_DISABLE_DB=y +CT_NCURSES_HOST_FALLBACKS="linux,xterm,xterm-color,xterm-256color,vt100" +CT_NCURSES_TARGET_CONFIG_ARGS="" +# CT_NCURSES_TARGET_DISABLE_DB is not set +CT_NCURSES_TARGET_FALLBACKS="" +CT_COMP_LIBS_ZLIB=y +CT_COMP_LIBS_ZLIB_PKG_KSYM="ZLIB" +CT_ZLIB_DIR_NAME="zlib" +CT_ZLIB_PKG_NAME="zlib" +CT_ZLIB_SRC_RELEASE=y +# CT_ZLIB_SRC_DEVEL is not set +CT_ZLIB_PATCH_ORDER="global" +CT_ZLIB_V_1_2_13=y +CT_ZLIB_VERSION="1.2.13" +CT_ZLIB_MIRRORS="https://github.com/madler/zlib/releases/download/v${CT_ZLIB_VERSION} https://www.zlib.net/" +CT_ZLIB_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_ZLIB_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_ZLIB_ARCHIVE_FORMATS=".tar.xz .tar.gz" +CT_ZLIB_SIGNATURE_FORMAT="packed/.asc" +CT_COMP_LIBS_ZSTD=y +CT_COMP_LIBS_ZSTD_PKG_KSYM="ZSTD" +CT_ZSTD_DIR_NAME="zstd" +CT_ZSTD_PKG_NAME="zstd" +CT_ZSTD_SRC_RELEASE=y +# CT_ZSTD_SRC_DEVEL is not set +CT_ZSTD_PATCH_ORDER="global" +CT_ZSTD_V_1_5_5=y +# CT_ZSTD_V_1_5_2 is not set +CT_ZSTD_VERSION="1.5.5" +CT_ZSTD_MIRRORS="https://github.com/facebook/zstd/releases/download/v${CT_ZSTD_VERSION} https://www.zstd.net/" +CT_ZSTD_ARCHIVE_FILENAME="@{pkg_name}-@{version}" +CT_ZSTD_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" +CT_ZSTD_ARCHIVE_FORMATS=".tar.gz" +CT_ZSTD_SIGNATURE_FORMAT="packed/.sig" +CT_ALL_COMP_LIBS_CHOICES="CLOOG EXPAT GETTEXT GMP GNUPRUMCU ISL LIBELF LIBICONV MPC MPFR NCURSES NEWLIB_NANO PICOLIBC ZLIB ZSTD" +CT_LIBICONV_NEEDED=y +CT_GETTEXT_NEEDED=y +CT_GMP_NEEDED=y +CT_MPFR_NEEDED=y +CT_ISL_NEEDED=y +CT_MPC_NEEDED=y +CT_EXPAT_NEEDED=y +CT_NCURSES_NEEDED=y +CT_ZLIB_NEEDED=y +CT_ZSTD_NEEDED=y +CT_LIBICONV=y +CT_GETTEXT=y +CT_GMP=y +CT_MPFR=y +CT_ISL=y +CT_MPC=y +CT_EXPAT=y +CT_NCURSES=y +CT_ZLIB=y +CT_ZSTD=y +# end of Companion libraries + +# +# Companion tools +# +# CT_COMP_TOOLS_FOR_HOST is not set +# CT_COMP_TOOLS_AUTOCONF is not set +# CT_COMP_TOOLS_AUTOMAKE is not set +# CT_COMP_TOOLS_BISON is not set +# CT_COMP_TOOLS_DTC is not set +# CT_COMP_TOOLS_LIBTOOL is not set +# CT_COMP_TOOLS_M4 is not set +# CT_COMP_TOOLS_MAKE is not set +CT_ALL_COMP_TOOLS_CHOICES="AUTOCONF AUTOMAKE BISON DTC LIBTOOL M4 MAKE" +# end of Companion tools From a184ae11e712ab3ad5eb0bf6df3995fdf96b5890 Mon Sep 17 00:00:00 2001 From: Arun Date: Wed, 19 Feb 2025 02:54:31 -0800 Subject: [PATCH 04/71] working on getting a nano compilation working --- src/.bazelrc | 3 +++ src/WORKSPACE | 8 ++------ src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ar | 2 +- src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_cpp | 2 +- src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_g++ | 2 +- src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcc | 2 +- .../wrapper/k8_jetson_nano_cross_compile_gcov | 2 +- src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ld | 2 +- src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_nm | 2 +- .../wrapper/k8_jetson_nano_cross_compile_objcopy | 2 +- .../wrapper/k8_jetson_nano_cross_compile_objdump | 2 +- .../wrapper/k8_jetson_nano_cross_compile_strip | 2 +- src/extlibs/k8_jetson_nano_cross_compile_gcc.BUILD | 8 ++++---- .../ai/navigator/trajectory/bang_bang_trajectory_1d.h | 1 + src/software/embedded/services/motor.h | 1 + src/software/logger/plotjuggler_sink.h | 1 + src/software/util/typename/typename.h | 2 ++ 17 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/.bazelrc b/src/.bazelrc index 8f694e5535..4ffc7ec33b 100644 --- a/src/.bazelrc +++ b/src/.bazelrc @@ -15,6 +15,9 @@ build --per_file_copt=//proto/.*,//proto/message_translation/.*,//proto/primitiv # Warn variable length arrays only when compiling cpp build --per_file_copt=.*\.cpp@-Wvla +# Boost 1.84 uses old concept syntax that is not compatible with C++20 +build --cxxopt=-DBOOST_ASIO_DISABLE_CONCEPTS + # Automatically set the CPU environment based on the `--cpu` flag as per our # defined CPU environments build --auto_cpu_environment_group=//cc_toolchain:cpus diff --git a/src/WORKSPACE b/src/WORKSPACE index 1814824150..8761168f63 100644 --- a/src/WORKSPACE +++ b/src/WORKSPACE @@ -233,14 +233,10 @@ new_local_repository( path = "/usr/lib/llvm-6.0/", ) -http_archive( +new_local_repository( name = "k8_jetson_nano_cross_compile_gcc", build_file = "@//extlibs:k8_jetson_nano_cross_compile_gcc.BUILD", - sha256 = "73eed74e593e2267504efbcf3678918bb22409ab7afa3dc7c135d2c6790c2345", - strip_prefix = "gcc-linaro-7.3.1-2018.05-x86_64_aarch64-linux-gnu", - urls = [ - "http://releases.linaro.org/components/toolchain/binaries/7.3-2018.05/aarch64-linux-gnu/gcc-linaro-7.3.1-2018.05-x86_64_aarch64-linux-gnu.tar.xz", - ], + path = "/opt/aarch64-tbots-linux-gnu/", ) # tool used to flash firmware with bazel diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ar b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ar index 233d5e3c4c..2ac2b3c4d3 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ar +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ar @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-ar "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-ar "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_cpp b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_cpp index 8977e9e4fd..dc7828aceb 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_cpp +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_cpp @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-cpp "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-cpp "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_g++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_g++ index a3f3025445..1e08e23e6f 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_g++ +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_g++ @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-g++ -Wl,--no-as-needed "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-g++ -Wl,--no-as-needed "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcc b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcc index cad72ed459..e67e200872 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcc +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcc @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-gcc -Wl,--no-as-needed "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-gcc -Wl,--no-as-needed "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcov b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcov index 7c3616749d..528098adf1 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcov +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcov @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-gcov "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-gcov "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ld b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ld index 7ed51ae2b3..34cb101ae6 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ld +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ld @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-ld "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-ld "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_nm b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_nm index 2eb52826c7..187d32c318 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_nm +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_nm @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-nm "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-nm "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objcopy b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objcopy index 2b324ad0cc..b48a7144d0 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objcopy +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objcopy @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-objcopy "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-objcopy "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objdump b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objdump index 1b69911e81..6a290fda08 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objdump +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objdump @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-objdump "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-objdump "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_strip b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_strip index 67a975cc89..9e7d8bcedf 100755 --- a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_strip +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_strip @@ -1,3 +1,3 @@ #!/bin/bash --norc -exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-strip "$@" +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-tbots-linux-gnu-strip "$@" diff --git a/src/extlibs/k8_jetson_nano_cross_compile_gcc.BUILD b/src/extlibs/k8_jetson_nano_cross_compile_gcc.BUILD index 1383be433f..649a3c48b0 100644 --- a/src/extlibs/k8_jetson_nano_cross_compile_gcc.BUILD +++ b/src/extlibs/k8_jetson_nano_cross_compile_gcc.BUILD @@ -7,20 +7,20 @@ filegroup( filegroup( name = "gcc", - srcs = ["bin/aarch64-linux-gnu-gcc"], + srcs = ["bin/aarch64-tbots-linux-gnu-gcc"], ) filegroup( name = "cpp", - srcs = ["bin/aarch64-linux-gnu-cpp"], + srcs = ["bin/aarch64-tbots-linux-gnu-cpp"], ) filegroup( name = "objcopy", - srcs = ["bin/aarch64-linux-gnu-objcopy"], + srcs = ["bin/aarch64-tbots-linux-gnu-objcopy"], ) filegroup( name = "gdb", - srcs = ["bin/aarch64-linux-gnu-gdb"], + srcs = ["bin/aarch64-tbots-linux-gnu-gdb"], ) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index 8c1a3535f8..cae2d16ff8 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "software/ai/navigator/trajectory/trajectory.hpp" diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 0d5c00c797..762fa3da5e 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -2,6 +2,7 @@ #include #include +#include #include #include "proto/robot_status_msg.pb.h" diff --git a/src/software/logger/plotjuggler_sink.h b/src/software/logger/plotjuggler_sink.h index ff40a4be47..99b1a8583b 100644 --- a/src/software/logger/plotjuggler_sink.h +++ b/src/software/logger/plotjuggler_sink.h @@ -1,5 +1,6 @@ #pragma once #include +#include #include #include "proto/visualization.pb.h" diff --git a/src/software/util/typename/typename.h b/src/software/util/typename/typename.h index 05cbdcfc7f..d94ff253e3 100644 --- a/src/software/util/typename/typename.h +++ b/src/software/util/typename/typename.h @@ -3,6 +3,8 @@ #include #include +#include + /** * Demangles typeid name From adf6f45f0415d0bb478709dd2481845ebce815c0 Mon Sep 17 00:00:00 2001 From: Arun Date: Wed, 19 Feb 2025 02:56:33 -0800 Subject: [PATCH 05/71] seems to compile --- src/software/embedded/gpio_char_dev.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/gpio_char_dev.h b/src/software/embedded/gpio_char_dev.h index bb879ccbc7..23f8263c3b 100644 --- a/src/software/embedded/gpio_char_dev.h +++ b/src/software/embedded/gpio_char_dev.h @@ -1,9 +1,10 @@ #pragma once -#include - #include "software/embedded/gpio.h" +#include +#include + /** * GPIO with the character device interface * From 74a219a718da52cc601d32f6de49b5fdbffd07fe Mon Sep 17 00:00:00 2001 From: Arun Date: Sat, 1 Mar 2025 12:47:14 -0800 Subject: [PATCH 06/71] Working impl of x-compiler --- environment_setup/setup_software.sh | 4 ++++ environment_setup/util.sh | 24 ++++++++++++++++-------- src/WORKSPACE | 2 +- src/cc_toolchain/BUILD | 14 ++++++++++---- 4 files changed, 31 insertions(+), 13 deletions(-) diff --git a/environment_setup/setup_software.sh b/environment_setup/setup_software.sh index 678904e574..18f26dca57 100755 --- a/environment_setup/setup_software.sh +++ b/environment_setup/setup_software.sh @@ -181,6 +181,10 @@ print_status_msg "Install clang-format" install_clang_format $arch print_status_msg "Done installing clang-format" +print_status_msg "Setting up cross compiler for robot software" +install_cross_compiler $arch +print_status_msg "Done setting up cross compiler for robot software" + print_status_msg "Setting Up PlatformIO" # setup platformio to compile arduino code diff --git a/environment_setup/util.sh b/environment_setup/util.sh index e72046ad56..d06e8b7df6 100755 --- a/environment_setup/util.sh +++ b/environment_setup/util.sh @@ -20,19 +20,27 @@ install_bazel() { } install_clang_format() { - download=https://github.com/llvm/llvm-project/releases/download/llvmorg-10.0.0/clang+llvm-10.0.0-aarch64-linux-gnu.tar.xz - clang_format_path=/tmp/tbots_download_cache/clang+llvm-10.0.0-aarch64-linux-gnu/bin/clang-format - - if is_x86 $1; then - download=https://github.com/llvm/llvm-project/releases/download/llvmorg-10.0.0/clang+llvm-10.0.0-x86_64-linux-gnu-ubuntu-18.04.tar.xz - clang_format_path=/tmp/tbots_download_cache/clang+llvm-10.0.0-x86_64-linux-gnu-ubuntu-18.04/bin/clang-format - fi - + arch=$1 + clang_format_file=clang+llvm-18.1.8-$arch-linux-gnu-ubuntu-18.04 + download=https://github.com/llvm/llvm-project/releases/download/llvmorg-18.1.8/$clang-format-file.tar.xz + clang_format_path=/tmp/tbots_download_cache/$clang_format_file/bin/clang-format wget $download -O /tmp/tbots_download_cache/clang.tar.xz tar -xf /tmp/tbots_download_cache/clang.tar.xz -C /tmp/tbots_download_cache/ sudo cp $clang_format_path /opt/tbotspython/bin/clang-format } +install_cross_compiler() { + file_name=aarch64-tbots-linux-gnu-for-aarch64 + if is_x86 $1; then + file_name=aarch64-tbots-linux-gnu-for-x86 + fi + full_file_name=$file_name.tar.xz + wget https://raw.githubusercontent.com/UBC-Thunderbots/Software-External-Dependencies/refs/heads/tbots_compiler/toolchain/$full_file_name -O /tmp/tbots_download_cache/$full_file_name + tar -xf /tmp/tbots_download_cache/$full_file_name -C /tmp/tbots_download_cache/ + sudo mv /tmp/tbots_download_cache/aarch64-tbots-linux-gnu /opt/tbotspython + rm /tmp/tbots_download_cache/$full_file_name +} + install_gamecontroller () { # TODO(#3335): Whenever we deprecate Ubuntu 20.04, we can just grab the latest version of the SSL game controller # binary from the releases page. This is a workaround since the latest version of the game controller is compiled diff --git a/src/WORKSPACE b/src/WORKSPACE index 8761168f63..ea6d6020c1 100644 --- a/src/WORKSPACE +++ b/src/WORKSPACE @@ -236,7 +236,7 @@ new_local_repository( new_local_repository( name = "k8_jetson_nano_cross_compile_gcc", build_file = "@//extlibs:k8_jetson_nano_cross_compile_gcc.BUILD", - path = "/opt/aarch64-tbots-linux-gnu/", + path = "/opt/tbotspython/aarch64-tbots-linux-gnu/", ) # tool used to flash firmware with bazel diff --git a/src/cc_toolchain/BUILD b/src/cc_toolchain/BUILD index d5396df343..49f0284a3a 100644 --- a/src/cc_toolchain/BUILD +++ b/src/cc_toolchain/BUILD @@ -43,11 +43,20 @@ config_setting( values = {"cpu": "k8_jetson_nano_cross_compile"}, ) +# Create a constraint for the glibc version to target the correct glibc version for the robot +constraint_setting(name = "glibc_version") + +constraint_value( + name = "glibc_2_27", + constraint_setting = ":glibc_version", +) + platform( name = "robot", constraint_values = [ "@platforms//cpu:aarch64", "@platforms//os:linux", + ":glibc_2_27", ], ) @@ -210,13 +219,10 @@ cc_toolchain_config_k8_jetson_nano_cross_compile( toolchain( name = "cc_toolchain_for_k8_jetson_nano_cross_compile", - exec_compatible_with = [ - "@platforms//cpu:x86_64", - "@platforms//os:linux", - ], target_compatible_with = [ "@platforms//cpu:aarch64", "@platforms//os:linux", + ":glibc_2_27", ], toolchain = ":cc_toolchain_k8_jetson_nano_cross_compile", toolchain_type = "@bazel_tools//tools/cpp:toolchain_type", From 329019db2d0a34da1cc76ccb4e68edf7cfd07be6 Mon Sep 17 00:00:00 2001 From: Arun Date: Sat, 1 Mar 2025 13:35:18 -0800 Subject: [PATCH 07/71] updat headed_estop_reader test timeout --- .../estop/threaded_estop_reader_test.cpp | 62 +++++++++++-------- 1 file changed, 37 insertions(+), 25 deletions(-) diff --git a/src/software/estop/threaded_estop_reader_test.cpp b/src/software/estop/threaded_estop_reader_test.cpp index 29a1744fd5..dfb836f156 100644 --- a/src/software/estop/threaded_estop_reader_test.cpp +++ b/src/software/estop/threaded_estop_reader_test.cpp @@ -33,7 +33,7 @@ TEST(ThreadedEstopReaderTest, estop_tick_is_called_multiple_times) { std::unique_ptr mock_uart = std::make_unique(); - int test_timeout_ms = 100; + int test_timeout_ms = 200; unsigned char arbitrary_garbage_val = 0b10111011; std::vector play_ret_val(1, ESTOP_PLAY_MSG); std::vector garbage_ret_val(1, arbitrary_garbage_val); @@ -54,10 +54,12 @@ TEST(ThreadedEstopReaderTest, estop_tick_is_called_multiple_times) .WillOnce(Return(play_ret_val)) .WillOnce(Return(play_ret_val)) .WillOnce(Return(play_ret_val)) - .WillOnce(DoAll(InvokeWithoutArgs([&cv, &ready] { - ready = true; - cv.notify_one(); - }), + .WillOnce(DoAll(InvokeWithoutArgs( + [&cv, &ready] + { + ready = true; + cv.notify_one(); + }), Return(play_ret_val))) .WillRepeatedly(Return(play_ret_val)); @@ -88,10 +90,12 @@ TEST(ThreadedEstopReaderTest, estop_state_changes_based_on_read_val) EXPECT_CALL(*mock_uart_ptr, serialRead(_)) .WillOnce(Return(play_ret_val)) - .WillOnce(DoAll(InvokeWithoutArgs([&cv, &ready] { - ready = true; - cv.notify_one(); - }), + .WillOnce(DoAll(InvokeWithoutArgs( + [&cv, &ready] + { + ready = true; + cv.notify_one(); + }), Return(play_ret_val))) .WillRepeatedly(Return(play_ret_val)); @@ -108,10 +112,12 @@ TEST(ThreadedEstopReaderTest, estop_state_changes_based_on_read_val) ready = false; EXPECT_CALL(*mock_uart_ptr, serialRead(_)) .WillOnce(Return(stop_ret_val)) - .WillOnce(DoAll(InvokeWithoutArgs([&cv, &ready] { - ready = true; - cv.notify_one(); - }), + .WillOnce(DoAll(InvokeWithoutArgs( + [&cv, &ready] + { + ready = true; + cv.notify_one(); + }), Return(stop_ret_val))) .WillRepeatedly(Return(stop_ret_val)); EXPECT_TRUE(cv.wait_until( @@ -124,10 +130,12 @@ TEST(ThreadedEstopReaderTest, estop_state_changes_based_on_read_val) ready = false; EXPECT_CALL(*mock_uart_ptr, serialRead(_)) .WillOnce(Return(play_ret_val)) - .WillOnce(DoAll(InvokeWithoutArgs([&cv, &ready] { - ready = true; - cv.notify_one(); - }), + .WillOnce(DoAll(InvokeWithoutArgs( + [&cv, &ready] + { + ready = true; + cv.notify_one(); + }), Return(play_ret_val))) .WillRepeatedly(Return(play_ret_val)); EXPECT_TRUE(cv.wait_until( @@ -159,10 +167,12 @@ TEST(ThreadedEstopReaderTest, estop_play_is_false_after_reading_unexpected_messa .Times(AtLeast(3)) .WillOnce(Return(play_ret_val)) .WillOnce(Return(garbage_ret_val)) - .WillOnce(DoAll(InvokeWithoutArgs([&cv, &ready] { - ready = true; - cv.notify_one(); - }), + .WillOnce(DoAll(InvokeWithoutArgs( + [&cv, &ready] + { + ready = true; + cv.notify_one(); + }), Return(garbage_ret_val))) .WillOnce(Return(garbage_ret_val)) .WillOnce(Return(garbage_ret_val)) @@ -206,10 +216,12 @@ TEST(ThreadedEstopReaderTest, .WillOnce(Return(garbage_ret_val)) .WillOnce(Return(garbage_ret_val)) .WillOnce(Return(play_ret_val)) - .WillRepeatedly(DoAll(InvokeWithoutArgs([&cv, &ready] { - ready = true; - cv.notify_one(); - }), + .WillRepeatedly(DoAll(InvokeWithoutArgs( + [&cv, &ready] + { + ready = true; + cv.notify_one(); + }), Return(play_ret_val))); int startup_interval_ms = 5; From f84a9455287efdaf32e4b63cd23d56aed3fd291f Mon Sep 17 00:00:00 2001 From: Arun Date: Sat, 1 Mar 2025 14:45:51 -0800 Subject: [PATCH 08/71] Update clang-format to work on ubuntu 24 --- environment_setup/util.sh | 29 +++++--- scripts/lint_and_format.sh | 2 +- .../src/amun/simulator/simrobot.cpp | 6 +- .../src/amun/simulator/simulator.cpp | 9 ++- .../message_translation/ssl_detection.cpp | 4 +- src/proto/message_translation/ssl_wrapper.cpp | 5 +- .../message_translation/tbots_geometry.cpp | 5 +- .../message_translation/tbots_protobuf.cpp | 5 +- src/software/ai/ai.cpp | 20 +++--- src/software/ai/evaluation/calc_best_shot.cpp | 6 +- .../ai/evaluation/calc_best_shot_test.cpp | 30 ++++---- .../ai/evaluation/defender_assignment.cpp | 13 ++-- .../deflect_off_enemy_target_test.cpp | 2 +- src/software/ai/evaluation/enemy_threat.cpp | 9 ++- src/software/ai/evaluation/find_passes.cpp | 10 ++- .../ai/evaluation/find_passes_test.cpp | 6 +- src/software/ai/evaluation/intercept.cpp | 3 +- src/software/ai/evaluation/keep_away.cpp | 13 ++-- .../crease_defense_play_test.cpp | 3 +- .../ai/hl/stp/play/example_play_test.cpp | 37 +++++----- .../stp/play/free_kick/free_kick_play_fsm.cpp | 3 +- .../play/free_kick/free_kick_play_test.cpp | 6 +- .../dribbling_parcour_play_test.cpp | 6 +- .../pass_endurance_play_test.cpp | 6 +- ...ng_from_contested_possession_play_test.cpp | 6 +- ...coring_with_static_defenders_play_test.cpp | 6 +- .../hl/stp/play/kickoff_enemy_play_test.cpp | 3 +- .../stp/play/kickoff_friendly_play_test.cpp | 6 +- .../hl/stp/play/offense/offense_play_fsm.cpp | 6 +- .../penalty_kick/penalty_kick_play_test.cpp | 24 +++---- .../penalty_kick_enemy_play_test.cpp | 11 +-- src/software/ai/hl/stp/play/play.cpp | 16 ++--- .../hl/stp/play/shoot_or_chip_play_test.cpp | 3 +- .../ai/hl/stp/play/stop_play_test.cpp | 3 +- .../attacker_tactic_keep_away_test.cpp | 17 +++-- .../attacker/attacker_tactic_passing_test.cpp | 3 +- .../attacker_tactic_shoot_goal_test.cpp | 4 +- .../hl/stp/tactic/chip/chip_tactic_test.cpp | 3 +- .../crease_defender/crease_defender_fsm.cpp | 7 +- .../crease_defender_tactic_test.cpp | 11 +-- .../dribble_tactic_push_enemy_test.cpp | 5 +- .../tactic/dribble/dribble_tactic_test.cpp | 53 ++++++++------ .../hl/stp/tactic/halt/halt_tactic_test.cpp | 6 +- .../hl/stp/tactic/kick/kick_tactic_test.cpp | 3 +- .../hl/stp/tactic/move/move_tactic_test.cpp | 19 +++-- .../penalty_kick/penalty_kick_fsm_test.cpp | 4 +- .../penalty_kick/penalty_kick_tactic_test.cpp | 20 +++--- .../pivot_kick/pivot_kick_tactic_test.cpp | 3 +- .../tactic/receiver/receiver_tactic_test.cpp | 6 +- .../shadow_enemy/shadow_enemy_fsm_test.cpp | 8 +-- .../shadow_enemy/shadow_enemy_tactic_test.cpp | 17 +++-- src/software/ai/hl/stp/tactic/tactic.cpp | 12 ++-- .../trajectory/trajectory_path_node.h | 4 +- src/software/ai/passing/cost_function.cpp | 5 +- .../passing/eighteen_zone_pitch_division.cpp | 5 +- src/software/ai/passing/pass.cpp | 2 +- src/software/ai/passing/pass_generator.cpp | 17 ++--- .../passing/receiver_position_generator.hpp | 15 ++-- .../backend/unix_simulator_backend.cpp | 5 +- src/software/embedded/gpio_char_dev.h | 5 +- src/software/embedded/primitive_executor.cpp | 2 +- src/software/embedded/redis/redis_client.cpp | 13 ++-- .../embedded/redis/redis_client_test.cpp | 55 ++++++++------- src/software/embedded/services/motor.cpp | 5 +- src/software/er_force_simulator_main.cpp | 24 +++++-- .../estop/threaded_estop_reader_test.cpp | 4 +- .../end_in_obstacle_sample_test.cpp | 2 +- .../geom/algorithms/find_open_circles.cpp | 12 ++-- .../algorithms/find_open_circles_test.cpp | 2 +- .../geom/algorithms/furthest_point.cpp | 5 +- .../geom/algorithms/rasterize_test.cpp | 5 +- .../geom/algorithms/step_along_perimeter.cpp | 5 +- src/software/geom/angle_map.cpp | 2 +- src/software/geom/linear_spline2d.cpp | 12 ++-- src/software/geom/polygon.cpp | 6 +- src/software/logger/proto_logger.h | 2 +- src/software/math/statistics_functions.hpp | 5 +- .../first_in_first_out_threaded_observer.h | 2 +- .../last_in_first_out_threaded_observer.h | 2 +- src/software/multithreading/observer_test.cpp | 12 ++-- .../multithreading/thread_safe_buffer.hpp | 4 +- .../thread_safe_buffer_test.cpp | 38 +++++----- src/software/network_log_listener_main.cpp | 3 +- .../latency_tester_node.cpp | 2 +- .../gradient_descent_optimizer.hpp | 12 ++-- .../optimization/gradient_descent_test.cpp | 10 ++- src/software/py_constants.cpp | 18 ++--- src/software/python_bindings.cpp | 66 ++++++++++------- .../sensor_fusion/filter/ball_filter.cpp | 2 +- .../filter/ball_occlusion_test.cpp | 3 +- .../possession/possession_tracker.cpp | 5 +- src/software/sensor_fusion/sensor_fusion.cpp | 4 +- ...t_not_excessively_dribbling_validation.cpp | 3 +- .../robot_in_center_circle_validation.cpp | 14 ++-- .../robot_in_polygon_validation.cpp | 3 +- .../robot_received_ball_validation.cpp | 13 ++-- .../robot_state_validation.cpp | 9 ++- ...on_terminating_function_validator_test.cpp | 10 +-- .../terminating_function_validator_test.cpp | 37 +++++----- .../test_util/equal_within_tolerance.cpp | 5 +- src/software/uart/mock_uart_communication.h | 4 +- src/software/unix_full_system_main.cpp | 12 ++-- .../util/generic_factory/generic_factory.h | 5 +- src/software/util/sml_fsm/sml_fsm.h | 70 ++++++++++--------- src/software/world/world.cpp | 5 +- 105 files changed, 617 insertions(+), 498 deletions(-) diff --git a/environment_setup/util.sh b/environment_setup/util.sh index d06e8b7df6..72012e9ec4 100755 --- a/environment_setup/util.sh +++ b/environment_setup/util.sh @@ -1,10 +1,11 @@ install_autoref() { autoref_commit=b30660b78728c3ce159de8ae096181a1ec52e9ba - sudo wget -N https://github.com/TIGERs-Mannheim/AutoReferee/archive/${autoref_commit}.zip -O /tmp/tbots_download_cache/autoReferee.zip + wget -N https://github.com/TIGERs-Mannheim/AutoReferee/archive/${autoref_commit}.zip -O /tmp/tbots_download_cache/autoReferee.zip unzip -q -o -d /tmp/tbots_download_cache/ /tmp/tbots_download_cache/autoReferee.zip /tmp/tbots_download_cache/AutoReferee-${autoref_commit}/./gradlew installDist -p /tmp/tbots_download_cache/AutoReferee-${autoref_commit} -Dorg.gradle.java.home=/opt/tbotspython/bin/jdk mv /tmp/tbots_download_cache/AutoReferee-${autoref_commit}/build/install/autoReferee /opt/tbotspython/ + rm -rf /tmp/tbots_download_cache/autoReferee.zip /tmp/tbots_download_cache/AutoReferee-${autoref_commit} } install_bazel() { @@ -20,13 +21,23 @@ install_bazel() { } install_clang_format() { - arch=$1 - clang_format_file=clang+llvm-18.1.8-$arch-linux-gnu-ubuntu-18.04 - download=https://github.com/llvm/llvm-project/releases/download/llvmorg-18.1.8/$clang-format-file.tar.xz - clang_format_path=/tmp/tbots_download_cache/$clang_format_file/bin/clang-format + download=https://github.com/llvm/llvm-project/releases/download/llvmorg-19.1.7/clang+llvm-19.1.7-aarch64-linux-gnu.tar.xz + clang_folder=clang+llvm-19.1.7-aarch64-linux-gnu + + if is_x86 $1; then + download=https://github.com/llvm/llvm-project/releases/download/llvmorg-19.1.7/LLVM-19.1.7-Linux-X64.tar.xz + clang_folder=LLVM-19.1.7-Linux-X64 + fi + wget $download -O /tmp/tbots_download_cache/clang.tar.xz - tar -xf /tmp/tbots_download_cache/clang.tar.xz -C /tmp/tbots_download_cache/ + + # Temporarily need more space to extract the clang tarball + mkdir -p ~/.tbots + tar -xf /tmp/tbots_download_cache/clang.tar.xz -C ~/.tbots/ + + clang_format_path=~/.tbots/$clang_folder/bin/clang-format sudo cp $clang_format_path /opt/tbotspython/bin/clang-format + rm -rf ~/.tbots } install_cross_compiler() { @@ -69,8 +80,7 @@ install_gamecontroller () { sudo chmod +x /opt/tbotspython/gamecontroller cd - - sudo rm -rf /tmp/tbots_download_cache/ssl-game-controller-3.12.3 - sudo rm -rf /tmp/tbots_download_cache/go + sudo rm -rf /tmp/tbots_download_cache/ssl-game-controller-3.12.3 /tmp/tbots_download_cache/go /tmp/tbots_download_cache/go.tar.gz /tmp/tbots_download_cache/ssl-game-controller.zip } install_java () { @@ -79,9 +89,10 @@ install_java () { if is_x86 $1; then java_download=https://download.oracle.com/java/21/latest/jdk-21_linux-x64_bin.tar.gz fi - sudo wget -N $java_download -O /tmp/tbots_download_cache/jdk-21.tar.gz + wget -N $java_download -O /tmp/tbots_download_cache/jdk-21.tar.gz tar -xzf /tmp/tbots_download_cache/jdk-21.tar.gz -C /opt/tbotspython/ mv /opt/tbotspython/jdk-21* /opt/tbotspython/bin/jdk + rm /tmp/tbots_download_cache/jdk-21.tar.gz } is_x86() { diff --git a/scripts/lint_and_format.sh b/scripts/lint_and_format.sh index 22b358d1e6..2f98267501 100755 --- a/scripts/lint_and_format.sh +++ b/scripts/lint_and_format.sh @@ -36,7 +36,7 @@ function run_clang_format () { # clang-format as arguments # We remove the last -o flag from the extension string find $CURR_DIR/../src/ ${EXTENSION_STRING::-2} \ - | xargs -I{} -n1000 $CLANG_BIN -i -style=file + | xargs -I{} -n1000 $CLANG_BIN -i -style=file:$CURR_DIR/../.clang-format if [[ "$?" != 0 ]]; then printf "\n***Failed to run clang-format over all files!***\n\n" diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp index 571dfd38e9..9fe0de3f0f 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp @@ -235,7 +235,8 @@ void SimRobot::begin(SimBall &ball, double time) stopDribbling(); } - auto sendPartialCoordError = [this](const std::string &msg) { + auto sendPartialCoordError = [this](const std::string &msg) + { std::cerr << "Partial coordinates are not implemented yet" << msg << std::endl; if (!m_move.has_by_force() || !m_move.by_force()) { @@ -428,7 +429,8 @@ void SimRobot::begin(SimBall &ball, double time) power = std::clamp(m_sslCommand.kick_speed(), 0.05f, maxShootSpeed); } - const auto getSpeedCompensation = [&]() -> float { + const auto getSpeedCompensation = [&]() -> float + { if (m_sslCommand.kick_angle() == 0) { return 0.0f; diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp index a37333174e..2c00c5a798 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp @@ -45,7 +45,8 @@ Simulator::Simulator(const amun::SimulatorSetup &setup) m_data->solver.get(), m_data->collision.get()); m_data->dynamicsWorld->setGravity(btVector3(0.0f, 0.0f, -9.81f * SIMULATOR_SCALE)); m_data->dynamicsWorld->setInternalTickCallback( - [](btDynamicsWorld *world, btScalar timeStep) { + [](btDynamicsWorld *world, btScalar timeStep) + { Simulator *sim = reinterpret_cast(world->getWorldUserInfo()); sim->handleSimulatorTick(timeStep); }, @@ -170,7 +171,8 @@ void Simulator::handleSimulatorTick(double time_s) } // find out if ball and any robot collide - auto robot_ball_collision = [&](const auto &kv_pair) { + auto robot_ball_collision = [&](const auto &kv_pair) + { auto &[robotId, robot] = kv_pair; return robot->touchesBall(*m_data->ball); }; @@ -723,7 +725,8 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr { m_data->ball->restoreState(sim.set_simulator_state().ball()); } - const auto restoreRobots = [](RobotMap &map, auto robots) { + const auto restoreRobots = [](RobotMap &map, auto robots) + { for (const auto &robot : robots) { if (map.contains(robot.id())) diff --git a/src/proto/message_translation/ssl_detection.cpp b/src/proto/message_translation/ssl_detection.cpp index fc9f4d9d0b..4a72f315ca 100644 --- a/src/proto/message_translation/ssl_detection.cpp +++ b/src/proto/message_translation/ssl_detection.cpp @@ -98,7 +98,7 @@ std::vector createBallDetections( // Convert all data to meters and radians BallDetection ball_detection{ .position = Point(ball.x() * METERS_PER_MILLIMETER, - ball.y() * METERS_PER_MILLIMETER), + ball.y() * METERS_PER_MILLIMETER), .distance_from_ground = ball.z() * METERS_PER_MILLIMETER, .timestamp = Timestamp::fromSeconds(detection.t_capture()), .confidence = ball.confidence()}; @@ -136,7 +136,7 @@ std::vector createTeamDetection( RobotDetection robot_detection{ .id = ssl_robot_detection.robot_id(), .position = Point(ssl_robot_detection.x() * METERS_PER_MILLIMETER, - ssl_robot_detection.y() * METERS_PER_MILLIMETER), + ssl_robot_detection.y() * METERS_PER_MILLIMETER), .orientation = Angle::fromRadians(ssl_robot_detection.orientation()), .confidence = ssl_robot_detection.confidence(), .timestamp = Timestamp::fromSeconds(detection.t_capture())}; diff --git a/src/proto/message_translation/ssl_wrapper.cpp b/src/proto/message_translation/ssl_wrapper.cpp index cb4eeb2448..8ba95ed4da 100644 --- a/src/proto/message_translation/ssl_wrapper.cpp +++ b/src/proto/message_translation/ssl_wrapper.cpp @@ -24,9 +24,8 @@ std::unique_ptr createSSLWrapperPacket( std::unique_ptr createSSLWrapperPacket( const World& world, TeamColour friendly_team_colour) { - constexpr auto robot_to_robotstate_with_id_fn = [](const Robot& robot) { - return RobotStateWithId{.id = robot.id(), .robot_state = robot.currentState()}; - }; + constexpr auto robot_to_robotstate_with_id_fn = [](const Robot& robot) + { return RobotStateWithId{.id = robot.id(), .robot_state = robot.currentState()}; }; std::vector friendly_robot_states; friendly_robot_states.reserve(world.friendlyTeam().numRobots()); diff --git a/src/proto/message_translation/tbots_geometry.cpp b/src/proto/message_translation/tbots_geometry.cpp index 4929b296a2..1c35cef8e2 100644 --- a/src/proto/message_translation/tbots_geometry.cpp +++ b/src/proto/message_translation/tbots_geometry.cpp @@ -36,9 +36,8 @@ std::unique_ptr createPolygonProto(const Polygon& polygon) auto polygon_msg = std::make_unique(); const auto& points = polygon.getPoints(); - std::for_each(points.begin(), points.end(), [&](const Point& point) { - *(polygon_msg->add_points()) = *createPointProto(point); - }); + std::for_each(points.begin(), points.end(), [&](const Point& point) + { *(polygon_msg->add_points()) = *createPointProto(point); }); return polygon_msg; } diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 91e011c3f8..c4e236de8f 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -49,9 +49,8 @@ std::unique_ptr createTeam(const Team& team) auto team_msg = std::make_unique(); const auto& robots = team.getAllRobots(); - std::for_each(robots.begin(), robots.end(), [&](const Robot& robot) { - *(team_msg->add_team_robots()) = *createRobot(robot); - }); + std::for_each(robots.begin(), robots.end(), [&](const Robot& robot) + { *(team_msg->add_team_robots()) = *createRobot(robot); }); auto goalie_id = team.getGoalieId(); if (goalie_id.has_value()) diff --git a/src/software/ai/ai.cpp b/src/software/ai/ai.cpp index 37c2aac1c6..8233ba851c 100644 --- a/src/software/ai/ai.cpp +++ b/src/software/ai/ai.cpp @@ -71,24 +71,22 @@ std::unique_ptr Ai::getPrimitives(const WorldPtr& worl checkAiConfig(); - fsm->process_event(PlaySelectionFSM::Update( - [this](std::unique_ptr play) { current_play = std::move(play); }, - world_ptr->gameState(), ai_config_)); + fsm->process_event(PlaySelectionFSM::Update([this](std::unique_ptr play) + { current_play = std::move(play); }, + world_ptr->gameState(), ai_config_)); std::unique_ptr primitive_set; if (static_cast(override_play)) { - primitive_set = override_play->get(world_ptr, inter_play_communication, - [this](InterPlayCommunication comm) { - inter_play_communication = std::move(comm); - }); + primitive_set = override_play->get( + world_ptr, inter_play_communication, [this](InterPlayCommunication comm) + { inter_play_communication = std::move(comm); }); } else { - primitive_set = current_play->get(world_ptr, inter_play_communication, - [this](InterPlayCommunication comm) { - inter_play_communication = std::move(comm); - }); + primitive_set = current_play->get( + world_ptr, inter_play_communication, [this](InterPlayCommunication comm) + { inter_play_communication = std::move(comm); }); } FrameMarkEnd(TracyConstants::AI_FRAME_MARKER); diff --git a/src/software/ai/evaluation/calc_best_shot.cpp b/src/software/ai/evaluation/calc_best_shot.cpp index 118546c281..28036db17f 100644 --- a/src/software/ai/evaluation/calc_best_shot.cpp +++ b/src/software/ai/evaluation/calc_best_shot.cpp @@ -82,9 +82,9 @@ std::optional calcBestShotOnGoal(const Segment &goal_post, const Point &sh } Point top_point = Point(goal_post.getStart().x(), - (top_angle.sin() / top_angle.cos()) * - (goal_post.getStart().x() - shot_origin.x()) + - shot_origin.y()); + (top_angle.sin() / top_angle.cos()) * + (goal_post.getStart().x() - shot_origin.x()) + + shot_origin.y()); Point bottom_point = Point(goal_post.getStart().x(), (bottom_angle.sin() / bottom_angle.cos()) * (goal_post.getStart().x() - shot_origin.x()) + diff --git a/src/software/ai/evaluation/calc_best_shot_test.cpp b/src/software/ai/evaluation/calc_best_shot_test.cpp index 3df846a792..2e3d77f983 100644 --- a/src/software/ai/evaluation/calc_best_shot_test.cpp +++ b/src/software/ai/evaluation/calc_best_shot_test.cpp @@ -10,7 +10,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_enemy_goal_with_no_obstacles) std::shared_ptr world = ::TestUtil::createBlankTestingWorld(); Team team = Team(Duration::fromSeconds(1)); Robot shooting_robot = Robot(0, Point(0, 0), Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); @@ -31,7 +31,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_friendly_goal_with_no_obstacles) std::shared_ptr world = ::TestUtil::createBlankTestingWorld(); Team team = Team(Duration::fromSeconds(1)); Robot shooting_robot = Robot(0, Point(0, 0), Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); @@ -218,7 +218,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_friendly_goal_with_best_shot_top_segmen Point shooting_robot_pos = Point(-3.641, 0.113); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateEnemyTeamState(team); @@ -247,7 +247,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_enemy_goal_with_best_shot_top_segment) Point shooting_robot_pos = Point(3.641, 0.113); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); @@ -276,7 +276,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_friendly_goal_with_best_shot_middle_seg Point shooting_robot_pos = Point(-3.641, 0.113); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateEnemyTeamState(team); @@ -305,7 +305,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_enemy_goal_with_best_shot_middle_segmen Point shooting_robot_pos = Point(3.641, 0.113); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); @@ -334,7 +334,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_friendly_goal_with_best_shot_bottom_seg Point shooting_robot_pos = Point(-3.641, 0.113); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateEnemyTeamState(team); @@ -362,7 +362,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_enemy_goal_with_best_shot_bottom_segmen Point shooting_robot_pos = Point(3.641, 0.113); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); @@ -390,7 +390,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_friendly_goal_with_overlapping_obstacle Point shooting_robot_pos = Point(-3.44615, 0.0102564); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateEnemyTeamState(team); @@ -419,7 +419,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_enemy_goal_with_overlapping_obstacles) Point shooting_robot_pos = Point(3.44615, 0.0102564); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); @@ -448,7 +448,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_friendly_goal_with_negative_y_ball_plac Point shooting_robot_pos = Point(-3.50769, -1.00513); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateEnemyTeamState(team); @@ -476,7 +476,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_enemy_goal_with_negative_y_ball_placeme Point shooting_robot_pos = Point(3.50769, -1.00513); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); @@ -504,7 +504,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_friendly_goal_with_positive_y_ball_plac Point shooting_robot_pos = Point(-3.49744, 0.984615); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateEnemyTeamState(team); @@ -532,7 +532,7 @@ TEST(CalcBestShotTest, calc_best_shot_on_enemy_goal_with_positive_y_ball_placeme Point shooting_robot_pos = Point(3.49744, 0.984615); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); @@ -560,7 +560,7 @@ TEST(CalcBestShotTest, calc_best_shot_out_of_field_bounds) Point shooting_robot_pos = Point(world->field().enemyCornerPos().x() + 2, world->field().enemyCornerPos().y()); Robot shooting_robot = Robot(0, shooting_robot_pos, Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); team.updateRobots({shooting_robot}); world->updateFriendlyTeamState(team); diff --git a/src/software/ai/evaluation/defender_assignment.cpp b/src/software/ai/evaluation/defender_assignment.cpp index b2b7ad0c10..ba444ae472 100644 --- a/src/software/ai/evaluation/defender_assignment.cpp +++ b/src/software/ai/evaluation/defender_assignment.cpp @@ -103,16 +103,16 @@ std::vector getAllDefenderAssignments( // Remove pass defender assignments with targets in the defense area assignments.erase( std::remove_if(assignments.begin(), assignments.end(), - [&field](const auto &assignment) { + [&field](const auto &assignment) + { return assignment.type == PASS_DEFENDER && field.pointInFriendlyDefenseArea(assignment.target); }), assignments.end()); // Sort the potential assignments by coverage rating in descending order - std::sort(assignments.begin(), assignments.end(), [](const auto &a, const auto &b) { - return a.coverage_rating > b.coverage_rating; - }); + std::sort(assignments.begin(), assignments.end(), [](const auto &a, const auto &b) + { return a.coverage_rating > b.coverage_rating; }); return assignments; } @@ -179,9 +179,8 @@ std::vector> groupGoalLanesByDensity( } // Sort goal lanes by angle to the goal in increasing order - std::sort(goal_lanes.begin(), goal_lanes.end(), [](const auto &a, const auto &b) { - return a.angle_to_goal < b.angle_to_goal; - }); + std::sort(goal_lanes.begin(), goal_lanes.end(), [](const auto &a, const auto &b) + { return a.angle_to_goal < b.angle_to_goal; }); std::vector> groups; diff --git a/src/software/ai/evaluation/deflect_off_enemy_target_test.cpp b/src/software/ai/evaluation/deflect_off_enemy_target_test.cpp index 9b5346daab..282b33c538 100644 --- a/src/software/ai/evaluation/deflect_off_enemy_target_test.cpp +++ b/src/software/ai/evaluation/deflect_off_enemy_target_test.cpp @@ -10,7 +10,7 @@ TEST(DeflectOffEnemyTargetTest, deflectOffEnemyTarget_test) Robot friendly_robot = Robot(0, Point(0, 0), Vector(0, 0), Angle::zero(), AngularVelocity::zero(), Timestamp::fromMilliseconds(0)); Robot enemy_robot = Robot(0, Point(-3, 0), Vector(0, 0), Angle::zero(), - AngularVelocity::zero(), Timestamp::fromMilliseconds(0)); + AngularVelocity::zero(), Timestamp::fromMilliseconds(0)); std::vector friendly_robots; std::vector enemy_robots; friendly_robots.emplace_back(friendly_robot); diff --git a/src/software/ai/evaluation/enemy_threat.cpp b/src/software/ai/evaluation/enemy_threat.cpp index ea35a26854..e0805bf2c6 100644 --- a/src/software/ai/evaluation/enemy_threat.cpp +++ b/src/software/ai/evaluation/enemy_threat.cpp @@ -35,7 +35,8 @@ std::map, Robot::cmpRobotByID> findAllReceiverPasserPa // robots bool pass_blocked = std::any_of(obstacles.begin(), obstacles.end(), - [passer, receiver](const Robot &obstacle) { + [passer, receiver](const Robot &obstacle) + { return intersects( Circle(obstacle.position(), ROBOT_MAX_RADIUS_METERS), Segment(passer.position(), receiver.position())); @@ -146,7 +147,8 @@ std::optional>> getNumPassesToRobot( // Remove any receivers from the unvisited robots, since they have // now been visited unvisited_robots.erase(remove_if(unvisited_robots.begin(), unvisited_robots.end(), - [&](auto x) { + [&](auto x) + { return find(all_receivers.begin(), all_receivers.end(), x) != all_receivers.end(); @@ -164,7 +166,8 @@ void sortThreatsInDecreasingOrder(std::vector &threats) { // A lambda function that implements the '<' operator for the EnemyThreat struct // so it can be sorted. Lower threats are "less than" higher threats. - auto enemyThreatLessThanComparator = [](const EnemyThreat &a, const EnemyThreat &b) { + auto enemyThreatLessThanComparator = [](const EnemyThreat &a, const EnemyThreat &b) + { // Robots with the ball are more threatening than robots without the ball, and // robots with the ball are the most threatening since they can shoot or move // the ball towards our net diff --git a/src/software/ai/evaluation/find_passes.cpp b/src/software/ai/evaluation/find_passes.cpp index 28a8fc98aa..b4059d43d9 100644 --- a/src/software/ai/evaluation/find_passes.cpp +++ b/src/software/ai/evaluation/find_passes.cpp @@ -24,9 +24,8 @@ std::vector findOpenFriendlyRobots(const Team& friendly_team, for (const Robot& friendly : friendly_team.getAllRobots()) { if (std::all_of(obstacles.begin(), obstacles.end(), - [friendly](const Circle enemy_circle) { - return !contains(enemy_circle, friendly.position()); - })) + [friendly](const Circle enemy_circle) + { return !contains(enemy_circle, friendly.position()); })) { open_robots.push_back(friendly); } @@ -56,9 +55,8 @@ AllPasses findAllPasses(const Robot& robot, const Team& friendly_team, obstacles.erase(remove(obstacles.begin(), obstacles.end(), obstacle)); Segment possible_pass = Segment(robot.position(), open_robot.position()); if (std::any_of(obstacles.begin(), obstacles.end(), - [possible_pass](const Circle obstacle) { - return intersects(possible_pass, obstacle); - })) + [possible_pass](const Circle obstacle) + { return intersects(possible_pass, obstacle); })) { indirect_passes.push_back(open_robot); } diff --git a/src/software/ai/evaluation/find_passes_test.cpp b/src/software/ai/evaluation/find_passes_test.cpp index 62c58eb1c7..a0efc82316 100644 --- a/src/software/ai/evaluation/find_passes_test.cpp +++ b/src/software/ai/evaluation/find_passes_test.cpp @@ -30,7 +30,7 @@ TEST(FindPasses, find_all_passes_with_no_enemies) AllPasses all_passes = findAllPasses(initial_robot, friendly_team, enemy_team); std::vector expected_direct_passes = {friendly_robot_1, friendly_robot_2, - friendly_robot_3, friendly_robot_4}; + friendly_robot_3, friendly_robot_4}; std::vector expected_indirect_passes = {}; EXPECT_EQ(expected_direct_passes, all_passes.direct_passes); EXPECT_EQ(expected_indirect_passes, all_passes.indirect_passes); @@ -66,7 +66,7 @@ TEST(FindPasses, find_all_passes_with_one_enemy) AllPasses all_passes = findAllPasses(initial_robot, friendly_team, enemy_team); std::vector expected_direct_passes = {friendly_robot_2, friendly_robot_3, - friendly_robot_4}; + friendly_robot_4}; std::vector expected_indirect_passes = {friendly_robot_1}; EXPECT_EQ(expected_direct_passes, all_passes.direct_passes); EXPECT_EQ(expected_indirect_passes, all_passes.indirect_passes); @@ -103,7 +103,7 @@ TEST(FindPasses, find_all_passes_with_one_enemy_not_directly_in_path) std::vector expected_direct_passes = {friendly_robot_2, friendly_robot_3, - friendly_robot_4}; + friendly_robot_4}; std::vector expected_indirect_passes = {friendly_robot_1}; EXPECT_EQ(expected_direct_passes, all_passes.direct_passes); EXPECT_EQ(expected_indirect_passes, all_passes.indirect_passes); diff --git a/src/software/ai/evaluation/intercept.cpp b/src/software/ai/evaluation/intercept.cpp index 98373518f2..5a86740364 100644 --- a/src/software/ai/evaluation/intercept.cpp +++ b/src/software/ai/evaluation/intercept.cpp @@ -17,7 +17,8 @@ std::optional> findBestInterceptForBall(const Ball &b // This is the objective function that we want to minimize, finding the // shortest duration in the future at which we can feasibly intercept the // ball - auto objective_function = [&](std::array x) { + auto objective_function = [&](std::array x) + { // We take the absolute value here because a negative time makes no sense double duration = std::abs(x.at(0)); diff --git a/src/software/ai/evaluation/keep_away.cpp b/src/software/ai/evaluation/keep_away.cpp index d36f342e88..aefa88770c 100644 --- a/src/software/ai/evaluation/keep_away.cpp +++ b/src/software/ai/evaluation/keep_away.cpp @@ -22,13 +22,14 @@ Point findKeepAwayTargetPoint(const World& world, const Pass& best_pass_so_far, // boundaries const auto& field_bounds = world.field().fieldLines(); Point reduced_bottom_left = Point(field_bounds.xMin() + FIELD_SIZE_REDUCTION_M, - field_bounds.yMin() + FIELD_SIZE_REDUCTION_M); + field_bounds.yMin() + FIELD_SIZE_REDUCTION_M); Point reduced_top_right = Point(field_bounds.xMax() - FIELD_SIZE_REDUCTION_M, - field_bounds.yMax() - FIELD_SIZE_REDUCTION_M); + field_bounds.yMax() - FIELD_SIZE_REDUCTION_M); Rectangle reduced_field_bounds = Rectangle(reduced_bottom_left, reduced_top_right); // the position rating function we want to maximize - const auto keepaway_point_cost = [&](const std::array& passer_pt_array) { + const auto keepaway_point_cost = [&](const std::array& passer_pt_array) + { Point passer_pt(std::get<0>(passer_pt_array), std::get<1>(passer_pt_array)); return rateKeepAwayPosition(passer_pt, world, best_pass_so_far, reduced_field_bounds, passing_config); @@ -50,7 +51,8 @@ Point findKeepAwayTargetPoint(const World& world, const Pass& best_pass_so_far, field_line_segments.size()); std::transform(field_line_segments.begin(), field_line_segments.end(), closest_points_and_distances.begin(), - [keepaway_target_point](const Segment& seg) { + [keepaway_target_point](const Segment& seg) + { auto closest_point = closestPoint(keepaway_target_point, seg); return std::make_pair( closest_point, @@ -58,7 +60,8 @@ Point findKeepAwayTargetPoint(const World& world, const Pass& best_pass_so_far, }); const auto& closest_field_line_seg_and_dist = *std::min_element( closest_points_and_distances.begin(), closest_points_and_distances.end(), - [](const auto& lhs_point_and_dist, const auto& rhs_point_and_dist) { + [](const auto& lhs_point_and_dist, const auto& rhs_point_and_dist) + { // compare the distances return lhs_point_and_dist.second < rhs_point_and_dist.second; }); diff --git a/src/software/ai/hl/stp/play/crease_defense/crease_defense_play_test.cpp b/src/software/ai/hl/stp/play/crease_defense/crease_defense_play_test.cpp index 70f7d050a3..771c2dceea 100644 --- a/src/software/ai/hl/stp/play/crease_defense/crease_defense_play_test.cpp +++ b/src/software/ai/hl/stp/play/crease_defense/crease_defense_play_test.cpp @@ -46,7 +46,8 @@ TEST_F(CreaseDefensePlayTest, test_defense_play) setRefereeCommand(RefereeCommand::STOP, RefereeCommand::STOP); std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // Wait for all robots to come to a halt robotHalt(world_ptr, yield); diff --git a/src/software/ai/hl/stp/play/example_play_test.cpp b/src/software/ai/hl/stp/play/example_play_test.cpp index 6916f08391..2d39958651 100644 --- a/src/software/ai/hl/stp/play/example_play_test.cpp +++ b/src/software/ai/hl/stp/play/example_play_test.cpp @@ -28,26 +28,27 @@ TEST_F(ExamplePlayTest, test_example_play) setRefereeCommand(RefereeCommand::FORCE_START, RefereeCommand::HALT); std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - auto friendly_robots_1_meter_from_ball = - [](std::shared_ptr world_ptr) { - Point ball_position = world_ptr->ball().position(); - for (const auto& robot : world_ptr->friendlyTeam().getAllRobots()) + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { + auto friendly_robots_1_meter_from_ball = [](std::shared_ptr world_ptr) + { + Point ball_position = world_ptr->ball().position(); + for (const auto& robot : world_ptr->friendlyTeam().getAllRobots()) + { + // Skips the robot assigned as goalie + if (robot.id() == 0) + { + continue; + } + double abs_error = + std::fabs((robot.position() - ball_position).length() - 1.0); + if (abs_error > 0.05) { - // Skips the robot assigned as goalie - if (robot.id() == 0) - { - continue; - } - double abs_error = - std::fabs((robot.position() - ball_position).length() - 1.0); - if (abs_error > 0.05) - { - return false; - } + return false; } - return true; - }; + } + return true; + }; while (!friendly_robots_1_meter_from_ball(world_ptr)) { diff --git a/src/software/ai/hl/stp/play/free_kick/free_kick_play_fsm.cpp b/src/software/ai/hl/stp/play/free_kick/free_kick_play_fsm.cpp index 5ec0b9bced..63ce0d0e6c 100644 --- a/src/software/ai/hl/stp/play/free_kick/free_kick_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/free_kick/free_kick_play_fsm.cpp @@ -102,7 +102,8 @@ void FreeKickPlayFSM::setReceiverAndDefenderTactics( { defense_play->updateTactics(PlayUpdate( event.common.world_ptr, num_defenders, - [&tactics_to_run](PriorityTacticVector new_tactics) { + [&tactics_to_run](PriorityTacticVector new_tactics) + { for (const auto &tactic_vector : new_tactics) { tactics_to_run.push_back(tactic_vector); diff --git a/src/software/ai/hl/stp/play/free_kick/free_kick_play_test.cpp b/src/software/ai/hl/stp/play/free_kick/free_kick_play_test.cpp index 7b62842e34..1466da0b79 100644 --- a/src/software/ai/hl/stp/play/free_kick/free_kick_play_test.cpp +++ b/src/software/ai/hl/stp/play/free_kick/free_kick_play_test.cpp @@ -38,7 +38,8 @@ TEST_F(FreeKickPlayTest, test_free_kick_play_on_enemy_half) // time to settle into position and be observed with the Visualizer // TODO: Implement proper validation // https://github.com/UBC-Thunderbots/Software/issues/1971 - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); @@ -72,7 +73,8 @@ TEST_F(FreeKickPlayTest, test_free_kick_play_on_friendly_half) // time to settle into position and be observed with the Visualizer // TODO: Implement proper validation // https://github.com/UBC-Thunderbots/Software/issues/1971 - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); diff --git a/src/software/ai/hl/stp/play/hardware_challenge_plays/dribbling_parcour_play_test.cpp b/src/software/ai/hl/stp/play/hardware_challenge_plays/dribbling_parcour_play_test.cpp index c42119af19..b3f7dd4d04 100644 --- a/src/software/ai/hl/stp/play/hardware_challenge_plays/dribbling_parcour_play_test.cpp +++ b/src/software/ai/hl/stp/play/hardware_challenge_plays/dribbling_parcour_play_test.cpp @@ -28,7 +28,8 @@ TEST_F(DribblingParcourPlayTest, test_dribbling_parcour_play_stopped) // This will keep the test running for 9.5 seconds to give everything enough // time to settle into position and be observed with the Visualizer // TODO (#2108): Implement proper validation - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); @@ -55,7 +56,8 @@ TEST_F(DribblingParcourPlayTest, test_dribbling_parcour_play_force_start) // This will keep the test running for 9.5 seconds to give everything enough // time to settle into position and be observed with the Visualizer // TODO (#2108): Implement proper validation - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); diff --git a/src/software/ai/hl/stp/play/hardware_challenge_plays/pass_endurance_play_test.cpp b/src/software/ai/hl/stp/play/hardware_challenge_plays/pass_endurance_play_test.cpp index c698058f21..a98d672df0 100644 --- a/src/software/ai/hl/stp/play/hardware_challenge_plays/pass_endurance_play_test.cpp +++ b/src/software/ai/hl/stp/play/hardware_challenge_plays/pass_endurance_play_test.cpp @@ -29,7 +29,8 @@ TEST_F(PassEndurancePlayTest, test_pass_endurance_play_stopped) // This will keep the test running for 9.5 seconds to give everything enough // time to settle into position and be observed with the Visualizer // TODO (#2109): Implement proper validation - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); @@ -56,7 +57,8 @@ TEST_F(PassEndurancePlayTest, test_pass_endurance_play_force_start) // This will keep the test running for 9.5 seconds to give everything enough // time to settle into position and be observed with the Visualizer // TODO (#2109): Implement proper validation - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); diff --git a/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_from_contested_possession_play_test.cpp b/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_from_contested_possession_play_test.cpp index 25209fab85..5fad2c970d 100644 --- a/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_from_contested_possession_play_test.cpp +++ b/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_from_contested_possession_play_test.cpp @@ -30,7 +30,8 @@ TEST_F(ScoringFromContestedPossessionPlayTest, // This will keep the test running for 9.5 seconds to give everything enough // time to settle into position and be observed with the Visualizer // TODO (#2107): Implement proper validation - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); @@ -58,7 +59,8 @@ TEST_F(ScoringFromContestedPossessionPlayTest, // This will keep the test running for 9.5 seconds to give everything enough // time to settle into position and be observed with the Visualizer // TODO (#2107): Implement proper validation - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); diff --git a/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_with_static_defenders_play_test.cpp b/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_with_static_defenders_play_test.cpp index 351d158c7e..67b7c202b9 100644 --- a/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_with_static_defenders_play_test.cpp +++ b/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_with_static_defenders_play_test.cpp @@ -30,7 +30,8 @@ TEST_F(ScoringWithStaticDefendersPlayTest, // This will keep the test running for 9.5 seconds to give everything enough // time to settle into position and be observed with the Visualizer // TODO (#2106): Implement proper validation - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); @@ -58,7 +59,8 @@ TEST_F(ScoringWithStaticDefendersPlayTest, // This will keep the test running for 9.5 seconds to give everything enough // time to settle into position and be observed with the Visualizer // TODO (#2106): Implement proper validation - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); diff --git a/src/software/ai/hl/stp/play/kickoff_enemy_play_test.cpp b/src/software/ai/hl/stp/play/kickoff_enemy_play_test.cpp index e56b357599..e9ca71ca24 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy_play_test.cpp +++ b/src/software/ai/hl/stp/play/kickoff_enemy_play_test.cpp @@ -37,7 +37,8 @@ TEST_F(KickoffEnemyPlayTest, DISABLED_test_kickoff_enemy_play) setRefereeCommand(RefereeCommand::NORMAL_START, RefereeCommand::PREPARE_KICKOFF_THEM); std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // Two friendly robots in position to shadow enemy robots. Rectangles are // chosen to be generally in the way of the the front 3 enemy robots and the // friendly goal, based on where the enemy robots are initialized in the test. diff --git a/src/software/ai/hl/stp/play/kickoff_friendly_play_test.cpp b/src/software/ai/hl/stp/play/kickoff_friendly_play_test.cpp index e18817382b..26b164fcdf 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly_play_test.cpp +++ b/src/software/ai/hl/stp/play/kickoff_friendly_play_test.cpp @@ -38,7 +38,8 @@ TEST_F(KickoffFriendlyPlayTest, DISABLED_test_kickoff_friendly_play) setRefereeCommand(RefereeCommand::NORMAL_START, RefereeCommand::PREPARE_KICKOFF_US); std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // Robot 4 is the only robot allowed to be in the center circle and start // the kickoff robotInCenterCircle(world_ptr, yield); @@ -56,7 +57,8 @@ TEST_F(KickoffFriendlyPlayTest, DISABLED_test_kickoff_friendly_play) }}; std::vector non_terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { for (RobotId robot_id : {0, 1, 2, 3, 5}) { { diff --git a/src/software/ai/hl/stp/play/offense/offense_play_fsm.cpp b/src/software/ai/hl/stp/play/offense/offense_play_fsm.cpp index fd310f475f..e1a9ad0b8b 100644 --- a/src/software/ai/hl/stp/play/offense/offense_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/offense/offense_play_fsm.cpp @@ -55,7 +55,8 @@ void OffensePlayFSM::setTactics(const Update& event, int num_shoot_or_pass, { shoot_or_pass_play->updateTactics(PlayUpdate( event.common.world_ptr, num_shoot_or_pass, - [&tactics_to_return](PriorityTacticVector new_tactics) { + [&tactics_to_return](PriorityTacticVector new_tactics) + { for (const auto& tactic_vector : new_tactics) { tactics_to_return.push_back(tactic_vector); @@ -71,7 +72,8 @@ void OffensePlayFSM::setTactics(const Update& event, int num_shoot_or_pass, { defense_play->updateTactics(PlayUpdate( event.common.world_ptr, num_defenders, - [&tactics_to_return](PriorityTacticVector new_tactics) { + [&tactics_to_return](PriorityTacticVector new_tactics) + { for (const auto& tactic_vector : new_tactics) { tactics_to_return.push_back(tactic_vector); diff --git a/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_test.cpp b/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_test.cpp index 2058dd3231..1b583279e1 100644 --- a/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_test.cpp +++ b/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_test.cpp @@ -38,13 +38,15 @@ TEST_F(PenaltyKickPlayTest, test_penalty_kick_setup) RobotId shooter_id = 5; std::vector terminating_validation_functions = { [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { robotAtPosition(shooter_id, world_ptr, world_ptr->field().friendlyPenaltyMark(), 0.3, yield); }}; std::vector non_terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // making sure that the robot doesn't move the ball while the penalty is // setting up ASSERT_TRUE( @@ -52,7 +54,8 @@ TEST_F(PenaltyKickPlayTest, test_penalty_kick_setup) world_ptr->ball().position(), 1e-6)); }, [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { // Wait 2 seconds for robots to start moving adequately far away from the ball if (world_ptr->getMostRecentTimestamp() >= Timestamp::fromSeconds(2)) { @@ -95,17 +98,14 @@ TEST_F(PenaltyKickPlayTest, DISABLED_test_penalty_kick_take) std::vector non_terminating_validation_functions = { ballInPlay, [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - ballNeverMovesBackward(world_ptr, yield); - }, + ValidationCoroutine::push_type& yield) + { ballNeverMovesBackward(world_ptr, yield); }, [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); - }, + ValidationCoroutine::push_type& yield) + { robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); }, [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - robotsAvoidBall(1, {shooter_id}, world_ptr, yield); - }}; + ValidationCoroutine::push_type& yield) + { robotsAvoidBall(1, {shooter_id}, world_ptr, yield); }}; runTest(field_type, ball_state, friendly_robots, enemy_robots, terminating_validation_functions, non_terminating_validation_functions, diff --git a/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_test.cpp b/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_test.cpp index 3eca1a8dee..2cf77cb6c1 100644 --- a/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_test.cpp +++ b/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_test.cpp @@ -57,7 +57,8 @@ TEST_P(PenaltyKickEnemyPlayTest, test_penalty_kick_enemy_play_setup) std::vector terminating_validation_functions = { [behind_ball_region](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { robotAtOrientation(0, world_ptr, Angle::zero(), Angle::fromDegrees(5), yield); robotAtPosition(0, world_ptr, world_ptr->field().friendlyGoalCenter(), 0.05, yield); @@ -137,7 +138,8 @@ TEST_F(PenaltyKickEnemyPlayTest, test_penalty_kick_enemy_play_goalie) std::vector terminating_validation_functions = { // This will keep the test running for 10 seconds to give everything enough // time to settle into position and be observed with the Visualizer - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(10)) { yield("simulated penalty kick goalie test not finished!"); @@ -145,9 +147,8 @@ TEST_F(PenaltyKickEnemyPlayTest, test_penalty_kick_enemy_play_goalie) }}; std::vector non_terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - enemyNeverScores(world_ptr, yield); - }}; + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { enemyNeverScores(world_ptr, yield); }}; runTest(field_type, ball_state, friendly_robots, enemy_robots, terminating_validation_functions, non_terminating_validation_functions, diff --git a/src/software/ai/hl/stp/play/play.cpp b/src/software/ai/hl/stp/play/play.cpp index dd317da133..d8931cb61b 100644 --- a/src/software/ai/hl/stp/play/play.cpp +++ b/src/software/ai/hl/stp/play/play.cpp @@ -98,11 +98,9 @@ std::unique_ptr Play::get( ZoneNamedN(_tracy_tactics, "Play: Get Tactics from Play", true); updateTactics(PlayUpdate( - world_ptr, num_tactics, - [&priority_tactics](PriorityTacticVector new_tactics) { - priority_tactics = std::move(new_tactics); - }, - inter_play_communication, set_inter_play_communication_fun)); + world_ptr, num_tactics, [&priority_tactics](PriorityTacticVector new_tactics) + { priority_tactics = std::move(new_tactics); }, inter_play_communication, + set_inter_play_communication_fun)); } auto primitives_to_run = std::make_unique(); @@ -385,10 +383,10 @@ Play::assignTactics(const WorldPtr &world_ptr, TacticVector tactic_vector, primitives_to_run->mutable_robot_primitives()->insert( {robot_id, *primitive_proto}); remaining_robots.erase( - std::remove_if(remaining_robots.begin(), remaining_robots.end(), - [robots_to_assign, row](const Robot &robot) { - return robot.id() == robots_to_assign.at(row).id(); - }), + std::remove_if( + remaining_robots.begin(), remaining_robots.end(), + [robots_to_assign, row](const Robot &robot) + { return robot.id() == robots_to_assign.at(row).id(); }), remaining_robots.end()); primitives[robot_id]->getVisualizationProtos(obstacle_list, diff --git a/src/software/ai/hl/stp/play/shoot_or_chip_play_test.cpp b/src/software/ai/hl/stp/play/shoot_or_chip_play_test.cpp index ea46f87aa2..c617194c6e 100644 --- a/src/software/ai/hl/stp/play/shoot_or_chip_play_test.cpp +++ b/src/software/ai/hl/stp/play/shoot_or_chip_play_test.cpp @@ -47,7 +47,8 @@ TEST_F(ShootOrChipPlayTest, test_shoot_or_chip_play) // time to settle into position and be observed with the Visualizer // TODO: Implement proper validation // https://github.com/UBC-Thunderbots/Software/issues/1971 - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); diff --git a/src/software/ai/hl/stp/play/stop_play_test.cpp b/src/software/ai/hl/stp/play/stop_play_test.cpp index 7b66aee1bc..f7272e2359 100644 --- a/src/software/ai/hl/stp/play/stop_play_test.cpp +++ b/src/software/ai/hl/stp/play/stop_play_test.cpp @@ -21,7 +21,8 @@ class StopPlayTest : public SimulatedErForceSimPlayTestFixture std::vector initStopPlayRules() { return { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // Wait 2 seconds for robots that start too close to the ball to move away if (world_ptr->getMostRecentTimestamp() >= Timestamp::fromSeconds(8)) { diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp index 9b24f28fce..a44f5d7a0e 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp @@ -57,7 +57,8 @@ TEST_P(AttackerTacticKeepAwayTest, attacker_test_keep_away) std::vector enemy_team_robots; std::transform(enemy_robots.begin(), enemy_robots.end(), std::back_inserter(enemy_team_robots), - [](const RobotStateWithId& robot_state) { + [](const RobotStateWithId& robot_state) + { return Robot(robot_state.id, robot_state.robot_state, Timestamp::fromSeconds(0)); }); @@ -70,7 +71,8 @@ TEST_P(AttackerTacticKeepAwayTest, attacker_test_keep_away) // test the proximity risk every CHECK_SCORE_INTERVAL time and make sure // it doesn't get substantially worse compared to the last check // and that it is an improvement compared to the starting state - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (ignore_score_checks) { yield(""); @@ -116,7 +118,8 @@ TEST_P(AttackerTacticKeepAwayTest, attacker_test_keep_away) // test the ratePassEnemyRisk every CHECK_SCORE_INTERVAL time and make sure // it doesn't get substantially worse compared to the last check // and that it is an improvement compared to the starting state - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (ignore_score_checks) { yield(""); @@ -163,10 +166,10 @@ TEST_P(AttackerTacticKeepAwayTest, attacker_test_keep_away) } } }, - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - robotNotExcessivelyDribbling(1, world_ptr, yield); - }, - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { robotNotExcessivelyDribbling(1, world_ptr, yield); }, + [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // test that the ball is always in the field boundaries if (!contains(world_ptr->field().fieldLines(), world_ptr->ball().position())) { diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp index bf5ce7ac8a..924421eec0 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp @@ -42,7 +42,8 @@ TEST_P(AttackerTacticKeepAwayTest, attacker_test_passing) std::vector terminating_validation_functions = { [pass, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { // We check if the robot reaches the desired orientation, at the // desired position before checking if the ball has been kicked. // diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_shoot_goal_test.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_shoot_goal_test.cpp index 5fc0ef8688..63b06c135f 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_shoot_goal_test.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_shoot_goal_test.cpp @@ -46,8 +46,8 @@ TEST_P(AttackerTacticShootGoalTest, attacker_test_shoot_goal) setTactic(0, tactic, {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA}); std::vector terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + [tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (!tactic->done()) { yield("Tactic not done"); diff --git a/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp b/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp index 316c36c3d6..2c3499349e 100644 --- a/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp @@ -40,7 +40,8 @@ TEST_P(ChipTacticTest, chip_test) std::vector terminating_validation_functions = { [angle_to_kick_at, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { while (!tactic->done()) { yield("Tactic did not complete!"); diff --git a/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_fsm.cpp b/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_fsm.cpp index 1a4239c715..c83753ecba 100644 --- a/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_fsm.cpp @@ -38,9 +38,8 @@ bool CreaseDefenderFSM::isAnyEnemyInZone(const Update& event, const Stadium& zon { std::vector enemy_robots = event.common.world_ptr->enemyTeam().getAllRobots(); return std::any_of(enemy_robots.begin(), enemy_robots.end(), - [zone, enemy_robots](const Robot& robot) { - return contains(zone, robot.position()); - }); + [zone, enemy_robots](const Robot& robot) + { return contains(zone, robot.position()); }); } void CreaseDefenderFSM::blockThreat( @@ -168,7 +167,7 @@ std::optional CreaseDefenderFSM::findDefenseAreaIntersection( auto front_segment = Segment(inflated_defense_area.posXPosYCorner(), inflated_defense_area.posXNegYCorner()); auto left_segment = Segment(inflated_defense_area.posXPosYCorner(), - inflated_defense_area.negXPosYCorner()); + inflated_defense_area.negXPosYCorner()); auto right_segment = Segment(inflated_defense_area.posXNegYCorner(), inflated_defense_area.negXNegYCorner()); std::vector front_intersections = intersection(ray, front_segment); diff --git a/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.cpp b/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.cpp index ccec443b9b..dc43a12e97 100644 --- a/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.cpp @@ -43,8 +43,8 @@ TEST_F(CreaseDefenderTacticTest, test_not_bumping_ball_towards_net) setTactic(0, tactic); std::vector terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + [tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (!tactic->done()) { yield("Tactic not done"); @@ -52,8 +52,8 @@ TEST_F(CreaseDefenderTacticTest, test_not_bumping_ball_towards_net) }}; std::vector non_terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + [tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { if (world_ptr->ball().velocity().length() > 0.01) { yield("Ball was hit"); @@ -105,7 +105,8 @@ TEST_P(CreaseDefenderTacticTest, crease_defender_test) std::vector terminating_validation_functions = { [target_defend_region, defender_regions, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // Check that tactic is done while (!tactic->done()) { diff --git a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_push_enemy_test.cpp b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_push_enemy_test.cpp index e1e3d41980..cc17e7cd25 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_push_enemy_test.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_push_enemy_test.cpp @@ -67,9 +67,8 @@ TEST_P(DribbleTacticPushEnemyTest, DISABLED_test_steal_ball_from_behind_enemy) std::vector terminating_validation_functions = { [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - checkPossession(tactic, world_ptr, yield); - }}; + ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); }}; std::vector non_terminating_validation_functions = {}; diff --git a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.cpp b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.cpp index f1fe8dfc9a..b20e7d00db 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.cpp @@ -64,9 +64,8 @@ TEST_F(DribbleTacticTest, test_intercept_ball_behind_enemy_robot) std::vector terminating_validation_functions = { [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - checkPossession(tactic, world_ptr, yield); - }}; + ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); }}; std::vector non_terminating_validation_functions = {}; @@ -87,9 +86,8 @@ TEST_F(DribbleTacticTest, test_stopped_ball) std::vector terminating_validation_functions = { [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - checkPossession(tactic, world_ptr, yield); - }}; + ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); }}; std::vector non_terminating_validation_functions = {}; @@ -110,9 +108,8 @@ TEST_F(DribbleTacticTest, test_ball_bounce_off_of_enemy_robot) std::vector terminating_validation_functions = { [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - checkPossession(tactic, world_ptr, yield); - }}; + ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); }}; std::vector non_terminating_validation_functions = {}; @@ -135,14 +132,16 @@ TEST_F(DribbleTacticTest, test_moving_ball_dribble_dest) std::vector terminating_validation_functions = { [this, dribble_destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); ballAtPoint(dribble_destination, world_ptr, yield); checkPossession(tactic, world_ptr, yield); }}; std::vector non_terminating_validation_functions = { - [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // TODO (#2514): tune dribbling and re-enable // robotNotExcessivelyDribbling(1, world_ptr, yield); }}; @@ -166,7 +165,8 @@ TEST_F(DribbleTacticTest, test_moving_ball_dribble_orientation) std::vector terminating_validation_functions = { [this, dribble_orientation, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), yield); @@ -195,7 +195,8 @@ TEST_F(DribbleTacticTest, test_moving_ball_dribble_dest_and_orientation) std::vector terminating_validation_functions = { [this, dribble_destination, dribble_orientation, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); ballAtPoint(dribble_destination, world_ptr, yield); robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), @@ -204,7 +205,8 @@ TEST_F(DribbleTacticTest, test_moving_ball_dribble_dest_and_orientation) }}; std::vector non_terminating_validation_functions = { - [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // TODO (#2514): tune dribbling and re-enable // robotNotExcessivelyDribbling(1, world_ptr, yield); }}; @@ -228,7 +230,8 @@ TEST_F(DribbleTacticTest, test_dribble_dest_and_orientation_around_rectangle) std::vector terminating_validation_functions = { [this, dribble_destination, dribble_orientation, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); ballAtPoint(dribble_destination, world_ptr, yield); robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), @@ -237,7 +240,8 @@ TEST_F(DribbleTacticTest, test_dribble_dest_and_orientation_around_rectangle) }}; std::vector non_terminating_validation_functions = { - [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // TODO (#2514): tune dribbling and re-enable // robotNotExcessivelyDribbling(1, world_ptr, yield); }}; @@ -262,7 +266,8 @@ TEST_F(DribbleTacticTest, std::vector terminating_validation_functions = { [this, dribble_destination, dribble_orientation, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); ballAtPoint(dribble_destination, world_ptr, yield); robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), @@ -297,7 +302,8 @@ TEST_F(DribbleTacticTest, test_running_into_enemy_robot_knocking_ball_away) std::vector terminating_validation_functions = { [this, dribble_destination, dribble_orientation, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); ballAtPoint(dribble_destination, world_ptr, yield); robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), @@ -306,7 +312,8 @@ TEST_F(DribbleTacticTest, test_running_into_enemy_robot_knocking_ball_away) }}; std::vector non_terminating_validation_functions = { - [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // TODO (#2514): tune dribbling and re-enable // robotNotExcessivelyDribbling(1, world_ptr, yield); }}; @@ -332,12 +339,12 @@ TEST_F(DribbleTacticTest, test_robot_not_bumping_ball_when_turning_around) std::vector terminating_validation_functions = { [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - checkPossession(tactic, world_ptr, yield); - }}; + ValidationCoroutine::push_type& yield) + { checkPossession(tactic, world_ptr, yield); }}; std::vector non_terminating_validation_functions = { - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (distance(world_ptr->ball().position(), initial_ball_state.position()) > 0.05) { diff --git a/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp b/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp index e2867bae07..e88cb483cb 100644 --- a/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp @@ -27,7 +27,8 @@ TEST_F(HaltTacticTest, robot_already_stopped) setTactic(1, tactic); std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { for (unsigned i = 0; i < 1000; i++) { robotHalt(world_ptr, yield); @@ -53,7 +54,8 @@ TEST_F(HaltTacticTest, robot_start_moving) setTactic(1, tactic); std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { for (unsigned i = 0; i < 1000; i++) { robotHalt(world_ptr, yield); diff --git a/src/software/ai/hl/stp/tactic/kick/kick_tactic_test.cpp b/src/software/ai/hl/stp/tactic/kick/kick_tactic_test.cpp index e4ee881aa4..252eae594e 100644 --- a/src/software/ai/hl/stp/tactic/kick/kick_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/kick/kick_tactic_test.cpp @@ -40,7 +40,8 @@ TEST_P(KickTacticTest, kick_test) std::vector terminating_validation_functions = { [angle_to_kick_at, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { while (!tactic->done()) { yield("Tactic did not complete!"); diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp b/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp index 614db132ce..228c3f2957 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp @@ -38,7 +38,8 @@ TEST_F(MoveTacticTest, test_move_across_field) std::vector terminating_validation_functions = { [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { while (!tactic->done()) { yield("Tactic not done"); @@ -81,7 +82,8 @@ TEST_F(MoveTacticTest, test_autochip_move) std::vector terminating_validation_functions = { [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { while (!tactic->done()) { yield("Tactic not done"); @@ -114,8 +116,8 @@ TEST_F(MoveTacticTest, test_autokick_move) AngularVelocity::zero())}}; auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); + field.enemyDefenseArea().negXNegYCorner(), + field.enemyDefenseArea().negXPosYCorner()}); auto tactic = std::make_shared(); tactic->updateControlParams( @@ -127,7 +129,8 @@ TEST_F(MoveTacticTest, test_autokick_move) std::vector terminating_validation_functions = { [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { while (!tactic->done()) { yield("Tactic not done"); @@ -170,7 +173,8 @@ TEST_F(MoveTacticTest, test_spinning_move_clockwise) std::vector terminating_validation_functions = { [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { robotAtAngularVelocity(0, world_ptr, AngularVelocity::fromDegrees(1 * 360), AngularVelocity::fromDegrees(50), yield); robotAtPosition(0, world_ptr, destination, 0.05, yield); @@ -217,7 +221,8 @@ TEST_F(MoveTacticTest, test_spinning_move_counter_clockwise) std::vector terminating_validation_functions = { [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { robotAtAngularVelocity(0, world_ptr, AngularVelocity::fromDegrees(-4 * 360), AngularVelocity::fromDegrees(50), yield); robotAtPosition(0, world_ptr, destination, 0.05, yield); diff --git a/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_fsm_test.cpp b/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_fsm_test.cpp index 13c787da05..6b6e568d92 100644 --- a/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_fsm_test.cpp +++ b/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_fsm_test.cpp @@ -221,7 +221,7 @@ TEST(PenaltyKickFSMTest, enemy_goalie_left_shot_right) Point enemy_goalie_pos = Point(world->field().enemyGoalCenter().x(), 0.2); Robot enemy_goalie = Robot(0, enemy_goalie_pos, Vector(0, 0), Angle::half(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); Point shot_position = PenaltyKickFSM::evaluateNextShotPosition( std::optional(enemy_goalie), world->field()); @@ -238,7 +238,7 @@ TEST(PenaltyKickFSMTest, enemy_goalie_right_shot_left) Point enemy_goalie_pos = Point(world->field().enemyGoalCenter().x(), -0.2); Robot enemy_goalie = Robot(0, enemy_goalie_pos, Vector(0, 0), Angle::half(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); Point shot_position = PenaltyKickFSM::evaluateNextShotPosition( std::optional(enemy_goalie), world->field()); diff --git a/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp b/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp index ed9a6ddcd6..1935a24336 100644 --- a/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp @@ -41,12 +41,10 @@ TEST_P(PenaltyKickTacticTest, DISABLED_penalty_kick_test) std::vector non_terminating_validation_functions = { ballInPlay, - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - ballNeverMovesBackward(world_ptr, yield); - }, - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); - }}; + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { ballNeverMovesBackward(world_ptr, yield); }, + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); }}; runTest(field_type, ball, {shooter}, {enemy_robot}, terminating_validation_functions, non_terminating_validation_functions, Duration::fromSeconds(10)); @@ -65,12 +63,10 @@ TEST_F(PenaltyKickTacticTest, DISABLED_penalty_no_goalie) std::vector non_terminating_validation_functions = { ballInPlay, - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - ballNeverMovesBackward(world_ptr, yield); - }, - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); - }}; + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { ballNeverMovesBackward(world_ptr, yield); }, + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); }}; auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(0, -2.5)}); diff --git a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic_test.cpp b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic_test.cpp index 43ec635b3e..3f4d846090 100644 --- a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic_test.cpp @@ -42,7 +42,8 @@ TEST_P(PivotKickTacticTest, pivot_kick_test) std::vector terminating_validation_functions = { [angle_to_kick_at, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { while (!tactic->done()) { yield("Tactic did not complete!"); diff --git a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp index 8ac408e27a..dce42f443c 100644 --- a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp @@ -42,7 +42,8 @@ TEST_P(ReceiverTacticTest, perfect_pass_receiver_test) std::vector terminating_validation_functions = { [pass, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { // We check if the robot reaches the desired orientation, at the // desired position before checking if the ball has been received. // @@ -132,7 +133,8 @@ TEST_P(ReceiverTacticTestOneTouch, test_one_touch) std::vector terminating_validation_functions = { [pass, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { // We just care if we scored! friendlyScored(world_ptr, yield); }}; diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm_test.cpp b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm_test.cpp index 65b2e9e822..d943962064 100644 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm_test.cpp +++ b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm_test.cpp @@ -8,7 +8,7 @@ TEST(ShadowEnemyFSMTest, test_findBlockPassPoint) { Point ball_position = Point(0, 2); Robot shadowee = Robot(0, Point(0, -2), Vector(0, 0), Angle::half(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); double shadow_distance = 2; Point block_pass_point = ShadowEnemyFSM::findBlockPassPoint(ball_position, shadowee, shadow_distance); @@ -25,7 +25,7 @@ TEST(ShadowEnemyFSMTest, test_findBlockPassPoint) ball_position = Point(2, 0); shadowee = Robot(0, Point(-2, 0), Vector(0, 0), Angle::half(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); shadow_distance = 0.25; block_pass_point = ShadowEnemyFSM::findBlockPassPoint(ball_position, shadowee, shadow_distance); @@ -35,9 +35,9 @@ TEST(ShadowEnemyFSMTest, test_findBlockPassPoint) TEST(ShadowEnemyFSMTest, test_findBlockShotPoint) { Robot shadower = Robot(0, Point(0, 1), Vector(0, 0), Angle::half(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); Robot shadowee = Robot(0, Point(2, 0), Vector(0, 0), Angle::half(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); + AngularVelocity::zero(), Timestamp::fromSeconds(0)); Field field = Field::createSSLDivisionBField(); Team friendlyTeam = Team(Duration::fromSeconds(1)); friendlyTeam.updateRobots({shadower}); diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic_test.cpp b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic_test.cpp index 09959c6705..9346a10289 100644 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic_test.cpp @@ -57,8 +57,8 @@ TEST_F(ShadowEnemyTacticTest, test_block_pass) setTactic(0, tactic, motion_constraints); std::vector terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + [tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // As the shadowee is located at (0,-2) and the enemy robot that // has the ball is located at (0,2), we would like to block the pass // with a shadow distance of 2 @@ -104,7 +104,8 @@ TEST_F(ShadowEnemyTacticTest, test_block_pass_if_enemy_does_not_have_ball) std::vector terminating_validation_functions = { [this, tactic, shadowee](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { // As the shadowee is located at (0,-2) and the ball is located at (3,0), // we would like to block the pass with a shadow distance of 1.5 Vector pass = Point(3, 0) - shadowee.position(); @@ -149,7 +150,8 @@ TEST_F(ShadowEnemyTacticTest, test_block_net_then_steal_and_chip) std::vector terminating_validation_functions = { [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { // We compose a triangle consisting of the friendly goal posts // and the ball position. If our robot is in this triangle, then // it is blocking a possible shot on net @@ -159,7 +161,8 @@ TEST_F(ShadowEnemyTacticTest, test_block_net_then_steal_and_chip) robotInPolygon(shotTriangle, 1, world_ptr, yield); }, [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + ValidationCoroutine::push_type& yield) + { // As our friendly robot tries to steal and chip the ball, // it should chip the ball in the same direction is it // heading towards the ball @@ -200,8 +203,8 @@ TEST_F(ShadowEnemyTacticTest, test_block_net_if_enemy_threat_is_null) setTactic(0, tactic, motion_constraints); std::vector terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { + [tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { // We compose a triangle consisting of the friendly goal posts // and the ball position. If our robot is in this triangle, then // it is blocking a possible shot on net diff --git a/src/software/ai/hl/stp/tactic/tactic.cpp b/src/software/ai/hl/stp/tactic/tactic.cpp index 1af04330cf..0ff5c22322 100644 --- a/src/software/ai/hl/stp/tactic/tactic.cpp +++ b/src/software/ai/hl/stp/tactic/tactic.cpp @@ -38,13 +38,11 @@ std::map> Tactic::get(const WorldPtr &world_ for (const auto &robot : world_ptr->friendlyTeam().getAllRobots()) { - updatePrimitive( - TacticUpdate(robot, world_ptr, - [this](std::shared_ptr new_primitive) { - primitive = std::move(new_primitive); - }), - !last_execution_robot.has_value() || - last_execution_robot.value() != robot.id()); + updatePrimitive(TacticUpdate(robot, world_ptr, + [this](std::shared_ptr new_primitive) + { primitive = std::move(new_primitive); }), + !last_execution_robot.has_value() || + last_execution_robot.value() != robot.id()); CHECK(primitive != nullptr) << "Primitive for " << objectTypeName(*this) << " in state " diff --git a/src/software/ai/navigator/trajectory/trajectory_path_node.h b/src/software/ai/navigator/trajectory/trajectory_path_node.h index 08500684b6..4b1b4ee710 100644 --- a/src/software/ai/navigator/trajectory/trajectory_path_node.h +++ b/src/software/ai/navigator/trajectory/trajectory_path_node.h @@ -20,7 +20,7 @@ class TrajectoryPathNode */ TrajectoryPathNode(const std::shared_ptr &trajectory, double trajectory_end_time_s) - : trajectory(trajectory), trajectory_end_time_s(trajectory_end_time_s){}; + : trajectory(trajectory), trajectory_end_time_s(trajectory_end_time_s) {}; /** * Constructor for a trajectory. It is assumed that the end time @@ -28,7 +28,7 @@ class TrajectoryPathNode * @param trajectory Trajectory of this trajectory path node */ TrajectoryPathNode(const std::shared_ptr &trajectory) - : trajectory(trajectory), trajectory_end_time_s(trajectory->getTotalTime()){}; + : trajectory(trajectory), trajectory_end_time_s(trajectory->getTotalTime()) {}; /** * Get the trajectory of this trajectory path node diff --git a/src/software/ai/passing/cost_function.cpp b/src/software/ai/passing/cost_function.cpp index 247aed1085..794b3e5cd5 100644 --- a/src/software/ai/passing/cost_function.cpp +++ b/src/software/ai/passing/cost_function.cpp @@ -138,9 +138,8 @@ double calculateInterceptRisk(const Team& enemy_team, const Pass& pass, } std::vector enemy_intercept_risks(enemy_robots.size()); std::transform(enemy_robots.begin(), enemy_robots.end(), - enemy_intercept_risks.begin(), [&](const Robot& robot) { - return calculateInterceptRisk(robot, pass, passing_config); - }); + enemy_intercept_risks.begin(), [&](const Robot& robot) + { return calculateInterceptRisk(robot, pass, passing_config); }); return *std::max_element(enemy_intercept_risks.begin(), enemy_intercept_risks.end()); } diff --git a/src/software/ai/passing/eighteen_zone_pitch_division.cpp b/src/software/ai/passing/eighteen_zone_pitch_division.cpp index ab4893c458..f474d93291 100644 --- a/src/software/ai/passing/eighteen_zone_pitch_division.cpp +++ b/src/software/ai/passing/eighteen_zone_pitch_division.cpp @@ -38,9 +38,8 @@ EighteenZoneId EighteenZonePitchDivision::getZoneId(const Point& position) const } auto zone_id = *std::find_if(zones_.begin(), zones_.end(), - [this, position](const EighteenZoneId& id) { - return contains(getZone(id), position); - }); + [this, position](const EighteenZoneId& id) + { return contains(getZone(id), position); }); return zone_id; } diff --git a/src/software/ai/passing/pass.cpp b/src/software/ai/passing/pass.cpp index bf063d834f..446666711b 100644 --- a/src/software/ai/passing/pass.cpp +++ b/src/software/ai/passing/pass.cpp @@ -94,7 +94,7 @@ double Pass::getPassSpeed(const Point& ball_position, const Point& pass_destinat // Simplify and rearrange for initial velocity x: // x = sqrt((vf^2 - 2rD) / (c^2 - rc^2/s + r/s)) Vector pass_distance = Vector(pass_destination.x() - ball_position.x(), - pass_destination.y() - ball_position.y()); + pass_destination.y() - ball_position.y()); double pass_distance_length_m = pass_distance.length(); // calculating the constant value used in determining pass speed diff --git a/src/software/ai/passing/pass_generator.cpp b/src/software/ai/passing/pass_generator.cpp index 9b054f0fde..ab9618d9f4 100644 --- a/src/software/ai/passing/pass_generator.cpp +++ b/src/software/ai/passing/pass_generator.cpp @@ -125,14 +125,15 @@ PassWithRating PassGenerator::optimizeReceivingPositions( // The objective function we minimize in gradient descent to improve each pass // that we're optimizing const auto objective_function = - [this, &world](const std::array& pass_array) { - // get a pass with the new appropriate speed using the new destination - return ratePass(world, - Pass::fromDestReceiveSpeed( - world.ball().position(), - Point(pass_array[0], pass_array[1]), passing_config_), - passing_config_); - }; + [this, &world](const std::array& pass_array) + { + // get a pass with the new appropriate speed using the new destination + return ratePass(world, + Pass::fromDestReceiveSpeed(world.ball().position(), + Point(pass_array[0], pass_array[1]), + passing_config_), + passing_config_); + }; PassWithRating best_pass{Pass(Point(), Point(), 1.0), -1.0}; for (const auto& [robot_id, receiving_positions] : receiving_positions_map) diff --git a/src/software/ai/passing/receiver_position_generator.hpp b/src/software/ai/passing/receiver_position_generator.hpp index ea051dafd0..d7a691db25 100644 --- a/src/software/ai/passing/receiver_position_generator.hpp +++ b/src/software/ai/passing/receiver_position_generator.hpp @@ -180,7 +180,8 @@ std::vector ReceiverPositionGenerator::getBestReceivingPosition updateBestReceiverPositions(best_receiving_positions, world, pass_origin, top_zones, receiver_config.num_additional_samples_per_top_zone()); std::sort(top_zones.begin(), top_zones.end(), - [&](const ZoneEnum &z1, const ZoneEnum &z2) { + [&](const ZoneEnum &z1, const ZoneEnum &z2) + { return best_receiving_positions.find(z1)->second.rating > best_receiving_positions.find(z2)->second.rating; }); @@ -278,7 +279,8 @@ std::vector ReceiverPositionGenerator::getTopZones( // Sort the zones based on initial ratings auto all_zones = pitch_division_->getAllZoneIds(); std::sort(all_zones.begin(), all_zones.end(), - [&](const ZoneEnum &z1, const ZoneEnum &z2) { + [&](const ZoneEnum &z1, const ZoneEnum &z2) + { return best_receiving_positions.find(z1)->second.rating > best_receiving_positions.find(z2)->second.rating; }); @@ -297,8 +299,10 @@ std::vector ReceiverPositionGenerator::getTopZones( // Check that none of the previously selected top zones are close to the current // candidate zone - bool no_prev_receivers_close = - std::none_of(top_zones.begin(), top_zones.end(), [&](const ZoneEnum &zone) { + bool no_prev_receivers_close = std::none_of( + top_zones.begin(), top_zones.end(), + [&](const ZoneEnum &zone) + { return curr_pass_angle.minDiff(best_receiving_positions.find(zone) ->second.pass.passerOrientation()) < min_angle_diff_between_receivers; @@ -310,7 +314,8 @@ std::vector ReceiverPositionGenerator::getTopZones( no_prev_receivers_close && std::none_of( existing_receiver_positions.begin(), existing_receiver_positions.end(), - [&](const Point &existing_receiver_position) { + [&](const Point &existing_receiver_position) + { return curr_pass_angle.minDiff( (existing_receiver_position - pass_origin).orientation()) < min_angle_diff_between_receivers; diff --git a/src/software/backend/unix_simulator_backend.cpp b/src/software/backend/unix_simulator_backend.cpp index 977350dc26..c2aa916496 100644 --- a/src/software/backend/unix_simulator_backend.cpp +++ b/src/software/backend/unix_simulator_backend.cpp @@ -20,9 +20,8 @@ UnixSimulatorBackend::UnixSimulatorBackend( [&](TbotsProto::RobotStatus& msg) { receiveRobotStatus(msg); }, proto_logger)); ssl_wrapper_input.reset(new ThreadedProtoUnixListener( - runtime_dir + SSL_WRAPPER_PATH, - [&](SSLProto::SSL_WrapperPacket& msg) { receiveSSLWrapperPacket(msg); }, - proto_logger)); + runtime_dir + SSL_WRAPPER_PATH, [&](SSLProto::SSL_WrapperPacket& msg) + { receiveSSLWrapperPacket(msg); }, proto_logger)); ssl_referee_input.reset(new ThreadedProtoUnixListener( runtime_dir + SSL_REFEREE_PATH, diff --git a/src/software/embedded/gpio_char_dev.h b/src/software/embedded/gpio_char_dev.h index 23f8263c3b..87e69d3141 100644 --- a/src/software/embedded/gpio_char_dev.h +++ b/src/software/embedded/gpio_char_dev.h @@ -1,9 +1,10 @@ #pragma once -#include "software/embedded/gpio.h" +#include #include -#include + +#include "software/embedded/gpio.h" /** * GPIO with the character device interface diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index bf7bb900b8..f1b113059f 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -99,7 +99,7 @@ std::unique_ptr PrimitiveExecutor::stepPrimi case TbotsProto::Primitive::kStop: { auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); + TbotsProto::AutoChipOrKick()); auto output = std::make_unique( prim->direct_control()); status.set_running_primitive(false); diff --git a/src/software/embedded/redis/redis_client.cpp b/src/software/embedded/redis/redis_client.cpp index 8c43e95fcd..441a9a2f96 100644 --- a/src/software/embedded/redis/redis_client.cpp +++ b/src/software/embedded/redis/redis_client.cpp @@ -4,8 +4,9 @@ RedisClient::RedisClient(std::string host, size_t port) : subscriber_(), client_(), host_(host), port_(port) { - auto connection_callback = [](const std::string &host, std::size_t port, - cpp_redis::connect_state status) { + auto connection_callback = + [](const std::string &host, std::size_t port, cpp_redis::connect_state status) + { if (status == cpp_redis::connect_state::dropped) { LOG(WARNING) << "Redis subscriber_ connection dropped"; @@ -36,10 +37,10 @@ RedisClient::RedisClient(std::string host, size_t port) // subscribe to key 'set' event within the keyspace // adds key and its value to the key value set subscriber_.subscribe("__keyevent@0__:set", - [this](const std::string &channel, const std::string &key) { - client_.get(key, [this, key](cpp_redis::reply &value) { - key_value_set_[key] = value.as_string(); - }); + [this](const std::string &channel, const std::string &key) + { + client_.get(key, [this, key](cpp_redis::reply &value) + { key_value_set_[key] = value.as_string(); }); client_.commit(); }); subscriber_.commit(); diff --git a/src/software/embedded/redis/redis_client_test.cpp b/src/software/embedded/redis/redis_client_test.cpp index 065ad158c7..ecdfd51af6 100644 --- a/src/software/embedded/redis/redis_client_test.cpp +++ b/src/software/embedded/redis/redis_client_test.cpp @@ -17,7 +17,8 @@ TEST(RedisKeyValueStoreTests, DISABLED_cpp_redis_get_test) cpp_redis::client client; client.connect( REDIS_DEFAULT_HOST, REDIS_DEFAULT_PORT, - [](const std::string &host, std::size_t port, cpp_redis::connect_state status) { + [](const std::string &host, std::size_t port, cpp_redis::connect_state status) + { if (status == cpp_redis::connect_state::dropped) { std::cout << "client disconnected from " << host << ":" << port @@ -25,23 +26,25 @@ TEST(RedisKeyValueStoreTests, DISABLED_cpp_redis_get_test) } }); - client.set("A", "1", [](cpp_redis::reply &reply) { - std::cout << "Setting A to 1: " << reply << std::endl; - }); - - client.get("A", [](cpp_redis::reply &reply) { - std::cout << "Getting A: " << reply << std::endl; - ASSERT_EQ(atoi(reply.as_string().c_str()), 1); - }); - - client.set("A", "2", [](cpp_redis::reply &reply) { - std::cout << "Setting A to 2: " << reply << std::endl; - }); - - client.get("A", [](cpp_redis::reply &reply) { - std::cout << "Getting A: " << reply << std::endl; - ASSERT_EQ(atoi(reply.as_string().c_str()), 2); - }); + client.set("A", "1", [](cpp_redis::reply &reply) + { std::cout << "Setting A to 1: " << reply << std::endl; }); + + client.get("A", + [](cpp_redis::reply &reply) + { + std::cout << "Getting A: " << reply << std::endl; + ASSERT_EQ(atoi(reply.as_string().c_str()), 1); + }); + + client.set("A", "2", [](cpp_redis::reply &reply) + { std::cout << "Setting A to 2: " << reply << std::endl; }); + + client.get("A", + [](cpp_redis::reply &reply) + { + std::cout << "Getting A: " << reply << std::endl; + ASSERT_EQ(atoi(reply.as_string().c_str()), 2); + }); client.sync_commit(); } @@ -50,7 +53,8 @@ TEST(RedisKeyValueStoreTests, DISABLED_cpp_redis_get_and_set_speed_test) cpp_redis::client client; client.connect( REDIS_DEFAULT_HOST, REDIS_DEFAULT_PORT, - [](const std::string &host, std::size_t port, cpp_redis::connect_state status) { + [](const std::string &host, std::size_t port, cpp_redis::connect_state status) + { if (status == cpp_redis::connect_state::dropped) { std::cout << "client disconnected from " << host << ":" << port @@ -64,15 +68,13 @@ TEST(RedisKeyValueStoreTests, DISABLED_cpp_redis_get_and_set_speed_test) { client.set("A", "1", [](cpp_redis::reply &reply) {}); - client.get("A", [](cpp_redis::reply &reply) { - ASSERT_EQ(atoi(reply.as_string().c_str()), 1); - }); + client.get("A", [](cpp_redis::reply &reply) + { ASSERT_EQ(atoi(reply.as_string().c_str()), 1); }); client.set("A", "2", [](cpp_redis::reply &reply) {}); - client.get("A", [](cpp_redis::reply &reply) { - ASSERT_EQ(atoi(reply.as_string().c_str()), 2); - }); + client.get("A", [](cpp_redis::reply &reply) + { ASSERT_EQ(atoi(reply.as_string().c_str()), 2); }); } auto end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; @@ -90,7 +92,8 @@ TEST(RedisImplTests, DISABLED_redis_client_get_and_set_impl_test) cpp_redis::client client; client.connect( REDIS_DEFAULT_HOST, REDIS_DEFAULT_PORT, - [](const std::string &host, std::size_t port, cpp_redis::connect_state status) { + [](const std::string &host, std::size_t port, cpp_redis::connect_state status) + { if (status == cpp_redis::connect_state::dropped) { std::cout << "client disconnected from " << host << ":" << port diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index c8e06e9910..15744c6dc2 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -1142,8 +1142,9 @@ void MotorService::checkEncoderConnections() for (int num_iterations = 0; num_iterations < 10 && - std::any_of(calibrated_motors.begin(), calibrated_motors.end(), - [](bool calibration_status) { return !calibration_status; }); + 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) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index a1134e8cb4..8f8261b025 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -136,7 +136,9 @@ int main(int argc, char **argv) // Inputs // World State Input: Configures the ERForceSimulator auto world_state_input = ThreadedProtoUnixListener( - runtime_dir + WORLD_STATE_PATH, [&](TbotsProto::WorldState input) { + runtime_dir + WORLD_STATE_PATH, + [&](TbotsProto::WorldState input) + { std::scoped_lock lock(simulator_mutex); er_force_sim->setWorldState(input); @@ -153,13 +155,17 @@ int main(int argc, char **argv) // World Input: Buffer vision until we have primitives to tick // the simulator with auto blue_world_input = ThreadedProtoUnixListener( - runtime_dir + BLUE_WORLD_PATH, [&](TbotsProto::World input) { + runtime_dir + BLUE_WORLD_PATH, + [&](TbotsProto::World input) + { std::scoped_lock lock(simulator_mutex); blue_vision = input; }); auto yellow_world_input = ThreadedProtoUnixListener( - runtime_dir + YELLOW_WORLD_PATH, [&](TbotsProto::World input) { + runtime_dir + YELLOW_WORLD_PATH, + [&](TbotsProto::World input) + { std::scoped_lock lock(simulator_mutex); yellow_vision = input; }); @@ -167,7 +173,9 @@ int main(int argc, char **argv) // PrimitiveSet Input: set the primitive set with cached vision auto yellow_primitive_set_input = ThreadedProtoUnixListener( - runtime_dir + YELLOW_PRIMITIVE_SET, [&](TbotsProto::PrimitiveSet input) { + runtime_dir + YELLOW_PRIMITIVE_SET, + [&](TbotsProto::PrimitiveSet input) + { std::scoped_lock lock(simulator_mutex); er_force_sim->setYellowRobotPrimitiveSet( input, std::make_unique(yellow_vision)); @@ -175,7 +183,9 @@ int main(int argc, char **argv) auto blue_primitive_set_input = ThreadedProtoUnixListener( - runtime_dir + BLUE_PRIMITIVE_SET, [&](TbotsProto::PrimitiveSet input) { + runtime_dir + BLUE_PRIMITIVE_SET, + [&](TbotsProto::PrimitiveSet input) + { std::scoped_lock lock(simulator_mutex); er_force_sim->setBlueRobotPrimitiveSet( input, std::make_unique(blue_vision)); @@ -183,7 +193,9 @@ int main(int argc, char **argv) // Simulator Tick Input auto simulator_tick = ThreadedProtoUnixListener( - runtime_dir + SIMULATION_TICK_PATH, [&](TbotsProto::SimulatorTick input) { + runtime_dir + SIMULATION_TICK_PATH, + [&](TbotsProto::SimulatorTick input) + { std::scoped_lock lock(simulator_mutex); // Step the simulation and send back the wrapper packets and diff --git a/src/software/estop/threaded_estop_reader_test.cpp b/src/software/estop/threaded_estop_reader_test.cpp index dfb836f156..d8389c731a 100644 --- a/src/software/estop/threaded_estop_reader_test.cpp +++ b/src/software/estop/threaded_estop_reader_test.cpp @@ -33,7 +33,7 @@ TEST(ThreadedEstopReaderTest, estop_tick_is_called_multiple_times) { std::unique_ptr mock_uart = std::make_unique(); - int test_timeout_ms = 200; + int test_timeout_ms = 100; unsigned char arbitrary_garbage_val = 0b10111011; std::vector play_ret_val(1, ESTOP_PLAY_MSG); std::vector garbage_ret_val(1, arbitrary_garbage_val); @@ -44,7 +44,7 @@ TEST(ThreadedEstopReaderTest, estop_tick_is_called_multiple_times) std::mutex m; std::unique_lock lock(m); std::condition_variable cv; - bool ready; + bool ready = false; EXPECT_CALL(*mock_uart_ptr, flushSerialPort(_)).WillRepeatedly(Return(true)); diff --git a/src/software/geom/algorithms/end_in_obstacle_sample_test.cpp b/src/software/geom/algorithms/end_in_obstacle_sample_test.cpp index a34c7551b5..daf60a195c 100644 --- a/src/software/geom/algorithms/end_in_obstacle_sample_test.cpp +++ b/src/software/geom/algorithms/end_in_obstacle_sample_test.cpp @@ -17,7 +17,7 @@ class EndInObstacleSampleTest : public testing::Test public: EndInObstacleSampleTest() : world(TestUtil::createBlankTestingWorld(TbotsProto::FieldType::DIV_B)), - obstacle_factory(TbotsProto::RobotNavigationObstacleConfig()){}; + obstacle_factory(TbotsProto::RobotNavigationObstacleConfig()) {}; static bool isSamplePointValid(Point point, std::vector obstacles) { double min_distance = MAX_ALLOWABLE_SAMPLE_ERROR + 1; diff --git a/src/software/geom/algorithms/find_open_circles.cpp b/src/software/geom/algorithms/find_open_circles.cpp index 1d210ab800..166c675820 100644 --- a/src/software/geom/algorithms/find_open_circles.cpp +++ b/src/software/geom/algorithms/find_open_circles.cpp @@ -25,11 +25,10 @@ std::vector findOpenCircles(const Rectangle &bounding_box, // on the triangle that this vertex was created from // Filters out points that are outside of the bounding box - points.erase(std::remove_if(points.begin(), points.end(), - [&bounding_box](const Point &p) { - return !contains(bounding_box, p); - }), - points.end()); + points.erase( + std::remove_if(points.begin(), points.end(), [&bounding_box](const Point &p) + { return !contains(bounding_box, p); }), + points.end()); std::vector empty_circles; @@ -127,7 +126,8 @@ std::optional findClosestPoint(const Point &origin_point, { closest_point = *std::min_element(test_points.begin(), test_points.end(), - [&](const Point &test_point1, const Point &test_point2) { + [&](const Point &test_point1, const Point &test_point2) + { return (origin_point - test_point1).lengthSquared() < (origin_point - test_point2).lengthSquared(); }); diff --git a/src/software/geom/algorithms/find_open_circles_test.cpp b/src/software/geom/algorithms/find_open_circles_test.cpp index 3a29fb8462..e8748f39fb 100644 --- a/src/software/geom/algorithms/find_open_circles_test.cpp +++ b/src/software/geom/algorithms/find_open_circles_test.cpp @@ -122,7 +122,7 @@ TEST(FindOpenCirclesTest, test_find_open_circle_points_outside_of_box_one_in_box Rectangle rectangle(Point(-1, -1), Point(1, 1)); std::vector points = {Point(-2, -1), Point(3, -2), Point(-1.1, -1.1), - Point(0.9, 0.9)}; + Point(0.9, 0.9)}; std::vector empty_circles = findOpenCircles(rectangle, points); ASSERT_EQ(4, empty_circles.size()); diff --git a/src/software/geom/algorithms/furthest_point.cpp b/src/software/geom/algorithms/furthest_point.cpp index 43d5000e03..c2c866d9e9 100644 --- a/src/software/geom/algorithms/furthest_point.cpp +++ b/src/software/geom/algorithms/furthest_point.cpp @@ -9,7 +9,6 @@ Point furthestPoint(const Rectangle &a, const Point &b) std::vector corners = a.getPoints(); return *std::max_element(corners.begin(), corners.end(), - [&](const Point &corner1, const Point &corner2) { - return distance(b, corner1) < distance(b, corner2); - }); + [&](const Point &corner1, const Point &corner2) + { return distance(b, corner1) < distance(b, corner2); }); } diff --git a/src/software/geom/algorithms/rasterize_test.cpp b/src/software/geom/algorithms/rasterize_test.cpp index a2cd117c86..a81d41a69d 100644 --- a/src/software/geom/algorithms/rasterize_test.cpp +++ b/src/software/geom/algorithms/rasterize_test.cpp @@ -14,9 +14,8 @@ namespace TestUtil for (Point& p : all_points) { auto compare_based_on_distance_to_p = [p](const Point& a, - const Point& b) -> bool { - return distance(a, p) < distance(b, p); - }; + const Point& b) -> bool + { return distance(a, p) < distance(b, p); }; std::vector all_points_copy = all_points; all_points_copy.erase( diff --git a/src/software/geom/algorithms/step_along_perimeter.cpp b/src/software/geom/algorithms/step_along_perimeter.cpp index da5976f6fa..64552e0003 100644 --- a/src/software/geom/algorithms/step_along_perimeter.cpp +++ b/src/software/geom/algorithms/step_along_perimeter.cpp @@ -14,9 +14,8 @@ Point stepAlongPerimeter(const Polygon& polygon, const Point& start, const std::vector& polygon_segments = polygon.getSegments(); auto min_it = std::min_element(polygon_segments.begin(), polygon_segments.end(), - [&start](const auto& a, const auto& b) { - return distance(start, a) < distance(start, b); - }); + [&start](const auto& a, const auto& b) + { return distance(start, a) < distance(start, b); }); std::size_t start_segment_index = std::distance(polygon_segments.begin(), min_it); // finds the point closest to start point on the segment diff --git a/src/software/geom/angle_map.cpp b/src/software/geom/angle_map.cpp index 98ff5c2833..d3d79ed365 100644 --- a/src/software/geom/angle_map.cpp +++ b/src/software/geom/angle_map.cpp @@ -69,7 +69,7 @@ AngleSegment AngleMap::getBiggestViableAngleSegment() AngleSegment taken_angle_seg = i[0]; AngleSegment next_taken_angle_seg = i[1]; AngleSegment viable_angle_seg = AngleSegment(taken_angle_seg.getAngleBottom(), - next_taken_angle_seg.getAngleTop()); + next_taken_angle_seg.getAngleTop()); if (viable_angle_seg.getDeltaInDegrees() > biggest_viable_angle_seg.getDeltaInDegrees()) { diff --git a/src/software/geom/linear_spline2d.cpp b/src/software/geom/linear_spline2d.cpp index 2f35884ede..73b135de8a 100644 --- a/src/software/geom/linear_spline2d.cpp +++ b/src/software/geom/linear_spline2d.cpp @@ -26,11 +26,13 @@ const Point LinearSpline2d::getValueAt(double val) const else { // Note: this could be more performant with binary search - auto seg_it = std::find_if( - segments.begin(), segments.end(), [clamped_val](const SplineSegment2d& sseg) { - return (clamped_val >= sseg.getParametrizationStartVal() && - clamped_val <= sseg.getParametrizationEndVal()); - }); + auto seg_it = + std::find_if(segments.begin(), segments.end(), + [clamped_val](const SplineSegment2d& sseg) + { + return (clamped_val >= sseg.getParametrizationStartVal() && + clamped_val <= sseg.getParametrizationEndVal()); + }); if (seg_it == segments.end()) { diff --git a/src/software/geom/polygon.cpp b/src/software/geom/polygon.cpp index edf36f27c2..e6a906e7f3 100644 --- a/src/software/geom/polygon.cpp +++ b/src/software/geom/polygon.cpp @@ -144,9 +144,9 @@ const std::vector& Polygon::getPoints() const double Polygon::perimeter() const { - return (std::accumulate( - segments_.begin(), segments_.end(), 0.0, - [](double acc, const Segment& seg) { return acc + seg.length(); })); + return (std::accumulate(segments_.begin(), segments_.end(), 0.0, + [](double acc, const Segment& seg) + { return acc + seg.length(); })); } bool operator==(const Polygon& poly1, const Polygon& poly2) diff --git a/src/software/logger/proto_logger.h b/src/software/logger/proto_logger.h index ad920463ba..ba05e2dba2 100644 --- a/src/software/logger/proto_logger.h +++ b/src/software/logger/proto_logger.h @@ -60,7 +60,7 @@ class ProtoLogger * Removing copy constructor and assignment operator to prevent accidental copying * of the ProtoLogger. */ - ProtoLogger(const ProtoLogger&) = delete; + ProtoLogger(const ProtoLogger&) = delete; ProtoLogger& operator=(const ProtoLogger&) = delete; ~ProtoLogger(); diff --git a/src/software/math/statistics_functions.hpp b/src/software/math/statistics_functions.hpp index 52a53c1c6d..b06e501f04 100644 --- a/src/software/math/statistics_functions.hpp +++ b/src/software/math/statistics_functions.hpp @@ -49,9 +49,8 @@ double stdevSample(const std::vector data) double data_mean = mean(data); double sq_residuals = std::accumulate( - data.begin(), data.end(), 0.0, [data_mean](double res, double val) { - return res + std::pow(static_cast(val) - data_mean, 2); - }); + data.begin(), data.end(), 0.0, [data_mean](double res, double val) + { return res + std::pow(static_cast(val) - data_mean, 2); }); return std::sqrt(sq_residuals / (n - 1)); } diff --git a/src/software/multithreading/first_in_first_out_threaded_observer.h b/src/software/multithreading/first_in_first_out_threaded_observer.h index 9f1867353a..fbde13663d 100644 --- a/src/software/multithreading/first_in_first_out_threaded_observer.h +++ b/src/software/multithreading/first_in_first_out_threaded_observer.h @@ -16,7 +16,7 @@ template class FirstInFirstOutThreadedObserver : public ThreadedObserver { public: - FirstInFirstOutThreadedObserver() : ThreadedObserver(){}; + FirstInFirstOutThreadedObserver() : ThreadedObserver() {}; /** * Creates a new FirstInFirstOutThreadedObserver diff --git a/src/software/multithreading/last_in_first_out_threaded_observer.h b/src/software/multithreading/last_in_first_out_threaded_observer.h index ba00ec5397..8e542f659e 100644 --- a/src/software/multithreading/last_in_first_out_threaded_observer.h +++ b/src/software/multithreading/last_in_first_out_threaded_observer.h @@ -13,7 +13,7 @@ template class LastInFirstOutThreadedObserver : public ThreadedObserver { public: - LastInFirstOutThreadedObserver() : ThreadedObserver(){}; + LastInFirstOutThreadedObserver() : ThreadedObserver() {}; explicit LastInFirstOutThreadedObserver(size_t buffer_size) : ThreadedObserver(buffer_size){}; std::optional getNextValue(const Duration& max_wait_time) final; diff --git a/src/software/multithreading/observer_test.cpp b/src/software/multithreading/observer_test.cpp index 277b839689..0e3a16fcfd 100644 --- a/src/software/multithreading/observer_test.cpp +++ b/src/software/multithreading/observer_test.cpp @@ -79,12 +79,14 @@ TEST(Observer, receiveValue_value_not_yet_available) // Create a separate thread to grab the value for us std::optional result = std::nullopt; - std::thread receive_value_thread([&]() { - while (!result) + std::thread receive_value_thread( + [&]() { - result = test_observer.getMostRecentValueFromBufferWrapper(); - } - }); + while (!result) + { + result = test_observer.getMostRecentValueFromBufferWrapper(); + } + }); // Send the value over test_observer.receiveValue(202); diff --git a/src/software/multithreading/thread_safe_buffer.hpp b/src/software/multithreading/thread_safe_buffer.hpp index 50ce37fe53..a167deca58 100644 --- a/src/software/multithreading/thread_safe_buffer.hpp +++ b/src/software/multithreading/thread_safe_buffer.hpp @@ -169,7 +169,9 @@ std::unique_lock ThreadSafeBuffer::waitForBufferToHaveAValue( { std::unique_lock buffer_lock(buffer_mutex); received_new_value.wait_for( - buffer_lock, std::chrono::duration(max_wait_time.toSeconds()), [this] { + buffer_lock, std::chrono::duration(max_wait_time.toSeconds()), + [this] + { std::scoped_lock destructor_called_lock(destructor_called_mutex); return !buffer.empty() || destructor_called; }); diff --git a/src/software/multithreading/thread_safe_buffer_test.cpp b/src/software/multithreading/thread_safe_buffer_test.cpp index 67f84785fc..d6ccef4239 100644 --- a/src/software/multithreading/thread_safe_buffer_test.cpp +++ b/src/software/multithreading/thread_safe_buffer_test.cpp @@ -47,12 +47,14 @@ TEST(ThreadSafeBufferTest, pullLeastRecentlyAddedValue_single_value_when_buffer_ std::optional result = std::nullopt; // This "popLeastRecentlyAddedValue" call should block until something is "pushed" - std::thread puller_thread([&]() { - while (!result) + std::thread puller_thread( + [&]() { - result = buffer.popLeastRecentlyAddedValue(Duration::fromSeconds(0.1)); - } - }); + while (!result) + { + result = buffer.popLeastRecentlyAddedValue(Duration::fromSeconds(0.1)); + } + }); buffer.push(84); @@ -73,13 +75,15 @@ TEST(ThreadSafeBufferTest, pullLeastRecentlyAddedValue_single_value_when_buffer_ std::optional result = std::nullopt; // This "popLeastRecentlyAddedValue" call should block until something is "pushed" - std::thread puller_thread([&]() { - while (!result) + std::thread puller_thread( + [&]() { - // should find values already in the buffer - result = buffer.popLeastRecentlyAddedValue(Duration::fromSeconds(2)); - } - }); + while (!result) + { + // should find values already in the buffer + result = buffer.popLeastRecentlyAddedValue(Duration::fromSeconds(2)); + } + }); // this push should be too late to affect the previous call std::this_thread::sleep_for(std::chrono::seconds(2)); @@ -135,12 +139,14 @@ TEST(ThreadSafeBufferTest, pullMostRecentlyAddedValue_single_value_when_buffer_i std::optional result = std::nullopt; // This "popLeastRecentlyAddedValue" call should block until something is "pushed" - std::thread puller_thread([&]() { - while (!result) + std::thread puller_thread( + [&]() { - result = buffer.popMostRecentlyAddedValue(Duration::fromSeconds(0.1)); - } - }); + while (!result) + { + result = buffer.popMostRecentlyAddedValue(Duration::fromSeconds(0.1)); + } + }); buffer.push(84); diff --git a/src/software/network_log_listener_main.cpp b/src/software/network_log_listener_main.cpp index edc28740de..8335b6c9b6 100644 --- a/src/software/network_log_listener_main.cpp +++ b/src/software/network_log_listener_main.cpp @@ -95,7 +95,8 @@ int main(int argc, char **argv) } // Only show logs from robots in the selected_ids list, unless it is empty - auto robot_log_callback = [args](TbotsProto::RobotLog log) { + auto robot_log_callback = [args](TbotsProto::RobotLog log) + { if (!args.selected_ids.empty() && std::find(args.selected_ids.begin(), args.selected_ids.end(), log.robot_id()) == args.selected_ids.end()) diff --git a/src/software/networking/benchmarking_utils/latency_tester_node.cpp b/src/software/networking/benchmarking_utils/latency_tester_node.cpp index c85a33f941..2ef9022114 100644 --- a/src/software/networking/benchmarking_utils/latency_tester_node.cpp +++ b/src/software/networking/benchmarking_utils/latency_tester_node.cpp @@ -15,7 +15,7 @@ LatencyTesterNode::LatencyTesterNode(const std::string& interface, listener_ = std::make_unique( io_listener_service_, listen_ip, listen_port, interface, true, std::bind(&LatencyTesterNode::onReceive, this, std::placeholders::_1, - std::placeholders::_2)); + std::placeholders::_2)); std::string send_ip = ROBOT_MULTICAST_CHANNELS.at(send_channel); sender_ = std::make_unique(io_sender_service_, send_ip, send_port, diff --git a/src/software/optimization/gradient_descent_optimizer.hpp b/src/software/optimization/gradient_descent_optimizer.hpp index 34d5c6bf8f..c48a64e51a 100644 --- a/src/software/optimization/gradient_descent_optimizer.hpp +++ b/src/software/optimization/gradient_descent_optimizer.hpp @@ -176,9 +176,9 @@ std::array GradientDescentOptimizer::maximize( std::function)> objective_function, std::array initial_value, unsigned int num_iters) { - return followGradient( - objective_function, initial_value, num_iters, - [](double curr_value, double step) { return curr_value + step; }); + return followGradient(objective_function, initial_value, num_iters, + [](double curr_value, double step) + { return curr_value + step; }); } template @@ -186,9 +186,9 @@ std::array GradientDescentOptimizer::minimize( std::function)> objective_function, std::array initial_value, unsigned int num_iters) { - return followGradient( - objective_function, initial_value, num_iters, - [](double curr_value, double step) { return curr_value - step; }); + return followGradient(objective_function, initial_value, num_iters, + [](double curr_value, double step) + { return curr_value - step; }); } template diff --git a/src/software/optimization/gradient_descent_test.cpp b/src/software/optimization/gradient_descent_test.cpp index 0444c6c989..ec1bd94d4a 100644 --- a/src/software/optimization/gradient_descent_test.cpp +++ b/src/software/optimization/gradient_descent_test.cpp @@ -36,9 +36,8 @@ TEST(GradientDescentOptimizerTest, minimize_multi_valued_function) GradientDescentOptimizer<2> gradientDescentOptimizer({0.1, 0.05}); // f = x^2 + 2*y^2 + 20 - auto f = [](std::array x) { - return std::pow(x.at(0), 2) + 2 * std::pow(x.at(1), 2) + 20; - }; + auto f = [](std::array x) + { return std::pow(x.at(0), 2) + 2 * std::pow(x.at(1), 2) + 20; }; auto min = gradientDescentOptimizer.minimize(f, {1, -1}, 100); @@ -54,9 +53,8 @@ TEST(GradientDescentOptimizerTest, minimize_multi_valued_function_with_offsets) GradientDescentOptimizer<2> gradientDescentOptimizer({0.1, 0.05}); // f = (x+5)^2 + 2*(y-4)^2 + 20 - auto f = [](std::array x) { - return std::pow(x.at(0) + 5, 2) + 2 * std::pow(x.at(1) - 4, 2) + 20; - }; + auto f = [](std::array x) + { return std::pow(x.at(0) + 5, 2) + 2 * std::pow(x.at(1) - 4, 2) + 20; }; auto min = gradientDescentOptimizer.minimize(f, {0, 0}, 150); diff --git a/src/software/py_constants.cpp b/src/software/py_constants.cpp index d165b523b4..92e3016f9c 100644 --- a/src/software/py_constants.cpp +++ b/src/software/py_constants.cpp @@ -66,14 +66,16 @@ PYBIND11_MODULE(py_constants, m) m.attr("WORLD_STATE_RECEIVED_TRIGGER_PATH") = WORLD_STATE_RECEIVED_TRIGGER_PATH; // Multicast Channels - m.def("getRobotMulticastChannel", [](py::args& args) { - if (args.size() != 1) - { - throw std::runtime_error("must provide channel number only"); - } - - return ROBOT_MULTICAST_CHANNELS.at(args[0].cast()); - }); + m.def("getRobotMulticastChannel", + [](py::args& args) + { + if (args.size() != 1) + { + throw std::runtime_error("must provide channel number only"); + } + + return ROBOT_MULTICAST_CHANNELS.at(args[0].cast()); + }); // Ports m.attr("PRIMITIVE_PORT") = PRIMITIVE_PORT; diff --git a/src/software/python_bindings.cpp b/src/software/python_bindings.cpp index 4d17edf6e4..86a556a1bf 100644 --- a/src/software/python_bindings.cpp +++ b/src/software/python_bindings.cpp @@ -158,11 +158,13 @@ PYBIND11_MODULE(python_bindings, m) .def(-py::self) .def(py::self == Point()) .def(py::self != Point()) - .def("__repr__", [](const Point& v) { - std::stringstream stream; - stream << v; - return stream.str(); - }); + .def("__repr__", + [](const Point& v) + { + std::stringstream stream; + stream << v; + return stream.str(); + }); py::class_(m, "Vector") .def(py::init()) @@ -188,11 +190,13 @@ PYBIND11_MODULE(python_bindings, m) .def(py::self *= double()) .def(py::self / double()) .def(py::self /= double()) - .def("__repr__", [](const Vector& v) { - std::stringstream stream; - stream << v; - return stream.str(); - }); + .def("__repr__", + [](const Vector& v) + { + std::stringstream stream; + stream << v; + return stream.str(); + }); py::class_(m, "Polygon") .def(py::init>()) @@ -200,11 +204,13 @@ PYBIND11_MODULE(python_bindings, m) .def("getPoints", &Polygon::getPoints) .def("getSegments", &Polygon::getSegments) // Overloaded - .def("__repr__", [](const Polygon& v) { - std::stringstream stream; - stream << v; - return stream.str(); - }); + .def("__repr__", + [](const Polygon& v) + { + std::stringstream stream; + stream << v; + return stream.str(); + }); py::class_(m, "Angle") .def(py::init<>()) @@ -212,18 +218,21 @@ PYBIND11_MODULE(python_bindings, m) .def_static("fromDegrees", &Angle::fromDegrees) .def("toRadians", &Angle::toRadians) // Overloaded - .def("__repr__", [](const Angle& a) { - std::stringstream stream; - stream << a; - return stream.str(); - }); + .def("__repr__", + [](const Angle& a) + { + std::stringstream stream; + stream << a; + return stream.str(); + }); py::class_(m, "ConvexPolygon"); py::class_(m, "Rectangle") .def(py::init()) // Overloaded .def("__repr__", - [](const Rectangle& r) { + [](const Rectangle& r) + { std::stringstream stream; stream << r; return stream.str(); @@ -256,7 +265,8 @@ PYBIND11_MODULE(python_bindings, m) .def(py::init()) // Overloaded .def("__repr__", - [](const Circle& c) { + [](const Circle& c) + { std::stringstream stream; stream << c; return stream.str(); @@ -267,11 +277,13 @@ PYBIND11_MODULE(python_bindings, m) py::class_(m, "Stadium") .def(py::init()) - .def("__repr__", [](const Stadium& s) { - std::stringstream stream; - stream << s; - return stream.str(); - }); + .def("__repr__", + [](const Stadium& s) + { + std::stringstream stream; + stream << s; + return stream.str(); + }); py::class_(m, "RobotConstants") .def_readwrite("max_force_dribbler_speed_rpm", diff --git a/src/software/sensor_fusion/filter/ball_filter.cpp b/src/software/sensor_fusion/filter/ball_filter.cpp index 7a6f00bc5b..2338469fb2 100644 --- a/src/software/sensor_fusion/filter/ball_filter.cpp +++ b/src/software/sensor_fusion/filter/ball_filter.cpp @@ -340,7 +340,7 @@ std::optional BallFilter::estimateBallVelocity if (ball_regression_line) { current_position = closestPoint(current_detection.position, - ball_regression_line.value()); + ball_regression_line.value()); previous_position = closestPoint(previous_detection.position, ball_regression_line.value()); } diff --git a/src/software/sensor_fusion/filter/ball_occlusion_test.cpp b/src/software/sensor_fusion/filter/ball_occlusion_test.cpp index 21c3195be3..c24fcb7140 100644 --- a/src/software/sensor_fusion/filter/ball_occlusion_test.cpp +++ b/src/software/sensor_fusion/filter/ball_occlusion_test.cpp @@ -29,7 +29,8 @@ TEST_P(BallOcclusionTest, test_ball_occlusion) setAiPlay(TbotsProto::PlayName::HaltPlay); setRefereeCommand(RefereeCommand::HALT, RefereeCommand::HALT); std::vector terminating_validating_function = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { + [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) + { while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) { yield("Timestamp not at 9.5s"); diff --git a/src/software/sensor_fusion/possession/possession_tracker.cpp b/src/software/sensor_fusion/possession/possession_tracker.cpp index 955a3439a3..01ed4454c2 100644 --- a/src/software/sensor_fusion/possession/possession_tracker.cpp +++ b/src/software/sensor_fusion/possession/possession_tracker.cpp @@ -48,9 +48,8 @@ TeamPossession PossessionTracker::getTeamWithPossession(const Team &friendly_tea auto enemy_team_robots = enemy_team.getAllRobotsExceptGoalie(); auto num_enemies_in_friendly_half = std::count_if(enemy_team_robots.begin(), enemy_team_robots.end(), - [&field](const auto &enemy) { - return field.pointInFriendlyHalf(enemy.position()); - }); + [&field](const auto &enemy) + { return field.pointInFriendlyHalf(enemy.position()); }); if (field.pointInFriendlyHalf(ball.position()) || num_enemies_in_friendly_half > 0) diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 261cbeb6a0..eee75ed483 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -395,7 +395,9 @@ void SensorFusion::updateDribbleDisplacement() std::transform(ball_contacts_by_friendly_robots.begin(), ball_contacts_by_friendly_robots.end(), - std::back_inserter(dribble_displacements), [&](const auto &kv_pair) { + std::back_inserter(dribble_displacements), + [&](const auto &kv_pair) + { const Point contact_point = kv_pair.second; return Segment(contact_point, ball->position()); }); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.cpp index 82b717d5c3..7e547d60eb 100644 --- a/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.cpp +++ b/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.cpp @@ -5,7 +5,8 @@ void robotNotExcessivelyDribbling(RobotId robot_id, std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - auto robot_has_ball_in_dribbler = [robot_id](std::shared_ptr world_ptr) { + auto robot_has_ball_in_dribbler = [robot_id](std::shared_ptr world_ptr) + { std::optional robot_optional = world_ptr->friendlyTeam().getRobotById(robot_id); CHECK(robot_optional.has_value()) diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.cpp index f42abfbcbc..7c32365c3d 100644 --- a/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.cpp +++ b/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.cpp @@ -6,12 +6,16 @@ void robotInCenterCircle(std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - auto robot_in_center_circle = [](std::shared_ptr world_ptr) { + auto robot_in_center_circle = [](std::shared_ptr world_ptr) + { std::vector robots = world_ptr->friendlyTeam().getAllRobots(); - return std::any_of(robots.begin(), robots.end(), [world_ptr](Robot robot) { - Point position = robot.position(); - return contains(world_ptr->field().centerCircle(), position); - }); + return std::any_of(robots.begin(), robots.end(), + [world_ptr](Robot robot) + { + Point position = robot.position(); + return contains(world_ptr->field().centerCircle(), + position); + }); }; while (!robot_in_center_circle(world_ptr)) diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.cpp index 617bc92db1..7203cafd6b 100644 --- a/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.cpp +++ b/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.cpp @@ -6,7 +6,8 @@ void robotInPolygon(Polygon polygon, int count, std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - auto num_robots_in_polygon = [polygon](std::shared_ptr world_ptr) { + auto num_robots_in_polygon = [polygon](std::shared_ptr world_ptr) + { std::vector robots = world_ptr->friendlyTeam().getAllRobots(); int total = 0; for (Robot robot : robots) diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.cpp index e30a0abd6b..43b03b1358 100644 --- a/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.cpp +++ b/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.cpp @@ -6,12 +6,15 @@ void robotReceivedBall(std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - auto ball_near_dribbler = [](std::shared_ptr world_ptr) { + auto ball_near_dribbler = [](std::shared_ptr world_ptr) + { std::vector robots = world_ptr->friendlyTeam().getAllRobots(); - return std::any_of(robots.begin(), robots.end(), [world_ptr](Robot robot) { - Point ball_position = world_ptr->ball().position(); - return robot.isNearDribbler(ball_position, 0.02); - }); + return std::any_of(robots.begin(), robots.end(), + [world_ptr](Robot robot) + { + Point ball_position = world_ptr->ball().position(); + return robot.isNearDribbler(ball_position, 0.02); + }); }; while (!ball_near_dribbler(world_ptr)) diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp index 4e247fd320..ad28599b70 100644 --- a/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp +++ b/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp @@ -8,7 +8,8 @@ void robotAtPosition(RobotId robot_id, std::shared_ptr world_ptr, { auto robot_is_not_at_destination = [robot_id, destination, close_to_destination_threshold]( - std::shared_ptr world_ptr) -> std::optional { + std::shared_ptr world_ptr) -> std::optional + { std::optional robot_optional = world_ptr->friendlyTeam().getRobotById(robot_id); CHECK(robot_optional.has_value()) @@ -40,7 +41,8 @@ void robotAtOrientation(RobotId robot_id, std::shared_ptr world_ptr, { auto robot_is_not_at_orientation = [robot_id, orientation, close_to_orientation_threshold]( - std::shared_ptr world_ptr) -> std::optional { + std::shared_ptr world_ptr) -> std::optional + { std::optional robot_optional = world_ptr->friendlyTeam().getRobotById(robot_id); CHECK(robot_optional.has_value()) @@ -74,7 +76,8 @@ void robotAtAngularVelocity(RobotId robot_id, std::shared_ptr world_ptr, { auto robot_is_not_at_angular_velocity = [robot_id, angular_velocity, close_to_angular_velocity_threshold]( - std::shared_ptr world_ptr) -> std::optional { + std::shared_ptr world_ptr) -> std::optional + { std::optional robot_optional = world_ptr->friendlyTeam().getRobotById(robot_id); CHECK(robot_optional.has_value()) diff --git a/src/software/simulated_tests/validation/non_terminating_function_validator_test.cpp b/src/software/simulated_tests/validation/non_terminating_function_validator_test.cpp index 8d0ece857e..6143a493e7 100644 --- a/src/software/simulated_tests/validation/non_terminating_function_validator_test.cpp +++ b/src/software/simulated_tests/validation/non_terminating_function_validator_test.cpp @@ -29,8 +29,9 @@ TEST(NonTerminatingFunctionValidatorTest, test_yielding_error_message) { // This validation_functions uses exceptions as a way for the test to observe it's // internal state The exception will not be reached until the 3rd function call - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { yield("This is an error message 1"); yield("This is an error message 2"); }; @@ -46,8 +47,9 @@ TEST(NonTerminatingFunctionValidatorTest, test_yielding_error_message) TEST(NonTerminatingFunctionValidatorTest, test_world_updated_correctly_between_validation_function_restarts) { - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { if (world->ball().position() != Point(1, 1)) { yield("Ball not at (1,1)"); diff --git a/src/software/simulated_tests/validation/terminating_function_validator_test.cpp b/src/software/simulated_tests/validation/terminating_function_validator_test.cpp index a877a2a637..65b958a0d0 100644 --- a/src/software/simulated_tests/validation/terminating_function_validator_test.cpp +++ b/src/software/simulated_tests/validation/terminating_function_validator_test.cpp @@ -23,8 +23,9 @@ TEST(TerminatingFunctionValidatorTest, TEST(TerminatingFunctionValidatorTest, test_validation_function_that_has_code_but_does_not_yield_reports_success) { - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { int foo = 0; int bar = 3; int baz = foo + bar; @@ -40,10 +41,9 @@ TEST(TerminatingFunctionValidatorTest, TEST(TerminatingFunctionValidatorTest, test_validation_function_that_yields_once_succeeds_on_the_second_execution) { - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { - yield("Test message"); - }; + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { yield("Test message"); }; auto world = ::TestUtil::createBlankTestingWorld(); TerminatingFunctionValidator function_validator(validation_function, world); @@ -57,8 +57,9 @@ TEST(TerminatingFunctionValidatorTest, TEST(TerminatingFunctionValidatorTest, test_validation_function_that_yields_five_time_succeeds_on_the_sixth_execution) { - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { yield("Test message"); yield("Test message"); yield("Test message"); @@ -83,8 +84,9 @@ TEST(TerminatingFunctionValidatorTest, TEST(TerminatingFunctionValidatorTest, test_validation_function_with_early_return_reports_success_after_return) { - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { yield("First validation not done yet"); return; yield("Second validation not done yet"); @@ -104,8 +106,9 @@ TEST(TerminatingFunctionValidatorTest, TEST(TerminatingFunctionValidatorTest, test_validation_function_with_single_loop_succeeds_after_loop_termination) { - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { while (world->ball().position().x() < 0) { yield("The ball's x position is not less than 0"); @@ -140,8 +143,9 @@ TEST(TerminatingFunctionValidatorTest, { // This validation function will only pass if the ball's x-coordinate becomes positive // before the ball's y-coordinate becomes positive - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { while (world->ball().position().x() < 0) { yield("The ball's x position is not less than 0"); @@ -180,8 +184,9 @@ TEST(TerminatingFunctionValidatorTest, test_validation_function_error_message) { // This shows an example of using GoogleTest statements within a validation function. // Just like regular unit tests, if the condition is not met the test will fail. - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) { + ValidationFunction validation_function = + [](std::shared_ptr world, ValidationCoroutine::push_type& yield) + { while (world->gameState().isStopped()) { yield("Test message"); diff --git a/src/software/test_util/equal_within_tolerance.cpp b/src/software/test_util/equal_within_tolerance.cpp index d5012c190a..368c854fbf 100644 --- a/src/software/test_util/equal_within_tolerance.cpp +++ b/src/software/test_util/equal_within_tolerance.cpp @@ -27,9 +27,8 @@ namespace TestUtil auto ppts1 = poly1.getPoints(); auto ppts2 = poly2.getPoints(); if (std::equal(ppts1.begin(), ppts1.end(), ppts2.begin(), - [tolerance](const Point &p1, const Point &p2) { - return equalWithinTolerance(p1, p2, tolerance); - })) + [tolerance](const Point &p1, const Point &p2) + { return equalWithinTolerance(p1, p2, tolerance); })) { return ::testing::AssertionSuccess(); } diff --git a/src/software/uart/mock_uart_communication.h b/src/software/uart/mock_uart_communication.h index 7ca3d6052b..dfa9f49028 100644 --- a/src/software/uart/mock_uart_communication.h +++ b/src/software/uart/mock_uart_communication.h @@ -10,8 +10,8 @@ class MockUart : public UartCommunication { public: - MockUart(){}; - ~MockUart(){}; + MockUart() {}; + ~MockUart() {}; MOCK_METHOD1(serialWrite, bool(const std::vector &write_val)); MOCK_METHOD1(serialRead, std::vector(size_t num_read_bytes)); MOCK_METHOD1(flushSerialPort, bool(FlushType flush_type)); diff --git a/src/software/unix_full_system_main.cpp b/src/software/unix_full_system_main.cpp index 6c59bde203..8b45750ac9 100644 --- a/src/software/unix_full_system_main.cpp +++ b/src/software/unix_full_system_main.cpp @@ -95,7 +95,8 @@ int main(int argc, char** argv) if (!args.ci) { // Return the current time since epoch in seconds - time_provider = []() { + time_provider = []() + { return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()) .count(); @@ -121,8 +122,8 @@ int main(int argc, char** argv) if (args.ci) { // Update the time provider for ProtoLogger - proto_logger->updateTimeProvider( - [&backend]() { return backend->getLastWorldTimeSec(); }); + proto_logger->updateTimeProvider([&backend]() + { return backend->getLastWorldTimeSec(); }); } auto sensor_fusion = @@ -133,9 +134,8 @@ int main(int argc, char** argv) auto tactic_override_listener = ThreadedProtoUnixListener( args.runtime_dir + TACTIC_OVERRIDE_PATH, - [&ai](TbotsProto::AssignedTacticPlayControlParams input) { - ai->overrideTactics(input); - }); + [&ai](TbotsProto::AssignedTacticPlayControlParams input) + { ai->overrideTactics(input); }); auto play_override_listener = ThreadedProtoUnixListener( args.runtime_dir + PLAY_OVERRIDE_PATH, diff --git a/src/software/util/generic_factory/generic_factory.h b/src/software/util/generic_factory/generic_factory.h index f1a4698f8f..2cadcc7111 100644 --- a/src/software/util/generic_factory/generic_factory.h +++ b/src/software/util/generic_factory/generic_factory.h @@ -117,9 +117,8 @@ class TGenericFactory : public GenericFactory std::unique_ptr { - return std::make_unique(config); - }; + [](const ConfigType config) -> std::unique_ptr + { return std::make_unique(config); }; GenericFactory::registerCreator( TYPENAME(T), generic_creator); } diff --git a/src/software/util/sml_fsm/sml_fsm.h b/src/software/util/sml_fsm/sml_fsm.h index 3fcf1ff006..dafb5e7d87 100644 --- a/src/software/util/sml_fsm/sml_fsm.h +++ b/src/software/util/sml_fsm/sml_fsm.h @@ -54,10 +54,9 @@ using FSM = boost::sml::sm>; * @param SUB_FSM The sub fsm to update */ #define DEFINE_SML_SUB_FSM_UPDATE_ACTION(FUNCTION, SUB_FSM) \ - const auto FUNCTION##_A = [this](auto event, \ - back::process processEvent) { \ - FUNCTION(event, processEvent); \ - }; + const auto FUNCTION##_A = \ + [this](auto event, back::process processEvent) \ + { FUNCTION(event, processEvent); }; /** * Strips extraneous information such as boost::sml template information to return @@ -81,10 +80,12 @@ template std::string getCurrentStateName(const SM& state_machine) { std::string name; - state_machine.visit_current_states([&name](const auto& state) { - name = stripFSMState(TYPENAME( - boost::sml::back::policies::get_state_name_t>)); - }); + state_machine.visit_current_states( + [&name](const auto& state) + { + name = stripFSMState(TYPENAME(boost::sml::back::policies::get_state_name_t< + std::decay_t>)); + }); return name; } @@ -122,20 +123,22 @@ template std::string getCurrentSubStateName(const SM& state_machine) { std::string name; - state_machine.template visit_current_states([&name, - &state_machine](const auto& state) { - name = stripFSMState(TYPENAME( - boost::sml::back::policies::get_state_name_t>)); - using state_repr_t = std::decay_t; - using state_t = typename state_repr_t::type; - if constexpr (is_sub_state_machine::value) + state_machine.template visit_current_states( + [&name, &state_machine](const auto& state) { - using state_machine_t = typename state_machine_impl::type; - name += "."; - name += getCurrentSubStateName)>( - state_machine); - } - }); + name = stripFSMState(TYPENAME(boost::sml::back::policies::get_state_name_t< + std::decay_t>)); + using state_repr_t = std::decay_t; + using state_t = typename state_repr_t::type; + if constexpr (is_sub_state_machine::value) + { + using state_machine_t = typename state_machine_impl::type; + name += "."; + name += + getCurrentSubStateName)>( + state_machine); + } + }); return name; } @@ -151,17 +154,20 @@ template std::string getCurrentFullStateName(const SM& state_machine) { std::string name; - state_machine.visit_current_states([&name, &state_machine](const auto& state) { - name += getCurrentStateName(state_machine); - using state_repr_t = std::decay_t; - using state_t = typename state_repr_t::type; - if constexpr (is_sub_state_machine::value) + state_machine.visit_current_states( + [&name, &state_machine](const auto& state) { - using state_machine_t = typename state_machine_impl::type; - name += "."; - name += getCurrentSubStateName)>( - state_machine); - } - }); + name += getCurrentStateName(state_machine); + using state_repr_t = std::decay_t; + using state_t = typename state_repr_t::type; + if constexpr (is_sub_state_machine::value) + { + using state_machine_t = typename state_machine_impl::type; + name += "."; + name += + getCurrentSubStateName)>( + state_machine); + } + }); return name; } diff --git a/src/software/world/world.cpp b/src/software/world/world.cpp index df874fd7fa..d0b2558887 100644 --- a/src/software/world/world.cpp +++ b/src/software/world/world.cpp @@ -84,9 +84,8 @@ void World::updateRefereeCommand(const RefereeCommand &command) // Take the consensus of the previous referee messages if (!referee_command_history_.empty() && std::all_of(referee_command_history_.begin(), referee_command_history_.end(), - [&](auto game_state) { - return game_state == referee_command_history_.front(); - })) + [&](auto game_state) + { return game_state == referee_command_history_.front(); })) { current_game_state_.updateRefereeCommand(command); } From aab1eed17df2d9c89158b9b8f8f7ece66541455f Mon Sep 17 00:00:00 2001 From: arun Date: Sat, 1 Mar 2025 23:22:33 -0800 Subject: [PATCH 09/71] wip --- environment_setup/setup_software.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/environment_setup/setup_software.sh b/environment_setup/setup_software.sh index 18f26dca57..254283cbc3 100755 --- a/environment_setup/setup_software.sh +++ b/environment_setup/setup_software.sh @@ -95,7 +95,7 @@ if [[ $(lsb_release -rs) == "20.04" ]]; then fi # Clear the download cache -rm -rf /tmp/tbots_download_cache +sudo rm -rf /tmp/tbots_download_cache mkdir /tmp/tbots_download_cache if [[ $(lsb_release -rs) == "22.04" ]] || [[ $(lsb_release -rs) == "24.04" ]]; then From 71ae4dad68150cb2f11e6c825f08b4312b4d7da7 Mon Sep 17 00:00:00 2001 From: Arun Date: Sat, 15 Mar 2025 10:48:23 -0700 Subject: [PATCH 10/71] Update cross-compiler compilation paths --- environment_setup/util.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/environment_setup/util.sh b/environment_setup/util.sh index 72012e9ec4..7a60294ae3 100755 --- a/environment_setup/util.sh +++ b/environment_setup/util.sh @@ -46,7 +46,7 @@ install_cross_compiler() { file_name=aarch64-tbots-linux-gnu-for-x86 fi full_file_name=$file_name.tar.xz - wget https://raw.githubusercontent.com/UBC-Thunderbots/Software-External-Dependencies/refs/heads/tbots_compiler/toolchain/$full_file_name -O /tmp/tbots_download_cache/$full_file_name + wget https://raw.githubusercontent.com/UBC-Thunderbots/Software-External-Dependencies/refs/heads/main/toolchain/$full_file_name -O /tmp/tbots_download_cache/$full_file_name tar -xf /tmp/tbots_download_cache/$full_file_name -C /tmp/tbots_download_cache/ sudo mv /tmp/tbots_download_cache/aarch64-tbots-linux-gnu /opt/tbotspython rm /tmp/tbots_download_cache/$full_file_name From 6dea99156d160607d3c9a4bd8cc21338c9a62953 Mon Sep 17 00:00:00 2001 From: Arun Date: Sat, 15 Mar 2025 11:11:25 -0700 Subject: [PATCH 11/71] delete my local computer config --- src/.vimrc | 1 - 1 file changed, 1 deletion(-) delete mode 100644 src/.vimrc diff --git a/src/.vimrc b/src/.vimrc deleted file mode 100644 index 6e1fa11c06..0000000000 --- a/src/.vimrc +++ /dev/null @@ -1 +0,0 @@ -let $FZF_DEFAULT_COMMAND='find . \( -name .vscode -o -name .git -o -name bazel-bin -o -name bazel-out -o -name bazel-src -o -name bazel-testlogs -o -name cc-toolchain -o -name "*.swp" \) -prune -o -print' From 6ebef9cc4f5e6ef91e0a530a2db8b48dff2b677f Mon Sep 17 00:00:00 2001 From: arun Date: Wed, 19 Mar 2025 04:37:52 -0700 Subject: [PATCH 12/71] fix runtime issues with robot_info and robot_view widgets --- .../robot_diagnostics/robot_info.py | 25 +++++++++++++------ .../robot_diagnostics/robot_view.py | 2 +- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/robot_info.py b/src/software/thunderscope/robot_diagnostics/robot_info.py index 568c76fef1..24550863e8 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_info.py +++ b/src/software/thunderscope/robot_diagnostics/robot_info.py @@ -185,6 +185,10 @@ def __init__( self.layout.addLayout(self.status_layout) self.setLayout(self.layout) + # Last robot state + self.last_robot_status = None + self.last_robot_statistic = None + def create_robot_status_expand_button(self) -> QPushButton: """Creates the button to expand / collapse the robot status view @@ -292,24 +296,30 @@ def create_vision_pattern( return pixmap - def update(self, robot_status: RobotStatus, robot_statistic: RobotStatistic): - """Receives parts of a RobotStatus message + def update_robot_status(self, robot_status: RobotStatus): + """Receives a RobotStatus message Saves the current time as the last robot status time Sets the robot UI as connected and updates the UI Then sets a timer callback to disconnect the robot if needed :param robot_status: The robot status message for this robot - :param robot_statistic: The round trip time proto for this robot's message """ self.time_of_last_robot_status = time.time() self.robot_model.setPixmap(self.color_vision_pattern) - self.__update_ui(robot_status, robot_statistic) + self.last_robot_status = robot_status + + self.__update_ui() QtCore.QTimer.singleShot(int(DISCONNECT_DURATION_MS), self.disconnect_robot) + def update_rtt(self, robot_statistic: RobotStatistic): + self.last_robot_statistic = robot_statistic + + self.__update_ui() + def disconnect_robot(self) -> None: """Calculates the time between the last robot status and now If more than our threshold, resets UI @@ -338,7 +348,7 @@ def __update_stop_primitive(self, is_running: bool) -> None: ) def __update_ui( - self, robot_status: RobotStatus, robot_statistic: RobotStatistic + self ) -> None: """Receives important sections of RobotStatus proto for this robot and updates widget with alerts Checks for @@ -347,10 +357,9 @@ def __update_ui( - Battery voltage, and warns if it's too low - If this robot has errors - If the robot is stopped or running - - :param robot_status: The robot status message for this robot - :param robot_statistic: The round trip time message for this robot """ + if not self.last_robot_status or not self.last_robot_statistic: + return motor_status = robot_status.motor_status power_status = robot_status.power_status network_status = robot_status.network_status diff --git a/src/software/thunderscope/robot_diagnostics/robot_view.py b/src/software/thunderscope/robot_diagnostics/robot_view.py index 6f3aa335a9..c067ae5d5b 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_view.py +++ b/src/software/thunderscope/robot_diagnostics/robot_view.py @@ -71,7 +71,7 @@ def update( :param robot_statistic : robot statistic proto to update with new metrics """ if robot_status is not None: - self.robot_info.update(robot_status, robot_statistic) + self.robot_info.update_robot_status(robot_status) if self.robot_status: self.robot_status.update(robot_status) From 02d6ecfa9efaafac47670faf44dd3b8f506b983a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Wed, 19 Mar 2025 11:48:55 +0000 Subject: [PATCH 13/71] [pre-commit.ci lite] apply automatic fixes --- src/software/thunderscope/robot_diagnostics/robot_info.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/robot_info.py b/src/software/thunderscope/robot_diagnostics/robot_info.py index 24550863e8..8a631be5ca 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_info.py +++ b/src/software/thunderscope/robot_diagnostics/robot_info.py @@ -347,9 +347,7 @@ def __update_stop_primitive(self, is_running: bool) -> None: f"background-color: {'green' if is_running else 'red'}; border: 1px solid black;" ) - def __update_ui( - self - ) -> None: + def __update_ui(self) -> None: """Receives important sections of RobotStatus proto for this robot and updates widget with alerts Checks for - Whether breakbeam is tripped From e8dc7468907e2488a3ac75c4014a28b614b0ea67 Mon Sep 17 00:00:00 2001 From: Arun Date: Thu, 27 Mar 2025 03:33:37 -0700 Subject: [PATCH 14/71] delete crosstool-ng config --- src/cc_toolchain/crosstool-ng/.config | 932 -------------------------- 1 file changed, 932 deletions(-) delete mode 100644 src/cc_toolchain/crosstool-ng/.config diff --git a/src/cc_toolchain/crosstool-ng/.config b/src/cc_toolchain/crosstool-ng/.config deleted file mode 100644 index d319dd851c..0000000000 --- a/src/cc_toolchain/crosstool-ng/.config +++ /dev/null @@ -1,932 +0,0 @@ -# -# Automatically generated file; DO NOT EDIT. -# crosstool-NG 1.26.0 Configuration -# -CT_CONFIGURE_has_static_link=y -CT_CONFIGURE_has_cxx11=y -CT_CONFIGURE_has_wget=y -CT_CONFIGURE_has_curl=y -CT_CONFIGURE_has_rsync=y -CT_CONFIGURE_has_make_3_81_or_newer=y -CT_CONFIGURE_has_make_4_0_or_newer=y -CT_CONFIGURE_has_libtool_2_4_or_newer=y -CT_CONFIGURE_has_libtoolize_2_4_or_newer=y -CT_CONFIGURE_has_autoconf_2_65_or_newer=y -CT_CONFIGURE_has_autoreconf_2_65_or_newer=y -CT_CONFIGURE_has_automake_1_15_or_newer=y -CT_CONFIGURE_has_gnu_m4_1_4_12_or_newer=y -CT_CONFIGURE_has_python_3_4_or_newer=y -CT_CONFIGURE_has_bison_2_7_or_newer=y -CT_CONFIGURE_has_bison_3_0_4_or_newer=y -CT_CONFIGURE_has_python=y -CT_CONFIGURE_has_git=y -CT_CONFIGURE_has_md5sum=y -CT_CONFIGURE_has_sha1sum=y -CT_CONFIGURE_has_sha256sum=y -CT_CONFIGURE_has_sha512sum=y -CT_CONFIGURE_has_install_with_strip_program=y -CT_VERSION="1.26.0" -CT_VCHECK="" -CT_CONFIG_VERSION_ENV="4" -CT_CONFIG_VERSION_CURRENT="4" -CT_CONFIG_VERSION="4" -CT_MODULES=y - -# -# Paths and misc options -# - -# -# crosstool-NG behavior -# -# CT_OBSOLETE is not set -# CT_EXPERIMENTAL is not set -# CT_DEBUG_CT is not set - -# -# Paths -# -CT_LOCAL_TARBALLS_DIR="${HOME}/src" -CT_SAVE_TARBALLS=y -# CT_TARBALLS_BUILDROOT_LAYOUT is not set -CT_WORK_DIR="${CT_TOP_DIR}/.build" -CT_BUILD_TOP_DIR="${CT_WORK_DIR:-${CT_TOP_DIR}/.build}/${CT_HOST:+HOST-${CT_HOST}/}${CT_TARGET}" -CT_BUILD_DIR="${CT_BUILD_TOP_DIR}/build" -CT_PREFIX_DIR="${CT_PREFIX:-${HOME}/x-tools}/${CT_HOST:+HOST-${CT_HOST}/}${CT_TARGET}" -CT_RM_RF_PREFIX_DIR=y -CT_REMOVE_DOCS=y -CT_INSTALL_LICENSES=y -CT_PREFIX_DIR_RO=y -CT_STRIP_HOST_TOOLCHAIN_EXECUTABLES=y -# CT_STRIP_TARGET_TOOLCHAIN_EXECUTABLES is not set - -# -# Downloading -# -CT_DOWNLOAD_AGENT_WGET=y -# CT_DOWNLOAD_AGENT_CURL is not set -# CT_DOWNLOAD_AGENT_NONE is not set -# CT_FORBID_DOWNLOAD is not set -# CT_FORCE_DOWNLOAD is not set -CT_CONNECT_TIMEOUT=10 -CT_DOWNLOAD_WGET_OPTIONS="--passive-ftp --tries=3 -nc --progress=dot:binary" -# CT_ONLY_DOWNLOAD is not set -# CT_USE_MIRROR is not set -CT_VERIFY_DOWNLOAD_DIGEST=y -CT_VERIFY_DOWNLOAD_DIGEST_SHA512=y -# CT_VERIFY_DOWNLOAD_DIGEST_SHA256 is not set -# CT_VERIFY_DOWNLOAD_DIGEST_SHA1 is not set -# CT_VERIFY_DOWNLOAD_DIGEST_MD5 is not set -CT_VERIFY_DOWNLOAD_DIGEST_ALG="sha512" -# CT_VERIFY_DOWNLOAD_SIGNATURE is not set - -# -# Extracting -# -# CT_FORCE_EXTRACT is not set -CT_OVERRIDE_CONFIG_GUESS_SUB=y -# CT_ONLY_EXTRACT is not set -CT_PATCH_BUNDLED=y -# CT_PATCH_BUNDLED_LOCAL is not set -CT_PATCH_ORDER="bundled" - -# -# Build behavior -# -CT_PARALLEL_JOBS=0 -CT_LOAD="" -CT_USE_PIPES=y -CT_EXTRA_CFLAGS_FOR_BUILD="" -CT_EXTRA_CXXFLAGS_FOR_BUILD="" -CT_EXTRA_LDFLAGS_FOR_BUILD="" -CT_EXTRA_CFLAGS_FOR_HOST="" -CT_EXTRA_LDFLAGS_FOR_HOST="" -# CT_CONFIG_SHELL_SH is not set -# CT_CONFIG_SHELL_ASH is not set -CT_CONFIG_SHELL_BASH=y -# CT_CONFIG_SHELL_CUSTOM is not set -CT_CONFIG_SHELL="${bash}" - -# -# Logging -# -# CT_LOG_ERROR is not set -# CT_LOG_WARN is not set -# CT_LOG_INFO is not set -CT_LOG_EXTRA=y -# CT_LOG_ALL is not set -# CT_LOG_DEBUG is not set -CT_LOG_LEVEL_MAX="EXTRA" -# CT_LOG_SEE_TOOLS_WARN is not set -CT_LOG_PROGRESS_BAR=y -CT_LOG_TO_FILE=y -CT_LOG_FILE_COMPRESS=y -# end of Paths and misc options - -# -# Target options -# -# CT_ARCH_ALPHA is not set -# CT_ARCH_ARC is not set -CT_ARCH_ARM=y -# CT_ARCH_AVR is not set -# CT_ARCH_BPF is not set -# CT_ARCH_M68K is not set -# CT_ARCH_MIPS is not set -# CT_ARCH_NIOS2 is not set -# CT_ARCH_POWERPC is not set -# CT_ARCH_PRU is not set -# CT_ARCH_S390 is not set -# CT_ARCH_SH is not set -# CT_ARCH_SPARC is not set -# CT_ARCH_X86 is not set -# CT_ARCH_XTENSA is not set -CT_ARCH="arm" -CT_ARCH_CHOICE_KSYM="ARM" -CT_ARCH_CPU="" -CT_ARCH_TUNE="" -CT_ARCH_ARM_SHOW=y - -# -# Options for arm -# -CT_ARCH_ARM_PKG_KSYM="" -CT_ALL_ARCH_CHOICES="ALPHA ARC ARM AVR BPF C6X LOONGARCH M68K MICROBLAZE MIPS MOXIE MSP430 NIOS2 POWERPC PRU RISCV S390 SH SPARC X86 XTENSA" -CT_ARCH_SUFFIX="" -# CT_OMIT_TARGET_VENDOR is not set - -# -# Generic target options -# -# CT_MULTILIB is not set -CT_DEMULTILIB=y -CT_ARCH_SUPPORTS_BOTH_MMU=y -CT_ARCH_DEFAULT_HAS_MMU=y -CT_ARCH_USE_MMU=y -CT_ARCH_SUPPORTS_FLAT_FORMAT=y -CT_ARCH_SUPPORTS_LIBSANITIZER=y -CT_ARCH_SUPPORTS_EITHER_ENDIAN=y -CT_ARCH_DEFAULT_LE=y -# CT_ARCH_BE is not set -CT_ARCH_LE=y -CT_ARCH_ENDIAN="little" -CT_ARCH_SUPPORTS_32=y -CT_ARCH_SUPPORTS_64=y -CT_ARCH_DEFAULT_32=y -CT_ARCH_BITNESS=64 -# CT_ARCH_32 is not set -CT_ARCH_64=y - -# -# Target optimisations -# -CT_ARCH_SUPPORTS_WITH_ARCH=y -CT_ARCH_SUPPORTS_WITH_CPU=y -CT_ARCH_SUPPORTS_WITH_TUNE=y -CT_ARCH_EXCLUSIVE_WITH_CPU=y -CT_ARCH_ARCH="" -CT_TARGET_CFLAGS="" -CT_TARGET_LDFLAGS="" -# end of Target options - -# -# Toolchain options -# - -# -# General toolchain options -# -CT_FORCE_SYSROOT=y -CT_USE_SYSROOT=y -CT_SYSROOT_NAME="sysroot" -CT_SYSROOT_DIR_PREFIX="" -CT_WANTS_STATIC_LINK=y -CT_WANTS_STATIC_LINK_CXX=y -# CT_STATIC_TOOLCHAIN is not set -CT_SHOW_CT_VERSION=y -CT_TOOLCHAIN_PKGVERSION="" -CT_TOOLCHAIN_BUGURL="" - -# -# Tuple completion and aliasing -# -CT_TARGET_VENDOR="tbots" -CT_TARGET_ALIAS_SED_EXPR="" -CT_TARGET_ALIAS="" - -# -# Toolchain type -# -CT_CROSS=y -# CT_CANADIAN is not set -CT_TOOLCHAIN_TYPE="cross" - -# -# Build system -# -CT_BUILD="" -CT_BUILD_PREFIX="" -CT_BUILD_SUFFIX="" - -# -# Misc options -# -# CT_TOOLCHAIN_ENABLE_NLS is not set -# end of Toolchain options - -# -# Operating System -# -CT_KERNEL_SUPPORTS_SHARED_LIBS=y -# CT_KERNEL_BARE_METAL is not set -CT_KERNEL_LINUX=y -CT_KERNEL="linux" -CT_KERNEL_CHOICE_KSYM="LINUX" -CT_KERNEL_LINUX_SHOW=y - -# -# Options for linux -# -CT_KERNEL_LINUX_PKG_KSYM="LINUX" -CT_LINUX_DIR_NAME="linux" -CT_LINUX_USE_WWW_KERNEL_ORG=y -# CT_LINUX_USE_ORACLE is not set -CT_LINUX_USE="LINUX" -CT_LINUX_PKG_NAME="linux" -CT_LINUX_SRC_RELEASE=y -# CT_LINUX_SRC_DEVEL is not set -CT_LINUX_PATCH_ORDER="global" -# CT_LINUX_V_6_4 is not set -# CT_LINUX_V_6_3 is not set -# CT_LINUX_V_6_2 is not set -# CT_LINUX_V_6_1 is not set -# CT_LINUX_V_6_0 is not set -# CT_LINUX_V_5_19 is not set -# CT_LINUX_V_5_18 is not set -# CT_LINUX_V_5_17 is not set -# CT_LINUX_V_5_16 is not set -# CT_LINUX_V_5_15 is not set -# CT_LINUX_V_5_14 is not set -# CT_LINUX_V_5_13 is not set -# CT_LINUX_V_5_12 is not set -# CT_LINUX_V_5_11 is not set -# CT_LINUX_V_5_10 is not set -# CT_LINUX_V_5_9 is not set -# CT_LINUX_V_5_8 is not set -# CT_LINUX_V_5_7 is not set -# CT_LINUX_V_5_4 is not set -# CT_LINUX_V_5_3 is not set -# CT_LINUX_V_5_2 is not set -# CT_LINUX_V_5_1 is not set -# CT_LINUX_V_5_0 is not set -# CT_LINUX_V_4_20 is not set -# CT_LINUX_V_4_19 is not set -# CT_LINUX_V_4_18 is not set -# CT_LINUX_V_4_17 is not set -# CT_LINUX_V_4_16 is not set -# CT_LINUX_V_4_15 is not set -# CT_LINUX_V_4_14 is not set -# CT_LINUX_V_4_13 is not set -# CT_LINUX_V_4_12 is not set -# CT_LINUX_V_4_11 is not set -# CT_LINUX_V_4_10 is not set -CT_LINUX_V_4_9=y -# CT_LINUX_V_4_4 is not set -# CT_LINUX_V_4_1 is not set -# CT_LINUX_V_3_16 is not set -# CT_LINUX_V_3_13 is not set -# CT_LINUX_V_3_12 is not set -# CT_LINUX_V_3_10 is not set -CT_LINUX_VERSION="4.9.335" -CT_LINUX_MIRRORS="$(CT_Mirrors kernel.org linux ${CT_LINUX_VERSION})" -CT_LINUX_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_LINUX_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_LINUX_ARCHIVE_FORMATS=".tar.xz .tar.gz" -CT_LINUX_SIGNATURE_FORMAT="unpacked/.sign" -CT_LINUX_5_19_or_older=y -CT_LINUX_older_than_5_19=y -CT_LINUX_5_12_or_older=y -CT_LINUX_older_than_5_12=y -CT_LINUX_5_5_or_older=y -CT_LINUX_older_than_5_5=y -CT_LINUX_5_3_or_older=y -CT_LINUX_older_than_5_3=y -CT_LINUX_later_than_4_8=y -CT_LINUX_4_8_or_later=y -CT_LINUX_later_than_3_7=y -CT_LINUX_3_7_or_later=y -CT_LINUX_REQUIRE_3_7_or_later=y -CT_LINUX_later_than_3_2=y -CT_LINUX_3_2_or_later=y -CT_LINUX_REQUIRE_3_2_or_later=y -CT_KERNEL_DEP_RSYNC=y -CT_KERNEL_LINUX_VERBOSITY_0=y -# CT_KERNEL_LINUX_VERBOSITY_1 is not set -# CT_KERNEL_LINUX_VERBOSITY_2 is not set -CT_KERNEL_LINUX_VERBOSE_LEVEL=0 -CT_KERNEL_LINUX_INSTALL_CHECK=y -CT_ALL_KERNEL_CHOICES="BARE_METAL LINUX WINDOWS" - -# -# Common kernel options -# -CT_SHARED_LIBS=y -# end of Operating System - -# -# Binary utilities -# -CT_ARCH_BINFMT_ELF=y -CT_BINUTILS_BINUTILS=y -CT_BINUTILS="binutils" -CT_BINUTILS_CHOICE_KSYM="BINUTILS" -CT_BINUTILS_BINUTILS_SHOW=y - -# -# Options for binutils -# -CT_BINUTILS_BINUTILS_PKG_KSYM="BINUTILS" -CT_BINUTILS_DIR_NAME="binutils" -CT_BINUTILS_USE_GNU=y -# CT_BINUTILS_USE_ORACLE is not set -CT_BINUTILS_USE="BINUTILS" -CT_BINUTILS_PKG_NAME="binutils" -CT_BINUTILS_SRC_RELEASE=y -# CT_BINUTILS_SRC_DEVEL is not set -CT_BINUTILS_PATCH_ORDER="global" -# CT_BINUTILS_V_2_40 is not set -# CT_BINUTILS_V_2_39 is not set -# CT_BINUTILS_V_2_38 is not set -# CT_BINUTILS_V_2_37 is not set -# CT_BINUTILS_V_2_36 is not set -# CT_BINUTILS_V_2_35 is not set -# CT_BINUTILS_V_2_34 is not set -# CT_BINUTILS_V_2_33 is not set -# CT_BINUTILS_V_2_32 is not set -# CT_BINUTILS_V_2_31 is not set -# CT_BINUTILS_V_2_30 is not set -# CT_BINUTILS_V_2_29 is not set -# CT_BINUTILS_V_2_28 is not set -CT_BINUTILS_V_2_27=y -# CT_BINUTILS_V_2_26 is not set -CT_BINUTILS_VERSION="2.27" -CT_BINUTILS_MIRRORS="$(CT_Mirrors GNU binutils) $(CT_Mirrors sourceware binutils/releases)" -CT_BINUTILS_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_BINUTILS_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_BINUTILS_ARCHIVE_FORMATS=".tar.bz2 .tar.gz" -CT_BINUTILS_SIGNATURE_FORMAT="packed/.sig" -CT_BINUTILS_2_39_or_older=y -CT_BINUTILS_older_than_2_39=y -CT_BINUTILS_2_30_or_older=y -CT_BINUTILS_older_than_2_30=y -CT_BINUTILS_2_27_or_later=y -CT_BINUTILS_2_27_or_older=y -CT_BINUTILS_later_than_2_26=y -CT_BINUTILS_2_26_or_later=y - -# -# GNU binutils -# -CT_BINUTILS_GOLD_SUPPORTS_ARCH=y -CT_BINUTILS_GOLD_SUPPORT=y -CT_BINUTILS_FORCE_LD_BFD_DEFAULT=y -# CT_BINUTILS_LINKER_LD is not set -CT_BINUTILS_LINKER_LD_GOLD=y -CT_BINUTILS_GOLD_INSTALLED=y -CT_BINUTILS_GOLD_THREADS=y -CT_BINUTILS_LINKER_BOTH=y -CT_BINUTILS_LINKERS_LIST="ld,gold" -CT_BINUTILS_LD_WRAPPER=y -CT_BINUTILS_LINKER_DEFAULT="bfd" -CT_BINUTILS_PLUGINS=y -CT_BINUTILS_RELRO=m -CT_BINUTILS_DETERMINISTIC_ARCHIVES=y -CT_BINUTILS_EXTRA_CONFIG_ARRAY="" -# CT_BINUTILS_FOR_TARGET is not set -CT_ALL_BINUTILS_CHOICES="BINUTILS" -# end of Binary utilities - -# -# C-library -# -CT_LIBC_GLIBC=y -# CT_LIBC_UCLIBC_NG is not set -CT_LIBC="glibc" -CT_LIBC_CHOICE_KSYM="GLIBC" -CT_LIBC_GLIBC_SHOW=y - -# -# Options for glibc -# -CT_LIBC_GLIBC_PKG_KSYM="GLIBC" -CT_GLIBC_DIR_NAME="glibc" -CT_GLIBC_USE_GNU=y -# CT_GLIBC_USE_ORACLE is not set -CT_GLIBC_USE="GLIBC" -CT_GLIBC_PKG_NAME="glibc" -CT_GLIBC_SRC_RELEASE=y -# CT_GLIBC_SRC_DEVEL is not set -CT_GLIBC_PATCH_ORDER="global" -# CT_GLIBC_V_2_38 is not set -# CT_GLIBC_V_2_37 is not set -# CT_GLIBC_V_2_36 is not set -# CT_GLIBC_V_2_35 is not set -# CT_GLIBC_V_2_34 is not set -# CT_GLIBC_V_2_33 is not set -# CT_GLIBC_V_2_32 is not set -# CT_GLIBC_V_2_31 is not set -# CT_GLIBC_V_2_30 is not set -# CT_GLIBC_V_2_29 is not set -# CT_GLIBC_V_2_28 is not set -CT_GLIBC_V_2_27=y -# CT_GLIBC_V_2_26 is not set -# CT_GLIBC_V_2_25 is not set -# CT_GLIBC_V_2_24 is not set -# CT_GLIBC_V_2_23 is not set -# CT_GLIBC_V_2_19 is not set -# CT_GLIBC_V_2_17 is not set -CT_GLIBC_VERSION="2.27" -CT_GLIBC_MIRRORS="$(CT_Mirrors GNU glibc)" -CT_GLIBC_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_GLIBC_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_GLIBC_ARCHIVE_FORMATS=".tar.xz .tar.bz2 .tar.gz" -CT_GLIBC_SIGNATURE_FORMAT="packed/.sig" -CT_GLIBC_2_38_or_older=y -CT_GLIBC_older_than_2_38=y -CT_GLIBC_2_37_or_older=y -CT_GLIBC_older_than_2_37=y -CT_GLIBC_2_36_or_older=y -CT_GLIBC_older_than_2_36=y -CT_GLIBC_2_34_or_older=y -CT_GLIBC_older_than_2_34=y -CT_GLIBC_2_32_or_older=y -CT_GLIBC_older_than_2_32=y -CT_GLIBC_2_31_or_older=y -CT_GLIBC_older_than_2_31=y -CT_GLIBC_2_30_or_older=y -CT_GLIBC_older_than_2_30=y -CT_GLIBC_2_29_or_older=y -CT_GLIBC_older_than_2_29=y -CT_GLIBC_2_28_or_older=y -CT_GLIBC_older_than_2_28=y -CT_GLIBC_2_27_or_later=y -CT_GLIBC_2_27_or_older=y -CT_GLIBC_later_than_2_26=y -CT_GLIBC_2_26_or_later=y -CT_GLIBC_later_than_2_25=y -CT_GLIBC_2_25_or_later=y -CT_GLIBC_later_than_2_24=y -CT_GLIBC_2_24_or_later=y -CT_GLIBC_later_than_2_23=y -CT_GLIBC_2_23_or_later=y -CT_GLIBC_later_than_2_20=y -CT_GLIBC_2_20_or_later=y -CT_GLIBC_later_than_2_17=y -CT_GLIBC_2_17_or_later=y -CT_GLIBC_later_than_2_14=y -CT_GLIBC_2_14_or_later=y -CT_GLIBC_DEP_KERNEL_HEADERS_VERSION=y -CT_GLIBC_DEP_BINUTILS=y -CT_GLIBC_DEP_GCC=y -CT_GLIBC_DEP_PYTHON=y -CT_GLIBC_SPARC_ALLOW_V7=y -CT_THREADS="nptl" -CT_GLIBC_BUILD_SSP=y -CT_GLIBC_HAS_LIBIDN_ADDON=y -# CT_GLIBC_USE_LIBIDN_ADDON is not set -CT_GLIBC_NO_SPARC_V8=y -CT_GLIBC_HAS_OBSOLETE_RPC=y -CT_GLIBC_EXTRA_CONFIG_ARRAY="" -CT_GLIBC_CONFIGPARMS="" -CT_GLIBC_ENABLE_DEBUG=y -CT_GLIBC_EXTRA_CFLAGS="-gdwarf-4 -U_FORTIFY_SOURCE -fcommon" -CT_GLIBC_ENABLE_OBSOLETE_RPC=y -# CT_GLIBC_DISABLE_VERSIONING is not set -CT_GLIBC_OLDEST_ABI="" -CT_GLIBC_FORCE_UNWIND=y -# CT_GLIBC_LOCALES is not set -# CT_GLIBC_KERNEL_VERSION_NONE is not set -CT_GLIBC_KERNEL_VERSION_AS_HEADERS=y -# CT_GLIBC_KERNEL_VERSION_CHOSEN is not set -CT_GLIBC_MIN_KERNEL="4.9.335" -CT_GLIBC_SSP_DEFAULT=y -# CT_GLIBC_SSP_NO is not set -# CT_GLIBC_SSP_YES is not set -# CT_GLIBC_SSP_ALL is not set -# CT_GLIBC_SSP_STRONG is not set -# CT_GLIBC_ENABLE_COMMON_FLAG is not set -CT_ALL_LIBC_CHOICES="AVR_LIBC GLIBC MINGW_W64 MOXIEBOX MUSL NEWLIB NONE PICOLIBC UCLIBC_NG" -CT_LIBC_SUPPORT_THREADS_ANY=y -CT_LIBC_SUPPORT_THREADS_NATIVE=y - -# -# Common C library options -# -CT_THREADS_NATIVE=y -# CT_CREATE_LDSO_CONF is not set -CT_LIBC_XLDD=y -# end of C-library - -# -# C compiler -# -CT_CC_CORE_NEEDED=y -CT_CC_SUPPORT_CXX=y -CT_CC_SUPPORT_FORTRAN=y -CT_CC_SUPPORT_ADA=y -CT_CC_SUPPORT_D=y -CT_CC_SUPPORT_JIT=y -CT_CC_SUPPORT_OBJC=y -CT_CC_SUPPORT_OBJCXX=y -CT_CC_SUPPORT_GOLANG=y -CT_CC_GCC=y -CT_CC="gcc" -CT_CC_CHOICE_KSYM="GCC" -CT_CC_GCC_SHOW=y - -# -# Options for gcc -# -CT_CC_GCC_PKG_KSYM="GCC" -CT_GCC_DIR_NAME="gcc" -CT_GCC_USE_GNU=y -# CT_GCC_USE_ORACLE is not set -CT_GCC_USE="GCC" -CT_GCC_PKG_NAME="gcc" -CT_GCC_SRC_RELEASE=y -# CT_GCC_SRC_DEVEL is not set -CT_GCC_PATCH_ORDER="global" -CT_GCC_V_13=y -# CT_GCC_V_12 is not set -# CT_GCC_V_11 is not set -# CT_GCC_V_10 is not set -# CT_GCC_V_9 is not set -# CT_GCC_V_8 is not set -# CT_GCC_V_7 is not set -# CT_GCC_V_6 is not set -# CT_GCC_V_5 is not set -# CT_GCC_V_4_9 is not set -CT_GCC_VERSION="13.2.0" -CT_GCC_MIRRORS="$(CT_Mirrors GNU gcc/gcc-${CT_GCC_VERSION}) $(CT_Mirrors sourceware gcc/releases/gcc-${CT_GCC_VERSION})" -CT_GCC_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_GCC_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_GCC_ARCHIVE_FORMATS=".tar.xz .tar.gz" -CT_GCC_SIGNATURE_FORMAT="" -CT_GCC_later_than_13=y -CT_GCC_13_or_later=y -CT_GCC_later_than_12=y -CT_GCC_12_or_later=y -CT_GCC_later_than_11=y -CT_GCC_11_or_later=y -CT_GCC_later_than_10=y -CT_GCC_10_or_later=y -CT_GCC_later_than_9=y -CT_GCC_9_or_later=y -CT_GCC_later_than_8=y -CT_GCC_8_or_later=y -CT_GCC_later_than_7=y -CT_GCC_7_or_later=y -CT_GCC_later_than_6=y -CT_GCC_6_or_later=y -CT_GCC_later_than_5=y -CT_GCC_5_or_later=y -CT_GCC_later_than_4_9=y -CT_GCC_4_9_or_later=y -CT_GCC_REQUIRE_4_9_or_later=y -CT_CC_GCC_ENABLE_PLUGINS=y -CT_CC_GCC_GOLD=y -CT_CC_GCC_HAS_LIBMPX=y -CT_CC_GCC_ENABLE_CXX_FLAGS="" -CT_CC_GCC_CORE_EXTRA_CONFIG_ARRAY="" -CT_CC_GCC_EXTRA_CONFIG_ARRAY="" -CT_CC_GCC_STATIC_LIBSTDCXX=y -# CT_CC_GCC_SYSTEM_ZLIB is not set -CT_CC_GCC_CONFIG_TLS=m - -# -# Optimisation features -# -CT_CC_GCC_USE_GRAPHITE=y -CT_CC_GCC_USE_LTO=y -CT_CC_GCC_LTO_ZSTD=m - -# -# Settings for libraries running on target -# -# CT_CC_GCC_ENABLE_DEFAULT_PIE is not set -CT_CC_GCC_ENABLE_TARGET_OPTSPACE=y -# CT_CC_GCC_LIBMUDFLAP is not set -# CT_CC_GCC_LIBGOMP is not set -# CT_CC_GCC_LIBSSP is not set -# CT_CC_GCC_LIBQUADMATH is not set -# CT_CC_GCC_LIBSANITIZER is not set -CT_CC_GCC_LIBSTDCXX_VERBOSE=m - -# -# Misc. obscure options. -# -CT_CC_CXA_ATEXIT=y -CT_CC_GCC_TM_CLONE_REGISTRY=m -# CT_CC_GCC_DISABLE_PCH is not set -CT_CC_GCC_SJLJ_EXCEPTIONS=m -CT_CC_GCC_LDBL_128=m -# CT_CC_GCC_BUILD_ID is not set -CT_CC_GCC_LNK_HASH_STYLE_DEFAULT=y -# CT_CC_GCC_LNK_HASH_STYLE_SYSV is not set -# CT_CC_GCC_LNK_HASH_STYLE_GNU is not set -# CT_CC_GCC_LNK_HASH_STYLE_BOTH is not set -CT_CC_GCC_LNK_HASH_STYLE="" -CT_CC_GCC_DEC_FLOATS_AUTO=y -# CT_CC_GCC_DEC_FLOATS_BID is not set -# CT_CC_GCC_DEC_FLOATS_DPD is not set -# CT_CC_GCC_DEC_FLOATS_NO is not set -CT_CC_GCC_DEC_FLOATS="" -CT_ALL_CC_CHOICES="GCC" - -# -# Additional supported languages: -# -CT_CC_LANG_CXX=y -# CT_CC_LANG_FORTRAN is not set -# end of C compiler - -# -# Debug facilities -# -# CT_DEBUG_DUMA is not set -CT_DEBUG_GDB=y -CT_DEBUG_GDB_PKG_KSYM="GDB" -CT_GDB_DIR_NAME="gdb" -CT_GDB_PKG_NAME="gdb" -CT_GDB_SRC_RELEASE=y -# CT_GDB_SRC_DEVEL is not set -CT_GDB_PATCH_ORDER="global" -CT_GDB_V_13=y -# CT_GDB_V_12 is not set -# CT_GDB_V_11 is not set -# CT_GDB_V_10 is not set -# CT_GDB_V_9 is not set -# CT_GDB_V_8_3 is not set -CT_GDB_VERSION="13.2" -CT_GDB_MIRRORS="$(CT_Mirrors GNU gdb) $(CT_Mirrors sourceware gdb/releases)" -CT_GDB_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_GDB_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_GDB_ARCHIVE_FORMATS=".tar.xz .tar.gz" -CT_GDB_SIGNATURE_FORMAT="" -CT_GDB_later_than_13=y -CT_GDB_13_or_later=y -CT_GDB_later_than_12=y -CT_GDB_12_or_later=y -CT_GDB_later_than_11=y -CT_GDB_11_or_later=y -CT_GDB_later_than_10=y -CT_GDB_10_or_later=y -CT_GDB_later_than_8_3=y -CT_GDB_8_3_or_later=y -CT_GDB_CROSS=y -# CT_GDB_CROSS_STATIC is not set -# CT_GDB_CROSS_SIM is not set -CT_GDB_CROSS_PYTHON=y -CT_GDB_CROSS_PYTHON_BINARY="" -CT_GDB_CROSS_EXTRA_CONFIG_ARRAY="" -# CT_GDB_NATIVE is not set -CT_GDB_GDBSERVER=y -# CT_GDB_NATIVE_BUILD_IPA_LIB is not set -# CT_GDB_NATIVE_STATIC_LIBSTDCXX is not set -CT_GDB_GDBSERVER_TOPLEVEL=y -# CT_DEBUG_LTRACE is not set -# CT_DEBUG_STRACE is not set -CT_ALL_DEBUG_CHOICES="DUMA GDB LTRACE STRACE" -# end of Debug facilities - -# -# Companion libraries -# -# CT_COMPLIBS_CHECK is not set -# CT_COMP_LIBS_CLOOG is not set -CT_COMP_LIBS_EXPAT=y -CT_COMP_LIBS_EXPAT_PKG_KSYM="EXPAT" -CT_EXPAT_DIR_NAME="expat" -CT_EXPAT_PKG_NAME="expat" -CT_EXPAT_SRC_RELEASE=y -# CT_EXPAT_SRC_DEVEL is not set -CT_EXPAT_PATCH_ORDER="global" -CT_EXPAT_V_2_5=y -CT_EXPAT_VERSION="2.5.0" -CT_EXPAT_MIRRORS="http://downloads.sourceforge.net/project/expat/expat/${CT_EXPAT_VERSION} https://github.com/libexpat/libexpat/releases/download/R_${CT_EXPAT_VERSION//./_}" -CT_EXPAT_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_EXPAT_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_EXPAT_ARCHIVE_FORMATS=".tar.xz .tar.lz .tar.bz2 .tar.gz" -CT_EXPAT_SIGNATURE_FORMAT="" -CT_COMP_LIBS_GETTEXT=y -CT_COMP_LIBS_GETTEXT_PKG_KSYM="GETTEXT" -CT_GETTEXT_DIR_NAME="gettext" -CT_GETTEXT_PKG_NAME="gettext" -CT_GETTEXT_SRC_RELEASE=y -# CT_GETTEXT_SRC_DEVEL is not set -CT_GETTEXT_PATCH_ORDER="global" -CT_GETTEXT_V_0_21=y -# CT_GETTEXT_V_0_20_1 is not set -# CT_GETTEXT_V_0_19_8_1 is not set -CT_GETTEXT_VERSION="0.21" -CT_GETTEXT_MIRRORS="$(CT_Mirrors GNU gettext)" -CT_GETTEXT_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_GETTEXT_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_GETTEXT_ARCHIVE_FORMATS=".tar.xz .tar.gz" -CT_GETTEXT_SIGNATURE_FORMAT="packed/.sig" -CT_GETTEXT_0_21_or_later=y -CT_GETTEXT_0_21_or_older=y -CT_GETTEXT_INCOMPATIBLE_WITH_UCLIBC_NG=y - -# -# This version of gettext is not compatible with uClibc-NG. Select -# - -# -# a different version if uClibc-NG is used on the target or (in a -# - -# -# Canadian cross build) on the host. -# -CT_COMP_LIBS_GMP=y -CT_COMP_LIBS_GMP_PKG_KSYM="GMP" -CT_GMP_DIR_NAME="gmp" -CT_GMP_PKG_NAME="gmp" -CT_GMP_SRC_RELEASE=y -# CT_GMP_SRC_DEVEL is not set -CT_GMP_PATCH_ORDER="global" -CT_GMP_V_6_2=y -# CT_GMP_V_6_1 is not set -CT_GMP_VERSION="6.2.1" -CT_GMP_MIRRORS="https://gmplib.org/download/gmp https://gmplib.org/download/gmp/archive $(CT_Mirrors GNU gmp)" -CT_GMP_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_GMP_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_GMP_ARCHIVE_FORMATS=".tar.xz .tar.lz .tar.bz2" -CT_GMP_SIGNATURE_FORMAT="packed/.sig" -CT_COMP_LIBS_ISL=y -CT_COMP_LIBS_ISL_PKG_KSYM="ISL" -CT_ISL_DIR_NAME="isl" -CT_ISL_PKG_NAME="isl" -CT_ISL_SRC_RELEASE=y -# CT_ISL_SRC_DEVEL is not set -CT_ISL_PATCH_ORDER="global" -CT_ISL_V_0_26=y -# CT_ISL_V_0_25 is not set -# CT_ISL_V_0_24 is not set -# CT_ISL_V_0_23 is not set -# CT_ISL_V_0_22 is not set -# CT_ISL_V_0_21 is not set -# CT_ISL_V_0_20 is not set -# CT_ISL_V_0_19 is not set -# CT_ISL_V_0_18 is not set -# CT_ISL_V_0_17 is not set -# CT_ISL_V_0_16 is not set -# CT_ISL_V_0_15 is not set -CT_ISL_VERSION="0.26" -CT_ISL_MIRRORS="https://libisl.sourceforge.io" -CT_ISL_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_ISL_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_ISL_ARCHIVE_FORMATS=".tar.xz .tar.bz2 .tar.gz" -CT_ISL_SIGNATURE_FORMAT="" -CT_ISL_later_than_0_18=y -CT_ISL_0_18_or_later=y -CT_ISL_later_than_0_15=y -CT_ISL_0_15_or_later=y -# CT_COMP_LIBS_LIBELF is not set -CT_COMP_LIBS_LIBICONV=y -CT_COMP_LIBS_LIBICONV_PKG_KSYM="LIBICONV" -CT_LIBICONV_DIR_NAME="libiconv" -CT_LIBICONV_PKG_NAME="libiconv" -CT_LIBICONV_SRC_RELEASE=y -# CT_LIBICONV_SRC_DEVEL is not set -CT_LIBICONV_PATCH_ORDER="global" -CT_LIBICONV_V_1_16=y -# CT_LIBICONV_V_1_15 is not set -CT_LIBICONV_VERSION="1.16" -CT_LIBICONV_MIRRORS="$(CT_Mirrors GNU libiconv)" -CT_LIBICONV_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_LIBICONV_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_LIBICONV_ARCHIVE_FORMATS=".tar.gz" -CT_LIBICONV_SIGNATURE_FORMAT="packed/.sig" -CT_COMP_LIBS_MPC=y -CT_COMP_LIBS_MPC_PKG_KSYM="MPC" -CT_MPC_DIR_NAME="mpc" -CT_MPC_PKG_NAME="mpc" -CT_MPC_SRC_RELEASE=y -# CT_MPC_SRC_DEVEL is not set -CT_MPC_PATCH_ORDER="global" -CT_MPC_V_1_2=y -CT_MPC_VERSION="1.2.1" -CT_MPC_MIRRORS="https://www.multiprecision.org/downloads $(CT_Mirrors GNU mpc)" -CT_MPC_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_MPC_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_MPC_ARCHIVE_FORMATS=".tar.gz" -CT_MPC_SIGNATURE_FORMAT="packed/.sig" -CT_MPC_later_than_1_1_0=y -CT_MPC_1_1_0_or_later=y -CT_COMP_LIBS_MPFR=y -CT_COMP_LIBS_MPFR_PKG_KSYM="MPFR" -CT_MPFR_DIR_NAME="mpfr" -CT_MPFR_PKG_NAME="mpfr" -CT_MPFR_SRC_RELEASE=y -# CT_MPFR_SRC_DEVEL is not set -CT_MPFR_PATCH_ORDER="global" -CT_MPFR_V_4_2=y -CT_MPFR_VERSION="4.2.1" -CT_MPFR_MIRRORS="https://www.mpfr.org/mpfr-${CT_MPFR_VERSION} $(CT_Mirrors GNU mpfr)" -CT_MPFR_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_MPFR_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_MPFR_ARCHIVE_FORMATS=".tar.xz .tar.bz2 .tar.gz .zip" -CT_MPFR_SIGNATURE_FORMAT="packed/.asc" -CT_MPFR_later_than_4_0_0=y -CT_MPFR_4_0_0_or_later=y -CT_COMP_LIBS_NCURSES=y -CT_COMP_LIBS_NCURSES_PKG_KSYM="NCURSES" -CT_NCURSES_DIR_NAME="ncurses" -CT_NCURSES_PKG_NAME="ncurses" -CT_NCURSES_SRC_RELEASE=y -# CT_NCURSES_SRC_DEVEL is not set -CT_NCURSES_PATCH_ORDER="global" -CT_NCURSES_V_6_4=y -# CT_NCURSES_V_6_2 is not set -# CT_NCURSES_V_6_1 is not set -# CT_NCURSES_V_6_0 is not set -CT_NCURSES_VERSION="6.4" -CT_NCURSES_MIRRORS="https://invisible-mirror.net/archives/ncurses $(CT_Mirrors GNU ncurses)" -CT_NCURSES_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_NCURSES_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_NCURSES_ARCHIVE_FORMATS=".tar.gz" -CT_NCURSES_SIGNATURE_FORMAT="packed/.sig" -CT_NCURSES_NEW_ABI=y -CT_NCURSES_HOST_CONFIG_ARGS="" -CT_NCURSES_HOST_DISABLE_DB=y -CT_NCURSES_HOST_FALLBACKS="linux,xterm,xterm-color,xterm-256color,vt100" -CT_NCURSES_TARGET_CONFIG_ARGS="" -# CT_NCURSES_TARGET_DISABLE_DB is not set -CT_NCURSES_TARGET_FALLBACKS="" -CT_COMP_LIBS_ZLIB=y -CT_COMP_LIBS_ZLIB_PKG_KSYM="ZLIB" -CT_ZLIB_DIR_NAME="zlib" -CT_ZLIB_PKG_NAME="zlib" -CT_ZLIB_SRC_RELEASE=y -# CT_ZLIB_SRC_DEVEL is not set -CT_ZLIB_PATCH_ORDER="global" -CT_ZLIB_V_1_2_13=y -CT_ZLIB_VERSION="1.2.13" -CT_ZLIB_MIRRORS="https://github.com/madler/zlib/releases/download/v${CT_ZLIB_VERSION} https://www.zlib.net/" -CT_ZLIB_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_ZLIB_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_ZLIB_ARCHIVE_FORMATS=".tar.xz .tar.gz" -CT_ZLIB_SIGNATURE_FORMAT="packed/.asc" -CT_COMP_LIBS_ZSTD=y -CT_COMP_LIBS_ZSTD_PKG_KSYM="ZSTD" -CT_ZSTD_DIR_NAME="zstd" -CT_ZSTD_PKG_NAME="zstd" -CT_ZSTD_SRC_RELEASE=y -# CT_ZSTD_SRC_DEVEL is not set -CT_ZSTD_PATCH_ORDER="global" -CT_ZSTD_V_1_5_5=y -# CT_ZSTD_V_1_5_2 is not set -CT_ZSTD_VERSION="1.5.5" -CT_ZSTD_MIRRORS="https://github.com/facebook/zstd/releases/download/v${CT_ZSTD_VERSION} https://www.zstd.net/" -CT_ZSTD_ARCHIVE_FILENAME="@{pkg_name}-@{version}" -CT_ZSTD_ARCHIVE_DIRNAME="@{pkg_name}-@{version}" -CT_ZSTD_ARCHIVE_FORMATS=".tar.gz" -CT_ZSTD_SIGNATURE_FORMAT="packed/.sig" -CT_ALL_COMP_LIBS_CHOICES="CLOOG EXPAT GETTEXT GMP GNUPRUMCU ISL LIBELF LIBICONV MPC MPFR NCURSES NEWLIB_NANO PICOLIBC ZLIB ZSTD" -CT_LIBICONV_NEEDED=y -CT_GETTEXT_NEEDED=y -CT_GMP_NEEDED=y -CT_MPFR_NEEDED=y -CT_ISL_NEEDED=y -CT_MPC_NEEDED=y -CT_EXPAT_NEEDED=y -CT_NCURSES_NEEDED=y -CT_ZLIB_NEEDED=y -CT_ZSTD_NEEDED=y -CT_LIBICONV=y -CT_GETTEXT=y -CT_GMP=y -CT_MPFR=y -CT_ISL=y -CT_MPC=y -CT_EXPAT=y -CT_NCURSES=y -CT_ZLIB=y -CT_ZSTD=y -# end of Companion libraries - -# -# Companion tools -# -# CT_COMP_TOOLS_FOR_HOST is not set -# CT_COMP_TOOLS_AUTOCONF is not set -# CT_COMP_TOOLS_AUTOMAKE is not set -# CT_COMP_TOOLS_BISON is not set -# CT_COMP_TOOLS_DTC is not set -# CT_COMP_TOOLS_LIBTOOL is not set -# CT_COMP_TOOLS_M4 is not set -# CT_COMP_TOOLS_MAKE is not set -CT_ALL_COMP_TOOLS_CHOICES="AUTOCONF AUTOMAKE BISON DTC LIBTOOL M4 MAKE" -# end of Companion tools From 70dc8ce81ff7b2da63ed1c70d593eb098215e6bc Mon Sep 17 00:00:00 2001 From: arun Date: Thu, 3 Apr 2025 19:29:33 -0700 Subject: [PATCH 15/71] Remove boost build flag and build c++20 for robots --- src/.bazelrc | 3 --- src/cc_toolchain/cc_toolchain_config.bzl | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/.bazelrc b/src/.bazelrc index 4ffc7ec33b..8f694e5535 100644 --- a/src/.bazelrc +++ b/src/.bazelrc @@ -15,9 +15,6 @@ build --per_file_copt=//proto/.*,//proto/message_translation/.*,//proto/primitiv # Warn variable length arrays only when compiling cpp build --per_file_copt=.*\.cpp@-Wvla -# Boost 1.84 uses old concept syntax that is not compatible with C++20 -build --cxxopt=-DBOOST_ASIO_DISABLE_CONCEPTS - # Automatically set the CPU environment based on the `--cpu` flag as per our # defined CPU environments build --auto_cpu_environment_group=//cc_toolchain:cpus diff --git a/src/cc_toolchain/cc_toolchain_config.bzl b/src/cc_toolchain/cc_toolchain_config.bzl index 1544046401..9a2735fab8 100644 --- a/src/cc_toolchain/cc_toolchain_config.bzl +++ b/src/cc_toolchain/cc_toolchain_config.bzl @@ -671,7 +671,7 @@ def _k8_jetson_nano_cross_compile_impl(ctx): name = "common", implies = [ "build-id", - "c++17", + "c++2a", "colour", "determinism", "frame-pointer", From 9d0d377202cadd970f0b39ace5305b58ef8cc814 Mon Sep 17 00:00:00 2001 From: itsarune Date: Fri, 2 May 2025 21:52:58 -0700 Subject: [PATCH 16/71] Arun's refactoring of MotorService --- src/software/embedded/BUILD | 27 +- src/software/embedded/gpio/BUILD | 19 + src/software/embedded/{ => gpio}/gpio.h | 0 .../embedded/{ => gpio}/gpio_char_dev.cpp | 2 +- .../embedded/{ => gpio}/gpio_char_dev.h | 2 +- .../embedded/{ => gpio}/gpio_sysfs.cpp | 2 +- src/software/embedded/{ => gpio}/gpio_sysfs.h | 2 +- src/software/embedded/motor_controller/BUILD | 56 + .../embedded/motor_controller/motor_board.h | 11 + .../motor_controller/motor_controller.cpp | 54 + .../motor_controller/motor_controller.h | 21 + .../motor_controller_status.h | 3 + .../motor_fault_indicator.cpp | 9 + .../motor_controller/motor_fault_indicator.h | 30 + .../embedded/motor_controller/motor_index.h | 17 + .../stspin_motor_controller.cpp | 21 + .../stspin_motor_controller.h | 18 + .../motor_controller/tmc_motor_controller.cpp | 719 ++++++++++++ .../motor_controller/tmc_motor_controller.h | 288 +++++ src/software/embedded/services/BUILD | 35 +- src/software/embedded/services/motor.cpp | 1009 ++--------------- src/software/embedded/services/motor.h | 179 +-- src/software/embedded/spi_utils.cpp | 59 + src/software/embedded/spi_utils.h | 35 + 24 files changed, 1506 insertions(+), 1112 deletions(-) create mode 100644 src/software/embedded/gpio/BUILD rename src/software/embedded/{ => gpio}/gpio.h (100%) rename src/software/embedded/{ => gpio}/gpio_char_dev.cpp (97%) rename src/software/embedded/{ => gpio}/gpio_char_dev.h (96%) rename src/software/embedded/{ => gpio}/gpio_sysfs.cpp (97%) rename src/software/embedded/{ => gpio}/gpio_sysfs.h (95%) create mode 100644 src/software/embedded/motor_controller/BUILD create mode 100644 src/software/embedded/motor_controller/motor_board.h create mode 100644 src/software/embedded/motor_controller/motor_controller.cpp create mode 100644 src/software/embedded/motor_controller/motor_controller.h create mode 100644 src/software/embedded/motor_controller/motor_controller_status.h create mode 100644 src/software/embedded/motor_controller/motor_fault_indicator.cpp create mode 100644 src/software/embedded/motor_controller/motor_fault_indicator.h create mode 100644 src/software/embedded/motor_controller/motor_index.h create mode 100644 src/software/embedded/motor_controller/stspin_motor_controller.cpp create mode 100644 src/software/embedded/motor_controller/stspin_motor_controller.h create mode 100644 src/software/embedded/motor_controller/tmc_motor_controller.cpp create mode 100644 src/software/embedded/motor_controller/tmc_motor_controller.h create mode 100644 src/software/embedded/spi_utils.cpp create mode 100644 src/software/embedded/spi_utils.h diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index b961568f0a..9c58ff3b1a 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -54,6 +54,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"], @@ -73,24 +82,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/gpio/BUILD b/src/software/embedded/gpio/BUILD new file mode 100644 index 0000000000..f3cb8a7043 --- /dev/null +++ b/src/software/embedded/gpio/BUILD @@ -0,0 +1,19 @@ +package(default_visibility = ["//visibility:public"]) + +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", + ], +) diff --git a/src/software/embedded/gpio.h b/src/software/embedded/gpio/gpio.h similarity index 100% rename from src/software/embedded/gpio.h rename to src/software/embedded/gpio/gpio.h 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 96% rename from src/software/embedded/gpio_char_dev.h rename to src/software/embedded/gpio/gpio_char_dev.h index bb879ccbc7..3312fa82df 100644 --- a/src/software/embedded/gpio_char_dev.h +++ b/src/software/embedded/gpio/gpio_char_dev.h @@ -2,7 +2,7 @@ #include -#include "software/embedded/gpio.h" +#include "software/embedded/gpio/gpio.h" /** * GPIO with the character device interface diff --git a/src/software/embedded/gpio_sysfs.cpp b/src/software/embedded/gpio/gpio_sysfs.cpp similarity index 97% rename from src/software/embedded/gpio_sysfs.cpp rename to src/software/embedded/gpio/gpio_sysfs.cpp index 233f11d912..9c81ff441e 100644 --- a/src/software/embedded/gpio_sysfs.cpp +++ b/src/software/embedded/gpio/gpio_sysfs.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/gpio_sysfs.h" +#include "software/embedded/gpio/gpio_sysfs.h" #include #include diff --git a/src/software/embedded/gpio_sysfs.h b/src/software/embedded/gpio/gpio_sysfs.h similarity index 95% rename from src/software/embedded/gpio_sysfs.h rename to src/software/embedded/gpio/gpio_sysfs.h index 8d8501cd04..05967ec805 100644 --- a/src/software/embedded/gpio_sysfs.h +++ b/src/software/embedded/gpio/gpio_sysfs.h @@ -8,7 +8,7 @@ #include #include -#include "software/embedded/gpio.h" +#include "software/embedded/gpio/gpio.h" /** * GPIO with the sysfs interface diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD new file mode 100644 index 0000000000..c831f5ad72 --- /dev/null +++ b/src/software/embedded/motor_controller/BUILD @@ -0,0 +1,56 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "motor_controller_status", + hdrs = ["motor_controller_status.h"], +) + +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_controller_status", + ":motor_fault_indicator", + ":motor_index", + "//software/embedded:spi_utils", + "//software/embedded/constants", + "//software/embedded/gpio", + "//software/logger", + "@trinamic", + ], +) + +cc_library( + name = "motor_fault_indicator", + srcs = ["motor_fault_indicator.cpp"], + hdrs = ["motor_fault_indicator.h"], + deps = [ + ":motor_board", + "//proto:tbots_cc_proto", + ], +) 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.cpp b/src/software/embedded/motor_controller/motor_controller.cpp new file mode 100644 index 0000000000..252f8c1f1e --- /dev/null +++ b/src/software/embedded/motor_controller/motor_controller.cpp @@ -0,0 +1,54 @@ +void MotorService::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 MotorService::readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, + const uint8_t* write_tx, + const int write_msg_size, + const uint8_t* read_rx, uint32_t spi_speed, + const int read_msg_size) +{ + 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 = write_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 = read_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]); + + CHECK(ret1 >= 1 && ret2 >= 1) + << "SPI Transfer to motor failed, not safe to proceed: errno " << strerror(errno); +} 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..cf12bd81fa --- /dev/null +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -0,0 +1,21 @@ +#pragma once + +#include "software/embedded/motor_controller/motor_board.h" +#include "software/embedded/motor_controller/motor_controller_status.h" +#include "software/embedded/motor_controller/motor_fault_indicator.h" +#include "software/embedded/motor_controller/motor_index.h" + +class MotorController +{ + public: + virtual MotorControllerStatus earlyPoll() = 0; + + virtual void reset() = 0; + + virtual MotorFaultIndicator checkDriverFault(const MotorIndex& motor) = 0; + + virtual double readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) = 0; + + virtual void immediatelyDisable() = 0; +}; diff --git a/src/software/embedded/motor_controller/motor_controller_status.h b/src/software/embedded/motor_controller/motor_controller_status.h new file mode 100644 index 0000000000..25c970e325 --- /dev/null +++ b/src/software/embedded/motor_controller/motor_controller_status.h @@ -0,0 +1,3 @@ +#pragma once + +MAKE_ENUM(MotorControllerStatus, OK, CALIBRATION_FAILURE); 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..23ed20f50b --- /dev/null +++ b/src/software/embedded/motor_controller/motor_fault_indicator.cpp @@ -0,0 +1,9 @@ +#include "software/embedded/motor_controller/motor_fault_indicator.h" + +MotorFaultIndicator::MotorFaultIndicator() : drive_enabled(true), motor_faults() {} + +MotorFaultIndicator::MotorFaultIndicator( + bool drive_enabled, std::unordered_set& motor_faults) + : drive_enabled(drive_enabled), motor_faults(motor_faults) +{ +} 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..8747a30070 --- /dev/null +++ b/src/software/embedded/motor_controller/motor_fault_indicator.h @@ -0,0 +1,30 @@ +#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 motor_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, + std::unordered_set& motor_faults); +}; 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..58c0e57ca5 --- /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..44283674d3 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -0,0 +1,21 @@ +#include "software/embedded/motor_controller/stspin_motor_controller.h" + +MotorControllerStatus StSpinMotorController::earlyPoll() +{ + return MotorControllerStatus::OK; +} + +void StSpinMotorController::reset() {} + +MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) +{ + return MotorFaultIndicator(); +} + +double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) +{ + return 0.0; +} + +void StSpinMotorController::immediatelyDisable() {} 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..207cfe7b83 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -0,0 +1,18 @@ +#pragma once + +#include "software/embedded/motor_controller/motor_controller.h" + +class StSpinMotorController : public MotorController +{ + public: + MotorControllerStatus earlyPoll() override; + + void reset() override; + + MotorFaultIndicator checkDriverFault(const MotorIndex& motor) override; + + double readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) override; + + void immediatelyDisable() override; +}; 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..84492cdf72 --- /dev/null +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -0,0 +1,719 @@ +#include "software/embedded/motor_controller/tmc_motor_controller.h" + +#include "shared/constants.h" +#include "software/embedded/spi_utils.h" +#include "software/logger/logger.h" + +extern "C" +{ +#include "external/trinamic/tmc/ic/TMC4671/TMC4671.h" +#include "external/trinamic/tmc/ic/TMC4671/TMC4671_Register.h" +#include "external/trinamic/tmc/ic/TMC4671/TMC4671_Variants.h" +#include "external/trinamic/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 = NULL; + + uint8_t tmc4671_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) + { + return g_motor->tmc4671ReadWriteByte(motor, data, last_transfer); + } + + uint8_t tmc6100_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) + { + return g_motor->tmc6100ReadWriteByte(motor, data, last_transfer); + } +} + +TmcMotorController::TmcMotorController() + : 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)) +{ + openSpiFileDescriptor(MotorIndex::FRONT_LEFT); + openSpiFileDescriptor(MotorIndex::FRONT_RIGHT); + openSpiFileDescriptor(MotorIndex::BACK_LEFT); + openSpiFileDescriptor(MotorIndex::BACK_RIGHT); + openSpiFileDescriptor(MotorIndex::DRIBBLER); +} + +MotorControllerStatus TmcMotorController::earlyPoll() +{ + auto motors = driveMotors(); + bool encoders_calibrated = + std::accumulate(motors.begin(), motors.end(), false, + [&](const bool& acc, const MotorIndex& motor) { + return acc || encoder_calibrated_[motor]; + }); + + if (!encoders_calibrated) + { + return MotorControllerStatus::CALIBRATION_FAILURE; + } + + return MotorControllerStatus::OK; +} + +double TmcMotorController::readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) +{ + return readThenWriteValue(motor, TMC4671_PID_VELOCITY_ACTUAL, + TMC4671_PID_VELOCITY_TARGET, target_velocity); +} + +void TmcMotorController::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 TmcMotorController::writeToControllerOrDieTrying(const MotorIndex& 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(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_retires_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()) + { + 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); + checkDriverFault(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); +} + +MotorFaultIndicator TmcMotorController::checkDriverFault(const MotorIndex& motor) +{ + bool drive_enabled = true; + std::unordered_set motor_faults; + + int gstat = tmc6100_readInt(CHIP_SELECTS.at(motor), TMC6100_GSTAT); + 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.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]) + { + 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; + } + + if (gstat_bitset[14]) + { + 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 MotorFaultIndicator(drive_enabled, motor_faults); +} + +double 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); + checkDriverFault(motor); + // Start all the controllers as drive motor controllers + startController(motor, false); + tmc4671_setTargetVelocity(CHIP_SELECTS.at(motor), 0); +} + +void TmcMotorController::startDriver(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(MotorIndex motor, 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, 0x00030001); + configureHall(motor); + + configureDribblerPI(motor); + } + else + { + // Configure to brushless DC motor with 8 pole pairs + writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, 0x00030008); + configureEncoder(motor); + } +} + +uint8_t TmcMotorController::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); +} + +uint8_t TmcMotorController::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 TmcMotorController::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 TmcMotorController::checkEncoderConnections() +{ + LOG(INFO) << "Starting encoder connection check!"; + + std::unordered_map calibrated_motors; + std::unordered_map initial_velocities; + + for (const MotorIndex& motor : driveMotors()) + { + // 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..3b1a0c993b --- /dev/null +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -0,0 +1,288 @@ +#pragma once + +#include "software/embedded/constants/constants.h" +#include "software/embedded/gpio/gpio.h" +#include "software/embedded/gpio/gpio_char_dev.h" +#include "software/embedded/gpio/gpio_sysfs.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" + +class TmcMotorController : public MotorController +{ + public: + TmcMotorController(); + + MotorControllerStatus earlyPoll() override; + + void reset() override; + + MotorFaultIndicator checkDriverFault(const MotorIndex& motor) override; + + void immediatelyDisable() override; + + double readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) override; + + private: + /** + * 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) + */ + template + static std::unique_ptr setupGpio(const T& gpio_number, GpioDirection direction, + GpioState initial_state); + + /** + * Opens SPI file descriptor + * + * @param motor_index The index of the motor to open + */ + void openSpiFileDescriptor(const 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 + */ + double readThenWriteValue(const MotorIndex& motor, const uint8_t& read_addr, + const uint8_t& write_addr, const 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(const MotorIndex& motor, uint8_t address, + int32_t value); + void writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value); + + /** + * Clears previous faults, configures the motor and checks encoder connections. + */ + void setup(); + + /** + * Sets up motor as drive motor controllers + * + * @param motor drive motor number + */ + void setupDriveMotor(const 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(const MotorIndex& motor); + void configureDribblerPI(const MotorIndex& motor); + void configureDrivePI(const MotorIndex& motor); + void configureADC(const MotorIndex& motor); + void configureEncoder(const MotorIndex& motor); + void configureHall(const MotorIndex& motor); + + // 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 tmc4671ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); + uint8_t tmc6100ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); + + /** + * 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(const MotorIndex& motor); + void endEncoderCalibration(const MotorIndex& motor); + + /** + * 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. + * + * 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] = {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; + + // Tracks whether each motor's encoder has been calibrated + std::unordered_map encoder_calibrated_; + + // SPI Chip Selects + static constexpr uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT = 0; + static constexpr uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3; + static constexpr uint8_t BACK_LEFT_MOTOR_CHIP_SELECT = 1; + static constexpr uint8_t BACK_RIGHT_MOTOR_CHIP_SELECT = 2; + 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.1"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.2"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.3"}, + {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; + + // Motor names (indexed with chip select above) + static constexpr const char* MOTOR_NAMES[] = {"front_left", "back_left", "back_right", + "front_right", "dribbler"}; + + // 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; +}; + +template +std::unique_ptr TmcMotorController::setupGpio(const T& gpio_number, + GpioDirection direction, + GpioState initial_state) +{ + if constexpr (PLATFORM == Platform::JETSON_NANO) + { + return std::make_unique(gpio_number, direction, initial_state); + } + else + { + return std::make_unique(gpio_number, direction, initial_state, + "/dev/gpiochip4"); + } +} diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index a6a12201b7..09cad6e353 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -1,5 +1,30 @@ package(default_visibility = ["//visibility:public"]) +load("@bazel_skylib//rules:common_settings.bzl", "string_flag") + +string_flag( + name = "motor_board", + build_setting_default = "TRINAMIC", + values = [ + "TRINAMIC", + "STSPIN", + ], +) + +config_setting( + name = "build_trinamic", + flag_values = { + ":motor_board": "TRINAMIC", + }, +) + +config_setting( + name = "build_stspin", + flag_values = { + ":motor_board": "STSPIN", + }, +) + cc_library( name = "motor", srcs = ["motor.cpp"], @@ -9,17 +34,23 @@ cc_library( "//software/embedded:build_nano": ["NANO"], "//software/embedded:build_pi": ["PI"], "//software/embedded:build_limited": ["LIMITED"], + }) + + select({ + ":build_trinamic": ["TRINAMIC_MOTOR_BOARD"], + ":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", ], ) diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index c8e06e9910..4d87e76844 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -21,114 +21,42 @@ #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 "external/trinamic/tmc/ic/TMC4671/TMC4671.h" -#include "external/trinamic/tmc/ic/TMC4671/TMC4671_Register.h" -#include "external/trinamic/tmc/ic/TMC4671/TMC4671_Variants.h" -#include "external/trinamic/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 const uint32_t NUM_RETRIES_SPI = 3; 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 RobotConstants_t& 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)), + : motor_controller_(setupMotorController()), robot_constants_(robot_constants), euclidean_to_four_wheel_(robot_constants), - motor_fault_detector_(0), dribbler_ramp_rpm_(0), - tracked_motor_fault_start_time_(std::nullopt), - num_tracked_motor_resets_(0) + tracked_motor_fault_start_time_(std::nullopt) { - motorServiceInit(robot_constants, control_loop_frequency_hz); } -void MotorService::motorServiceInit(const RobotConstants_t& 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) +MotorService::~MotorService() {} - // Make this instance available to the static functions above - g_motor_service = this; +void MotorService::resetMotorBoard() +{ + motor_controller_->reset(); } -MotorService::~MotorService() {} - void MotorService::setup() { + for (const MotorIndex& motor : reflective_enum::values()) + { + cached_motor_faults_[motor] = MotorFaultIndicator(); + } + 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()) @@ -162,174 +90,21 @@ void MotorService::setup() 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); - } - - // 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); - } + motor_controller_->reset(); is_initialized_ = true; } -void MotorService::setUpDriveMotor(uint8_t motor) -{ - startDriver(motor); - checkDriverFault(motor); - // Start all the controllers as drive motor controllers - startController(motor, false); - tmc4671_setTargetVelocity(motor, 0); -} - -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]) + if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { - LOG(WARNING) << "shortdet_w: short counter has triggered at least once."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_COUNTER_DETECTED); + return std::make_unique(); } - - if (gstat_bitset[13]) - { - 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; - } - - 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(); } - - return MotorFaultIndicator(drive_enabled, motor_faults); } TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_velocity_mps, @@ -340,52 +115,54 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci { TbotsProto::MotorStatus motor_status; - cached_motor_faults_[motor_fault_detector_] = checkDriverFault(motor_fault_detector_); + cached_motor_faults_[motor_fault_detector_] = + motor_controller_->checkDriverFault(motor_fault_detector_); for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) - { - if (motor != DRIBBLER_MOTOR_CHIP_SELECT) + for (const MotorIndex& motor : reflective_enum::values()) { - TbotsProto::DriveUnit drive_status; - drive_status.set_enabled(cached_motor_faults_[motor].drive_enabled); - - for (const TbotsProto::MotorFault& fault : - cached_motor_faults_[motor].motor_faults) + if (motor != MotorIndex::DRIBBLER) { - drive_status.add_motor_faults(fault); + TbotsProto::DriveUnit drive_status; + drive_status.set_enabled(cached_motor_faults_[motor].drive_enabled); + + for (const TbotsProto::MotorFault& fault : + cached_motor_faults_[motor].motor_faults) + { + drive_status.add_motor_faults(fault); + } + + if (motor == MotorIndex::FRONT_LEFT) + { + *(motor_status.mutable_front_left()) = drive_status; + } + if (motor == MotorIndex::FRONT_RIGHT) + { + *(motor_status.mutable_front_right()) = drive_status; + } + if (motor == MotorIndex::BACK_RIGHT) + { + *(motor_status.mutable_back_right()) = drive_status; + } + if (motor == MotorIndex::BACK_LEFT) + { + *(motor_status.mutable_back_left()) = drive_status; + } } - - if (motor == FRONT_LEFT_MOTOR_CHIP_SELECT) + else { - *(motor_status.mutable_front_left()) = drive_status; - } - if (motor == FRONT_RIGHT_MOTOR_CHIP_SELECT) - { - *(motor_status.mutable_front_right()) = drive_status; - } - if (motor == BACK_LEFT_MOTOR_CHIP_SELECT) - { - *(motor_status.mutable_back_left()) = drive_status; - } - if (motor == BACK_RIGHT_MOTOR_CHIP_SELECT) - { - *(motor_status.mutable_back_right()) = drive_status; + 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.add_motor_faults(fault); + } + + *(motor_status.mutable_dribbler()) = dribbler_status; } } - else - { - 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.add_motor_faults(fault); - } - - *(motor_status.mutable_dribbler()) = dribbler_status; - } - } motor_status.mutable_front_left()->set_wheel_velocity( static_cast(front_left_velocity_mps)); @@ -397,7 +174,8 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci static_cast(back_right_velocity_mps)); motor_fault_detector_ = - static_cast((motor_fault_detector_ + 1) % NUM_MOTORS); + static_cast((static_cast(motor_fault_detector_) + 1) % + reflective_enum::size()); return motor_status; } @@ -405,22 +183,17 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor, 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 (motor_controller_->earlyPoll() != MotorControllerStatus::OK) { is_initialized_ = false; } // checks if any motor has reset, sends a log message if so - for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) + for (const MotorIndex& motor_index : reflective_enum::values()) { - if (requiresMotorReinit(motor)) + if (requiresMotorReinit(motor_index)) { - LOG(DEBUG) << "RESET DETECTED FOR MOTOR: " << MOTOR_NAMES[motor]; + LOG(DEBUG) << "RESET DETECTED FOR MOTOR: " << motor_index; is_initialized_ = false; } } @@ -431,38 +204,23 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor setup(); } - 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"; - // 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_)); + double front_right_velocity = motor_controller_->readThenWriteVelocity( + MotorIndex::FRONT_RIGHT, front_right_target_rpm) * + MECHANICAL_MPS_PER_ELECTRICAL_RPM; + double front_left_velocity = motor_controller_->readThenWriteVelocity( + MotorIndex::FRONT_LEFT, front_left_target_rpm) * + MECHANICAL_MPS_PER_ELECTRICAL_RPM; + double back_right_velocity = motor_controller_->readThenWriteVelocity( + MotorIndex::BACK_RIGHT, back_right_target_rpm) * + MECHANICAL_MPS_PER_ELECTRICAL_RPM; + double back_left_velocity = motor_controller_->readThenWriteVelocity( + MotorIndex::BACK_LEFT, back_left_target_rpm) * + MECHANICAL_MPS_PER_ELECTRICAL_RPM; + double dribbler_rpm = motor_controller_->readThenWriteVelocity(MotorIndex::DRIBBLER, + dribbler_ramp_rpm_); // Construct a MotorStatus object with the current velocities and dribbler rpm TbotsProto::MotorStatus motor_status = @@ -479,28 +237,28 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor 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"; } @@ -594,7 +352,7 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor return motor_status; } -bool MotorService::requiresMotorReinit(uint8_t motor) +bool MotorService::requiresMotorReinit(const MotorIndex& motor) { auto reset_search = cached_motor_faults_[motor].motor_faults.find(TbotsProto::MotorFault::RESET); @@ -602,598 +360,3 @@ bool MotorService::requiresMotorReinit(uint8_t motor) 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) -{ - 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]); - - 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--) - { - 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"; - } - - // 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); - - 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) - { - // Configure to brushless DC motor with 1 pole pair - writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, 0x00030001); - configureHall(motor); - - configureDribblerPI(motor); - } - 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"; - } - - // stop all motors, reset back to velocity control mode - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) - { - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x00000000); - tmc4671_switchToMotionMode(motor, TMC4671_MOTION_MODE_VELOCITY); - } - - LOG(INFO) << "All encoders appear to be connected!"; -} - -void MotorService::resetMotorBoard() -{ - reset_gpio_->setValue(GpioState::LOW); -} diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 0d5c00c797..f89c660ec0 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -2,6 +2,7 @@ #include #include +#include #include #include "proto/robot_status_msg.pb.h" @@ -9,9 +10,11 @@ #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/gpio/gpio_char_dev.h" +#include "software/embedded/gpio/gpio_sysfs.h" +#include "software/embedded/motor_controller/motor_controller.h" +#include "software/embedded/motor_controller/motor_fault_indicator.h" #include "software/embedded/platform.h" #include "software/physics/euclidean_to_wheel.h" @@ -67,23 +70,6 @@ class MotorService 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) * @@ -113,34 +99,6 @@ class MotorService */ 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 * @@ -188,68 +146,6 @@ class MotorService void motorServiceInit(const RobotConstants_t& 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. @@ -325,6 +221,8 @@ class MotorService double back_right_velocity_mps, double dribbler_rpm); + std::unique_ptr setupMotorController(); + /** * Helper function to setup a GPIO pin. Selects the appropriate GPIO implementation * based on the host platform. @@ -347,7 +245,7 @@ class MotorService * * @return true if the motor has returned a cached RESET fault, false otherwise */ - bool requiresMotorReinit(uint8_t motor); + bool requiresMotorReinit(const MotorIndex& motor); // All trinamic RPMS are electrical RPMS, they don't factor in the number of pole // pairs of the drive motor. @@ -358,41 +256,17 @@ class MotorService static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; + // Controller for communicating with the motor board + std::unique_ptr motor_controller_; + // 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_; - RobotConstants_t robot_constants_; // Drive Motors EuclideanToWheel euclidean_to_four_wheel_; - std::unordered_map encoder_calibrated_; - std::unordered_map cached_motor_faults_; + std::unordered_map cached_motor_faults_; // Previous wheel velocities WheelSpace_t prev_wheel_velocities_; @@ -403,7 +277,7 @@ class MotorService int back_right_target_rpm = 0; // the motor cs id to check for motor faults - uint8_t motor_fault_detector_; + MotorIndex motor_fault_detector_; static const int NUM_CALIBRATION_ATTEMPTS = 10; @@ -420,29 +294,4 @@ class MotorService 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"}; }; - -template -std::unique_ptr MotorService::setupGpio(const T& gpio_number, - GpioDirection direction, - GpioState initial_state) -{ - if constexpr (PLATFORM == Platform::JETSON_NANO) - { - return std::make_unique(gpio_number, direction, initial_state); - } - else - { - return std::make_unique(gpio_number, direction, initial_state, - "/dev/gpiochip4"); - } -} 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); From 1143567173c073a1763feb5c695c3b79a6ddaa0e Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 2 May 2025 22:27:51 -0700 Subject: [PATCH 17/71] Remove old method declarations from MotorService header --- .../motor_controller/motor_controller.cpp | 54 ------ .../motor_controller/motor_controller.h | 8 + .../motor_controller/tmc_motor_controller.h | 88 +++++---- src/software/embedded/services/motor.cpp | 79 ++++---- src/software/embedded/services/motor.h | 175 +----------------- src/software/embedded/thunderloop.cpp | 2 +- 6 files changed, 105 insertions(+), 301 deletions(-) delete mode 100644 src/software/embedded/motor_controller/motor_controller.cpp diff --git a/src/software/embedded/motor_controller/motor_controller.cpp b/src/software/embedded/motor_controller/motor_controller.cpp deleted file mode 100644 index 252f8c1f1e..0000000000 --- a/src/software/embedded/motor_controller/motor_controller.cpp +++ /dev/null @@ -1,54 +0,0 @@ -void MotorService::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 MotorService::readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, - const uint8_t* write_tx, - const int write_msg_size, - const uint8_t* read_rx, uint32_t spi_speed, - const int read_msg_size) -{ - 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 = write_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 = read_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]); - - CHECK(ret1 >= 1 && ret2 >= 1) - << "SPI Transfer to motor failed, not safe to proceed: errno " << strerror(errno); -} diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index cf12bd81fa..f1bd7760b2 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -12,6 +12,14 @@ class MotorController virtual void reset() = 0; + /** + * 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 + */ virtual MotorFaultIndicator checkDriverFault(const MotorIndex& motor) = 0; virtual double readThenWriteVelocity(const MotorIndex& motor, diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index 3b1a0c993b..cbd3445366 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -123,40 +123,50 @@ class TmcMotorController : public MotorController void configureEncoder(const MotorIndex& motor); void configureHall(const MotorIndex& motor); - // 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 - // + /** + * 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); @@ -191,9 +201,9 @@ class TmcMotorController : public MotorController void endEncoderCalibration(const MotorIndex& motor); /** - * 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. + * 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 */ @@ -256,10 +266,6 @@ class TmcMotorController : public MotorController static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0x3u; - // Motor names (indexed with chip select above) - static constexpr const char* MOTOR_NAMES[] = {"front_left", "back_left", "back_right", - "front_right", "dribbler"}; - // SPI File Descriptors mapping from Chip Select -> File Descriptor std::array()> file_descriptors_; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 4d87e76844..6f3fc083bd 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -45,7 +45,7 @@ MotorService::MotorService(const RobotConstants_t& robot_constants, MotorService::~MotorService() {} -void MotorService::resetMotorBoard() +void MotorService::reset() { motor_controller_->reset(); } @@ -118,51 +118,50 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci cached_motor_faults_[motor_fault_detector_] = motor_controller_->checkDriverFault(motor_fault_detector_); - for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) - for (const MotorIndex& motor : reflective_enum::values()) + for (const MotorIndex& motor : reflective_enum::values()) + { + if (motor != MotorIndex::DRIBBLER) { - if (motor != MotorIndex::DRIBBLER) + TbotsProto::DriveUnit drive_status; + drive_status.set_enabled(cached_motor_faults_[motor].drive_enabled); + + for (const TbotsProto::MotorFault& fault : + cached_motor_faults_[motor].motor_faults) + { + drive_status.add_motor_faults(fault); + } + + if (motor == MotorIndex::FRONT_LEFT) + { + *(motor_status.mutable_front_left()) = drive_status; + } + if (motor == MotorIndex::FRONT_RIGHT) { - TbotsProto::DriveUnit drive_status; - drive_status.set_enabled(cached_motor_faults_[motor].drive_enabled); - - for (const TbotsProto::MotorFault& fault : - cached_motor_faults_[motor].motor_faults) - { - drive_status.add_motor_faults(fault); - } - - if (motor == MotorIndex::FRONT_LEFT) - { - *(motor_status.mutable_front_left()) = drive_status; - } - if (motor == MotorIndex::FRONT_RIGHT) - { - *(motor_status.mutable_front_right()) = drive_status; - } - if (motor == MotorIndex::BACK_RIGHT) - { - *(motor_status.mutable_back_right()) = drive_status; - } - if (motor == MotorIndex::BACK_LEFT) - { - *(motor_status.mutable_back_left()) = drive_status; - } + *(motor_status.mutable_front_right()) = drive_status; } - else + if (motor == MotorIndex::BACK_RIGHT) { - 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.add_motor_faults(fault); - } - - *(motor_status.mutable_dribbler()) = dribbler_status; + *(motor_status.mutable_back_right()) = drive_status; } + if (motor == MotorIndex::BACK_LEFT) + { + *(motor_status.mutable_back_left()) = drive_status; + } + } + else + { + 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.add_motor_faults(fault); + } + + *(motor_status.mutable_dribbler()) = dribbler_status; } + } motor_status.mutable_front_left()->set_wheel_velocity( static_cast(front_left_velocity_mps)); diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index f89c660ec0..37ac632170 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -19,7 +19,7 @@ #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 @@ -29,14 +29,8 @@ 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. + * Service that interacts with the motors. * Opens all the required ports and maintains them until destroyed. * * @param RobotConstants_t The robot constants @@ -58,145 +52,19 @@ class MotorService 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); - - /** - * 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(); /** - * 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 - */ - 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 + * Reset the motor board by toggling the reset GPIO appropriately. Effectively stops + * the motors from moving. */ - 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 RobotConstants_t& robot_constants, - int control_loop_frequency_hz); - - - /** - * 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 - * - * @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); - - - /** - * 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. - * - * Leaves the motors connected in MOTION_MODE_VELOCITY - */ - void checkEncoderConnections(); + std::unique_ptr setupMotorController(); /** * Return a MotorStatus proto filled with motor faults. Some values of the proto are @@ -221,22 +89,6 @@ class MotorService double back_right_velocity_mps, double dribbler_rpm); - std::unique_ptr setupMotorController(); - - /** - * 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) - */ - template - static std::unique_ptr setupGpio(const T& gpio_number, GpioDirection direction, - GpioState initial_state); - /** * Returns true if we've detected a RESET in our cached motor faults indicators or if * we have a fault that disables drive. @@ -259,16 +111,14 @@ class MotorService // Controller for communicating with the motor board std::unique_ptr motor_controller_; - // to check if the motors have been calibrated + // Flag indicating whether the motors have been calibrated bool is_initialized_ = false; RobotConstants_t robot_constants_; - // Drive Motors EuclideanToWheel euclidean_to_four_wheel_; std::unordered_map cached_motor_faults_; - // Previous wheel velocities WheelSpace_t prev_wheel_velocities_; int front_left_target_rpm = 0; @@ -276,22 +126,17 @@ class MotorService int back_left_target_rpm = 0; int back_right_target_rpm = 0; - // the motor cs id to check for motor faults + // The current motor to check for motor faults; this is updated every poll + // and will cycle through all the MotorIndex values (only one motor is checked + // for faults each poll to reduce number of SPI transactions) MotorIndex motor_fault_detector_; - static const int NUM_CALIBRATION_ATTEMPTS = 10; - int dribbler_ramp_rpm_; std::optional> tracked_motor_fault_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; }; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 915739abc9..1fba624111 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -44,7 +44,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 From 558752c0bfdcdb9aeee2555656a5708c2e0c174c Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 3 May 2025 14:30:58 -0700 Subject: [PATCH 18/71] Fix bugs in TmcMotorController --- src/software/embedded/ansible/BUILD | 1 - .../motor_controller/motor_controller.h | 2 ++ .../motor_controller/tmc_motor_controller.cpp | 12 +++++++--- .../motor_controller/tmc_motor_controller.h | 9 +++----- src/software/embedded/services/motor.cpp | 22 +++++++++---------- 5 files changed, 24 insertions(+), 22 deletions(-) diff --git a/src/software/embedded/ansible/BUILD b/src/software/embedded/ansible/BUILD index d34e119a46..3986b1c652 100644 --- a/src/software/embedded/ansible/BUILD +++ b/src/software/embedded/ansible/BUILD @@ -26,7 +26,6 @@ py_binary( "//software/embedded/linux_configs/systemd:systemd_files", "//software/embedded/redis", "//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/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index f1bd7760b2..70e2dc9921 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -10,6 +10,8 @@ class MotorController public: virtual MotorControllerStatus earlyPoll() = 0; + virtual void setup() = 0; + virtual void reset() = 0; /** diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index 84492cdf72..95eb02e4c4 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -24,16 +24,16 @@ extern "C" // // 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 = NULL; + static TmcMotorController* g_motor_controller = NULL; uint8_t tmc4671_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) { - return g_motor->tmc4671ReadWriteByte(motor, data, 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->tmc6100ReadWriteByte(motor, data, last_transfer); + return g_motor_controller->tmc6100ReadWriteByte(motor, data, last_transfer); } } @@ -52,6 +52,9 @@ TmcMotorController::TmcMotorController() openSpiFileDescriptor(MotorIndex::BACK_LEFT); openSpiFileDescriptor(MotorIndex::BACK_RIGHT); openSpiFileDescriptor(MotorIndex::DRIBBLER); + + // Make this instance available to the static functions above + g_motor_controller = this; } MotorControllerStatus TmcMotorController::earlyPoll() @@ -155,6 +158,7 @@ void TmcMotorController::setup() checkDriverFault(MotorIndex::DRIBBLER); startController(MotorIndex::DRIBBLER, true); tmc4671_setTargetVelocity(DRIBBLER_MOTOR_CHIP_SELECT, 0); + checkEncoderConnections(); // calibrate the encoders @@ -602,6 +606,8 @@ void TmcMotorController::checkEncoderConnections() 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); diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index cbd3445366..7ff94d3da2 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -15,6 +15,8 @@ class TmcMotorController : public MotorController MotorControllerStatus earlyPoll() override; + void setup() override; + void reset() override; MotorFaultIndicator checkDriverFault(const MotorIndex& motor) override; @@ -77,11 +79,6 @@ class TmcMotorController : public MotorController int32_t value); void writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value); - /** - * Clears previous faults, configures the motor and checks encoder connections. - */ - void setup(); - /** * Sets up motor as drive motor controllers * @@ -237,9 +234,9 @@ class TmcMotorController : public MotorController // SPI Chip Selects static constexpr uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT = 0; - static constexpr uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3; 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 diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 6f3fc083bd..22383c1dfc 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -45,11 +45,6 @@ MotorService::MotorService(const RobotConstants_t& robot_constants, MotorService::~MotorService() {} -void MotorService::reset() -{ - motor_controller_->reset(); -} - void MotorService::setup() { for (const MotorIndex& motor : reflective_enum::values()) @@ -78,7 +73,6 @@ void MotorService::setup() num_tracked_motor_resets_ = 1; } - if (tracked_motor_fault_start_time_.has_value() && num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) { @@ -89,12 +83,16 @@ void MotorService::setup() prev_wheel_velocities_ = {0.0, 0.0, 0.0, 0.0}; - // Clear faults by resetting all the chips on the motor board - motor_controller_->reset(); + motor_controller_->setup(); is_initialized_ = true; } +void MotorService::reset() +{ + motor_controller_->reset(); +} + std::unique_ptr MotorService::setupMotorController() { if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) @@ -139,14 +137,14 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci { *(motor_status.mutable_front_right()) = drive_status; } - if (motor == MotorIndex::BACK_RIGHT) - { - *(motor_status.mutable_back_right()) = drive_status; - } if (motor == MotorIndex::BACK_LEFT) { *(motor_status.mutable_back_left()) = drive_status; } + if (motor == MotorIndex::BACK_RIGHT) + { + *(motor_status.mutable_back_right()) = drive_status; + } } else { From 9ba1129a9e532387ba57ae3493771f88b4ea059d Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 17 May 2025 00:57:06 -0700 Subject: [PATCH 19/71] Initial implementation of StSpinMotorController --- src/software/embedded/gpio/gpio_char_dev.h | 1 + .../stspin_motor_controller.cpp | 105 ++++++++++++++++- .../stspin_motor_controller.h | 110 ++++++++++++++++++ .../motor_controller/tmc_motor_controller.cpp | 28 ++--- 4 files changed, 227 insertions(+), 17 deletions(-) diff --git a/src/software/embedded/gpio/gpio_char_dev.h b/src/software/embedded/gpio/gpio_char_dev.h index c2a6a371ad..7771f0c3e7 100644 --- a/src/software/embedded/gpio/gpio_char_dev.h +++ b/src/software/embedded/gpio/gpio_char_dev.h @@ -1,6 +1,7 @@ #pragma once #include + #include #include "software/embedded/gpio/gpio.h" diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 44283674d3..e30cfae529 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -1,10 +1,27 @@ #include "software/embedded/motor_controller/stspin_motor_controller.h" +StSpinMotorController::StSpinMotorController() +{ + openSpiFileDescriptor(MotorIndex::FRONT_LEFT); + openSpiFileDescriptor(MotorIndex::FRONT_RIGHT); + openSpiFileDescriptor(MotorIndex::BACK_LEFT); + openSpiFileDescriptor(MotorIndex::BACK_RIGHT); + openSpiFileDescriptor(MotorIndex::DRIBBLER); +} + MotorControllerStatus StSpinMotorController::earlyPoll() { return MotorControllerStatus::OK; } +void StSpinMotorController::setup() +{ + for (const MotorIndex& motor : reflective_enum::values()) + { + sendFrame(motor, StSpinOpcode.START_MOTOR); + } +} + void StSpinMotorController::reset() {} MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) @@ -15,7 +32,93 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, const int& target_velocity) { - return 0.0; + const int16_t speed = sendAndReceiveFrame(motor, StSpinOpcode.GET_SPEED, 0); + + // SET_SPEEDRAMP expects the target motor speed to be in register ax. + sendAndReceiveFrame(motor, StSpinOpcode.MOV_AX, target_velocity); + + // SET_SPEEDRAMP expects the ramp time in millis to be in register bx. + // We do speed ramping ourselves in MotorService, so we just want to + // set the target speed without ramping (hence we set reg bx to 0). + sendAndReceiveFrame(motor, StSpinOpcode.MOV_BX, 0); + + sendFrame(motor, StSpinOpcode.SET_SPEEDRAMP); + + return speed; } void StSpinMotorController::immediatelyDisable() {} + +void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) +{ + file_descriptors_[CHIP_SELECTS.at(motor)] = open(SPI_PATHS.at(motor), O_RDWR); + CHECK(file_descriptors_[CHIP_SELECTS.at(motor)] >= 0) + << "can't open device: " << motor << "error: " << strerror(errno); + + int ret = + ioctl(file_descriptors_[CHIP_SELECTS.at(motor)], SPI_IOC_WR_MODE32, &SPI_MODE); + CHECK(ret != -1) << "can't set spi mode for: " << motor + << "error: " << strerror(errno); + + ret = ioctl(file_descriptors_[CHIP_SELECTS.at(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(file_descriptors_[CHIP_SELECTS.at(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); +} + +int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, + const StSpinOpcode opcode, + const int16_t data) +{ + // A full frame is 6 bytes long and has the following format: + // + // +-------+--------+---------------+-------+-------+ + // | SOF | OPCODE | DATA | CRC | EOF | + // +-------+--------+---------------+-------+-------+ + // 0 1 2 3 4 5 + // + // For slave frames, OPCODE will be either an ACK or NACK acknowledging + // receival of the master frame. + // + // The byte order of DATA is big endian; therefore the MSB is transmitted first. + // DATA may be omitted if the operation has no data to transmit/receive, making + // the frame only 4 bytes long. + // + // To receive data for GET operations, it is expected that we send a master frame + // with 2 dummy DATA bytes in order to clock out the 2 DATA bytes from the incoming + // slave frame. + + uint8_t tx[FRAME_MAX_LEN] = {0}; + uint8_t rx[FRAME_MAX_LEN] = {0}; + + tx[0] = FRAME_SOF; + tx[1] = static_cast(opcode); + tx[2] = 0xFF & (data >> 8); + tx[3] = 0xFF & data; + tx[4] = 0; // CRC; to be implemented later + tx[5] = FRAME_EOF; + + spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_MAX_LEN, + SPI_SPEED_HZ); + + return static_cast(static_cast(rx[2]) << 8) | rx[3]); +} + +void StSpinMotorController::sendFrame(const MotorIndex& motor, const StSpinOpcode opcode) +{ + uint8_t tx[FRAME_MIN_LEN] = {0}; + uint8_t rx[FRAME_MIN_LEN] = {0}; + + tx[0] = FRAME_SOF; + tx[1] = static_cast(opcode); + tx[2] = 0; // CRC; to be implemented later + tx[3] = FRAME_EOF; + + spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_MIN_LEN, + SPI_SPEED_HZ); +} diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 207cfe7b83..2e1417ae8e 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -1,12 +1,15 @@ #pragma once #include "software/embedded/motor_controller/motor_controller.h" +#include "software/util/make_enum/make_enum.hpp" class StSpinMotorController : public MotorController { public: MotorControllerStatus earlyPoll() override; + void setup() override; + void reset() override; MotorFaultIndicator checkDriverFault(const MotorIndex& motor) override; @@ -15,4 +18,111 @@ class StSpinMotorController : public MotorController const int& target_velocity) override; void immediatelyDisable() override; + + private: + /** + * Opens a SPI file descriptor for the given motor + * + * @param motor the motor to open a SPI file descriptor for + */ + void openSpiFileDescriptor(const 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 opcode the opcode to include in the transmitted frame + * @param data the data to include in the transmitted frame + * @return the data in the frame received from the motor + */ + int16_t sendAndReceiveFrame(const MotorIndex& motor, const StSpinOpcode opcode, + const int16_t data); + + /** + * Transmits a frame to the given motor over SPI without expecting any data + * back in response. + * + * @param motor the motor to send the frame to + * @param opcode the opcode to include in the transmitted frame + */ + void sendFrame(const MotorIndex& motor, const StSpinOpcode opcode); + + enum class StSpinOpcode + { + SPI_NOOP = 0b00000000, + MOV_AX = 0b10000010, + GET_AX = 0b10000011, + MOV_BX = 0b10000100, + GET_BX = 0b10000101, + SET_SPEEDRAMP = 0b00000010, + GET_SPEED = 0b00000011, + SET_ENCODER = 0b00000100, + GET_ENCODER = 0b00000101, + START_MOTOR = 0b00001000, + STOP_MOTOR = 0b11111111, + ACK_FAULTS = 0b00010000, + GET_FAULT = 0b00010001, + SET_CURRENT = 0b00100000, + GET_CURRENT = 0b00100001, + ACK = 0b11000000, + NACK = 0b11000001, + SPI_ERROR = 0b11100000 + }; + + 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, + }; + + // Start-of-frame and end-of-frame markers + static constexpr uint8_t FRAME_SOF = 0x73; + static constexpr uint8_t FRAME_EOF = 0x45; + + // Length of frame (in number of bytes) + static constexpr unsigned int FRAME_MIN_LEN = 4; + static constexpr unsigned int FRAME_MAX_LEN = FRAME_MIN_LEN + 2; + + // 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; + + static const inline std::unordered_map CHIP_SELECTS = { + {MotorIndex::FRONT_LEFT, FRONT_LEFT_MOTOR_CHIP_SELECT}, + {MotorIndex::BACK_LEFT, BACK_LEFT_MOTOR_CHIP_SELECT}, + {MotorIndex::BACK_RIGHT, BACK_RIGHT_MOTOR_CHIP_SELECT}, + {MotorIndex::FRONT_RIGHT, FRONT_RIGHT_MOTOR_CHIP_SELECT}, + {MotorIndex::DRIBBLER, DRIBBLER_MOTOR_CHIP_SELECT}, + }; + + // SPI Motor Driver Paths + static const inline std::unordered_map SPI_PATHS = { + {MotorIndex::FRONT_LEFT, "/dev/spidev0.0"}, + {MotorIndex::FRONT_RIGHT, "/dev/spidev0.1"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.2"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.3"}, + {MotorIndex::DRIBBLER, "/dev/spidev0.4"}, + }; + + // SPI Configs + static constexpr uint32_t SPI_SPEED_HZ = 8000000; // 8 Mhz + static constexpr uint32_t MAX_SPI_SPEED_HZ = 8000000; // 8 Mhz + static constexpr uint8_t SPI_BITS = 8; + static constexpr uint32_t SPI_MODE = 0; + + // SPI File Descriptors mapping from Chip Select -> File Descriptor + std::array()> file_descriptors_; }; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index 95eb02e4c4..58cd4a010c 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -59,12 +59,10 @@ TmcMotorController::TmcMotorController() MotorControllerStatus TmcMotorController::earlyPoll() { - auto motors = driveMotors(); - bool encoders_calibrated = - std::accumulate(motors.begin(), motors.end(), false, - [&](const bool& acc, const MotorIndex& motor) { - return acc || encoder_calibrated_[motor]; - }); + auto motors = driveMotors(); + bool encoders_calibrated = std::accumulate( + motors.begin(), motors.end(), false, [&](const bool& acc, const MotorIndex& motor) + { return acc || encoder_calibrated_[motor]; }); if (!encoders_calibrated) { @@ -174,12 +172,10 @@ void TmcMotorController::setup() 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]; - }); + 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"; } @@ -631,10 +627,10 @@ void TmcMotorController::checkEncoderConnections() 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; - }); + 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()) From bd17506c9ba2a337e69f61e1f5892200d1d01405 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 17 May 2025 12:54:36 -0700 Subject: [PATCH 20/71] Implement checkDriverFault in StSpinMotorController --- src/proto/robot_status_msg.proto | 15 +++ .../stspin_motor_controller.cpp | 109 +++++++++++++++++- .../stspin_motor_controller.h | 2 +- 3 files changed, 122 insertions(+), 4 deletions(-) diff --git a/src/proto/robot_status_msg.proto b/src/proto/robot_status_msg.proto index 78156f861a..bed2fe26f9 100644 --- a/src/proto/robot_status_msg.proto +++ b/src/proto/robot_status_msg.proto @@ -86,6 +86,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) @@ -104,6 +105,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 diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index e30cfae529..9bf1fe627c 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -11,22 +11,119 @@ StSpinMotorController::StSpinMotorController() MotorControllerStatus StSpinMotorController::earlyPoll() { + // No encoders to calibrate, so just return OK return MotorControllerStatus::OK; } void StSpinMotorController::setup() { + for (const MotorIndex& motor : reflective_enum::values()) + { + sendFrame(motor, StSpinOpcode.ACK_FAULTS); + } + for (const MotorIndex& motor : reflective_enum::values()) { sendFrame(motor, StSpinOpcode.START_MOTOR); + checkDriverFault(motor); + readThenWriteVelocity(motor, 0); } } -void StSpinMotorController::reset() {} +void StSpinMotorController::reset() +{ + immediatelyDisable(); +} MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) { - return MotorFaultIndicator(); + bool drive_enabled = true; + std::unordered_set motor_faults; + + const uint16_t faults = sendAndReceiveFrame(motor, StSpinOpcode.GET_FAULT, 0); + + if (faults != 0) + { + LOG(WARNING) << "======= Faults For Motor " << motor << "======="; + } + + if (faults & static_cast(StSpinFaultCode::DURATION)) + { + LOG(WARNING) << "DURATION: FOC rate too high"; + motor_faults.insert(TbotsProto::MotorFault::DURATION); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::OVER_VOLT)) + { + LOG(WARNING) << "OVER_VOLT: Software overvoltage"; + motor_faults.insert(TbotsProto::MotorFault::OVER_VOLT); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::UNDER_VOLT)) + { + LOG(WARNING) << "UNDER_VOLT: Software undervoltage"; + motor_faults.insert(TbotsProto::MotorFault::UNDER_VOLT); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::OVER_TEMP)) + { + LOG(WARNING) << "OVER_TEMP: Software over temperature"; + motor_faults.insert(TbotsProto::MotorFault::OVER_TEMP); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::START_UP)) + { + LOG(WARNING) << "START_UP: Start up failed"; + motor_faults.insert(TbotsProto::MotorFault::START_UP); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::SPEED_FDBK)) + { + LOG(WARNING) << "SPEED_FDBK: Speed feedback fault"; + motor_faults.insert(TbotsProto::MotorFault::SPEED_FDBK); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::OVER_CURR)) + { + LOG(WARNING) << "OVER_CURR: Software overcurrent"; + motor_faults.insert(TbotsProto::MotorFault::OVER_CURR); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::SW_ERROR)) + { + LOG(WARNING) << "SW_ERROR: Software error"; + motor_faults.insert(TbotsProto::MotorFault::SW_ERROR); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::SAMPLE_FAULT)) + { + LOG(WARNING) << "SAMPLE_FAULT: Sample fault for testing purposes"; + motor_faults.insert(TbotsProto::MotorFault::SAMPLE_FAULT); + } + + if (faults & static_cast(StSpinFaultCode::OVERCURR_SW)) + { + LOG(INFO) << "OVERCURR_SW: Software overcurrent"; + motor_faults.insert(TbotsProto::MotorFault::OVERCURR_SW); + drive_enabled = false; + } + + if (faults & static_cast(StSpinFaultCode::DP_FAULT)) + { + LOG(WARNING) << "DP_FAULT: Driver protection fault"; + motor_faults.insert(TbotsProto::MotorFault::DP_FAULT); + drive_enabled = false; + } + + return MotorFaultIndicator(drive_enabled, motor_faults); } double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, @@ -47,7 +144,13 @@ double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, return speed; } -void StSpinMotorController::immediatelyDisable() {} +void StSpinMotorController::immediatelyDisable() +{ + for (const MotorIndex& motor : reflective_enum::values()) + { + sendFrame(motor, StSpinOpcode.STOP_MOTOR); + } +} void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) { diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 2e1417ae8e..68d35ef238 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -82,7 +82,7 @@ class StSpinMotorController : public MotorController SW_ERROR = 0x0080, SAMPLE_FAULT = 0x0100, OVERCURR_SW = 0x0200, - DP_FAULT = 0x0400, + DP_FAULT = 0x0400 }; // Start-of-frame and end-of-frame markers From 0982f952678c5ab418074d3468d65c090f47599c Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 17 May 2025 16:37:36 -0700 Subject: [PATCH 21/71] Fix build errors --- src/software/embedded/gpio/BUILD | 2 + src/software/embedded/gpio/setup_gpio.hpp | 33 ++++++++++++ src/software/embedded/motor_controller/BUILD | 6 +++ .../motor_controller/stspin_constants.h | 39 ++++++++++++++ .../stspin_motor_controller.cpp | 44 ++++++++++------ .../stspin_motor_controller.h | 52 +++++-------------- .../motor_controller/tmc_motor_controller.h | 34 +----------- 7 files changed, 122 insertions(+), 88 deletions(-) create mode 100644 src/software/embedded/gpio/setup_gpio.hpp create mode 100644 src/software/embedded/motor_controller/stspin_constants.h diff --git a/src/software/embedded/gpio/BUILD b/src/software/embedded/gpio/BUILD index f3cb8a7043..22727825a9 100644 --- a/src/software/embedded/gpio/BUILD +++ b/src/software/embedded/gpio/BUILD @@ -10,8 +10,10 @@ cc_library( "gpio.h", "gpio_char_dev.h", "gpio_sysfs.h", + "setup_gpio.hpp", ], deps = [ + "//software/embedded/constants", "//software/logger", "//software/logger:network_logger", "//software/util/make_enum", diff --git a/src/software/embedded/gpio/setup_gpio.hpp b/src/software/embedded/gpio/setup_gpio.hpp new file mode 100644 index 0000000000..47e2b58372 --- /dev/null +++ b/src/software/embedded/gpio/setup_gpio.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include + +#include "software/embedded/constants/constants.h" +#include "software/embedded/gpio/gpio.h" +#include "software/embedded/gpio/gpio_char_dev.h" +#include "software/embedded/gpio/gpio_sysfs.h" + +/** + * 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) + */ +template +std::unique_ptr setupGpio(const T& gpio_number, GpioDirection direction, + GpioState initial_state) +{ + if constexpr (PLATFORM == Platform::JETSON_NANO) + { + return std::make_unique(gpio_number, direction, initial_state); + } + else + { + return std::make_unique(gpio_number, direction, initial_state, + "/dev/gpiochip4"); + } +} diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index c831f5ad72..fa5e0731de 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -37,6 +37,7 @@ cc_library( ":motor_controller_status", ":motor_fault_indicator", ":motor_index", + ":stspin_constants", "//software/embedded:spi_utils", "//software/embedded/constants", "//software/embedded/gpio", @@ -54,3 +55,8 @@ cc_library( "//proto:tbots_cc_proto", ], ) + +cc_library( + name = "stspin_constants", + hdrs = ["stspin_constants.h"], +) diff --git a/src/software/embedded/motor_controller/stspin_constants.h b/src/software/embedded/motor_controller/stspin_constants.h new file mode 100644 index 0000000000..6af84902c8 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_constants.h @@ -0,0 +1,39 @@ +#pragma once + +enum class StSpinOpcode +{ + SPI_NOOP = 0b00000000, + MOV_AX = 0b10000010, + GET_AX = 0b10000011, + MOV_BX = 0b10000100, + GET_BX = 0b10000101, + SET_SPEEDRAMP = 0b00000010, + GET_SPEED = 0b00000011, + SET_ENCODER = 0b00000100, + GET_ENCODER = 0b00000101, + START_MOTOR = 0b00001000, + STOP_MOTOR = 0b11111111, + ACK_FAULTS = 0b00010000, + GET_FAULT = 0b00010001, + SET_CURRENT = 0b00100000, + GET_CURRENT = 0b00100001, + ACK = 0b11000000, + NACK = 0b11000001, + SPI_ERROR = 0b11100000 +}; + +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 +}; \ No newline at end of file diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 9bf1fe627c..1e09ea9e17 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -1,6 +1,16 @@ #include "software/embedded/motor_controller/stspin_motor_controller.h" +#include + +#include "shared/constants.h" +#include "software/embedded/spi_utils.h" +#include "software/logger/logger.h" + StSpinMotorController::StSpinMotorController() + : driver_control_enable_gpio_( + setupGpio(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), + reset_gpio_( + setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)) { openSpiFileDescriptor(MotorIndex::FRONT_LEFT); openSpiFileDescriptor(MotorIndex::FRONT_RIGHT); @@ -17,14 +27,20 @@ MotorControllerStatus StSpinMotorController::earlyPoll() void StSpinMotorController::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()) { - sendFrame(motor, StSpinOpcode.ACK_FAULTS); + sendFrame(motor, StSpinOpcode::ACK_FAULTS); } for (const MotorIndex& motor : reflective_enum::values()) { - sendFrame(motor, StSpinOpcode.START_MOTOR); + sendFrame(motor, StSpinOpcode::START_MOTOR); checkDriverFault(motor); readThenWriteVelocity(motor, 0); } @@ -32,7 +48,7 @@ void StSpinMotorController::setup() void StSpinMotorController::reset() { - immediatelyDisable(); + reset_gpio_->setValue(GpioState::LOW); } MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) @@ -40,7 +56,7 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo bool drive_enabled = true; std::unordered_set motor_faults; - const uint16_t faults = sendAndReceiveFrame(motor, StSpinOpcode.GET_FAULT, 0); + const uint16_t faults = sendAndReceiveFrame(motor, StSpinOpcode::GET_FAULT, 0); if (faults != 0) { @@ -129,27 +145,25 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, const int& target_velocity) { - const int16_t speed = sendAndReceiveFrame(motor, StSpinOpcode.GET_SPEED, 0); + const int16_t speed = sendAndReceiveFrame(motor, StSpinOpcode::GET_SPEED, 0); // SET_SPEEDRAMP expects the target motor speed to be in register ax. - sendAndReceiveFrame(motor, StSpinOpcode.MOV_AX, target_velocity); + sendAndReceiveFrame(motor, StSpinOpcode::MOV_AX, + static_cast(target_velocity)); // SET_SPEEDRAMP expects the ramp time in millis to be in register bx. // We do speed ramping ourselves in MotorService, so we just want to // set the target speed without ramping (hence we set reg bx to 0). - sendAndReceiveFrame(motor, StSpinOpcode.MOV_BX, 0); + sendAndReceiveFrame(motor, StSpinOpcode::MOV_BX, 0); - sendFrame(motor, StSpinOpcode.SET_SPEEDRAMP); + sendFrame(motor, StSpinOpcode::SET_SPEEDRAMP); return speed; } void StSpinMotorController::immediatelyDisable() { - for (const MotorIndex& motor : reflective_enum::values()) - { - sendFrame(motor, StSpinOpcode.STOP_MOTOR); - } + driver_control_enable_gpio_->setValue(GpioState::LOW); } void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) @@ -201,15 +215,15 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, tx[0] = FRAME_SOF; tx[1] = static_cast(opcode); - tx[2] = 0xFF & (data >> 8); - tx[3] = 0xFF & data; + tx[2] = static_cast(0xFF & (data >> 8)); + tx[3] = static_cast(0xFF & data); tx[4] = 0; // CRC; to be implemented later tx[5] = FRAME_EOF; spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_MAX_LEN, SPI_SPEED_HZ); - return static_cast(static_cast(rx[2]) << 8) | rx[3]); + return static_cast((static_cast(rx[2]) << 8) | rx[3]); } void StSpinMotorController::sendFrame(const MotorIndex& motor, const StSpinOpcode opcode) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 68d35ef238..c167e1c960 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -1,11 +1,17 @@ #pragma once +#include "software/embedded/constants/constants.h" +#include "software/embedded/gpio/setup_gpio.hpp" #include "software/embedded/motor_controller/motor_controller.h" -#include "software/util/make_enum/make_enum.hpp" +#include "software/embedded/motor_controller/motor_fault_indicator.h" +#include "software/embedded/motor_controller/motor_index.h" +#include "software/embedded/motor_controller/stspin_constants.h" class StSpinMotorController : public MotorController { public: + explicit StSpinMotorController(); + MotorControllerStatus earlyPoll() override; void setup() override; @@ -47,44 +53,6 @@ class StSpinMotorController : public MotorController */ void sendFrame(const MotorIndex& motor, const StSpinOpcode opcode); - enum class StSpinOpcode - { - SPI_NOOP = 0b00000000, - MOV_AX = 0b10000010, - GET_AX = 0b10000011, - MOV_BX = 0b10000100, - GET_BX = 0b10000101, - SET_SPEEDRAMP = 0b00000010, - GET_SPEED = 0b00000011, - SET_ENCODER = 0b00000100, - GET_ENCODER = 0b00000101, - START_MOTOR = 0b00001000, - STOP_MOTOR = 0b11111111, - ACK_FAULTS = 0b00010000, - GET_FAULT = 0b00010001, - SET_CURRENT = 0b00100000, - GET_CURRENT = 0b00100001, - ACK = 0b11000000, - NACK = 0b11000001, - SPI_ERROR = 0b11100000 - }; - - 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 - }; - // Start-of-frame and end-of-frame markers static constexpr uint8_t FRAME_SOF = 0x73; static constexpr uint8_t FRAME_EOF = 0x45; @@ -119,10 +87,14 @@ class StSpinMotorController : public MotorController // SPI Configs static constexpr uint32_t SPI_SPEED_HZ = 8000000; // 8 Mhz - static constexpr uint32_t MAX_SPI_SPEED_HZ = 8000000; // 8 Mhz + static constexpr uint32_t MAX_SPI_SPEED_HZ = 32000000; // 32 Mhz static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0; // SPI File Descriptors mapping from Chip Select -> File Descriptor std::array()> file_descriptors_; + + // Enable driver gpio + std::unique_ptr driver_control_enable_gpio_; + std::unique_ptr reset_gpio_; }; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index 7ff94d3da2..3198865f72 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -1,9 +1,7 @@ #pragma once #include "software/embedded/constants/constants.h" -#include "software/embedded/gpio/gpio.h" -#include "software/embedded/gpio/gpio_char_dev.h" -#include "software/embedded/gpio/gpio_sysfs.h" +#include "software/embedded/gpio/setup_gpio.hpp" #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" @@ -27,20 +25,6 @@ class TmcMotorController : public MotorController const int& target_velocity) override; private: - /** - * 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) - */ - template - static std::unique_ptr setupGpio(const T& gpio_number, GpioDirection direction, - GpioState initial_state); - /** * Opens SPI file descriptor * @@ -273,19 +257,3 @@ class TmcMotorController : public MotorController // Trinamics communicate with 5 byte messages static constexpr uint32_t TMC_CMD_MSG_SIZE = 5; }; - -template -std::unique_ptr TmcMotorController::setupGpio(const T& gpio_number, - GpioDirection direction, - GpioState initial_state) -{ - if constexpr (PLATFORM == Platform::JETSON_NANO) - { - return std::make_unique(gpio_number, direction, initial_state); - } - else - { - return std::make_unique(gpio_number, direction, initial_state, - "/dev/gpiochip4"); - } -} From b9a1ddba9c9840ab255d8133ec6d72c048b4649c Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 24 May 2025 15:15:09 -0700 Subject: [PATCH 22/71] Fix spi path bug and add additional xbox controller support --- .../embedded/motor_controller/tmc_motor_controller.h | 6 +++--- src/software/thunderscope/constants.py | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index 3198865f72..1efdd51f85 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -226,9 +226,9 @@ class TmcMotorController : public MotorController // SPI Trinamic Motor Driver Paths static const inline std::unordered_map SPI_PATHS = { {MotorIndex::FRONT_LEFT, "/dev/spidev0.0"}, - {MotorIndex::FRONT_RIGHT, "/dev/spidev0.1"}, - {MotorIndex::BACK_LEFT, "/dev/spidev0.2"}, - {MotorIndex::BACK_RIGHT, "/dev/spidev0.3"}, + {MotorIndex::FRONT_RIGHT, "/dev/spidev0.3"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.1"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.2"}, {MotorIndex::DRIBBLER, "/dev/spidev0.4"}, }; diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index e86f2582e5..126ab24e5c 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -331,6 +331,7 @@ class DiagnosticsConstants: SUPPORTED_CONTROLLERS = { "Microsoft Xbox One X pad", "Microsoft X-Box One S pad", + "Microsoft X-Box 360 pad", "Microsoft Xbox 360 pad", } From 17a2fe97e3469e9dee0d8d5cd1b7cab63176b265 Mon Sep 17 00:00:00 2001 From: x4132 <31595285+x4132@users.noreply.github.com> Date: Sat, 31 May 2025 15:03:56 -0700 Subject: [PATCH 23/71] Add Frame Alignment --- .../motor_controller/stspin_constants.h | 1 + .../stspin_motor_controller.cpp | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/src/software/embedded/motor_controller/stspin_constants.h b/src/software/embedded/motor_controller/stspin_constants.h index 6af84902c8..341c140229 100644 --- a/src/software/embedded/motor_controller/stspin_constants.h +++ b/src/software/embedded/motor_controller/stspin_constants.h @@ -19,6 +19,7 @@ enum class StSpinOpcode GET_CURRENT = 0b00100001, ACK = 0b11000000, NACK = 0b11000001, + FRAME_ALIGN = 0b11000010, SPI_ERROR = 0b11100000 }; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 1e09ea9e17..2ddddec273 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -5,6 +5,7 @@ #include "shared/constants.h" #include "software/embedded/spi_utils.h" #include "software/logger/logger.h" +#include "stspin_constants.h" StSpinMotorController::StSpinMotorController() : driver_control_enable_gpio_( @@ -209,6 +210,16 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, // To receive data for GET operations, it is expected that we send a master frame // with 2 dummy DATA bytes in order to clock out the 2 DATA bytes from the incoming // slave frame. + // + // However, we first have to align the frame. We send null bytes until we receive the + // frame aligned bit (0xC2). + + uint8_t align_rx = 0; + uint8_t align_tx = 0; + + while (align_rx != StSpinOpcode::FRAME_ALIGN) { + spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx_static, fr_alg, FRAME_MAX_LEN, SPI_SPEED_HZ); + } uint8_t tx[FRAME_MAX_LEN] = {0}; uint8_t rx[FRAME_MAX_LEN] = {0}; @@ -228,6 +239,13 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, void StSpinMotorController::sendFrame(const MotorIndex& motor, const StSpinOpcode opcode) { + uint8_t align_rx = 0; + uint8_t align_tx = 0; + + while (align_rx != StSpinOpcode::FRAME_ALIGN) { + spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx_static, fr_alg, FRAME_MAX_LEN, SPI_SPEED_HZ); + } + uint8_t tx[FRAME_MIN_LEN] = {0}; uint8_t rx[FRAME_MIN_LEN] = {0}; From e9ce0eb0b165fe8411561e1b3227fcfa974dbe8a Mon Sep 17 00:00:00 2001 From: x4132 <31595285+x4132@users.noreply.github.com> Date: Sat, 31 May 2025 15:16:27 -0700 Subject: [PATCH 24/71] Revert "Add Frame Alignment" This reverts commit 17a2fe97e3469e9dee0d8d5cd1b7cab63176b265. --- .../motor_controller/stspin_constants.h | 1 - .../stspin_motor_controller.cpp | 18 ------------------ 2 files changed, 19 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_constants.h b/src/software/embedded/motor_controller/stspin_constants.h index 341c140229..6af84902c8 100644 --- a/src/software/embedded/motor_controller/stspin_constants.h +++ b/src/software/embedded/motor_controller/stspin_constants.h @@ -19,7 +19,6 @@ enum class StSpinOpcode GET_CURRENT = 0b00100001, ACK = 0b11000000, NACK = 0b11000001, - FRAME_ALIGN = 0b11000010, SPI_ERROR = 0b11100000 }; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 2ddddec273..1e09ea9e17 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -5,7 +5,6 @@ #include "shared/constants.h" #include "software/embedded/spi_utils.h" #include "software/logger/logger.h" -#include "stspin_constants.h" StSpinMotorController::StSpinMotorController() : driver_control_enable_gpio_( @@ -210,16 +209,6 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, // To receive data for GET operations, it is expected that we send a master frame // with 2 dummy DATA bytes in order to clock out the 2 DATA bytes from the incoming // slave frame. - // - // However, we first have to align the frame. We send null bytes until we receive the - // frame aligned bit (0xC2). - - uint8_t align_rx = 0; - uint8_t align_tx = 0; - - while (align_rx != StSpinOpcode::FRAME_ALIGN) { - spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx_static, fr_alg, FRAME_MAX_LEN, SPI_SPEED_HZ); - } uint8_t tx[FRAME_MAX_LEN] = {0}; uint8_t rx[FRAME_MAX_LEN] = {0}; @@ -239,13 +228,6 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, void StSpinMotorController::sendFrame(const MotorIndex& motor, const StSpinOpcode opcode) { - uint8_t align_rx = 0; - uint8_t align_tx = 0; - - while (align_rx != StSpinOpcode::FRAME_ALIGN) { - spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx_static, fr_alg, FRAME_MAX_LEN, SPI_SPEED_HZ); - } - uint8_t tx[FRAME_MIN_LEN] = {0}; uint8_t rx[FRAME_MIN_LEN] = {0}; From a813a365521d719e606f469cdac82eb967421729 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 14 Jul 2025 23:28:08 -0700 Subject: [PATCH 25/71] Update StSpinMotorController frame transmission to match new spec --- .../stspin_motor_controller.cpp | 51 +++++++------------ .../stspin_motor_controller.h | 19 +++---- 2 files changed, 26 insertions(+), 44 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 1e09ea9e17..26a2618114 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -35,12 +35,12 @@ void StSpinMotorController::setup() for (const MotorIndex& motor : reflective_enum::values()) { - sendFrame(motor, StSpinOpcode::ACK_FAULTS); + sendAndReceiveFrame(motor, StSpinOpcode::ACK_FAULTS); } for (const MotorIndex& motor : reflective_enum::values()) { - sendFrame(motor, StSpinOpcode::START_MOTOR); + sendAndReceiveFrame(motor, StSpinOpcode::START_MOTOR); checkDriverFault(motor); readThenWriteVelocity(motor, 0); } @@ -56,7 +56,11 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo bool drive_enabled = true; std::unordered_set motor_faults; - const uint16_t faults = sendAndReceiveFrame(motor, StSpinOpcode::GET_FAULT, 0); + sendAndReceiveFrame(motor, StSpinOpcode::GET_FAULT); + + // We must send a SPI_NOOP in order to clock out the response + // to the GET_FAULT request. + const uint16_t faults = sendAndReceiveFrame(motor, StSpinOpcode::SPI_NOOP); if (faults != 0) { @@ -145,10 +149,12 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, const int& target_velocity) { - const int16_t speed = sendAndReceiveFrame(motor, StSpinOpcode::GET_SPEED, 0); + sendAndReceiveFrame(motor, StSpinOpcode::GET_SPEED); // SET_SPEEDRAMP expects the target motor speed to be in register ax. - sendAndReceiveFrame(motor, StSpinOpcode::MOV_AX, + // Also, the frame we receive here contains the response to the GET_SPEED + // request made in the previous frame. + const int16_t current_velocity = sendAndReceiveFrame(motor, StSpinOpcode::MOV_AX, static_cast(target_velocity)); // SET_SPEEDRAMP expects the ramp time in millis to be in register bx. @@ -156,9 +162,9 @@ double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, // set the target speed without ramping (hence we set reg bx to 0). sendAndReceiveFrame(motor, StSpinOpcode::MOV_BX, 0); - sendFrame(motor, StSpinOpcode::SET_SPEEDRAMP); + sendAndReceiveFrame(motor, StSpinOpcode::SET_SPEEDRAMP); - return speed; + return current_velocity; } void StSpinMotorController::immediatelyDisable() @@ -192,7 +198,7 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, const StSpinOpcode opcode, const int16_t data) { - // A full frame is 6 bytes long and has the following format: + // A frame is 6 bytes long and has the following format: // // +-------+--------+---------------+-------+-------+ // | SOF | OPCODE | DATA | CRC | EOF | @@ -200,18 +206,13 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, // 0 1 2 3 4 5 // // For slave frames, OPCODE will be either an ACK or NACK acknowledging - // receival of the master frame. + // receival of the last master frame. // // The byte order of DATA is big endian; therefore the MSB is transmitted first. - // DATA may be omitted if the operation has no data to transmit/receive, making - // the frame only 4 bytes long. - // - // To receive data for GET operations, it is expected that we send a master frame - // with 2 dummy DATA bytes in order to clock out the 2 DATA bytes from the incoming - // slave frame. + // DATA may be ignored if the operation has no data to transmit/receive. - uint8_t tx[FRAME_MAX_LEN] = {0}; - uint8_t rx[FRAME_MAX_LEN] = {0}; + uint8_t tx[FRAME_LEN] = {0}; + uint8_t rx[FRAME_LEN] = {0}; tx[0] = FRAME_SOF; tx[1] = static_cast(opcode); @@ -220,22 +221,8 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, tx[4] = 0; // CRC; to be implemented later tx[5] = FRAME_EOF; - spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_MAX_LEN, + spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_LEN, SPI_SPEED_HZ); return static_cast((static_cast(rx[2]) << 8) | rx[3]); } - -void StSpinMotorController::sendFrame(const MotorIndex& motor, const StSpinOpcode opcode) -{ - uint8_t tx[FRAME_MIN_LEN] = {0}; - uint8_t rx[FRAME_MIN_LEN] = {0}; - - tx[0] = FRAME_SOF; - tx[1] = static_cast(opcode); - tx[2] = 0; // CRC; to be implemented later - tx[3] = FRAME_EOF; - - spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_MIN_LEN, - SPI_SPEED_HZ); -} diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index c167e1c960..f1e5de68b0 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -36,30 +36,25 @@ class StSpinMotorController : public MotorController /** * Transmits a frame to the given motor and receives a frame back over SPI. * + * Note: to receive data for GET operations, it is expected that we send a + * second frame (e.g. a NOOP) after the initial frame requesting the GET. + * The data in the second frame received will contain the response to the + * GET request. + * * @param motor the motor to send the frame to * @param opcode the opcode to include in the transmitted frame * @param data the data to include in the transmitted frame * @return the data in the frame received from the motor */ int16_t sendAndReceiveFrame(const MotorIndex& motor, const StSpinOpcode opcode, - const int16_t data); - - /** - * Transmits a frame to the given motor over SPI without expecting any data - * back in response. - * - * @param motor the motor to send the frame to - * @param opcode the opcode to include in the transmitted frame - */ - void sendFrame(const MotorIndex& motor, const StSpinOpcode opcode); + const int16_t data = 0); // Start-of-frame and end-of-frame markers static constexpr uint8_t FRAME_SOF = 0x73; static constexpr uint8_t FRAME_EOF = 0x45; // Length of frame (in number of bytes) - static constexpr unsigned int FRAME_MIN_LEN = 4; - static constexpr unsigned int FRAME_MAX_LEN = FRAME_MIN_LEN + 2; + static constexpr unsigned int FRAME_LEN = 6; // SPI Chip Selects static constexpr uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT = 0; From ae9d0c1c1711a438705ac55753f0290906a642cf Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 15 Jul 2025 22:46:29 -0700 Subject: [PATCH 26/71] Add CRC checksum --- src/WORKSPACE | 7 ++ src/extlibs/cppcrc.BUILD | 9 ++ src/software/embedded/motor_controller/BUILD | 1 + .../motor_controller/stspin_constants.h | 2 +- .../stspin_motor_controller.cpp | 13 ++- .../stspin_motor_controller.h | 4 +- .../motor_controller/tmc_motor_controller.h | 94 +++++++++---------- 7 files changed, 77 insertions(+), 53 deletions(-) create mode 100644 src/extlibs/cppcrc.BUILD diff --git a/src/WORKSPACE b/src/WORKSPACE index d2e317c669..bb1bad762d 100644 --- a/src/WORKSPACE +++ b/src/WORKSPACE @@ -139,6 +139,13 @@ http_archive( url = "https://github.com/ReneNyffenegger/cpp-base64/archive/refs/tags/V2.rc.08.zip", ) +http_archive( + name = "cppcrc", + build_file = "@//extlibs:cppcrc.BUILD", + strip_prefix = "cppcrc-6360eee1c8966d32b2552ce156b135ec6a3235f1", + url = "https://github.com/DarrenLevine/cppcrc/archive/6360eee1c8966d32b2552ce156b135ec6a3235f1.zip", +) + load("@bazel_tools//tools/build_defs/repo:git.bzl", "git_repository") git_repository( 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/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index fa5e0731de..df459a13a0 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -42,6 +42,7 @@ cc_library( "//software/embedded/constants", "//software/embedded/gpio", "//software/logger", + "@cppcrc", "@trinamic", ], ) diff --git a/src/software/embedded/motor_controller/stspin_constants.h b/src/software/embedded/motor_controller/stspin_constants.h index 6af84902c8..a7cc4a41b6 100644 --- a/src/software/embedded/motor_controller/stspin_constants.h +++ b/src/software/embedded/motor_controller/stspin_constants.h @@ -36,4 +36,4 @@ enum class StSpinFaultCode SAMPLE_FAULT = 0x0100, OVERCURR_SW = 0x0200, DP_FAULT = 0x0400 -}; \ No newline at end of file +}; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 26a2618114..9f8df0da0e 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -2,10 +2,15 @@ #include +#include "cppcrc.h" #include "shared/constants.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() : driver_control_enable_gpio_( setupGpio(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), @@ -154,8 +159,8 @@ double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, // SET_SPEEDRAMP expects the target motor speed to be in register ax. // Also, the frame we receive here contains the response to the GET_SPEED // request made in the previous frame. - const int16_t current_velocity = sendAndReceiveFrame(motor, StSpinOpcode::MOV_AX, - static_cast(target_velocity)); + const int16_t current_velocity = sendAndReceiveFrame( + motor, StSpinOpcode::MOV_AX, static_cast(target_velocity)); // SET_SPEEDRAMP expects the ramp time in millis to be in register bx. // We do speed ramping ourselves in MotorService, so we just want to @@ -218,9 +223,11 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, tx[1] = static_cast(opcode); tx[2] = static_cast(0xFF & (data >> 8)); tx[3] = static_cast(0xFF & data); - tx[4] = 0; // CRC; to be implemented later tx[5] = FRAME_EOF; + const uint8_t message[] = {tx[1], tx[2], tx[3]}; + tx[4] = Crc8Autosar::calc(message, sizeof(message)); + spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_LEN, SPI_SPEED_HZ); diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index f1e5de68b0..beb9ae63f4 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -81,8 +81,8 @@ class StSpinMotorController : public MotorController }; // SPI Configs - static constexpr uint32_t SPI_SPEED_HZ = 8000000; // 8 Mhz - static constexpr uint32_t MAX_SPI_SPEED_HZ = 32000000; // 32 Mhz + static constexpr uint32_t SPI_SPEED_HZ = 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 = 0; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index 1efdd51f85..3d4a2835d1 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -24,6 +24,53 @@ class TmcMotorController : public MotorController double readThenWriteVelocity(const MotorIndex& motor, const int& target_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 @@ -104,53 +151,6 @@ class TmcMotorController : public MotorController void configureEncoder(const MotorIndex& motor); void configureHall(const MotorIndex& motor); - /** - * 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); - /** * Trinamic API Binding function * From a6de20e1c18003f3de3597815d58cf1c7691fa14 Mon Sep 17 00:00:00 2001 From: williamckha Date: Wed, 16 Jul 2025 12:48:51 -0700 Subject: [PATCH 27/71] Remove unnecessary includes --- src/software/embedded/services/motor.cpp | 21 --------------------- src/software/embedded/services/motor.h | 1 - 2 files changed, 22 deletions(-) diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 22383c1dfc..ce6c31cb19 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -1,31 +1,10 @@ #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" static const uint32_t NUM_RETRIES_SPI = 3; diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 37ac632170..eeda4d5081 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -3,7 +3,6 @@ #include #include #include -#include #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" From 9169e469ed9b0cb8ca5aafb8e4860e2101212859 Mon Sep 17 00:00:00 2001 From: williamckha Date: Wed, 16 Jul 2025 23:32:04 -0700 Subject: [PATCH 28/71] Ignore conversion warnings for cppcrc --- .../embedded/motor_controller/stspin_motor_controller.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 9f8df0da0e..0bffc70ecb 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -2,7 +2,11 @@ #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wconversion" #include "cppcrc.h" +#pragma GCC diagnostic pop + #include "shared/constants.h" #include "software/embedded/spi_utils.h" #include "software/logger/logger.h" From d515cee6a015e1f6a7aa5eed1cb66df84f5b4c0c Mon Sep 17 00:00:00 2001 From: williamckha Date: Thu, 14 Aug 2025 23:39:25 -0700 Subject: [PATCH 29/71] testing spi --- src/software/embedded/BUILD | 23 ++++ src/software/embedded/ansible/BUILD | 1 + .../playbooks/robot_auto_test_playbook.yml | 10 -- .../stspin_motor_controller.cpp | 54 +++++++--- .../stspin_motor_controller.h | 11 +- src/software/embedded/services/BUILD | 29 +---- .../embedded/services/robot_auto_test.cpp | 100 ++++-------------- src/software/embedded/thunderloop.cpp | 4 +- 8 files changed, 100 insertions(+), 132 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 4a15a5e006..34c38659b2 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -33,6 +33,29 @@ string_flag( ], ) +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 = "platform", hdrs = ["platform.h"], diff --git a/src/software/embedded/ansible/BUILD b/src/software/embedded/ansible/BUILD index 3986b1c652..d34e119a46 100644 --- a/src/software/embedded/ansible/BUILD +++ b/src/software/embedded/ansible/BUILD @@ -26,6 +26,7 @@ py_binary( "//software/embedded/linux_configs/systemd:systemd_files", "//software/embedded/redis", "//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/robot_auto_test_playbook.yml b/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml index 5e337bacf4..4e7e2fab86 100644 --- a/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml +++ b/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml @@ -54,13 +54,3 @@ - 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/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 0bffc70ecb..984ea33aa2 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -16,16 +16,16 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController() - : driver_control_enable_gpio_( - setupGpio(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), - reset_gpio_( + : reset_gpio_( setupGpio(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); + for (const MotorIndex& motor : reflective_enum::values()) + { + if (ENABLED_MOTORS.at(motor)) + { + openSpiFileDescriptor(motor); + } + } } MotorControllerStatus StSpinMotorController::earlyPoll() @@ -44,20 +44,30 @@ void StSpinMotorController::setup() for (const MotorIndex& motor : reflective_enum::values()) { - sendAndReceiveFrame(motor, StSpinOpcode::ACK_FAULTS); + if (ENABLED_MOTORS.at(motor)) + { + sendAndReceiveFrame(motor, StSpinOpcode::ACK_FAULTS); + } } for (const MotorIndex& motor : reflective_enum::values()) { - sendAndReceiveFrame(motor, StSpinOpcode::START_MOTOR); - checkDriverFault(motor); - readThenWriteVelocity(motor, 0); + if (ENABLED_MOTORS.at(motor)) + { + sendAndReceiveFrame(motor, StSpinOpcode::START_MOTOR); + checkDriverFault(motor); + readThenWriteVelocity(motor, 0); + } } } void StSpinMotorController::reset() { reset_gpio_->setValue(GpioState::LOW); + usleep(MICROSECONDS_PER_MILLISECOND * 100); + + reset_gpio_->setValue(GpioState::HIGH); + usleep(MICROSECONDS_PER_MILLISECOND * 100); } MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) @@ -65,6 +75,13 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo bool drive_enabled = true; std::unordered_set motor_faults; + if (!ENABLED_MOTORS.at(motor)) + { + // Motor is disabled; pretend that the motor is working fine so + // that Thunderloop doesn't attempt to reset it and crash + return MotorFaultIndicator(drive_enabled, motor_faults); + } + sendAndReceiveFrame(motor, StSpinOpcode::GET_FAULT); // We must send a SPI_NOOP in order to clock out the response @@ -158,6 +175,13 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, const int& target_velocity) { + if (!ENABLED_MOTORS.at(motor)) + { + // Motor is disabled; pretend that the motor is working fine so + // that Thunderloop doesn't attempt to reset it and crash + return 0; + } + sendAndReceiveFrame(motor, StSpinOpcode::GET_SPEED); // SET_SPEEDRAMP expects the target motor speed to be in register ax. @@ -178,7 +202,6 @@ double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, void StSpinMotorController::immediatelyDisable() { - driver_control_enable_gpio_->setValue(GpioState::LOW); } void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) @@ -235,5 +258,10 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_LEN, SPI_SPEED_HZ); + LOG(INFO) << "TX " << static_cast(tx[0]) << " " << static_cast(tx[1]) << " " << static_cast(tx[2]) + << " " << static_cast(tx[3]) << " " << static_cast(tx[4]) << " " << static_cast(tx[5]); + LOG(INFO) << "RX " << static_cast(rx[0]) << " " << static_cast(rx[1]) << " " << static_cast(rx[2]) + << " " << static_cast(rx[3]) << " " << static_cast(rx[4]) << " " << static_cast(rx[5]); + return static_cast((static_cast(rx[2]) << 8) | rx[3]); } diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index beb9ae63f4..dd49c18cf5 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -25,7 +25,6 @@ class StSpinMotorController : public MotorController void immediatelyDisable() override; - private: /** * Opens a SPI file descriptor for the given motor * @@ -63,6 +62,14 @@ class StSpinMotorController : public MotorController static constexpr uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3; static constexpr uint8_t DRIBBLER_MOTOR_CHIP_SELECT = 4; + static const inline std::unordered_map ENABLED_MOTORS = { + {MotorIndex::FRONT_LEFT, true}, + {MotorIndex::BACK_LEFT, false}, + {MotorIndex::BACK_RIGHT, false}, + {MotorIndex::FRONT_RIGHT, false}, + {MotorIndex::DRIBBLER, false}, + }; + static const inline std::unordered_map CHIP_SELECTS = { {MotorIndex::FRONT_LEFT, FRONT_LEFT_MOTOR_CHIP_SELECT}, {MotorIndex::BACK_LEFT, BACK_LEFT_MOTOR_CHIP_SELECT}, @@ -89,7 +96,5 @@ class StSpinMotorController : public MotorController // SPI File Descriptors mapping from Chip Select -> File Descriptor std::array()> file_descriptors_; - // Enable driver gpio - std::unique_ptr driver_control_enable_gpio_; std::unique_ptr reset_gpio_; }; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 09cad6e353..e2b59842e4 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -1,30 +1,5 @@ package(default_visibility = ["//visibility:public"]) -load("@bazel_skylib//rules:common_settings.bzl", "string_flag") - -string_flag( - name = "motor_board", - build_setting_default = "TRINAMIC", - values = [ - "TRINAMIC", - "STSPIN", - ], -) - -config_setting( - name = "build_trinamic", - flag_values = { - ":motor_board": "TRINAMIC", - }, -) - -config_setting( - name = "build_stspin", - flag_values = { - ":motor_board": "STSPIN", - }, -) - cc_library( name = "motor", srcs = ["motor.cpp"], @@ -36,8 +11,8 @@ cc_library( "//software/embedded:build_limited": ["LIMITED"], }) + select({ - ":build_trinamic": ["TRINAMIC_MOTOR_BOARD"], - ":build_stspin": ["STSPIN_MOTOR_BOARD"], + "//software/embedded:build_trinamic": ["TRINAMIC_MOTOR_BOARD"], + "//software/embedded:build_stspin": ["STSPIN_MOTOR_BOARD"], }), deps = [ "//proto:tbots_cc_proto", diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index 5307b16815..6c31a5092b 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -2,106 +2,52 @@ #include "proto/primitive/primitive_msg_factory.h" #include "shared/2021_robot_constants.h" #include "shared/constants.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/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 "external/trinamic/tmc/ic/TMC4671/TMC4671.h" -#include "external/trinamic/tmc/ic/TMC4671/TMC4671_Variants.h" -#include "external/trinamic/tmc/ic/TMC6100/TMC6100.h" -} +#include +#include -std::unique_ptr motor_service_; +std::unique_ptr motor_controller_; std::unique_ptr power_service_; RobotConstants_t 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"; +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 Jetson Nano!"; + LOG(INFO) << "Running Robot Auto Test"; - motor_service_ = - std::make_unique(create2021RobotConstants(), THUNDERLOOP_HZ); + motor_controller_ = std::make_unique(); + motor_controller_->setup(); - // Testing Motor board SPI transfer - for (uint8_t chip_select : CHIP_SELECT) + if (motor_controller_->earlyPoll() == MotorControllerStatus::OK) { - 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); + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_LEFT, 60); + std::this_thread::sleep_for(std::chrono::seconds(15)); + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_LEFT, 0); + } - // 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"; - } - } +// for (int i = 0; i < 1000; ++i) +// { +// motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::SPI_NOOP); +// } - motor_service_->resetMotorBoard(); + motor_controller_->reset(); - // 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"; - } + LOG(INFO) << "Robot Auto Test Complete"; return 0; } diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 7e4065034a..739f11dd9b 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -120,7 +120,7 @@ Thunderloop::Thunderloop(const RobotConstants_t& robot_constants, bool enable_lo return; } - power_service_ = std::make_unique(); + //power_service_ = std::make_unique(); LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; @@ -288,7 +288,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)); From 7695852e99ff0db97a7c800c49865912570a86aa Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 19 Aug 2025 21:57:33 -0700 Subject: [PATCH 30/71] Changes from scrimmage weekend --- docs/useful-robot-commands.md | 3 +- src/shared/2021_robot_constants.cpp | 6 +- src/shared/constants.h | 2 +- src/software/constants.h | 2 +- .../stspin_motor_controller.cpp | 20 +++---- .../stspin_motor_controller.h | 20 +++---- src/software/embedded/services/motor.cpp | 57 ++++++++++--------- src/software/embedded/services/motor.h | 7 ++- .../embedded/services/robot_auto_test.cpp | 55 ++++++++++++++---- 9 files changed, 103 insertions(+), 69 deletions(-) diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md index 20a54dd65c..27fff65a51 100644 --- a/docs/useful-robot-commands.md +++ b/docs/useful-robot-commands.md @@ -107,8 +107,9 @@ 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. -`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform= --//software/embedded:motor_board= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` * \ is the host platform on the robot (either `PI` or `NANO`) +* 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 diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp index f2900737a4..34e48c1777 100644 --- a/src/shared/2021_robot_constants.cpp +++ b/src/shared/2021_robot_constants.cpp @@ -8,11 +8,9 @@ RobotConstants_t create2021RobotConstants(void) .mass_kg = 2.5f, // determined experimentally .inertial_factor = 0.37f, // determined experimentally .robot_radius_m = ROBOT_MAX_RADIUS_METERS, - // TODO (#2112): update this .jerk_limit_kg_m_per_s_3 = 40.0f, - .front_wheel_angle_deg = 32.06f, - .back_wheel_angle_deg = 46.04f, - // TODO (#2112): update this + .front_wheel_angle_deg = 25.0f, + .back_wheel_angle_deg = 40.0f, .front_of_robot_width_meters = 0.11f, .dribbler_width_meters = 0.07825f, diff --git a/src/shared/constants.h b/src/shared/constants.h index 8030105b04..d247c0b31a 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -72,7 +72,7 @@ static const double BALL_IN_PLAY_DISTANCE_THRESHOLD_METERS = 0.05; // The max allowed height of the robots, in metres static const double ROBOT_MAX_HEIGHT_METERS = 0.15; // The max allowed radius of the robots, in metres -static const double ROBOT_MAX_RADIUS_METERS = 0.09; +static const double ROBOT_MAX_RADIUS_METERS = 0.085; // The distance from the center of the robot to the front face (the flat part), in meters static const double DIST_TO_FRONT_OF_ROBOT_METERS = 0.078; // The approximate radius of the ball according to the SSL rulebook diff --git a/src/software/constants.h b/src/software/constants.h index 0d7156f665..f2af29fb19 100644 --- a/src/software/constants.h +++ b/src/software/constants.h @@ -56,7 +56,7 @@ const std::string ROBOT_BATTERY_VOLTAGE_REDIS_KEY = "/battery_voltage"; const std::string ROBOT_CAPACITOR_VOLTAGE_REDIS_KEY = "/cap_voltage"; 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/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 984ea33aa2..edb5f708e0 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -36,12 +36,6 @@ MotorControllerStatus StSpinMotorController::earlyPoll() void StSpinMotorController::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()) { if (ENABLED_MOTORS.at(motor)) @@ -54,20 +48,20 @@ void StSpinMotorController::setup() { if (ENABLED_MOTORS.at(motor)) { - sendAndReceiveFrame(motor, StSpinOpcode::START_MOTOR); - checkDriverFault(motor); + checkDriverFault(motor); readThenWriteVelocity(motor, 0); + sendAndReceiveFrame(motor, StSpinOpcode::START_MOTOR); } } } void StSpinMotorController::reset() { - reset_gpio_->setValue(GpioState::LOW); - usleep(MICROSECONDS_PER_MILLISECOND * 100); - - reset_gpio_->setValue(GpioState::HIGH); - usleep(MICROSECONDS_PER_MILLISECOND * 100); +// reset_gpio_->setValue(GpioState::LOW); +// usleep(MICROSECONDS_PER_MILLISECOND * 100); +// +// reset_gpio_->setValue(GpioState::HIGH); +// usleep(MICROSECONDS_PER_MILLISECOND * 100); } MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index dd49c18cf5..b92ab02494 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -64,9 +64,9 @@ class StSpinMotorController : public MotorController static const inline std::unordered_map ENABLED_MOTORS = { {MotorIndex::FRONT_LEFT, true}, - {MotorIndex::BACK_LEFT, false}, - {MotorIndex::BACK_RIGHT, false}, - {MotorIndex::FRONT_RIGHT, false}, + {MotorIndex::BACK_LEFT, true}, + {MotorIndex::BACK_RIGHT, true}, + {MotorIndex::FRONT_RIGHT, true}, {MotorIndex::DRIBBLER, false}, }; @@ -80,16 +80,16 @@ class StSpinMotorController : public MotorController // SPI Motor Driver Paths static const inline std::unordered_map SPI_PATHS = { - {MotorIndex::FRONT_LEFT, "/dev/spidev0.0"}, - {MotorIndex::FRONT_RIGHT, "/dev/spidev0.1"}, - {MotorIndex::BACK_LEFT, "/dev/spidev0.2"}, - {MotorIndex::BACK_RIGHT, "/dev/spidev0.3"}, - {MotorIndex::DRIBBLER, "/dev/spidev0.4"}, + {MotorIndex::FRONT_LEFT, "/dev/spidev0.2"}, + {MotorIndex::FRONT_RIGHT, "/dev/spidev0.4"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.3"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.0"}, + {MotorIndex::DRIBBLER, "/dev/spidev0.1"}, }; // SPI Configs - static constexpr uint32_t SPI_SPEED_HZ = 1000000; // 1 Mhz - static constexpr uint32_t MAX_SPI_SPEED_HZ = 2000000; // 2 Mhz + static constexpr uint32_t SPI_SPEED_HZ = 100000; + static constexpr uint32_t MAX_SPI_SPEED_HZ = 100000; static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index ce6c31cb19..1aba92ad8e 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -209,33 +209,36 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor 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) - { - 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) - { - 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) - { - 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) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Back right motor runaway"; + // TODO: fix me + if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { + if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - + prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > + RUNAWAY_PROTECTION_THRESHOLD_MPS) + { + 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) + { + 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) + { + 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) + { + motor_controller_->immediatelyDisable(); + LOG(FATAL) << "Back right motor runaway"; + } } // Convert to Euclidean velocity_delta diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index eeda4d5081..60692a76bd 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -103,9 +103,14 @@ class MotorService // // TODO (#2720): compute from robot constants (this was computed by hand and is // accurate) - static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 0.000111; +#ifdef TRINAMIC_MOTOR_BOARD + static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 0.000111; static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; +#elif STSPIN_MOTOR_BOARD + static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = 60 / (3.1415926 * 0.06); + static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 1 / ELECTRICAL_RPM_PER_MECHANICAL_MPS; +#endif // Controller for communicating with the motor board std::unique_ptr motor_controller_; diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index 6c31a5092b..adbeb067d1 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -33,21 +33,54 @@ int main(int argc, char **argv) motor_controller_ = std::make_unique(); motor_controller_->setup(); - if (motor_controller_->earlyPoll() == MotorControllerStatus::OK) + LOG(INFO) << "Motor controller setup complete"; + + LOG(INFO) << "Waiting..."; + std::this_thread::sleep_for(std::chrono::seconds(2)); + + /*if (motor_controller_->earlyPoll() == MotorControllerStatus::OK) + { + for (int i = 0; i <= 100; ++i) + { + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 1500); + LOG(INFO) << "Waiting..."; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 0); + }*/ + + LOG(INFO) << "Waiting..."; + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 1500); + LOG(INFO) << "Waiting..."; + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 0); + + +/* + for (int i = 0; i < 1000; ++i) { - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_LEFT, 60); - std::this_thread::sleep_for(std::chrono::seconds(15)); - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_LEFT, 0); - } + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::SPI_NOOP); + } +*/ -// for (int i = 0; i < 1000; ++i) -// { -// motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::SPI_NOOP); -// } +// motor_controller_->reset(); - motor_controller_->reset(); +/* + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::MOV_AX, + static_cast( + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch() + ).count() % 5000 + )); +*/ - LOG(INFO) << "Robot Auto Test Complete"; +/* + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::MOV_BX, 1000); + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::SET_SPEEDRAMP); + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::START_MOTOR); +*/ + LOG(INFO) << "Robot Auto Test Complete"; return 0; } From 17ae9211fca0c84831a321f1f1cf6e5dc8effccd Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 24 Oct 2025 12:02:42 -0700 Subject: [PATCH 31/71] Fix build issues and update docs --- docs/getting-started.md | 6 +++--- docs/robot-software-architecture.md | 2 +- docs/useful-robot-commands.md | 12 ++++++------ docs/wifi-communication-readme.md | 2 +- src/MODULE.bazel | 7 +++++++ src/WORKSPACE | 0 .../motor_controller/tmc_motor_controller.cpp | 8 ++++---- 7 files changed, 22 insertions(+), 15 deletions(-) delete mode 100644 src/WORKSPACE diff --git a/docs/getting-started.md b/docs/getting-started.md index d180789f6d..8c6ef4d242 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -357,7 +357,7 @@ Tracy also samples call stacks. If the profiled binary is run with root permissi ## Building for the robot -To build for the robot computer, build the target with the `--platforms=//cc_toolchain:robot` flag and the toolchain will automatically build using the ARM toolchain. For example, `bazel build --platforms=//cc_toolchain:robot //software/geom/...`. +To build for the robot computer, build the target with the `--platforms=//toolchains/cc:robot` flag and the toolchain will automatically build using the ARM toolchain. For example, `bazel build --platforms=//toolchains/cc:robot //software/geom/...`. ## Deploying Robot Software to the robot @@ -365,7 +365,7 @@ We use Ansible to automatically update software running on the robot. [More info To update binaries on a working robot, you can run: -`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` Where `` is the robot platform you are deploying to (`PI` or `NANO`), and `` is the IP address of the robot you are deploying to. The `robot_password` is the password used to login to the `robot` user on the robot. @@ -388,7 +388,7 @@ It is possible to run Thunderloop without having a fully-working robot. Using th 2. If you have a robot PC that doesn't have proper communication with the power or motor board, you can still run Thunderloop in a limited capacity to test software features (eg. networking). 1. First, build the Thunderloop binary: - - `bazel build //software/embedded:thunderloop_main --//software/embedded:host_platform=LIMITED --platforms=//cc_toolchain:robot` + - `bazel build //software/embedded:thunderloop_main --//software/embedded:host_platform=LIMITED --platforms=//toolchains/cc:robot` 2. Find the `` of the robot you want to run Thunderloop on. This guide may help you find the IP address of the robot: [Useful Robot Commands](useful-robot-commands.md#Wifi-Disclaimer). 3. Copy the binary to the robot: - `scp bazel-bin/software/embedded/thunderloop_main robot@:/home/robot/thunderloop_main` diff --git a/docs/robot-software-architecture.md b/docs/robot-software-architecture.md index cc69d1560c..08f9ecd6bd 100644 --- a/docs/robot-software-architecture.md +++ b/docs/robot-software-architecture.md @@ -26,7 +26,7 @@ For a more detailed look at how Ansible works, [see the RFC](https://docs.google.com/document/d/1hN3Us2Vjr8z6ihqUVp_3L7rrjKc-EZ-l2hZJc31gNOc/edit) -Example command: `bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` +Example command: `bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` * : `PI` or `NANO` depending on the computer on the robot * : IP address of the robot * : Password of the robot diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md index 27fff65a51..e99c19a1ee 100644 --- a/docs/useful-robot-commands.md +++ b/docs/useful-robot-commands.md @@ -93,7 +93,7 @@ If you are not using the tbots network you will need to use a network utility (` Individual miscellaneous tasks (ex reboot, shutdown, rtt test) can be run through the `misc.yml` playbook by specifying the corresponding tag. To view a list of supported arguments, run: -`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot -h` +`bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot -h` If desired, the `-ho`, `--hosts` argument can be replaced with `-p`, `--port`, defining a port to listen to for Announcements from hosts. @@ -107,7 +107,7 @@ 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. -`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform= --//software/embedded:motor_board= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:host_platform= --//software/embedded:motor_board= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` * \ is the host platform on the robot (either `PI` or `NANO`) * is the type of motor driver board on the robot (either `STSPIN` or `TRINAMIC`) * is the IP address of the robot @@ -130,7 +130,7 @@ This will flash powerloop, the current firmware in `software/power/`, onto the p Looking from the back of the robot the reset and boot buttons are on right side of the battery holder on the lowest board with the reset being on the left and the boot on the right. Warning it may kick/chip when pressed. -`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot -- --playbook deploy_powerboard.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot -- --playbook deploy_powerboard.yml --hosts --ssh_pass ` ## Setting up the embedded host @@ -140,11 +140,11 @@ This section refers to setting up the computer on the robot for the first time. ### Jetson Nano -`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform=NANO -- --playbook setup_nano.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:host_platform=NANO -- --playbook setup_nano.yml --hosts --ssh_pass ` ### Raspberry Pi -`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform=PI -- --playbook setup_raspberry_pi.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:host_platform=PI -- --playbook setup_raspberry_pi.yml --hosts --ssh_pass ` ## Robot Diagnostics @@ -171,7 +171,7 @@ Runs the robot auto test fixture on a robot through Ansible, which tests the mot From Software/src: -`bazel run //software/embedded/ansible:run_ansible --//software/embedded:host_platform= --platforms=//cc_toolchain:robot -- --playbook robot_auto_test_playbook.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --//software/embedded:host_platform= --platforms=//toolchains/cc:robot -- --playbook robot_auto_test_playbook.yml --hosts --ssh_pass ` * replace the \ with the target platform for the robot (either `PI` or `NANO`) * replace the \ with the actual ip address of the jetson nano for the ssh connection. * replace the with the actual password for the jetson nano for the ssh connection. diff --git a/docs/wifi-communication-readme.md b/docs/wifi-communication-readme.md index 57389699bd..b10648ee19 100644 --- a/docs/wifi-communication-readme.md +++ b/docs/wifi-communication-readme.md @@ -22,7 +22,7 @@ You must know: - The WiFi interface of the robot. We will refer to this interface as `{robot_wifi_interface}`. This interface is typically found by running `ifconfig` or `ip a` on the robot. - The network interface of the host computer. We will refer to this interface as `{host_interface}`. This interface is typically found by running `ifconfig` or `ip a` on the host computer. -1. Build the latency tester secondary node: `./tbots.py build latency_tester_secondary_node --platforms=//cc_toolchain:robot` +1. Build the latency tester secondary node: `./tbots.py build latency_tester_secondary_node --platforms=//toolchains/cc:robot` 2. Copy the binary to the robot: `scp bazel-bin/software/networking/benchmarking_utils/latency_tester_secondary_node robot@{robot_ip}:/home/robot/latency_tester_secondary_node` 3. SSH into the robot: `ssh robot@{robot_ip}` 4. There are two test modes: multicast or unicast diff --git a/src/MODULE.bazel b/src/MODULE.bazel index e8c9f70171..d7fb311aab 100644 --- a/src/MODULE.bazel +++ b/src/MODULE.bazel @@ -216,6 +216,13 @@ http_archive( url = "https://github.com/saebyn/munkres-cpp/archive/61086fcf5b3a8ad4bda578cdc2d1dca57b548786.tar.gz", ) +http_archive( + name = "cppcrc", + build_file = "@//extlibs:cppcrc.BUILD", + strip_prefix = "cppcrc-6360eee1c8966d32b2552ce156b135ec6a3235f1", + url = "https://github.com/DarrenLevine/cppcrc/archive/6360eee1c8966d32b2552ce156b135ec6a3235f1.zip", +) + ############################################## # Add new git repos (these cannot be used offline) # All of this SHOULD BE REMOVED in favour of archive_override diff --git a/src/WORKSPACE b/src/WORKSPACE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index 58cd4a010c..ecb3a06b12 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -6,10 +6,10 @@ extern "C" { -#include "external/trinamic/tmc/ic/TMC4671/TMC4671.h" -#include "external/trinamic/tmc/ic/TMC4671/TMC4671_Register.h" -#include "external/trinamic/tmc/ic/TMC4671/TMC4671_Variants.h" -#include "external/trinamic/tmc/ic/TMC6100/TMC6100.h" +#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 From 31db8ec4664b11472ec76696ed69b98072ec0488 Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 7 Nov 2025 08:43:58 -0800 Subject: [PATCH 32/71] Testing changes --- .../playbooks/robot_auto_test_playbook.yml | 18 +- .../stspin_motor_controller.cpp | 26 +- .../stspin_motor_controller.h | 4 +- src/software/embedded/services/motor.cpp | 3 +- src/software/embedded/services/motor.h | 5 +- .../embedded/services/robot_auto_test.cpp | 90 ++--- src/software/embedded/thunderloop.cpp | 15 +- .../field_tests/movement_robot_field_test.py | 354 +++++++++--------- 8 files changed, 259 insertions(+), 256 deletions(-) diff --git a/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml b/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml index 4e7e2fab86..87bcd49f4d 100644 --- a/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml +++ b/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml @@ -41,15 +41,15 @@ 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: 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: diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index edb5f708e0..1876fcd3e9 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -48,7 +48,7 @@ void StSpinMotorController::setup() { if (ENABLED_MOTORS.at(motor)) { - checkDriverFault(motor); + checkDriverFault(motor); readThenWriteVelocity(motor, 0); sendAndReceiveFrame(motor, StSpinOpcode::START_MOTOR); } @@ -57,11 +57,11 @@ void StSpinMotorController::setup() void StSpinMotorController::reset() { -// reset_gpio_->setValue(GpioState::LOW); -// usleep(MICROSECONDS_PER_MILLISECOND * 100); -// -// reset_gpio_->setValue(GpioState::HIGH); -// usleep(MICROSECONDS_PER_MILLISECOND * 100); + // reset_gpio_->setValue(GpioState::LOW); + // usleep(MICROSECONDS_PER_MILLISECOND * 100); + // + // reset_gpio_->setValue(GpioState::HIGH); + // usleep(MICROSECONDS_PER_MILLISECOND * 100); } MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) @@ -194,9 +194,7 @@ double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, return current_velocity; } -void StSpinMotorController::immediatelyDisable() -{ -} +void StSpinMotorController::immediatelyDisable() {} void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) { @@ -252,10 +250,12 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_LEN, SPI_SPEED_HZ); - LOG(INFO) << "TX " << static_cast(tx[0]) << " " << static_cast(tx[1]) << " " << static_cast(tx[2]) - << " " << static_cast(tx[3]) << " " << static_cast(tx[4]) << " " << static_cast(tx[5]); - LOG(INFO) << "RX " << static_cast(rx[0]) << " " << static_cast(rx[1]) << " " << static_cast(rx[2]) - << " " << static_cast(rx[3]) << " " << static_cast(rx[4]) << " " << static_cast(rx[5]); + LOG(INFO) << "TX " << static_cast(tx[0]) << " " << static_cast(tx[1]) << " " + << static_cast(tx[2]) << " " << static_cast(tx[3]) << " " + << static_cast(tx[4]) << " " << static_cast(tx[5]); + LOG(INFO) << "RX " << static_cast(rx[0]) << " " << static_cast(rx[1]) << " " + << static_cast(rx[2]) << " " << static_cast(rx[3]) << " " + << static_cast(rx[4]) << " " << static_cast(rx[5]); return static_cast((static_cast(rx[2]) << 8) | rx[3]); } diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index b92ab02494..beb50c23e2 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -88,8 +88,8 @@ class StSpinMotorController : public MotorController }; // SPI Configs - static constexpr uint32_t SPI_SPEED_HZ = 100000; - static constexpr uint32_t MAX_SPI_SPEED_HZ = 100000; + static constexpr uint32_t SPI_SPEED_HZ = 250000; // 250 KHz + static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; // 250 MHz static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 1aba92ad8e..67db6b5c5b 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -210,7 +210,8 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor // Run-away protection // TODO: fix me - if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { + if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) + { if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 60692a76bd..c02ebf6581 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -104,12 +104,13 @@ class MotorService // TODO (#2720): compute from robot constants (this was computed by hand and is // accurate) #ifdef TRINAMIC_MOTOR_BOARD - static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 0.000111; + static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 0.000111; static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; #elif STSPIN_MOTOR_BOARD static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = 60 / (3.1415926 * 0.06); - static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 1 / ELECTRICAL_RPM_PER_MECHANICAL_MPS; + static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = + 1 / ELECTRICAL_RPM_PER_MECHANICAL_MPS; #endif // Controller for communicating with the motor board diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index adbeb067d1..01f4412622 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -1,3 +1,6 @@ +#include +#include + #include "proto/message_translation/tbots_geometry.h" #include "proto/primitive/primitive_msg_factory.h" #include "shared/2021_robot_constants.h" @@ -11,9 +14,6 @@ #include "software/logger/network_logger.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" -#include -#include - std::unique_ptr motor_controller_; std::unique_ptr power_service_; RobotConstants_t robot_constants_; @@ -31,56 +31,62 @@ int main(int argc, char **argv) LOG(INFO) << "Running Robot Auto Test"; motor_controller_ = std::make_unique(); - motor_controller_->setup(); - LOG(INFO) << "Motor controller setup complete"; + motor_controller_->setup(); + + LOG(INFO) << "Motor controller setup complete"; - LOG(INFO) << "Waiting..."; + LOG(INFO) << "Waiting..."; + std::this_thread::sleep_for(std::chrono::seconds(1)); + + motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 1500); + LOG(INFO) << "Waiting..."; std::this_thread::sleep_for(std::chrono::seconds(2)); + motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 0); + + - /*if (motor_controller_->earlyPoll() == MotorControllerStatus::OK) + /*if (motor_controller_->earlyPoll() == MotorControllerStatus::OK) { for (int i = 0; i <= 100; ++i) { - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 1500); - LOG(INFO) << "Waiting..."; - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 1500); + LOG(INFO) << "Waiting..."; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 0); - }*/ + }*/ - LOG(INFO) << "Waiting..."; - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 1500); - LOG(INFO) << "Waiting..."; - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 0); - -/* - for (int i = 0; i < 1000; ++i) - { - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::SPI_NOOP); - } -*/ - -// motor_controller_->reset(); - -/* - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::MOV_AX, - static_cast( - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch() - ).count() % 5000 - )); -*/ - -/* - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::MOV_BX, 1000); - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::SET_SPEEDRAMP); - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, StSpinOpcode::START_MOTOR); -*/ - LOG(INFO) << "Robot Auto Test Complete"; + /* + for (int i = 0; i < 1000; ++i) + { + motor_controller_->sendAndReceiveFrame(MotorIndex::BACK_RIGHT, + StSpinOpcode::SPI_NOOP); + } + */ + + // motor_controller_->reset(); + + /* + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, + StSpinOpcode::MOV_AX, static_cast( + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch() + ).count() % 5000 + )); + */ + + /* + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, + StSpinOpcode::MOV_BX, 1000); + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, + StSpinOpcode::SET_SPEEDRAMP); + motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, + StSpinOpcode::START_MOTOR); + */ + LOG(INFO) << "Robot Auto Test Complete"; return 0; } diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 739f11dd9b..1bf9a7b62c 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -120,13 +120,13 @@ Thunderloop::Thunderloop(const RobotConstants_t& robot_constants, bool enable_lo return; } - //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); - g_motor_service = motor_service_.get(); - motor_service_->setup(); + // motor_service_ = std::make_unique(robot_constants, loop_hz); + // g_motor_service = motor_service_.get(); + // motor_service_->setup(); LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_ @@ -288,7 +288,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)); @@ -328,8 +328,9 @@ void Thunderloop::runLoop() } // Motor Service: execute the motor control command - motor_status_ = pollMotorService(poll_time, direct_control_.motor_control(), - time_since_prev_iter); + // motor_status_ = pollMotorService(poll_time, + // direct_control_.motor_control(), + // time_since_prev_iter); thunderloop_status_.set_motor_service_poll_time_ms( getMilliseconds(poll_time)); diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index bd436869bf..27c0ede5bc 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -4,6 +4,8 @@ from software.simulated_tests.simulated_test_fixture import * from software.logger.logger import create_logger import math +from proto.message_translation.tbots_protobuf import create_world_state +import software.python_bindings as tbots_cpp logger = create_logger(__name__) @@ -14,194 +16,186 @@ # "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_worldState(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, -# ) - - -# this test can only be run on the field -def test_basic_rotation(field_test_runner): - test_angles = [0, 45, 90, 180, 270, 0] - - world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) - if len(world.friendly_team.team_robots) == 0: - raise Exception("The first world received had no robots in it!") - - print("Here are the robots:") - print( - [ - robot.current_state.global_position - for robot in world.friendly_team.team_robots - ] +def test_basic_movement(simulated_test_runner): + robot_starting_x = -2 + robot_starting_y = 0 + ROBOT_ID = 0 + angle = math.pi / 2 + robot_x_destination = 2 + robot_y_destination = 0 + 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_worldState(initial_worldstate) - id = world.friendly_team.team_robots[0].id - print(f"Running test on robot {id}") - - 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 - params = AssignedTacticPlayControlParams() - - params.assigned_tactics[id].move.CopyFrom(move_tactic) - - field_test_runner.set_tactics(params, True) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=5, - ) - # Send a halt tactic after the test finishes - halt_tactic = HaltTactic() - params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].stop.CopyFrom(halt_tactic) - # send the halt tactic - field_test_runner.set_tactics(params, True) - - # validate by eye - logger.info(f"robot set to {angle} orientation") - - time.sleep(2) - - -def test_one_robots_square(field_test_runner): - world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) - if len(world.friendly_team.team_robots) == 0: - raise Exception("The first world received had no robots in it!") - - print("Here are the robots:") - print( - [ - robot.current_state.global_position - for robot in world.friendly_team.team_robots - ] - ) + # Setup Tactic + params = AssignedTacticPlayControlParams() - id = world.friendly_team.team_robots[0].id - print(f"Running test on robot {id}") + params.assigned_tactics[ROBOT_ID].move.CopyFrom(move_tactic) - 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) + # Eventually Validation + eventually_validation_sequence_set = [[]] + simulated_test_runner.set_tactics(params, True) - tactic_0 = MoveTactic( - destination=point1, - dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), - ball_collision_type=BallCollisionType.AVOID, - auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), - max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, - obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, - ) - tactic_1 = MoveTactic( - destination=point2, - dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), - ball_collision_type=BallCollisionType.AVOID, - auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), - max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, - obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, - ) - tactic_2 = MoveTactic( - destination=point3, - dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), - ball_collision_type=BallCollisionType.AVOID, - auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), - max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, - obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, + simulated_test_runner.run_test( + inv_always_validation_sequence_set=[[]], + inv_eventually_validation_sequence_set=[[]], + ag_always_validation_sequence_set=[[]], + ag_eventually_validation_sequence_set=[[]], + test_timeout_s=5, ) - tactic_3 = MoveTactic( - destination=point4, - dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), - ball_collision_type=BallCollisionType.AVOID, - auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), - 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}") - params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].move.CopyFrom(tactic) - field_test_runner.set_tactics(params, True) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=4, - ) - - # Send a halt tactic after the test finishes - halt_tactic = HaltTactic() - params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].stop.CopyFrom(halt_tactic) +# this test can only be run on the field +# def test_basic_rotation(field_test_runner): +# test_angles = [0, 45, 90, 180, 270, 0] +# +# world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) +# if len(world.friendly_team.team_robots) == 0: +# raise Exception("The first world received had no robots in it!") +# +# print("Here are the robots:") +# print( +# [ +# robot.current_state.global_position +# for robot in world.friendly_team.team_robots +# ] +# ) +# +# id = world.friendly_team.team_robots[0].id +# print(f"Running test on robot {id}") +# +# 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 +# params = AssignedTacticPlayControlParams() +# +# params.assigned_tactics[id].move.CopyFrom(move_tactic) +# +# field_test_runner.set_tactics(params, True) +# field_test_runner.run_test( +# always_validation_sequence_set=[[]], +# eventually_validation_sequence_set=[[]], +# test_timeout_s=5, +# ) +# # Send a halt tactic after the test finishes +# halt_tactic = HaltTactic() +# params = AssignedTacticPlayControlParams() +# params.assigned_tactics[id].stop.CopyFrom(halt_tactic) +# # send the halt tactic +# field_test_runner.set_tactics(params, True) +# +# # validate by eye +# logger.info(f"robot set to {angle} orientation") +# +# time.sleep(2) +# +# +# def test_one_robots_square(field_test_runner): +# world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) +# if len(world.friendly_team.team_robots) == 0: +# raise Exception("The first world received had no robots in it!") +# +# print("Here are the robots:") +# print( +# [ +# robot.current_state.global_position +# for robot in world.friendly_team.team_robots +# ] +# ) +# +# 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) +# +# tactic_0 = MoveTactic( +# destination=point1, +# dribbler_mode=DribblerMode.OFF, +# final_orientation=Angle(radians=-math.pi / 2), +# ball_collision_type=BallCollisionType.AVOID, +# auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), +# max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, +# obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, +# ) +# tactic_1 = MoveTactic( +# destination=point2, +# dribbler_mode=DribblerMode.OFF, +# final_orientation=Angle(radians=-math.pi / 2), +# ball_collision_type=BallCollisionType.AVOID, +# auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), +# max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, +# obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, +# ) +# tactic_2 = MoveTactic( +# destination=point3, +# dribbler_mode=DribblerMode.OFF, +# final_orientation=Angle(radians=-math.pi / 2), +# ball_collision_type=BallCollisionType.AVOID, +# auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), +# max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, +# obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, +# ) +# tactic_3 = MoveTactic( +# destination=point4, +# dribbler_mode=DribblerMode.OFF, +# final_orientation=Angle(radians=-math.pi / 2), +# ball_collision_type=BallCollisionType.AVOID, +# auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), +# 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}") +# params = AssignedTacticPlayControlParams() +# params.assigned_tactics[id].move.CopyFrom(tactic) +# +# field_test_runner.set_tactics(params, True) +# field_test_runner.run_test( +# always_validation_sequence_set=[[]], +# eventually_validation_sequence_set=[[]], +# test_timeout_s=4, +# ) +# +# # Send a halt tactic after the test finishes +# halt_tactic = HaltTactic() +# params = AssignedTacticPlayControlParams() +# params.assigned_tactics[id].stop.CopyFrom(halt_tactic) +# if __name__ == "__main__": pytest_main(__file__) From eb1cdeb7e432b25ebcb4ecc955506d61a21e10ff Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 22 Nov 2025 00:19:18 -0800 Subject: [PATCH 33/71] Wait on data ready signal --- .../motor_controller/motor_controller.h | 2 + .../stspin_motor_controller.cpp | 44 +++++++++------ .../stspin_motor_controller.h | 3 +- .../embedded/services/robot_auto_test.cpp | 53 ++----------------- 4 files changed, 37 insertions(+), 65 deletions(-) diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index 70e2dc9921..617760a3a6 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -8,6 +8,8 @@ class MotorController { public: + virtual ~MotorController() = default; + virtual MotorControllerStatus earlyPoll() = 0; virtual void setup() = 0; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 1876fcd3e9..6106a4b9ab 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -1,6 +1,8 @@ #include "software/embedded/motor_controller/stspin_motor_controller.h" #include +#include +#include #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" @@ -17,7 +19,9 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController() : reset_gpio_( - setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)) + setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), + data_ready_gpio_( + setupGpio(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::INPUT, GpioState::LOW)) { for (const MotorIndex& motor : reflective_enum::values()) { @@ -36,6 +40,8 @@ MotorControllerStatus StSpinMotorController::earlyPoll() void StSpinMotorController::setup() { + reset(); + for (const MotorIndex& motor : reflective_enum::values()) { if (ENABLED_MOTORS.at(motor)) @@ -57,11 +63,9 @@ void StSpinMotorController::setup() void StSpinMotorController::reset() { - // reset_gpio_->setValue(GpioState::LOW); - // usleep(MICROSECONDS_PER_MILLISECOND * 100); - // - // reset_gpio_->setValue(GpioState::HIGH); - // usleep(MICROSECONDS_PER_MILLISECOND * 100); + reset_gpio_->setValue(GpioState::LOW); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + reset_gpio_->setValue(GpioState::HIGH); } MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) @@ -235,8 +239,13 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, // The byte order of DATA is big endian; therefore the MSB is transmitted first. // DATA may be ignored if the operation has no data to transmit/receive. - uint8_t tx[FRAME_LEN] = {0}; - uint8_t rx[FRAME_LEN] = {0}; + while (data_ready_gpio_->getValue() != GpioState::HIGH) + { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + + uint8_t tx[FRAME_LEN] = {}; + uint8_t rx[FRAME_LEN] = {}; tx[0] = FRAME_SOF; tx[1] = static_cast(opcode); @@ -244,18 +253,21 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, tx[3] = static_cast(0xFF & data); tx[5] = FRAME_EOF; - const uint8_t message[] = {tx[1], tx[2], tx[3]}; - tx[4] = Crc8Autosar::calc(message, sizeof(message)); + const uint8_t tx_msg[] = {tx[1], tx[2], tx[3]}; + tx[4] = Crc8Autosar::calc(tx_msg, sizeof(tx_msg)); spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_LEN, SPI_SPEED_HZ); - LOG(INFO) << "TX " << static_cast(tx[0]) << " " << static_cast(tx[1]) << " " - << static_cast(tx[2]) << " " << static_cast(tx[3]) << " " - << static_cast(tx[4]) << " " << static_cast(tx[5]); - LOG(INFO) << "RX " << static_cast(rx[0]) << " " << static_cast(rx[1]) << " " - << static_cast(rx[2]) << " " << static_cast(rx[3]) << " " - << static_cast(rx[4]) << " " << static_cast(rx[5]); + // Frame integrity check + const uint8_t rx_msg[] = {rx[1], rx[2], rx[3]}; + const uint8_t rx_crc = Crc8Autosar::calc(rx_msg, sizeof(rx_msg)); + if (rx[0] != FRAME_SOF || rx[5] != FRAME_EOF || rx[4] != rx_crc) + { + LOG(WARNING) << "Received frame that failed integrity check. Expected CRC " + << static_cast(rx_crc) << " got " << static_cast(rx[4]); + } + // Return the DATA field of the received frame return static_cast((static_cast(rx[2]) << 8) | rx[3]); } diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index beb50c23e2..80b6118e2f 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -88,7 +88,7 @@ class StSpinMotorController : public MotorController }; // SPI Configs - static constexpr uint32_t SPI_SPEED_HZ = 250000; // 250 KHz + static constexpr uint32_t SPI_SPEED_HZ = 2000000; // 2 MHz static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; // 250 MHz static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0; @@ -97,4 +97,5 @@ class StSpinMotorController : public MotorController std::array()> file_descriptors_; std::unique_ptr reset_gpio_; + std::unique_ptr data_ready_gpio_; }; diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index 01f4412622..01b4f46055 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -36,56 +36,13 @@ int main(int argc, char **argv) LOG(INFO) << "Motor controller setup complete"; - LOG(INFO) << "Waiting..."; - std::this_thread::sleep_for(std::chrono::seconds(1)); - - motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 1500); - LOG(INFO) << "Waiting..."; - std::this_thread::sleep_for(std::chrono::seconds(2)); - motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 0); - - - - /*if (motor_controller_->earlyPoll() == MotorControllerStatus::OK) + for (int i = 0; i < 100000; ++i) { - for (int i = 0; i <= 100; ++i) - { - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 1500); - LOG(INFO) << "Waiting..."; - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 0); - }*/ - - - /* - for (int i = 0; i < 1000; ++i) - { - motor_controller_->sendAndReceiveFrame(MotorIndex::BACK_RIGHT, - StSpinOpcode::SPI_NOOP); - } - */ - - // motor_controller_->reset(); - - /* - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, - StSpinOpcode::MOV_AX, static_cast( - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch() - ).count() % 5000 - )); - */ + motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 300); + motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 0); + LOG(INFO) << i; + } - /* - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, - StSpinOpcode::MOV_BX, 1000); - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, - StSpinOpcode::SET_SPEEDRAMP); - motor_controller_->sendAndReceiveFrame(MotorIndex::FRONT_LEFT, - StSpinOpcode::START_MOTOR); - */ LOG(INFO) << "Robot Auto Test Complete"; return 0; From 5e56f18c642f112c922a1a71ea8b915229793941 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 22 Nov 2025 00:45:59 -0800 Subject: [PATCH 34/71] Nits --- .../stspin_motor_controller.cpp | 1 + .../stspin_motor_controller.h | 7 ++- .../motor_controller/tmc_motor_controller.cpp | 12 ++-- src/software/embedded/services/motor.cpp | 57 +++++++++---------- .../embedded/services/robot_auto_test.cpp | 9 +-- 5 files changed, 38 insertions(+), 48 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 6106a4b9ab..9b0dd02130 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -239,6 +239,7 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, // The byte order of DATA is big endian; therefore the MSB is transmitted first. // DATA may be ignored if the operation has no data to transmit/receive. + // Busy wait for slave data ready assertion while (data_ready_gpio_->getValue() != GpioState::HIGH) { std::this_thread::sleep_for(std::chrono::microseconds(100)); diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 80b6118e2f..520fe79394 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -25,6 +25,7 @@ class StSpinMotorController : public MotorController void immediatelyDisable() override; + private: /** * Opens a SPI file descriptor for the given motor * @@ -63,10 +64,10 @@ class StSpinMotorController : public MotorController static constexpr uint8_t DRIBBLER_MOTOR_CHIP_SELECT = 4; static const inline std::unordered_map ENABLED_MOTORS = { - {MotorIndex::FRONT_LEFT, true}, - {MotorIndex::BACK_LEFT, true}, + {MotorIndex::FRONT_LEFT, false}, + {MotorIndex::BACK_LEFT, false}, {MotorIndex::BACK_RIGHT, true}, - {MotorIndex::FRONT_RIGHT, true}, + {MotorIndex::FRONT_RIGHT, false}, {MotorIndex::DRIBBLER, false}, }; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index ecb3a06b12..69198d8441 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -82,12 +82,12 @@ double TmcMotorController::readThenWriteVelocity(const MotorIndex& motor, void TmcMotorController::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value) { - int num_retires_left = NUM_RETRIES_SPI; + 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_retires_left > 0) + while (num_retries_left > 0) { tmc6100_writeInt(motor, address, value); read_value = tmc6100_readInt(motor, address); @@ -96,7 +96,7 @@ void TmcMotorController::writeToDriverOrDieTrying(uint8_t motor, uint8_t address return; } LOG(DEBUG) << "SPI Transfer to Driver Failed, retrying..."; - num_retires_left--; + num_retries_left--; } // If we get here, we have failed to write to the driver. We reset @@ -112,12 +112,12 @@ void TmcMotorController::writeToDriverOrDieTrying(uint8_t motor, uint8_t address void TmcMotorController::writeToControllerOrDieTrying(const MotorIndex& motor, uint8_t address, int32_t value) { - int num_retires_left = NUM_RETRIES_SPI; + 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_retires_left > 0) + while (num_retries_left > 0) { tmc4671_writeInt(CHIP_SELECTS.at(motor), address, value); read_value = tmc4671_readInt(CHIP_SELECTS.at(motor), address); @@ -126,7 +126,7 @@ void TmcMotorController::writeToControllerOrDieTrying(const MotorIndex& motor, return; } LOG(DEBUG) << "SPI Transfer to Controller Failed, retrying..."; - num_retires_left--; + num_retries_left--; } } diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 67db6b5c5b..c7be725cdc 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -208,38 +208,33 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor WheelSpace_t current_wheel_velocities = {front_right_velocity, front_left_velocity, back_left_velocity, back_right_velocity}; - // Run-away protection - // TODO: fix me - if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) + if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - + prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > + RUNAWAY_PROTECTION_THRESHOLD_MPS) { - if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - 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) - { - 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) - { - 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) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Back right motor runaway"; - } + 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) + { + 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) + { + 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) + { + motor_controller_->immediatelyDisable(); + LOG(FATAL) << "Back right motor runaway"; } // Convert to Euclidean velocity_delta diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index 01b4f46055..e4598d5e9d 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -5,23 +5,16 @@ #include "proto/primitive/primitive_msg_factory.h" #include "shared/2021_robot_constants.h" #include "shared/constants.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/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" -std::unique_ptr motor_controller_; +std::unique_ptr motor_controller_; std::unique_ptr power_service_; -RobotConstants_t robot_constants_; -int read_value; -// SPI Chip Selects -constexpr double THRESHOLD = 0.0001; -constexpr int DELAY_NS = 10000; std::string runtime_dir = "/tmp/tbots/yellow_test"; int main(int argc, char **argv) From 38218584544977c1858e567a7c6f335aebc86330 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 22 Nov 2025 01:00:11 -0800 Subject: [PATCH 35/71] Remove unused robot constant --- src/shared/2021_robot_constants.cpp | 3 +-- src/shared/robot_constants.h | 4 ---- src/software/python_bindings.cpp | 2 -- 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp index f8f0537416..e3fb2307a9 100644 --- a/src/shared/2021_robot_constants.cpp +++ b/src/shared/2021_robot_constants.cpp @@ -30,7 +30,6 @@ RobotConstants_t create2021RobotConstants(void) .robot_max_ang_speed_rad_per_s = 10.0f, .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, - .wheel_radius_meters = 0.03f, - .wheel_rotations_per_motor_rotation = 17.0f / 60.0f}; + .wheel_radius_meters = 0.03f}; return robot_constants; } diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index af9bbee29e..6c3b587e7f 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -94,8 +94,4 @@ typedef struct RobotConstants // The radius of the wheel, in meters float wheel_radius_meters; - // The gear ratio between the motor shaft and wheel shaft - // [# of wheel rotations / 1 motor rotation] - float wheel_rotations_per_motor_rotation; - } RobotConstants_t; diff --git a/src/software/python_bindings.cpp b/src/software/python_bindings.cpp index 64730ddde0..7ba448bf60 100644 --- a/src/software/python_bindings.cpp +++ b/src/software/python_bindings.cpp @@ -305,8 +305,6 @@ PYBIND11_MODULE(python_bindings, m) .def_readwrite("indefinite_dribbler_speed_rpm", &RobotConstants::indefinite_dribbler_speed_rpm) .def_readwrite("wheel_radius_meters", &RobotConstants::wheel_radius_meters) - .def_readwrite("wheel_rotations_per_motor_rotation", - &RobotConstants::wheel_rotations_per_motor_rotation) .def_readwrite("robot_max_speed_m_per_s", &RobotConstants::robot_max_speed_m_per_s) .def_readwrite("robot_max_ang_speed_rad_per_s", From 6156d7fe62743bdf5f418cbbed13d6f167247d73 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 22 Nov 2025 01:05:23 -0800 Subject: [PATCH 36/71] euclidean_to_wheel.cpp changes --- src/software/physics/euclidean_to_wheel.cpp | 6 +++--- src/software/physics/euclidean_to_wheel.h | 7 +------ 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 0b4cd839e7..a6c6d70d12 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -9,7 +9,7 @@ #include "software/geom/vector.h" EuclideanToWheel::EuclideanToWheel(const RobotConstants_t& robot_constants) - : robot_radius_m_(robot_constants.robot_radius_m), robot_constants_(robot_constants) + : robot_constants_(robot_constants) { // Phi, the angle between the hemisphere line of the robot and the front wheel axles // [rads] @@ -61,7 +61,7 @@ WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_veloc // need to multiply the angular velocity by the robot radius to // calculate the wheel velocity (robot tangential velocity) // ref: http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 8 - euclidean_velocity[2] = euclidean_velocity[2] * robot_radius_m_; + euclidean_velocity[2] = euclidean_velocity[2] * robot_constants_.robot_radius_m; return euclidean_to_wheel_velocity_D_ * euclidean_velocity; } @@ -76,7 +76,7 @@ EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity( // velocity. This can be divided by the robot radius to calculate // the angular velocity // ref: http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 8 - euclidean_velocity[2] = euclidean_velocity[2] / robot_radius_m_; + euclidean_velocity[2] = euclidean_velocity[2] / robot_constants_.robot_radius_m; return euclidean_velocity; } diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index 984ea63aae..407d9836de 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -64,22 +64,17 @@ class EuclideanToWheel * Ramp the velocity over the given timestep and set the target velocity on the motor. * * NOTE: This function has no state. - * Also NOTE: This function handles all electrical rpm to meters/second conversion. * * @param current_wheel_velocity The current 4-wheel velocity in m/s * @param target_wheel_velocity The target 4-wheel velocity m/s * @param time_to_ramp The time allocated for acceleration in seconds - * + * @return The ramped 4-wheel velocity in m/s */ WheelSpace_t rampWheelVelocity(const WheelSpace_t& current_wheel_velocity, const WheelSpace_t& target_wheel_velocity, const double& time_to_ramp); private: - /** - * The radius of the robot in meters. - */ - const double robot_radius_m_{}; const RobotConstants_t robot_constants_; /** From 3ca4df138248aa8e328e39533922c779f8c6228f Mon Sep 17 00:00:00 2001 From: williamckha Date: Wed, 26 Nov 2025 12:17:16 -0800 Subject: [PATCH 37/71] Per-motor data ready GPIO --- .../stspin_motor_controller.cpp | 15 +++-- .../stspin_motor_controller.h | 55 +++++++++++-------- .../embedded/services/robot_auto_test.cpp | 21 ++++++- 3 files changed, 61 insertions(+), 30 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 9b0dd02130..3d68bf75d3 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -19,15 +19,17 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController() : reset_gpio_( - setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), - data_ready_gpio_( - setupGpio(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::INPUT, GpioState::LOW)) + setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)) { for (const MotorIndex& motor : reflective_enum::values()) { if (ENABLED_MOTORS.at(motor)) { openSpiFileDescriptor(motor); + + data_ready_gpio_[motor] = + setupGpio(DATA_READY_GPIO_PINS.at(motor), GpioDirection::INPUT, + GpioState::LOW); } } } @@ -191,7 +193,7 @@ double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, // SET_SPEEDRAMP expects the ramp time in millis to be in register bx. // We do speed ramping ourselves in MotorService, so we just want to // set the target speed without ramping (hence we set reg bx to 0). - sendAndReceiveFrame(motor, StSpinOpcode::MOV_BX, 0); + sendAndReceiveFrame(motor, StSpinOpcode::MOV_BX, 300); sendAndReceiveFrame(motor, StSpinOpcode::SET_SPEEDRAMP); @@ -240,7 +242,7 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, // DATA may be ignored if the operation has no data to transmit/receive. // Busy wait for slave data ready assertion - while (data_ready_gpio_->getValue() != GpioState::HIGH) + while (data_ready_gpio_.at(motor)->getValue() != GpioState::HIGH) { std::this_thread::sleep_for(std::chrono::microseconds(100)); } @@ -266,7 +268,8 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, if (rx[0] != FRAME_SOF || rx[5] != FRAME_EOF || rx[4] != rx_crc) { LOG(WARNING) << "Received frame that failed integrity check. Expected CRC " - << static_cast(rx_crc) << " got " << static_cast(rx[4]); + << static_cast(rx_crc) << " but got " << static_cast(rx[4]) + << " for motor " << motor; } // Return the DATA field of the received frame diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 520fe79394..4de96a0f9f 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -56,40 +56,49 @@ class StSpinMotorController : public MotorController // Length of frame (in number of bytes) static constexpr unsigned int FRAME_LEN = 6; - // 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; - static const inline std::unordered_map ENABLED_MOTORS = { - {MotorIndex::FRONT_LEFT, false}, - {MotorIndex::BACK_LEFT, false}, - {MotorIndex::BACK_RIGHT, true}, - {MotorIndex::FRONT_RIGHT, false}, - {MotorIndex::DRIBBLER, false}, + {MotorIndex::FRONT_LEFT, false}, + {MotorIndex::BACK_LEFT, false}, + {MotorIndex::BACK_RIGHT, false}, + {MotorIndex::FRONT_RIGHT, true}, + {MotorIndex::DRIBBLER, false}, }; + // SPI Chip Selects + // clang-format off static const inline std::unordered_map CHIP_SELECTS = { - {MotorIndex::FRONT_LEFT, FRONT_LEFT_MOTOR_CHIP_SELECT}, - {MotorIndex::BACK_LEFT, BACK_LEFT_MOTOR_CHIP_SELECT}, - {MotorIndex::BACK_RIGHT, BACK_RIGHT_MOTOR_CHIP_SELECT}, - {MotorIndex::FRONT_RIGHT, FRONT_RIGHT_MOTOR_CHIP_SELECT}, - {MotorIndex::DRIBBLER, DRIBBLER_MOTOR_CHIP_SELECT}, + {MotorIndex::FRONT_LEFT, 0}, + {MotorIndex::BACK_LEFT, 1}, + {MotorIndex::BACK_RIGHT, 2}, + {MotorIndex::FRONT_RIGHT, 3}, + {MotorIndex::DRIBBLER, 4}, }; + // clang-format on // SPI Motor Driver Paths + // clang-format off static const inline std::unordered_map SPI_PATHS = { - {MotorIndex::FRONT_LEFT, "/dev/spidev0.2"}, + {MotorIndex::FRONT_LEFT, "/dev/spidev0.2"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.3"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.0"}, {MotorIndex::FRONT_RIGHT, "/dev/spidev0.4"}, - {MotorIndex::BACK_LEFT, "/dev/spidev0.3"}, - {MotorIndex::BACK_RIGHT, "/dev/spidev0.0"}, - {MotorIndex::DRIBBLER, "/dev/spidev0.1"}, + {MotorIndex::DRIBBLER, "/dev/spidev0.1"}, + }; + // clang-format on + + // Data Ready GPIO Pins + // clang-format off + static const inline std::unordered_map DATA_READY_GPIO_PINS = { + {MotorIndex::FRONT_LEFT, 24}, + {MotorIndex::BACK_LEFT, 16}, + {MotorIndex::BACK_RIGHT, 26}, + {MotorIndex::FRONT_RIGHT, 23}, + {MotorIndex::DRIBBLER, 0}, }; + // clang-format on // SPI Configs - static constexpr uint32_t SPI_SPEED_HZ = 2000000; // 2 MHz + static constexpr uint32_t SPI_SPEED_HZ = 1000000; // 2 MHz static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; // 250 MHz static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0; @@ -98,5 +107,5 @@ class StSpinMotorController : public MotorController std::array()> file_descriptors_; std::unique_ptr reset_gpio_; - std::unique_ptr data_ready_gpio_; + std::unordered_map> data_ready_gpio_; }; diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index e4598d5e9d..b4a27f686a 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -31,9 +31,28 @@ int main(int argc, char **argv) for (int i = 0; i < 100000; ++i) { + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 500); + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_LEFT, 300); + motor_controller_->readThenWriteVelocity(MotorIndex::BACK_LEFT, 300); motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 300); + + LOG(INFO) << "Spinning all motors"; + + std::this_thread::sleep_for(std::chrono::seconds(3)); + + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 0); + motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_LEFT, 0); + motor_controller_->readThenWriteVelocity(MotorIndex::BACK_LEFT, 0); motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 0); - LOG(INFO) << i; + + motor_controller_->checkDriverFault(MotorIndex::FRONT_RIGHT); + motor_controller_->checkDriverFault(MotorIndex::FRONT_LEFT); + motor_controller_->checkDriverFault(MotorIndex::BACK_LEFT); + motor_controller_->checkDriverFault(MotorIndex::BACK_RIGHT); + + LOG(INFO) << "Completed iteration " << i; + + std::this_thread::sleep_for(std::chrono::seconds(3)); } LOG(INFO) << "Robot Auto Test Complete"; From 3a10ac7079173e9d426674374aeade1f28583ff3 Mon Sep 17 00:00:00 2001 From: williamckha Date: Thu, 18 Dec 2025 12:40:55 -0800 Subject: [PATCH 38/71] Working motor service --- .../stspin_motor_controller.cpp | 32 ++++++----- .../stspin_motor_controller.h | 2 +- src/software/embedded/services/motor.cpp | 57 ++++++++++--------- src/software/embedded/services/motor.h | 6 +- src/software/embedded/thunderloop.cpp | 11 ++-- 5 files changed, 57 insertions(+), 51 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 3d68bf75d3..f04f406e01 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -49,13 +49,6 @@ void StSpinMotorController::setup() if (ENABLED_MOTORS.at(motor)) { sendAndReceiveFrame(motor, StSpinOpcode::ACK_FAULTS); - } - } - - for (const MotorIndex& motor : reflective_enum::values()) - { - if (ENABLED_MOTORS.at(motor)) - { checkDriverFault(motor); readThenWriteVelocity(motor, 0); sendAndReceiveFrame(motor, StSpinOpcode::START_MOTOR); @@ -68,6 +61,7 @@ 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)); } MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) @@ -102,21 +96,21 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo if (faults & static_cast(StSpinFaultCode::OVER_VOLT)) { - LOG(WARNING) << "OVER_VOLT: Software overvoltage"; + LOG(WARNING) << "OVER_VOLT: Over voltage"; motor_faults.insert(TbotsProto::MotorFault::OVER_VOLT); drive_enabled = false; } if (faults & static_cast(StSpinFaultCode::UNDER_VOLT)) { - LOG(WARNING) << "UNDER_VOLT: Software undervoltage"; + LOG(WARNING) << "UNDER_VOLT: Under voltage"; motor_faults.insert(TbotsProto::MotorFault::UNDER_VOLT); drive_enabled = false; } if (faults & static_cast(StSpinFaultCode::OVER_TEMP)) { - LOG(WARNING) << "OVER_TEMP: Software over temperature"; + LOG(WARNING) << "OVER_TEMP: Over temperature"; motor_faults.insert(TbotsProto::MotorFault::OVER_TEMP); drive_enabled = false; } @@ -137,7 +131,7 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo if (faults & static_cast(StSpinFaultCode::OVER_CURR)) { - LOG(WARNING) << "OVER_CURR: Software overcurrent"; + LOG(WARNING) << "OVER_CURR: Over current"; motor_faults.insert(TbotsProto::MotorFault::OVER_CURR); drive_enabled = false; } @@ -157,7 +151,7 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo if (faults & static_cast(StSpinFaultCode::OVERCURR_SW)) { - LOG(INFO) << "OVERCURR_SW: Software overcurrent"; + LOG(INFO) << "OVERCURR_SW: Software over current"; motor_faults.insert(TbotsProto::MotorFault::OVERCURR_SW); drive_enabled = false; } @@ -193,7 +187,7 @@ double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, // SET_SPEEDRAMP expects the ramp time in millis to be in register bx. // We do speed ramping ourselves in MotorService, so we just want to // set the target speed without ramping (hence we set reg bx to 0). - sendAndReceiveFrame(motor, StSpinOpcode::MOV_BX, 300); + sendAndReceiveFrame(motor, StSpinOpcode::MOV_BX, 0); sendAndReceiveFrame(motor, StSpinOpcode::SET_SPEEDRAMP); @@ -270,6 +264,18 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, LOG(WARNING) << "Received frame that failed integrity check. Expected CRC " << static_cast(rx_crc) << " but got " << static_cast(rx[4]) << " for motor " << motor; + + LOG(WARNING) << "RX motor " << motor << " " + << static_cast(rx[0]) << " " + << static_cast(rx[1]) << " " + << static_cast(rx[2]) << " " + << static_cast(rx[3]) << " " + << static_cast(rx[4]) << " " + << static_cast(rx[5]); + } + else if (static_cast(rx[1]) == StSpinOpcode::NACK) + { + LOG(WARNING) << "Frame not acknowledged by motor " << motor; } // Return the DATA field of the received frame diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 4de96a0f9f..15d46d743b 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -98,7 +98,7 @@ class StSpinMotorController : public MotorController // clang-format on // SPI Configs - static constexpr uint32_t SPI_SPEED_HZ = 1000000; // 2 MHz + static constexpr uint32_t SPI_SPEED_HZ = 100000; // 100 KHz static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; // 250 MHz static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index c7be725cdc..0a2d01f17d 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -208,34 +208,35 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor WheelSpace_t current_wheel_velocities = {front_right_velocity, front_left_velocity, back_left_velocity, back_right_velocity}; - if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - 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) - { - 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) - { - 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) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Back right motor runaway"; - } + // if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - + // prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > + // RUNAWAY_PROTECTION_THRESHOLD_MPS) + // { + // 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) + // { + // 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) + // { + // 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) + // { + // motor_controller_->immediatelyDisable(); + // LOG(FATAL) << "Back right motor runaway"; + // } + (void)RUNAWAY_PROTECTION_THRESHOLD_MPS; // Convert to Euclidean velocity_delta EuclideanSpace_t current_euclidean_velocity = diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index c02ebf6581..becefaece7 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -108,9 +108,9 @@ class MotorService static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; #elif STSPIN_MOTOR_BOARD - static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = 60 / (3.1415926 * 0.06); - static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = - 1 / ELECTRICAL_RPM_PER_MECHANICAL_MPS; + static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 2 * M_PI * 0.03 / 60; + static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = + 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; #endif // Controller for communicating with the motor board diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 1bf9a7b62c..3a01c3b4e6 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -124,9 +124,9 @@ Thunderloop::Thunderloop(const RobotConstants_t& robot_constants, bool enable_lo LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; - // motor_service_ = std::make_unique(robot_constants, loop_hz); - // g_motor_service = motor_service_.get(); - // motor_service_->setup(); + motor_service_ = std::make_unique(robot_constants, loop_hz); + g_motor_service = motor_service_.get(); + motor_service_->setup(); LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_ @@ -328,9 +328,8 @@ void Thunderloop::runLoop() } // Motor Service: execute the motor control command - // motor_status_ = pollMotorService(poll_time, - // direct_control_.motor_control(), - // time_since_prev_iter); + motor_status_ = pollMotorService(poll_time, direct_control_.motor_control(), + time_since_prev_iter); thunderloop_status_.set_motor_service_poll_time_ms( getMilliseconds(poll_time)); From d58d9e4ad62c1c7f4840ee5acd5ee2809e1ba360 Mon Sep 17 00:00:00 2001 From: williamckha Date: Thu, 8 Jan 2026 20:31:59 -0800 Subject: [PATCH 39/71] Move busy wait code to GPIO class --- src/software/embedded/gpio/BUILD | 1 + src/software/embedded/gpio/gpio.cpp | 21 +++++++++++++++++++ src/software/embedded/gpio/gpio.h | 13 ++++++++++++ .../stspin_motor_controller.cpp | 13 +++++++----- .../stspin_motor_controller.h | 9 +++++--- 5 files changed, 49 insertions(+), 8 deletions(-) create mode 100644 src/software/embedded/gpio/gpio.cpp diff --git a/src/software/embedded/gpio/BUILD b/src/software/embedded/gpio/BUILD index 22727825a9..460b1ceb59 100644 --- a/src/software/embedded/gpio/BUILD +++ b/src/software/embedded/gpio/BUILD @@ -3,6 +3,7 @@ package(default_visibility = ["//visibility:public"]) cc_library( name = "gpio", srcs = [ + "gpio.cpp", "gpio_char_dev.cpp", "gpio_sysfs.cpp", ], diff --git a/src/software/embedded/gpio/gpio.cpp b/src/software/embedded/gpio/gpio.cpp new file mode 100644 index 0000000000..c224eeea00 --- /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; +} \ No newline at end of file diff --git a/src/software/embedded/gpio/gpio.h b/src/software/embedded/gpio/gpio.h index c07e15c239..26fc958d67 100644 --- a/src/software/embedded/gpio/gpio.h +++ b/src/software/embedded/gpio/gpio.h @@ -2,6 +2,8 @@ #include "software/util/make_enum/make_enum.hpp" +#include + MAKE_ENUM(GpioState, LOW, HIGH); MAKE_ENUM(GpioDirection, INPUT, OUTPUT); @@ -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/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index f04f406e01..4625c7b8e7 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -235,10 +235,10 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, // The byte order of DATA is big endian; therefore the MSB is transmitted first. // DATA may be ignored if the operation has no data to transmit/receive. - // Busy wait for slave data ready assertion - while (data_ready_gpio_.at(motor)->getValue() != GpioState::HIGH) + // Wait for slave data ready assertion + while (!data_ready_gpio_.at(motor)->pollValue(GpioState::HIGH, DATA_READY_TIMEOUT_MS)) { - std::this_thread::sleep_for(std::chrono::microseconds(100)); + LOG(WARNING) << "Timed out waiting for " << motor << " to signal data ready"; } uint8_t tx[FRAME_LEN] = {}; @@ -272,10 +272,13 @@ int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, << static_cast(rx[3]) << " " << static_cast(rx[4]) << " " << static_cast(rx[5]); + + return 0; } - else if (static_cast(rx[1]) == StSpinOpcode::NACK) + + if (static_cast(rx[1]) == StSpinOpcode::NACK) { - LOG(WARNING) << "Frame not acknowledged by motor " << motor; + LOG(WARNING) << "Last frame sent was not acknowledged by motor " << motor; } // Return the DATA field of the received frame diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 15d46d743b..91df579003 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -57,9 +57,9 @@ class StSpinMotorController : public MotorController static constexpr unsigned int FRAME_LEN = 6; static const inline std::unordered_map ENABLED_MOTORS = { - {MotorIndex::FRONT_LEFT, false}, - {MotorIndex::BACK_LEFT, false}, - {MotorIndex::BACK_RIGHT, false}, + {MotorIndex::FRONT_LEFT, true}, + {MotorIndex::BACK_LEFT, true}, + {MotorIndex::BACK_RIGHT, true}, {MotorIndex::FRONT_RIGHT, true}, {MotorIndex::DRIBBLER, false}, }; @@ -103,6 +103,9 @@ class StSpinMotorController : public MotorController static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0; + static constexpr std::chrono::milliseconds DATA_READY_TIMEOUT_MS = + std::chrono::milliseconds(200); + // SPI File Descriptors mapping from Chip Select -> File Descriptor std::array()> file_descriptors_; From 6fd9133803444e169c1750425af828bd484f3ae1 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 18 Jan 2026 18:54:01 -0800 Subject: [PATCH 40/71] Update data ready GPIO pins --- .../embedded/motor_controller/stspin_motor_controller.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 91df579003..ed80a0f1a1 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -90,8 +90,8 @@ class StSpinMotorController : public MotorController // clang-format off static const inline std::unordered_map DATA_READY_GPIO_PINS = { {MotorIndex::FRONT_LEFT, 24}, - {MotorIndex::BACK_LEFT, 16}, - {MotorIndex::BACK_RIGHT, 26}, + {MotorIndex::BACK_LEFT, 21}, + {MotorIndex::BACK_RIGHT, 16}, {MotorIndex::FRONT_RIGHT, 23}, {MotorIndex::DRIBBLER, 0}, }; From 8f1d01751d8f6ce1e3288b5975b8cfde825dba9a Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 19 Jan 2026 17:06:05 -0800 Subject: [PATCH 41/71] Updates for circular DMA based SPI with new protocol --- src/shared/2021_robot_constants.cpp | 8 +- src/software/embedded/gpio/gpio.cpp | 2 +- src/software/embedded/gpio/gpio.h | 4 +- .../motor_controller/motor_controller.h | 4 +- .../motor_controller/stspin_constants.h | 22 --- .../stspin_motor_controller.cpp | 142 +++++++++--------- .../stspin_motor_controller.h | 73 +++++---- .../motor_controller/tmc_motor_controller.cpp | 37 ++--- .../motor_controller/tmc_motor_controller.h | 8 +- .../embedded/services/robot_auto_test.cpp | 2 +- src/software/embedded/thunderloop.cpp | 2 +- 11 files changed, 137 insertions(+), 167 deletions(-) diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp index e3fb2307a9..79d1a206cd 100644 --- a/src/shared/2021_robot_constants.cpp +++ b/src/shared/2021_robot_constants.cpp @@ -5,9 +5,9 @@ RobotConstants_t create2021RobotConstants(void) { RobotConstants_t robot_constants = { - .mass_kg = 2.5f, // determined experimentally - .inertial_factor = 0.37f, // determined experimentally - .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), + .mass_kg = 2.5f, // determined experimentally + .inertial_factor = 0.37f, // determined experimentally + .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), .jerk_limit_kg_m_per_s_3 = 40.0f, .front_wheel_angle_deg = 25.0f, .back_wheel_angle_deg = 40.0f, @@ -30,6 +30,6 @@ RobotConstants_t create2021RobotConstants(void) .robot_max_ang_speed_rad_per_s = 10.0f, .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, - .wheel_radius_meters = 0.03f}; + .wheel_radius_meters = 0.03f}; return robot_constants; } diff --git a/src/software/embedded/gpio/gpio.cpp b/src/software/embedded/gpio/gpio.cpp index c224eeea00..3fd57d4179 100644 --- a/src/software/embedded/gpio/gpio.cpp +++ b/src/software/embedded/gpio/gpio.cpp @@ -18,4 +18,4 @@ bool Gpio::pollValue(GpioState state, std::chrono::milliseconds timeout_ms) } } return true; -} \ No newline at end of file +} diff --git a/src/software/embedded/gpio/gpio.h b/src/software/embedded/gpio/gpio.h index 26fc958d67..da60b19c50 100644 --- a/src/software/embedded/gpio/gpio.h +++ b/src/software/embedded/gpio/gpio.h @@ -1,9 +1,9 @@ #pragma once -#include "software/util/make_enum/make_enum.hpp" - #include +#include "software/util/make_enum/make_enum.hpp" + MAKE_ENUM(GpioState, LOW, HIGH); MAKE_ENUM(GpioDirection, INPUT, OUTPUT); diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index 617760a3a6..d4cf23b255 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -26,8 +26,8 @@ class MotorController */ virtual MotorFaultIndicator checkDriverFault(const MotorIndex& motor) = 0; - virtual double readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) = 0; + virtual int readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) = 0; virtual void immediatelyDisable() = 0; }; diff --git a/src/software/embedded/motor_controller/stspin_constants.h b/src/software/embedded/motor_controller/stspin_constants.h index a7cc4a41b6..7b00c6a521 100644 --- a/src/software/embedded/motor_controller/stspin_constants.h +++ b/src/software/embedded/motor_controller/stspin_constants.h @@ -1,27 +1,5 @@ #pragma once -enum class StSpinOpcode -{ - SPI_NOOP = 0b00000000, - MOV_AX = 0b10000010, - GET_AX = 0b10000011, - MOV_BX = 0b10000100, - GET_BX = 0b10000101, - SET_SPEEDRAMP = 0b00000010, - GET_SPEED = 0b00000011, - SET_ENCODER = 0b00000100, - GET_ENCODER = 0b00000101, - START_MOTOR = 0b00001000, - STOP_MOTOR = 0b11111111, - ACK_FAULTS = 0b00010000, - GET_FAULT = 0b00010001, - SET_CURRENT = 0b00100000, - GET_CURRENT = 0b00100001, - ACK = 0b11000000, - NACK = 0b11000001, - SPI_ERROR = 0b11100000 -}; - enum class StSpinFaultCode { NO_FAULT = 0x0000, diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 4625c7b8e7..debe5c88c4 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -1,6 +1,7 @@ #include "software/embedded/motor_controller/stspin_motor_controller.h" #include + #include #include @@ -10,6 +11,7 @@ #pragma GCC diagnostic pop #include "shared/constants.h" +#include "software/embedded/motor_controller/stspin_constants.h" #include "software/embedded/spi_utils.h" #include "software/logger/logger.h" @@ -26,10 +28,6 @@ StSpinMotorController::StSpinMotorController() if (ENABLED_MOTORS.at(motor)) { openSpiFileDescriptor(motor); - - data_ready_gpio_[motor] = - setupGpio(DATA_READY_GPIO_PINS.at(motor), GpioDirection::INPUT, - GpioState::LOW); } } } @@ -48,10 +46,9 @@ void StSpinMotorController::setup() { if (ENABLED_MOTORS.at(motor)) { - sendAndReceiveFrame(motor, StSpinOpcode::ACK_FAULTS); - checkDriverFault(motor); - readThenWriteVelocity(motor, 0); - sendAndReceiveFrame(motor, StSpinOpcode::START_MOTOR); + motor_enabled_[motor] = true; + motor_measured_speed_rpm_[motor] = 0; + motor_faults_[motor] = 0; } } } @@ -76,11 +73,7 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo return MotorFaultIndicator(drive_enabled, motor_faults); } - sendAndReceiveFrame(motor, StSpinOpcode::GET_FAULT); - - // We must send a SPI_NOOP in order to clock out the response - // to the GET_FAULT request. - const uint16_t faults = sendAndReceiveFrame(motor, StSpinOpcode::SPI_NOOP); + const uint16_t faults = motor_faults_.at(motor); if (faults != 0) { @@ -166,8 +159,8 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo return MotorFaultIndicator(drive_enabled, motor_faults); } -double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) +int StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) { if (!ENABLED_MOTORS.at(motor)) { @@ -176,25 +169,32 @@ double StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, return 0; } - sendAndReceiveFrame(motor, StSpinOpcode::GET_SPEED); - - // SET_SPEEDRAMP expects the target motor speed to be in register ax. - // Also, the frame we receive here contains the response to the GET_SPEED - // request made in the previous frame. - const int16_t current_velocity = sendAndReceiveFrame( - motor, StSpinOpcode::MOV_AX, static_cast(target_velocity)); + const auto outgoingFrame = OutgoingFrame{ + .motor_enabled = motor_enabled_.at(motor), + .motor_target_speed_rpm = static_cast(target_velocity), + }; - // SET_SPEEDRAMP expects the ramp time in millis to be in register bx. - // We do speed ramping ourselves in MotorService, so we just want to - // set the target speed without ramping (hence we set reg bx to 0). - sendAndReceiveFrame(motor, StSpinOpcode::MOV_BX, 0); + const auto incomingFrame = sendAndReceiveFrame(motor, outgoingFrame); - sendAndReceiveFrame(motor, StSpinOpcode::SET_SPEEDRAMP); + if (incomingFrame.has_value()) + { + motor_measured_speed_rpm_[motor] = incomingFrame->motor_measured_speed_rpm; + motor_faults_[motor] = incomingFrame->motor_faults; + } - return current_velocity; + return motor_measured_speed_rpm_.at(motor); } -void StSpinMotorController::immediatelyDisable() {} +void StSpinMotorController::immediatelyDisable() +{ + for (const MotorIndex& motor : reflective_enum::values()) + { + if (ENABLED_MOTORS.at(motor)) + { + motor_enabled_[motor] = false; + } + } +} void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) { @@ -218,69 +218,63 @@ void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) << "error: " << strerror(errno); } -int16_t StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, - const StSpinOpcode opcode, - const int16_t data) +std::optional +StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, + const OutgoingFrame outgoing_frame) const { - // A frame is 6 bytes long and has the following format: + uint8_t tx[FRAME_LEN] = {}; + uint8_t rx[FRAME_LEN] = {}; + + // Outgoing frames have the following format: // // +-------+--------+---------------+-------+-------+ - // | SOF | OPCODE | DATA | CRC | EOF | + // | DELIM | ENABLE | SPEED_RPM | | CRC | // +-------+--------+---------------+-------+-------+ // 0 1 2 3 4 5 // - // For slave frames, OPCODE will be either an ACK or NACK acknowledging - // receival of the last master frame. - // - // The byte order of DATA is big endian; therefore the MSB is transmitted first. - // DATA may be ignored if the operation has no data to transmit/receive. - - // Wait for slave data ready assertion - while (!data_ready_gpio_.at(motor)->pollValue(GpioState::HIGH, DATA_READY_TIMEOUT_MS)) - { - LOG(WARNING) << "Timed out waiting for " << motor << " to signal data ready"; - } - - uint8_t tx[FRAME_LEN] = {}; - uint8_t rx[FRAME_LEN] = {}; + // The byte order of SPEED_RPM is big endian; i.e. the MSB is + // transmitted first. Byte 4 is currently unused. - tx[0] = FRAME_SOF; - tx[1] = static_cast(opcode); - tx[2] = static_cast(0xFF & (data >> 8)); - tx[3] = static_cast(0xFF & data); - tx[5] = FRAME_EOF; - - const uint8_t tx_msg[] = {tx[1], tx[2], tx[3]}; - tx[4] = Crc8Autosar::calc(tx_msg, sizeof(tx_msg)); + tx[0] = FRAME_DELIMITER; + tx[1] = static_cast(outgoing_frame.motor_enabled); + tx[2] = static_cast(0xFF & (outgoing_frame.motor_target_speed_rpm >> 8)); + tx[3] = static_cast(0xFF & outgoing_frame.motor_target_speed_rpm); + tx[4] = 0; + tx[5] = Crc8Autosar::calc(tx, FRAME_LEN - 1); spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_LEN, SPI_SPEED_HZ); + // Incoming frames have the following format: + // + // +-------+---------------+----------------+-------+ + // | DELIM | SPEED_RPM | FAULTS | CRC | + // +-------+---------------+----------------+-------+ + // 0 1 2 3 4 5 + // + // The byte order of SPEED_RPM and FAULTS is big endian; + // i.e. the MSB is transmitted first. + // Frame integrity check - const uint8_t rx_msg[] = {rx[1], rx[2], rx[3]}; - const uint8_t rx_crc = Crc8Autosar::calc(rx_msg, sizeof(rx_msg)); - if (rx[0] != FRAME_SOF || rx[5] != FRAME_EOF || rx[4] != rx_crc) + const uint8_t rx_crc = Crc8Autosar::calc(rx, FRAME_LEN - 1); + if (rx[0] != FRAME_DELIMITER || rx[5] != rx_crc) { LOG(WARNING) << "Received frame that failed integrity check. Expected CRC " - << static_cast(rx_crc) << " but got " << static_cast(rx[4]) + << static_cast(rx_crc) << " but got " << static_cast(rx[5]) << " for motor " << motor; - LOG(WARNING) << "RX motor " << motor << " " - << static_cast(rx[0]) << " " - << static_cast(rx[1]) << " " - << static_cast(rx[2]) << " " - << static_cast(rx[3]) << " " - << static_cast(rx[4]) << " " + LOG(WARNING) << "RX motor " << motor << " " << static_cast(rx[0]) << " " + << static_cast(rx[1]) << " " << static_cast(rx[2]) << " " + << static_cast(rx[3]) << " " << static_cast(rx[4]) << " " << static_cast(rx[5]); - return 0; - } - - if (static_cast(rx[1]) == StSpinOpcode::NACK) - { - LOG(WARNING) << "Last frame sent was not acknowledged by motor " << motor; + return std::nullopt; } - // Return the DATA field of the received frame - return static_cast((static_cast(rx[2]) << 8) | rx[3]); + return IncomingFrame{ + .motor_measured_speed_rpm = + static_cast((static_cast(rx[1]) << 8) | rx[2]), + .motor_faults = + static_cast((static_cast(rx[3]) << 8) | rx[4]), + }; } diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index ed80a0f1a1..e8b280efcb 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -5,7 +5,6 @@ #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_constants.h" class StSpinMotorController : public MotorController { @@ -20,8 +19,8 @@ class StSpinMotorController : public MotorController MotorFaultIndicator checkDriverFault(const MotorIndex& motor) override; - double readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) override; + int readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) override; void immediatelyDisable() override; @@ -33,36 +32,45 @@ class StSpinMotorController : public MotorController */ void openSpiFileDescriptor(const MotorIndex& motor); + struct IncomingFrame + { + int16_t motor_measured_speed_rpm; + uint16_t motor_faults; + }; + + struct OutgoingFrame + { + bool motor_enabled; + int16_t motor_target_speed_rpm; + }; + /** * Transmits a frame to the given motor and receives a frame back over SPI. * - * Note: to receive data for GET operations, it is expected that we send a - * second frame (e.g. a NOOP) after the initial frame requesting the GET. - * The data in the second frame received will contain the response to the - * GET request. - * * @param motor the motor to send the frame to - * @param opcode the opcode to include in the transmitted frame - * @param data the data to include in the transmitted frame - * @return the data in the frame received from the motor + * @param outgoing_frame the outgoing frame to send to the motor + * @return the incoming frame received from the motor */ - int16_t sendAndReceiveFrame(const MotorIndex& motor, const StSpinOpcode opcode, - const int16_t data = 0); - - // Start-of-frame and end-of-frame markers - static constexpr uint8_t FRAME_SOF = 0x73; - static constexpr uint8_t FRAME_EOF = 0x45; + std::optional sendAndReceiveFrame(const MotorIndex& motor, + OutgoingFrame outgoing_frame) const; // Length of frame (in number of bytes) static constexpr unsigned int FRAME_LEN = 6; + // Byte marking the start of a new frame. + // We don't _need_ frame delimiters, but they're useful as part of a + // quick frame integrity check. + static constexpr uint8_t FRAME_DELIMITER = 0x67; + + // clang-format off static const inline std::unordered_map ENABLED_MOTORS = { - {MotorIndex::FRONT_LEFT, true}, - {MotorIndex::BACK_LEFT, true}, - {MotorIndex::BACK_RIGHT, true}, + {MotorIndex::FRONT_LEFT, true}, + {MotorIndex::BACK_LEFT, true}, + {MotorIndex::BACK_RIGHT, true}, {MotorIndex::FRONT_RIGHT, true}, - {MotorIndex::DRIBBLER, false}, + {MotorIndex::DRIBBLER, false}, }; + // clang-format on // SPI Chip Selects // clang-format off @@ -86,29 +94,18 @@ class StSpinMotorController : public MotorController }; // clang-format on - // Data Ready GPIO Pins - // clang-format off - static const inline std::unordered_map DATA_READY_GPIO_PINS = { - {MotorIndex::FRONT_LEFT, 24}, - {MotorIndex::BACK_LEFT, 21}, - {MotorIndex::BACK_RIGHT, 16}, - {MotorIndex::FRONT_RIGHT, 23}, - {MotorIndex::DRIBBLER, 0}, - }; - // clang-format on - // SPI Configs - static constexpr uint32_t SPI_SPEED_HZ = 100000; // 100 KHz - static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; // 250 MHz + static constexpr uint32_t SPI_SPEED_HZ = 100000; // 100 KHz + static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; // 250 MHz static constexpr uint8_t SPI_BITS = 8; static constexpr uint32_t SPI_MODE = 0; - static constexpr std::chrono::milliseconds DATA_READY_TIMEOUT_MS = - std::chrono::milliseconds(200); - // SPI File Descriptors mapping from Chip Select -> File Descriptor std::array()> file_descriptors_; std::unique_ptr reset_gpio_; - std::unordered_map> data_ready_gpio_; + + std::unordered_map motor_enabled_; + std::unordered_map motor_measured_speed_rpm_; + std::unordered_map motor_faults_; }; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index 69198d8441..fd0bf29876 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -59,10 +59,11 @@ TmcMotorController::TmcMotorController() MotorControllerStatus TmcMotorController::earlyPoll() { - auto motors = driveMotors(); - bool encoders_calibrated = std::accumulate( - motors.begin(), motors.end(), false, [&](const bool& acc, const MotorIndex& motor) - { return acc || encoder_calibrated_[motor]; }); + auto motors = driveMotors(); + bool encoders_calibrated = + std::accumulate(motors.begin(), motors.end(), false, + [&](const bool& acc, const MotorIndex& motor) + { return acc || encoder_calibrated_[motor]; }); if (!encoders_calibrated) { @@ -72,8 +73,8 @@ MotorControllerStatus TmcMotorController::earlyPoll() return MotorControllerStatus::OK; } -double TmcMotorController::readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) +int TmcMotorController::readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) { return readThenWriteValue(motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, target_velocity); @@ -172,10 +173,11 @@ void TmcMotorController::setup() 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]; }); + 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"; } @@ -390,10 +392,10 @@ MotorFaultIndicator TmcMotorController::checkDriverFault(const MotorIndex& motor return MotorFaultIndicator(drive_enabled, motor_faults); } -double TmcMotorController::readThenWriteValue(const MotorIndex& motor, - const uint8_t& read_addr, - const uint8_t& write_addr, - const int& write_data) +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); @@ -627,10 +629,9 @@ void TmcMotorController::checkEncoderConnections() 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; }); + 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()) diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index 3d4a2835d1..48b49102cf 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -21,8 +21,8 @@ class TmcMotorController : public MotorController void immediatelyDisable() override; - double readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) override; + int readThenWriteVelocity(const MotorIndex& motor, + const int& target_velocity) override; /** * Trinamic API binding, sets spi_demux_select_0|1 pins appropriately and @@ -90,8 +90,8 @@ class TmcMotorController : public MotorController * @param write_data the data to write * @return the value read from the trinamic controller */ - double readThenWriteValue(const MotorIndex& motor, const uint8_t& read_addr, - const uint8_t& write_addr, const int& write_data); + int readThenWriteValue(const MotorIndex& motor, const uint8_t& read_addr, + const uint8_t& write_addr, const int& write_data); /** * A lot of initialization parameters are necessary to function. Even if diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index b4a27f686a..dfa610b6d8 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -15,7 +15,7 @@ std::unique_ptr motor_controller_; std::unique_ptr power_service_; -std::string runtime_dir = "/tmp/tbots/yellow_test"; +std::string runtime_dir = "/tmp/tbots/yellow_test"; int main(int argc, char **argv) { diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 3a01c3b4e6..e280a15671 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -329,7 +329,7 @@ void Thunderloop::runLoop() // Motor Service: execute the motor control command motor_status_ = pollMotorService(poll_time, direct_control_.motor_control(), - time_since_prev_iter); + time_since_prev_iter); thunderloop_status_.set_motor_service_poll_time_ms( getMilliseconds(poll_time)); From aae2f547fabe864ab3910a8d7c98f587ca5519d9 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 24 Jan 2026 12:27:04 -0800 Subject: [PATCH 42/71] Change MPS_PER_RPM constant --- .../stspin_motor_controller.cpp | 7 +- .../motor_controller/tmc_motor_controller.cpp | 17 +++-- .../motor_controller/tmc_motor_controller.h | 13 ++++ src/software/embedded/services/motor.cpp | 66 +++++++++---------- src/software/embedded/services/motor.h | 52 +++++---------- src/software/embedded/thunderloop.cpp | 3 +- 6 files changed, 79 insertions(+), 79 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index debe5c88c4..2fa9301ffa 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -75,11 +75,14 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo const uint16_t faults = motor_faults_.at(motor); - if (faults != 0) + if (faults == 0) { - LOG(WARNING) << "======= Faults For Motor " << motor << "======="; + // No faults; early return + return MotorFaultIndicator(drive_enabled, motor_faults); } + LOG(WARNING) << "======= Faults For Motor " << motor << "======="; + if (faults & static_cast(StSpinFaultCode::DURATION)) { LOG(WARNING) << "DURATION: FOC rate too high"; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index fd0bf29876..496eaca0ea 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -76,8 +76,15 @@ MotorControllerStatus TmcMotorController::earlyPoll() int TmcMotorController::readThenWriteVelocity(const MotorIndex& motor, const int& target_velocity) { - return readThenWriteValue(motor, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, target_velocity); + const int velocity_erpm = readThenWriteValue( + motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, target_velocity); + + if (motor == MotorIndex::DRIBBLER) + { + return velocity_erpm * DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + } + + return velocity_erpm * DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; } void TmcMotorController::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, @@ -504,7 +511,8 @@ void TmcMotorController::startController(MotorIndex motor, bool dribbler) if (dribbler) { // Configure to brushless DC motor with 1 pole pair - writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, 0x00030001); + writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, + 0x00030000 + DRIBBLER_MOTOR_NUM_POLE_PAIRS); configureHall(motor); configureDribblerPI(motor); @@ -512,7 +520,8 @@ void TmcMotorController::startController(MotorIndex motor, bool dribbler) else { // Configure to brushless DC motor with 8 pole pairs - writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, 0x00030008); + writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, + 0x00030000 + DRIVE_MOTOR_NUM_POLE_PAIRS); configureEncoder(motor); } } diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index 48b49102cf..f251736509 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -256,4 +256,17 @@ class TmcMotorController : public MotorController // 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; + + // 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 / DRIVE_MOTOR_NUM_POLE_PAIRS; + static constexpr double DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM = + 1 / DRIBBLER_MOTOR_NUM_POLE_PAIRS; }; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 0a2d01f17d..4c7a4648b6 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -6,24 +6,18 @@ #include "software/embedded/motor_controller/tmc_motor_controller.h" #include "software/logger/logger.h" -static const uint32_t NUM_RETRIES_SPI = 3; - static double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; static int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; - -MotorService::MotorService(const RobotConstants_t& robot_constants, - int control_loop_frequency_hz) +MotorService::MotorService(const RobotConstants_t& robot_constants) : motor_controller_(setupMotorController()), robot_constants_(robot_constants), euclidean_to_four_wheel_(robot_constants), - dribbler_ramp_rpm_(0), + drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60), tracked_motor_fault_start_time_(std::nullopt) { } -MotorService::~MotorService() {} - void MotorService::setup() { for (const MotorIndex& motor : reflective_enum::values()) @@ -48,7 +42,7 @@ void MotorService::setup() } else { - tracked_motor_fault_start_time_ = std::make_optional(now); + tracked_motor_fault_start_time_ = now; num_tracked_motor_resets_ = 1; } @@ -61,6 +55,11 @@ void MotorService::setup() } prev_wheel_velocities_ = {0.0, 0.0, 0.0, 0.0}; + front_left_target_rpm = 0; + front_right_target_rpm = 0; + back_left_target_rpm = 0; + back_right_target_rpm = 0; + dribbler_target_rpm_ = 0; motor_controller_->setup(); @@ -180,31 +179,27 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor setup(); } - // 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 = motor_controller_->readThenWriteVelocity( MotorIndex::FRONT_RIGHT, front_right_target_rpm) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; + drive_motor_mps_per_rpm_; double front_left_velocity = motor_controller_->readThenWriteVelocity( MotorIndex::FRONT_LEFT, front_left_target_rpm) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; + drive_motor_mps_per_rpm_; double back_right_velocity = motor_controller_->readThenWriteVelocity( MotorIndex::BACK_RIGHT, back_right_target_rpm) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; + drive_motor_mps_per_rpm_; double back_left_velocity = motor_controller_->readThenWriteVelocity( MotorIndex::BACK_LEFT, back_left_target_rpm) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; + drive_motor_mps_per_rpm_; double dribbler_rpm = motor_controller_->readThenWriteVelocity(MotorIndex::DRIBBLER, - dribbler_ramp_rpm_); + dribbler_target_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); - // 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 + // This order needs to match euclidean_to_four_wheel converters order. + // We also want to work in the meters per second space rather than RPMs. WheelSpace_t current_wheel_velocities = {front_right_velocity, front_left_velocity, back_left_velocity, back_right_velocity}; @@ -276,7 +271,7 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor 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); @@ -285,17 +280,17 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor // Calculate speeds accounting for acceleration front_right_target_rpm = - static_cast(target_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); + static_cast(target_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] / + drive_motor_mps_per_rpm_); front_left_target_rpm = - static_cast(target_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); + static_cast(target_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] / + drive_motor_mps_per_rpm_); back_left_target_rpm = - static_cast(target_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); + static_cast(target_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] / + drive_motor_mps_per_rpm_); back_right_target_rpm = - static_cast(target_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); + static_cast(target_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] / + drive_motor_mps_per_rpm_); // Get target dribbler rpm from the primitive int target_dribbler_rpm; @@ -313,17 +308,16 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor // 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(dribbler_target_rpm_); return motor_status; } diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index becefaece7..2fa0d45544 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -32,21 +32,20 @@ class MotorService * Service that interacts with the motors. * Opens all the required ports and maintains them until destroyed. * - * @param RobotConstants_t The robot constants - * @param control_loop_frequency_hz The frequency the main loop will call poll at + * @param robot_constants The robot constants */ - MotorService(const RobotConstants_t& robot_constants, int control_loop_frequency_hz); + MotorService(const RobotConstants_t& robot_constants); - virtual ~MotorService(); + virtual ~MotorService() = default; /** * When the motor service is polled with a DirectControlPrimitive msg, * call the appropriate trinamic api function to spin the appropriate motor. * - * @param motor The motor msg to unpack and execute on the motors + * @param motor_control 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 + * @return The status of all the drive units */ TbotsProto::MotorStatus poll(const TbotsProto::MotorControl& motor_control, double time_elapsed_since_last_poll_s); @@ -69,15 +68,11 @@ class MotorService * 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 front_left_velocity_mps the front left motor's velocity in m/s + * @param front_right_velocity_mps the front right motor's velocity in m/s + * @param back_left_velocity_mps the back left motor's velocity in m/s + * @param back_right_velocity_mps the back right motor's velocity 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) @@ -98,22 +93,7 @@ class MotorService */ bool requiresMotorReinit(const MotorIndex& 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) -#ifdef TRINAMIC_MOTOR_BOARD - static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 0.000111; - static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = - 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; -#elif STSPIN_MOTOR_BOARD - static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 2 * M_PI * 0.03 / 60; - static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = - 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; -#endif - - // Controller for communicating with the motor board + // Controller for communicating with the motor driver board(s) std::unique_ptr motor_controller_; // Flag indicating whether the motors have been calibrated @@ -122,7 +102,6 @@ class MotorService RobotConstants_t robot_constants_; EuclideanToWheel euclidean_to_four_wheel_; - std::unordered_map cached_motor_faults_; WheelSpace_t prev_wheel_velocities_; @@ -130,18 +109,21 @@ class MotorService int front_right_target_rpm = 0; int back_left_target_rpm = 0; int back_right_target_rpm = 0; + int dribbler_target_rpm_ = 0; + + double drive_motor_mps_per_rpm_; // The current motor to check for motor faults; this is updated every poll // and will cycle through all the MotorIndex values (only one motor is checked // for faults each poll to reduce number of SPI transactions) MotorIndex motor_fault_detector_; - int dribbler_ramp_rpm_; + std::unordered_map cached_motor_faults_; std::optional> tracked_motor_fault_start_time_; int num_tracked_motor_resets_; - static const int MOTOR_FAULT_TIME_THRESHOLD_S = 60; - static const int MOTOR_FAULT_THRESHOLD_COUNT = 3; + static constexpr int MOTOR_FAULT_TIME_THRESHOLD_S = 60; + static constexpr int MOTOR_FAULT_THRESHOLD_COUNT = 3; }; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index e280a15671..953cde77d7 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -70,7 +70,6 @@ extern "C" Thunderloop::Thunderloop(const RobotConstants_t& robot_constants, bool enable_log_merging, const int loop_hz) - // TODO (#2495): Set the friendly team colour : redis_client_( std::make_unique(REDIS_DEFAULT_HOST, REDIS_DEFAULT_PORT)), motor_status_(std::nullopt), @@ -124,7 +123,7 @@ Thunderloop::Thunderloop(const RobotConstants_t& robot_constants, bool enable_lo 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!"; From 77b89d152717cbb2f53590ecc63d243d644b4b97 Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 6 Feb 2026 13:39:56 -0800 Subject: [PATCH 43/71] Implement command based SPI protocol --- .../motor_controller/stspin_constants.h | 54 ++++++++ .../stspin_motor_controller.cpp | 128 +++++++++++------- .../stspin_motor_controller.h | 28 ++-- src/software/embedded/services/BUILD | 1 + src/software/embedded/services/motor.cpp | 15 +- .../embedded/services/robot_auto_test.cpp | 96 +++++++++---- 6 files changed, 218 insertions(+), 104 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_constants.h b/src/software/embedded/motor_controller/stspin_constants.h index 7b00c6a521..d5feac56ee 100644 --- a/src/software/embedded/motor_controller/stspin_constants.h +++ b/src/software/embedded/motor_controller/stspin_constants.h @@ -1,5 +1,24 @@ #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, +}; + +enum class StSpinResponseType +{ + SPEED_AND_FAULTS = 0x01, + IQ_AND_ID = 0x02, +}; + enum class StSpinFaultCode { NO_FAULT = 0x0000, @@ -15,3 +34,38 @@ enum class StSpinFaultCode OVERCURR_SW = 0x0200, DP_FAULT = 0x0400 }; + +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; +}; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 2fa9301ffa..635156d23c 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -49,6 +49,8 @@ void StSpinMotorController::setup() motor_enabled_[motor] = true; motor_measured_speed_rpm_[motor] = 0; motor_faults_[motor] = 0; + motor_iq_[motor] = 0; + motor_id_[motor] = 0; } } } @@ -172,18 +174,12 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, return 0; } - const auto outgoingFrame = OutgoingFrame{ + const auto outgoing_frame = SetTargetSpeedFrame{ .motor_enabled = motor_enabled_.at(motor), .motor_target_speed_rpm = static_cast(target_velocity), }; - const auto incomingFrame = sendAndReceiveFrame(motor, outgoingFrame); - - if (incomingFrame.has_value()) - { - motor_measured_speed_rpm_[motor] = incomingFrame->motor_measured_speed_rpm; - motor_faults_[motor] = incomingFrame->motor_faults; - } + sendAndReceiveFrame(motor, outgoing_frame); return motor_measured_speed_rpm_.at(motor); } @@ -195,6 +191,7 @@ void StSpinMotorController::immediatelyDisable() if (ENABLED_MOTORS.at(motor)) { motor_enabled_[motor] = false; + readThenWriteVelocity(motor, 0); } } } @@ -221,63 +218,98 @@ void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) << "error: " << strerror(errno); } -std::optional -StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, - const OutgoingFrame outgoing_frame) const +void StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, + const OutgoingFrame outgoing_frame) { uint8_t tx[FRAME_LEN] = {}; uint8_t rx[FRAME_LEN] = {}; - // Outgoing frames have the following format: - // - // +-------+--------+---------------+-------+-------+ - // | DELIM | ENABLE | SPEED_RPM | | CRC | - // +-------+--------+---------------+-------+-------+ - // 0 1 2 3 4 5 - // - // The byte order of SPEED_RPM is big endian; i.e. the MSB is - // transmitted first. Byte 4 is currently unused. - - tx[0] = FRAME_DELIMITER; - tx[1] = static_cast(outgoing_frame.motor_enabled); - tx[2] = static_cast(0xFF & (outgoing_frame.motor_target_speed_rpm >> 8)); - tx[3] = static_cast(0xFF & outgoing_frame.motor_target_speed_rpm); - tx[4] = 0; + std::visit( + [&](TFrame&& frame) + { + using T = std::decay_t; + 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); + } + }, + outgoing_frame); + tx[5] = Crc8Autosar::calc(tx, FRAME_LEN - 1); spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_LEN, SPI_SPEED_HZ); - // Incoming frames have the following format: - // - // +-------+---------------+----------------+-------+ - // | DELIM | SPEED_RPM | FAULTS | CRC | - // +-------+---------------+----------------+-------+ - // 0 1 2 3 4 5 - // - // The byte order of SPEED_RPM and FAULTS is big endian; - // i.e. the MSB is transmitted first. - // Frame integrity check const uint8_t rx_crc = Crc8Autosar::calc(rx, FRAME_LEN - 1); - if (rx[0] != FRAME_DELIMITER || rx[5] != rx_crc) + if (rx[5] != rx_crc) { LOG(WARNING) << "Received frame that failed integrity check. Expected CRC " << static_cast(rx_crc) << " but got " << static_cast(rx[5]) - << " for motor " << motor; - - LOG(WARNING) << "RX motor " << motor << " " << static_cast(rx[0]) << " " + << " for motor " << motor << "\n" + << "RX: " << static_cast(rx[0]) << " " << static_cast(rx[1]) << " " << static_cast(rx[2]) << " " << static_cast(rx[3]) << " " << static_cast(rx[4]) << " " << static_cast(rx[5]); - - return std::nullopt; + return; } - return IncomingFrame{ - .motor_measured_speed_rpm = - static_cast((static_cast(rx[1]) << 8) | rx[2]), - .motor_faults = - static_cast((static_cast(rx[3]) << 8) | rx[4]), - }; + switch (static_cast(rx[0])) + { + case StSpinResponseType::SPEED_AND_FAULTS: + { + motor_measured_speed_rpm_[motor] = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_faults_[motor] = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::IQ_AND_ID: + { + motor_iq_[motor] = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_id_[motor] = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + } } diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index e8b280efcb..fb0551defa 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -5,6 +5,7 @@ #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_constants.h" class StSpinMotorController : public MotorController { @@ -32,36 +33,21 @@ class StSpinMotorController : public MotorController */ void openSpiFileDescriptor(const MotorIndex& motor); - struct IncomingFrame - { - int16_t motor_measured_speed_rpm; - uint16_t motor_faults; - }; - - struct OutgoingFrame - { - bool motor_enabled; - int16_t motor_target_speed_rpm; - }; + using OutgoingFrame = + std::variant; /** * 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 - * @return the incoming frame received from the motor */ - std::optional sendAndReceiveFrame(const MotorIndex& motor, - OutgoingFrame outgoing_frame) const; + void sendAndReceiveFrame(const MotorIndex& motor, OutgoingFrame outgoing_frame); // Length of frame (in number of bytes) static constexpr unsigned int FRAME_LEN = 6; - // Byte marking the start of a new frame. - // We don't _need_ frame delimiters, but they're useful as part of a - // quick frame integrity check. - static constexpr uint8_t FRAME_DELIMITER = 0x67; - // clang-format off static const inline std::unordered_map ENABLED_MOTORS = { {MotorIndex::FRONT_LEFT, true}, @@ -108,4 +94,8 @@ class StSpinMotorController : public MotorController std::unordered_map motor_enabled_; std::unordered_map motor_measured_speed_rpm_; std::unordered_map motor_faults_; + std::unordered_map motor_iq_; + std::unordered_map motor_id_; + + friend void runRobotAutoTest(); }; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index e2b59842e4..bec8413a01 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -54,6 +54,7 @@ cc_binary( ":motor", ":power", "//proto/message_translation:tbots_geometry", + "//proto/message_translation:tbots_protobuf", "//proto/primitive:primitive_msg_factory", "//shared:robot_constants", "//software/embedded:primitive_executor", diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 4c7a4648b6..fe6d92d2e4 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -282,15 +282,12 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor front_right_target_rpm = static_cast(target_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] / drive_motor_mps_per_rpm_); - front_left_target_rpm = - static_cast(target_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] / - drive_motor_mps_per_rpm_); - back_left_target_rpm = - static_cast(target_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] / - drive_motor_mps_per_rpm_); - back_right_target_rpm = - static_cast(target_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] / - drive_motor_mps_per_rpm_); + front_left_target_rpm = static_cast( + target_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] / drive_motor_mps_per_rpm_); + back_left_target_rpm = static_cast( + target_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] / drive_motor_mps_per_rpm_); + back_right_target_rpm = static_cast( + target_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] / drive_motor_mps_per_rpm_); // Get target dribbler rpm from the primitive int target_dribbler_rpm; diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index dfa610b6d8..87993deff3 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -1,61 +1,101 @@ #include #include -#include "proto/message_translation/tbots_geometry.h" +#include "proto/message_translation/tbots_protobuf.h" #include "proto/primitive/primitive_msg_factory.h" -#include "shared/2021_robot_constants.h" -#include "shared/constants.h" #include "software/embedded/motor_controller/stspin_motor_controller.h" -#include "software/embedded/motor_controller/tmc_motor_controller.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" -std::unique_ptr motor_controller_; +std::unique_ptr motor_controller_; std::unique_ptr power_service_; std::string runtime_dir = "/tmp/tbots/yellow_test"; -int main(int argc, char **argv) +void runRobotAutoTest() { - LoggerSingleton::initializeLogger(runtime_dir, nullptr, false); - LOG(INFO) << "Running Robot Auto Test"; motor_controller_ = std::make_unique(); - motor_controller_->setup(); - LOG(INFO) << "Motor controller setup complete"; - - for (int i = 0; i < 100000; ++i) + for (auto motor : driveMotors()) { - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 500); - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_LEFT, 300); - motor_controller_->readThenWriteVelocity(MotorIndex::BACK_LEFT, 300); - motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 300); + motor_controller_->sendAndReceiveFrame( + motor, SetPidTorqueKpKiFrame{.kp = 1250, .ki = 2000}); + motor_controller_->sendAndReceiveFrame( + motor, SetPidFluxKpKiFrame{.kp = 1250, .ki = 2000}); + motor_controller_->sendAndReceiveFrame( + motor, SetPidSpeedKpKiFrame{.kp = 128, .ki = 100}); + } - LOG(INFO) << "Spinning all motors"; + LOG(INFO) << "Motor controller setup complete"; - std::this_thread::sleep_for(std::chrono::seconds(3)); + LOG(INFO) << "Running torque step test"; - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_RIGHT, 0); - motor_controller_->readThenWriteVelocity(MotorIndex::FRONT_LEFT, 0); - motor_controller_->readThenWriteVelocity(MotorIndex::BACK_LEFT, 0); - motor_controller_->readThenWriteVelocity(MotorIndex::BACK_RIGHT, 0); + for (auto motor : driveMotors()) + { + motor_controller_->sendAndReceiveFrame( + motor, SetResponseTypeFrame{StSpinResponseType::IQ_AND_ID}); + } - motor_controller_->checkDriverFault(MotorIndex::FRONT_RIGHT); - motor_controller_->checkDriverFault(MotorIndex::FRONT_LEFT); - motor_controller_->checkDriverFault(MotorIndex::BACK_LEFT); - motor_controller_->checkDriverFault(MotorIndex::BACK_RIGHT); + for (const int16_t torque : {0, 250, 500, 250, 0}) + { + for (int i = 0; i < 1000; ++i) + { + for (auto motor : driveMotors()) + { + motor_controller_->sendAndReceiveFrame(motor, + SetTargetTorqueFrame{ + .motor_enabled = true, + .motor_target_torque = torque, + }); + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"Iq", motor_controller_->motor_iq_.at(motor)}, + {"Id", motor_controller_->motor_id_.at(motor)}, + }); + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } - LOG(INFO) << "Completed iteration " << i; + LOG(INFO) << "Running speed step test"; - std::this_thread::sleep_for(std::chrono::seconds(3)); + for (auto motor : driveMotors()) + { + motor_controller_->sendAndReceiveFrame( + motor, SetResponseTypeFrame{StSpinResponseType::SPEED_AND_FAULTS}); + } + + for (const int16_t speed : {0, 250, 500, 250, 0}) + { + for (int i = 0; i < 1000; ++i) + { + for (auto motor : driveMotors()) + { + motor_controller_->readThenWriteVelocity(motor, speed); + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"target_speed_rpm", speed}, + {"measured_speed_rpm", + motor_controller_->motor_measured_speed_rpm_.at(motor)}, + }); + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } } + motor_controller_->immediatelyDisable(); + LOG(INFO) << "Robot Auto Test Complete"; +} +int main(int argc, char **argv) +{ + LoggerSingleton::initializeLogger(runtime_dir, nullptr, false); + runRobotAutoTest(); return 0; } From 311f839d25a30b4fa609bd86e485bacb02786236 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 15 Feb 2026 12:12:21 -0800 Subject: [PATCH 44/71] Add more SPI commands --- src/software/embedded/motor_controller/BUILD | 7 ++- .../stspin_motor_controller.cpp | 60 +++++++++++++------ .../stspin_motor_controller.h | 26 +++++--- .../{stspin_constants.h => stspin_types.h} | 12 +++- src/software/embedded/services/BUILD | 2 +- src/software/logger/network_logger.cpp | 5 +- src/software/thunderscope/constants.py | 1 + 7 files changed, 79 insertions(+), 34 deletions(-) rename src/software/embedded/motor_controller/{stspin_constants.h => stspin_types.h} (83%) diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index df459a13a0..3b9f5295ea 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -37,7 +37,8 @@ cc_library( ":motor_controller_status", ":motor_fault_indicator", ":motor_index", - ":stspin_constants", + ":stspin_types", + "//proto/message_translation:tbots_protobuf", "//software/embedded:spi_utils", "//software/embedded/constants", "//software/embedded/gpio", @@ -58,6 +59,6 @@ cc_library( ) cc_library( - name = "stspin_constants", - hdrs = ["stspin_constants.h"], + name = "stspin_types", + hdrs = ["stspin_types.h"], ) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 635156d23c..8c5d69e076 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -10,8 +10,9 @@ #include "cppcrc.h" #pragma GCC diagnostic pop +#include "proto/message_translation/tbots_protobuf.h" #include "shared/constants.h" -#include "software/embedded/motor_controller/stspin_constants.h" +#include "software/embedded/motor_controller/stspin_types.h" #include "software/embedded/spi_utils.h" #include "software/logger/logger.h" @@ -46,11 +47,11 @@ void StSpinMotorController::setup() { if (ENABLED_MOTORS.at(motor)) { - motor_enabled_[motor] = true; - motor_measured_speed_rpm_[motor] = 0; - motor_faults_[motor] = 0; - motor_iq_[motor] = 0; - motor_id_[motor] = 0; + motor_status_[motor].enabled = true; + + sendAndReceiveFrame(motor, SetPidTorqueKpKiFrame{.kp = 13530, .ki = 5772}); + sendAndReceiveFrame(motor, SetPidFluxKpKiFrame{.kp = 13530, .ki = 5772}); + sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 40, .ki = 30}); } } } @@ -75,7 +76,7 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo return MotorFaultIndicator(drive_enabled, motor_faults); } - const uint16_t faults = motor_faults_.at(motor); + const uint16_t faults = motor_status_.at(motor).faults; if (faults == 0) { @@ -175,13 +176,13 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, } const auto outgoing_frame = SetTargetSpeedFrame{ - .motor_enabled = motor_enabled_.at(motor), + .motor_enabled = motor_status_.at(motor).enabled, .motor_target_speed_rpm = static_cast(target_velocity), }; sendAndReceiveFrame(motor, outgoing_frame); - return motor_measured_speed_rpm_.at(motor); + return motor_status_.at(motor).measured_speed_rpm; } void StSpinMotorController::immediatelyDisable() @@ -190,7 +191,7 @@ void StSpinMotorController::immediatelyDisable() { if (ENABLED_MOTORS.at(motor)) { - motor_enabled_[motor] = false; + motor_status_[motor].enabled = false; readThenWriteVelocity(motor, 0); } } @@ -271,6 +272,12 @@ void StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, 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_KD); + tx[1] = static_cast(0xFF & (frame.kd >> 8)); + tx[2] = static_cast(0xFF & frame.kd); + } }, outgoing_frame); @@ -285,11 +292,10 @@ void StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, { LOG(WARNING) << "Received frame that failed integrity check. Expected CRC " << static_cast(rx_crc) << " but got " << static_cast(rx[5]) - << " for motor " << motor << "\n" - << "RX: " << static_cast(rx[0]) << " " - << static_cast(rx[1]) << " " << static_cast(rx[2]) << " " - << static_cast(rx[3]) << " " << static_cast(rx[4]) << " " - << static_cast(rx[5]); + << " for motor " << motor << ". RX: " << static_cast(rx[0]) + << " " << static_cast(rx[1]) << " " << static_cast(rx[2]) + << " " << static_cast(rx[3]) << " " << static_cast(rx[4]) + << " " << static_cast(rx[5]); return; } @@ -297,17 +303,33 @@ void StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, { case StSpinResponseType::SPEED_AND_FAULTS: { - motor_measured_speed_rpm_[motor] = + motor_status_[motor].measured_speed_rpm = static_cast((static_cast(rx[1]) << 8) | rx[2]); - motor_faults_[motor] = + motor_status_[motor].faults = static_cast((static_cast(rx[3]) << 8) | rx[4]); break; } case StSpinResponseType::IQ_AND_ID: { - motor_iq_[motor] = + 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_id_[motor] = + motor_status_[motor].phase_voltage = static_cast((static_cast(rx[3]) << 8) | rx[4]); break; } diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index fb0551defa..6a8a7562ae 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -5,7 +5,7 @@ #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_constants.h" +#include "software/embedded/motor_controller/stspin_types.h" class StSpinMotorController : public MotorController { @@ -35,7 +35,8 @@ class StSpinMotorController : public MotorController using OutgoingFrame = std::variant; + SetPidTorqueKpKiFrame, SetPidFluxKpKiFrame, SetPidSpeedKpKiFrame, + SetPidSpeedKdFrame>; /** * Transmits a frame to the given motor and receives a frame back over SPI. @@ -89,13 +90,22 @@ class StSpinMotorController : public MotorController // SPI File Descriptors mapping from Chip Select -> File Descriptor std::array()> file_descriptors_; - std::unique_ptr reset_gpio_; + struct MotorStatus + { + bool enabled; + int16_t measured_speed_rpm; + uint16_t faults; + int16_t iq; + int16_t id; + int16_t vq; + int16_t vd; + int16_t phase_current; + int16_t phase_voltage; + }; - std::unordered_map motor_enabled_; - std::unordered_map motor_measured_speed_rpm_; - std::unordered_map motor_faults_; - std::unordered_map motor_iq_; - std::unordered_map motor_id_; + std::unordered_map motor_status_; + + std::unique_ptr reset_gpio_; friend void runRobotAutoTest(); }; diff --git a/src/software/embedded/motor_controller/stspin_constants.h b/src/software/embedded/motor_controller/stspin_types.h similarity index 83% rename from src/software/embedded/motor_controller/stspin_constants.h rename to src/software/embedded/motor_controller/stspin_types.h index d5feac56ee..f079947ca4 100644 --- a/src/software/embedded/motor_controller/stspin_constants.h +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -11,12 +11,15 @@ enum class StSpinOpcode SET_PID_TORQUE_KP_KI = 0x04, SET_PID_FLUX_KP_KI = 0x05, SET_PID_SPEED_KP_KI = 0x06, + SET_PID_SPEED_KD = 0x07, }; enum class StSpinResponseType { - SPEED_AND_FAULTS = 0x01, - IQ_AND_ID = 0x02, + SPEED_AND_FAULTS = 0x01, + IQ_AND_ID = 0x02, + VQ_AND_VD = 0x03, + PHASE_CURRENT_AND_VOLTAGE = 0x04, }; enum class StSpinFaultCode @@ -69,3 +72,8 @@ struct SetPidSpeedKpKiFrame int16_t kp; int16_t ki; }; + +struct SetPidSpeedKdFrame +{ + int16_t kd; +}; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index bec8413a01..4397ba6481 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -58,7 +58,7 @@ cc_binary( "//proto/primitive:primitive_msg_factory", "//shared:robot_constants", "//software/embedded:primitive_executor", - "//software/logger", + "//software/logger:network_logger", "@trinamic", ], ) diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index 518050305f..5dc03a69c8 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -21,9 +21,12 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); // Sink for PlotJuggler plotting - auto plotjuggler_handle = logWorker->addSink(std::make_unique(), + auto plotjuggler_handle = logWorker->addSink(std::make_unique("end0"), &PlotJugglerSink::sendToPlotJuggler); + g3::only_change_at_initialization::addLogLevel(CSV); + g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); + g3::initializeLogging(logWorker.get()); } diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 542ec4b50d..38b5990ee8 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -333,6 +333,7 @@ class DiagnosticsConstants: "Microsoft X-Box One S pad", "Microsoft X-Box 360 pad", "Microsoft Xbox 360 pad", + "Generic X-Box pad", } BUTTON_PRESSED_THRESHOLD = 0.5 From 44beb7198baba425c8db386c9d0042247e5abd83 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 15 Feb 2026 14:11:22 -0800 Subject: [PATCH 45/71] Add stspin_motor_controller_test --- docs/useful-robot-commands.md | 13 +- src/software/embedded/ansible/BUILD | 2 +- ...> deploy_stspin_motor_controller_test.yml} | 24 +- src/software/embedded/motor_controller/BUILD | 15 ++ .../stspin_motor_controller.h | 2 +- .../stspin_motor_controller_test.cpp | 231 ++++++++++++++++++ src/software/embedded/services/BUILD | 20 -- .../embedded/services/robot_auto_test.cpp | 101 -------- 8 files changed, 259 insertions(+), 149 deletions(-) rename src/software/embedded/ansible/playbooks/{robot_auto_test_playbook.yml => deploy_stspin_motor_controller_test.yml} (57%) create mode 100644 src/software/embedded/motor_controller/stspin_motor_controller_test.cpp delete mode 100644 src/software/embedded/services/robot_auto_test.cpp diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md index 371fcf8ee6..62d5beb255 100644 --- a/docs/useful-robot-commands.md +++ b/docs/useful-robot-commands.md @@ -17,7 +17,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) @@ -184,18 +184,17 @@ 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 --//software/embedded:host_platform= --//software/embedded:motor_board= -- --playbook robot_auto_test_playbook.yml --hosts --ssh_pass +bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:host_platform=PI --//software/embedded:motor_board=STSPIN -- --playbook deploy_stspin_motor_controller_test.yml --hosts --ssh_pass ``` -* replace the \ with the target platform for the robot (either `PI` or `NANO`) -* replace the \ with the actual ip address of the jetson nano for the ssh connection. -* replace the with the actual password for the jetson nano for the ssh connection. +* replace the \ with the actual ip address of the robot for the SSH connection. +* replace the \ with the actual password for the robot nano for the SSH connection. # On Robot Commands diff --git a/src/software/embedded/ansible/BUILD b/src/software/embedded/ansible/BUILD index f20fb92ee5..dd2ea896a2 100644 --- a/src/software/embedded/ansible/BUILD +++ b/src/software/embedded/ansible/BUILD @@ -23,9 +23,9 @@ py_binary( "//software/embedded/linux_configs/jetson_nano:jetson_nano_files", "//software/embedded/linux_configs/pi:pi_files", "//software/embedded/linux_configs/systemd:systemd_files", + "//software/embedded/motor_controller:stspin_motor_controller_test", "//software/embedded/redis", "//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/robot_auto_test_playbook.yml b/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml similarity index 57% rename from src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml rename to src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml index 87bcd49f4d..54676dd0fd 100644 --- a/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml +++ b/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml @@ -1,5 +1,5 @@ --- -- name: Robot Auto Test Playbook +- name: Deploy stspin_motor_controller_test binary to robot hosts: THUNDERBOTS_HOSTS tasks: @@ -8,7 +8,7 @@ msg: "[Robot ID = {{ inventory_hostname }}]" tags: always - - name: Start Robot Auto Test Process + - name: Deploy stspin_motor_controller_test binary block: - name: Stop Services become: true @@ -20,10 +20,10 @@ state: stopped tags: always - - name: Delete robot_auto_test binary under ~/thunderbots_binaries + - name: Delete stspin_motor_controller_test binary under ~/thunderbots_binaries ansible.builtin.file: state: absent - path: ~/thunderbots_binaries/robot_auto_test + path: ~/thunderbots_binaries/stspin_motor_controller_test become_method: ansible.builtin.sudo become: true register: result @@ -35,22 +35,8 @@ - name: Sync Binary ansible.posix.synchronize: - src: ../../services/robot_auto_test + src: ../../motor_controller/stspin_motor_controller_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 diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index 3b9f5295ea..2cc728ab56 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -62,3 +62,18 @@ 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", + "//proto/message_translation:tbots_protobuf", + "//software/logger", + "@boost//:program_options", + ], +) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 6a8a7562ae..e17fcd69e2 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -107,5 +107,5 @@ class StSpinMotorController : public MotorController std::unique_ptr reset_gpio_; - friend void runRobotAutoTest(); + 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..60bb68060f --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -0,0 +1,231 @@ +#include "software/embedded/motor_controller/stspin_motor_controller.h" + +#include +#include +#include + +#include "proto/message_translation/tbots_protobuf.h" +#include "software/logger/logger.h" + +class StSpinMotorControllerTest +{ + public: + struct CommandLineArgs + { + struct PidOption + { + std::optional kp; + std::optional ki; + }; + + bool help = false; + std::string mode = "speed"; + std::vector setpoints; + PidOption torque_pid; + PidOption flux_pid; + PidOption speed_pid; + }; + + explicit StSpinMotorControllerTest(const CommandLineArgs& args) : args_(args) {} + + void runMotorTest() const + { + LOG(INFO) << "Running Motor Test"; + + const auto motor_controller = std::make_unique(); + motor_controller->setup(); + + LOG(INFO) << "Motor controller setup complete"; + + const std::vector motors = {MotorIndex::FRONT_RIGHT}; + + for (auto motor : 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"; + + if (args_.mode == "speed") + { + LOG(INFO) << "Running speed profile"; + + for (auto motor : motors) + { + motor_controller->sendAndReceiveFrame( + motor, SetResponseTypeFrame{StSpinResponseType::SPEED_AND_FAULTS}); + } + + for (const int16_t setpoint : args_.setpoints) + { + for (int i = 0; i < 2000; ++i) + { + for (auto motor : motors) + { + motor_controller->readThenWriteVelocity(motor, setpoint); + motor_controller->checkDriverFault(motor); + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"target_speed_rpm", setpoint}, + {"measured_speed_rpm", + motor_controller->motor_status_.at(motor) + .measured_speed_rpm}, + }); + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } + } + else if (args_.mode == "torque") + { + LOG(INFO) << "Running torque profile"; + + for (auto motor : motors) + { + motor_controller->sendAndReceiveFrame( + motor, SetResponseTypeFrame{StSpinResponseType::IQ_AND_ID}); + } + + for (const int16_t setpoint : args_.setpoints) + { + for (int i = 0; i < 2000; ++i) + { + for (auto motor : motors) + { + motor_controller->sendAndReceiveFrame( + motor, SetTargetTorqueFrame{ + .motor_enabled = true, + .motor_target_torque = setpoint, + }); + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"Iq", motor_controller->motor_status_.at(motor).iq}, + {"Id", motor_controller->motor_status_.at(motor).id}, + }); + } + 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& setpointsString) +{ + std::vector setpoints; + std::stringstream ss(setpointsString); + std::string item; + + while (std::getline(ss, item, ',')) + { + if (!item.empty()) + { + setpoints.push_back(static_cast(std::stoi(item))); + } + } + + return setpoints; +} + +int main(int argc, char** argv) +{ + std::string runtime_dir = "/tmp/tbots/motor_test"; + LoggerSingleton::initializeLogger(runtime_dir, nullptr); + + 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)") + ("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; + } + + 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(); + } + + StSpinMotorControllerTest stspin_motor_controller_test(args); + stspin_motor_controller_test.runMotorTest(); + + return 0; +} diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 4397ba6481..727c938678 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -42,23 +42,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/message_translation:tbots_protobuf", - "//proto/primitive:primitive_msg_factory", - "//shared:robot_constants", - "//software/embedded:primitive_executor", - "//software/logger:network_logger", - "@trinamic", - ], -) 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 87993deff3..0000000000 --- a/src/software/embedded/services/robot_auto_test.cpp +++ /dev/null @@ -1,101 +0,0 @@ -#include -#include - -#include "proto/message_translation/tbots_protobuf.h" -#include "proto/primitive/primitive_msg_factory.h" -#include "software/embedded/motor_controller/stspin_motor_controller.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" - -std::unique_ptr motor_controller_; -std::unique_ptr power_service_; - -std::string runtime_dir = "/tmp/tbots/yellow_test"; - -void runRobotAutoTest() -{ - LOG(INFO) << "Running Robot Auto Test"; - - motor_controller_ = std::make_unique(); - motor_controller_->setup(); - - for (auto motor : driveMotors()) - { - motor_controller_->sendAndReceiveFrame( - motor, SetPidTorqueKpKiFrame{.kp = 1250, .ki = 2000}); - motor_controller_->sendAndReceiveFrame( - motor, SetPidFluxKpKiFrame{.kp = 1250, .ki = 2000}); - motor_controller_->sendAndReceiveFrame( - motor, SetPidSpeedKpKiFrame{.kp = 128, .ki = 100}); - } - - LOG(INFO) << "Motor controller setup complete"; - - LOG(INFO) << "Running torque step test"; - - for (auto motor : driveMotors()) - { - motor_controller_->sendAndReceiveFrame( - motor, SetResponseTypeFrame{StSpinResponseType::IQ_AND_ID}); - } - - for (const int16_t torque : {0, 250, 500, 250, 0}) - { - for (int i = 0; i < 1000; ++i) - { - for (auto motor : driveMotors()) - { - motor_controller_->sendAndReceiveFrame(motor, - SetTargetTorqueFrame{ - .motor_enabled = true, - .motor_target_torque = torque, - }); - - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"Iq", motor_controller_->motor_iq_.at(motor)}, - {"Id", motor_controller_->motor_id_.at(motor)}, - }); - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } - } - - LOG(INFO) << "Running speed step test"; - - for (auto motor : driveMotors()) - { - motor_controller_->sendAndReceiveFrame( - motor, SetResponseTypeFrame{StSpinResponseType::SPEED_AND_FAULTS}); - } - - for (const int16_t speed : {0, 250, 500, 250, 0}) - { - for (int i = 0; i < 1000; ++i) - { - for (auto motor : driveMotors()) - { - motor_controller_->readThenWriteVelocity(motor, speed); - - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"target_speed_rpm", speed}, - {"measured_speed_rpm", - motor_controller_->motor_measured_speed_rpm_.at(motor)}, - }); - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } - } - - motor_controller_->immediatelyDisable(); - - LOG(INFO) << "Robot Auto Test Complete"; -} - -int main(int argc, char **argv) -{ - LoggerSingleton::initializeLogger(runtime_dir, nullptr, false); - runRobotAutoTest(); - return 0; -} From af8a626a85d6465c8a3df685373e4dfba520a8ac Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 21 Feb 2026 11:20:35 -0800 Subject: [PATCH 46/71] Add enabled_motors option to stspin_motor_controller_test --- src/shared/constants.h | 4 +- src/software/embedded/motor_controller/BUILD | 1 + .../stspin_motor_controller_test.cpp | 79 ++++++++++++++++--- src/software/logger/network_logger.cpp | 4 +- 4 files changed, 71 insertions(+), 17 deletions(-) diff --git a/src/shared/constants.h b/src/shared/constants.h index 36b7195213..da0a675053 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -21,9 +21,7 @@ static const std::unordered_map ROBOT_MULTICAST_CHANNELS = { {14, "ff02::c3d0:42d2:bb14"}, {15, "ff02::c3d0:42d2:bb15"}}; // PlotJuggler's default host and port -// Should be updated to your local machine's IP address if -// you want to plot from the robot -static const std::string PLOTJUGGLER_GUI_DEFAULT_HOST = "127.0.0.1"; +static const std::string PLOTJUGGLER_GUI_DEFAULT_HOST = "ff02::c3d0:42d2:aaaa"; static const short unsigned int PLOTJUGGLER_GUI_DEFAULT_PORT = 9870; // ProtoLogger constants for replay files diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index 2cc728ab56..db0063bd9b 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -74,6 +74,7 @@ cc_binary( ":motor_controller", "//proto/message_translation:tbots_protobuf", "//software/logger", + "//software/logger:network_logger", "@boost//:program_options", ], ) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp index 60bb68060f..6b8a2a17b7 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -3,9 +3,11 @@ #include #include #include +#include #include "proto/message_translation/tbots_protobuf.h" #include "software/logger/logger.h" +#include "software/logger/network_logger.h" class StSpinMotorControllerTest { @@ -21,25 +23,31 @@ class StSpinMotorControllerTest bool help = false; std::string mode = "speed"; std::vector setpoints; + std::unordered_set enabled_motors; PidOption torque_pid; PidOption flux_pid; PidOption speed_pid; }; - explicit StSpinMotorControllerTest(const CommandLineArgs& args) : args_(args) {} + 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(); motor_controller->setup(); LOG(INFO) << "Motor controller setup complete"; - const std::vector motors = {MotorIndex::FRONT_RIGHT}; - - for (auto motor : motors) + for (auto motor : args_.enabled_motors) { if (args_.torque_pid.kp && args_.torque_pid.ki) { @@ -69,7 +77,7 @@ class StSpinMotorControllerTest { LOG(INFO) << "Running speed profile"; - for (auto motor : motors) + for (auto motor : args_.enabled_motors) { motor_controller->sendAndReceiveFrame( motor, SetResponseTypeFrame{StSpinResponseType::SPEED_AND_FAULTS}); @@ -79,7 +87,7 @@ class StSpinMotorControllerTest { for (int i = 0; i < 2000; ++i) { - for (auto motor : motors) + for (auto motor : args_.enabled_motors) { motor_controller->readThenWriteVelocity(motor, setpoint); motor_controller->checkDriverFault(motor); @@ -99,7 +107,7 @@ class StSpinMotorControllerTest { LOG(INFO) << "Running torque profile"; - for (auto motor : motors) + for (auto motor : args_.enabled_motors) { motor_controller->sendAndReceiveFrame( motor, SetResponseTypeFrame{StSpinResponseType::IQ_AND_ID}); @@ -109,7 +117,7 @@ class StSpinMotorControllerTest { for (int i = 0; i < 2000; ++i) { - for (auto motor : motors) + for (auto motor : args_.enabled_motors) { motor_controller->sendAndReceiveFrame( motor, SetTargetTorqueFrame{ @@ -136,10 +144,10 @@ class StSpinMotorControllerTest CommandLineArgs args_; }; -static std::vector parseSetpoints(const std::string& setpointsString) +static std::vector parseSetpoints(const std::string& setpoints_str) { std::vector setpoints; - std::stringstream ss(setpointsString); + std::stringstream ss(setpoints_str); std::string item; while (std::getline(ss, item, ',')) @@ -153,10 +161,45 @@ static std::vector parseSetpoints(const std::string& setpointsString) 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) { - std::string runtime_dir = "/tmp/tbots/motor_test"; - LoggerSingleton::initializeLogger(runtime_dir, nullptr); + NetworkLoggerSingleton::initializeLogger(0, true); StSpinMotorControllerTest::CommandLineArgs args; @@ -168,6 +211,8 @@ int main(int argc, char** argv) "Control mode: speed | torque") ("setpoints", boost::program_options::value(), "Comma-separated setpoints (required)") + ("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") @@ -192,6 +237,16 @@ int main(int argc, char** argv) 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")) diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index 5dc03a69c8..54fc9f13a0 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -21,8 +21,8 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); // Sink for PlotJuggler plotting - auto plotjuggler_handle = logWorker->addSink(std::make_unique("end0"), - &PlotJugglerSink::sendToPlotJuggler); + auto plotjuggler_handle = logWorker->addSink( + std::make_unique("end0"), &PlotJugglerSink::sendToPlotJuggler); g3::only_change_at_initialization::addLogLevel(CSV); g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); From bb4c163c8da08e54d6fb5fcdc438ab51d1fb81d2 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 15 Mar 2026 23:06:44 -0700 Subject: [PATCH 47/71] Change function signatures for MotorController --- src/software/embedded/motor_controller/BUILD | 1 - .../motor_controller/motor_controller.h | 5 +- .../stspin_motor_controller.cpp | 125 +++++++++--------- .../stspin_motor_controller.h | 72 ++++------ .../stspin_motor_controller_test.cpp | 28 ++-- .../motor_controller/tmc_motor_controller.cpp | 5 +- .../motor_controller/tmc_motor_controller.h | 5 +- src/software/logger/network_logger.cpp | 4 +- 8 files changed, 115 insertions(+), 130 deletions(-) diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index db0063bd9b..6954be24a2 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -72,7 +72,6 @@ cc_binary( ], deps = [ ":motor_controller", - "//proto/message_translation:tbots_protobuf", "//software/logger", "//software/logger:network_logger", "@boost//:program_options", diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index d4cf23b255..28b183320f 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -24,10 +24,9 @@ class MotorController * @return a struct containing the motor faults and whether the motor was disabled due * to the fault */ - virtual MotorFaultIndicator checkDriverFault(const MotorIndex& motor) = 0; + virtual MotorFaultIndicator checkDriverFault(MotorIndex motor) = 0; - virtual int readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) = 0; + virtual int readThenWriteVelocity(MotorIndex motor, int target_velocity) = 0; virtual void immediatelyDisable() = 0; }; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 8c5d69e076..fee00e01c5 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -11,7 +11,6 @@ #pragma GCC diagnostic pop #include "proto/message_translation/tbots_protobuf.h" -#include "shared/constants.h" #include "software/embedded/motor_controller/stspin_types.h" #include "software/embedded/spi_utils.h" #include "software/logger/logger.h" @@ -24,12 +23,9 @@ StSpinMotorController::StSpinMotorController() : reset_gpio_( setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)) { - for (const MotorIndex& motor : reflective_enum::values()) + for (const MotorIndex& motor : driveMotors()) { - if (ENABLED_MOTORS.at(motor)) - { - openSpiFileDescriptor(motor); - } + openSpiFileDescriptor(motor); } } @@ -43,16 +39,9 @@ void StSpinMotorController::setup() { reset(); - for (const MotorIndex& motor : reflective_enum::values()) + for (const MotorIndex& motor : driveMotors()) { - if (ENABLED_MOTORS.at(motor)) - { - motor_status_[motor].enabled = true; - - sendAndReceiveFrame(motor, SetPidTorqueKpKiFrame{.kp = 13530, .ki = 5772}); - sendAndReceiveFrame(motor, SetPidFluxKpKiFrame{.kp = 13530, .ki = 5772}); - sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 40, .ki = 30}); - } + motor_status_[motor].enabled = true; } } @@ -64,16 +53,14 @@ void StSpinMotorController::reset() std::this_thread::sleep_for(std::chrono::milliseconds(100)); } -MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& motor) +MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex motor) { bool drive_enabled = true; std::unordered_set motor_faults; - if (!ENABLED_MOTORS.at(motor)) + if (motor == MotorIndex::DRIBBLER) { - // Motor is disabled; pretend that the motor is working fine so - // that Thunderloop doesn't attempt to reset it and crash - return MotorFaultIndicator(drive_enabled, motor_faults); + return {drive_enabled, motor_faults}; } const uint16_t faults = motor_status_.at(motor).faults; @@ -81,7 +68,7 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo if (faults == 0) { // No faults; early return - return MotorFaultIndicator(drive_enabled, motor_faults); + return {drive_enabled, motor_faults}; } LOG(WARNING) << "======= Faults For Motor " << motor << "======="; @@ -162,16 +149,14 @@ MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex& mo drive_enabled = false; } - return MotorFaultIndicator(drive_enabled, motor_faults); + return {drive_enabled, motor_faults}; } -int StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) +int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, + const int target_velocity) { - if (!ENABLED_MOTORS.at(motor)) + if (motor == MotorIndex::DRIBBLER) { - // Motor is disabled; pretend that the motor is working fine so - // that Thunderloop doesn't attempt to reset it and crash return 0; } @@ -182,49 +167,75 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex& motor, sendAndReceiveFrame(motor, outgoing_frame); + std::stringstream target_speed_label; + target_speed_label << "target_speed_rpm_" << motor; + std::stringstream measured_speed_label; + measured_speed_label << "measured_speed_rpm_" << motor; + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {target_speed_label.str(), target_velocity}, + {measured_speed_label.str(), motor_status_.at(motor).measured_speed_rpm}, + }); + return motor_status_.at(motor).measured_speed_rpm; } void StSpinMotorController::immediatelyDisable() { - for (const MotorIndex& motor : reflective_enum::values()) + for (const MotorIndex& motor : driveMotors()) { - if (ENABLED_MOTORS.at(motor)) - { - motor_status_[motor].enabled = false; - readThenWriteVelocity(motor, 0); - } + motor_status_[motor].enabled = false; + readThenWriteVelocity(motor, 0); } } -void StSpinMotorController::openSpiFileDescriptor(const MotorIndex& motor) +void StSpinMotorController::openSpiFileDescriptor(const MotorIndex motor) { - file_descriptors_[CHIP_SELECTS.at(motor)] = open(SPI_PATHS.at(motor), O_RDWR); - CHECK(file_descriptors_[CHIP_SELECTS.at(motor)] >= 0) + 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(file_descriptors_[CHIP_SELECTS.at(motor)], SPI_IOC_WR_MODE32, &SPI_MODE); + 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(file_descriptors_[CHIP_SELECTS.at(motor)], SPI_IOC_WR_BITS_PER_WORD, - &SPI_BITS); + 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(file_descriptors_[CHIP_SELECTS.at(motor)], SPI_IOC_WR_MAX_SPEED_HZ, - &MAX_SPI_SPEED_HZ); + 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) +void StSpinMotorController::sendAndReceiveFrame(const MotorIndex motor, + const OutgoingFrame& outgoing_frame) { - uint8_t tx[FRAME_LEN] = {}; - uint8_t rx[FRAME_LEN] = {}; + std::array tx{}; + std::array rx{}; + + populateTx(outgoing_frame, tx); + + spiTransfer(spi_fds_[motor], tx.data(), rx.data(), FRAME_LEN, SPI_SPEED_HZ); + + // Frame integrity check + const uint8_t rx_crc = Crc8Autosar::calc(rx.data(), FRAME_LEN - 1); + if (rx[5] != rx_crc) + { + LOG(WARNING) << "Received frame that failed integrity check. Expected CRC " + << static_cast(rx_crc) << " but got " << static_cast(rx[5]) + << " for motor " << motor << ". RX: " << static_cast(rx[0]) + << " " << static_cast(rx[1]) << " " << static_cast(rx[2]) + << " " << static_cast(rx[3]) << " " << static_cast(rx[4]) + << " " << static_cast(rx[5]); + return; + } + processRx(motor, rx); +} + +void StSpinMotorController::populateTx(const OutgoingFrame& outgoing_frame, + std::array& tx) +{ std::visit( [&](TFrame&& frame) { @@ -281,24 +292,12 @@ void StSpinMotorController::sendAndReceiveFrame(const MotorIndex& motor, }, outgoing_frame); - tx[5] = Crc8Autosar::calc(tx, FRAME_LEN - 1); - - spiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], tx, rx, FRAME_LEN, - SPI_SPEED_HZ); - - // Frame integrity check - const uint8_t rx_crc = Crc8Autosar::calc(rx, FRAME_LEN - 1); - if (rx[5] != rx_crc) - { - LOG(WARNING) << "Received frame that failed integrity check. Expected CRC " - << static_cast(rx_crc) << " but got " << static_cast(rx[5]) - << " for motor " << motor << ". RX: " << static_cast(rx[0]) - << " " << static_cast(rx[1]) << " " << static_cast(rx[2]) - << " " << static_cast(rx[3]) << " " << static_cast(rx[4]) - << " " << static_cast(rx[5]); - return; - } + 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: diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index e17fcd69e2..e6c2af5eed 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -18,77 +18,37 @@ class StSpinMotorController : public MotorController void reset() override; - MotorFaultIndicator checkDriverFault(const MotorIndex& motor) override; + MotorFaultIndicator checkDriverFault(MotorIndex motor) override; - int readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) override; + int readThenWriteVelocity(MotorIndex motor, int target_velocity) override; void immediatelyDisable() override; private: - /** - * Opens a SPI file descriptor for the given motor - * - * @param motor the motor to open a SPI file descriptor for - */ - void openSpiFileDescriptor(const MotorIndex& motor); - using OutgoingFrame = std::variant; - /** - * 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(const MotorIndex& motor, OutgoingFrame outgoing_frame); - // Length of frame (in number of bytes) static constexpr unsigned int FRAME_LEN = 6; - // clang-format off - static const inline std::unordered_map ENABLED_MOTORS = { - {MotorIndex::FRONT_LEFT, true}, - {MotorIndex::BACK_LEFT, true}, - {MotorIndex::BACK_RIGHT, true}, - {MotorIndex::FRONT_RIGHT, true}, - {MotorIndex::DRIBBLER, false}, - }; - // clang-format on - - // SPI Chip Selects - // clang-format off - static const inline std::unordered_map CHIP_SELECTS = { - {MotorIndex::FRONT_LEFT, 0}, - {MotorIndex::BACK_LEFT, 1}, - {MotorIndex::BACK_RIGHT, 2}, - {MotorIndex::FRONT_RIGHT, 3}, - {MotorIndex::DRIBBLER, 4}, - }; - // clang-format on - - // SPI Motor Driver Paths // clang-format off static const inline std::unordered_map SPI_PATHS = { {MotorIndex::FRONT_LEFT, "/dev/spidev0.2"}, {MotorIndex::BACK_LEFT, "/dev/spidev0.3"}, {MotorIndex::BACK_RIGHT, "/dev/spidev0.0"}, {MotorIndex::FRONT_RIGHT, "/dev/spidev0.4"}, - {MotorIndex::DRIBBLER, "/dev/spidev0.1"}, }; // clang-format on - // SPI Configs - static constexpr uint32_t SPI_SPEED_HZ = 100000; // 100 KHz - static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; // 250 MHz + 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; - // SPI File Descriptors mapping from Chip Select -> File Descriptor - std::array()> file_descriptors_; + // SPI file descriptors + std::unordered_map spi_fds_; struct MotorStatus { @@ -107,5 +67,25 @@ class StSpinMotorController : public MotorController std::unique_ptr reset_gpio_; + /** + * 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); + + void populateTx(const OutgoingFrame& outgoing_frame, + std::array& tx); + + void processRx(MotorIndex motor, const std::array& rx); + 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 index 6b8a2a17b7..2a70ec3b57 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -47,7 +47,7 @@ class StSpinMotorControllerTest LOG(INFO) << "Motor controller setup complete"; - for (auto motor : args_.enabled_motors) + for (const MotorIndex motor : args_.enabled_motors) { if (args_.torque_pid.kp && args_.torque_pid.ki) { @@ -77,7 +77,7 @@ class StSpinMotorControllerTest { LOG(INFO) << "Running speed profile"; - for (auto motor : args_.enabled_motors) + for (const MotorIndex motor : args_.enabled_motors) { motor_controller->sendAndReceiveFrame( motor, SetResponseTypeFrame{StSpinResponseType::SPEED_AND_FAULTS}); @@ -87,14 +87,18 @@ class StSpinMotorControllerTest { for (int i = 0; i < 2000; ++i) { - for (auto motor : args_.enabled_motors) + for (const MotorIndex motor : args_.enabled_motors) { motor_controller->readThenWriteVelocity(motor, setpoint); motor_controller->checkDriverFault(motor); + std::stringstream target_speed_label; + target_speed_label << "target_speed_rpm_" << motor; + std::stringstream measured_speed_label; + measured_speed_label << "measured_speed_rpm_" << motor; LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"target_speed_rpm", setpoint}, - {"measured_speed_rpm", + {target_speed_label.str(), setpoint}, + {measured_speed_label.str(), motor_controller->motor_status_.at(motor) .measured_speed_rpm}, }); @@ -107,7 +111,7 @@ class StSpinMotorControllerTest { LOG(INFO) << "Running torque profile"; - for (auto motor : args_.enabled_motors) + for (const MotorIndex motor : args_.enabled_motors) { motor_controller->sendAndReceiveFrame( motor, SetResponseTypeFrame{StSpinResponseType::IQ_AND_ID}); @@ -117,7 +121,7 @@ class StSpinMotorControllerTest { for (int i = 0; i < 2000; ++i) { - for (auto motor : args_.enabled_motors) + for (const MotorIndex motor : args_.enabled_motors) { motor_controller->sendAndReceiveFrame( motor, SetTargetTorqueFrame{ @@ -125,9 +129,15 @@ class StSpinMotorControllerTest .motor_target_torque = setpoint, }); + std::stringstream iq_label; + iq_label << "Iq_" << motor; + std::stringstream id_label; + id_label << "Id_" << motor; LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"Iq", motor_controller->motor_status_.at(motor).iq}, - {"Id", motor_controller->motor_status_.at(motor).id}, + {iq_label.str(), + motor_controller->motor_status_.at(motor).iq}, + {id_label.str(), + motor_controller->motor_status_.at(motor).id}, }); } std::this_thread::sleep_for(std::chrono::milliseconds(2)); diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index 496eaca0ea..d399642ea7 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -73,8 +73,7 @@ MotorControllerStatus TmcMotorController::earlyPoll() return MotorControllerStatus::OK; } -int TmcMotorController::readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) +int TmcMotorController::readThenWriteVelocity(MotorIndex motor, int target_velocity) { const int velocity_erpm = readThenWriteValue( motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, target_velocity); @@ -281,7 +280,7 @@ void TmcMotorController::configureHall(const MotorIndex& motor) TMC4671_VELOCITY_PHI_E_HAL); } -MotorFaultIndicator TmcMotorController::checkDriverFault(const MotorIndex& motor) +MotorFaultIndicator TmcMotorController::checkDriverFault(MotorIndex motor) { bool drive_enabled = true; std::unordered_set motor_faults; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index f251736509..472908f106 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -17,12 +17,11 @@ class TmcMotorController : public MotorController void reset() override; - MotorFaultIndicator checkDriverFault(const MotorIndex& motor) override; + MotorFaultIndicator checkDriverFault(MotorIndex motor) override; void immediatelyDisable() override; - int readThenWriteVelocity(const MotorIndex& motor, - const int& target_velocity) override; + int readThenWriteVelocity(MotorIndex motor, int target_velocity) override; /** * Trinamic API binding, sets spi_demux_select_0|1 pins appropriately and diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index 54fc9f13a0..5eed09cf93 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -21,8 +21,8 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); // Sink for PlotJuggler plotting - auto plotjuggler_handle = logWorker->addSink( - std::make_unique("end0"), &PlotJugglerSink::sendToPlotJuggler); + auto plotjuggler_handle = logWorker->addSink(std::make_unique(), + &PlotJugglerSink::sendToPlotJuggler); g3::only_change_at_initialization::addLogLevel(CSV); g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); From ff88bcb1fbbf3dab509034caf3d00cf182cfb14b Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 16 Mar 2026 10:01:56 -0700 Subject: [PATCH 48/71] Add reflective_enum::nameOf utility function --- src/software/util/make_enum/make_enum.hpp | 33 ++++++++++++++++++----- 1 file changed, 27 insertions(+), 6 deletions(-) diff --git a/src/software/util/make_enum/make_enum.hpp b/src/software/util/make_enum/make_enum.hpp index 8b26be8bc2..e5c29f8017 100644 --- a/src/software/util/make_enum/make_enum.hpp +++ b/src/software/util/make_enum/make_enum.hpp @@ -61,11 +61,16 @@ } \ 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 @@ -104,6 +109,22 @@ constexpr auto values(); template 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 From 7b0216fc196e4ef55965c9c369963a2f671f94e2 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 16 Mar 2026 10:53:05 -0700 Subject: [PATCH 49/71] Cancel StSpinMotorControllerTest with Ctrl+C --- .../stspin_motor_controller.cpp | 37 ++++-- .../stspin_motor_controller.h | 10 +- .../stspin_motor_controller_test.cpp | 113 ++++++++---------- .../embedded/motor_controller/stspin_types.h | 4 + 4 files changed, 86 insertions(+), 78 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index fee00e01c5..d10d7764b5 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -167,15 +167,6 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, sendAndReceiveFrame(motor, outgoing_frame); - std::stringstream target_speed_label; - target_speed_label << "target_speed_rpm_" << motor; - std::stringstream measured_speed_label; - measured_speed_label << "measured_speed_rpm_" << motor; - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {target_speed_label.str(), target_velocity}, - {measured_speed_label.str(), motor_status_.at(motor).measured_speed_rpm}, - }); - return motor_status_.at(motor).measured_speed_rpm; } @@ -240,7 +231,11 @@ void StSpinMotorController::populateTx(const OutgoingFrame& outgoing_frame, [&](TFrame&& frame) { using T = std::decay_t; - if constexpr (std::is_same_v) + 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); @@ -334,3 +329,25 @@ void StSpinMotorController::processRx(const MotorIndex motor, } } } + +void StSpinMotorController::sendMotorStatusToPlotJuggler(const MotorIndex motor) +{ + for (const StSpinResponseType response_type : + {StSpinResponseType::IQ_AND_ID, StSpinResponseType::SPEED_AND_FAULTS}) + { + sendAndReceiveFrame(motor, SetResponseTypeFrame{response_type}); + } + + // Response type changes only take effect *after* we request them, + // so send a no-op frame to receive a response of the last type we requested + sendAndReceiveFrame(motor, NoOpFrame{}); + + const MotorStatus& motor_status = motor_status_.at(motor); + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"target_speed_rpm_" + motor, motor_status.target_speed_rpm}, + {"measured_speed_rpm_" + motor, motor_status.measured_speed_rpm}, + {"iq_" + motor, motor_status.iq}, + {"id_" + motor, motor_status.id}, + }); +} diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index e6c2af5eed..1c4af253e7 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -1,6 +1,5 @@ #pragma once -#include "software/embedded/constants/constants.h" #include "software/embedded/gpio/setup_gpio.hpp" #include "software/embedded/motor_controller/motor_controller.h" #include "software/embedded/motor_controller/motor_fault_indicator.h" @@ -26,9 +25,9 @@ class StSpinMotorController : public MotorController private: using OutgoingFrame = - std::variant; + std::variant; // Length of frame (in number of bytes) static constexpr unsigned int FRAME_LEN = 6; @@ -53,6 +52,7 @@ class StSpinMotorController : public MotorController struct MotorStatus { bool enabled; + int16_t target_speed_rpm; int16_t measured_speed_rpm; uint16_t faults; int16_t iq; @@ -87,5 +87,7 @@ class StSpinMotorController : public MotorController void processRx(MotorIndex motor, const std::array& rx); + 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 index 2a70ec3b57..85bf440706 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -2,13 +2,16 @@ #include #include +#include #include #include -#include "proto/message_translation/tbots_protobuf.h" #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: @@ -22,6 +25,7 @@ class StSpinMotorControllerTest bool help = false; std::string mode = "speed"; + int setpoint_duration_ms = 2000; std::vector setpoints; std::unordered_set enabled_motors; PidOption torque_pid; @@ -73,75 +77,52 @@ class StSpinMotorControllerTest LOG(INFO) << "PID configuration complete"; - if (args_.mode == "speed") - { - LOG(INFO) << "Running speed profile"; + LOG(INFO) << "Running " << args_.mode << " profile"; - for (const MotorIndex motor : args_.enabled_motors) + for (const int16_t setpoint : args_.setpoints) + { + if (g_stop_requested) { - motor_controller->sendAndReceiveFrame( - motor, SetResponseTypeFrame{StSpinResponseType::SPEED_AND_FAULTS}); + break; } - for (const int16_t setpoint : args_.setpoints) + StSpinMotorController::OutgoingFrame command_frame; + if (args_.mode == "speed") { - for (int i = 0; i < 2000; ++i) - { - for (const MotorIndex motor : args_.enabled_motors) - { - motor_controller->readThenWriteVelocity(motor, setpoint); - motor_controller->checkDriverFault(motor); - - std::stringstream target_speed_label; - target_speed_label << "target_speed_rpm_" << motor; - std::stringstream measured_speed_label; - measured_speed_label << "measured_speed_rpm_" << motor; - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {target_speed_label.str(), setpoint}, - {measured_speed_label.str(), - motor_controller->motor_status_.at(motor) - .measured_speed_rpm}, - }); - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } + command_frame = SetTargetSpeedFrame{ + .motor_enabled = true, + .motor_target_speed_rpm = setpoint, + }; } - } - else if (args_.mode == "torque") - { - LOG(INFO) << "Running torque profile"; - - for (const MotorIndex motor : args_.enabled_motors) + else if (args_.mode == "torque") { - motor_controller->sendAndReceiveFrame( - motor, SetResponseTypeFrame{StSpinResponseType::IQ_AND_ID}); + command_frame = SetTargetTorqueFrame{ + .motor_enabled = true, + .motor_target_torque = setpoint, + }; } - for (const int16_t setpoint : args_.setpoints) + auto start_time = std::chrono::system_clock::now(); + auto duration = std::chrono::milliseconds(args_.setpoint_duration_ms); + while (std::chrono::system_clock::now() - start_time < duration) { - for (int i = 0; i < 2000; ++i) + if (g_stop_requested) { - for (const MotorIndex motor : args_.enabled_motors) - { - motor_controller->sendAndReceiveFrame( - motor, SetTargetTorqueFrame{ - .motor_enabled = true, - .motor_target_torque = setpoint, - }); - - std::stringstream iq_label; - iq_label << "Iq_" << motor; - std::stringstream id_label; - id_label << "Id_" << motor; - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {iq_label.str(), - motor_controller->motor_status_.at(motor).iq}, - {id_label.str(), - motor_controller->motor_status_.at(motor).id}, - }); - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); + break; } + + for (const MotorIndex motor : args_.enabled_motors) + { + motor_controller->sendAndReceiveFrame(motor, command_frame); + } + + for (const MotorIndex motor : args_.enabled_motors) + { + motor_controller->sendMotorStatusToPlotJuggler(motor); + motor_controller->checkDriverFault(motor); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(2)); } } @@ -221,14 +202,16 @@ int main(int argc, char** argv) "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"); + ("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; @@ -289,6 +272,8 @@ int main(int argc, char** argv) 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(); diff --git a/src/software/embedded/motor_controller/stspin_types.h b/src/software/embedded/motor_controller/stspin_types.h index f079947ca4..058749a7b7 100644 --- a/src/software/embedded/motor_controller/stspin_types.h +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -38,6 +38,10 @@ enum class StSpinFaultCode DP_FAULT = 0x0400 }; +struct NoOpFrame +{ +}; + struct SetTargetSpeedFrame { bool motor_enabled; From 67cbc4d1de9b2f04d0666d3bf555b10974078242 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 16 Mar 2026 12:03:37 -0700 Subject: [PATCH 50/71] Update robot constants with correct wheel angles for 2026 robot --- .../ssl_simulation_robot_control.cpp | 2 +- .../ssl_simulation_robot_control.h | 2 +- .../ssl_simulation_robot_control_test.cpp | 4 +- .../message_translation/tbots_protobuf.cpp | 4 +- .../message_translation/tbots_protobuf.h | 4 +- .../tbots_protobuf_test.cpp | 2 +- .../primitive/primitive_msg_factory_test.cpp | 4 +- src/shared/2021_robot_constants.cpp | 35 ------- src/shared/2021_robot_constants.h | 10 -- src/shared/BUILD | 9 +- src/shared/constants.h | 2 +- src/shared/robot_constants.cpp | 61 +++++++++++++ src/shared/robot_constants.h | 91 ++++++++----------- src/software/ai/evaluation/intercept_test.cpp | 2 +- .../ai/evaluation/time_to_travel_test.cpp | 2 +- .../ai/hl/stp/tactic/primitive_test.cpp | 8 +- .../embedded/constants/py_constants.py | 2 +- .../stspin_motor_controller_test.cpp | 6 +- src/software/embedded/primitive_executor.cpp | 2 +- src/software/embedded/primitive_executor.h | 4 +- src/software/embedded/services/motor.cpp | 2 +- src/software/embedded/services/motor.h | 4 +- src/software/embedded/thunderloop.cpp | 4 +- src/software/embedded/thunderloop.h | 6 +- src/software/embedded/thunderloop_main.cpp | 4 +- src/software/er_force_simulator_main.cpp | 4 +- src/software/physics/euclidean_to_wheel.cpp | 4 +- src/software/physics/euclidean_to_wheel.h | 4 +- .../physics/euclidean_to_wheel_test.cpp | 6 +- src/software/python_bindings.cpp | 6 +- .../simulated_er_force_sim_test_fixture.cpp | 4 +- .../simulation/er_force_simulator.cpp | 2 +- src/software/simulation/er_force_simulator.h | 4 +- .../simulation/er_force_simulator_test.cpp | 12 +-- .../drive_and_dribbler_widget.py | 2 +- .../handheld_controller_widget.py | 2 +- src/software/world/robot.cpp | 8 +- src/software/world/robot.h | 12 +-- src/software/world/robot_state.h | 1 - src/software/world/robot_test.cpp | 2 +- 40 files changed, 172 insertions(+), 177 deletions(-) delete mode 100644 src/shared/2021_robot_constants.cpp delete mode 100644 src/shared/2021_robot_constants.h create mode 100644 src/shared/robot_constants.cpp diff --git a/src/proto/message_translation/ssl_simulation_robot_control.cpp b/src/proto/message_translation/ssl_simulation_robot_control.cpp index 7f1d7d998f..09826f49ef 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control.cpp +++ b/src/proto/message_translation/ssl_simulation_robot_control.cpp @@ -52,7 +52,7 @@ std::unique_ptr createRobotMoveCommand( std::unique_ptr getRobotCommandFromDirectControl( unsigned int robot_id, std::unique_ptr direct_control, - RobotConstants_t& robot_constants) + RobotConstants& robot_constants) { auto move_command = createRobotMoveCommand( *direct_control, robot_constants.front_wheel_angle_deg, diff --git a/src/proto/message_translation/ssl_simulation_robot_control.h b/src/proto/message_translation/ssl_simulation_robot_control.h index 4af93ad139..dfc3952f9f 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control.h +++ b/src/proto/message_translation/ssl_simulation_robot_control.h @@ -34,7 +34,7 @@ std::unique_ptr createRobotMoveCommand( std::unique_ptr getRobotCommandFromDirectControl( unsigned int robot_id, std::unique_ptr direct_control, - RobotConstants_t& robot_constants); + RobotConstants& robot_constants); /** * Creates a RobotCommand proto diff --git a/src/proto/message_translation/ssl_simulation_robot_control_test.cpp b/src/proto/message_translation/ssl_simulation_robot_control_test.cpp index 42abf85524..0f5b84828d 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control_test.cpp +++ b/src/proto/message_translation/ssl_simulation_robot_control_test.cpp @@ -2,12 +2,12 @@ #include -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" class SSLSimulationProtoTest : public ::testing::Test { protected: - RobotConstants_t robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); }; TEST_F(SSLSimulationProtoTest, test_create_robot_move_command_forward_from_primitive) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index cc21417cd7..a54018cf3a 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -489,7 +489,7 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( } int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, - RobotConstants_t robot_constants) + RobotConstants robot_constants) { switch (dribbler_mode) { @@ -507,7 +507,7 @@ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, double convertMaxAllowedSpeedModeToMaxAllowedSpeed( TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, - RobotConstants_t robot_constants) + RobotConstants robot_constants) { switch (max_allowed_speed_mode) { diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 50ab53c374..c56045774d 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -268,7 +268,7 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( * @return the dribbler speed in RPM */ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, - RobotConstants_t robot_constants); + RobotConstants robot_constants); /** * Convert max allowed speed mode to max allowed speed @@ -280,4 +280,4 @@ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, */ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, - RobotConstants_t robot_constants); + RobotConstants robot_constants); diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 9cdde04170..9d9958697d 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -152,7 +152,7 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) // Generate a trajectory, and then generate a TbotsProto::TrajectoryPathParams2D // with the same parameters as the trajectory, finally, generate a second trajectory // from the parameters and make sure the two trajectories are equal. - RobotConstants robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); Point start_position(0.0, 0.0); Point destination(0.0, 0.0); diff --git a/src/proto/primitive/primitive_msg_factory_test.cpp b/src/proto/primitive/primitive_msg_factory_test.cpp index 3bc46d40e1..994113ecd8 100644 --- a/src/proto/primitive/primitive_msg_factory_test.cpp +++ b/src/proto/primitive/primitive_msg_factory_test.cpp @@ -2,14 +2,14 @@ #include -#include "shared/2021_robot_constants.h" #include "shared/constants.h" +#include "shared/robot_constants.h" #include "software/test_util/test_util.h" class PrimitiveFactoryTest : public testing::Test { protected: - RobotConstants_t robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); }; TEST_F(PrimitiveFactoryTest, test_auto_chip_or_kick_equality) diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp deleted file mode 100644 index 79d1a206cd..0000000000 --- a/src/shared/2021_robot_constants.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include "shared/2021_robot_constants.h" - -#include "shared/constants.h" - -RobotConstants_t create2021RobotConstants(void) -{ - RobotConstants_t robot_constants = { - .mass_kg = 2.5f, // determined experimentally - .inertial_factor = 0.37f, // determined experimentally - .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), - .jerk_limit_kg_m_per_s_3 = 40.0f, - .front_wheel_angle_deg = 25.0f, - .back_wheel_angle_deg = 40.0f, - - .front_of_robot_width_meters = 0.11f, - .dribbler_width_meters = 0.07825f, - // Dribbler speeds are negative as that is the direction that sucks the ball in - .indefinite_dribbler_speed_rpm = -10000, - .max_force_dribbler_speed_rpm = -12000, - - // Motor constant - .motor_max_acceleration_m_per_s_2 = 4.5f, - - // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, - - // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, - - .wheel_radius_meters = 0.03f}; - return robot_constants; -} diff --git a/src/shared/2021_robot_constants.h b/src/shared/2021_robot_constants.h deleted file mode 100644 index 3c071d3f6f..0000000000 --- a/src/shared/2021_robot_constants.h +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -#include "shared/robot_constants.h" - -/** - * Creates robot constants for the 2021 robot - * - * @return robot constants for the 2021 robot - */ -RobotConstants_t create2021RobotConstants(void); diff --git a/src/shared/BUILD b/src/shared/BUILD index 49839f3e46..8b77da71e1 100644 --- a/src/shared/BUILD +++ b/src/shared/BUILD @@ -22,12 +22,7 @@ cc_library( cc_library( name = "robot_constants", - srcs = [ - "2021_robot_constants.cpp", - ], - hdrs = [ - "2021_robot_constants.h", - "robot_constants.h", - ], + srcs = ["robot_constants.cpp"], + hdrs = ["robot_constants.h"], deps = [":constants"], ) diff --git a/src/shared/constants.h b/src/shared/constants.h index da0a675053..fd529c352e 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -70,7 +70,7 @@ static const double BALL_IN_PLAY_DISTANCE_THRESHOLD_METERS = 0.05; // The max allowed height of the robots, in metres static const double ROBOT_MAX_HEIGHT_METERS = 0.15; // The max allowed radius of the robots, in metres -static const double ROBOT_MAX_RADIUS_METERS = 0.085; +static const double ROBOT_MAX_RADIUS_METERS = 0.09; // The distance from the center of the robot to the front face (the flat part), in meters static const double DIST_TO_FRONT_OF_ROBOT_METERS = 0.078; // The approximate radius of the ball according to the SSL rulebook diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp new file mode 100644 index 0000000000..05d3242ac0 --- /dev/null +++ b/src/shared/robot_constants.cpp @@ -0,0 +1,61 @@ +#include "shared/robot_constants.h" + +#include "shared/constants.h" + +RobotConstants create2026RobotConstants() +{ + return { + .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), + .front_wheel_angle_deg = 26.0f, + .back_wheel_angle_deg = 40.0f, + + .front_of_robot_width_meters = 0.11f, + .dribbler_width_meters = 0.07825f, + + // Dribbler speeds are negative as that is the direction that sucks the ball in + .indefinite_dribbler_speed_rpm = -10000, + .max_force_dribbler_speed_rpm = -12000, + + // Motor constant + .motor_max_acceleration_m_per_s_2 = 4.5f, + + // Robot's linear movement constants + .robot_max_speed_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, + + // Robot's angular movement constants + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f}; +} + +RobotConstants create2021RobotConstants() +{ + return { + .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), + .front_wheel_angle_deg = 32.06f, + .back_wheel_angle_deg = 46.04f, + + .front_of_robot_width_meters = 0.11f, + .dribbler_width_meters = 0.07825f, + + // Dribbler speeds are negative as that is the direction that sucks the ball in + .indefinite_dribbler_speed_rpm = -10000, + .max_force_dribbler_speed_rpm = -12000, + + // Motor constant + .motor_max_acceleration_m_per_s_2 = 4.5f, + + // Robot's linear movement constants + .robot_max_speed_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, + + // Robot's angular movement constants + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f}; +} diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 6c3b587e7f..8dd2e049df 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -1,63 +1,39 @@ #pragma once -/** - * This struct represents robot constants - */ -typedef struct RobotConstants +struct RobotConstants { - // The mass of the entire robot including batteries [kg] - // Determined experimentally by weighing the robot and battery - float mass_kg; - - // The inertial factor - float inertial_factor; - // The radius of the robot [m] float robot_radius_m; - // The maximum jerk this robot may safely undergo [m/s^3] - float jerk_limit_kg_m_per_s_3; - - // The front_wheel_angle_deg and back_wheel_angle_deg are measured as absolute angle - // to each of the wheels from the front y axis of the robot. In the ASCII art below, - // front_wheel_angle_deg = A and back_wheel_angle_deg = A + B. The angles are assumed - // to be left/right symmetrical + // The front_wheel_angle_deg and back_wheel_angle_deg are measured as absolute + // angles from the robot's y-axis to each wheel axle. + // + // In the ASCII diagram below: + // - front_wheel_angle_deg = A + // - back_wheel_angle_deg = B // + // The angles are assumed to be symmetric for the left and right sides of the robot. + // + // y // ▲ - // │ - // │ - // │ - // │ - // │ - // │ - // │ - // │ - // │ - // *#### ### ###│### ### ####* - // *## │ ##* - // *## │ ##* wheel - // *## │ ##* │ - // *## │ xx##*◄──┘ - // *## │ A xxx ##* - // *## │ xxxx ##* - // *## │ xxxx ##* - // *## │xxxx ##* - // *## xx B ##* - // *## xx ##* - // *## xx ##* - // *## xx ##* - // *## xx ##* - // *## xx ##* - // *## x ##* - // *## ##*◄──┐ - // *## ##* │ - // *## ##* wheel - // *** ### ### *** - - // angle between each front wheel and the front y axis of the robot [degrees] + // | + // Back wheel │ Front wheel + // └────────► , - │ - , ◄───────┘ + // , '\ │ /' , + // , \ B │ A / │ + // , \ │ / │ + // , \ │ / │ + // , └────────┼───────► x Front of robot + // , │ + // , │ + // , │ + // , .' + // ' - , _ , ' + + // The angle between y-axis of the robot and the front wheel axles [degrees] float front_wheel_angle_deg; - // angle between each back wheel and the front y axis of the robot [degrees] + // The angle between y-axis of the robot and the rear wheel axles [degrees] float back_wheel_angle_deg; // The total width of the entire flat face on the front of the robot [meters] @@ -93,5 +69,18 @@ typedef struct RobotConstants // The radius of the wheel, in meters float wheel_radius_meters; +}; + +/** + * Creates robot constants for the 2026 robot + * + * @return robot constants for the 2026 robot + */ +RobotConstants create2026RobotConstants(); -} RobotConstants_t; +/** + * Creates robot constants for the 2021 robot + * + * @return robot constants for the 2021 robot + */ +RobotConstants create2021RobotConstants(); diff --git a/src/software/ai/evaluation/intercept_test.cpp b/src/software/ai/evaluation/intercept_test.cpp index 613510eb74..0d8d979675 100644 --- a/src/software/ai/evaluation/intercept_test.cpp +++ b/src/software/ai/evaluation/intercept_test.cpp @@ -39,7 +39,7 @@ TEST(InterceptEvaluationTest, Ball ball({0, 0}, {6, 0}, Timestamp::fromSeconds(0)); Robot robot(0, {2, 0}, {0, 0}, Angle::zero(), AngularVelocity::zero(), Timestamp::fromSeconds(0), std::set(), - create2021RobotConstants()); + create2026RobotConstants()); // We should be able to find an intercept auto best_intercept = findBestInterceptForBall(ball, field, robot); diff --git a/src/software/ai/evaluation/time_to_travel_test.cpp b/src/software/ai/evaluation/time_to_travel_test.cpp index e0b35794f1..123d28a280 100644 --- a/src/software/ai/evaluation/time_to_travel_test.cpp +++ b/src/software/ai/evaluation/time_to_travel_test.cpp @@ -7,7 +7,7 @@ class TimeToTravel : public ::testing::Test { protected: - RobotConstants_t robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); }; TEST_F(TimeToTravel, getTimeToTravelDistance_already_at_dest) diff --git a/src/software/ai/hl/stp/tactic/primitive_test.cpp b/src/software/ai/hl/stp/tactic/primitive_test.cpp index c31e68ded8..a0015fd7da 100644 --- a/src/software/ai/hl/stp/tactic/primitive_test.cpp +++ b/src/software/ai/hl/stp/tactic/primitive_test.cpp @@ -1,6 +1,6 @@ #include -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" #include "software/ai/hl/stp/tactic/move_primitive.h" #include "software/ai/hl/stp/tactic/stop_primitive.h" #include "software/geom/algorithms/contains.h" @@ -16,9 +16,9 @@ class PrimitiveTest : public testing::Test } protected: - RobotConstants_t robot_constants = create2021RobotConstants(); - Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); - std::shared_ptr world = TestUtil::createBlankTestingWorld(); + RobotConstants robot_constants = create2026RobotConstants(); + Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); + std::shared_ptr world = TestUtil::createBlankTestingWorld(); RobotNavigationObstacleFactory obstacle_factory = RobotNavigationObstacleFactory(TbotsProto::RobotNavigationObstacleConfig()); }; diff --git a/src/software/embedded/constants/py_constants.py b/src/software/embedded/constants/py_constants.py index 43b8e1af03..d0c14d17c8 100644 --- a/src/software/embedded/constants/py_constants.py +++ b/src/software/embedded/constants/py_constants.py @@ -3,7 +3,7 @@ import software.python_bindings as tbots_cpp DEFAULT_PRIMITIVE_DURATION = 2.0 -ROBOT_CONSTANTS = tbots_cpp.create2021RobotConstants() +ROBOT_CONSTANTS = tbots_cpp.create2026RobotConstants() ROBOT_MAX_ANG_SPEED_RAD_PER_S = ROBOT_CONSTANTS.robot_max_ang_speed_rad_per_s ROBOT_MAX_SPEED_M_PER_S = ROBOT_CONSTANTS.robot_max_speed_m_per_s diff --git a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp index 85bf440706..d293800565 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -23,8 +23,8 @@ class StSpinMotorControllerTest std::optional ki; }; - bool help = false; - std::string mode = "speed"; + bool help = false; + std::string mode = "speed"; int setpoint_duration_ms = 2000; std::vector setpoints; std::unordered_set enabled_motors; @@ -103,7 +103,7 @@ class StSpinMotorControllerTest } auto start_time = std::chrono::system_clock::now(); - auto duration = std::chrono::milliseconds(args_.setpoint_duration_ms); + auto duration = std::chrono::milliseconds(args_.setpoint_duration_ms); while (std::chrono::system_clock::now() - start_time < duration) { if (g_stop_requested) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index f1b113059f..c351a22e55 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,7 +11,7 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor(const Duration time_step, - const RobotConstants_t &robot_constants, + const RobotConstants &robot_constants, const TeamColour friendly_team_colour, const RobotId robot_id) : current_primitive_(), diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index fb6ef8dd51..bd5a7cd028 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -21,7 +21,7 @@ class PrimitiveExecutor * @param robot_id The id of the robot which uses this primitive executor */ explicit PrimitiveExecutor(const Duration time_step, - const RobotConstants_t &robot_constants, + const RobotConstants &robot_constants, const TeamColour friendly_team_colour, const RobotId robot_id); @@ -82,7 +82,7 @@ class PrimitiveExecutor AngularVelocity angular_velocity_; Angle orientation_; TeamColour friendly_team_colour_; - RobotConstants_t robot_constants_; + RobotConstants robot_constants_; std::optional trajectory_path_; std::optional angular_trajectory_; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index fe6d92d2e4..9d7e04929c 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -9,7 +9,7 @@ static double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; static int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; -MotorService::MotorService(const RobotConstants_t& robot_constants) +MotorService::MotorService(const RobotConstants& robot_constants) : motor_controller_(setupMotorController()), robot_constants_(robot_constants), euclidean_to_four_wheel_(robot_constants), diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 2fa0d45544..4109aaa32e 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -34,7 +34,7 @@ class MotorService * * @param robot_constants The robot constants */ - MotorService(const RobotConstants_t& robot_constants); + MotorService(const RobotConstants& robot_constants); virtual ~MotorService() = default; @@ -99,7 +99,7 @@ class MotorService // Flag indicating whether the motors have been calibrated bool is_initialized_ = false; - RobotConstants_t robot_constants_; + RobotConstants robot_constants_; EuclideanToWheel euclidean_to_four_wheel_; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 953cde77d7..e39f96acb0 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -7,8 +7,8 @@ #include "proto/robot_crash_msg.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" -#include "shared/2021_robot_constants.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" @@ -68,7 +68,7 @@ extern "C" } } -Thunderloop::Thunderloop(const RobotConstants_t& robot_constants, bool enable_log_merging, +Thunderloop::Thunderloop(const RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz) : redis_client_( std::make_unique(REDIS_DEFAULT_HOST, REDIS_DEFAULT_PORT)), diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 3657f21165..959eab6fb4 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -7,8 +7,8 @@ #include #include "proto/tbots_software_msgs.pb.h" -#include "shared/2021_robot_constants.h" #include "shared/constants.h" +#include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" #include "software/embedded/redis/redis_client.h" #include "software/embedded/services/motor.h" @@ -49,7 +49,7 @@ class Thunderloop * @param enable_log_merging Whether to merge repeated log message or not * @param loop_hz The rate to run the loop */ - Thunderloop(const RobotConstants_t &robot_constants, bool enable_log_merging, + Thunderloop(const RobotConstants &robot_constants, bool enable_log_merging, const int loop_hz); ~Thunderloop(); @@ -141,7 +141,7 @@ class Thunderloop TbotsProto::Timestamp time_sent_; // Current State - RobotConstants_t robot_constants_; + RobotConstants robot_constants_; Angle current_orientation_; int robot_id_; int channel_id_; diff --git a/src/software/embedded/thunderloop_main.cpp b/src/software/embedded/thunderloop_main.cpp index 7218380629..cb7c529ffd 100644 --- a/src/software/embedded/thunderloop_main.cpp +++ b/src/software/embedded/thunderloop_main.cpp @@ -11,8 +11,8 @@ #include #include "proto/tbots_software_msgs.pb.h" -#include "shared/2021_robot_constants.h" #include "shared/constants.h" +#include "shared/robot_constants.h" #include "software/embedded/thunderloop.h" #include "software/logger/network_logger.h" #include "software/world/robot_state.h" @@ -112,7 +112,7 @@ int main(int argc, char** argv) reserveProcessMemory(pre_allocation_size); auto thunderloop = - Thunderloop(create2021RobotConstants(), args.enable_log_merging, THUNDERLOOP_HZ); + Thunderloop(create2026RobotConstants(), args.enable_log_merging, THUNDERLOOP_HZ); thunderloop.runLoop(); return 0; diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 8f8261b025..db2bb11352 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -88,12 +88,12 @@ int main(int argc, char **argv) if (args.division == "div_a") { er_force_sim = std::make_shared( - TbotsProto::FieldType::DIV_A, create2021RobotConstants(), realism_config); + TbotsProto::FieldType::DIV_A, create2026RobotConstants(), realism_config); } else { er_force_sim = std::make_shared( - TbotsProto::FieldType::DIV_B, create2021RobotConstants(), realism_config); + TbotsProto::FieldType::DIV_B, create2026RobotConstants(), realism_config); } std::mutex simulator_mutex; diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index a6c6d70d12..1c42c17512 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -4,11 +4,11 @@ #include "proto/message_translation/tbots_geometry.h" #include "proto/primitive/primitive_msg_factory.h" -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" -EuclideanToWheel::EuclideanToWheel(const RobotConstants_t& robot_constants) +EuclideanToWheel::EuclideanToWheel(const RobotConstants& robot_constants) : robot_constants_(robot_constants) { // Phi, the angle between the hemisphere line of the robot and the front wheel axles diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index 407d9836de..bd8ead3fdc 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -41,7 +41,7 @@ class EuclideanToWheel * * @param robot_constants The constants of the robot we are computing for. */ - explicit EuclideanToWheel(const RobotConstants_t& robot_constants); + explicit EuclideanToWheel(const RobotConstants& robot_constants); /** * Gets the wheel velocity from the Euclidean velocity. @@ -75,7 +75,7 @@ class EuclideanToWheel const double& time_to_ramp); private: - const RobotConstants_t robot_constants_; + const RobotConstants robot_constants_; /** * Euclidean velocity to wheel velocity coupling matrix. diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index b408e76248..7ce54bace7 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -12,8 +12,8 @@ class EuclideanToWheelTest : public ::testing::Test EuclideanSpace_t target_euclidean_velocity{}; WheelSpace_t expected_wheel_speeds{}; WheelSpace_t calculated_wheel_speeds{}; - RobotConstants robot_constants = create2021RobotConstants(); - double robot_radius = create2021RobotConstants().robot_radius_m; + RobotConstants robot_constants = create2026RobotConstants(); + double robot_radius = create2026RobotConstants().robot_radius_m; WheelSpace_t target_wheel_velocity{}; WheelSpace_t current_wheel_velocity{}; @@ -22,7 +22,7 @@ class EuclideanToWheelTest : public ::testing::Test EuclideanToWheel euclidean_to_four_wheel = - EuclideanToWheel(create2021RobotConstants()); + EuclideanToWheel(create2026RobotConstants()); }; TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_zero) diff --git a/src/software/python_bindings.cpp b/src/software/python_bindings.cpp index f72acae9be..d10009fad1 100644 --- a/src/software/python_bindings.cpp +++ b/src/software/python_bindings.cpp @@ -21,7 +21,6 @@ #include "proto/team.pb.h" #include "proto/world.pb.h" #include "pybind11_protobuf/native_proto_caster.h" -#include "shared/2021_robot_constants.h" #include "shared/robot_constants.h" #include "software/ai/passing/eighteen_zone_pitch_division.h" #include "software/ai/passing/pass_generator.h" @@ -272,10 +271,6 @@ PYBIND11_MODULE(python_bindings, m) .def_readwrite("max_force_dribbler_speed_rpm", &RobotConstants::max_force_dribbler_speed_rpm) .def_readwrite("robot_radius_m", &RobotConstants::robot_radius_m) - .def_readwrite("mass_kg", &RobotConstants::mass_kg) - .def_readwrite("inertial_factor", &RobotConstants::inertial_factor) - .def_readwrite("jerk_limit_kg_m_per_s_3", - &RobotConstants::jerk_limit_kg_m_per_s_3) .def_readwrite("front_wheel_angle_deg", &RobotConstants::front_wheel_angle_deg) .def_readwrite("back_wheel_angle_deg", &RobotConstants::back_wheel_angle_deg) .def_readwrite("front_of_robot_width_meters", @@ -292,6 +287,7 @@ PYBIND11_MODULE(python_bindings, m) &RobotConstants::robot_max_speed_m_per_s) .def_readwrite("robot_max_ang_speed_rad_per_s", &RobotConstants::robot_max_ang_speed_rad_per_s); + m.def("create2026RobotConstants", &create2026RobotConstants); m.def("create2021RobotConstants", &create2021RobotConstants); m.def("createPoint", &createPoint); diff --git a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp index a855576af0..e04bfeb7d7 100644 --- a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp +++ b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp @@ -11,7 +11,7 @@ #include "proto/message_translation/er_force_world.h" #include "proto/message_translation/ssl_wrapper.h" #include "proto/message_translation/tbots_protobuf.h" -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" #include "shared/test_util/test_util.h" #include "software/logger/logger.h" #include "software/simulation/er_force_simulator.h" @@ -181,7 +181,7 @@ void SimulatedErForceSimTestFixture::runTest( auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator(std::make_shared( - field_type, create2021RobotConstants(), realism_config, ramping)); + field_type, create2026RobotConstants(), realism_config, ramping)); // TODO (#2419): remove this to re-enable sigfpe checks fedisableexcept(FE_INVALID | FE_OVERFLOW); diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index b58d4989a3..69a45069fa 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -17,7 +17,7 @@ #include "software/world/robot_state.h" ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, - const RobotConstants_t& robot_constants, + const RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping, double primitive_executor_time_step) diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index b4e7833012..11485b76ae 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -28,7 +28,7 @@ class ErForceSimulator * @param realism_config realism configuration */ explicit ErForceSimulator(const TbotsProto::FieldType& field_type, - const RobotConstants_t& robot_constants, + const RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping = false, double primitive_executor_time_step_s = @@ -222,7 +222,7 @@ class ErForceSimulator std::unique_ptr er_force_sim; EuclideanToWheel euclidean_to_four_wheel; - RobotConstants_t robot_constants; + RobotConstants robot_constants; Field field; std::optional blue_robot_with_ball; diff --git a/src/software/simulation/er_force_simulator_test.cpp b/src/software/simulation/er_force_simulator_test.cpp index bda9a6d7c2..2b28b24101 100644 --- a/src/software/simulation/er_force_simulator_test.cpp +++ b/src/software/simulation/er_force_simulator_test.cpp @@ -7,7 +7,7 @@ #include "proto/message_translation/er_force_world.h" #include "proto/message_translation/tbots_protobuf.h" #include "proto/primitive/primitive_msg_factory.h" -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" #include "software/test_util/test_util.h" class ErForceSimulatorTest : public ::testing::Test @@ -24,7 +24,7 @@ class ErForceSimulatorTest : public ::testing::Test } std::shared_ptr simulator; - RobotConstants_t robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); }; TEST_F(ErForceSimulatorTest, set_ball_state_when_ball_does_not_already_exist) @@ -309,8 +309,8 @@ TEST_F(ErForceSimulatorTest, yellow_robot_add_robots_and_change_position) TEST(ErForceSimulatorFieldTest, check_field_A_configuration) { - RobotConstants_t robot_constants = create2021RobotConstants(); - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + RobotConstants robot_constants = create2026RobotConstants(); + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_A, robot_constants, realism_config); simulator->resetCurrentTime(); @@ -321,8 +321,8 @@ TEST(ErForceSimulatorFieldTest, check_field_A_configuration) TEST(ErForceSimulatorFieldTest, check_field_B_configuration) { - RobotConstants_t robot_constants = create2021RobotConstants(); - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + RobotConstants robot_constants = create2026RobotConstants(); + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_B, robot_constants, realism_config); simulator->resetCurrentTime(); diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 6df6356947..5331f5d301 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -32,7 +32,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.constants = tbots_cpp.create2021RobotConstants() + self.constants = tbots_cpp.create2026RobotConstants() self.enabled = True self.control_mode = ControlMode.VELOCITY diff --git a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py index ba9473509e..a0bdbdb767 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py @@ -33,7 +33,7 @@ def __init__(self) -> None: """Initialize the HandheldControllerWidget.""" super().__init__() - self.constants = tbots_cpp.create2021RobotConstants() + self.constants = tbots_cpp.create2026RobotConstants() self.handheld_controller: HandheldController | None = None diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 1b0dece191..d0db7cdc10 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -8,7 +8,7 @@ Robot::Robot(RobotId id, const Point &position, const Vector &velocity, const Angle &orientation, const AngularVelocity &angular_velocity, const Timestamp ×tamp, bool breakbeam_tripped, const std::set &unavailable_capabilities, - const RobotConstants_t &robot_constants) + const RobotConstants &robot_constants) : id_(id), current_state_(position, velocity, orientation, angular_velocity, breakbeam_tripped), @@ -22,7 +22,7 @@ Robot::Robot(RobotId id, const Point &position, const Vector &velocity, const Angle &orientation, const AngularVelocity &angular_velocity, const Timestamp ×tamp, const std::set &unavailable_capabilities, - const RobotConstants_t &robot_constants) + const RobotConstants &robot_constants) : id_(id), current_state_(position, velocity, orientation, angular_velocity, false), timestamp_(timestamp), @@ -33,7 +33,7 @@ Robot::Robot(RobotId id, const Point &position, const Vector &velocity, Robot::Robot(RobotId id, const RobotState &initial_state, const Timestamp ×tamp, const std::set &unavailable_capabilities, - const RobotConstants_t &robot_constants) + const RobotConstants &robot_constants) : id_(id), current_state_(initial_state), timestamp_(timestamp), @@ -171,7 +171,7 @@ std::set &Robot::getMutableRobotCapabilities() return unavailable_capabilities_; } -const RobotConstants_t &Robot::robotConstants() const +const RobotConstants &Robot::robotConstants() const { return robot_constants_; } diff --git a/src/software/world/robot.h b/src/software/world/robot.h index e6a509591e..a7f4e9b056 100644 --- a/src/software/world/robot.h +++ b/src/software/world/robot.h @@ -35,7 +35,7 @@ class Robot const Timestamp ×tamp, bool breakbeam_tripped = false, const std::set &unavailable_capabilities = std::set(), - const RobotConstants_t &robot_constants = DEFAULT_ROBOT_CONSTANTS); + const RobotConstants &robot_constants = DEFAULT_ROBOT_CONSTANTS); /** * Creates a new robot given robot data @@ -55,7 +55,7 @@ class Robot const Angle &orientation, const AngularVelocity &angular_velocity, const Timestamp ×tamp, const std::set &unavailable_capabilities, - const RobotConstants_t &robot_constants); + const RobotConstants &robot_constants); /** @@ -72,7 +72,7 @@ class Robot const Timestamp ×tamp, const std::set &unavailable_capabilities = std::set(), - const RobotConstants_t &robot_constants = DEFAULT_ROBOT_CONSTANTS); + const RobotConstants &robot_constants = DEFAULT_ROBOT_CONSTANTS); /** @@ -183,7 +183,7 @@ class Robot * * @return the robot constants for this robot */ - const RobotConstants_t &robotConstants() const; + const RobotConstants &robotConstants() const; /** * Decides if a point is near the dribbler of the robot @@ -269,9 +269,9 @@ class Robot // The hardware capabilities of the robot, generated from // RobotCapabilityFlags::broken_dribblers/chippers/kickers dynamic parameters std::set unavailable_capabilities_; - RobotConstants_t robot_constants_; + RobotConstants robot_constants_; // Default robot constants that should be used for all robots inline static const RobotConstants DEFAULT_ROBOT_CONSTANTS = - create2021RobotConstants(); + create2026RobotConstants(); }; diff --git a/src/software/world/robot_state.h b/src/software/world/robot_state.h index 80aa2d73ce..61ff3fb4d9 100644 --- a/src/software/world/robot_state.h +++ b/src/software/world/robot_state.h @@ -1,7 +1,6 @@ #pragma once #include "proto/vision.pb.h" -#include "shared/2021_robot_constants.h" #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/geom/angle.h" diff --git a/src/software/world/robot_test.cpp b/src/software/world/robot_test.cpp index e3bda4b593..4b4021bfcf 100644 --- a/src/software/world/robot_test.cpp +++ b/src/software/world/robot_test.cpp @@ -372,7 +372,7 @@ TEST_F(RobotTest, Angle target_angle = Angle::fromDegrees(-45.0); Robot robot(0, {1, 1}, Vector(0, 0), Angle::fromDegrees(45.0), AngularVelocity::fromDegrees(0), Timestamp::fromSeconds(0), {}, - create2021RobotConstants()); + create2026RobotConstants()); // Figure out a lower bound on the time required, based on us being able to constantly // accelerate at the max acceleration From 6bdc90767122a436a037de694c676dd2b0805ea8 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 16 Mar 2026 13:11:12 -0700 Subject: [PATCH 51/71] Delete robot_auto_test.cpp --- .../stspin_motor_controller.cpp | 6 +++--- .../motor_controller/tmc_motor_controller.cpp | 18 +++++++++--------- .../embedded/services/robot_auto_test.cpp | 0 3 files changed, 12 insertions(+), 12 deletions(-) delete mode 100644 src/software/embedded/services/robot_auto_test.cpp diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 6fd65e3364..2faadf21b2 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -11,9 +11,9 @@ #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/embedded/gpio/gpio_char_dev.h" #include "software/logger/logger.h" // AUTOSAR variant of CRC-8 @@ -21,8 +21,8 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController() - : reset_gpio_( - std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, GpioState::HIGH)) + : reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, + GpioState::HIGH)) { for (const MotorIndex& motor : driveMotors()) { diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index 7d6bb52c0a..288321a2ac 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -1,8 +1,8 @@ #include "software/embedded/motor_controller/tmc_motor_controller.h" #include "shared/constants.h" -#include "software/embedded/spi_utils.h" #include "software/embedded/gpio/gpio_char_dev.h" +#include "software/embedded/spi_utils.h" #include "software/logger/logger.h" extern "C" @@ -39,14 +39,14 @@ extern "C" } 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)) + : 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); 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 e69de29bb2..0000000000 From 3db198aa4bc9ac8f35ad9ecd9c6673779d16f71f Mon Sep 17 00:00:00 2001 From: williamckha Date: Thu, 19 Mar 2026 13:20:46 -0700 Subject: [PATCH 52/71] Add additional response types --- .../stspin_motor_controller.cpp | 41 +++++++++++++++---- .../stspin_motor_controller.h | 8 ++-- .../stspin_motor_controller_test.cpp | 10 ++--- .../embedded/motor_controller/stspin_types.h | 3 ++ src/software/logger/network_logger.cpp | 4 +- 5 files changed, 47 insertions(+), 19 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 2faadf21b2..ba718bc5aa 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -168,7 +168,7 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, sendAndReceiveFrame(motor, outgoing_frame); - return motor_status_.at(motor).measured_speed_rpm; + return motor_status_.at(motor).speed; } void StSpinMotorController::immediatelyDisable() @@ -298,7 +298,7 @@ void StSpinMotorController::processRx(const MotorIndex motor, { case StSpinResponseType::SPEED_AND_FAULTS: { - motor_status_[motor].measured_speed_rpm = + motor_status_[motor].speed = static_cast((static_cast(rx[1]) << 8) | rx[2]); motor_status_[motor].faults = static_cast((static_cast(rx[3]) << 8) | rx[4]); @@ -328,27 +328,50 @@ void StSpinMotorController::processRx(const MotorIndex motor, 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::IQ_AND_ID, StSpinResponseType::SPEED_AND_FAULTS}) + {StSpinResponseType::SPEED_AND_SPEED_REF, StSpinResponseType::IQ_AND_IQ_REF, + StSpinResponseType::ID_AND_ID_REF, StSpinResponseType::SPEED_AND_FAULTS}) { sendAndReceiveFrame(motor, SetResponseTypeFrame{response_type}); } - // Response type changes only take effect *after* we request them, - // so send a no-op frame to receive a response of the last type we requested - sendAndReceiveFrame(motor, NoOpFrame{}); - const MotorStatus& motor_status = motor_status_.at(motor); LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"target_speed_rpm_" + motor, motor_status.target_speed_rpm}, - {"measured_speed_rpm_" + motor, motor_status.measured_speed_rpm}, + {"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 index f6eb3b13ca..d8652684cd 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -52,21 +52,23 @@ class StSpinMotorController : public MotorController std::unordered_map spi_fds_; std::unique_ptr reset_gpio_; - struct MotorStatus { bool enabled; - int16_t target_speed_rpm; - int16_t measured_speed_rpm; uint16_t faults; + 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_; /** diff --git a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp index d293800565..0d8050952d 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -102,6 +102,11 @@ class StSpinMotorControllerTest }; } + for (const MotorIndex motor : args_.enabled_motors) + { + motor_controller->sendAndReceiveFrame(motor, command_frame); + } + auto start_time = std::chrono::system_clock::now(); auto duration = std::chrono::milliseconds(args_.setpoint_duration_ms); while (std::chrono::system_clock::now() - start_time < duration) @@ -111,11 +116,6 @@ class StSpinMotorControllerTest break; } - for (const MotorIndex motor : args_.enabled_motors) - { - motor_controller->sendAndReceiveFrame(motor, command_frame); - } - for (const MotorIndex motor : args_.enabled_motors) { motor_controller->sendMotorStatusToPlotJuggler(motor); diff --git a/src/software/embedded/motor_controller/stspin_types.h b/src/software/embedded/motor_controller/stspin_types.h index 058749a7b7..5c8c036c06 100644 --- a/src/software/embedded/motor_controller/stspin_types.h +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -20,6 +20,9 @@ enum class StSpinResponseType 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 diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index 5eed09cf93..54fc9f13a0 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -21,8 +21,8 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); // Sink for PlotJuggler plotting - auto plotjuggler_handle = logWorker->addSink(std::make_unique(), - &PlotJugglerSink::sendToPlotJuggler); + auto plotjuggler_handle = logWorker->addSink( + std::make_unique("end0"), &PlotJugglerSink::sendToPlotJuggler); g3::only_change_at_initialization::addLogLevel(CSV); g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); From 1985bf9d5fc07e26df96afbeb8ab1c551e0e8048 Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 14 Apr 2026 12:26:06 -0700 Subject: [PATCH 53/71] Fix errors in drive and dribbler widget --- .../drive_and_dribbler_widget.py | 44 ++++++++++--------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 5331f5d301..cd1a2567f6 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -87,27 +87,29 @@ def override_slider_values(self, motor_control: MotorControl) -> None: :param motor_control: the MotorControl values to override the widget's sliders with """ - self.x_velocity_slider.setValue( - motor_control.direct_velocity_control.velocity.x_component_meters - ) - self.y_velocity_slider.setValue( - motor_control.direct_velocity_control.velocity.y_component_meters - ) - self.angular_velocity_slider.setValue( - motor_control.direct_velocity_control.angular_velocity.radians_per_second - ) - self.front_left_motor_slider.setValue( - motor_control.direct_velocity_control.front_left_wheel_velocity - ) - self.front_right_motor_slider.setValue( - motor_control.direct_velocity_control.front_right_wheel_velocity - ) - self.back_left_motor_slider.setValue( - motor_control.direct_velocity_control.back_left_wheel_velocity - ) - self.back_right_motor_slider.setValue( - motor_control.direct_velocity_control.back_right_wheel_velocity - ) + if motor_control.HasField("direct_velocity_control"): + self.x_velocity_slider.setValue( + motor_control.direct_velocity_control.velocity.x_component_meters + ) + self.y_velocity_slider.setValue( + motor_control.direct_velocity_control.velocity.y_component_meters + ) + self.angular_velocity_slider.setValue( + motor_control.direct_velocity_control.angular_velocity.radians_per_second + ) + else: + self.front_left_motor_slider.setValue( + motor_control.direct_per_wheel_control.front_left_wheel_velocity + ) + self.front_right_motor_slider.setValue( + motor_control.direct_per_wheel_control.front_right_wheel_velocity + ) + self.back_left_motor_slider.setValue( + motor_control.direct_per_wheel_control.back_left_wheel_velocity + ) + self.back_right_motor_slider.setValue( + motor_control.direct_per_wheel_control.back_right_wheel_velocity + ) self.dribbler_speed_rpm_slider.setValue(motor_control.dribbler_speed_rpm) From 08350e76ee609c04c1ea6121dcff762c2a59b4c2 Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 14 Apr 2026 12:27:24 -0700 Subject: [PATCH 54/71] Add frame counter --- src/shared/robot_constants.cpp | 4 ++-- .../stspin_motor_controller.cpp | 18 +++++++++++++----- .../motor_controller/stspin_motor_controller.h | 3 ++- src/software/logger/network_logger.cpp | 5 +++-- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp index 05d3242ac0..e3d012a0c0 100644 --- a/src/shared/robot_constants.cpp +++ b/src/shared/robot_constants.cpp @@ -6,8 +6,8 @@ RobotConstants create2026RobotConstants() { return { .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), - .front_wheel_angle_deg = 26.0f, - .back_wheel_angle_deg = 40.0f, + .front_wheel_angle_deg = 32.0f, + .back_wheel_angle_deg = 44.0f, .front_of_robot_width_meters = 0.11f, .dribbler_width_meters = 0.07825f, diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index ba718bc5aa..3a64f37c17 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -43,6 +43,8 @@ void StSpinMotorController::setup() for (const MotorIndex& motor : driveMotors()) { motor_status_[motor].enabled = true; + + sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 806, .ki = 154}); } } @@ -168,6 +170,8 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, sendAndReceiveFrame(motor, outgoing_frame); + sendMotorStatusToPlotJuggler(motor); + return motor_status_.at(motor).speed; } @@ -209,16 +213,20 @@ void StSpinMotorController::sendAndReceiveFrame(const MotorIndex motor, 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(WARNING) << "Received frame that failed integrity check. Expected CRC " + LOG(WARNING) << "Frame #" << motor_status_[motor].frame_count + << " received from motor " << motor + << " failed integrity check. Expected CRC " << static_cast(rx_crc) << " but got " << static_cast(rx[5]) - << " for motor " << motor << ". RX: " << static_cast(rx[0]) - << " " << static_cast(rx[1]) << " " << static_cast(rx[2]) - << " " << static_cast(rx[3]) << " " << static_cast(rx[4]) - << " " << static_cast(rx[5]); + << ". RX: " << static_cast(rx[0]) << " " + << static_cast(rx[1]) << " " << static_cast(rx[2]) << " " + << static_cast(rx[3]) << " " << static_cast(rx[4]) << " " + << static_cast(rx[5]); return; } diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index d8652684cd..2149c65a2b 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -52,8 +52,10 @@ class StSpinMotorController : public MotorController std::unordered_map spi_fds_; std::unique_ptr reset_gpio_; + struct MotorStatus { + unsigned int frame_count; bool enabled; uint16_t faults; int16_t speed; @@ -68,7 +70,6 @@ class StSpinMotorController : public MotorController int16_t phase_voltage; }; - std::unordered_map motor_status_; /** diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index 54fc9f13a0..bfadaf828b 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -21,8 +21,9 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); // Sink for PlotJuggler plotting - auto plotjuggler_handle = logWorker->addSink( - std::make_unique("end0"), &PlotJugglerSink::sendToPlotJuggler); + auto plotjuggler_handle = + logWorker->addSink(std::make_unique("tbotswifi5"), + &PlotJugglerSink::sendToPlotJuggler); g3::only_change_at_initialization::addLogLevel(CSV); g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); From db871f004345e0ad403ddbe5c1a4fbfaeb22a3ac Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 14 Apr 2026 22:57:33 -0700 Subject: [PATCH 55/71] Major refactoring --- .../motor_controller/motor_controller.h | 11 +- .../motor_fault_indicator.cpp | 11 +- .../motor_controller/motor_fault_indicator.h | 6 +- .../embedded/motor_controller/motor_index.h | 2 +- .../stspin_motor_controller.cpp | 105 ++++--- .../stspin_motor_controller.h | 7 +- .../stspin_motor_controller_test.cpp | 2 +- .../motor_controller/tmc_motor_controller.cpp | 154 ++++++---- .../motor_controller/tmc_motor_controller.h | 54 ++-- src/software/embedded/services/motor.cpp | 289 ++++++++---------- src/software/embedded/services/motor.h | 57 +--- .../embedded/services/robot_auto_test.cpp | 0 src/software/logger/network_logger.cpp | 16 +- 13 files changed, 336 insertions(+), 378 deletions(-) delete mode 100644 src/software/embedded/services/robot_auto_test.cpp diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index 28b183320f..134ab59804 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -1,6 +1,5 @@ #pragma once -#include "software/embedded/motor_controller/motor_board.h" #include "software/embedded/motor_controller/motor_controller_status.h" #include "software/embedded/motor_controller/motor_fault_indicator.h" #include "software/embedded/motor_controller/motor_index.h" @@ -16,15 +15,7 @@ class MotorController virtual void reset() = 0; - /** - * 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 - */ - virtual MotorFaultIndicator checkDriverFault(MotorIndex motor) = 0; + virtual const MotorFaultIndicator& checkFaults(MotorIndex motor) = 0; virtual int readThenWriteVelocity(MotorIndex motor, int target_velocity) = 0; diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.cpp b/src/software/embedded/motor_controller/motor_fault_indicator.cpp index 23ed20f50b..0c0f398279 100644 --- a/src/software/embedded/motor_controller/motor_fault_indicator.cpp +++ b/src/software/embedded/motor_controller/motor_fault_indicator.cpp @@ -1,9 +1,14 @@ #include "software/embedded/motor_controller/motor_fault_indicator.h" -MotorFaultIndicator::MotorFaultIndicator() : drive_enabled(true), motor_faults() {} +MotorFaultIndicator::MotorFaultIndicator() : drive_enabled(true) {} MotorFaultIndicator::MotorFaultIndicator( - bool drive_enabled, std::unordered_set& motor_faults) - : drive_enabled(drive_enabled), motor_faults(motor_faults) + 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 index 8747a30070..f2b248ba97 100644 --- a/src/software/embedded/motor_controller/motor_fault_indicator.h +++ b/src/software/embedded/motor_controller/motor_fault_indicator.h @@ -11,7 +11,7 @@ struct MotorFaultIndicator { bool drive_enabled; - std::unordered_set motor_faults; + std::unordered_set faults; /** * Construct a default indicator of no faults and running motors. @@ -26,5 +26,7 @@ struct MotorFaultIndicator * @param motor_faults a set of faults associated with this motor */ MotorFaultIndicator(bool drive_enabled, - std::unordered_set& motor_faults); + const std::unordered_set& motor_faults); + + bool requiresReset() const; }; diff --git a/src/software/embedded/motor_controller/motor_index.h b/src/software/embedded/motor_controller/motor_index.h index 58c0e57ca5..4a58a63d6a 100644 --- a/src/software/embedded/motor_controller/motor_index.h +++ b/src/software/embedded/motor_controller/motor_index.h @@ -14,4 +14,4 @@ constexpr std::array driveMotors() 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 index 3a64f37c17..9865fcd814 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -24,7 +24,7 @@ StSpinMotorController::StSpinMotorController() : reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, GpioState::HIGH)) { - for (const MotorIndex& motor : driveMotors()) + for (const MotorIndex motor : driveMotors()) { openSpiFileDescriptor(motor); } @@ -40,10 +40,14 @@ void StSpinMotorController::setup() { reset(); - for (const MotorIndex& motor : driveMotors()) + 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 = 806, .ki = 154}); } } @@ -56,103 +60,103 @@ void StSpinMotorController::reset() std::this_thread::sleep_for(std::chrono::milliseconds(100)); } -MotorFaultIndicator StSpinMotorController::checkDriverFault(const MotorIndex motor) +const MotorFaultIndicator& StSpinMotorController::checkFaults(const MotorIndex motor) { - bool drive_enabled = true; - std::unordered_set motor_faults; - - if (motor == MotorIndex::DRIBBLER) - { - return {drive_enabled, motor_faults}; - } + return motor_status_.at(motor).faults; +} - const uint16_t faults = motor_status_.at(motor).faults; +void StSpinMotorController::updateFaults(const MotorIndex motor, + const uint16_t fault_flags) +{ + MotorStatus& motor_status = motor_status_.at(motor); - if (faults == 0) + if (motor_status.fault_flags == fault_flags) { - // No faults; early return - return {drive_enabled, motor_faults}; + // No change in faults; early return + return; } + motor_status.fault_flags = fault_flags; + MotorFaultIndicator& motor_faults = motor_status.faults; + motor_faults.faults.clear(); + LOG(WARNING) << "======= Faults For Motor " << motor << "======="; - if (faults & static_cast(StSpinFaultCode::DURATION)) + if (fault_flags & static_cast(StSpinFaultCode::DURATION)) { LOG(WARNING) << "DURATION: FOC rate too high"; - motor_faults.insert(TbotsProto::MotorFault::DURATION); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::DURATION); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::OVER_VOLT)) + if (fault_flags & static_cast(StSpinFaultCode::OVER_VOLT)) { LOG(WARNING) << "OVER_VOLT: Over voltage"; - motor_faults.insert(TbotsProto::MotorFault::OVER_VOLT); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_VOLT); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::UNDER_VOLT)) + if (fault_flags & static_cast(StSpinFaultCode::UNDER_VOLT)) { LOG(WARNING) << "UNDER_VOLT: Under voltage"; - motor_faults.insert(TbotsProto::MotorFault::UNDER_VOLT); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::UNDER_VOLT); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::OVER_TEMP)) + if (fault_flags & static_cast(StSpinFaultCode::OVER_TEMP)) { LOG(WARNING) << "OVER_TEMP: Over temperature"; - motor_faults.insert(TbotsProto::MotorFault::OVER_TEMP); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_TEMP); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::START_UP)) + if (fault_flags & static_cast(StSpinFaultCode::START_UP)) { LOG(WARNING) << "START_UP: Start up failed"; - motor_faults.insert(TbotsProto::MotorFault::START_UP); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::START_UP); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::SPEED_FDBK)) + if (fault_flags & static_cast(StSpinFaultCode::SPEED_FDBK)) { LOG(WARNING) << "SPEED_FDBK: Speed feedback fault"; - motor_faults.insert(TbotsProto::MotorFault::SPEED_FDBK); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::SPEED_FDBK); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::OVER_CURR)) + if (fault_flags & static_cast(StSpinFaultCode::OVER_CURR)) { LOG(WARNING) << "OVER_CURR: Over current"; - motor_faults.insert(TbotsProto::MotorFault::OVER_CURR); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_CURR); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::SW_ERROR)) + if (fault_flags & static_cast(StSpinFaultCode::SW_ERROR)) { LOG(WARNING) << "SW_ERROR: Software error"; - motor_faults.insert(TbotsProto::MotorFault::SW_ERROR); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::SW_ERROR); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::SAMPLE_FAULT)) + if (fault_flags & static_cast(StSpinFaultCode::SAMPLE_FAULT)) { LOG(WARNING) << "SAMPLE_FAULT: Sample fault for testing purposes"; - motor_faults.insert(TbotsProto::MotorFault::SAMPLE_FAULT); + motor_faults.faults.insert(TbotsProto::MotorFault::SAMPLE_FAULT); } - if (faults & static_cast(StSpinFaultCode::OVERCURR_SW)) + if (fault_flags & static_cast(StSpinFaultCode::OVERCURR_SW)) { LOG(INFO) << "OVERCURR_SW: Software over current"; - motor_faults.insert(TbotsProto::MotorFault::OVERCURR_SW); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::OVERCURR_SW); + motor_faults.drive_enabled = false; } - if (faults & static_cast(StSpinFaultCode::DP_FAULT)) + if (fault_flags & static_cast(StSpinFaultCode::DP_FAULT)) { LOG(WARNING) << "DP_FAULT: Driver protection fault"; - motor_faults.insert(TbotsProto::MotorFault::DP_FAULT); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::DP_FAULT); + motor_faults.drive_enabled = false; } - - return {drive_enabled, motor_faults}; } int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, @@ -177,7 +181,7 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, void StSpinMotorController::immediatelyDisable() { - for (const MotorIndex& motor : driveMotors()) + for (const MotorIndex motor : reflective_enum::values()) { motor_status_[motor].enabled = false; readThenWriteVelocity(motor, 0); @@ -308,8 +312,9 @@ void StSpinMotorController::processRx(const MotorIndex motor, { motor_status_[motor].speed = static_cast((static_cast(rx[1]) << 8) | rx[2]); - motor_status_[motor].faults = + const uint16_t fault_flags = static_cast((static_cast(rx[3]) << 8) | rx[4]); + updateFaults(motor, fault_flags); break; } case StSpinResponseType::IQ_AND_ID: diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 2149c65a2b..46b01a3680 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -17,7 +17,7 @@ class StSpinMotorController : public MotorController void reset() override; - MotorFaultIndicator checkDriverFault(MotorIndex motor) override; + const MotorFaultIndicator& checkFaults(MotorIndex motor) override; int readThenWriteVelocity(MotorIndex motor, int target_velocity) override; @@ -57,7 +57,8 @@ class StSpinMotorController : public MotorController { unsigned int frame_count; bool enabled; - uint16_t faults; + MotorFaultIndicator faults; + uint16_t fault_flags; int16_t speed; int16_t speed_ref; int16_t iq; @@ -92,6 +93,8 @@ class StSpinMotorController : public MotorController void processRx(MotorIndex motor, const std::array& rx); + void updateFaults(MotorIndex motor, uint16_t fault_flags); + 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 index 0d8050952d..1882f7b874 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -119,7 +119,7 @@ class StSpinMotorControllerTest for (const MotorIndex motor : args_.enabled_motors) { motor_controller->sendMotorStatusToPlotJuggler(motor); - motor_controller->checkDriverFault(motor); + motor_controller->checkFaults(motor); } std::this_thread::sleep_for(std::chrono::milliseconds(2)); diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index 288321a2ac..f14f4b6d13 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -63,7 +63,7 @@ MotorControllerStatus TmcMotorController::earlyPoll() auto motors = driveMotors(); bool encoders_calibrated = std::accumulate(motors.begin(), motors.end(), false, - [&](const bool& acc, const MotorIndex& motor) + [&](const bool& acc, const MotorIndex motor) { return acc || encoder_calibrated_[motor]; }); if (!encoders_calibrated) @@ -74,7 +74,8 @@ MotorControllerStatus TmcMotorController::earlyPoll() return MotorControllerStatus::OK; } -int TmcMotorController::readThenWriteVelocity(MotorIndex motor, int target_velocity) +int TmcMotorController::readThenWriteVelocity(const MotorIndex motor, + const int target_velocity) { const int velocity_erpm = readThenWriteValue( motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, target_velocity); @@ -87,8 +88,9 @@ int TmcMotorController::readThenWriteVelocity(MotorIndex motor, int target_veloc return velocity_erpm * DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; } -void TmcMotorController::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, - int32_t value) +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; @@ -117,8 +119,9 @@ void TmcMotorController::writeToDriverOrDieTrying(uint8_t motor, uint8_t address << " received: " << read_value; } -void TmcMotorController::writeToControllerOrDieTrying(const MotorIndex& motor, - uint8_t address, int32_t 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; @@ -146,7 +149,13 @@ void TmcMotorController::setup() reset_gpio_->setValue(GpioState::HIGH); usleep(MICROSECONDS_PER_MILLISECOND * 100); - for (const MotorIndex& motor : reflective_enum::values()) + 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); @@ -154,28 +163,28 @@ void TmcMotorController::setup() } // Drive Motor Setup - for (const MotorIndex& motor : driveMotors()) + for (const MotorIndex motor : driveMotors()) { setupDriveMotor(motor); } // Dribbler Motor Setup startDriver(MotorIndex::DRIBBLER); - checkDriverFault(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()) + for (const MotorIndex motor : driveMotors()) { startEncoderCalibration(motor); } sleep(1); - for (const MotorIndex& motor : driveMotors()) + for (const MotorIndex motor : driveMotors()) { endEncoderCalibration(motor); } @@ -183,13 +192,13 @@ void TmcMotorController::setup() auto motors = driveMotors(); bool has_encoders_calibrated = std::accumulate(motors.begin(), motors.end(), false, - [&](const bool& acc, const MotorIndex& motor) + [&](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) +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 @@ -199,7 +208,7 @@ void TmcMotorController::configurePWM(const MotorIndex& motor) writeToControllerOrDieTrying(motor, TMC4671_PWM_SV_CHOP, 0x00000107); } -void TmcMotorController::configureDrivePI(const MotorIndex& motor) +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 @@ -220,7 +229,7 @@ void TmcMotorController::configureDrivePI(const MotorIndex& motor) tmc4671_switchToMotionMode(CHIP_SELECTS.at(motor), TMC4671_MOTION_MODE_VELOCITY); } -void TmcMotorController::configureDribblerPI(const MotorIndex& motor) +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 @@ -240,7 +249,7 @@ void TmcMotorController::configureDribblerPI(const MotorIndex& motor) writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 15000); } -void TmcMotorController::configureADC(const MotorIndex& motor) +void TmcMotorController::configureADC(const MotorIndex motor) { LOG(INFO) << "Configuring ADC for motor " << static_cast(motor); // ADC configuration @@ -260,7 +269,7 @@ void TmcMotorController::configureADC(const MotorIndex& motor) writeToControllerOrDieTrying(motor, TMC4671_ADC_I1_SCALE_OFFSET, 0x0009818E); } -void TmcMotorController::configureEncoder(const MotorIndex& motor) +void TmcMotorController::configureEncoder(const MotorIndex motor) { LOG(INFO) << "Configuring Encoder for motor " << static_cast(motor); // ABN encoder settings @@ -268,7 +277,7 @@ void TmcMotorController::configureEncoder(const MotorIndex& motor) writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PPR, 0x00001000); } -void TmcMotorController::configureHall(const MotorIndex& motor) +void TmcMotorController::configureHall(const MotorIndex motor) { LOG(INFO) << "Configuring Hall for motor " << static_cast(motor); // Digital hall settings @@ -281,13 +290,22 @@ void TmcMotorController::configureHall(const MotorIndex& motor) TMC4671_VELOCITY_PHI_E_HAL); } -MotorFaultIndicator TmcMotorController::checkDriverFault(MotorIndex motor) +const MotorFaultIndicator& TmcMotorController::checkFaults(const MotorIndex motor) { - bool drive_enabled = true; - std::unordered_set motor_faults; + 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(); - int gstat = tmc6100_readInt(CHIP_SELECTS.at(motor), TMC6100_GSTAT); - std::bitset<32> gstat_bitset(gstat); + const int gstat = tmc6100_readInt(CHIP_SELECTS.at(motor), TMC6100_GSTAT); + const std::bitset<32> gstat_bitset(gstat); if (gstat_bitset.any()) { @@ -299,7 +317,7 @@ MotorFaultIndicator TmcMotorController::checkDriverFault(MotorIndex motor) 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); + motor_faults.faults.insert(TbotsProto::MotorFault::RESET); } if (gstat_bitset[1]) @@ -307,7 +325,8 @@ MotorFaultIndicator TmcMotorController::checkDriverFault(MotorIndex motor) 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); + motor_faults.faults.insert( + TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE_PREWARNING); } if (gstat_bitset[2]) @@ -316,7 +335,7 @@ MotorFaultIndicator TmcMotorController::checkDriverFault(MotorIndex motor) << "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); + motor_faults.faults.insert(TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE); } if (gstat_bitset[3]) @@ -324,85 +343,87 @@ MotorFaultIndicator TmcMotorController::checkDriverFault(MotorIndex motor) 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; + 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.insert(TbotsProto::MotorFault::PHASE_U_SHORT_COUNTER_DETECTED); - drive_enabled = false; + 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.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_GND_DETECTED); - drive_enabled = false; + 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.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_VS_DETECTED); - drive_enabled = false; + 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.insert(TbotsProto::MotorFault::PHASE_V_SHORT_COUNTER_DETECTED); + 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.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_GND_DETECTED); - drive_enabled = false; + 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.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_VS_DETECTED); - drive_enabled = false; + 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.insert(TbotsProto::MotorFault::PHASE_W_SHORT_COUNTER_DETECTED); + 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.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_GND_DETECTED); - drive_enabled = false; + 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.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_VS_DETECTED); - drive_enabled = false; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_VS_DETECTED); + motor_faults.drive_enabled = false; } - return MotorFaultIndicator(drive_enabled, motor_faults); + return motor_faults; } -int TmcMotorController::readThenWriteValue(const MotorIndex& motor, - const uint8_t& read_addr, - const uint8_t& write_addr, - const int& write_data) +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); @@ -443,7 +464,7 @@ int TmcMotorController::readThenWriteValue(const MotorIndex& motor, return value; } -void TmcMotorController::openSpiFileDescriptor(const MotorIndex& motor_index) +void TmcMotorController::openSpiFileDescriptor(const MotorIndex motor_index) { file_descriptors_[CHIP_SELECTS.at(motor_index)] = open(SPI_PATHS.at(motor_index), O_RDWR); @@ -466,16 +487,16 @@ void TmcMotorController::openSpiFileDescriptor(const MotorIndex& motor_index) << "error: " << strerror(errno); } -void TmcMotorController::setupDriveMotor(const MotorIndex& motor) +void TmcMotorController::setupDriveMotor(const MotorIndex motor) { startDriver(motor); - checkDriverFault(motor); + checkFaults(motor); // Start all the controllers as drive motor controllers startController(motor, false); tmc4671_setTargetVelocity(CHIP_SELECTS.at(motor), 0); } -void TmcMotorController::startDriver(MotorIndex motor) +void TmcMotorController::startDriver(const MotorIndex motor) { uint8_t motor_cs = CHIP_SELECTS.at(motor); @@ -493,7 +514,7 @@ void TmcMotorController::startDriver(MotorIndex motor) LOG(DEBUG) << "Driver " << motor << " accepted conf"; } -void TmcMotorController::startController(MotorIndex motor, bool dribbler) +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); @@ -526,24 +547,25 @@ void TmcMotorController::startController(MotorIndex motor, bool dribbler) } } -uint8_t TmcMotorController::tmc4671ReadWriteByte(uint8_t motor, uint8_t data, - uint8_t last_transfer) +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(uint8_t motor, uint8_t data, - uint8_t last_transfer) +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(uint8_t motor, uint8_t data, - uint8_t last_transfer, uint32_t 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; @@ -611,7 +633,7 @@ void TmcMotorController::checkEncoderConnections() std::unordered_map calibrated_motors; std::unordered_map initial_velocities; - for (const MotorIndex& motor : driveMotors()) + for (const MotorIndex motor : driveMotors()) { calibrated_motors[motor] = false; @@ -643,7 +665,7 @@ void TmcMotorController::checkEncoderConnections() { return !calibration_status_pair.second; }); ++num_iterations) { - for (const MotorIndex& motor : driveMotors()) + for (const MotorIndex motor : driveMotors()) { if (calibrated_motors[motor]) { @@ -666,7 +688,7 @@ void TmcMotorController::checkEncoderConnections() } bool calibrated = true; - for (const MotorIndex& motor : driveMotors()) + for (const MotorIndex motor : driveMotors()) { if (!calibrated_motors[motor]) { @@ -682,7 +704,7 @@ void TmcMotorController::checkEncoderConnections() } // stop all motors, reset back to velocity control mode - for (const MotorIndex& motor : driveMotors()) + for (const MotorIndex motor : driveMotors()) { writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x00000000); tmc4671_switchToMotionMode(CHIP_SELECTS.at(motor), TMC4671_MOTION_MODE_VELOCITY); @@ -696,7 +718,7 @@ void TmcMotorController::reset() reset_gpio_->setValue(GpioState::LOW); } -void TmcMotorController::startEncoderCalibration(const MotorIndex& motor) +void TmcMotorController::startEncoderCalibration(const MotorIndex motor) { LOG(WARNING) << "Calibrating the encoder, ensure the robot is lifted off the ground"; @@ -712,7 +734,7 @@ void TmcMotorController::startEncoderCalibration(const MotorIndex& motor) writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000FFF); } -void TmcMotorController::endEncoderCalibration(const MotorIndex& motor) +void TmcMotorController::endEncoderCalibration(const MotorIndex motor) { LOG(WARNING) << "Calibrating the encoder, wheels may move"; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index df08806d9f..3f103baa6e 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -16,7 +16,7 @@ class TmcMotorController : public MotorController void reset() override; - MotorFaultIndicator checkDriverFault(MotorIndex motor) override; + const MotorFaultIndicator& checkFaults(MotorIndex motor) override; void immediatelyDisable() override; @@ -75,7 +75,7 @@ class TmcMotorController : public MotorController * * @param motor_index The index of the motor to open */ - void openSpiFileDescriptor(const MotorIndex& motor_name); + void openSpiFileDescriptor(MotorIndex motor_name); /** * A function which is written in the same style as the rest of the Trinamic API. @@ -88,8 +88,8 @@ class TmcMotorController : public MotorController * @param write_data the data to write * @return the value read from the trinamic controller */ - int readThenWriteValue(const MotorIndex& motor, const uint8_t& read_addr, - const uint8_t& write_addr, const int& write_data); + 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 @@ -104,8 +104,7 @@ class TmcMotorController : public MotorController * @param value The value to write * */ - void writeToControllerOrDieTrying(const MotorIndex& motor, uint8_t address, - int32_t value); + void writeToControllerOrDieTrying(MotorIndex motor, uint8_t address, int32_t value); void writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value); /** @@ -113,7 +112,7 @@ class TmcMotorController : public MotorController * * @param motor drive motor number */ - void setupDriveMotor(const MotorIndex& motor); + void setupDriveMotor(MotorIndex motor); /** * Calls the configuration functions below in the right sequence @@ -142,12 +141,12 @@ class TmcMotorController : public MotorController * * @param motor The motor to configure (the same value as the chip select) */ - void configurePWM(const MotorIndex& motor); - void configureDribblerPI(const MotorIndex& motor); - void configureDrivePI(const MotorIndex& motor); - void configureADC(const MotorIndex& motor); - void configureEncoder(const MotorIndex& motor); - void configureHall(const MotorIndex& motor); + 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 @@ -176,8 +175,8 @@ class TmcMotorController : public MotorController * * @param motor The motor to initialize the encoder for */ - void startEncoderCalibration(const MotorIndex& motor); - void endEncoderCalibration(const MotorIndex& motor); + void startEncoderCalibration(MotorIndex motor); + void endEncoderCalibration(MotorIndex motor); /** * Spin each drive motor in open loop mode to check if the encoder @@ -197,13 +196,13 @@ class TmcMotorController : public MotorController std::unique_ptr reset_gpio_; // Transfer Buffers for spiTransfer - uint8_t tx_[5] = {0}; - uint8_t rx_[5] = {0}; + uint8_t tx_[5] = {}; + uint8_t rx_[5] = {}; // Transfer Buffers for readThenWriteSpiTransfer - uint8_t write_tx_[5] = {0}; - uint8_t read_tx_[5] = {0}; - uint8_t read_rx_[5] = {0}; + uint8_t write_tx_[5] = {}; + uint8_t read_tx_[5] = {}; + uint8_t read_rx_[5] = {}; // Transfer State bool transfer_started_ = false; @@ -214,6 +213,17 @@ class TmcMotorController : public MotorController // 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; @@ -264,9 +274,9 @@ class TmcMotorController : public MotorController // // RPM = eRPM / # of pole pairs static constexpr double DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM = - 1 / DRIVE_MOTOR_NUM_POLE_PAIRS; + 1.0 / DRIVE_MOTOR_NUM_POLE_PAIRS; static constexpr double DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM = - 1 / DRIBBLER_MOTOR_NUM_POLE_PAIRS; + 1.0 / DRIBBLER_MOTOR_NUM_POLE_PAIRS; static constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = 16; static constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO = 19; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 9d7e04929c..fff4fee3d3 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -6,64 +6,21 @@ #include "software/embedded/motor_controller/tmc_motor_controller.h" #include "software/logger/logger.h" -static double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; -static int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; - MotorService::MotorService(const RobotConstants& robot_constants) : motor_controller_(setupMotorController()), robot_constants_(robot_constants), euclidean_to_four_wheel_(robot_constants), - drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60), - tracked_motor_fault_start_time_(std::nullopt) + drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60) { } void MotorService::setup() { - for (const MotorIndex& motor : reflective_enum::values()) - { - cached_motor_faults_[motor] = MotorFaultIndicator(); - } - - 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_ = 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}; - front_left_target_rpm = 0; - front_right_target_rpm = 0; - back_left_target_rpm = 0; - back_right_target_rpm = 0; - dribbler_target_rpm_ = 0; + prev_wheel_velocities_ = WheelSpace_t::Zero(); + target_wheel_velocities_ = WheelSpace_t::Zero(); + dribbler_target_rpm_ = 0; motor_controller_->setup(); - - is_initialized_ = true; } void MotorService::reset() @@ -83,26 +40,21 @@ std::unique_ptr MotorService::setupMotorController() } } -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_] = - motor_controller_->checkDriverFault(motor_fault_detector_); - - for (const MotorIndex& motor : reflective_enum::values()) + for (const MotorIndex motor : reflective_enum::values()) { + 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); } @@ -128,9 +80,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); } @@ -140,17 +92,13 @@ 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((static_cast(motor_fault_detector_) + 1) % - reflective_enum::size()); + static_cast(current_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX])); return motor_status; } @@ -158,83 +106,68 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor, double time_elapsed_since_last_poll_s) { - if (motor_controller_->earlyPoll() != MotorControllerStatus::OK) + if (motor_controller_->earlyPoll() != MotorControllerStatus::OK || + anyMotorRequiresReset()) { - is_initialized_ = false; + LOG(INFO) << "MotorService resetting"; + trackMotorReset(); + setup(); } - // checks if any motor has reset, sends a log message if so - for (const MotorIndex& motor_index : reflective_enum::values()) - { - if (requiresMotorReinit(motor_index)) - { - LOG(DEBUG) << "RESET DETECTED FOR MOTOR: " << motor_index; - 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]), + }; + + const double dribbler_rpm = motor_controller_->readThenWriteVelocity( + MotorIndex::DRIBBLER, dribbler_target_rpm_); - double front_right_velocity = motor_controller_->readThenWriteVelocity( - MotorIndex::FRONT_RIGHT, front_right_target_rpm) * - drive_motor_mps_per_rpm_; - double front_left_velocity = motor_controller_->readThenWriteVelocity( - MotorIndex::FRONT_LEFT, front_left_target_rpm) * - drive_motor_mps_per_rpm_; - double back_right_velocity = motor_controller_->readThenWriteVelocity( - MotorIndex::BACK_RIGHT, back_right_target_rpm) * - drive_motor_mps_per_rpm_; - double back_left_velocity = motor_controller_->readThenWriteVelocity( - MotorIndex::BACK_LEFT, back_left_target_rpm) * - drive_motor_mps_per_rpm_; - double dribbler_rpm = motor_controller_->readThenWriteVelocity(MotorIndex::DRIBBLER, - dribbler_target_rpm_); + const WheelSpace_t current_wheel_velocities = + current_wheel_rpms.cast() * drive_motor_mps_per_rpm_; TbotsProto::MotorStatus motor_status = - updateMotorStatus(front_left_velocity, front_right_velocity, back_left_velocity, - back_right_velocity, 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 RPMs. - WheelSpace_t current_wheel_velocities = {front_right_velocity, front_left_velocity, - back_left_velocity, back_right_velocity}; - - // if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - - // prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > - // RUNAWAY_PROTECTION_THRESHOLD_MPS) - // { - // 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) - // { - // 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) - // { - // 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) - // { - // motor_controller_->immediatelyDisable(); - // LOG(FATAL) << "Back right motor runaway"; - // } - (void)RUNAWAY_PROTECTION_THRESHOLD_MPS; + createMotorStatus(current_wheel_velocities, dribbler_rpm); + + if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - + prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > + RUNAWAY_PROTECTION_THRESHOLD_MPS) + { + 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) + { + 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) + { + 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) + { + 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( @@ -244,14 +177,12 @@ 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()) { - TbotsProto::MotorControl_DirectPerWheelControl direct_per_wheel = - motor.direct_per_wheel_control(); - target_wheel_velocities = { + const auto& direct_per_wheel = motor.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(), @@ -260,34 +191,23 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor } else if (motor.has_direct_velocity_control()) { - TbotsProto::MotorControl_DirectVelocityControl direct_velocity = - motor.direct_velocity_control(); - EuclideanSpace_t target_euclidean_velocity = { + const auto& direct_velocity = motor.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 = + target_wheel_velocities_ = euclidean_to_four_wheel_.getWheelVelocity(target_euclidean_velocity); } // 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); - - prev_wheel_velocities_ = target_wheel_velocities; - - // Calculate speeds accounting for acceleration - front_right_target_rpm = - static_cast(target_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] / - drive_motor_mps_per_rpm_); - front_left_target_rpm = static_cast( - target_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] / drive_motor_mps_per_rpm_); - back_left_target_rpm = static_cast( - target_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] / drive_motor_mps_per_rpm_); - back_right_target_rpm = static_cast( - target_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] / drive_motor_mps_per_rpm_); + 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_; // Get target dribbler rpm from the primitive int target_dribbler_rpm; @@ -314,16 +234,49 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor dribbler_target_rpm_ = std::clamp(dribbler_target_rpm_, -max_dribbler_rpm, max_dribbler_rpm); - motor_status.mutable_dribbler()->set_dribbler_rpm(dribbler_target_rpm_); + motor_status.mutable_dribbler()->set_dribbler_rpm( + static_cast(dribbler_target_rpm_)); return motor_status; } -bool MotorService::requiresMotorReinit(const MotorIndex& motor) +void MotorService::trackMotorReset() { - auto reset_search = - cached_motor_faults_[motor].motor_faults.find(TbotsProto::MotorFault::RESET); + const auto now = std::chrono::system_clock::now(); - return !cached_motor_faults_[motor].drive_enabled || - (reset_search != cached_motor_faults_[motor].motor_faults.end()); + if (num_tracked_motor_resets_ == 0) + { + tracked_motor_fault_start_time_ = now; + num_tracked_motor_resets_ = 1; + return; + } + + const auto elapsed_s = std::chrono::duration_cast( + now - tracked_motor_fault_start_time_) + .count(); + + if (elapsed_s < MOTOR_FAULT_TIME_THRESHOLD_S) + { + num_tracked_motor_resets_++; + } + else + { + tracked_motor_fault_start_time_ = now; + num_tracked_motor_resets_ = 1; + } + + if (num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) + { + LOG(FATAL) << "Motor board reset too frequently (" << num_tracked_motor_resets_ + << " times in " << elapsed_s + << " seconds). Thunderloop crashing for safety."; + } +} + +bool MotorService::anyMotorRequiresReset() const +{ + return std::any_of(reflective_enum::values().begin(), + reflective_enum::values().end(), + [this](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 2a7cb6feec..48af2b33a7 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -1,16 +1,12 @@ #pragma once -#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/gpio/gpio.h" #include "software/embedded/motor_controller/motor_controller.h" -#include "software/embedded/motor_controller/motor_fault_indicator.h" #include "software/physics/euclidean_to_wheel.h" /** @@ -30,7 +26,7 @@ class MotorService * * @param robot_constants The robot constants */ - MotorService(const RobotConstants& robot_constants); + explicit MotorService(const RobotConstants& robot_constants); virtual ~MotorService() = default; @@ -64,62 +60,37 @@ class MotorService * 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_velocity_mps the front left motor's velocity in m/s - * @param front_right_velocity_mps the front right motor's velocity in m/s - * @param back_left_velocity_mps the back left motor's velocity in m/s - * @param back_right_velocity_mps the back right motor's velocity in m/s + * @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; - /** - * Returns true if we've detected a RESET in our cached motor faults indicators or if - * we have a fault that disables drive. - * - * @param motor chip select to check for RESETs - * - * @return true if the motor has returned a cached RESET fault, false otherwise - */ - bool requiresMotorReinit(const MotorIndex& motor); + void trackMotorReset(); - // Controller for communicating with the motor driver board(s) - std::unique_ptr motor_controller_; + bool anyMotorRequiresReset() const; - // Flag indicating whether the motors have been calibrated - bool is_initialized_ = false; + std::unique_ptr motor_controller_; RobotConstants robot_constants_; EuclideanToWheel euclidean_to_four_wheel_; 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; - int dribbler_target_rpm_ = 0; + int dribbler_target_rpm_ = 0; double drive_motor_mps_per_rpm_; - // The current motor to check for motor faults; this is updated every poll - // and will cycle through all the MotorIndex values (only one motor is checked - // for faults each poll to reduce number of SPI transactions) - MotorIndex motor_fault_detector_; - - std::unordered_map cached_motor_faults_; - - std::optional> - tracked_motor_fault_start_time_; + std::chrono::time_point tracked_motor_fault_start_time_; int num_tracked_motor_resets_; - static constexpr int MOTOR_FAULT_TIME_THRESHOLD_S = 60; - static constexpr int MOTOR_FAULT_THRESHOLD_COUNT = 3; + static constexpr int MOTOR_FAULT_TIME_THRESHOLD_S = 60; + static constexpr int MOTOR_FAULT_THRESHOLD_COUNT = 3; + static constexpr double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; + static constexpr int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; }; 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 e69de29bb2..0000000000 diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index bfadaf828b..4620e14393 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -13,17 +13,13 @@ 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("tbotswifi5"), - &PlotJugglerSink::sendToPlotJuggler); + logWorker->addSink(std::make_unique("tbotswifi5"), + &PlotJugglerSink::sendToPlotJuggler); g3::only_change_at_initialization::addLogLevel(CSV); g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); @@ -35,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)); } } From 98d8fa8fe10a94d2f4c4637d6540a833756972b9 Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 14 Apr 2026 23:16:29 -0700 Subject: [PATCH 56/71] Minor cleanup --- .../stspin_motor_controller.cpp | 20 ++++++++++++------- src/software/embedded/services/motor.cpp | 16 +++++++-------- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 9865fcd814..5bda1c83c1 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #pragma GCC diagnostic push @@ -223,14 +224,19 @@ void StSpinMotorController::sendAndReceiveFrame(const MotorIndex motor, 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 (size_t i = 0; i < FRAME_LEN; ++i) + { + oss << "0x" << std::setw(2) << static_cast(rx[i]) << " "; + } + LOG(WARNING) << "Frame #" << motor_status_[motor].frame_count - << " received from motor " << motor - << " failed integrity check. Expected CRC " - << static_cast(rx_crc) << " but got " << static_cast(rx[5]) - << ". RX: " << static_cast(rx[0]) << " " - << static_cast(rx[1]) << " " << static_cast(rx[2]) << " " - << static_cast(rx[3]) << " " << static_cast(rx[4]) << " " - << static_cast(rx[5]); + << " received from motor " << motor << " failed integrity check. " + << oss.str(); return; } diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index fff4fee3d3..ad93895509 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -103,8 +103,8 @@ TbotsProto::MotorStatus MotorService::createMotorStatus( 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) { if (motor_controller_->earlyPoll() != MotorControllerStatus::OK || anyMotorRequiresReset()) @@ -178,9 +178,9 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor current_euclidean_velocity[2]); // Get target wheel velocities from the primitive - if (motor.has_direct_per_wheel_control()) + if (motor_control.has_direct_per_wheel_control()) { - const auto& direct_per_wheel = motor.direct_per_wheel_control(); + const auto& direct_per_wheel = motor_control.direct_per_wheel_control(); target_wheel_velocities_ = { direct_per_wheel.front_right_wheel_velocity(), @@ -189,9 +189,9 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor direct_per_wheel.back_right_wheel_velocity(), }; } - else if (motor.has_direct_velocity_control()) + else if (motor_control.has_direct_velocity_control()) { - const auto& direct_velocity = motor.direct_velocity_control(); + const auto& direct_velocity = motor_control.direct_velocity_control(); const EuclideanSpace_t target_euclidean_velocity = { -direct_velocity.velocity().y_component_meters(), @@ -211,14 +211,14 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor // 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 From e0c2eb29f062cfdc57955e996db7feb363e13645 Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 14 Apr 2026 23:48:58 -0700 Subject: [PATCH 57/71] Remove unnecessary earlyPoll method --- src/software/embedded/motor_controller/BUILD | 6 --- .../motor_controller/motor_controller.h | 3 -- .../motor_controller_status.h | 3 -- .../stspin_motor_controller.cpp | 40 ++++++++++--------- .../stspin_motor_controller.h | 2 - .../motor_controller/tmc_motor_controller.cpp | 18 +-------- .../motor_controller/tmc_motor_controller.h | 2 - src/software/embedded/services/motor.cpp | 7 ++-- 8 files changed, 26 insertions(+), 55 deletions(-) delete mode 100644 src/software/embedded/motor_controller/motor_controller_status.h diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index d5e7c99ebf..248afd638f 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -1,10 +1,5 @@ package(default_visibility = ["//visibility:public"]) -cc_library( - name = "motor_controller_status", - hdrs = ["motor_controller_status.h"], -) - cc_library( name = "motor_index", hdrs = ["motor_index.h"], @@ -34,7 +29,6 @@ cc_library( ], deps = [ ":motor_board", - ":motor_controller_status", ":motor_fault_indicator", ":motor_index", ":stspin_types", diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index 134ab59804..9167a13b0b 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -1,6 +1,5 @@ #pragma once -#include "software/embedded/motor_controller/motor_controller_status.h" #include "software/embedded/motor_controller/motor_fault_indicator.h" #include "software/embedded/motor_controller/motor_index.h" @@ -9,8 +8,6 @@ class MotorController public: virtual ~MotorController() = default; - virtual MotorControllerStatus earlyPoll() = 0; - virtual void setup() = 0; virtual void reset() = 0; diff --git a/src/software/embedded/motor_controller/motor_controller_status.h b/src/software/embedded/motor_controller/motor_controller_status.h deleted file mode 100644 index 25c970e325..0000000000 --- a/src/software/embedded/motor_controller/motor_controller_status.h +++ /dev/null @@ -1,3 +0,0 @@ -#pragma once - -MAKE_ENUM(MotorControllerStatus, OK, CALIBRATION_FAILURE); diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 5bda1c83c1..224eb7af1d 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -31,12 +31,6 @@ StSpinMotorController::StSpinMotorController() } } -MotorControllerStatus StSpinMotorController::earlyPoll() -{ - // No encoders to calibrate, so just return OK - return MotorControllerStatus::OK; -} - void StSpinMotorController::setup() { reset(); @@ -79,85 +73,95 @@ void StSpinMotorController::updateFaults(const MotorIndex motor, motor_status.fault_flags = fault_flags; MotorFaultIndicator& motor_faults = motor_status.faults; + motor_faults.drive_enabled = true; motor_faults.faults.clear(); - LOG(WARNING) << "======= Faults For Motor " << motor << "======="; + if (fault_flags == 0) + { + // No faults; early return + return; + } + + std::ostringstream oss; + oss << "======= Faults For Motor " << motor << "=======\n"; if (fault_flags & static_cast(StSpinFaultCode::DURATION)) { - LOG(WARNING) << "DURATION: FOC rate too high"; + 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)) { - LOG(WARNING) << "OVER_VOLT: Over voltage"; + 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)) { - LOG(WARNING) << "UNDER_VOLT: Under voltage"; + 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)) { - LOG(WARNING) << "OVER_TEMP: Over temperature"; + 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)) { - LOG(WARNING) << "START_UP: Start up failed"; + 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)) { - LOG(WARNING) << "SPEED_FDBK: Speed feedback fault"; + 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)) { - LOG(WARNING) << "OVER_CURR: Over current"; + 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)) { - LOG(WARNING) << "SW_ERROR: Software 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)) { - LOG(WARNING) << "SAMPLE_FAULT: Sample fault for testing purposes"; + 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)) { - LOG(INFO) << "OVERCURR_SW: Software over current"; + 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)) { - LOG(WARNING) << "DP_FAULT: Driver protection 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, diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 46b01a3680..d1b0d475f2 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -11,8 +11,6 @@ class StSpinMotorController : public MotorController public: explicit StSpinMotorController(); - MotorControllerStatus earlyPoll() override; - void setup() override; void reset() override; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index f14f4b6d13..d9c8a0b3f1 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -25,7 +25,7 @@ extern "C" // // 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 = NULL; + static TmcMotorController* g_motor_controller = nullptr; uint8_t tmc4671_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) { @@ -58,22 +58,6 @@ TmcMotorController::TmcMotorController() g_motor_controller = this; } -MotorControllerStatus TmcMotorController::earlyPoll() -{ - auto motors = driveMotors(); - bool encoders_calibrated = - std::accumulate(motors.begin(), motors.end(), false, - [&](const bool& acc, const MotorIndex motor) - { return acc || encoder_calibrated_[motor]; }); - - if (!encoders_calibrated) - { - return MotorControllerStatus::CALIBRATION_FAILURE; - } - - return MotorControllerStatus::OK; -} - int TmcMotorController::readThenWriteVelocity(const MotorIndex motor, const int target_velocity) { diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index 3f103baa6e..cae8d31c16 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -10,8 +10,6 @@ class TmcMotorController : public MotorController public: TmcMotorController(); - MotorControllerStatus earlyPoll() override; - void setup() override; void reset() override; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index ad93895509..46db9f3578 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -106,10 +106,9 @@ TbotsProto::MotorStatus MotorService::createMotorStatus( TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor_control, const double time_elapsed_since_last_poll_s) { - if (motor_controller_->earlyPoll() != MotorControllerStatus::OK || - anyMotorRequiresReset()) + if (anyMotorRequiresReset()) { - LOG(INFO) << "MotorService resetting"; + LOG(INFO) << "Resetting motors due to fault indicators requiring reset"; trackMotorReset(); setup(); } @@ -267,7 +266,7 @@ void MotorService::trackMotorReset() if (num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) { - LOG(FATAL) << "Motor board reset too frequently (" << num_tracked_motor_resets_ + LOG(FATAL) << "Motors reset too frequently (" << num_tracked_motor_resets_ << " times in " << elapsed_s << " seconds). Thunderloop crashing for safety."; } From f0d7f3ff6bb6bda3ccaa429b071d14724411a3e5 Mon Sep 17 00:00:00 2001 From: williamckha Date: Thu, 16 Apr 2026 10:54:36 -0700 Subject: [PATCH 58/71] Minor renamings --- .../motor_controller/motor_fault_indicator.cpp | 2 +- src/software/embedded/services/motor.cpp | 10 +++++----- src/software/embedded/services/motor.h | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.cpp b/src/software/embedded/motor_controller/motor_fault_indicator.cpp index 0c0f398279..75d98bba69 100644 --- a/src/software/embedded/motor_controller/motor_fault_indicator.cpp +++ b/src/software/embedded/motor_controller/motor_fault_indicator.cpp @@ -3,7 +3,7 @@ MotorFaultIndicator::MotorFaultIndicator() : drive_enabled(true) {} MotorFaultIndicator::MotorFaultIndicator( - bool drive_enabled, const std::unordered_set& motor_faults) + const bool drive_enabled, const std::unordered_set& motor_faults) : drive_enabled(drive_enabled), faults(motor_faults) { } diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 46db9f3578..b584a2890f 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -245,26 +245,26 @@ void MotorService::trackMotorReset() if (num_tracked_motor_resets_ == 0) { - tracked_motor_fault_start_time_ = now; + tracked_motor_reset_start_time_ = now; num_tracked_motor_resets_ = 1; return; } const auto elapsed_s = std::chrono::duration_cast( - now - tracked_motor_fault_start_time_) + now - tracked_motor_reset_start_time_) .count(); - if (elapsed_s < MOTOR_FAULT_TIME_THRESHOLD_S) + if (elapsed_s < MOTOR_RESET_TIME_THRESHOLD_S) { num_tracked_motor_resets_++; } else { - tracked_motor_fault_start_time_ = now; + tracked_motor_reset_start_time_ = now; num_tracked_motor_resets_ = 1; } - if (num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) + if (num_tracked_motor_resets_ > MOTOR_RESET_THRESHOLD_COUNT) { LOG(FATAL) << "Motors reset too frequently (" << num_tracked_motor_resets_ << " times in " << elapsed_s diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 48af2b33a7..292a6b52a3 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -86,11 +86,11 @@ class MotorService double drive_motor_mps_per_rpm_; - std::chrono::time_point tracked_motor_fault_start_time_; + std::chrono::time_point tracked_motor_reset_start_time_; int num_tracked_motor_resets_; - static constexpr int MOTOR_FAULT_TIME_THRESHOLD_S = 60; - static constexpr int MOTOR_FAULT_THRESHOLD_COUNT = 3; + 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; }; From d74a4933aeb8a1a46cbedf8551a3063a4387dfb3 Mon Sep 17 00:00:00 2001 From: williamckha Date: Thu, 16 Apr 2026 15:13:24 -0700 Subject: [PATCH 59/71] Fix issue with reflective_enum::values --- .../embedded/motor_controller/stspin_motor_controller.h | 2 +- src/software/embedded/services/motor.cpp | 2 +- src/software/util/make_enum/make_enum.hpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index d1b0d475f2..5c31d67e95 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -33,9 +33,9 @@ class StSpinMotorController : public MotorController // 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"}, - {MotorIndex::FRONT_RIGHT, "/dev/spidev0.4"}, }; // clang-format on diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index b584a2890f..5b392c1437 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -276,6 +276,6 @@ bool MotorService::anyMotorRequiresReset() const { return std::any_of(reflective_enum::values().begin(), reflective_enum::values().end(), - [this](const MotorIndex motor) + [&](const MotorIndex motor) { return motor_controller_->checkFaults(motor).requiresReset(); }); } diff --git a/src/software/util/make_enum/make_enum.hpp b/src/software/util/make_enum/make_enum.hpp index e5c29f8017..abb1cea245 100644 --- a/src/software/util/make_enum/make_enum.hpp +++ b/src/software/util/make_enum/make_enum.hpp @@ -50,12 +50,12 @@ 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; \ } \ @@ -98,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 @@ -107,7 +107,7 @@ 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. From 278f1c70fd2f73f5c92c94c54dc62a9286007204 Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 17 Apr 2026 19:52:39 -0700 Subject: [PATCH 60/71] Add frame type for setting speed feedforward gains --- .../motor_fault_indicator.cpp | 3 ++- .../stspin_motor_controller.cpp | 10 +++++---- .../stspin_motor_controller.h | 2 +- .../embedded/motor_controller/stspin_types.h | 21 ++++++++++--------- src/software/util/make_enum/make_enum.hpp | 4 ++-- 5 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.cpp b/src/software/embedded/motor_controller/motor_fault_indicator.cpp index 75d98bba69..366710838b 100644 --- a/src/software/embedded/motor_controller/motor_fault_indicator.cpp +++ b/src/software/embedded/motor_controller/motor_fault_indicator.cpp @@ -3,7 +3,8 @@ MotorFaultIndicator::MotorFaultIndicator() : drive_enabled(true) {} MotorFaultIndicator::MotorFaultIndicator( - const bool drive_enabled, const std::unordered_set& motor_faults) + const bool drive_enabled, + const std::unordered_set& motor_faults) : drive_enabled(drive_enabled), faults(motor_faults) { } diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 224eb7af1d..bd1117d525 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -301,11 +301,13 @@ void StSpinMotorController::populateTx(const OutgoingFrame& outgoing_frame, tx[3] = static_cast(0xFF & (frame.ki >> 8)); tx[4] = static_cast(0xFF & frame.ki); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_PID_SPEED_KD); - tx[1] = static_cast(0xFF & (frame.kd >> 8)); - tx[2] = static_cast(0xFF & frame.kd); + 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); } }, outgoing_frame); diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 5c31d67e95..fbe635fdd1 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -25,7 +25,7 @@ class StSpinMotorController : public MotorController using OutgoingFrame = std::variant; + SetPidSpeedKpKiFrame, SetSpeedFeedForwardKaKvFrame>; // Length of frame (in number of bytes) static constexpr unsigned int FRAME_LEN = 6; diff --git a/src/software/embedded/motor_controller/stspin_types.h b/src/software/embedded/motor_controller/stspin_types.h index 5c8c036c06..f0e3c39b2c 100644 --- a/src/software/embedded/motor_controller/stspin_types.h +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -4,14 +4,14 @@ 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_PID_SPEED_KD = 0x07, + 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, }; enum class StSpinResponseType @@ -80,7 +80,8 @@ struct SetPidSpeedKpKiFrame int16_t ki; }; -struct SetPidSpeedKdFrame +struct SetSpeedFeedForwardKaKvFrame { - int16_t kd; + int16_t ka; + int16_t kv; }; diff --git a/src/software/util/make_enum/make_enum.hpp b/src/software/util/make_enum/make_enum.hpp index abb1cea245..eeccb9f970 100644 --- a/src/software/util/make_enum/make_enum.hpp +++ b/src/software/util/make_enum/make_enum.hpp @@ -50,12 +50,12 @@ 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; \ } \ From 92ceb8d914a6cbd4f97b70d15d1a572d3dabcadb Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 17 Apr 2026 20:34:41 -0700 Subject: [PATCH 61/71] Lower max acceleration --- src/shared/robot_constants.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp index e3d012a0c0..0e2742a8da 100644 --- a/src/shared/robot_constants.cpp +++ b/src/shared/robot_constants.cpp @@ -17,10 +17,10 @@ RobotConstants create2026RobotConstants() .max_force_dribbler_speed_rpm = -12000, // Motor constant - .motor_max_acceleration_m_per_s_2 = 4.5f, + .motor_max_acceleration_m_per_s_2 = 2.0f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, + .robot_max_speed_m_per_s = 3.0f, .robot_max_acceleration_m_per_s_2 = 3.0f, .robot_max_deceleration_m_per_s_2 = 3.0f, From e1b15750d6325a2939c683a03bba38c5691e8684 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 16 Mar 2026 12:03:37 -0700 Subject: [PATCH 62/71] Update robot constants for 2026 --- .../ssl_simulation_robot_control.cpp | 2 +- .../ssl_simulation_robot_control.h | 2 +- .../ssl_simulation_robot_control_test.cpp | 4 +- .../message_translation/tbots_protobuf.cpp | 4 +- .../message_translation/tbots_protobuf.h | 4 +- .../tbots_protobuf_test.cpp | 2 +- .../primitive/primitive_msg_factory_test.cpp | 4 +- src/shared/2021_robot_constants.cpp | 38 -------- src/shared/2021_robot_constants.h | 10 -- src/shared/BUILD | 9 +- src/shared/robot_constants.cpp | 61 ++++++++++++ src/shared/robot_constants.h | 93 ++++++++----------- src/software/ai/evaluation/intercept_test.cpp | 2 +- .../ai/evaluation/time_to_travel_test.cpp | 2 +- .../ai/hl/stp/tactic/primitive_test.cpp | 8 +- .../embedded/constants/py_constants.py | 2 +- src/software/embedded/primitive_executor.cpp | 2 +- src/software/embedded/primitive_executor.h | 4 +- src/software/embedded/services/motor.cpp | 2 +- src/software/embedded/services/motor.h | 4 +- .../embedded/services/robot_auto_test.cpp | 2 +- src/software/embedded/thunderloop.cpp | 4 +- src/software/embedded/thunderloop.h | 6 +- src/software/embedded/thunderloop_main.cpp | 4 +- src/software/er_force_simulator_main.cpp | 4 +- src/software/physics/euclidean_to_wheel.cpp | 4 +- src/software/physics/euclidean_to_wheel.h | 4 +- .../physics/euclidean_to_wheel_test.cpp | 6 +- src/software/python_bindings.cpp | 8 +- .../simulated_er_force_sim_test_fixture.cpp | 4 +- .../simulation/er_force_simulator.cpp | 2 +- src/software/simulation/er_force_simulator.h | 4 +- .../simulation/er_force_simulator_test.cpp | 12 +-- .../drive_and_dribbler_widget.py | 2 +- .../handheld_controller_widget.py | 2 +- src/software/world/robot.cpp | 8 +- src/software/world/robot.h | 12 +-- src/software/world/robot_state.h | 1 - src/software/world/robot_test.cpp | 2 +- 39 files changed, 168 insertions(+), 182 deletions(-) delete mode 100644 src/shared/2021_robot_constants.cpp delete mode 100644 src/shared/2021_robot_constants.h create mode 100644 src/shared/robot_constants.cpp diff --git a/src/proto/message_translation/ssl_simulation_robot_control.cpp b/src/proto/message_translation/ssl_simulation_robot_control.cpp index c928a8bbcd..145a857254 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control.cpp +++ b/src/proto/message_translation/ssl_simulation_robot_control.cpp @@ -52,7 +52,7 @@ std::unique_ptr createRobotMoveCommand( std::unique_ptr getRobotCommandFromDirectControl( unsigned int robot_id, std::unique_ptr direct_control, - RobotConstants_t& robot_constants) + RobotConstants& robot_constants) { auto move_command = createRobotMoveCommand( *direct_control, robot_constants.front_wheel_angle_deg, diff --git a/src/proto/message_translation/ssl_simulation_robot_control.h b/src/proto/message_translation/ssl_simulation_robot_control.h index 4af93ad139..dfc3952f9f 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control.h +++ b/src/proto/message_translation/ssl_simulation_robot_control.h @@ -34,7 +34,7 @@ std::unique_ptr createRobotMoveCommand( std::unique_ptr getRobotCommandFromDirectControl( unsigned int robot_id, std::unique_ptr direct_control, - RobotConstants_t& robot_constants); + RobotConstants& robot_constants); /** * Creates a RobotCommand proto diff --git a/src/proto/message_translation/ssl_simulation_robot_control_test.cpp b/src/proto/message_translation/ssl_simulation_robot_control_test.cpp index 42abf85524..0f5b84828d 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control_test.cpp +++ b/src/proto/message_translation/ssl_simulation_robot_control_test.cpp @@ -2,12 +2,12 @@ #include -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" class SSLSimulationProtoTest : public ::testing::Test { protected: - RobotConstants_t robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); }; TEST_F(SSLSimulationProtoTest, test_create_robot_move_command_forward_from_primitive) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index cc21417cd7..a54018cf3a 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -489,7 +489,7 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( } int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, - RobotConstants_t robot_constants) + RobotConstants robot_constants) { switch (dribbler_mode) { @@ -507,7 +507,7 @@ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, double convertMaxAllowedSpeedModeToMaxAllowedSpeed( TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, - RobotConstants_t robot_constants) + RobotConstants robot_constants) { switch (max_allowed_speed_mode) { diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 50ab53c374..c56045774d 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -268,7 +268,7 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( * @return the dribbler speed in RPM */ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, - RobotConstants_t robot_constants); + RobotConstants robot_constants); /** * Convert max allowed speed mode to max allowed speed @@ -280,4 +280,4 @@ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, */ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, - RobotConstants_t robot_constants); + RobotConstants robot_constants); diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 9cdde04170..9d9958697d 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -152,7 +152,7 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) // Generate a trajectory, and then generate a TbotsProto::TrajectoryPathParams2D // with the same parameters as the trajectory, finally, generate a second trajectory // from the parameters and make sure the two trajectories are equal. - RobotConstants robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); Point start_position(0.0, 0.0); Point destination(0.0, 0.0); diff --git a/src/proto/primitive/primitive_msg_factory_test.cpp b/src/proto/primitive/primitive_msg_factory_test.cpp index 3bc46d40e1..994113ecd8 100644 --- a/src/proto/primitive/primitive_msg_factory_test.cpp +++ b/src/proto/primitive/primitive_msg_factory_test.cpp @@ -2,14 +2,14 @@ #include -#include "shared/2021_robot_constants.h" #include "shared/constants.h" +#include "shared/robot_constants.h" #include "software/test_util/test_util.h" class PrimitiveFactoryTest : public testing::Test { protected: - RobotConstants_t robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); }; TEST_F(PrimitiveFactoryTest, test_auto_chip_or_kick_equality) diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp deleted file mode 100644 index 1513e825f5..0000000000 --- a/src/shared/2021_robot_constants.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "shared/2021_robot_constants.h" - -#include "shared/constants.h" - -RobotConstants_t create2021RobotConstants(void) -{ - RobotConstants_t robot_constants = { - .mass_kg = 2.5f, // determined experimentally - .inertial_factor = 0.37f, // determined experimentally - .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), - // TODO (#2112): update this - .jerk_limit_kg_m_per_s_3 = 40.0f, - .front_wheel_angle_deg = 32.06f, - .back_wheel_angle_deg = 46.04f, - // TODO (#2112): update this - - .front_of_robot_width_meters = 0.11f, - .dribbler_width_meters = 0.07825f, - // Dribbler speeds are negative as that is the direction that sucks the ball in - .indefinite_dribbler_speed_rpm = -10000, - .max_force_dribbler_speed_rpm = -12000, - - // Motor constant - .motor_max_acceleration_m_per_s_2 = 4.5f, - - // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, - - // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, - - .wheel_radius_meters = 0.03f, - .wheel_rotations_per_motor_rotation = 17.0f / 60.0f}; - return robot_constants; -} diff --git a/src/shared/2021_robot_constants.h b/src/shared/2021_robot_constants.h deleted file mode 100644 index 3c071d3f6f..0000000000 --- a/src/shared/2021_robot_constants.h +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -#include "shared/robot_constants.h" - -/** - * Creates robot constants for the 2021 robot - * - * @return robot constants for the 2021 robot - */ -RobotConstants_t create2021RobotConstants(void); diff --git a/src/shared/BUILD b/src/shared/BUILD index 49839f3e46..8b77da71e1 100644 --- a/src/shared/BUILD +++ b/src/shared/BUILD @@ -22,12 +22,7 @@ cc_library( cc_library( name = "robot_constants", - srcs = [ - "2021_robot_constants.cpp", - ], - hdrs = [ - "2021_robot_constants.h", - "robot_constants.h", - ], + srcs = ["robot_constants.cpp"], + hdrs = ["robot_constants.h"], deps = [":constants"], ) diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp new file mode 100644 index 0000000000..f9dc353a76 --- /dev/null +++ b/src/shared/robot_constants.cpp @@ -0,0 +1,61 @@ +#include "shared/robot_constants.h" + +#include "shared/constants.h" + +RobotConstants create2026RobotConstants() +{ + return { + .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), + .front_wheel_angle_deg = 32.06f, + .back_wheel_angle_deg = 46.04f, + + .front_of_robot_width_meters = 0.11f, + .dribbler_width_meters = 0.07825f, + + // Dribbler speeds are negative as that is the direction that sucks the ball in + .indefinite_dribbler_speed_rpm = -10000, + .max_force_dribbler_speed_rpm = -12000, + + // Motor constant + .motor_max_acceleration_m_per_s_2 = 4.5f, + + // Robot's linear movement constants + .robot_max_speed_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, + + // Robot's angular movement constants + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f}; +} + +RobotConstants create2021RobotConstants() +{ + return { + .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), + .front_wheel_angle_deg = 32.06f, + .back_wheel_angle_deg = 46.04f, + + .front_of_robot_width_meters = 0.11f, + .dribbler_width_meters = 0.07825f, + + // Dribbler speeds are negative as that is the direction that sucks the ball in + .indefinite_dribbler_speed_rpm = -10000, + .max_force_dribbler_speed_rpm = -12000, + + // Motor constant + .motor_max_acceleration_m_per_s_2 = 4.5f, + + // Robot's linear movement constants + .robot_max_speed_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, + + // Robot's angular movement constants + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f}; +} diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index af9bbee29e..8dd2e049df 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -1,63 +1,39 @@ #pragma once -/** - * This struct represents robot constants - */ -typedef struct RobotConstants +struct RobotConstants { - // The mass of the entire robot including batteries [kg] - // Determined experimentally by weighing the robot and battery - float mass_kg; - - // The inertial factor - float inertial_factor; - // The radius of the robot [m] float robot_radius_m; - // The maximum jerk this robot may safely undergo [m/s^3] - float jerk_limit_kg_m_per_s_3; - - // The front_wheel_angle_deg and back_wheel_angle_deg are measured as absolute angle - // to each of the wheels from the front y axis of the robot. In the ASCII art below, - // front_wheel_angle_deg = A and back_wheel_angle_deg = A + B. The angles are assumed - // to be left/right symmetrical + // The front_wheel_angle_deg and back_wheel_angle_deg are measured as absolute + // angles from the robot's y-axis to each wheel axle. + // + // In the ASCII diagram below: + // - front_wheel_angle_deg = A + // - back_wheel_angle_deg = B // + // The angles are assumed to be symmetric for the left and right sides of the robot. + // + // y // ▲ - // │ - // │ - // │ - // │ - // │ - // │ - // │ - // │ - // │ - // *#### ### ###│### ### ####* - // *## │ ##* - // *## │ ##* wheel - // *## │ ##* │ - // *## │ xx##*◄──┘ - // *## │ A xxx ##* - // *## │ xxxx ##* - // *## │ xxxx ##* - // *## │xxxx ##* - // *## xx B ##* - // *## xx ##* - // *## xx ##* - // *## xx ##* - // *## xx ##* - // *## xx ##* - // *## x ##* - // *## ##*◄──┐ - // *## ##* │ - // *## ##* wheel - // *** ### ### *** - - // angle between each front wheel and the front y axis of the robot [degrees] + // | + // Back wheel │ Front wheel + // └────────► , - │ - , ◄───────┘ + // , '\ │ /' , + // , \ B │ A / │ + // , \ │ / │ + // , \ │ / │ + // , └────────┼───────► x Front of robot + // , │ + // , │ + // , │ + // , .' + // ' - , _ , ' + + // The angle between y-axis of the robot and the front wheel axles [degrees] float front_wheel_angle_deg; - // angle between each back wheel and the front y axis of the robot [degrees] + // The angle between y-axis of the robot and the rear wheel axles [degrees] float back_wheel_angle_deg; // The total width of the entire flat face on the front of the robot [meters] @@ -93,9 +69,18 @@ typedef struct RobotConstants // The radius of the wheel, in meters float wheel_radius_meters; +}; - // The gear ratio between the motor shaft and wheel shaft - // [# of wheel rotations / 1 motor rotation] - float wheel_rotations_per_motor_rotation; +/** + * Creates robot constants for the 2026 robot + * + * @return robot constants for the 2026 robot + */ +RobotConstants create2026RobotConstants(); -} RobotConstants_t; +/** + * Creates robot constants for the 2021 robot + * + * @return robot constants for the 2021 robot + */ +RobotConstants create2021RobotConstants(); diff --git a/src/software/ai/evaluation/intercept_test.cpp b/src/software/ai/evaluation/intercept_test.cpp index 613510eb74..0d8d979675 100644 --- a/src/software/ai/evaluation/intercept_test.cpp +++ b/src/software/ai/evaluation/intercept_test.cpp @@ -39,7 +39,7 @@ TEST(InterceptEvaluationTest, Ball ball({0, 0}, {6, 0}, Timestamp::fromSeconds(0)); Robot robot(0, {2, 0}, {0, 0}, Angle::zero(), AngularVelocity::zero(), Timestamp::fromSeconds(0), std::set(), - create2021RobotConstants()); + create2026RobotConstants()); // We should be able to find an intercept auto best_intercept = findBestInterceptForBall(ball, field, robot); diff --git a/src/software/ai/evaluation/time_to_travel_test.cpp b/src/software/ai/evaluation/time_to_travel_test.cpp index e0b35794f1..123d28a280 100644 --- a/src/software/ai/evaluation/time_to_travel_test.cpp +++ b/src/software/ai/evaluation/time_to_travel_test.cpp @@ -7,7 +7,7 @@ class TimeToTravel : public ::testing::Test { protected: - RobotConstants_t robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); }; TEST_F(TimeToTravel, getTimeToTravelDistance_already_at_dest) diff --git a/src/software/ai/hl/stp/tactic/primitive_test.cpp b/src/software/ai/hl/stp/tactic/primitive_test.cpp index c31e68ded8..a0015fd7da 100644 --- a/src/software/ai/hl/stp/tactic/primitive_test.cpp +++ b/src/software/ai/hl/stp/tactic/primitive_test.cpp @@ -1,6 +1,6 @@ #include -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" #include "software/ai/hl/stp/tactic/move_primitive.h" #include "software/ai/hl/stp/tactic/stop_primitive.h" #include "software/geom/algorithms/contains.h" @@ -16,9 +16,9 @@ class PrimitiveTest : public testing::Test } protected: - RobotConstants_t robot_constants = create2021RobotConstants(); - Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); - std::shared_ptr world = TestUtil::createBlankTestingWorld(); + RobotConstants robot_constants = create2026RobotConstants(); + Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); + std::shared_ptr world = TestUtil::createBlankTestingWorld(); RobotNavigationObstacleFactory obstacle_factory = RobotNavigationObstacleFactory(TbotsProto::RobotNavigationObstacleConfig()); }; diff --git a/src/software/embedded/constants/py_constants.py b/src/software/embedded/constants/py_constants.py index 43b8e1af03..d0c14d17c8 100644 --- a/src/software/embedded/constants/py_constants.py +++ b/src/software/embedded/constants/py_constants.py @@ -3,7 +3,7 @@ import software.python_bindings as tbots_cpp DEFAULT_PRIMITIVE_DURATION = 2.0 -ROBOT_CONSTANTS = tbots_cpp.create2021RobotConstants() +ROBOT_CONSTANTS = tbots_cpp.create2026RobotConstants() ROBOT_MAX_ANG_SPEED_RAD_PER_S = ROBOT_CONSTANTS.robot_max_ang_speed_rad_per_s ROBOT_MAX_SPEED_M_PER_S = ROBOT_CONSTANTS.robot_max_speed_m_per_s diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index bbe665dfd0..d0ef84fe90 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,7 +11,7 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor(const Duration time_step, - const RobotConstants_t& robot_constants, + const RobotConstants& robot_constants, const TeamColour friendly_team_colour, const RobotId robot_id) : current_primitive_(), diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index fde63b1719..dc3c2d8db2 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -21,7 +21,7 @@ class PrimitiveExecutor * @param robot_id The id of the robot which uses this primitive executor */ explicit PrimitiveExecutor(const Duration time_step, - const RobotConstants_t& robot_constants, + const RobotConstants& robot_constants, const TeamColour friendly_team_colour, const RobotId robot_id); @@ -82,7 +82,7 @@ class PrimitiveExecutor AngularVelocity angular_velocity_; Angle orientation_; TeamColour friendly_team_colour_; - RobotConstants_t robot_constants_; + RobotConstants robot_constants_; std::optional trajectory_path_; std::optional angular_trajectory_; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 7e83b85851..1459b92ef5 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -65,7 +65,7 @@ extern "C" } } -MotorService::MotorService(const RobotConstants_t& robot_constants, +MotorService::MotorService(const 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)), diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 41b3ee501d..049dffe62d 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -39,7 +39,7 @@ class MotorService * @param RobotConstants_t The robot constants * @param control_loop_frequency_hz The frequency the main loop will call poll at */ - MotorService(const RobotConstants_t& robot_constants, int control_loop_frequency_hz); + MotorService(const RobotConstants& robot_constants, int control_loop_frequency_hz); virtual ~MotorService(); @@ -387,7 +387,7 @@ class MotorService // SPI File Descriptors std::unordered_map file_descriptors_; - RobotConstants_t robot_constants_; + RobotConstants robot_constants_; // Drive Motors EuclideanToWheel euclidean_to_four_wheel_; diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp index 098049e2ef..13b20c475d 100644 --- a/src/software/embedded/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv) LOG(INFO) << "Running on the Raspberry Pi!"; motor_service_ = - std::make_unique(create2021RobotConstants(), THUNDERLOOP_HZ); + std::make_unique(create2026RobotConstants(), THUNDERLOOP_HZ); // Testing Motor board SPI transfer for (uint8_t chip_select : CHIP_SELECT) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 63e067f203..02703fa4f9 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -7,8 +7,8 @@ #include "proto/robot_crash_msg.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" -#include "shared/2021_robot_constants.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" @@ -68,7 +68,7 @@ extern "C" } } -Thunderloop::Thunderloop(const RobotConstants_t& robot_constants, bool enable_log_merging, +Thunderloop::Thunderloop(const 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)), diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index e1e1cfe041..46503b78bd 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -7,8 +7,8 @@ #include #include "proto/tbots_software_msgs.pb.h" -#include "shared/2021_robot_constants.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/network/network.h" @@ -49,7 +49,7 @@ class Thunderloop * @param enable_log_merging Whether to merge repeated log message or not * @param loop_hz The rate to run the loop */ - Thunderloop(const RobotConstants_t& robot_constants, bool enable_log_merging, + Thunderloop(const RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz); ~Thunderloop(); @@ -140,7 +140,7 @@ class Thunderloop TbotsProto::Timestamp time_sent_; // Current State - RobotConstants_t robot_constants_; + RobotConstants robot_constants_; Angle current_orientation_; int robot_id_; int channel_id_; diff --git a/src/software/embedded/thunderloop_main.cpp b/src/software/embedded/thunderloop_main.cpp index 7218380629..cb7c529ffd 100644 --- a/src/software/embedded/thunderloop_main.cpp +++ b/src/software/embedded/thunderloop_main.cpp @@ -11,8 +11,8 @@ #include #include "proto/tbots_software_msgs.pb.h" -#include "shared/2021_robot_constants.h" #include "shared/constants.h" +#include "shared/robot_constants.h" #include "software/embedded/thunderloop.h" #include "software/logger/network_logger.h" #include "software/world/robot_state.h" @@ -112,7 +112,7 @@ int main(int argc, char** argv) reserveProcessMemory(pre_allocation_size); auto thunderloop = - Thunderloop(create2021RobotConstants(), args.enable_log_merging, THUNDERLOOP_HZ); + Thunderloop(create2026RobotConstants(), args.enable_log_merging, THUNDERLOOP_HZ); thunderloop.runLoop(); return 0; diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 92606942cc..ecfbc7ed6b 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -88,12 +88,12 @@ int main(int argc, char** argv) if (args.division == "div_a") { er_force_sim = std::make_shared( - TbotsProto::FieldType::DIV_A, create2021RobotConstants(), realism_config); + TbotsProto::FieldType::DIV_A, create2026RobotConstants(), realism_config); } else { er_force_sim = std::make_shared( - TbotsProto::FieldType::DIV_B, create2021RobotConstants(), realism_config); + TbotsProto::FieldType::DIV_B, create2026RobotConstants(), realism_config); } std::mutex simulator_mutex; diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 0b4cd839e7..b3b7adbd41 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -4,11 +4,11 @@ #include "proto/message_translation/tbots_geometry.h" #include "proto/primitive/primitive_msg_factory.h" -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" -EuclideanToWheel::EuclideanToWheel(const RobotConstants_t& robot_constants) +EuclideanToWheel::EuclideanToWheel(const RobotConstants& robot_constants) : robot_radius_m_(robot_constants.robot_radius_m), robot_constants_(robot_constants) { // Phi, the angle between the hemisphere line of the robot and the front wheel axles diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index 984ea63aae..2460ff0fdb 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -41,7 +41,7 @@ class EuclideanToWheel * * @param robot_constants The constants of the robot we are computing for. */ - explicit EuclideanToWheel(const RobotConstants_t& robot_constants); + explicit EuclideanToWheel(const RobotConstants& robot_constants); /** * Gets the wheel velocity from the Euclidean velocity. @@ -80,7 +80,7 @@ class EuclideanToWheel * The radius of the robot in meters. */ const double robot_radius_m_{}; - const RobotConstants_t robot_constants_; + const RobotConstants robot_constants_; /** * Euclidean velocity to wheel velocity coupling matrix. diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index b408e76248..7ce54bace7 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -12,8 +12,8 @@ class EuclideanToWheelTest : public ::testing::Test EuclideanSpace_t target_euclidean_velocity{}; WheelSpace_t expected_wheel_speeds{}; WheelSpace_t calculated_wheel_speeds{}; - RobotConstants robot_constants = create2021RobotConstants(); - double robot_radius = create2021RobotConstants().robot_radius_m; + RobotConstants robot_constants = create2026RobotConstants(); + double robot_radius = create2026RobotConstants().robot_radius_m; WheelSpace_t target_wheel_velocity{}; WheelSpace_t current_wheel_velocity{}; @@ -22,7 +22,7 @@ class EuclideanToWheelTest : public ::testing::Test EuclideanToWheel euclidean_to_four_wheel = - EuclideanToWheel(create2021RobotConstants()); + EuclideanToWheel(create2026RobotConstants()); }; TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_zero) diff --git a/src/software/python_bindings.cpp b/src/software/python_bindings.cpp index dffc56f837..a89691c04d 100644 --- a/src/software/python_bindings.cpp +++ b/src/software/python_bindings.cpp @@ -21,7 +21,6 @@ #include "proto/team.pb.h" #include "proto/world.pb.h" #include "pybind11_protobuf/native_proto_caster.h" -#include "shared/2021_robot_constants.h" #include "shared/robot_constants.h" #include "software/ai/passing/eighteen_zone_pitch_division.h" #include "software/ai/passing/pass_generator.h" @@ -278,10 +277,6 @@ PYBIND11_MODULE(python_bindings, m) .def_readwrite("max_force_dribbler_speed_rpm", &RobotConstants::max_force_dribbler_speed_rpm) .def_readwrite("robot_radius_m", &RobotConstants::robot_radius_m) - .def_readwrite("mass_kg", &RobotConstants::mass_kg) - .def_readwrite("inertial_factor", &RobotConstants::inertial_factor) - .def_readwrite("jerk_limit_kg_m_per_s_3", - &RobotConstants::jerk_limit_kg_m_per_s_3) .def_readwrite("front_wheel_angle_deg", &RobotConstants::front_wheel_angle_deg) .def_readwrite("back_wheel_angle_deg", &RobotConstants::back_wheel_angle_deg) .def_readwrite("front_of_robot_width_meters", @@ -294,12 +289,11 @@ PYBIND11_MODULE(python_bindings, m) .def_readwrite("indefinite_dribbler_speed_rpm", &RobotConstants::indefinite_dribbler_speed_rpm) .def_readwrite("wheel_radius_meters", &RobotConstants::wheel_radius_meters) - .def_readwrite("wheel_rotations_per_motor_rotation", - &RobotConstants::wheel_rotations_per_motor_rotation) .def_readwrite("robot_max_speed_m_per_s", &RobotConstants::robot_max_speed_m_per_s) .def_readwrite("robot_max_ang_speed_rad_per_s", &RobotConstants::robot_max_ang_speed_rad_per_s); + m.def("create2026RobotConstants", &create2026RobotConstants); m.def("create2021RobotConstants", &create2021RobotConstants); m.def("createPoint", &createPoint); diff --git a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp index f825628c46..63181a6d44 100644 --- a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp +++ b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp @@ -11,7 +11,7 @@ #include "proto/message_translation/er_force_world.h" #include "proto/message_translation/ssl_wrapper.h" #include "proto/message_translation/tbots_protobuf.h" -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" #include "shared/test_util/test_util.h" #include "software/logger/logger.h" #include "software/simulation/er_force_simulator.h" @@ -181,7 +181,7 @@ void SimulatedErForceSimTestFixture::runTest( auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator(std::make_shared( - field_type, create2021RobotConstants(), realism_config, ramping)); + field_type, create2026RobotConstants(), realism_config, ramping)); // TODO (#2419): remove this to re-enable sigfpe checks fedisableexcept(FE_INVALID | FE_OVERFLOW); diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index b58d4989a3..69a45069fa 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -17,7 +17,7 @@ #include "software/world/robot_state.h" ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, - const RobotConstants_t& robot_constants, + const RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping, double primitive_executor_time_step) diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index b4e7833012..11485b76ae 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -28,7 +28,7 @@ class ErForceSimulator * @param realism_config realism configuration */ explicit ErForceSimulator(const TbotsProto::FieldType& field_type, - const RobotConstants_t& robot_constants, + const RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping = false, double primitive_executor_time_step_s = @@ -222,7 +222,7 @@ class ErForceSimulator std::unique_ptr er_force_sim; EuclideanToWheel euclidean_to_four_wheel; - RobotConstants_t robot_constants; + RobotConstants robot_constants; Field field; std::optional blue_robot_with_ball; diff --git a/src/software/simulation/er_force_simulator_test.cpp b/src/software/simulation/er_force_simulator_test.cpp index bda9a6d7c2..2b28b24101 100644 --- a/src/software/simulation/er_force_simulator_test.cpp +++ b/src/software/simulation/er_force_simulator_test.cpp @@ -7,7 +7,7 @@ #include "proto/message_translation/er_force_world.h" #include "proto/message_translation/tbots_protobuf.h" #include "proto/primitive/primitive_msg_factory.h" -#include "shared/2021_robot_constants.h" +#include "shared/robot_constants.h" #include "software/test_util/test_util.h" class ErForceSimulatorTest : public ::testing::Test @@ -24,7 +24,7 @@ class ErForceSimulatorTest : public ::testing::Test } std::shared_ptr simulator; - RobotConstants_t robot_constants = create2021RobotConstants(); + RobotConstants robot_constants = create2026RobotConstants(); }; TEST_F(ErForceSimulatorTest, set_ball_state_when_ball_does_not_already_exist) @@ -309,8 +309,8 @@ TEST_F(ErForceSimulatorTest, yellow_robot_add_robots_and_change_position) TEST(ErForceSimulatorFieldTest, check_field_A_configuration) { - RobotConstants_t robot_constants = create2021RobotConstants(); - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + RobotConstants robot_constants = create2026RobotConstants(); + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_A, robot_constants, realism_config); simulator->resetCurrentTime(); @@ -321,8 +321,8 @@ TEST(ErForceSimulatorFieldTest, check_field_A_configuration) TEST(ErForceSimulatorFieldTest, check_field_B_configuration) { - RobotConstants_t robot_constants = create2021RobotConstants(); - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + RobotConstants robot_constants = create2026RobotConstants(); + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_B, robot_constants, realism_config); simulator->resetCurrentTime(); diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 6df6356947..5331f5d301 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -32,7 +32,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.constants = tbots_cpp.create2021RobotConstants() + self.constants = tbots_cpp.create2026RobotConstants() self.enabled = True self.control_mode = ControlMode.VELOCITY diff --git a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py index 12e9464703..4d9bd208e6 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py @@ -37,7 +37,7 @@ def __init__(self) -> None: """Initialize the HandheldControllerWidget.""" super().__init__() - self.constants = tbots_cpp.create2021RobotConstants() + self.constants = tbots_cpp.create2026RobotConstants() self.handheld_controller: HandheldController | None = None diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 2154b9b163..92c1fa3d21 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -8,7 +8,7 @@ Robot::Robot(RobotId id, const Point& position, const Vector& velocity, const Angle& orientation, const AngularVelocity& angular_velocity, const Timestamp& timestamp, bool breakbeam_tripped, const std::set& unavailable_capabilities, - const RobotConstants_t& robot_constants) + const RobotConstants& robot_constants) : id_(id), current_state_(position, velocity, orientation, angular_velocity, breakbeam_tripped), @@ -22,7 +22,7 @@ Robot::Robot(RobotId id, const Point& position, const Vector& velocity, const Angle& orientation, const AngularVelocity& angular_velocity, const Timestamp& timestamp, const std::set& unavailable_capabilities, - const RobotConstants_t& robot_constants) + const RobotConstants& robot_constants) : id_(id), current_state_(position, velocity, orientation, angular_velocity, false), timestamp_(timestamp), @@ -33,7 +33,7 @@ Robot::Robot(RobotId id, const Point& position, const Vector& velocity, Robot::Robot(RobotId id, const RobotState& initial_state, const Timestamp& timestamp, const std::set& unavailable_capabilities, - const RobotConstants_t& robot_constants) + const RobotConstants& robot_constants) : id_(id), current_state_(initial_state), timestamp_(timestamp), @@ -171,7 +171,7 @@ std::set& Robot::getMutableRobotCapabilities() return unavailable_capabilities_; } -const RobotConstants_t& Robot::robotConstants() const +const RobotConstants& Robot::robotConstants() const { return robot_constants_; } diff --git a/src/software/world/robot.h b/src/software/world/robot.h index 163d60133f..2f8373d368 100644 --- a/src/software/world/robot.h +++ b/src/software/world/robot.h @@ -35,7 +35,7 @@ class Robot const Timestamp& timestamp, bool breakbeam_tripped = false, const std::set& unavailable_capabilities = std::set(), - const RobotConstants_t& robot_constants = DEFAULT_ROBOT_CONSTANTS); + const RobotConstants& robot_constants = DEFAULT_ROBOT_CONSTANTS); /** * Creates a new robot given robot data @@ -55,7 +55,7 @@ class Robot const Angle& orientation, const AngularVelocity& angular_velocity, const Timestamp& timestamp, const std::set& unavailable_capabilities, - const RobotConstants_t& robot_constants); + const RobotConstants& robot_constants); /** @@ -72,7 +72,7 @@ class Robot const Timestamp& timestamp, const std::set& unavailable_capabilities = std::set(), - const RobotConstants_t& robot_constants = DEFAULT_ROBOT_CONSTANTS); + const RobotConstants& robot_constants = DEFAULT_ROBOT_CONSTANTS); /** @@ -183,7 +183,7 @@ class Robot * * @return the robot constants for this robot */ - const RobotConstants_t& robotConstants() const; + const RobotConstants& robotConstants() const; /** * Decides if a point is near the dribbler of the robot @@ -269,9 +269,9 @@ class Robot // The hardware capabilities of the robot, generated from // RobotCapabilityFlags::broken_dribblers/chippers/kickers dynamic parameters std::set unavailable_capabilities_; - RobotConstants_t robot_constants_; + RobotConstants robot_constants_; // Default robot constants that should be used for all robots inline static const RobotConstants DEFAULT_ROBOT_CONSTANTS = - create2021RobotConstants(); + create2026RobotConstants(); }; diff --git a/src/software/world/robot_state.h b/src/software/world/robot_state.h index 94fdaad306..9f62084931 100644 --- a/src/software/world/robot_state.h +++ b/src/software/world/robot_state.h @@ -1,7 +1,6 @@ #pragma once #include "proto/vision.pb.h" -#include "shared/2021_robot_constants.h" #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/geom/angle.h" diff --git a/src/software/world/robot_test.cpp b/src/software/world/robot_test.cpp index e3bda4b593..4b4021bfcf 100644 --- a/src/software/world/robot_test.cpp +++ b/src/software/world/robot_test.cpp @@ -372,7 +372,7 @@ TEST_F(RobotTest, Angle target_angle = Angle::fromDegrees(-45.0); Robot robot(0, {1, 1}, Vector(0, 0), Angle::fromDegrees(45.0), AngularVelocity::fromDegrees(0), Timestamp::fromSeconds(0), {}, - create2021RobotConstants()); + create2026RobotConstants()); // Figure out a lower bound on the time required, based on us being able to constantly // accelerate at the max acceleration From bf90e6a221290b7b2644234d7acdbe14cb8d3ad6 Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 24 Apr 2026 11:54:51 -0700 Subject: [PATCH 63/71] Use robot constants directly in euclidean_to_wheel.cpp --- src/software/physics/euclidean_to_wheel.cpp | 6 +++--- src/software/physics/euclidean_to_wheel.h | 7 +------ 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index b3b7adbd41..1c42c17512 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -9,7 +9,7 @@ #include "software/geom/vector.h" EuclideanToWheel::EuclideanToWheel(const RobotConstants& robot_constants) - : robot_radius_m_(robot_constants.robot_radius_m), robot_constants_(robot_constants) + : robot_constants_(robot_constants) { // Phi, the angle between the hemisphere line of the robot and the front wheel axles // [rads] @@ -61,7 +61,7 @@ WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_veloc // need to multiply the angular velocity by the robot radius to // calculate the wheel velocity (robot tangential velocity) // ref: http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 8 - euclidean_velocity[2] = euclidean_velocity[2] * robot_radius_m_; + euclidean_velocity[2] = euclidean_velocity[2] * robot_constants_.robot_radius_m; return euclidean_to_wheel_velocity_D_ * euclidean_velocity; } @@ -76,7 +76,7 @@ EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity( // velocity. This can be divided by the robot radius to calculate // the angular velocity // ref: http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 8 - euclidean_velocity[2] = euclidean_velocity[2] / robot_radius_m_; + euclidean_velocity[2] = euclidean_velocity[2] / robot_constants_.robot_radius_m; return euclidean_velocity; } diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index 2460ff0fdb..bd8ead3fdc 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -64,22 +64,17 @@ class EuclideanToWheel * Ramp the velocity over the given timestep and set the target velocity on the motor. * * NOTE: This function has no state. - * Also NOTE: This function handles all electrical rpm to meters/second conversion. * * @param current_wheel_velocity The current 4-wheel velocity in m/s * @param target_wheel_velocity The target 4-wheel velocity m/s * @param time_to_ramp The time allocated for acceleration in seconds - * + * @return The ramped 4-wheel velocity in m/s */ WheelSpace_t rampWheelVelocity(const WheelSpace_t& current_wheel_velocity, const WheelSpace_t& target_wheel_velocity, const double& time_to_ramp); private: - /** - * The radius of the robot in meters. - */ - const double robot_radius_m_{}; const RobotConstants robot_constants_; /** From 59dc90ed4206314cd51db8f339844be9b9c15dd9 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Tue, 28 Apr 2026 16:57:10 -0700 Subject: [PATCH 64/71] Implement static feedforward gain based on driving angle --- src/software/embedded/motor_controller/BUILD | 1 + .../motor_controller/motor_controller.h | 3 ++ .../stspin_motor_controller.cpp | 49 ++++++++++++++++++- .../stspin_motor_controller.h | 9 +++- .../stspin_motor_controller_test.cpp | 3 +- .../embedded/motor_controller/stspin_types.h | 6 +++ .../motor_controller/tmc_motor_controller.h | 2 + src/software/embedded/services/motor.cpp | 8 +-- src/software/embedded/services/motor.h | 4 +- 9 files changed, 75 insertions(+), 10 deletions(-) diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index 248afd638f..26518edbfd 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -36,6 +36,7 @@ cc_library( "//software/embedded:spi_utils", "//software/embedded/gpio", "//software/logger", + "//software/physics:euclidean_to_wheel", "@cppcrc", "@trinamic", ], diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index 9167a13b0b..22fd1d20c2 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -2,6 +2,7 @@ #include "software/embedded/motor_controller/motor_fault_indicator.h" #include "software/embedded/motor_controller/motor_index.h" +#include "software/physics/euclidean_to_wheel.h" class MotorController { @@ -16,5 +17,7 @@ class MotorController virtual int readThenWriteVelocity(MotorIndex motor, int target_velocity) = 0; + virtual void updateEuclideanVelocity(EuclideanSpace_t target_euclidean_velocity) = 0; + virtual void immediatelyDisable() = 0; }; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index bd1117d525..cda539181f 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -21,8 +21,9 @@ // (https://reveng.sourceforge.io/crc-catalogue/all.htm#crc.cat.crc-8-autosar) using Crc8Autosar = crc_utils::crc; -StSpinMotorController::StSpinMotorController() - : reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, +StSpinMotorController::StSpinMotorController(const RobotConstants& robot_constants) + : robot_constants_(robot_constants), + reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, GpioState::HIGH)) { for (const MotorIndex motor : driveMotors()) @@ -43,7 +44,11 @@ void StSpinMotorController::setup() for (const MotorIndex motor : driveMotors()) { + // sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 0, .ki = 0}); + sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 806, .ki = 154}); + // sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 600, .ki = 0}); + sendAndReceiveFrame(motor, SetSpeedFeedForwardKaKvFrame{.ka = 0, .kv = 100}); } } @@ -184,6 +189,40 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, 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]); + const Angle direction = local_velocity.orientation(); + + const int16_t front_left_ks = static_cast(std::abs( + (direction - Angle::fromDegrees(90 - robot_constants_.front_wheel_angle_deg)) + .sin() * + 750)); + const int16_t front_right_ks = static_cast(std::abs( + (direction + Angle::fromDegrees(90 - robot_constants_.front_wheel_angle_deg)) + .sin() * + 750)); + const int16_t back_right_ks = static_cast(std::abs( + (direction + Angle::fromDegrees(90 + robot_constants_.back_wheel_angle_deg)) + .sin() * + 750)); + const int16_t back_left_ks = static_cast(std::abs( + (direction - Angle::fromDegrees(90 + robot_constants_.back_wheel_angle_deg)) + .sin() * + 750)); + + 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()) @@ -309,6 +348,12 @@ void StSpinMotorController::populateTx(const OutgoingFrame& outgoing_frame, 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); diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index fbe635fdd1..b8c88d706b 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -9,7 +9,7 @@ class StSpinMotorController : public MotorController { public: - explicit StSpinMotorController(); + explicit StSpinMotorController(const RobotConstants& robot_constants); void setup() override; @@ -19,13 +19,16 @@ class StSpinMotorController : public MotorController int readThenWriteVelocity(MotorIndex motor, int target_velocity) override; + void updateEuclideanVelocity(EuclideanSpace_t target_euclidean_velocity) override; + void immediatelyDisable() override; private: using OutgoingFrame = std::variant; + SetPidSpeedKpKiFrame, SetSpeedFeedForwardKaKvFrame, + SetSpeedFeedForwardKsFrame>; // Length of frame (in number of bytes) static constexpr unsigned int FRAME_LEN = 6; @@ -46,6 +49,8 @@ class StSpinMotorController : public MotorController static constexpr int RESET_GPIO_PIN = 12; + RobotConstants robot_constants_; + // SPI file descriptors std::unordered_map spi_fds_; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp index 1882f7b874..598364afd1 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -46,7 +46,8 @@ class StSpinMotorControllerTest } LOG(INFO) << "Enabled motors: " << ss.str(); - const auto motor_controller = std::make_unique(); + const auto motor_controller = + std::make_unique(create2026RobotConstants()); motor_controller->setup(); LOG(INFO) << "Motor controller setup complete"; diff --git a/src/software/embedded/motor_controller/stspin_types.h b/src/software/embedded/motor_controller/stspin_types.h index f0e3c39b2c..7b1ba2e3c8 100644 --- a/src/software/embedded/motor_controller/stspin_types.h +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -12,6 +12,7 @@ enum class StSpinOpcode 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 @@ -85,3 +86,8 @@ struct SetSpeedFeedForwardKaKvFrame int16_t ka; int16_t kv; }; + +struct SetSpeedFeedForwardKsFrame +{ + int16_t ks; +}; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index cae8d31c16..a259f3a557 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -20,6 +20,8 @@ class TmcMotorController : public MotorController 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. diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 5b392c1437..b78a3424b6 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -7,8 +7,8 @@ #include "software/logger/logger.h" MotorService::MotorService(const RobotConstants& robot_constants) - : motor_controller_(setupMotorController()), - robot_constants_(robot_constants), + : robot_constants_(robot_constants), + motor_controller_(setupMotorController()), euclidean_to_four_wheel_(robot_constants), drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60) { @@ -36,7 +36,7 @@ std::unique_ptr MotorService::setupMotorController() } else { - return std::make_unique(); + return std::make_unique(robot_constants_); } } @@ -197,6 +197,8 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor direct_velocity.velocity().x_component_meters(), direct_velocity.angular_velocity().radians_per_second()}; + motor_controller_->updateEuclideanVelocity(target_euclidean_velocity); + target_wheel_velocities_ = euclidean_to_four_wheel_.getWheelVelocity(target_euclidean_velocity); } diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 292a6b52a3..d381781d1f 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -73,10 +73,10 @@ class MotorService bool anyMotorRequiresReset() const; - std::unique_ptr motor_controller_; - RobotConstants robot_constants_; + std::unique_ptr motor_controller_; + EuclideanToWheel euclidean_to_four_wheel_; WheelSpace_t prev_wheel_velocities_; From 42ef088f2536f1e3839e089f2c2b678b0364a5c7 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Wed, 29 Apr 2026 12:17:36 -0700 Subject: [PATCH 65/71] Cleanup feedforward static gain calculation --- .../stspin_motor_controller.cpp | 36 +++++++++---------- .../stspin_motor_controller.h | 2 ++ 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index cda539181f..80709ae0a5 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -44,10 +44,7 @@ void StSpinMotorController::setup() for (const MotorIndex motor : driveMotors()) { - // sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 0, .ki = 0}); - sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 806, .ki = 154}); - // sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 600, .ki = 0}); sendAndReceiveFrame(motor, SetSpeedFeedForwardKaKvFrame{.ka = 0, .kv = 100}); } } @@ -196,22 +193,23 @@ void StSpinMotorController::updateEuclideanVelocity( -target_euclidean_velocity[0]); const Angle direction = local_velocity.orientation(); - const int16_t front_left_ks = static_cast(std::abs( - (direction - Angle::fromDegrees(90 - robot_constants_.front_wheel_angle_deg)) - .sin() * - 750)); - const int16_t front_right_ks = static_cast(std::abs( - (direction + Angle::fromDegrees(90 - robot_constants_.front_wheel_angle_deg)) - .sin() * - 750)); - const int16_t back_right_ks = static_cast(std::abs( - (direction + Angle::fromDegrees(90 + robot_constants_.back_wheel_angle_deg)) - .sin() * - 750)); - const int16_t back_left_ks = static_cast(std::abs( - (direction - Angle::fromDegrees(90 + robot_constants_.back_wheel_angle_deg)) - .sin() * - 750)); + 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}); diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index b8c88d706b..624dcab304 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -49,6 +49,8 @@ class StSpinMotorController : public MotorController static constexpr int RESET_GPIO_PIN = 12; + static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750; + RobotConstants robot_constants_; // SPI file descriptors From d2072c810b1c84cdab2db7bcc422324d1f9edb6c Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Wed, 29 Apr 2026 17:56:56 -0700 Subject: [PATCH 66/71] Update static gain constants --- .../stspin_motor_controller.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 80709ae0a5..bcce771845 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -44,8 +44,10 @@ void StSpinMotorController::setup() for (const MotorIndex motor : driveMotors()) { + // sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 500, .ki = 60}); + sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = 806, .ki = 154}); - sendAndReceiveFrame(motor, SetSpeedFeedForwardKaKvFrame{.ka = 0, .kv = 100}); + sendAndReceiveFrame(motor, SetSpeedFeedForwardKaKvFrame{.ka = 0, .kv = 0}); } } @@ -191,6 +193,20 @@ void StSpinMotorController::updateEuclideanVelocity( { const Vector local_velocity(target_euclidean_velocity[1], -target_euclidean_velocity[0]); + + if (local_velocity.length() <= 0.01) + { + sendAndReceiveFrame(MotorIndex::FRONT_LEFT, + SetSpeedFeedForwardKsFrame{.ks = 270}); + sendAndReceiveFrame(MotorIndex::FRONT_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = 270}); + sendAndReceiveFrame(MotorIndex::BACK_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = 270}); + sendAndReceiveFrame(MotorIndex::BACK_LEFT, + SetSpeedFeedForwardKsFrame{.ks = 270}); + return; + } + const Angle direction = local_velocity.orientation(); const Angle front_wheel_angle = From 03f740b08e0f3840ea3029609b608e67ce84604d Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Thu, 30 Apr 2026 11:56:07 -0700 Subject: [PATCH 67/71] Fix thunderscope_main --- src/shared/robot_constants.cpp | 2 +- src/software/thunderscope/thunderscope_main.py | 9 ++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp index 0e2742a8da..7e14ae2781 100644 --- a/src/shared/robot_constants.cpp +++ b/src/shared/robot_constants.cpp @@ -17,7 +17,7 @@ RobotConstants create2026RobotConstants() .max_force_dribbler_speed_rpm = -12000, // Motor constant - .motor_max_acceleration_m_per_s_2 = 2.0f, + .motor_max_acceleration_m_per_s_2 = 4.5f, // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index c35b675f52..02fb9c905f 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -329,6 +329,9 @@ layout_path=args.layout, ) + # Fetch the AI runtime/backends + runtime_config = runtime_manager_instance.fetch_runtime_config() + if args.run_blue: runtime_dir = args.blue_full_system_runtime_dir friendly_colour_yellow = False @@ -378,12 +381,8 @@ ) if args.run_blue or args.run_yellow: - full_system_runtime_dir = ( - args.blue_full_system_runtime_dir - if args.run_blue - else args.yellow_full_system_runtime_dir - ) with FullSystem( + path_to_binary=runtime_config.get_blue_runtime_path(), full_system_runtime_dir=runtime_dir, debug_full_system=debug, friendly_colour_yellow=friendly_colour_yellow, From e7ff0afff0da8436150fc3df5018679984b593a8 Mon Sep 17 00:00:00 2001 From: williamckha Date: Wed, 29 Apr 2026 12:37:18 -0700 Subject: [PATCH 68/71] Add ability to drag to orient robot in GLMovementFieldTestLayer --- .../gl/layers/gl_movement_field_test_layer.py | 70 ++++++++++++++++--- 1 file changed, 62 insertions(+), 8 deletions(-) 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..069410a3e5 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 @@ -30,6 +30,11 @@ def __init__( self.selected_robot_id = 0 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) - - def move_to_point(self, point): - """Move to a 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, 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: @@ -85,12 +97,15 @@ def move_to_point(self, point): return 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""" From 9c2ba447043d1bad64e2b2ca3ba8ce2e231fdd43 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Tue, 5 May 2026 15:36:24 -0700 Subject: [PATCH 69/71] Fix incorrect conversion rate in tmc_motor_controller --- .../motor_controller/tmc_motor_controller.cpp | 19 +++++++++++++++---- .../motor_controller/tmc_motor_controller.h | 8 +++++++- 2 files changed, 22 insertions(+), 5 deletions(-) diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index d9c8a0b3f1..468919fd4c 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -61,17 +61,28 @@ TmcMotorController::TmcMotorController() int TmcMotorController::readThenWriteVelocity(const MotorIndex motor, const int target_velocity) { - const int velocity_erpm = readThenWriteValue( - motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, 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; + } +} - 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) diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index a259f3a557..c88ed1f40e 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -268,15 +268,21 @@ class TmcMotorController : public MotorController 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; + 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; From f9ce19d8b1921657375e34f8e5a3c97e5b55d120 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 17 May 2026 12:45:39 -0700 Subject: [PATCH 70/71] Run formatting --- .../stspin_motor_controller.cpp | 5 ++--- .../gl/layers/gl_movement_field_test_layer.py | 22 +++++++++---------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index bcce771845..a9653d1f0b 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -194,7 +194,7 @@ void StSpinMotorController::updateEuclideanVelocity( const Vector local_velocity(target_euclidean_velocity[1], -target_euclidean_velocity[0]); - if (local_velocity.length() <= 0.01) + if (local_velocity.length() <= 0.01) { sendAndReceiveFrame(MotorIndex::FRONT_LEFT, SetSpeedFeedForwardKsFrame{.ks = 270}); @@ -202,8 +202,7 @@ void StSpinMotorController::updateEuclideanVelocity( SetSpeedFeedForwardKsFrame{.ks = 270}); sendAndReceiveFrame(MotorIndex::BACK_RIGHT, SetSpeedFeedForwardKsFrame{.ks = 270}); - sendAndReceiveFrame(MotorIndex::BACK_LEFT, - SetSpeedFeedForwardKsFrame{.ks = 270}); + sendAndReceiveFrame(MotorIndex::BACK_LEFT, SetSpeedFeedForwardKsFrame{.ks = 270}); return; } 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 069410a3e5..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 @@ -30,7 +30,7 @@ def __init__( self.selected_robot_id = 0 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 @@ -80,7 +80,7 @@ def mouse_in_scene_pressed(self, event: MouseInSceneEvent) -> None: 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 @@ -97,7 +97,7 @@ def move_to_point(self, point, orientation: float = None): return robot_id = self.selected_robot_id - + if orientation is None: orientation = -math.pi / 2 @@ -120,37 +120,37 @@ def move_to_point(self, point, orientation: float = None): @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 From 9157ff0860a11bcb544fd37246393f10737d299d Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sun, 17 May 2026 13:07:44 -0700 Subject: [PATCH 71/71] refactor constants for 2026 --- src/.bazelrc | 3 + .../ssl_simulation_robot_control.cpp | 2 +- .../ssl_simulation_robot_control.h | 2 +- .../ssl_simulation_robot_control_test.cpp | 2 +- .../message_translation/tbots_protobuf.cpp | 8 +- .../message_translation/tbots_protobuf.h | 8 +- .../tbots_protobuf_test.cpp | 3 +- .../primitive/primitive_msg_factory_test.cpp | 3 +- src/shared/BUILD | 4 +- src/shared/constants.h | 4 +- src/shared/robot_constants.cpp | 61 --------------- src/shared/robot_constants.h | 76 ++++++++++++++++--- src/software/ai/evaluation/intercept_test.cpp | 2 +- .../ai/evaluation/time_to_travel_test.cpp | 3 +- .../ai/hl/stp/tactic/primitive_test.cpp | 2 +- .../embedded/constants/py_constants.py | 2 +- src/software/embedded/primitive_executor.cpp | 2 +- src/software/embedded/primitive_executor.h | 4 +- src/software/embedded/services/motor.cpp | 2 +- src/software/embedded/services/motor.h | 4 +- src/software/embedded/thunderloop.cpp | 2 +- src/software/embedded/thunderloop.h | 4 +- src/software/embedded/thunderloop_main.cpp | 2 +- src/software/er_force_simulator_main.cpp | 4 +- src/software/physics/euclidean_to_wheel.cpp | 2 +- src/software/physics/euclidean_to_wheel.h | 4 +- .../physics/euclidean_to_wheel_test.cpp | 6 +- src/software/python_bindings.cpp | 29 ++++--- .../simulated_er_force_sim_test_fixture.cpp | 2 +- .../simulation/er_force_simulator.cpp | 2 +- src/software/simulation/er_force_simulator.h | 4 +- .../simulation/er_force_simulator_test.cpp | 6 +- .../drive_and_dribbler_widget.py | 2 +- .../handheld_controller_widget.py | 2 +- src/software/world/robot.cpp | 8 +- src/software/world/robot.h | 14 ++-- src/software/world/robot_test.cpp | 2 +- 37 files changed, 145 insertions(+), 147 deletions(-) delete mode 100644 src/shared/robot_constants.cpp diff --git a/src/.bazelrc b/src/.bazelrc index 28695e559f..f8c8b2845d 100644 --- a/src/.bazelrc +++ b/src/.bazelrc @@ -75,6 +75,9 @@ build --cxxopt="-DG3_DYNAMIC_LOGGING" build --per_file_copt=.*\.pb\.(h|cc|cpp)$@-w build --per_file_copt=./external/.*,./bazel-out/.*@-w +# Define Robot Versions +build --copt="-DCURRENT_ROBOT_VERSION=2026" + ############## External Deps Compatibility ############## build --noincompatible_disallow_ctx_resolve_tools build --check_direct_dependencies=off diff --git a/src/proto/message_translation/ssl_simulation_robot_control.cpp b/src/proto/message_translation/ssl_simulation_robot_control.cpp index 145a857254..882102d676 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control.cpp +++ b/src/proto/message_translation/ssl_simulation_robot_control.cpp @@ -52,7 +52,7 @@ std::unique_ptr createRobotMoveCommand( std::unique_ptr getRobotCommandFromDirectControl( unsigned int robot_id, std::unique_ptr direct_control, - RobotConstants& robot_constants) + robot_constants::RobotConstants& robot_constants) { auto move_command = createRobotMoveCommand( *direct_control, robot_constants.front_wheel_angle_deg, diff --git a/src/proto/message_translation/ssl_simulation_robot_control.h b/src/proto/message_translation/ssl_simulation_robot_control.h index dfc3952f9f..fb4a3347aa 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control.h +++ b/src/proto/message_translation/ssl_simulation_robot_control.h @@ -34,7 +34,7 @@ std::unique_ptr createRobotMoveCommand( std::unique_ptr getRobotCommandFromDirectControl( unsigned int robot_id, std::unique_ptr direct_control, - RobotConstants& robot_constants); + robot_constants::RobotConstants& robot_constants); /** * Creates a RobotCommand proto diff --git a/src/proto/message_translation/ssl_simulation_robot_control_test.cpp b/src/proto/message_translation/ssl_simulation_robot_control_test.cpp index 0f5b84828d..0fd504c322 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control_test.cpp +++ b/src/proto/message_translation/ssl_simulation_robot_control_test.cpp @@ -7,7 +7,7 @@ class SSLSimulationProtoTest : public ::testing::Test { protected: - RobotConstants robot_constants = create2026RobotConstants(); + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); }; TEST_F(SSLSimulationProtoTest, test_create_robot_move_command_forward_from_primitive) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index a54018cf3a..641d11701d 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -426,7 +426,7 @@ std::unique_ptr createCostVisualization( std::optional createTrajectoryPathFromParams( const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, - const RobotConstants& robot_constants) + const robot_constants::RobotConstants& robot_constants) { double max_speed = convertMaxAllowedSpeedModeToMaxAllowedSpeed( params.max_speed_mode(), robot_constants); @@ -476,7 +476,7 @@ std::optional createTrajectoryPathFromParams( BangBangTrajectory1DAngular createAngularTrajectoryFromParams( const TbotsProto::TrajectoryParamsAngular1D& params, - const AngularVelocity& initial_velocity, const RobotConstants& robot_constants) + const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants) { return BangBangTrajectory1DAngular( createAngle(params.start_angle()), createAngle(params.final_angle()), @@ -489,7 +489,7 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( } int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, - RobotConstants robot_constants) + robot_constants::RobotConstants robot_constants) { switch (dribbler_mode) { @@ -507,7 +507,7 @@ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, double convertMaxAllowedSpeedModeToMaxAllowedSpeed( TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, - RobotConstants robot_constants) + robot_constants::RobotConstants robot_constants) { switch (max_allowed_speed_mode) { diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index c56045774d..1816c12d0d 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -245,7 +245,7 @@ std::unique_ptr createCostVisualization( */ std::optional createTrajectoryPathFromParams( const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, - const RobotConstants& robot_constants); + const robot_constants::RobotConstants& robot_constants); /** * Generate an angular trajectory Path given angular trajectory proto parameters @@ -257,7 +257,7 @@ std::optional createTrajectoryPathFromParams( */ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( const TbotsProto::TrajectoryParamsAngular1D& params, - const AngularVelocity& initial_velocity, const RobotConstants& robot_constants); + const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants); /** * Convert dribbler mode to dribbler speed @@ -268,7 +268,7 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( * @return the dribbler speed in RPM */ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, - RobotConstants robot_constants); + robot_constants::RobotConstants robot_constants); /** * Convert max allowed speed mode to max allowed speed @@ -280,4 +280,4 @@ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, */ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, - RobotConstants robot_constants); + robot_constants::RobotConstants robot_constants); diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 9d9958697d..62b1c45293 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -152,8 +152,7 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) // Generate a trajectory, and then generate a TbotsProto::TrajectoryPathParams2D // with the same parameters as the trajectory, finally, generate a second trajectory // from the parameters and make sure the two trajectories are equal. - RobotConstants robot_constants = create2026RobotConstants(); - Point start_position(0.0, 0.0); + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); Point start_position(0.0, 0.0); Point destination(0.0, 0.0); std::vector sub_destinations = std::get<0>(GetParam()); diff --git a/src/proto/primitive/primitive_msg_factory_test.cpp b/src/proto/primitive/primitive_msg_factory_test.cpp index 994113ecd8..fcf2ca44db 100644 --- a/src/proto/primitive/primitive_msg_factory_test.cpp +++ b/src/proto/primitive/primitive_msg_factory_test.cpp @@ -9,8 +9,7 @@ class PrimitiveFactoryTest : public testing::Test { protected: - RobotConstants robot_constants = create2026RobotConstants(); -}; + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants();}; TEST_F(PrimitiveFactoryTest, test_auto_chip_or_kick_equality) { diff --git a/src/shared/BUILD b/src/shared/BUILD index 8b77da71e1..8bb0e2e05e 100644 --- a/src/shared/BUILD +++ b/src/shared/BUILD @@ -22,7 +22,7 @@ cc_library( cc_library( name = "robot_constants", - srcs = ["robot_constants.cpp"], - hdrs = ["robot_constants.h"], + srcs = [], + hdrs = [":robot_constants.h"], deps = [":constants"], ) diff --git a/src/shared/constants.h b/src/shared/constants.h index c54b88c79e..ed94c0cde9 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -7,6 +7,8 @@ #include #include +#define CHECK_VERSION(v) (CURRENT_ROBOT_VERSION == v) + // Networking // the IPv6 multicast address, only ff02 is important, the rest is random // see https://en.wikipedia.org/wiki/Solicited-node_multicast_address for why ff02 matters @@ -68,7 +70,7 @@ static const double BALL_IN_PLAY_DISTANCE_THRESHOLD_METERS = 0.05; // The max allowed height of the robots, in metres static const double ROBOT_MAX_HEIGHT_METERS = 0.15; // The max allowed radius of the robots, in metres -static const double ROBOT_MAX_RADIUS_METERS = 0.09; +constexpr double ROBOT_MAX_RADIUS_METERS = 0.09; // The distance from the center of the robot to the front face (the flat part), in meters static const double DIST_TO_FRONT_OF_ROBOT_METERS = 0.078; // The approximate radius of the ball according to the SSL rulebook diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp deleted file mode 100644 index 0e2742a8da..0000000000 --- a/src/shared/robot_constants.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include "shared/robot_constants.h" - -#include "shared/constants.h" - -RobotConstants create2026RobotConstants() -{ - return { - .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), - .front_wheel_angle_deg = 32.0f, - .back_wheel_angle_deg = 44.0f, - - .front_of_robot_width_meters = 0.11f, - .dribbler_width_meters = 0.07825f, - - // Dribbler speeds are negative as that is the direction that sucks the ball in - .indefinite_dribbler_speed_rpm = -10000, - .max_force_dribbler_speed_rpm = -12000, - - // Motor constant - .motor_max_acceleration_m_per_s_2 = 2.0f, - - // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.0f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, - - // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, - - .wheel_radius_meters = 0.03f}; -} - -RobotConstants create2021RobotConstants() -{ - return { - .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), - .front_wheel_angle_deg = 32.06f, - .back_wheel_angle_deg = 46.04f, - - .front_of_robot_width_meters = 0.11f, - .dribbler_width_meters = 0.07825f, - - // Dribbler speeds are negative as that is the direction that sucks the ball in - .indefinite_dribbler_speed_rpm = -10000, - .max_force_dribbler_speed_rpm = -12000, - - // Motor constant - .motor_max_acceleration_m_per_s_2 = 4.5f, - - // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, - - // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, - - .wheel_radius_meters = 0.03f}; -} diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 8dd2e049df..b9cd4d7723 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -1,5 +1,10 @@ #pragma once +#include "shared/constants.h" + +namespace robot_constants +{ + struct RobotConstants { // The radius of the robot [m] @@ -72,15 +77,68 @@ struct RobotConstants }; /** - * Creates robot constants for the 2026 robot + * Creates robot constants for the robot * - * @return robot constants for the 2026 robot + * @return robot constants for the robot */ -RobotConstants create2026RobotConstants(); +#if CHECK_VERSION(2026) +constexpr RobotConstants createRobotConstants() +{ + return { + .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), + .front_wheel_angle_deg = 32.0f, + .back_wheel_angle_deg = 44.0f, + + .front_of_robot_width_meters = 0.11f, + .dribbler_width_meters = 0.07825f, + + // Dribbler speeds are negative as that is the direction that sucks the ball in + .indefinite_dribbler_speed_rpm = -10000, + .max_force_dribbler_speed_rpm = -12000, + + // Motor constant + .motor_max_acceleration_m_per_s_2 = 2.0f, + + // Robot's linear movement constants + .robot_max_speed_m_per_s = 3.0f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, + + // Robot's angular movement constants + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f}; +} +#elif CHECK_VERSION(2021) +constexpr RobotConstants createRobotConstants() +{ + return { + .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), + .front_wheel_angle_deg = 32.06f, + .back_wheel_angle_deg = 46.04f, -/** - * Creates robot constants for the 2021 robot - * - * @return robot constants for the 2021 robot - */ -RobotConstants create2021RobotConstants(); + .front_of_robot_width_meters = 0.11f, + .dribbler_width_meters = 0.07825f, + + // Dribbler speeds are negative as that is the direction that sucks the ball in + .indefinite_dribbler_speed_rpm = -10000, + .max_force_dribbler_speed_rpm = -12000, + + // Motor constant + .motor_max_acceleration_m_per_s_2 = 4.5f, + + // Robot's linear movement constants + .robot_max_speed_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, + + // Robot's angular movement constants + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f}; +} +#endif + +} diff --git a/src/software/ai/evaluation/intercept_test.cpp b/src/software/ai/evaluation/intercept_test.cpp index 0d8d979675..f283a15736 100644 --- a/src/software/ai/evaluation/intercept_test.cpp +++ b/src/software/ai/evaluation/intercept_test.cpp @@ -39,7 +39,7 @@ TEST(InterceptEvaluationTest, Ball ball({0, 0}, {6, 0}, Timestamp::fromSeconds(0)); Robot robot(0, {2, 0}, {0, 0}, Angle::zero(), AngularVelocity::zero(), Timestamp::fromSeconds(0), std::set(), - create2026RobotConstants()); + robot_constants::createRobotConstants()); // We should be able to find an intercept auto best_intercept = findBestInterceptForBall(ball, field, robot); diff --git a/src/software/ai/evaluation/time_to_travel_test.cpp b/src/software/ai/evaluation/time_to_travel_test.cpp index 123d28a280..1adfc383b6 100644 --- a/src/software/ai/evaluation/time_to_travel_test.cpp +++ b/src/software/ai/evaluation/time_to_travel_test.cpp @@ -7,8 +7,7 @@ class TimeToTravel : public ::testing::Test { protected: - RobotConstants robot_constants = create2026RobotConstants(); -}; + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants();}; TEST_F(TimeToTravel, getTimeToTravelDistance_already_at_dest) { diff --git a/src/software/ai/hl/stp/tactic/primitive_test.cpp b/src/software/ai/hl/stp/tactic/primitive_test.cpp index a0015fd7da..7b396d6bca 100644 --- a/src/software/ai/hl/stp/tactic/primitive_test.cpp +++ b/src/software/ai/hl/stp/tactic/primitive_test.cpp @@ -16,7 +16,7 @@ class PrimitiveTest : public testing::Test } protected: - RobotConstants robot_constants = create2026RobotConstants(); + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); std::shared_ptr world = TestUtil::createBlankTestingWorld(); RobotNavigationObstacleFactory obstacle_factory = diff --git a/src/software/embedded/constants/py_constants.py b/src/software/embedded/constants/py_constants.py index d0c14d17c8..db23eeb6c4 100644 --- a/src/software/embedded/constants/py_constants.py +++ b/src/software/embedded/constants/py_constants.py @@ -3,7 +3,7 @@ import software.python_bindings as tbots_cpp DEFAULT_PRIMITIVE_DURATION = 2.0 -ROBOT_CONSTANTS = tbots_cpp.create2026RobotConstants() +ROBOT_CONSTANTS = tbots_cpp.robot_constants.createRobotConstants() ROBOT_MAX_ANG_SPEED_RAD_PER_S = ROBOT_CONSTANTS.robot_max_ang_speed_rad_per_s ROBOT_MAX_SPEED_M_PER_S = ROBOT_CONSTANTS.robot_max_speed_m_per_s diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index d0ef84fe90..f485344590 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,7 +11,7 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor(const Duration time_step, - const RobotConstants& robot_constants, + const robot_constants::RobotConstants& robot_constants, const TeamColour friendly_team_colour, const RobotId robot_id) : current_primitive_(), diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index dc3c2d8db2..145874f3c2 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -21,7 +21,7 @@ class PrimitiveExecutor * @param robot_id The id of the robot which uses this primitive executor */ explicit PrimitiveExecutor(const Duration time_step, - const RobotConstants& robot_constants, + const robot_constants::RobotConstants& robot_constants, const TeamColour friendly_team_colour, const RobotId robot_id); @@ -82,7 +82,7 @@ class PrimitiveExecutor AngularVelocity angular_velocity_; Angle orientation_; TeamColour friendly_team_colour_; - RobotConstants robot_constants_; + robot_constants::RobotConstants robot_constants_; std::optional trajectory_path_; std::optional angular_trajectory_; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 5b392c1437..fafdfd461d 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -6,7 +6,7 @@ #include "software/embedded/motor_controller/tmc_motor_controller.h" #include "software/logger/logger.h" -MotorService::MotorService(const RobotConstants& robot_constants) +MotorService::MotorService(const robot_constants::RobotConstants& robot_constants) : motor_controller_(setupMotorController()), robot_constants_(robot_constants), euclidean_to_four_wheel_(robot_constants), diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 292a6b52a3..6c42f2a871 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -26,7 +26,7 @@ class MotorService * * @param robot_constants The robot constants */ - explicit MotorService(const RobotConstants& robot_constants); + explicit MotorService(const robot_constants::RobotConstants& robot_constants); virtual ~MotorService() = default; @@ -75,7 +75,7 @@ class MotorService std::unique_ptr motor_controller_; - RobotConstants robot_constants_; + robot_constants::RobotConstants robot_constants_; EuclideanToWheel euclidean_to_four_wheel_; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 1f9891a605..b238068982 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -68,7 +68,7 @@ extern "C" } } -Thunderloop::Thunderloop(const RobotConstants& robot_constants, bool enable_log_merging, +Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz) : toml_config_client_(std::make_unique(TOML_CONFIG_FILE_PATH)), motor_status_(std::nullopt), diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 685d2d564b..d6e0da78eb 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -49,7 +49,7 @@ class Thunderloop * @param enable_log_merging Whether to merge repeated log message or not * @param loop_hz The rate to run the loop */ - Thunderloop(const RobotConstants& robot_constants, bool enable_log_merging, + Thunderloop(const robot_constants::RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz); ~Thunderloop(); @@ -140,7 +140,7 @@ class Thunderloop TbotsProto::Timestamp time_sent_; // Current State - RobotConstants robot_constants_; + robot_constants::RobotConstants robot_constants_; Angle current_orientation_; int robot_id_; int channel_id_; diff --git a/src/software/embedded/thunderloop_main.cpp b/src/software/embedded/thunderloop_main.cpp index cb7c529ffd..1b6e3e5f6d 100644 --- a/src/software/embedded/thunderloop_main.cpp +++ b/src/software/embedded/thunderloop_main.cpp @@ -112,7 +112,7 @@ int main(int argc, char** argv) reserveProcessMemory(pre_allocation_size); auto thunderloop = - Thunderloop(create2026RobotConstants(), args.enable_log_merging, THUNDERLOOP_HZ); + Thunderloop(robot_constants::createRobotConstants(), args.enable_log_merging, THUNDERLOOP_HZ); thunderloop.runLoop(); return 0; diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index ecfbc7ed6b..7681503c85 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -88,12 +88,12 @@ int main(int argc, char** argv) if (args.division == "div_a") { er_force_sim = std::make_shared( - TbotsProto::FieldType::DIV_A, create2026RobotConstants(), realism_config); + TbotsProto::FieldType::DIV_A, robot_constants::createRobotConstants(), realism_config); } else { er_force_sim = std::make_shared( - TbotsProto::FieldType::DIV_B, create2026RobotConstants(), realism_config); + TbotsProto::FieldType::DIV_B, robot_constants::createRobotConstants(), realism_config); } std::mutex simulator_mutex; diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 1c42c17512..1b9cde8dac 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -8,7 +8,7 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" -EuclideanToWheel::EuclideanToWheel(const RobotConstants& robot_constants) +EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants) : robot_constants_(robot_constants) { // Phi, the angle between the hemisphere line of the robot and the front wheel axles diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index bd8ead3fdc..b1fe981abe 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -41,7 +41,7 @@ class EuclideanToWheel * * @param robot_constants The constants of the robot we are computing for. */ - explicit EuclideanToWheel(const RobotConstants& robot_constants); + explicit EuclideanToWheel(const robot_constants::RobotConstants& robot_constants); /** * Gets the wheel velocity from the Euclidean velocity. @@ -75,7 +75,7 @@ class EuclideanToWheel const double& time_to_ramp); private: - const RobotConstants robot_constants_; + const robot_constants::RobotConstants robot_constants_; /** * Euclidean velocity to wheel velocity coupling matrix. diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index 7ce54bace7..62c8d50932 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -12,8 +12,8 @@ class EuclideanToWheelTest : public ::testing::Test EuclideanSpace_t target_euclidean_velocity{}; WheelSpace_t expected_wheel_speeds{}; WheelSpace_t calculated_wheel_speeds{}; - RobotConstants robot_constants = create2026RobotConstants(); - double robot_radius = create2026RobotConstants().robot_radius_m; + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); + double robot_radius = robot_constants::createRobotConstants().robot_radius_m; WheelSpace_t target_wheel_velocity{}; WheelSpace_t current_wheel_velocity{}; @@ -22,7 +22,7 @@ class EuclideanToWheelTest : public ::testing::Test EuclideanToWheel euclidean_to_four_wheel = - EuclideanToWheel(create2026RobotConstants()); + EuclideanToWheel(robot_constants::createRobotConstants()); }; TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_zero) diff --git a/src/software/python_bindings.cpp b/src/software/python_bindings.cpp index a89691c04d..5dcbea48ac 100644 --- a/src/software/python_bindings.cpp +++ b/src/software/python_bindings.cpp @@ -273,28 +273,27 @@ PYBIND11_MODULE(python_bindings, m) return stream.str(); }); - py::class_(m, "RobotConstants") + py::class_(m, "RobotConstants") .def_readwrite("max_force_dribbler_speed_rpm", - &RobotConstants::max_force_dribbler_speed_rpm) - .def_readwrite("robot_radius_m", &RobotConstants::robot_radius_m) - .def_readwrite("front_wheel_angle_deg", &RobotConstants::front_wheel_angle_deg) - .def_readwrite("back_wheel_angle_deg", &RobotConstants::back_wheel_angle_deg) + &robot_constants::RobotConstants::max_force_dribbler_speed_rpm) + .def_readwrite("robot_radius_m", &robot_constants::RobotConstants::robot_radius_m) + .def_readwrite("front_wheel_angle_deg", &robot_constants::RobotConstants::front_wheel_angle_deg) + .def_readwrite("back_wheel_angle_deg", &robot_constants::RobotConstants::back_wheel_angle_deg) .def_readwrite("front_of_robot_width_meters", - &RobotConstants::front_of_robot_width_meters) - .def_readwrite("dribbler_width_meters", &RobotConstants::dribbler_width_meters) + &robot_constants::RobotConstants::front_of_robot_width_meters) + .def_readwrite("dribbler_width_meters", &robot_constants::RobotConstants::dribbler_width_meters) .def_readwrite("robot_max_acceleration_m_per_s_2", - &RobotConstants::robot_max_acceleration_m_per_s_2) + &robot_constants::RobotConstants::robot_max_acceleration_m_per_s_2) .def_readwrite("robot_max_ang_acceleration_rad_per_s_2", - &RobotConstants::robot_max_ang_acceleration_rad_per_s_2) + &robot_constants::RobotConstants::robot_max_ang_acceleration_rad_per_s_2) .def_readwrite("indefinite_dribbler_speed_rpm", - &RobotConstants::indefinite_dribbler_speed_rpm) - .def_readwrite("wheel_radius_meters", &RobotConstants::wheel_radius_meters) + &robot_constants::RobotConstants::indefinite_dribbler_speed_rpm) + .def_readwrite("wheel_radius_meters", &robot_constants::RobotConstants::wheel_radius_meters) .def_readwrite("robot_max_speed_m_per_s", - &RobotConstants::robot_max_speed_m_per_s) + &robot_constants::RobotConstants::robot_max_speed_m_per_s) .def_readwrite("robot_max_ang_speed_rad_per_s", - &RobotConstants::robot_max_ang_speed_rad_per_s); - m.def("create2026RobotConstants", &create2026RobotConstants); - m.def("create2021RobotConstants", &create2021RobotConstants); + &robot_constants::RobotConstants::robot_max_ang_speed_rad_per_s); + m.def("createRobotConstants", &robot_constants::createRobotConstants); m.def("createPoint", &createPoint); m.def("createPolygon", &createPolygon); diff --git a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp index 63181a6d44..10c849163d 100644 --- a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp +++ b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp @@ -181,7 +181,7 @@ void SimulatedErForceSimTestFixture::runTest( auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator(std::make_shared( - field_type, create2026RobotConstants(), realism_config, ramping)); + field_type, robot_constants::createRobotConstants(), realism_config, ramping)); // TODO (#2419): remove this to re-enable sigfpe checks fedisableexcept(FE_INVALID | FE_OVERFLOW); diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 69a45069fa..7a645b85e1 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -17,7 +17,7 @@ #include "software/world/robot_state.h" ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, - const RobotConstants& robot_constants, + const robot_constants::RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping, double primitive_executor_time_step) diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 11485b76ae..80459c4df4 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -28,7 +28,7 @@ class ErForceSimulator * @param realism_config realism configuration */ explicit ErForceSimulator(const TbotsProto::FieldType& field_type, - const RobotConstants& robot_constants, + const robot_constants::RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping = false, double primitive_executor_time_step_s = @@ -222,7 +222,7 @@ class ErForceSimulator std::unique_ptr er_force_sim; EuclideanToWheel euclidean_to_four_wheel; - RobotConstants robot_constants; + robot_constants::RobotConstants robot_constants; Field field; std::optional blue_robot_with_ball; diff --git a/src/software/simulation/er_force_simulator_test.cpp b/src/software/simulation/er_force_simulator_test.cpp index 2b28b24101..1f7297efdd 100644 --- a/src/software/simulation/er_force_simulator_test.cpp +++ b/src/software/simulation/er_force_simulator_test.cpp @@ -24,7 +24,7 @@ class ErForceSimulatorTest : public ::testing::Test } std::shared_ptr simulator; - RobotConstants robot_constants = create2026RobotConstants(); + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); }; TEST_F(ErForceSimulatorTest, set_ball_state_when_ball_does_not_already_exist) @@ -309,7 +309,7 @@ TEST_F(ErForceSimulatorTest, yellow_robot_add_robots_and_change_position) TEST(ErForceSimulatorFieldTest, check_field_A_configuration) { - RobotConstants robot_constants = create2026RobotConstants(); + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_A, robot_constants, realism_config); @@ -321,7 +321,7 @@ TEST(ErForceSimulatorFieldTest, check_field_A_configuration) TEST(ErForceSimulatorFieldTest, check_field_B_configuration) { - RobotConstants robot_constants = create2026RobotConstants(); + robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_B, robot_constants, realism_config); diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index cd1a2567f6..e2f666a5bc 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -32,7 +32,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.constants = tbots_cpp.create2026RobotConstants() + self.constants = tbots_cpp.robot_constants.createRobotConstants() self.enabled = True self.control_mode = ControlMode.VELOCITY diff --git a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py index 4d9bd208e6..a57600af60 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py @@ -37,7 +37,7 @@ def __init__(self) -> None: """Initialize the HandheldControllerWidget.""" super().__init__() - self.constants = tbots_cpp.create2026RobotConstants() + self.constants = tbots_cpp.robot_constants.createRobotConstants() self.handheld_controller: HandheldController | None = None diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 92c1fa3d21..66e2244d76 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -8,7 +8,7 @@ Robot::Robot(RobotId id, const Point& position, const Vector& velocity, const Angle& orientation, const AngularVelocity& angular_velocity, const Timestamp& timestamp, bool breakbeam_tripped, const std::set& unavailable_capabilities, - const RobotConstants& robot_constants) + const robot_constants::RobotConstants& robot_constants) : id_(id), current_state_(position, velocity, orientation, angular_velocity, breakbeam_tripped), @@ -22,7 +22,7 @@ Robot::Robot(RobotId id, const Point& position, const Vector& velocity, const Angle& orientation, const AngularVelocity& angular_velocity, const Timestamp& timestamp, const std::set& unavailable_capabilities, - const RobotConstants& robot_constants) + const robot_constants::RobotConstants& robot_constants) : id_(id), current_state_(position, velocity, orientation, angular_velocity, false), timestamp_(timestamp), @@ -33,7 +33,7 @@ Robot::Robot(RobotId id, const Point& position, const Vector& velocity, Robot::Robot(RobotId id, const RobotState& initial_state, const Timestamp& timestamp, const std::set& unavailable_capabilities, - const RobotConstants& robot_constants) + const robot_constants::RobotConstants& robot_constants) : id_(id), current_state_(initial_state), timestamp_(timestamp), @@ -171,7 +171,7 @@ std::set& Robot::getMutableRobotCapabilities() return unavailable_capabilities_; } -const RobotConstants& Robot::robotConstants() const +const robot_constants::RobotConstants& Robot::robotConstants() const { return robot_constants_; } diff --git a/src/software/world/robot.h b/src/software/world/robot.h index 2f8373d368..32c2145b74 100644 --- a/src/software/world/robot.h +++ b/src/software/world/robot.h @@ -35,7 +35,7 @@ class Robot const Timestamp& timestamp, bool breakbeam_tripped = false, const std::set& unavailable_capabilities = std::set(), - const RobotConstants& robot_constants = DEFAULT_ROBOT_CONSTANTS); + const robot_constants::RobotConstants& robot_constants = DEFAULT_ROBOT_CONSTANTS); /** * Creates a new robot given robot data @@ -55,7 +55,7 @@ class Robot const Angle& orientation, const AngularVelocity& angular_velocity, const Timestamp& timestamp, const std::set& unavailable_capabilities, - const RobotConstants& robot_constants); + const robot_constants::RobotConstants& robot_constants); /** @@ -72,7 +72,7 @@ class Robot const Timestamp& timestamp, const std::set& unavailable_capabilities = std::set(), - const RobotConstants& robot_constants = DEFAULT_ROBOT_CONSTANTS); + const robot_constants::RobotConstants& robot_constants = DEFAULT_ROBOT_CONSTANTS); /** @@ -183,7 +183,7 @@ class Robot * * @return the robot constants for this robot */ - const RobotConstants& robotConstants() const; + const robot_constants::RobotConstants& robotConstants() const; /** * Decides if a point is near the dribbler of the robot @@ -269,9 +269,9 @@ class Robot // The hardware capabilities of the robot, generated from // RobotCapabilityFlags::broken_dribblers/chippers/kickers dynamic parameters std::set unavailable_capabilities_; - RobotConstants robot_constants_; + robot_constants::RobotConstants robot_constants_; // Default robot constants that should be used for all robots - inline static const RobotConstants DEFAULT_ROBOT_CONSTANTS = - create2026RobotConstants(); + inline static const robot_constants::RobotConstants DEFAULT_ROBOT_CONSTANTS = + robot_constants::createRobotConstants(); }; diff --git a/src/software/world/robot_test.cpp b/src/software/world/robot_test.cpp index 4b4021bfcf..83af20297f 100644 --- a/src/software/world/robot_test.cpp +++ b/src/software/world/robot_test.cpp @@ -372,7 +372,7 @@ TEST_F(RobotTest, Angle target_angle = Angle::fromDegrees(-45.0); Robot robot(0, {1, 1}, Vector(0, 0), Angle::fromDegrees(45.0), AngularVelocity::fromDegrees(0), Timestamp::fromSeconds(0), {}, - create2026RobotConstants()); + robot_constants::createRobotConstants()); // Figure out a lower bound on the time required, based on us being able to constantly // accelerate at the max acceleration