From d265b55fdbbf13e543b7a816b96ee54408aebc9b Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Fri, 20 Mar 2026 10:17:51 +0100 Subject: [PATCH 1/1] Initial commit --- .gitmodules | 3 + .screen-startup | 6 + CMakeLists.txt | 50 ++ LICENSE | 17 + avr/motor_ctrl/.gitignore | 8 + avr/motor_ctrl/Makefile | 439 ++++++++++++ avr/motor_ctrl/fuses.txt | 5 + avr/motor_ctrl/main.c | 1250 +++++++++++++++++++++++++++++++++++ avr/motor_ctrl/main.hex | 747 +++++++++++++++++++++ avr/motor_ctrl/ringbuffer.c | 4 + avr/motor_ctrl/ringbuffer.h | 36 + avr/motor_ctrl/uart.c | 97 +++ avr/motor_ctrl/uart.h | 18 + docker/Dockerfile | 6 + docker/build.sh | 7 + docker/entrypoint.sh | 6 + package.xml | 32 + params/wiimote.config.yaml | 16 + scripts/engine_man_test.py | 23 + scripts/max44009_light.py | 25 + scripts/oled_ssd1780.py | 142 ++++ scripts/pyshared | 1 + scripts/step_response.py | 32 + src/hw_node.cpp | 277 ++++++++ 24 files changed, 3247 insertions(+) create mode 100644 .gitmodules create mode 100644 .screen-startup create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 avr/motor_ctrl/.gitignore create mode 100644 avr/motor_ctrl/Makefile create mode 100644 avr/motor_ctrl/fuses.txt create mode 100644 avr/motor_ctrl/main.c create mode 100644 avr/motor_ctrl/main.hex create mode 100644 avr/motor_ctrl/ringbuffer.c create mode 100644 avr/motor_ctrl/ringbuffer.h create mode 100644 avr/motor_ctrl/uart.c create mode 100644 avr/motor_ctrl/uart.h create mode 100644 docker/Dockerfile create mode 100755 docker/build.sh create mode 100755 docker/entrypoint.sh create mode 100644 package.xml create mode 100644 params/wiimote.config.yaml create mode 100755 scripts/engine_man_test.py create mode 100755 scripts/max44009_light.py create mode 100755 scripts/oled_ssd1780.py create mode 160000 scripts/pyshared create mode 100755 scripts/step_response.py create mode 100644 src/hw_node.cpp diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..4a47d86 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "scripts/pyshared"] + path = scripts/pyshared + url = ssh://erik@defiant.homedns.org/home/erik_alt/git/pyshared.git diff --git a/.screen-startup b/.screen-startup new file mode 100644 index 0000000..5f9573f --- /dev/null +++ b/.screen-startup @@ -0,0 +1,6 @@ +# crontab: +# @reboot screen -d -m -c ~/ros2_ws/src/a4wd3/.screen-startup + +source $HOME/.screenrc + +screen 0 zsh -is eval 'ros2 run a4wd3 hw_node --ros-args --log-level info' diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..81637a4 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(a4wd3) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +add_executable(hw_node src/hw_node.cpp) +ament_target_dependencies(hw_node rclcpp std_msgs sensor_msgs geometry_msgs std_srvs nav_msgs tf2_geometry_msgs) +target_include_directories(hw_node PUBLIC + $ + $) +target_link_libraries(hw_node i2c) +target_compile_features(hw_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS hw_node + DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params + DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY maps + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/avr/motor_ctrl/.gitignore b/avr/motor_ctrl/.gitignore new file mode 100644 index 0000000..9cb13a5 --- /dev/null +++ b/avr/motor_ctrl/.gitignore @@ -0,0 +1,8 @@ +.dep +*.eep +*.elf +*.lss +*.lst +*.map +*.o +*.sym diff --git a/avr/motor_ctrl/Makefile b/avr/motor_ctrl/Makefile new file mode 100644 index 0000000..9ad9dd1 --- /dev/null +++ b/avr/motor_ctrl/Makefile @@ -0,0 +1,439 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# WinAVR makefile written by Eric B. Weddington, Jörg Wunsch, et al. +# Released to the Public Domain +# Please read the make user manual! +# +# Additional material for this makefile was submitted by: +# Tim Henigan +# Peter Fleury +# Reiner Patommel +# Sander Pool +# Frederik Rouleau +# Markus Pfaff +# +# On command line: +# +# make all = Make software. +# +# make clean = Clean out built project files. +# +# make coff = Convert ELF to AVR COFF (for use with AVR Studio 3.x or VMLAB). +# +# make extcoff = Convert ELF to AVR Extended COFF (for use with AVR Studio +# 4.07 or greater). +# +# make program = Download the hex file to the device, using avrdude. Please +# customize the avrdude settings below first! +# +# make filename.s = Just compile filename.c into the assembler code only +# +# To rebuild project do "make clean" then "make all". +# + +# mth 2004/09 +# Differences from WinAVR 20040720 sample: +# - DEPFLAGS according to Eric Weddingtion's fix (avrfreaks/gcc-forum) +# - F_OSC Define in CFLAGS and AFLAGS + + +# MCU name +MCU = atmega32 + +# Main Oscillator Frequency +# This is only used to define F_OSC in all assembler and c-sources. +F_OSC = 3686400 + +# Output format. (can be srec, ihex, binary) +FORMAT = ihex + +# Target file name (without extension). +TARGET = main + + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c uart.c ringbuffer.c + + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + + + +# Optimization level, can be [0, 1, 2, 3, s]. +# 0 = turn off optimization. s = optimize for size. +# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) +OPT = s + +# Debugging format. +# Native formats for AVR-GCC's -g are stabs [default], or dwarf-2. +# AVR (extended) COFF requires stabs, plus an avr-objcopy run. +DEBUG = stabs +#DEBUG = dwarf-2 + +# List any extra directories to look for include files here. +# Each directory must be seperated by a space. +EXTRAINCDIRS = + + +# Compiler flag to set the C Standard level. +# c89 - "ANSI" C +# gnu89 - c89 plus GCC extensions +# c99 - ISO C99 standard (not yet fully implemented) +# gnu99 - c99 plus GCC extensions +CSTANDARD = -std=gnu99 + +# Place -D or -U options here +CDEFS = -DF_CPU=16000000 + +# Place -I options here +CINCS = -Ii2c/ + + +# Compiler flags. +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CFLAGS = -g$(DEBUG) +CFLAGS += $(CDEFS) $(CINCS) +CFLAGS += -O$(OPT) +CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums +CFLAGS += -Wall -Wstrict-prototypes +CFLAGS += -Wa,-adhlns=$(<:.c=.lst) +CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +CFLAGS += $(CSTANDARD) +CFLAGS += -DF_OSC=$(F_OSC) + + + +# Assembler flags. +# -Wa,...: tell GCC to pass this to the assembler. +# -ahlms: create listing +# -gstabs: have the assembler create line number information; note that +# for use in COFF files, additional information about filenames +# and function names needs to be present in the assembler source +# files -- see avr-libc docs [FIXME: not yet described there] +ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs +ASFLAGS += -DF_OSC=$(F_OSC) + + +#Additional libraries. + +# Minimalistic printf version +PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min + +# Floating point printf version (requires MATH_LIB = -lm below) +PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt -lm + +PRINTF_LIB = + +# Minimalistic scanf version +SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min + +# Floating point + %[ scanf version (requires MATH_LIB = -lm below) +SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt + +SCANF_LIB = + +MATH_LIB = -lm + +# External memory options + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# used for variables (.data/.bss) and heap (malloc()). +#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# only used for heap (malloc()). +#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff + +EXTMEMOPTS = + +# Linker flags. +# -Wl,...: tell GCC to pass this to linker. +# -Map: create map file +# --cref: add cross reference to map file +LDFLAGS = -Wl,-Map=$(TARGET).map,--cref +LDFLAGS += $(EXTMEMOPTS) +LDFLAGS += $(PRINTF_LIB_MIN) $(SCANF_LIB) $(MATH_LIB) + + + + +# Programming support using avrdude. Settings and variables. + +# Programming hardware: alf avr910 avrisp bascom bsd +# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500 +# +# Type: avrdude -c ? +# to get a full listing. +# +#AVRDUDE_PROGRAMMER = avr911 +AVRDUDE_PROGRAMMER = avrisp2 + +# com1 = serial port. Use lpt1 to connect to parallel port. +#AVRDUDE_PORT = /dev/ttyUSB0 # programmer connected to serial device +AVRDUDE_PORT = usb # programmer connected to serial device + +AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex +#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep + + +# Uncomment the following if you want avrdude's erase cycle counter. +# Note that this counter needs to be initialized first using -Yn, +# see avrdude manual. +#AVRDUDE_ERASE_COUNTER = -y + +# Uncomment the following if you do /not/ wish a verification to be +# performed after programming the device. +#AVRDUDE_NO_VERIFY = -V + +# Increase verbosity level. Please use this when submitting bug +# reports about avrdude. See +# to submit bug reports. +#AVRDUDE_VERBOSE = -v -v + +AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) +AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY) +AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE) +AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER) + + + +# --------------------------------------------------------------------------- + +# Define directories, if needed. +DIRAVR = c:/winavr +DIRAVRBIN = $(DIRAVR)/bin +DIRAVRUTILS = $(DIRAVR)/utils/bin +DIRINC = . +DIRLIB = $(DIRAVR)/avr/lib + + +# Define programs and commands. +SHELL = sh +CC = avr-gcc +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump +SIZE = avr-size +NM = avr-nm +AVRDUDE = avrdude +REMOVE = rm -f +COPY = cp + + + + +# Define Messages +# English +MSG_ERRORS_NONE = Errors: none +MSG_BEGIN = -------- begin -------- +MSG_END = -------- end -------- +MSG_SIZE_BEFORE = Size before: +MSG_SIZE_AFTER = Size after: +MSG_COFF = Converting to AVR COFF: +MSG_EXTENDED_COFF = Converting to AVR Extended COFF: +MSG_FLASH = Creating load file for Flash: +MSG_EEPROM = Creating load file for EEPROM: +MSG_EXTENDED_LISTING = Creating Extended Listing: +MSG_SYMBOL_TABLE = Creating Symbol Table: +MSG_LINKING = Linking: +MSG_COMPILING = Compiling: +MSG_ASSEMBLING = Assembling: +MSG_CLEANING = Cleaning project: + + + + +# Define all object files. +OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) + +# Define all listing files. +LST = $(ASRC:.S=.lst) $(SRC:.c=.lst) + + +# Compiler flags to generate dependency files. +### GENDEPFLAGS = -Wp,-M,-MP,-MT,$(*F).o,-MF,.dep/$(@F).d +GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d + +# Combine all necessary flags and optional flags. +# Add target processor to flags. +ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS) +ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) + + + + + +# Default target. +all: begin gccversion sizebefore build sizeafter finished end + +build: elf hex eep lss sym + +elf: $(TARGET).elf +hex: $(TARGET).hex +eep: $(TARGET).eep +lss: $(TARGET).lss +sym: $(TARGET).sym + + + +# Eye candy. +# AVR Studio 3.x does not check make's exit code but relies on +# the following magic strings to be generated by the compile job. +begin: + @echo + @echo $(MSG_BEGIN) + +finished: + @echo $(MSG_ERRORS_NONE) + +end: + @echo $(MSG_END) + @echo + + +# Display size of file. +HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex +ELFSIZE = $(SIZE) -A $(TARGET).elf +sizebefore: + @if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi + +sizeafter: + @if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi + + + +# Display compiler version information. +gccversion: + @$(CC) --version + + + +# Program the device. +program: $(TARGET).hex $(TARGET).eep + ../../scripts/pyshared/bootloader.py -b -j 0x50 $(TARGET).hex + +isp: $(TARGET).hex $(TARGET).eep + $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) + + +# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB. +COFFCONVERT=$(OBJCOPY) --debugging \ +--change-section-address .data-0x800000 \ +--change-section-address .bss-0x800000 \ +--change-section-address .noinit-0x800000 \ +--change-section-address .eeprom-0x810000 + + +coff: $(TARGET).elf + @echo + @echo $(MSG_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-avr $< $(TARGET).cof + + +extcoff: $(TARGET).elf + @echo + @echo $(MSG_EXTENDED_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof + + + +# Create final output files (.hex, .eep) from ELF output file. +%.hex: %.elf + @echo + @echo $(MSG_FLASH) $@ + $(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@ + +%.eep: %.elf + @echo + @echo $(MSG_EEPROM) $@ + -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ + --change-section-lma .eeprom=0 -O $(FORMAT) $< $@ + +# Create extended listing file from ELF output file. +%.lss: %.elf + @echo + @echo $(MSG_EXTENDED_LISTING) $@ + $(OBJDUMP) -h -S $< > $@ + +# Create a symbol table from ELF output file. +%.sym: %.elf + @echo + @echo $(MSG_SYMBOL_TABLE) $@ + $(NM) -n $< > $@ + + + +# Link: create ELF output file from object files. +.SECONDARY : $(TARGET).elf +.PRECIOUS : $(OBJ) +%.elf: $(OBJ) + @echo + @echo $(MSG_LINKING) $@ + $(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS) + + +# Compile: create object files from C source files. +%.o : %.c + @echo + @echo $(MSG_COMPILING) $< + $(CC) -c $(ALL_CFLAGS) $< -o $@ + + +# Compile: create assembler files from C source files. +%.s : %.c + $(CC) -S $(ALL_CFLAGS) $< -o $@ + + +# Assemble: create object files from assembler source files. +%.o : %.S + @echo + @echo $(MSG_ASSEMBLING) $< + $(CC) -c $(ALL_ASFLAGS) $< -o $@ + + + +# Target: clean project. +clean: begin clean_list finished end + +clean_list : + @echo + @echo $(MSG_CLEANING) + $(REMOVE) $(TARGET).hex + $(REMOVE) $(TARGET).eep + $(REMOVE) $(TARGET).obj + $(REMOVE) $(TARGET).cof + $(REMOVE) $(TARGET).elf + $(REMOVE) $(TARGET).map + $(REMOVE) $(TARGET).obj + $(REMOVE) $(TARGET).a90 + $(REMOVE) $(TARGET).sym + $(REMOVE) $(TARGET).lnk + $(REMOVE) $(TARGET).lss + $(REMOVE) $(OBJ) + $(REMOVE) $(LST) + $(REMOVE) $(SRC:.c=.s) + $(REMOVE) $(SRC:.c=.d) + $(REMOVE) .dep/* + + + +# Include the dependency files. +-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*) + + +# Listing of phony targets. +.PHONY : all begin finish end sizebefore sizeafter gccversion \ +build elf hex eep lss sym coff extcoff \ +clean clean_list program + diff --git a/avr/motor_ctrl/fuses.txt b/avr/motor_ctrl/fuses.txt new file mode 100644 index 0000000..396aa90 --- /dev/null +++ b/avr/motor_ctrl/fuses.txt @@ -0,0 +1,5 @@ +https://www.engbedded.com/fusecalc/ + +avrdude -p atmega32 -P usb -c avrisp2 -U lfuse:r:-:i -U hfuse:r:-:i + +avrdude -p atmega32 -P usb -c avrisp2 -U lfuse:w:0x3f:m -U hfuse:w:0xca:m diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c new file mode 100644 index 0000000..30cadeb --- /dev/null +++ b/avr/motor_ctrl/main.c @@ -0,0 +1,1250 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "uart.h" + +/* + * I2C Register Map (8 Bit) + * 0x00 Register select + * 0x01 Motor 1 PWM MSB + * 0x02 Motor 1 PWM LSB + * 0x03 Motor 2 PWM MSB + * 0x04 Motor 2 PWM LSB + * 0x05 Motor 3 PWM MSB + * 0x06 Motor 3 PWM LSB + * 0x07 Motor 4 PWM MSB + * 0x08 Motor 4 PWM LSB + * free + * 0x10 Hall 1 MSB + * 0x11 Hall 1 LSB + * 0x12 Hall 2 MSB + * 0x13 Hall 2 LSB + * 0x14 Hall 3 MSB + * 0x15 Hall 3 LSB + * 0x16 Hall 4 MSB + * 0x17 Hall 4 LSB + * free + * 0x20 Motor 1 speed wish MSB + * 0x21 Motor 1 speed wish LSB + * 0x22 Motor 2 speed wish MSB + * 0x23 Motor 2 speed wish LSB + * 0x24 Motor 3 speed wish MSB + * 0x25 Motor 3 speed wish LSB + * 0x26 Motor 4 speed wish MSB + * 0x27 Motor 4 speed wish LSB + * 0x28 Left speed wish (m/s) MSB + * 0x29 Left speed wish (m/s) + * 0x2A Left speed wish (m/s) + * 0x2B Left speed wish (m/s) LSB + * 0x2C Right speed wish (m/s) MSB + * 0x2D Right speed wish (m/s) + * 0x2E Right speed wish (m/s) + * 0x2F Right speed wish (m/s) LSB + * 0x30 Motor 1 speed MSB + * 0x31 Motor 1 speed LSB + * 0x32 Motor 2 speed MSB + * 0x33 Motor 2 speed LSB + * 0x34 Motor 3 speed MSB + * 0x35 Motor 3 speed LSB + * 0x36 Motor 4 speed MSB + * 0x37 Motor 4 speed LSB + * 0x38 Speed (m/s) MSB + * 0x39 Speed (m/s) + * 0x3A Speed (m/s) + * 0x3B Speed (m/s) LSB + * 0x3C Angle (rad/s) MSB + * 0x3D Angle (rad/s) + * 0x3E Angle (rad/s) + * 0x3F Angle (rad/s) LSB + * 0x40 Position x (m) MSB + * 0x41 Position x (m) + * 0x42 Position x (m) + * 0x43 Position x (m) LSB + * 0x44 Position y (m) MSB + * 0x45 Position y (m) + * 0x46 Position y (m) + * 0x47 Position y (m) LSB + * 0x48 Position angle MSB + * 0x49 Position angle + * 0x4A Position angle + * 0x4B Position angle LSB + * free + * 0x50 speed wish (m/s) MSB + * 0x51 speed wish (m/s) + * 0x52 speed wish (m/s) + * 0x53 speed wish (m/s) LSB + * 0x54 angle wish (rad/s) MSB + * 0x55 angle wish (rad/s) + * 0x56 angle wish (rad/s) + * 0x57 angle wish (rad/s) LSB + * free + * 0x90 Motor 1 switch + * 0x91 Motor 2 switch + * 0x92 Motor 3 switch + * 0x93 Motor 4 switch + * 0x94 Front Handicap + * 0x95 Aft Handicap + * free + * 0xA0 Reset reason + * 0xA1 Error status + * 0xA2 count test + * 0xA3 last i2c status before boot + * 0xA4 Watchdog enable + * free + * 0xB0 Sound HZ MSB + * 0xB1 Sound HZ LSB + * free + * 0xff Bootloader + */ + + +#define KP 0.010499758766982191 +#define KI 1.049975876698219 +#define KD 0.0 +#define PID_T 0.01 +// wheel diameter=15.8cm, encoder=12*4cpr, gear ratio=1:51 +// STEP_PER_M = 48*51/(d*pi) +// Left real diameter: 0.12808, Right real diameter: 0.121 +#define STEP_PER_M 4969.2 +#define STEP_PER_M_LEFT (STEP_PER_M) +#define STEP_PER_M_RIGHT (STEP_PER_M) +#define WHEEL_DIST 0.49066 // Measured: 0.36 +#define PWM_BREAK INT16_MIN +#define STALL_LIMIT 140000 +#define I2C_TIMEOUT_DISABLE 255 + +#define TWI_ACK TWCR = (1<>8; + TWI_ACK; + break; + case 0x11: // Hall 1 LSB + TWDR = tmp16; + TWI_ACK; + break; + case 0x12: // Hall 2 MSB + tmp16 = pos2; + TWDR = tmp16>>8; + TWI_ACK; + break; + case 0x13: // Hall 2 LSB + TWDR = tmp16; + TWI_ACK; + break; + case 0x14: // Hall 3 MSB + tmp16 = pos3; + TWDR = tmp16>>8; + TWI_ACK; + break; + case 0x15: // Hall 3 LSB + TWDR = tmp16; + TWI_ACK; + break; + case 0x16: // Hall 4 MSB + tmp16 = pos4; + TWDR = tmp16>>8; + TWI_ACK; + break; + case 0x17: // Hall 4 LSB + TWDR = tmp16; + TWI_ACK; + break; + case 0x20: // Motor 1 speed wish MSB + TWDR = speed1_wish>>8; + TWI_ACK; + break; + case 0x21: // Motor 1 speed wish LSB + TWDR = speed1_wish; + TWI_ACK; + break; + case 0x22: // Motor 2 speed wish MSB + TWDR = speed2_wish>>8; + TWI_ACK; + break; + case 0x23: // Motor 2 speed wish LSB + TWDR = speed2_wish; + TWI_ACK; + break; + case 0x24: // Motor 3 speed wish MSB + TWDR = speed3_wish>>8; + TWI_ACK; + break; + case 0x25: // Motor 3 speed wish LSB + TWDR = speed3_wish; + TWI_ACK; + break; + case 0x26: // Motor 4 speed wish MSB + TWDR = speed4_wish>>8; + TWI_ACK; + break; + case 0x27: // Motor 4 speed wish LSB + TWDR = speed4_wish; + TWI_ACK; + break; + case 0x30: // Motor 1 speed MSB + TWDR = speed1>>8; + TWI_ACK; + break; + case 0x31: // Motor 1 speed LSB + TWDR = speed1; + TWI_ACK; + break; + case 0x32: // Motor 2 speed MSB + TWDR = speed2>>8; + TWI_ACK; + break; + case 0x33: // Motor 2 speed LSB + TWDR = speed2; + TWI_ACK; + break; + case 0x34: // Motor 3 speed MSB + TWDR = speed3>>8; + TWI_ACK; + break; + case 0x35: // Motor 3 speed LSB + TWDR = speed3; + TWI_ACK; + break; + case 0x36: // Motor 4 speed MSB + TWDR = speed4>>8; + TWI_ACK; + break; + case 0x37: // Motor 4 speed LSB + TWDR = speed4; + TWI_ACK; + break; + case 0x38: // speed MSB + tmp_speed.f = cur_speed_lin; + TWDR = tmp_speed.i>>24; + TWI_ACK; + break; + case 0x39: // speed + TWDR = tmp_speed.i>>16; + TWI_ACK; + break; + case 0x3A: // speed + TWDR = tmp_speed.i>>8; + TWI_ACK; + break; + case 0x3B: // speed LSB + TWDR = tmp_speed.i; + TWI_ACK; + break; + case 0x3C: // angle MSB + tmp_angle.f = cur_speed_rot; + TWDR = tmp_angle.i>>24; + TWI_ACK; + break; + case 0x3D: // angle + TWDR = tmp_angle.i>>16; + TWI_ACK; + break; + case 0x3E: // angle + TWDR = tmp_angle.i>>8; + TWI_ACK; + break; + case 0x3F: // angle LSB + TWDR = tmp_angle.i; + TWI_ACK; + break; + case 0x40: // Position x MSB + TWDR = pos_x.i>>24; + TWI_ACK; + break; + case 0x41: // Position x + TWDR = pos_x.i>>16; + TWI_ACK; + break; + case 0x42: // Position x + TWDR = pos_x.i>>8; + TWI_ACK; + break; + case 0x43: // Position x LSB + TWDR = pos_x.i; + TWI_ACK; + break; + case 0x44: // Position y MSB + TWDR = pos_y.i>>24; + TWI_ACK; + break; + case 0x45: // Position y + TWDR = pos_y.i>>16; + TWI_ACK; + break; + case 0x46: // Position y + TWDR = pos_y.i>>8; + TWI_ACK; + break; + case 0x47: // Position y LSB + TWDR = pos_y.i; + TWI_ACK; + break; + case 0x48: // Position angle MSB + TWDR = angle.i>>24; + TWI_ACK; + break; + case 0x49: // Position angle + TWDR = angle.i>>16; + TWI_ACK; + break; + case 0x4A: // Position angle + TWDR = angle.i>>8; + TWI_ACK; + break; + case 0x4B: // Position angle LSB + TWDR = angle.i; + TWI_ACK; + break; + case 0xA0: // Reset reason + TWDR = MCUCSR & 0x0f; + MCUCSR = 0x0; + TWI_ACK; + break; + case 0xA1: // Error status + TWDR = error_state; + TWI_ACK; + break; + case 0xA2: // count test + TWDR = count_test; + TWI_ACK; + case 0xA3: // last i2c status before boot + TWDR = last_i2c_status_boot; + TWI_ACK; + case 0xA4: // Watchdog enable + TWDR = watchdog_enable; + TWI_ACK; + default: + TWDR = 0; + TWI_NAK; + } + ireg++; + break; + case TW_SR_STOP: + TWI_ACK; + break; + case TW_NO_INFO: + break; + default: + TWI_RESET; + } + + if (watchdog_enable == 2) { + wdt_reset(); + } +} + + +static void update_hall1(void) { + unsigned char status = (PINA >> 0) & 0x3; + static unsigned char oldstatus=0; + unsigned char diff, new; + + new = 0; + if (status & 0x1) + new = 0x3; + if (status & 0x2) + new ^= 0x1; // convert gray to binary + diff = oldstatus - new; // difference last - new + if (diff & 0x1) { // bit 0 = value (1) + oldstatus = new; // store new as next last + if (motor1_switch) pos1 -= (diff & 2) - 1; // bit 1 = direction (+/-) + else pos1 += (diff & 2) - 1; + } +} + + +static void update_hall2(void) { + unsigned char status = (PINA >> 4) & 0x3; + static unsigned char oldstatus=0; + unsigned char diff, new; + + new = 0; + if (status & 0x1) + new = 0x3; + if (status & 0x2) + new ^= 0x1; // convert gray to binary + diff = oldstatus - new; // difference last - new + if (diff & 0x1) { // bit 0 = value (1) + oldstatus = new; // store new as next last + if (motor2_switch) pos2 += (diff & 2) - 1; // bit 1 = direction (+/-) + else pos2 -= (diff & 2) - 1; + } +} + + +static void update_hall3(void) { + unsigned char status = (PINA >> 2) & 0x3; + static unsigned char oldstatus=0; + unsigned char diff, new; + + new = 0; + if (status & 0x1) + new = 0x3; + if (status & 0x2) + new ^= 0x1; // convert gray to binary + diff = oldstatus - new; // difference last - new + if (diff & 0x1) { // bit 0 = value (1) + oldstatus = new; // store new as next last + if (motor3_switch) pos3 += (diff & 2) - 1; // bit 1 = direction (+/-) + else pos3 -= (diff & 2) - 1; + } +} + + +static void update_hall4(void) { + unsigned char status = (PINA >> 6) & 0x3; + static unsigned char oldstatus=0; + unsigned char diff, new; + + new = 0; + if (status & 0x1) + new = 0x3; + if (status & 0x2) + new ^= 0x1; // convert gray to binary + diff = oldstatus - new; // difference last - new + if (diff & 0x1) { // bit 0 = value (1) + oldstatus = new; // store new as next last + if (motor4_switch) pos4 -= (diff & 2) - 1; // bit 1 = direction (+/-) + else pos4 += (diff & 2) - 1; + } +} + + +static void update_motor(void) { + static int16_t m1_old=SHRT_MIN; + static int16_t m2_old=SHRT_MIN; + static int16_t m3_old=SHRT_MIN; + static int16_t m4_old=SHRT_MIN; + + error_state &= 0xf0; // clear lower bits + error_state |= ~((PIND & 0x40)>>3 | (PINB & 0x07)) & 0xf; + + if (m1_old != motor1) { // update only when changed + if (motor1 == 0) { + // stop + PORTC &= ~(1 << 3) & ~(1 << 2); + DISABLE_PWM_MOTOR1; + } else if (motor1 == PWM_BREAK) { + PORTC |= (1 << 3) | (1 << 2); + ENABLE_PWM_MOTOR1; + } else if ((!motor1_switch && motor1 > 0) || (motor1_switch && motor1 < 0)) { + // forward + uint8_t tmp=PORTC; + tmp &= ~(1 << 3); + tmp |= (1 << 2); + PORTC = tmp; + ENABLE_PWM_MOTOR1; + } else { // motor1 < 0 + // backward + uint8_t tmp=PORTC; + tmp &= ~(1 << 2); + tmp |= (1 << 3); + PORTC = tmp; + ENABLE_PWM_MOTOR1; + } + + m1_old = motor1; + OCR1A = abs(motor1); + } + + if (m2_old != motor2) { // update only when changed + if (motor2 == 0) { + // stop + PORTC &= ~(1 << 5) & ~(1 << 4); + DISABLE_PWM_MOTOR2; + } else if (motor2 == PWM_BREAK) { + PORTC |= (1 << 5) | (1 << 4); + ENABLE_PWM_MOTOR2; + } else if ((!motor2_switch && motor2 > 0) || (motor2_switch && motor2 < 0)) { + // forward + uint8_t tmp=PORTC; + tmp &= ~(1 << 5); + tmp |= (1 << 4); + PORTC = tmp; + ENABLE_PWM_MOTOR2; + } else { // motor2 < 0 + // backward + uint8_t tmp=PORTC; + tmp &= ~(1 << 4); + tmp |= (1 << 5); + PORTC = tmp; + ENABLE_PWM_MOTOR2; + } + + m2_old = motor2; + OCR1B = abs(motor2); + } + + if (m3_old != motor3) { // update only when changed + if (motor3 == 0) { + // stop + PORTC &= ~(1 << 7) & ~(1 << 6); + DISABLE_PWM_MOTOR3; + } else if (motor3 == PWM_BREAK) { + PORTC |= (1 << 7) | (1 << 6); + ENABLE_PWM_MOTOR3; + } else if ((!motor3_switch && motor3 > 0) || (motor3_switch && motor3 < 0)) { + // forward + uint8_t tmp=PORTC; + tmp &= ~(1 << 7); + tmp |= (1 << 6); + PORTC = tmp; + ENABLE_PWM_MOTOR3; + } else { // motor3 < 0 + // backward + uint8_t tmp=PORTC; + tmp &= ~(1 << 6); + tmp |= (1 << 7); + PORTC = tmp; + ENABLE_PWM_MOTOR3; + } + + m3_old = motor3; + OCR2 = abs(motor3); + } + + if (m4_old != motor4) { // update only when changed + if (motor4 == 0) { + // stop + PORTD &= ~(1 << 3) & ~(1 << 2); + DISABLE_PWM_MOTOR4; + } else if (motor4 == PWM_BREAK) { + PORTD |= (1 << 3) | (1 << 2); + ENABLE_PWM_MOTOR4; + } else if ((!motor4_switch && motor4 > 0) || (motor4_switch && motor4 < 0)) { + // forward + uint8_t tmp=PORTD; + tmp &= ~(1 << 3); + tmp |= (1 << 2); + PORTD = tmp; + ENABLE_PWM_MOTOR4; + } else { // motor4 < 0 + // backward + uint8_t tmp=PORTD; + tmp &= ~(1 << 2); + tmp |= (1 << 3); + PORTD = tmp; + ENABLE_PWM_MOTOR4; + } + + m4_old = motor4; + OCR0 = abs(motor4); + } +} + + +static void update_pos(void) { + static int16_t pos1_last=0; + static int16_t pos2_last=0; + static int16_t pos3_last=0; + static int16_t pos4_last=0; + int16_t pos1_diff; // steps + int16_t pos2_diff; + int16_t pos3_diff; + int16_t pos4_diff; + float diff_left_m, diff_right_m, angle_diff, translation; + float pos_x_new, pos_y_new, angle_new; + float tmp_speed_lin, tmp_speed_rot; + int16_t cur_pos1, cur_pos2, cur_pos3, cur_pos4; + int16_t new_speed1, new_speed2, new_speed3, new_speed4; + + // copy to tmp + cli(); + cur_pos1 = pos1; + cur_pos2 = pos2; + cur_pos3 = pos3; + cur_pos4 = pos4; + sei(); + + pos1_diff = cur_pos1 - pos1_last; + pos2_diff = cur_pos2 - pos2_last; + pos3_diff = cur_pos3 - pos3_last; + pos4_diff = cur_pos4 - pos4_last; + + new_speed1 = pos1_diff/PID_T; + new_speed2 = pos2_diff/PID_T; + new_speed3 = pos3_diff/PID_T; + new_speed4 = pos4_diff/PID_T; + + diff_left_m = (pos1_diff + pos2_diff)/(2*STEP_PER_M_LEFT); + diff_right_m = (pos3_diff + pos4_diff)/(2*STEP_PER_M_RIGHT); + angle_diff = (diff_right_m - diff_left_m) / WHEEL_DIST; + + angle_new = angle.f + angle_diff; + if (angle_new > 2*M_PI) angle_new-=2*M_PI; + else if (angle_new < -2*M_PI) angle_new+=2*M_PI; + + translation = (diff_left_m + diff_right_m)/2.0; + pos_x_new = pos_x.f + cos(angle_new)*translation; + pos_y_new = pos_y.f + sin(angle_new)*translation; + + tmp_speed_lin = translation/PID_T; + tmp_speed_rot = angle_diff/PID_T; + + // copy from tmp + cli(); + angle.f = angle_new; + pos_x.f = pos_x_new; + pos_y.f = pos_y_new; + speed1 = new_speed1; + speed2 = new_speed2; + speed3 = new_speed3; + speed4 = new_speed4; + cur_speed_lin = tmp_speed_lin; + cur_speed_rot = tmp_speed_rot; + sei(); + + pos1_last = cur_pos1; + pos2_last = cur_pos2; + pos3_last = cur_pos3; + pos4_last = cur_pos4; +} + + +static void update_pid(void) { + static int16_t eold1=0; + static int16_t eold2=0; + static int16_t eold3=0; + static int16_t eold4=0; + static int32_t esum1=0; + static int32_t esum2=0; + static int32_t esum3=0; + static int32_t esum4=0; + + // protect motors from damage if stalling + if (labs(esum1) > STALL_LIMIT && speed1 == 0) { + motor1 = 0; + motor1_mode = MOTOR_MANUAL; + error_state |= (1<<4); + esum1 = 0; + } + if (labs(esum2) > STALL_LIMIT && speed2 == 0) { + motor2 = 0; + motor2_mode = MOTOR_MANUAL; + error_state |= (1<<5); + esum2 = 0; + } + if (labs(esum3) > STALL_LIMIT && speed3 == 0) { + motor3 = 0; + motor3_mode = MOTOR_MANUAL; + error_state |= (1<<6); + esum3 = 0; + } + if (labs(esum4) > STALL_LIMIT && speed4 == 0) { + motor4 = 0; + motor4_mode = MOTOR_MANUAL; + error_state |= (1<<7); + esum4 = 0; + } + + if (motor1_mode == MOTOR_PID) { + if (speed1_wish != speed1_wish_old) { + if (abs(speed1_wish - speed1_wish_old) > 500) esum1 = 0; + speed1_wish_old = speed1_wish; + } + + uint8_t dir_change = (speed1_wish > 0 && speed1 < 0) || (speed1_wish < 0 && speed1 > 0); // Prevent dangerous immediate engine reverse + if (speed1_wish == 0 || dir_change) { + motor1 = 0; + eold1 = 0; + error_state &= ~(1<<4); + } else { + int16_t e = speed1_wish - speed1; + esum1+=e; + motor1 = KP*e + KI*PID_T*esum1 + KD/PID_T*(e - eold1); + eold1 = e; + + if (motor1 > 0 && speed1_wish < 0) motor1=PWM_BREAK; + else if (motor1 < 0 && speed1_wish > 0) motor1=PWM_BREAK; + else if (motor1 > 255) motor1 = 255; + else if (motor1 < -255) motor1 = -255; + } + } + if (motor2_mode == MOTOR_PID) { + if (speed2_wish != speed2_wish_old) { + if (abs(speed2_wish - speed2_wish_old) > 500) esum2 = 0; + speed2_wish_old = speed2_wish; + } + + uint8_t dir_change = (speed2_wish > 0 && speed2 < 0) || (speed2_wish < 0 && speed2 > 0); // Prevent dangerous immediate engine reverse + if (speed2_wish == 0 || dir_change) { + motor2 = 0; + eold2 = 0; + error_state &= ~(1<<5); + } else { + int16_t e = speed2_wish - speed2; + esum2+=e; + motor2 = KP*e + KI*PID_T*esum2 + KD/PID_T*(e - eold2); + eold2 = e; + + if (motor2 > 0 && speed2_wish < 0) motor2=PWM_BREAK; + else if (motor2 < 0 && speed2_wish > 0) motor2=PWM_BREAK; + else if (motor2 > 255) motor2 = 255; + else if (motor2 < -255) motor2 = -255; + } + } + if (motor3_mode == MOTOR_PID) { + if (speed3_wish != speed3_wish_old) { + if (abs(speed3_wish - speed3_wish_old) > 500) esum3 = 0; + speed3_wish_old = speed3_wish; + } + + uint8_t dir_change = (speed3_wish > 0 && speed3 < 0) || (speed3_wish < 0 && speed3 > 0); // Prevent dangerous immediate engine reverse + if (speed3_wish == 0 || dir_change) { + motor3 = 0; + eold3 = 0; + error_state &= ~(1<<6); + } else { + int16_t e = speed3_wish - speed3; + esum3+=e; + motor3 = KP*e + KI*PID_T*esum3 + KD/PID_T*(e - eold3); + eold3 = e; + + if (motor3 > 0 && speed3_wish < 0) motor3=PWM_BREAK; + else if (motor3 < 0 && speed3_wish > 0) motor3=PWM_BREAK; + else if (motor3 > 255) motor3 = 255; + else if (motor3 < -255) motor3 = -255; + } + } + if (motor4_mode == MOTOR_PID) { + if (speed4_wish != speed4_wish_old) { + if (abs(speed4_wish - speed4_wish_old) > 500) esum4 = 0; + speed4_wish_old = speed4_wish; + } + + uint8_t dir_change = (speed4_wish > 0 && speed4 < 0) || (speed4_wish < 0 && speed4 > 0); // Prevent dangerous immediate engine reverse + if (speed4_wish == 0 || dir_change) { + motor4 = 0; + eold4 = 0; + error_state &= ~(1<<7); + } else { + int16_t e = speed4_wish - speed4; + esum4+=e; + motor4 = KP*e + KI*PID_T*esum4 + KD/PID_T*(e - eold4); + eold4 = e; + + if (motor4 > 0 && speed4_wish < 0) motor4=PWM_BREAK; + else if (motor4 < 0 && speed4_wish > 0) motor4=PWM_BREAK; + else if (motor4 > 255) motor4 = 255; + else if (motor4 < -255) motor4 = -255; + } + } +} + + +ISR(TIMER1_OVF_vect) { + update_hall1(); + update_hall2(); + update_hall3(); + update_hall4(); + + run_update++; +} + +ISR(TIMER0_OVF_vect) { + sound_update++; + if (sound_update >= sound_max) { + sound_update=0; + motor4 = -motor4; + update_motor(); + } +} + +static void set_motor4_sound(uint16_t hz) { + if (hz) { + // 5e6/255/hz/2 + sound_max = 19608/hz/2; + sound_update=0; + // Enable Timer 0 Overflow Interrupt + TIMSK |= (1 << TOIE0); + last_man_update_count = I2C_TIMEOUT_DISABLE; + } else { + // Disable Timer 0 Overflow Interrupt + TIMSK &= ~(1 << TOIE0); + motor4 = 0; + } + +} + +int main(void) { + // Outputs + DDRB = (1 << 3); + DDRC = (1 << 7) | (1 << 6) | (1 << 5) | (1 << 4) | (1 << 3) | (1 << 2); + DDRD = (1 << 7) | (1 << 5) | (1 << 4) | (1 << 3) | (1 << 2); + // Pullup Diag/Enable + PORTB = (1 << 0) | (1 << 1) | (1 << 2); + PORTD = (1 << 6); + + bootloader = 0x00; + setup_uart(9600); + uart_setup_stdout(); + + // I2C + TWAR = 0x50; + TWI_ACK; + + // Motor 1 & 2 + // Also used for PWM frequency TIMER1_FREQ (F_CPU/256) + // Timer 1: Fast PWM non-inverting mode, Top=255 => 7.8125kHz + // Prescaler=8 + //TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM10); + // Avoid narrow spike on extreme pwm value 0 by not setting COM1*1 + TCCR1A = (1 << WGM10); + TCCR1B = (1 << WGM12) | (1 << CS11); + OCR1A = 0; + OCR1B = 0; + + // Motor 3 + // Timer 2: Fast PWM non-inverting mode, Top=255 + // Prescaler=8 + //TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << COM21) | (1 << CS20); + // Avoid narrow spike on extreme pwm value 0 by not setting COM21 + TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << CS21); + OCR2 = 0; + + // Motor 4 + // Timer 0: Fast PWM non-inverting mode, Top=255 + // Prescaler=8 + //TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << COM01) | (1 << CS00); + // Avoid narrow spike on extreme pwm value 0 by not setting COM01 + TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << CS01); + OCR0 = 0; + + printf_P(PSTR("\r\nStart\r\n")); + + last_i2c_status_boot = eeprom_read_byte((uint8_t*)1); + + set_sleep_mode(SLEEP_MODE_IDLE); + // Enable Timer 1 Overflow Interrupt + TIMSK = (1 << TOIE1); + sei(); + + while(1) { + switch(ireg) { + case 0xA4: // Watchdog enable + if (watchdog_enable == 1) { + wdt_enable(WDTO_2S); + watchdog_enable = 2; + } else if (watchdog_enable == 0) { + wdt_disable(); + watchdog_enable = 3; + } + break; + case 0xff: // Magic reg that starts the bootloader + if (bootloader == 0xa5) { + cli(); + // write mark to first area in eeprom + eeprom_write_byte((uint8_t*)0, 123); + eeprom_busy_wait(); + // Use watchdog to restart + wdt_enable(WDTO_15MS); + } + break; + } + + if (cmd_vel.bUpdate) { + float speed_wish_right, speed_wish_left; + float speed, angle; + + cli(); + speed = cmd_vel.speed; + angle = cmd_vel.angle; + cmd_vel.bUpdate = 0; + sei(); + + speed_wish_right = (angle*WHEEL_DIST)/2 + speed; + speed_wish_left = speed*2-speed_wish_right; + + speed_wish_left*=STEP_PER_M_LEFT; + speed_wish_right*=STEP_PER_M_RIGHT; + + if (aft_handicap > 0) { + speed1_wish = speed_wish_left * (100-aft_handicap)/100.0; + speed3_wish = speed_wish_right * (100-aft_handicap)/100.0; + } else { + speed1_wish = speed_wish_left; + speed3_wish = speed_wish_right; + } + if (front_handicap > 0) { + speed2_wish = speed_wish_left * (100-front_handicap)/100.0; + speed4_wish = speed_wish_right * (100-front_handicap)/100.0; + } else { + speed2_wish = speed_wish_left; + speed4_wish = speed_wish_right; + } + motor1_mode = MOTOR_PID; + motor2_mode = MOTOR_PID; + motor3_mode = MOTOR_PID; + motor4_mode = MOTOR_PID; + } + + if (run_update >= 78) { // TIMER1_FREQ/78 = ~100Hz + run_update=0; + + update_pos(); + update_pid(); + update_motor(); + count_test++; + if (last_man_update_count != I2C_TIMEOUT_DISABLE) { + last_man_update_count++; + + if (last_man_update_count >= 100) { + // ~1s without a new i2c command + cmd_vel.speed = 0; + cmd_vel.angle = 0; + cmd_vel.bUpdate = 1; + if (last_man_update_count == 100) { + //printf_P(PSTR("I2C State: 0x%x\r\n"), last_i2c_status); + eeprom_write_byte((uint8_t*)1, last_i2c_status); + eeprom_busy_wait(); + } + last_man_update_count = I2C_TIMEOUT_DISABLE; + } + } + } + + sleep_mode(); + } + + return 0; +} diff --git a/avr/motor_ctrl/main.hex b/avr/motor_ctrl/main.hex new file mode 100644 index 0000000..f5fbb5a --- /dev/null +++ b/avr/motor_ctrl/main.hex @@ -0,0 +1,747 @@ +:100000000C943B000C9458000C9458000C9458002D +:100010000C9458000C9458000C9458000C94580000 +:100020000C9458000C94EA040C9458000C94B105FC +:100030000C9458000C94ED050C9458000C94580046 +:100040000C9458000C9458000C9458000C94C20165 +:100050000C9458000D0A53746172740D0A004546E1 +:100060004765666700202B2D2E303132333435360C +:1000700037383968000011241FBECFE5D8E0DEBF55 +:10008000CDBF10E0A0E6B0E0E0E9FEE202C00590DE +:100090000D92A037B107D9F721E0A0E7B0E001C089 +:1000A0001D92AC36B207E1F70E94DD060C942616CD +:1000B0000C9400008091A300807F8093A30080B304 +:1000C00036B32091A30090E086FB8827992783F917 +:1000D0003770832B3FE08327822B8093A30080918E +:1000E000EA009091EB00209166003091670028179C +:1000F000390701F18091EA009091EB00892B09F01A +:1001000099C085B3837F85BB8FB58F778FBD809175 +:10011000EA009091EB0090936700809366008091D5 +:10012000EA009091EB0097FF03C0919581959109AA +:100130009BBD8ABD8091E8009091E9002091640008 +:10014000309165002817390701F18091E8009091FE +:10015000E900892B09F098C085B38F7C85BB8FB5EA +:100160008F7D8FBD8091E8009091E90090936500AC +:10017000809364008091E8009091E90097FF03C0AC +:1001800091958195910999BD88BD8091E6009091E6 +:10019000E700209162003091630028173907F9F0D9 +:1001A0008091E6009091E700892B09F097C085B314 +:1001B0008F7385BB85B58F7D85BD8091E60090915D +:1001C000E70090936300809362008091E600909135 +:1001D000E70097FF03C091958195910983BD8091B8 +:1001E000E4009091E50020916000309161002817B3 +:1001F0003907F9F08091E4009091E500892B09F02E +:1002000097C082B3837F82BB83B78F7D83BF80918A +:10021000E4009091E50090936100809360008091EC +:10022000E4009091E50097FF03C0919581959109B5 +:100230008CBF08958091EA009091EB008115904861 +:1002400031F485B38C6085BB8FB580685FCF8091BA +:10025000D700811107C08091EA009091EB00181639 +:10026000190654F08091D700811551F08091EA0071 +:100270009091EB0097FF04C085B3877F8460E3CF44 +:1002800085B38B7F8860DFCF8091E8009091E90093 +:100290008115904831F485B3806385BB8FB580624A +:1002A00060CF8091D600811107C08091E8009091C5 +:1002B000E9001816190654F08091D600811551F006 +:1002C0008091E8009091E90097FF04C085B38F7D8D +:1002D0008061E3CF85B38F7E8062DFCF8091E600BF +:1002E0009091E7008115904831F485B3806C85BB0F +:1002F00085B5806261CF80916900811107C08091CE +:10030000E6009091E7001816190654F080916900F4 +:10031000811551F08091E6009091E70097FF04C0AD +:1003200085B38F778064E3CF85B38F7B8068DFCF21 +:100330008091E4009091E5008115904831F482B3FA +:100340008C6082BB83B7806261CF8091680081112D +:1003500007C08091E4009091E5001816190654F04A +:1003600080916800811551F08091E4009091E50042 +:1003700097FF04C082B3877F8460E3CF82B38B7F13 +:100380008860DFCF1F920F920FB60F9211242F9328 +:100390003F934F935F936F937F938F939F93AF930D +:1003A000BF93EF93FF9381B1887F8093A10081B1C8 +:1003B000887F803A09F40BC330F48036A9F0803886 +:1003C00069F185ED05C3883B21F0883F89F0883AC3 +:1003D000C1F7E091ED00E150E43A08F0F5C2F0E039 +:1003E000E95CF94E0C945E1385EC86BF1092ED002B +:1003F00080919F00823009F4A895FF91EF91BF9101 +:10040000AF919F918F917F916F915F914F913F91AC +:100410002F910F900FBE0F901F901895E091ED0057 +:10042000E83570F0E53A88F4E03910F485E825C045 +:10043000E059E531D8F7F0E0E552F94E0C945E133F +:10044000F0E0E051F94E0C945E13E13B09F43FC13A +:10045000EF3F09F460C1E03B49F783B190E090932E +:1004600095008093940008C083B18093ED00809143 +:10047000ED0081508093ED0085EC86BF8091ED000A +:100480008F3F09F4B5CF8091ED008F5F8093ED0031 +:10049000AFCF83B190919E009093EB008093EA00E0 +:1004A0001092DB00E9CF83B190919E009093E90018 +:1004B0008093E8001092DA00DFCF83B190919E0024 +:1004C0009093E7008093E6001092D900D5CF83B1D6 +:1004D00090919E009093E5008093E4001092D800E4 +:1004E000CBCF83B190919E009093D5008093D400A0 +:1004F00081E08093DB00C0CF83B190919E00909308 +:10050000D3008093D20081E08093DA00B5CF83B12D +:1005100090919E009093D1008093D00081E08093D1 +:10052000D900AACF83B180939E00A6CF83B19091CA +:100530009E009093CF008093CE0081E08093D800FE +:100540009BCF23B180919A0090919B00A0919C0039 +:10055000B0919D00BA2FA92F982F8827822B8093C6 +:100560009A0090939B00A0939C00B0939D0084CF31 +:1005700023B160919A0070919B0080919C009091B2 +:100580009D00982F872F762F6627622B60939A0005 +:1005900070939B0080939C0090939D002AE939E41E +:1005A0004BE955E40E946D120E944111462F572FCE +:1005B0005093D5004093D4005093D3004093D20081 +:1005C00081E08093DB00A0CF83B190E0A0E0B0E0B9 +:1005D000C6CF23B180919A0090919B00A0919C007E +:1005E000B0919D00BA2FA92F982F8827822B809336 +:1005F0009A0090939B00A0939C00B0939D008093E1 +:10060000EE009093EF00A093F000B093F10034CF90 +:1006100083B190E0A0E0B0E08093960090939700C3 +:10062000A0939800B093990027CF23B180919600B2 +:1006300090919700A0919800B0919900BA2FA92F9E +:10064000982F8827822BE8CF23B180919600909134 +:100650009700A0919800B0919900BA2FA92F982FD8 +:100660008827822B8093960090939700A093980000 +:10067000B09399008093F2009093F300A093F4005C +:10068000B093F50081E08093F6001092A200F4CEC2 +:1006900083B18093D700F0CE83B18093D600ECCEA7 +:1006A00083B180936900E8CE83B180936800E4CE83 +:1006B00083B18093A50081E08093F600DDCE83B105 +:1006C0008093A400F8CF83B180939F00D5CE83B1EF +:1006D00060E070E070919400682B709395006093D7 +:1006E00094006115710579F08CE496E20E942B1359 +:1006F0006093C3001092C40089B7816089BF8FEFF7 +:100700008093A200B9CE89B78E7F89BF1092E50091 +:100710001092E400B1CE83B18093EC0087CE8AB50D +:100720009BB583B908C088B599B5FBCF83B5F9CF20 +:100730008CB7F7CF13B885EC86BFA5CE8091E200C9 +:100740009091E300909395008093940093B9F3CF38 +:100750008091E0009091E100F5CF8091DE009091D2 +:10076000DF00F0CF8091DC009091DD00EBCF809135 +:100770009400D7CF8091D4009091D500E7CF80919D +:10078000D4009091D500CDCF8091D2009091D3002C +:10079000DDCF8091D2009091D300C3CF8091D00063 +:1007A0009091D100D3CF8091D0009091D100B9CF5A +:1007B0008091CE009091CF00C9CF8091CE009091D2 +:1007C000CF00AFCF8091C1009091C200BFCF809188 +:1007D000C1009091C200A5CF8091BF009091C00050 +:1007E000B5CF8091BF009091C0009BCF8091BD009C +:1007F0009091BE00ABCF8091BD009091BE0091CF93 +:100800008091BB009091BC00A1CF8091BB009091E2 +:10081000BC0087CF8091AB009091AC00A091AD005F +:10082000B091AE0080939A0090939B00A0939C009F +:10083000B0939D008B2F9927AA27BB2772CF809159 +:100840009A0090919B00A0919C00B0919D00CD01D9 +:10085000AA27BB2766CF80919A0090919B00A09118 +:100860009C00B0919D00892F9A2FAB2FBB2759CFA9 +:1008700080919A0056CF8091A7009091A800A091F6 +:10088000A900B091AA008093960090939700A0933E +:100890009800B0939900CECF8091960090919700E8 +:1008A000A0919800B0919900D2CF8091960090913C +:1008B0009700A0919800B0919900D5CF80919600B3 +:1008C00030CF8091B7009091B800A091B900B0915D +:1008D000BA00B0CF8091B7009091B800A091B90054 +:1008E000B091BA00B4CF8091B7009091B800A091B8 +:1008F000B900B091BA00B7CF8091B7009091B8001D +:10090000A091B900B091BA000CCF8091B300909142 +:10091000B400A091B500B091B6008CCF8091B30027 +:100920009091B400A091B500B091B60090CF8091A5 +:10093000B3009091B400A091B500B091B60093CFF0 +:100940008091B3009091B400A091B500B091B60031 +:10095000E8CE8091AF009091B000A091B100B0912D +:10096000B20068CF8091AF009091B000A091B1002B +:10097000B091B2006CCF8091AF009091B000A09187 +:10098000B100B091B2006FCF8091AF009091B000F4 +:10099000A091B100B091B200C4CE84B78F7083B97A +:1009A00014BEC9CE8091A300BCCE8091A60083B9AD +:1009B00085EC86BF8091A00083B985EC86BF8091CD +:1009C0009F0083B985EC86BF13B885E8B5CE85EC6A +:1009D00086BF0ECD1F921FB61F9211242F933F93F7 +:1009E0008F939F9389B3982F917080FD93E081FF3F +:1009F00002C081E0982780919300891B80FF14C07A +:100A0000909393002091D70082708150990B21150B +:100A100009F48BC02091E2003091E300281B390BD0 +:100A20003093E3002093E20089B3282F22952F70A2 +:100A300084FB992790F984FD93E021FF02C081E0B7 +:100A4000982780919200891B80FF14C09093920098 +:100A50002091D60082708150990B211509F46CC049 +:100A60002091E0003091E100280F391F3093E10020 +:100A70002093E00089B3282F2695269582FB99279D +:100A800090F982FD93E021FF02C081E098278091D8 +:100A90009100891B80FF14C0909391002091690000 +:100AA00082708150990B211509F44DC02091DE0010 +:100AB0003091DF00280F391F3093DF002093DE00D4 +:100AC00029B3822F829586958695837026FB992778 +:100AD00090F926FD93E08270811511F081E098274E +:100AE00080919000891B80FF13C09093900020910B +:100AF000680082708150990B211561F12091DC0012 +:100B00003091DD00281B390B3093DD002093DC0091 +:100B10008091C5008F5F8093C5009F918F913F9119 +:100B20002F911F901FBE1F9018952091E2003091C9 +:100B3000E300280F391F74CF2091E0003091E100CD +:100B4000281B390B93CF2091DE003091DF00281B4A +:100B5000390BB2CF2091DC003091DD00280F391F16 +:100B6000D3CF1F920F920FB60F9211242F933F9362 +:100B70004F935F936F937F938F939F93AF93BF93A5 +:100B8000EF93FF938091C4008F5F8093C400909196 +:100B9000C4008091C300981778F01092C40080912F +:100BA000E4009091E5009195819591099093E5007D +:100BB0008093E4000E945A00FF91EF91BF91AF91A2 +:100BC0009F918F917F916F915F914F913F912F9165 +:100BD0000F900FBE0F901F9018958F938FB78F9324 +:100BE0009F93EF93FF935F9B0FC0E091F900F0910B +:100BF000FA008CB1808381E0EE35F80760F431961D +:100C0000F093FA00E093F900FF91EF919F918F919B +:100C10008FBF8F9118958BEF90E09093FA0080939F +:100C2000F900F2CF2AB128612AB9579A20B5266077 +:100C300020BDA0E0B0E09C01AD0184E0220F331F95 +:100C4000441F551F8A95D1F760E47BE48CE490E063 +:100C50000E943F13DA01C9010197A109B109892F47 +:100C60009A2FAB2FBB27A7FDBA9580BD215029B97C +:100C70008BEF90E09093F8008093F7009093FA0048 +:100C80008093F9000895FC015D9BFECF80818CB9B3 +:100C90000895CF93DF930F92CDB7DEB78983CE014E +:100CA00001960E94430680E090E00F90DF91CF9183 +:100CB0000895CF93DF93EC01CE012196FC012081B2 +:100CC000211103C0DF91CF9108950E944306F4CF14 +:100CD0005F9BFECF8CB190E008950C9468066DE6A2 +:100CE00076E089E496E00C94C813CF93DF939C01DF +:100CF000FC01E253FF4FA081B181EC01C453DF4FEF +:100D000088819981A817B90781F05D9B0EC08C91ED +:100D10008CB980819181A90145535F4F8417950754 +:100D200008F040C0019691838083E091F700F09134 +:100D3000F8008091F9009091FA008E179F0779F1E1 +:100D4000B9016C597F4FDB01CD91DC911197808106 +:100D50008883CD91DC91C9018B529F4FDC019C911E +:100D60008881981307C0A9014C525F4FDA018C911A +:100D70008F5F8C93C9018D599F4FC817D90718F4FD +:100D80009E012F5F3F4FDB012D933C93B1E0EE3589 +:100D9000FB0760F43196F093F800E093F700DF91E1 +:100DA000CF910895C90188599F4FBDCF8BEF90E037 +:100DB0009093F8008093F700F2CFCDB7DEB76E972F +:100DC0000FB6F894DEBF0FBECDBF88E087BB8CEFB7 +:100DD00084BB8CEB81BB87E088BB80E482BB109234 +:100DE000EC0080E895E20E9412060E946F0680E502 +:100DF00082B985EC86BF81E08FBD8AE08EBD1BBCC9 +:100E00001ABC19BC18BC8AE485BD13BC83BF1CBEC8 +:100E100084E590E09F938F930E94421481E090E0DC +:100E20000E9411168093A00085B78F7885BF84E05B +:100E300089BF78940F900F9084E6282E312C8091F2 +:100E4000ED00843A09F479C38F3F09F496C3809189 +:100E5000F600811509F4CFC0F894C090EE00D09050 +:100E6000EF00E090F000F090F1006091F2007091DE +:100E7000F3008091F4009091F5001092F6007894C0 +:100E80002AEC37E34BEF5EE30E946D1220E030E086 +:100E900040E05FE30E946D12A70196010E94591085 +:100EA0008B018D839983A7019601C701B6010E942A +:100EB000591098014D8159810E9458102AE939E44E +:100EC0004BE955E40E946D126B017C012AE939E47B +:100ED0004BE955E4B8018D8199810E946D128B0117 +:100EE0008D8399838091A400811509F45CC360917E +:100EF000A4009101261B3109B901072E000C880BB3 +:100F0000990B0E9479119601A7010E946D1220E0B1 +:100F100030E048EC52E40E94CF100E9441117093DF +:100F2000D5006093D4006091A400C101861B910993 +:100F3000BC01072E000C880B990B0E9479119801B7 +:100F40004D8159810E946D1220E030E048EC52E45E +:100F50000E94CF100E9441117093D1006093D00085 +:100F60008091A500811509F42AC36091A500D101E3 +:100F7000A61BB109BD01072E000C880B990B0E941E +:100F800079119601A7010E946D1220E030E048EC33 +:100F900052E40E94CF100E944111462F572F5093C8 +:100FA000D3004093D2006091A5009101261B310926 +:100FB000B901072E000C880B990B0E94791198013A +:100FC0004D8159810E946D1220E030E048EC52E4DE +:100FD0000E94CF100E944111462F572F5093CF00EF +:100FE0004093CE0031E03093DB003093DA00309351 +:100FF000D9003093D8008091C5008E3408F47AC6A9 +:101000001092C500F8948091E2009091E3009A8BD1 +:10101000898BA091E000B091E100BC8BAB8B20915B +:10102000DE003091DF003E8B2D8B8091DC009091B3 +:10103000DD00988F8F8B789480918E0090918F0037 +:10104000C988DA88C81AD90A80918C0090918D00DD +:101050007D01E81AF90A80918A0090918B00281B83 +:10106000390B3A8F298F80918800909189000F89E0 +:10107000188D081B190BB6010D2C000C880B990B51 +:101080000E9479112AE037ED43E25CE30E94CF1021 +:101090000E94411169837A838B839C83B7010F2C53 +:1010A000000C880B990B0E9479112AE037ED43E27E +:1010B0005CE30E94CF100E9441116D837E838F8379 +:1010C0009887A98DBA8DBD01BB0F880B990B0E9423 +:1010D00079112AE037ED43E25CE30E94CF100E94D1 +:1010E000411169877A878B879C87B801012E000C94 +:1010F000880B990B0E9479112AE037ED43E25CE3FB +:101100000E94CF100E9441116D877E878F87988B38 +:10111000B6016E0D7F1D072E000C880B990B0E94E7 +:1011200079112AE939E44BE156E40E94CF104B01D2 +:101130005C01698D7A8D600F711F072E000C880B82 +:10114000990B0E9479112AE939E44BE156E40E9497 +:10115000CF102B013C019401A5010E9458102AECEC +:1011600037E34BEF5EE30E94CF106D8F7E8F182F19 +:10117000092F2091AF003091B0004091B100509103 +:10118000B2000E9459106B017C012BED3FE049EC4D +:1011900050E4C701B6010E94681218160CF01DC277 +:1011A0002BED3FE049EC50E4C701B6010E94581016 +:1011B0006B017C019201A301B401C5010E94591089 +:1011C00020E030E040E05FE30E946D124B015C01E3 +:1011D0004090B7005090B8006090B9007090BA008D +:1011E000C701B6010E94CA109B01AC01B401C50140 +:1011F0000E946D12A30192010E945910AB01BC0123 +:10120000498F5A8F6B8F7C8F4090B3005090B40001 +:101210006090B5007090B600C701B6010E94DA1266 +:101220009B01AC01B401C5010E946D12A3019201A2 +:101230000E9459102B013C012AE037ED43E25CE3A8 +:10124000B401C5010E94CF104B015C012AE037EDCB +:1012500043E25CE36D8D7E8D812F902F0E94CF1035 +:10126000AB01BC01F894C092AF00D092B000E09204 +:10127000B100F092B200898D9A8DAB8DBC8D8093B8 +:10128000B7009093B800A093B900B093BA00409211 +:10129000B3005092B4006092B5007092B600A9817C +:1012A000BA81B093C200A093C1002D813E813093DA +:1012B000C0002093BF0089859A859093BE008093DB +:1012C000BD00AD85BE85B093BC00A093BB008092ED +:1012D000AB009092AC00A092AD00B092AE004093F3 +:1012E000A7005093A8006093A9007093AA00789477 +:1012F00029893A8930938F0020938E008B899C89AD +:1013000090938D0080938C00AD89BE89B0938B0043 +:10131000A0938A002F89388D30938900209388000C +:101320008091840090918500A0918600B091870003 +:10133000B7FF07C0B095A095909581959F4FAF4F8F +:10134000BF4F813E9242A240B105CCF08091C100D6 +:101350009091C200892B99F41092EB001092EA0050 +:101360001092DB008091A30080618093A300109213 +:101370008400109285001092860010928700809160 +:10138000800090918100A0918200B0918300B7FF0E +:1013900007C0B095A095909581959F4FAF4FBF4FD7 +:1013A000813E9242A240B105CCF08091BF00909165 +:1013B000C000892B99F41092E9001092E800109275 +:1013C000DA008091A30080628093A30010928000D5 +:1013D00010928100109282001092830080917C0014 +:1013E00090917D00A0917E00B0917F00B7FF07C073 +:1013F000B095A095909581959F4FAF4FBF4F813E7F +:101400009242A240B105CCF08091BD009091BE0007 +:10141000892B99F41092E7001092E6001092D900FF +:101420008091A30080648093A30010927C001092AE +:101430007D0010927E0010927F0080917800909144 +:101440007900A0917A00B0917B00B7FF07C0B095FA +:10145000A095909581959F4FAF4FBF4F813E92428F +:10146000A240B105CCF08091BB009091BC00892BCB +:1014700099F41092E5001092E4001092D800809147 +:10148000A30080688093A3001092780010927900E6 +:1014900010927A0010927B008091DB00813009F07D +:1014A000C6C02091D4003091D5008091CC0090919D +:1014B000CD002817390719F12091D4003091D500BB +:1014C0004091CC005091CD00C901841B950B97FF32 +:1014D00003C0CA01821B930B853F914044F01092D8 +:1014E00084001092850010928600109287008091EF +:1014F000D4009091D5009093CD008093CC00809142 +:10150000D4009091D5001816190634F48091C100CA +:101510009091C20097FD74C08091D4009091D50045 +:1015200097FF09C02091C1003091C20081E01216DE +:1015300013060CF466C080E064C080919F00813087 +:1015400061F428E13FE00FB6F894A89521BD0FBEE5 +:1015500031BD82E080939F007ACC80919F00811101 +:1015600076CC0FB6F894A89581B5886181BD11BC81 +:101570000FBE93E090939F006ACC8091EC00853A77 +:1015800009F065CCF8946BE780E090E00E941916B2 +:10159000E199FECFA8E1B8E00FB6F894A895A1BDF7 +:1015A0000FBEB1BD54CCB601C7010E94411170936A +:1015B000D5006093D400B8018D819981CBCCB60160 +:1015C000C7010E944111462F572F5093D3004093DB +:1015D000D200B8018D819981FDCC2BED3FE049EC23 +:1015E00050ECC701B6010E94C51087FFE3CD2BED7B +:1015F0003FE049EC50E4C701B6010E945910D8CD34 +:1016000081E02091D4003091D500232B19F0811571 +:1016100009F45EC01092EB001092EA00109277007D +:10162000109276008091A3008F7E8093A30080911A +:10163000DA00813009F008C12091D2003091D30046 +:101640008091CA009091CB002817390719F1209199 +:10165000D2003091D3004091CA005091CB00C90113 +:10166000841B950B97FF03C0CA01821B930B853F18 +:10167000914044F0109280001092810010928200FC +:10168000109283008091D2009091D3009093CB0070 +:101690008093CA008091D2009091D3001816190649 +:1016A00034F48091BF009091C00097FDB6C0809146 +:1016B000D2009091D30097FF09C02091BF003091D4 +:1016C000C00081E0121613060CF4A8C080E0A6C08A +:1016D0008091D4009091D5009A8389838091C10034 +:1016E0009091C200A981BA81A81BB90BBA83A983C2 +:1016F000BD01BB0F880B990BC0908400D090850072 +:10170000E0908600F0908700C60ED71EE81EF91EF6 +:10171000C0928400D0928500E0928600F09287000B +:101720000E9479112EE237E04CE25CE30E946D12D8 +:101730005B018C01C701B6010E9479112EE237E0EE +:101740004CE25CE30E946D129B01AC01B501C80143 +:101750000E9459107B018C01809176009091770056 +:1017600069817A81681B790B072E000C880B990B15 +:101770000E94791120E030E0A9010E946D129B01C6 +:10178000AC01B701C8010E9459100E944111462FB7 +:10179000572F5093EB004093EA0029813A81309310 +:1017A0007700209376008091EA009091EB00181664 +:1017B00019066CF48091D4009091D50097FF07C072 +:1017C00080E090E89093EB008093EA0030CF809126 +:1017D000EA009091EB0097FF07C08091D4009091B0 +:1017E000D5001816190664F38091EA009091EB0079 +:1017F000811591403CF02FEF30E03093EB002093C7 +:10180000EA0015CF8091EA009091EB0081309F4F64 +:101810000CF00DCF81E09FEFD5CF81E02091D20079 +:101820003091D300232B19F0811509F45EC010927A +:10183000E9001092E80010927500109274008091F7 +:10184000A3008F7D8093A3008091D900813009F09F +:101850000BC12091D0003091D1008091C8009091AF +:10186000C9002817390719F12091D0003091D10013 +:101870004091C8005091C900C901841B950B97FF86 +:1018800003C0CA01821B930B853F914044F0109224 +:101890007C0010927D0010927E0010927F0080915B +:1018A000D0009091D1009093C9008093C80080919E +:1018B000D0009091D1001816190634F48091BD0023 +:1018C0009091BE0097FDB9C08091D0009091D10059 +:1018D00097FF09C02091BD003091BE0081E0121633 +:1018E00013060CF4ABC080E0A9C02091D200309167 +:1018F000D3008091BF009091C000281B390B3A8320 +:101900002983B901330F880B990BC0908000D090C8 +:101910008100E0908200F0908300C60ED71EE81E82 +:10192000F91EC0928000D0928100E0928200F09275 +:1019300083000E9479112EE237E04CE25CE30E94C2 +:101940006D125B018C01C701B6010E9479112EE274 +:1019500037E04CE25CE30E946D129B01AC01B501E3 +:10196000C8010E9459107B018C01809174009091F4 +:10197000750069817A81681B790B072E000C880B32 +:10198000990B0E94791120E030E0A9010E946D12AC +:101990009B01AC01B701C8010E9459100E9441117E +:1019A000462F572F5093E9004093E80089819A8190 +:1019B00090937500809374008091E8009091E90005 +:1019C000181619066CF48091D2009091D30097FFFD +:1019D00007C0A0E0B0E8B093E900A093E80034CFDE +:1019E0008091E8009091E90097FF0EC08091D200AD +:1019F0009091D300181619063CF480E090E890937B +:101A0000E9008093E80020CF8091E8009091E90000 +:101A1000811591403CF02FEF30E03093E9002093A6 +:101A2000E80012CF8091E8009091E90081309F4F4B +:101A30000CF00ACF81E09FEFE2CF81E02091D0004F +:101A40003091D100232B19F0811509F45EC010925A +:101A5000E7001092E60010927300109272008091DD +:101A6000A3008F7B8093A3008091D800813009F080 +:101A70000BC12091CE003091CF008091C600909193 +:101A8000C7002817390719F12091CE003091CF00F7 +:101A90004091C6005091C700C901841B950B97FF68 +:101AA00003C0CA01821B930B853F914044F0109202 +:101AB00078001092790010927A0010927B00809149 +:101AC000CE009091CF009093C7008093C600809184 +:101AD000CE009091CF001816190634F48091BB0007 +:101AE0009091BC0097FDB9C08091CE009091CF003D +:101AF00097FF09C02091BB003091BC0081E0121615 +:101B000013060CF4ABC080E0A9C02091D000309146 +:101B1000D1008091BD009091BE00281B390B3A8303 +:101B20002983B901330F880B990BC0907C00D090AA +:101B30007D00E0907E00F0907F00C60ED71EE81E6C +:101B4000F91EC0927C00D0927D00E0927E00F0925F +:101B50007F000E9479112EE237E04CE25CE30E94A4 +:101B60006D125B018C01C701B6010E9479112EE252 +:101B700037E04CE25CE30E946D129B01AC01B501C1 +:101B8000C8010E9459107B018C01809172009091D4 +:101B9000730069817A81681B790B072E000C880B12 +:101BA000990B0E94791120E030E0A9010E946D128A +:101BB0009B01AC01B701C8010E9459100E9441115C +:101BC000462F572F5093E7004093E60089819A8172 +:101BD00090937300809372008091E6009091E700EB +:101BE000181619066CF48091D0009091D10097FFDF +:101BF00007C0A0E0B0E8B093E700A093E60034CFC0 +:101C00008091E6009091E70097FF0EC08091D00090 +:101C10009091D100181619063CF480E090E890935A +:101C2000E7008093E60020CF8091E6009091E700E6 +:101C3000811591403CF02FEF30E03093E700209386 +:101C4000E60012CF8091E6009091E70081309F4F2F +:101C50000CF00ACF81E09FEFE2CF81E02091CE002F +:101C60003091CF00232B19F0811509F44BC010924D +:101C7000E5001092E40010927100109270008091C3 +:101C8000A3008F778093A3000E945A008091A60042 +:101C90008F5F8093A6008091A2008F3F59F18091C1 +:101CA000A2008F5F8093A2008091A200843610F181 +:101CB0001092EE001092EF001092F0001092F100DE +:101CC0001092F2001092F3001092F4001092F500BE +:101CD00081E08093F6008091A200843641F4609107 +:101CE000A10081E090E00E941916E199FECF9FEFDC +:101CF0009093A20085B7806885BF889585B78F7758 +:101D000085BF9DC82091CE003091CF008091BB004F +:101D10009091BC00281B390B3A832983B901330FFA +:101D2000880B990BC0907800D0907900E0907A00F1 +:101D3000F0907B00C60ED71EE81EF91EC0927800F8 +:101D4000D0927900E0927A00F0927B000E947911A3 +:101D50002EE237E04CE25CE30E946D125B018C01E5 +:101D6000C701B6010E9479112EE237E04CE25CE334 +:101D70000E946D129B01AC01B501C8010E9459106F +:101D80007B018C01809170009091710069817A8152 +:101D9000681B790B072E000C880B990B0E94791198 +:101DA00020E030E0A9010E946D129B01AC01B70157 +:101DB000C8010E9459100E944111462F572F50937D +:101DC000E5004093E40089819A81909371008093AB +:101DD00070008091E4009091E500181619066CF4EB +:101DE0008091CE009091CF0097FF07C0A0E0B0E8AF +:101DF000B093E500A093E40047CF8091E400909178 +:101E0000E50097FF0EC08091CE009091CF0018168C +:101E100019063CF480E090E89093E5008093E4009C +:101E200033CF8091E4009091E500811591403CF022 +:101E30002FEF30E03093E5002093E40025CF809130 +:101E4000E4009091E50081309F4F0CF01DCF81E0C0 +:101E50009FEFE2CFABE0B0E0E0E3FFE00C9468136B +:101E60005C016B013A01FC0117821682838181FFBC +:101E70001CC1AE014F5F5F4F4A01F5010381F601BE +:101E800003FD859103FF81916F01811509F4B8C0AD +:101E9000853239F403FD159103FF11916F0115325D +:101EA00039F4B50190E00E94121483013801E5CFA6 +:101EB000F12CE12C111509F4A3C0612F70E085E627 +:101EC00090E00E94BD13892B21F4133209F5689428 +:101ED000F4F8F60103FD159103FF11916F01EF2C4A +:101EE000F7FEE8CF111509F48BC0612F70E08EE585 +:101EF00090E00E94BD13892BC1F08301F3FE12C054 +:101F0000085F1F4FB5018FE390E00E941214CECFFF +:101F10001C3619F46894F7F8DCCF1C3431F7689458 +:101F2000F3F8D7CF0C5F1F4FEDCF1336E9F113371E +:101F300009F442C01335F1F1143619F0193609F0DD +:101F400055C0F301E7FE4AC0619171918191919171 +:101F50008F01E894F4F897FF09C0909580957095EB +:101F600061957F4F8F4F9F4F6894F6F82AE030E0DD +:101F7000AE014F5F5F4F3A010E946914E82EF6FCF4 +:101F80008EC0E618EA94F12C5FEFE51AF50AE80C2A +:101F9000F91CF70182917F01B50190E00E941214B3 +:101FA0008E149F04B1F782CFB501F3018191919115 +:101FB0008F01ABCF6894F0F8F301619071908F01BD +:101FC000F301F0FC8591F0FE81913F01811509F448 +:101FD0006DCFB50190E00E941214F2CF6191719122 +:101FE0008F01072E000C880B990BB3CF103709F423 +:101FF00045C068F4183509F444C01F3609F446C0DA +:10200000F501868197812B96EEE00C9484131537A9 +:1020100041F01837A9F75F2D5462F52E20E130E02A +:1020200004C0E894F4F82AE030E0F301F7FE31C090 +:1020300061917191819191918F01AE014F5F5F4FDD +:102040003A010E94691486199709E82EF4FE9ACF86 +:10205000FE01E80FF11D8081803309F493CFB501B3 +:1020600080E390E00E941214F2FE8CCF8F2D8072DC +:10207000885AB50190E00E94121484CF6894F4F855 +:10208000CACF6894F2F820E132E0CFCF28E030E008 +:10209000CCCF619171918F0180E090E0CECFB501FE +:1020A0008DE290E00E9412146CCF8FEF9FEFABCFC8 +:1020B0005058BB27AA270E9470100C941A120E9435 +:1020C000E11138F00E94E81120F039F49F3F19F433 +:1020D00026F40C94DE110EF4E095E7FB0C94D81175 +:1020E000E92F0E943F1258F3BA176207730784075B +:1020F000950720F079F4A6F50C9461120EF4E095A2 +:102100000B2EBA2FA02D0B01B90190010C01CA01B1 +:10211000A0011124FF27591B99F0593F50F4503E5C +:1021200068F11A16F040A22F232F342F4427585F4E +:10213000F3CF469537952795A795F0405395C9F766 +:102140007EF41F16BA0B620B730B840BBAF091501E +:10215000A1F0FF0FBB1F661F771F881FC2F70EC0BD +:10216000BA0F621F731F841F48F487957795679590 +:10217000B795F7959E3F08F0B0CF9395880F08F07C +:102180009927EE0F9795879508950E94B41108F44A +:1021900081E008950E94F111E3950C942B120E94A6 +:1021A000E3100C941A120E94E81158F00E94E111F9 +:1021B00040F029F45F3F29F00C94D81151110C9490 +:1021C00062120C94DE110E943F1268F39923B1F35E +:1021D000552391F3951B550BBB27AA27621773074D +:1021E000840738F09F5F5F4F220F331F441FAA1FE1 +:1021F000A9F335D00E2E3AF0E0E832D0915050409D +:10220000E695001CCAF72BD0FE2F29D0660F771F4A +:10221000881FBB1F261737074807AB07B0E809F030 +:10222000BB0B802DBF01FF2793585F4F3AF09E3FB5 +:10223000510578F00C94D8110C9462125F3FE4F3CE +:10224000983ED4F3869577956795B795F7959F5FF8 +:10225000C9F7880F911D9695879597F90895E1E044 +:10226000660F771F881FBB1F621773078407BA07A3 +:1022700020F0621B730B840BBA0BEE1F88F7E095FE +:1022800008950E9448116894B1110C946212089547 +:102290000E94471288F09F5798F0B92F9927B7519D +:1022A000B0F0E1F0660F771F881F991F1AF0BA95FA +:1022B000C9F714C0B13091F00E946112B1E00895E5 +:1022C0000C946112672F782F8827B85F39F0B93FD7 +:1022D000CCF3869577956795B395D9F73EF49095AD +:1022E0008095709561957F4F8F4F9F4F0895E8942B +:1022F00009C097FB3EF490958095709561957F4F4E +:102300008F4F9F4F9923A9F0F92F96E9BB279395FB +:10231000F695879577956795B795F111F8CFFAF40B +:10232000BB0F11F460FF1BC06F5F7F4F8F4F9F4F3C +:1023300016C0882311F096E911C0772321F09EE89A +:10234000872F762F05C0662371F096E8862F70E000 +:1023500060E02AF09A95660F771F881FDAF7880FDA +:102360009695879597F90895990F0008550FAA0B30 +:10237000E0E8FEEF16161706E807F907C0F0121698 +:102380001306E407F50798F0621B730B840B950B9B +:1023900039F40A2661F0232B242B252B21F40895F0 +:1023A0000A2609F4A140A6958FEF811D811D08958D +:1023B00097F99F6780E870E060E008959FEF80ECF8 +:1023C000089500240A94161617061806090608959B +:1023D00000240A94121613061406050608950C9498 +:1023E000DE110E944712D8F3E894E0E0BB279F5724 +:1023F000F0F02AED3FE049EC06C0EE0FBB0F661F80 +:10240000771F881F28F0B23A62077307840728F005 +:10241000B25A620B730B840BE3959A9572F780386E +:1024200030F49A95BB0F661F771F881FD2F790482C +:102430000C94E412092E0394000C11F4882352F03A +:10244000BB0F40F4BF2B11F460FF04C06F5F7F4FE0 +:102450008F4F9F4F0895EF93E0FF07C0A2EA2AED48 +:102460003FE049EC5FEB0E9470100E941A120F903F +:10247000039401FC9058E0E5FCE20C94F01257FD47 +:102480009058440F551F59F05F3F71F04795880FE2 +:1024900097FB991F61F09F3F79F087950895121679 +:1024A00013061406551FF2CF4695F1DF08C0161625 +:1024B00017061806991FF1CF8695710561050894D6 +:1024C0000895E894BB2766277727CB0197F90895ED +:1024D0000E94B41108F48FEF08950E9480120C94AA +:1024E0001A120E94E11138F00E94E81120F09523A1 +:1024F00011F00C94D8110C94DE1111240C9462127A +:102500000E943F1270F3959FC1F3950F50E0551F45 +:10251000629FF001729FBB27F00DB11D639FAA2738 +:10252000F00DB11DAA1F649F6627B00DA11D661F87 +:10253000829F2227B00DA11D621F739FB00DA11DA8 +:10254000621F839FA00D611D221F749F3327A00D62 +:10255000611D231F849F600D211D822F762F6A2FFE +:1025600011249F5750409AF0F1F088234AF0EE0F63 +:10257000FF1FBB1F661F771F881F91505040A9F790 +:102580009E3F510580F00C94D8110C9462125F3F6D +:10259000E4F3983ED4F3869577956795B795F795CC +:1025A000E7959F5FC1F7FE2B880F911D9695879544 +:1025B00097F908959F930E94F1110F9007FCEE5F29 +:1025C0000C942B129F3F31F0915020F48795779512 +:1025D0006795B795880F911D9695879597F90895FA +:1025E0009F938F937F936F93FF93EF939B01AC0126 +:1025F0000E946D12EF91FF910E9404132F913F9161 +:102600004F915F910C946D12DF93CF931F930F93B3 +:10261000FF92EF92DF927B018C01689406C0DA2E64 +:10262000EF010E948012FE01E894A5912591359159 +:1026300045915591A6F3EF010E947010FE0197019C +:10264000A801DA9469F7DF90EF90FF900F911F9146 +:10265000CF91DF910895AA1BBB1B51E107C0AA1FB0 +:10266000BB1FA617B70710F0A61BB70B881F991F33 +:102670005A95A9F780959095BC01CD010895052E36 +:1026800097FB1EF400940E94561357FD07D00E943A +:102690009B1307FC03D04EF40C94561350954095B1 +:1026A000309521953F4F4F4F5F4F089590958095FE +:1026B000709561957F4F8F4F9F4F0895EE0FFF1FCD +:1026C0000590F491E02D09942F923F924F925F92E2 +:1026D0006F927F928F929F92AF92BF92CF92DF9232 +:1026E000EF92FF920F931F93CF93DF93CDB7DEB797 +:1026F000CA1BDB0B0FB6F894DEBF0FBECDBF09942B +:102700002A88398848885F846E847D848C849B8481 +:10271000AA84B984C884DF80EE80FD800C811B818F +:10272000AA81B981CE0FD11D0FB6F894DEBF0FBEBE +:10273000CDBFED010895A1E21A2EAA1BBB1BFD011E +:102740000DC0AA1FBB1FEE1FFF1FA217B307E40790 +:10275000F50720F0A21BB30BE40BF50B661F771FE8 +:10276000881F991F1A9469F76095709580959095C8 +:102770009B01AC01BD01CF010895FC010590061637 +:1027800021F00020D9F7C00108953197CF010895B5 +:102790000F931F93CF93DF93EC018B01892B51F49F +:1027A000672B41F4E0E0F0E0CF01DF91CF911F9182 +:1027B0000F9108956EE070E081E090E00E94C714F0 +:1027C000FC01892B79F380E883830115110571F0F1 +:1027D0001387028781E8838380916201909163016E +:1027E000892B21F4F0936301E09362012097E1F2D9 +:1027F000D187C087838182608383809164019091B7 +:102800006501892B89F6F0936501E093640180915D +:10281000660190916701892B39F6F0936701E09387 +:102820006601C2CF0F931F93CF93DF93182F092F09 +:10283000EB018B8181FD09C01FEF0FEF812F902FDE +:10284000DF91CF911F910F91089582FF14C02E81C7 +:102850003F818C819D81281739073CF4E881F981FB +:10286000CF0101969983888310838E819F81019681 +:102870009F838E83E3CFE885F985812F0995892B86 +:10288000A1F3DACFA0E0B0E0E8E4F4E10C94721335 +:10289000AE01495F5F4FDA016D917D91AD0102E6B6 +:1028A00011E0F80182819381DC0113962C9113973A +:1028B000286013962C930E942A0F9C01D80112962F +:1028C000ED91FC919381977F9383C901E4E00C948F +:1028D0008E13FA01AA27283051F1203181F1E894B2 +:1028E0006F936E7F6E5F7F4F8F4F9F4FAF4FB1E003 +:1028F0003ED0B4E03CD0670F781F891F9A1FA11DFE +:10290000680F791F8A1F911DA11D6A0F711D811DFE +:10291000911DA11D20D009F468943F912AE0269FC3 +:1029200011243019305D3193DEF6CF010895462F22 +:102930004770405D4193B3E00FD0C9F7F6CF462F03 +:102940004F70405D4A3318F0495D31FD405241936C +:1029500002D0A9F7EACFB4E0A69597958795779529 +:102960006795BA95C9F700976105710508959B01B0 +:10297000AC010A2E06945795479537952795BA9539 +:10298000C9F7620F731F841F951FA01D08950F9331 +:102990001F93CF93DF93869F8001879F100D969F93 +:1029A000100D1124C8010E94E314EC01009729F0D6 +:1029B000A80160E070E00E940A16CE01DF91CF917D +:1029C0001F910F9108950F931F93CF93DF939C0155 +:1029D000029710F422E030E0E0916801F091690183 +:1029E00080E090E0A0E0B0E0309791F4009709F427 +:1029F00048C0821B930B84309105D0F58A819B815E +:102A00006115710581F1FB0193838283FE0111C081 +:102A1000408151810281138142175307D8F0421738 +:102A2000530799F4109761F012960C93129713962E +:102A30001C933296CF01DF91CF911F910F91089592 +:102A40000093680110936901F4CF009751F0481783 +:102A5000590738F0AC01DB01CA01BD01DF01F80103 +:102A6000C3CFEF01F9CF9093690180936801CECF76 +:102A7000FE01E80FF91F2193319302979983888310 +:102A8000D9CF40916A0150916B014115510541F434 +:102A900040916C0050916D0050936B0140936A011E +:102AA00080916A0090916B00009741F48DB79EB7BA +:102AB00060916E0070916F00861B970B4817590745 +:102AC00008F0B8CF841B950B8217930708F4B2CF98 +:102AD000B9016E5F7F4F8617970708F4ABCF640F7D +:102AE000751F70936B0160936A01FA012193319312 +:102AF000A1CFCF93DF939C010097E9F0FC013297BF +:102B000013821282C0916801D0916901DE0180E0D8 +:102B100090E02097A1F480819181280F391F809146 +:102B20006A0190916B018217930709F068C0F093D6 +:102B30006B01E0936A01DF91CF910895DA01AE173E +:102B4000BF0708F042C012964D915C911397CD01DA +:102B50004115510599F7DC011396FC93EE931297FA +:102B60004D915D919D01240F351FE217F30781F40C +:102B7000208131812E5F3F4F240F351FDC011196DC +:102B80003C932E932281338113963C932E9312977C +:102B9000E0E0F0E08A819B810097B1F5DE018D9144 +:102BA0009D918A0F9B1F20916A0130916B0128171C +:102BB000390709F6309759F51092690110926801AA +:102BC000D0936B01C0936A01B6CFB383A2834081D7 +:102BD0005181240F351FA217B30771F44E5F5F4F69 +:102BE0002D913C911197240F351F3183208312962C +:102BF0002D913C9133832283009709F0ACCFF09361 +:102C00006901E093680197CFFE01EC01C3CF138205 +:102C10001282D6CFDC0101C06D9341505040E0F7E5 +:102C20000895E199FECF9FBB8EBBE09A99278DB3A3 +:102C30000895262FE199FECF9FBB8EBB2DBB0FB60B +:102C4000F894E29AE19A0FBE01960895F894FFCFA6 +:102C500005A84CCDB2D44EB93836A9020C50B99162 +:102C60008688083CA6AAAA2ABE000000803F9A03D4 +:102C70008F039A0393039A0396039A0398039A0384 +:102C8000E404E404E404E404E404E4049E03B70379 +:102C9000A803B703AD03B703B203B703E404E40426 +:102CA000E404E404E404E404E404E404BA03BF0335 +:102CB000C403C903CE03D303D803DD03E404E4044F +:102CC000E404E404E404E404E404E404E203E703C5 +:102CD000EC03F103F603FB03000405040A041F04DC +:102CE0002B0438043B044C0455045E0461046A045C +:102CF00073047C0485048E049704A004A904B20420 +:102D0000BB04C404E404E404E404E404E404E404CC +:102D1000E404E404E404E404E404E404E404E40473 +:102D2000E404E404E404E404E404E404E404E40463 +:102D3000E404E404E404E404E404E404E404E40453 +:102D4000E404E404E404E404E404E404E404E40443 +:102D5000E404E404E404E404E404E404E404E40433 +:102D6000E404E404E404E404E404E404E404E40423 +:102D7000E404E404E404E404E404E404E404E40413 +:102D8000E404E404E404E404E404E404E404E40403 +:102D9000E404E404E404E404E404E404E404E404F3 +:102DA000E404E404E404E404E404E404CD04D2040C +:102DB000D504DA04DF0448034C03500354035803DA +:102DC0005F031602160216021602160216021602F9 +:102DD00016021602160216021602160216026303E5 +:102DE0003402920249029202530292025D0292025E +:102DF00067021602160216021602160216021602C2 +:102E00001602160216021602160216021602160202 +:102E100016021602160216021602160216021602F2 +:102E20009202710292027C02920287029202960240 +:102E3000E402A102A102B802E402A102A102B802C6 +:102E400016021602160216021602160216021602C2 +:102E500016021602160216021602160216021602B2 +:102E600016021602160216021602160216021602A2 +:102E70001602160216021602160216021602160292 +:102E8000E402A102A102E9020803150315032403C9 +:102E90000080008000800080010100006C012000A3 +:00000001FF diff --git a/avr/motor_ctrl/ringbuffer.c b/avr/motor_ctrl/ringbuffer.c new file mode 100644 index 0000000..08e99d2 --- /dev/null +++ b/avr/motor_ctrl/ringbuffer.c @@ -0,0 +1,4 @@ +#include "ringbuffer.h" + +ringbuffer_t *ringbuffers[RINGBUFFER_MAX_NUM]; +uint8_t num_ringbuffers; diff --git a/avr/motor_ctrl/ringbuffer.h b/avr/motor_ctrl/ringbuffer.h new file mode 100644 index 0000000..ddbc762 --- /dev/null +++ b/avr/motor_ctrl/ringbuffer.h @@ -0,0 +1,36 @@ +#ifndef RINGBUFFER_H +#define RINGBUFFER_H + +#include + +#define RINGBUFFER_LEN 100 +#define RINGBUFFER_MAX_NUM 1 + +typedef struct ringbuffer { + char read[RINGBUFFER_LEN]; + char *in_read_ptr; + char *out_read_ptr; + char write[RINGBUFFER_LEN]; + char *in_write_ptr; + char *out_write_ptr; + FILE *dev; + FILE *dev_in_as_out; + uint8_t newlines; + char block_read; +} ringbuffer_t; + +extern ringbuffer_t *ringbuffers[RINGBUFFER_MAX_NUM]; +extern uint8_t num_ringbuffers; + +void ringbuffer_setup(FILE *stream, ringbuffer_t *buffer); +void ringbuffer_setup_in_as_out(FILE *stream, ringbuffer_t *buffer); +void init_ringbuffers(void); +void stream_setup(FILE *uart_stream); +void stream_setup_out_only(FILE *stream); +void stream_setup_in_as_out(FILE *stream, int ringbuffer_putchar_sound(char, FILE*)); +uint8_t ringbuffer_busy(void); +void ringbuffer_set_read_noblock(FILE *stream); +void ringbuffer_set_read_block(FILE *stream, char c); + +#endif + diff --git a/avr/motor_ctrl/uart.c b/avr/motor_ctrl/uart.c new file mode 100644 index 0000000..24de2f2 --- /dev/null +++ b/avr/motor_ctrl/uart.c @@ -0,0 +1,97 @@ +#include +#include "uart.h" +#include + +#define UART_UBRR_CALC(BAUD_,FREQ_) ((FREQ_)/((BAUD_)*16L)-1) + +static volatile char read[RINGBUFFER_LEN]; +static volatile char *in_read_ptr; +static volatile char *out_read_ptr; + +ISR(USART_RXC_vect) { + // update read from uart + if (UCSRA & (1<> 8); + UBRRL = (uint8_t)UART_UBRR_CALC(rate, F_CPU); + + in_read_ptr = out_read_ptr = read; +} + + +void uart_putc(char *c) { + while (!(UCSRA & (1<out_write_ptr != buffer->in_write_ptr) { + if (UCSRA & (1<out_write_ptr; + if (buffer->out_write_ptr < buffer->write + RINGBUFFER_LEN - 1) buffer->out_write_ptr++; + else buffer->out_write_ptr = buffer->write; + } + } + + // update read from uart + if (in_read_ptr != out_read_ptr) { + *buffer->in_read_ptr = *out_read_ptr; + + // update newline chars + if (buffer->block_read == *buffer->in_read_ptr) buffer->newlines++; + + if (buffer->in_read_ptr < buffer->read + RINGBUFFER_LEN - 1) buffer->in_read_ptr++; + else buffer->in_read_ptr = buffer->read; + + // move pointer + if (out_read_ptr < read + RINGBUFFER_LEN - 1) out_read_ptr++; + else out_read_ptr = read; + } +} + diff --git a/avr/motor_ctrl/uart.h b/avr/motor_ctrl/uart.h new file mode 100644 index 0000000..1225590 --- /dev/null +++ b/avr/motor_ctrl/uart.h @@ -0,0 +1,18 @@ +#ifndef UART_H +#define UART_H + +#include +#include "ringbuffer.h" + +void setup_uart(unsigned int rate); +void uart_putc(char *c); +void uart_puts(char *s); +int uart_getc(void); +void uart_puti(int i); +int uart_putchar(char c, FILE *stream); +int uart_getchar(FILE *stream); +void uart_setup_stdout(void); +void uart_stream_update(ringbuffer_t *buffer); + +#endif + diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..21ec223 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,6 @@ +FROM ros:jazzy-ros-base + +RUN apt-get update && \ + apt-get install -y ros-jazzy-rplidar-ros less libi2c-dev ros-jazzy-robot-localization \ + ros-jazzy-slam-toolbox ros-jazzy-navigation2 ros-jazzy-nav2-bringup \ + ros-jazzy-demo-nodes-cpp ros-jazzy-teleop-twist-keyboard ros-jazzy-gpsd-client libgdal-dev ros-jazzy-teleop-twist-joy ros-jazzy-twist-mux diff --git a/docker/build.sh b/docker/build.sh new file mode 100755 index 0000000..acef091 --- /dev/null +++ b/docker/build.sh @@ -0,0 +1,7 @@ +#!/bin/sh + +docker pull ros:jazzy-ros-base + +docker build -t ros2 . + +docker image prune diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh new file mode 100755 index 0000000..387c4ad --- /dev/null +++ b/docker/entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source /root/ros2_ws/install/setup.bash +exec "$@" diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..a4f4f33 --- /dev/null +++ b/package.xml @@ -0,0 +1,32 @@ + + + + a4wd3 + 0.0.0 + TODO: Package description + root + MIT + + ament_cmake + + rclcpp + std_msgs + sensor_msgs + geometry_msgs + std_srvs + nav_msgs + tf2_geometry_msgs + + ament_lint_auto + ament_lint_common + + ros2launch + bno055 + tf2_ros + robot_localization + twist_mux + + + ament_cmake + + diff --git a/params/wiimote.config.yaml b/params/wiimote.config.yaml new file mode 100644 index 0000000..2f5e1f7 --- /dev/null +++ b/params/wiimote.config.yaml @@ -0,0 +1,16 @@ +teleop_twist_joy_node: + ros__parameters: + axis_linear: + x: 1 + scale_linear: + x: -1.0 + scale_linear_turbo: + x: 4.0 + + axis_angular: + yaw: 0 + scale_angular: + yaw: 4.0 + + enable_button: 1 + enable_turbo_button: 0 diff --git a/scripts/engine_man_test.py b/scripts/engine_man_test.py new file mode 100755 index 0000000..434faf4 --- /dev/null +++ b/scripts/engine_man_test.py @@ -0,0 +1,23 @@ +#!/usr/bin/env tauthon +# -*- coding: iso-8859-15 -*- + +import sys +import struct +from time import sleep +from pyshared.i2c import i2c_write_reg, i2c_read_reg + + +def set_pwm(left, right): + #left = right = -32768 + #i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", left, left, right, right)) + i2c_write_reg(0x50, 0x20, struct.pack(">hhhh", left, left, right, right)) + +if __name__ == "__main__": + set_pwm(int(sys.argv[1]), int(sys.argv[2])) + + while True: + motor1,motor2,motor3,motor4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x1, 8)) + speed1, speed2, speed3, speed4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x30, 8)) + error, = struct.unpack(">B", i2c_read_reg(0x50, 0xA1, 1)) + print "PWM: %3d %3d %3d %3d, Speed: %5d %5d %5d %5d, Error: %d" % (motor1, motor2, motor3, motor4, speed1, speed2, speed3, speed4, error) + sleep(0.1) diff --git a/scripts/max44009_light.py b/scripts/max44009_light.py new file mode 100755 index 0000000..f0b83d1 --- /dev/null +++ b/scripts/max44009_light.py @@ -0,0 +1,25 @@ +#!/usr/bin/env tauthon +# -*- coding: iso-8859-15 -*- + +from pyshared.i2c import * +import struct +from time import sleep +from datetime import datetime + +class MAX44009: + def __init__(self): + self.addr = 0x94 + + def get_lux(self): + high, low = struct.unpack("bb", i2c_read_reg(self.addr, 3, 2)) + exponent = high>>4 + mantissa = ((high&0x0f)<<4) | low&0xf + lux = 2**exponent * mantissa * 0.045 + return lux + + +if __name__ == "__main__": + pLight = MAX44009() + while True: + print "%s: %.3f lx" % (datetime.now().strftime("%d.%m.%Y %H:%M:%S"), pLight.get_lux()) + sleep(1) diff --git a/scripts/oled_ssd1780.py b/scripts/oled_ssd1780.py new file mode 100755 index 0000000..f0bf6f1 --- /dev/null +++ b/scripts/oled_ssd1780.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import sys +import struct +import gi +gi.require_version('Gst', '1.0') +from gi.repository import GObject, Gst +from time import sleep +from pyshared.i2c import * + +SET_DISPLAY_ON=0xaf +SET_DISPLAY_OFF=0xa4 +SET_MULTIPLEX_RATIO=0xa8 +SET_DISPLAY_OFFSET=0xd3 +SET_COM_PINS_HARDWARE_CONFIGURATION=0xda +SET_NORMAL_DISPLAY=0xa6 +SET_INVERSE_DISPLAY=0xa7 +SET_DISPLAY_CLOCK_DIVIDE_RATIO=0xd5 +SET_MEMORY_ADDRESSING_MODE=0x20 +SET_COLUMN_ADDRESS=0x21 +SET_PAGE_ADDRESS=0x22 +SET_CONTRAST=0x81 + +def binarize(px, thres=127): + return px > thres + +class ssd1306: + def __init__(self): + Gst.init(None) + self.addr=0x7a + self.width=128 + self.height=64 + self.initialize() + + def play(self, src): + self.pipeline = Gst.parse_launch('%s ! decodebin ! videoconvert ! videoscale add-borders=true ! videorate average-period=1000000000 max-rate=4 ! video/x-raw,format=GRAY8,width=128,height=64 ! appsink' % (src)) + appsink = self.pipeline.get_by_name('appsink0') + bus = self.pipeline.get_bus() + bus.add_signal_watch() + bus.connect('message', self.on_gst_message) + appsink.set_property('emit-signals', True) + appsink.connect('new-sample', self.new_appsink_buffer) + self.pipeline.set_state(Gst.State.PLAYING) + loop = GObject.MainLoop() + loop.run() + + def new_appsink_buffer(self, appsink): + sample = appsink.emit("pull-sample") + buf = sample.get_buffer() + imgbuf = buf.extract_dup(0, buf.get_size()) + pOled.ssd1306_data(list(imgbuf)) + return Gst.FlowReturn.OK + + def on_gst_message(self, bus, message): + if message.type == Gst.MessageType.EOS: + sys.exit(0) + elif message.type == Gst.MessageType.ERROR: + err, debug = message.parse_error() + print err, debug + sys.exit(1) + elif message.type == Gst.MessageType.WARNING: + err, debug = message.parse_warning() + print err, debug + sys.exit(1) + + def ssd1306_cmd(self, ctrlbyte, databytes=[]): + data = chr(ctrlbyte) + "".join([chr(i) for i in databytes]) + i2c_write_reg(self.addr, 0b00000000, data) + + def initialize(self): + # Set MUX Ratio + self.ssd1306_cmd(SET_MULTIPLEX_RATIO, [63]) + + # Set Display Offset + self.ssd1306_cmd(SET_DISPLAY_OFFSET, [0]) + + # Set Display Start Line + self.ssd1306_cmd(0x40) + + # Set Segment re-map 0xA0/0xA1 + self.ssd1306_cmd(0xa1) + + # Set COM Output Scan Direction 0xC0/0xC8 + self.ssd1306_cmd(0xc8) + + # Set COM Pins hardware configuration + self.ssd1306_cmd(SET_COM_PINS_HARDWARE_CONFIGURATION, [0x12]) + + # Disable Entire Display On + self.ssd1306_cmd(SET_DISPLAY_OFF) + + # Set Normal Display + self.ssd1306_cmd(SET_NORMAL_DISPLAY) + + # Set Osc Frequency + self.ssd1306_cmd(SET_DISPLAY_CLOCK_DIVIDE_RATIO, [0x80]) + + # Enable charge pump regulator + self.ssd1306_cmd(0x8d, [0x14]) + self.ssd1306_cmd(0xd9, [0xf1]) + self.ssd1306_cmd(0xdb, [0x40]) + + # Misc setup + self.ssd1306_cmd(SET_MEMORY_ADDRESSING_MODE, [0x00]) + self.ssd1306_cmd(SET_CONTRAST, [0x7f]) + + # Display On + self.ssd1306_cmd(SET_DISPLAY_ON) + + self.clear() + + def ssd1306_data(self, img): + img = [ord(c) if type(c) == str else c for c in img] + thres = sum(img)/len(img) + print "Threshold=%d" % (thres) + if thres <= 31: thres=31 + elif thres > 223: thres=223 + data = [] + for y in range(0, self.height, 8): # step with page increment + for x in range(0, self.width): + px = binarize(img[(y+0)*self.width + x], thres) << 0 + px |= binarize(img[(y+1)*self.width + x], thres) << 1 + px |= binarize(img[(y+2)*self.width + x], thres) << 2 + px |= binarize(img[(y+3)*self.width + x], thres) << 3 + px |= binarize(img[(y+4)*self.width + x], thres) << 4 + px |= binarize(img[(y+5)*self.width + x], thres) << 5 + px |= binarize(img[(y+6)*self.width + x], thres) << 6 + px |= binarize(img[(y+7)*self.width + x], thres) << 7 + data.append(chr(px)) + + self.ssd1306_cmd(SET_COLUMN_ADDRESS, [0x00, 0x7f]) + self.ssd1306_cmd(SET_PAGE_ADDRESS, [0x00, 0x07]) + i2c_write_reg(self.addr, 0b01000000, "".join(data)) + + def clear(self): + img = [0x00] * (128*64) + self.ssd1306_data(img) + +if __name__ == "__main__": + pOled = ssd1306() + pOled.play(sys.argv[1]) diff --git a/scripts/pyshared b/scripts/pyshared new file mode 160000 index 0000000..5a2e876 --- /dev/null +++ b/scripts/pyshared @@ -0,0 +1 @@ +Subproject commit 5a2e8766d52bbd4251cfa784ca71e087673789ad diff --git a/scripts/step_response.py b/scripts/step_response.py new file mode 100755 index 0000000..bf8c630 --- /dev/null +++ b/scripts/step_response.py @@ -0,0 +1,32 @@ +#!/usr/bin/env tauthon +# -*- coding: iso-8859-15 -*- + +import struct +from time import sleep +from pyshared.i2c import i2c_write_reg, i2c_read_reg + +PWM_VALUES = [255, 0, 255] + +def set_pwm(val): + i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", val, val, -val, -val)) + +if __name__ == "__main__": + last_count = 0 + pwm = 0 + overflow = 0; + pwm_i = 0 + for i in range(200*(len(PWM_VALUES))): + count, = struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1)) + motor1, motor2, motor3, motor4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x30, 8)) + if last_count != count: + if count < last_count: + overflow+=1 + #print "%d;%d;%d;%d;%d" % (count, motor1, motor2, motor3, motor4) + print "%.2f;%d;%d" % ((overflow*255+count)/100.0, pwm, motor2) + last_count = count; + if i % 200 == 0: + pwm = PWM_VALUES[pwm_i] + set_pwm(pwm) + pwm_i+=1 + sleep(0.0005) + set_pwm(0) diff --git a/src/hw_node.cpp b/src/hw_node.cpp new file mode 100644 index 0000000..18625c2 --- /dev/null +++ b/src/hw_node.cpp @@ -0,0 +1,277 @@ +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +extern "C" { +#include +} + +/** + * ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" + */ + +#define I2C_FILE "/dev/i2c-7" +union ufloat { + uint32_t i; + float f; +}; + +using namespace std::chrono_literals; + +int i2c_write_reg(const uint8_t addr, const uint8_t command, const std::vector values) { + int ret, file=-1; + + if ((file = open(I2C_FILE, O_RDWR)) < 0) { + perror("open"); + goto error; + } + if (ioctl(file, I2C_SLAVE, (addr>>1)) < 0) { + perror("ioctl"); + goto error; + } + + if ((ret = i2c_smbus_write_i2c_block_data(file, command, values.size(), &values[0])) != 0) { + perror("i2c_smbus_write_block_data"); + goto error; + } + + close(file); + return ret; + +error: + if (file > 0) { + close(file); + } + throw -1; +} + +int i2c_read_reg(const uint8_t addr, const uint8_t command, const uint8_t length, uint8_t *values) { + int ret, file=-1; + + if ((file = open(I2C_FILE, O_RDWR)) < 0) { + perror("open"); + goto error; + } + if (ioctl(file, I2C_SLAVE, (addr>>1)) < 0) { + perror("ioctl"); + goto error; + } + + if ((ret = i2c_smbus_read_i2c_block_data(file, command, length, values)) != length) { + perror("i2c_smbus_read_block_data"); + goto error; + } + + close(file); + return ret; + +error: + if (file > 0) { + close(file); + } + throw -1; +} + +class A4wd3 : public rclcpp::Node { + public: + A4wd3() : Node("A4WD3") { + pub_odom = this->create_publisher("odom", 10); + pub_bat = this->create_publisher("battery", 10); + sub_cmd_vel = this->create_subscription("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1)); + this->declare_parameter("odom_covar_xy", 0.01); + this->declare_parameter("odom_covar_angle", 0.52); + + set_speed(0, 0); + init_pwr(); + timer = this->create_wall_timer(100ms, std::bind(&A4wd3::timer_callback, this)); + + RCLCPP_INFO(this->get_logger(), "Started."); + } + + private: + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr pub_odom; + rclcpp::Publisher::SharedPtr pub_bat; + rclcpp::Subscription::SharedPtr sub_cmd_vel; + std::vector cmd_vel; + double pos_x=0, pos_y=0, angle=0; + + void set_speed(float trans, float rot) { + rot*=-1; + std::vector buf; + buf.push_back((*(uint32_t*)&trans)>>24); + buf.push_back((*(uint32_t*)&trans)>>16); + buf.push_back((*(uint32_t*)&trans)>>8); + buf.push_back((*(uint32_t*)&trans)>>0); + buf.push_back((*(uint32_t*)&rot)>>24); + buf.push_back((*(uint32_t*)&rot)>>16); + buf.push_back((*(uint32_t*)&rot)>>8); + buf.push_back((*(uint32_t*)&rot)>>0); + int ret = i2c_write_reg(0x50, 0x50, buf); + if (ret != 0) { + RCLCPP_ERROR(this->get_logger(), "Failed to write Speed err=%d", ret); + } + } + + void cmdvel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) { + RCLCPP_DEBUG(this->get_logger(), "Set new cmd_vel: %.2f %.2f", msg->linear.x, msg->angular.z); + cmd_vel.resize(2); + cmd_vel[0] = msg->linear.x; + cmd_vel[1] = msg->angular.z; + } + + void update_odom() { + union ufloat values[5]; + const int ret = i2c_read_reg(0x50, 0x38, 20, (uint8_t*)values); + if (ret != 20) { + RCLCPP_ERROR(this->get_logger(), "Failed to read Odometry err=%d", ret); + return; + } + uint8_t count; + const int retcount = i2c_read_reg(0x50, 0xa2, 1, &count); + RCLCPP_INFO(this->get_logger(), "Count: %d, ret: %d", count, retcount); + + values[0].i = __bswap_32(values[0].i); + values[1].i = __bswap_32(values[1].i); + values[2].i = __bswap_32(values[2].i); + values[3].i = __bswap_32(values[3].i); + values[4].i = __bswap_32(values[4].i); + + float speed_trans = values[0].f; + float speed_rot = values[1].f; + float pos_x = values[2].f; + float pos_y = values[3].f; + float angle = values[4].f; + speed_rot*=-1; + angle*=-1; + + RCLCPP_DEBUG(this->get_logger(), "Odom from I2C: [%.2f, %.2f], Angle: %d, Speed: [%.2f, %.2f]", pos_x, pos_y, (int)(angle*180/M_PI+0.5), speed_trans, speed_rot); + + double odom_covar_xy = this->get_parameter("odom_covar_xy").as_double(); + double odom_covar_angle = this->get_parameter("odom_covar_angle").as_double(); + + auto odom = nav_msgs::msg::Odometry(); + odom.header.stamp = this->get_clock()->now(); + odom.header.frame_id = "odom"; + + tf2::Quaternion odom_quat; + odom_quat.setRPY(0, 0, angle); + + // set the position + odom.pose.pose.position.x = pos_x; + odom.pose.pose.position.y = pos_y; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = tf2::toMsg(odom_quat); + odom.pose.covariance[0] = odom_covar_xy; // x + odom.pose.covariance[7] = odom_covar_xy; // y + odom.pose.covariance[14] = odom_covar_xy; // z + odom.pose.covariance[21] = odom_covar_angle; // rotation about X axis + odom.pose.covariance[28] = odom_covar_angle; // rotation about Y axis + odom.pose.covariance[35] = odom_covar_angle; // rotation about Z axis + + // set the velocity + odom.child_frame_id = "base_link"; + odom.twist.twist.linear.x = speed_trans; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.linear.z = 0.0; + odom.twist.twist.angular.z = speed_rot; + odom.twist.covariance[0] = odom_covar_xy; // x + odom.twist.covariance[7] = odom_covar_xy; // y + odom.twist.covariance[14] = odom_covar_xy; // z + odom.twist.covariance[21] = odom_covar_angle; // rotation about X axis + odom.twist.covariance[28] = odom_covar_angle; // rotation about Y axis + odom.twist.covariance[35] = odom_covar_angle; // rotation about Z axis + + pub_odom->publish(odom); + } + + void init_pwr() { + std::vector buf; + uint16_t values[1]; + uint16_t reset = 1<<15; // reset + buf.push_back(reset>>8); + buf.push_back(reset); + int ret = i2c_write_reg(0x80, 0x50, buf); + if (ret != 0) { + RCLCPP_ERROR(this->get_logger(), "Failed to reset INA260 err=%d", ret); + return; + } + ret = i2c_read_reg(0x80, 0x00, 2, (uint8_t*)&values); + if (ret != 2) { + RCLCPP_ERROR(this->get_logger(), "Failed to read INA260 configuration err=%d", ret); + return; + } + values[0] = __bswap_16(values[0]); + RCLCPP_INFO(this->get_logger(), "INA260: 0x%x", values[0]); + values[0] |= 0b110<<9; // number of averages=512 => 0.5632s + buf.clear(); + buf.push_back(values[0]>>8); + buf.push_back(values[0]); + ret = i2c_write_reg(0x80, 0x50, buf); + if (ret != 0) { + RCLCPP_ERROR(this->get_logger(), "Failed to configure INA260 err=%d", ret); + return; + } + } + + void update_pwr() { + uint16_t value; + auto msg = sensor_msgs::msg::BatteryState(); + int ret = i2c_read_reg(0x80, 0x01, 2, (uint8_t*)&value); + if (ret != 2) { + RCLCPP_ERROR(this->get_logger(), "Failed to read INA260 current err=%d", ret); + return; + } + msg.current = __bswap_16(value)*1.25e-3; + + ret = i2c_read_reg(0x80, 0x02, 2, (uint8_t*)&value); + if (ret != 2) { + RCLCPP_ERROR(this->get_logger(), "Failed to read INA260 voltage err=%d", ret); + return; + } + msg.voltage = __bswap_16(value)*1.25e-3; + + msg.header.stamp = this->get_clock()->now(); + msg.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_GOOD; + msg.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_NIMH; + msg.design_capacity = 4.5; + msg.present = true; + pub_bat->publish(msg); + } + + void timer_callback() { + update_odom(); + update_pwr(); + + if (!cmd_vel.empty()) { + set_speed(cmd_vel[0], cmd_vel[1]); + cmd_vel.clear(); + } + } + +}; + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} -- 2.39.5