--- /dev/null
+[submodule "scripts/pyshared"]
+ path = scripts/pyshared
+ url = ssh://erik@defiant.homedns.org/home/erik_alt/git/pyshared.git
--- /dev/null
+# 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'
--- /dev/null
+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(<dependency> 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
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>)
+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()
--- /dev/null
+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.
--- /dev/null
+.dep
+*.eep
+*.elf
+*.lss
+*.lst
+*.map
+*.o
+*.sym
--- /dev/null
+# 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 <http://savannah.nongnu.org/projects/avrdude>
+# 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
+
--- /dev/null
+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
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <limits.h>
+#include <math.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <avr/sleep.h>
+#include <util/twi.h>
+#include <avr/eeprom.h>
+#include <avr/wdt.h>
+#include <avr/pgmspace.h>
+#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<<TWINT) | (1<<TWEA) | (1<<TWEN) | (1<<TWIE)
+#define TWI_NAK TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
+#define TWI_RESET TWCR = (1<<TWINT) | (1<<TWEA) | (1<<TWSTO) | (1<<TWEN) | (1<<TWIE);
+#define ENABLE_PWM_MOTOR1 TCCR1A |= (1 << COM1A1)
+#define ENABLE_PWM_MOTOR2 TCCR1A |= (1 << COM1B1)
+#define ENABLE_PWM_MOTOR3 TCCR2 |= (1 << COM21);
+#define ENABLE_PWM_MOTOR4 TCCR0 |= (1 << COM01);
+#define DISABLE_PWM_MOTOR1 TCCR1A &= ~(1 << COM1A1)
+#define DISABLE_PWM_MOTOR2 TCCR1A &= ~(1 << COM1B1)
+#define DISABLE_PWM_MOTOR3 TCCR2 &= ~(1 << COM21);
+#define DISABLE_PWM_MOTOR4 TCCR0 &= ~(1 << COM01);
+
+
+enum mode {
+ MOTOR_MANUAL,
+ MOTOR_PID
+};
+
+typedef union {
+ float f;
+ uint32_t i;
+} ufloat_t;
+
+static volatile struct {
+ float speed;
+ float angle;
+ uint8_t bUpdate;
+} cmd_vel = {0, 0, 0};
+
+static volatile uint8_t ireg=0;
+static volatile uint8_t bootloader=0;
+static volatile int16_t motor1=0; // -255..+255
+static volatile int16_t motor2=0;
+static volatile int16_t motor3=0;
+static volatile int16_t motor4=0;
+static volatile int16_t pos1=0; // step
+static volatile int16_t pos2=0;
+static volatile int16_t pos3=0;
+static volatile int16_t pos4=0;
+static volatile enum mode motor1_mode=MOTOR_MANUAL;
+static volatile enum mode motor2_mode=MOTOR_MANUAL;
+static volatile enum mode motor3_mode=MOTOR_MANUAL;
+static volatile enum mode motor4_mode=MOTOR_MANUAL;
+static volatile uint8_t motor1_switch=0;
+static volatile uint8_t motor2_switch=0;
+static volatile uint8_t motor3_switch=1;
+static volatile uint8_t motor4_switch=1;
+static volatile int16_t speed1_wish=0; // step/s
+static volatile int16_t speed2_wish=0;
+static volatile int16_t speed3_wish=0;
+static volatile int16_t speed4_wish=0;
+static volatile int16_t speed1_wish_old=0;
+static volatile int16_t speed2_wish_old=0;
+static volatile int16_t speed3_wish_old=0;
+static volatile int16_t speed4_wish_old=0;
+static volatile uint8_t run_update=0;
+static volatile uint8_t sound_update=0;
+static volatile uint8_t sound_max=0;
+static volatile int16_t speed1=0; // step/s
+static volatile int16_t speed2=0;
+static volatile int16_t speed3=0;
+static volatile int16_t speed4=0;
+static volatile ufloat_t pos_x={0.0};
+static volatile ufloat_t pos_y={0.0};
+static volatile ufloat_t angle={0.0};
+static volatile float cur_speed_lin=0;
+static volatile float cur_speed_rot=0;
+static volatile uint8_t count_test=0;
+static volatile uint8_t front_handicap=0;
+static volatile uint8_t aft_handicap=0;
+static volatile uint8_t error_state=0;
+static volatile uint8_t last_man_update_count=0;
+static volatile uint8_t last_i2c_status = 0;
+static uint8_t last_i2c_status_boot = 0;
+static volatile uint8_t watchdog_enable = 0;
+
+static void set_motor4_sound(uint16_t);
+
+ISR(TWI_vect)
+{
+ static uint8_t tmp=0;
+ static int16_t tmp16=0;
+ static ufloat_t tmp_speed;
+ static ufloat_t tmp_angle;
+
+ last_i2c_status = TW_STATUS;
+ switch(TW_STATUS)
+ {
+ case TW_SR_SLA_ACK: // start write
+ TWI_ACK;
+ ireg = 0;
+ break;
+ case TW_SR_DATA_ACK: // write
+ switch(ireg) {
+ case 0x00: // register select
+ ireg = TWDR;
+ ireg--; // because we do ireg++ below
+ TWI_ACK;
+ break;
+ case 0x01: // Motor 1 MSB
+ tmp = TWDR;
+ TWI_ACK;
+ break;
+ case 0x02: // Motor 1 LSB
+ motor1 = tmp<<8 | TWDR;
+ motor1_mode = MOTOR_MANUAL;
+ TWI_ACK;
+ break;
+ case 0x03: // Motor 2 MSB
+ tmp = TWDR;
+ TWI_ACK;
+ break;
+ case 0x04: // Motor 2 LSB
+ motor2 = tmp<<8 | TWDR;
+ motor2_mode = MOTOR_MANUAL;
+ TWI_ACK;
+ break;
+ case 0x05: // Motor 3 MSB
+ tmp = TWDR;
+ TWI_ACK;
+ break;
+ case 0x06: // Motor 3 LSB
+ motor3 = tmp<<8 | TWDR;
+ motor3_mode = MOTOR_MANUAL;
+ TWI_ACK;
+ break;
+ case 0x07: // Motor 4 MSB
+ tmp = TWDR;
+ TWI_ACK;
+ break;
+ case 0x08: // Motor 4 LSB
+ motor4 = tmp<<8 | TWDR;
+ motor4_mode = MOTOR_MANUAL;
+ TWI_ACK;
+ break;
+ case 0x20: // Motor 1 speed wish MSB
+ tmp = TWDR;
+ TWI_ACK;
+ break;
+ case 0x21: // Motor 1 speed wish LSB
+ speed1_wish = tmp<<8 | TWDR;
+ motor1_mode = MOTOR_PID;
+ TWI_ACK;
+ break;
+ case 0x22: // Motor 2 speed wish MSB
+ tmp = TWDR;
+ TWI_ACK;
+ break;
+ case 0x23: // Motor 2 speed wish LSB
+ speed2_wish = tmp<<8 | TWDR;
+ motor2_mode = MOTOR_PID;
+ TWI_ACK;
+ break;
+ case 0x24: // Motor 3 speed wish MSB
+ tmp = TWDR;
+ TWI_ACK;
+ break;
+ case 0x25: // Motor 3 speed wish LSB
+ speed3_wish = tmp<<8 | TWDR;
+ motor3_mode = MOTOR_PID;
+ TWI_ACK;
+ break;
+ case 0x26: // Motor 4 speed wish MSB
+ tmp = TWDR;
+ TWI_ACK;
+ break;
+ case 0x27: // Motor 4 speed wish LSB
+ speed4_wish = tmp<<8 | TWDR;
+ motor4_mode = MOTOR_PID;
+ TWI_ACK;
+ break;
+ case 0x28: // Left speed wish MSB
+ tmp_speed.i = TWDR;
+ TWI_ACK;
+ break;
+ case 0x29: // Left speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x2A: // Left speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x2B: // Left speed wish LSB
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ speed1_wish = tmp_speed.f*STEP_PER_M_LEFT;
+ speed2_wish = tmp_speed.f*STEP_PER_M_LEFT;
+ motor1_mode = MOTOR_PID;
+ motor2_mode = MOTOR_PID;
+ TWI_ACK;
+ break;
+ case 0x2C: // Right speed wish MSB
+ tmp_speed.i = TWDR;
+ TWI_ACK;
+ break;
+ case 0x2D: // Right speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x2E: // Right speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x2F: // Right speed wish LSB
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ speed1_wish = tmp_speed.f*STEP_PER_M_RIGHT;
+ speed2_wish = tmp_speed.f*STEP_PER_M_RIGHT;
+ motor1_mode = MOTOR_PID;
+ motor2_mode = MOTOR_PID;
+ TWI_ACK;
+ break;
+ case 0x50: // speed wish MSB
+ tmp_speed.i = TWDR;
+ TWI_ACK;
+ break;
+ case 0x51: // speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x52: // speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x53: // speed wish LSB
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ cmd_vel.speed = tmp_speed.f;
+ TWI_ACK;
+ break;
+ case 0x54: // angle wish MSB
+ tmp_angle.i = TWDR;
+ TWI_ACK;
+ break;
+ case 0x55: // angle wish
+ tmp_angle.i = tmp_angle.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x56: // angle wish
+ tmp_angle.i = tmp_angle.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x57: // angle wish LSB
+ tmp_angle.i = tmp_angle.i << 8 | TWDR;
+ cmd_vel.angle = tmp_angle.f;
+ cmd_vel.bUpdate = 1;
+ last_man_update_count = 0;
+ TWI_ACK;
+ break;
+ case 0x90: // Motor 1 switch
+ motor1_switch = TWDR;
+ TWI_ACK;
+ break;
+ case 0x91: // Motor 2 switch
+ motor2_switch = TWDR;
+ TWI_ACK;
+ break;
+ case 0x92: // Motor 3 switch
+ motor3_switch = TWDR;
+ TWI_ACK;
+ break;
+ case 0x93: // Motor 4 switch
+ motor4_switch = TWDR;
+ TWI_ACK;
+ break;
+ case 0x94: // Front Handicap
+ front_handicap = TWDR;
+ cmd_vel.bUpdate = 1;
+ TWI_ACK;
+ break;
+ case 0x95: // Aft Handicap
+ aft_handicap = TWDR;
+ cmd_vel.bUpdate = 1;
+ TWI_ACK;
+ break;
+ case 0xA4: // Watchdog enable
+ watchdog_enable = TWDR;
+ TWI_ACK;
+ break;
+ case 0xB0: // Sound freq MSB
+ tmp16 = TWDR;
+ TWI_ACK;
+ break;
+ case 0xB1: // Sound freq LSB
+ tmp16 = tmp16<<8 | TWDR;
+ set_motor4_sound(tmp16);
+ TWI_ACK;
+ break;
+ case 0xff: // bootloader
+ bootloader = TWDR;
+ default:
+ TWI_NAK;
+ }
+ if (ireg < 0xff) ireg++;
+ break;
+ case TW_ST_SLA_ACK: // start read
+ case TW_ST_DATA_ACK: // read
+ switch(ireg) {
+ case 0x01: // Dummy to allow continous read
+ TWDR = 0;
+ TWI_ACK;
+ break;
+ case 0x02: // Motor 1 PWM
+ TWDR = OCR1A;
+ TWI_ACK;
+ break;
+ case 0x03: // Dummy to allow continous read
+ TWDR = 0;
+ TWI_ACK;
+ break;
+ case 0x04: // Motor 2 PWM
+ TWDR = OCR1B;
+ TWI_ACK;
+ break;
+ case 0x05: // Dummy to allow continous read
+ TWDR = 0;
+ TWI_ACK;
+ break;
+ case 0x06: // Motor 3 PWM
+ TWDR = OCR2;
+ TWI_ACK;
+ break;
+ case 0x07: // Dummy to allow continous read
+ TWDR = 0;
+ TWI_ACK;
+ break;
+ case 0x08: // Motor 4 PWM
+ TWDR = OCR0;
+ TWI_ACK;
+ break;
+ case 0x09: // Dummy to allow continous read
+ TWDR = 0;
+ TWI_ACK;
+ break;
+ case 0x10: // Hall 1 MSB
+ tmp16 = pos1;
+ TWDR = tmp16>>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;
+}
--- /dev/null
+:100000000C943B000C9458000C9458000C9458002D\r
+:100010000C9458000C9458000C9458000C94580000\r
+:100020000C9458000C94EA040C9458000C94B105FC\r
+:100030000C9458000C94ED050C9458000C94580046\r
+:100040000C9458000C9458000C9458000C94C20165\r
+:100050000C9458000D0A53746172740D0A004546E1\r
+:100060004765666700202B2D2E303132333435360C\r
+:1000700037383968000011241FBECFE5D8E0DEBF55\r
+:10008000CDBF10E0A0E6B0E0E0E9FEE202C00590DE\r
+:100090000D92A037B107D9F721E0A0E7B0E001C089\r
+:1000A0001D92AC36B207E1F70E94DD060C942616CD\r
+:1000B0000C9400008091A300807F8093A30080B304\r
+:1000C00036B32091A30090E086FB8827992783F917\r
+:1000D0003770832B3FE08327822B8093A30080918E\r
+:1000E000EA009091EB00209166003091670028179C\r
+:1000F000390701F18091EA009091EB00892B09F01A\r
+:1001000099C085B3837F85BB8FB58F778FBD809175\r
+:10011000EA009091EB0090936700809366008091D5\r
+:10012000EA009091EB0097FF03C0919581959109AA\r
+:100130009BBD8ABD8091E8009091E9002091640008\r
+:10014000309165002817390701F18091E8009091FE\r
+:10015000E900892B09F098C085B38F7C85BB8FB5EA\r
+:100160008F7D8FBD8091E8009091E90090936500AC\r
+:10017000809364008091E8009091E90097FF03C0AC\r
+:1001800091958195910999BD88BD8091E6009091E6\r
+:10019000E700209162003091630028173907F9F0D9\r
+:1001A0008091E6009091E700892B09F097C085B314\r
+:1001B0008F7385BB85B58F7D85BD8091E60090915D\r
+:1001C000E70090936300809362008091E600909135\r
+:1001D000E70097FF03C091958195910983BD8091B8\r
+:1001E000E4009091E50020916000309161002817B3\r
+:1001F0003907F9F08091E4009091E500892B09F02E\r
+:1002000097C082B3837F82BB83B78F7D83BF80918A\r
+:10021000E4009091E50090936100809360008091EC\r
+:10022000E4009091E50097FF03C0919581959109B5\r
+:100230008CBF08958091EA009091EB008115904861\r
+:1002400031F485B38C6085BB8FB580685FCF8091BA\r
+:10025000D700811107C08091EA009091EB00181639\r
+:10026000190654F08091D700811551F08091EA0071\r
+:100270009091EB0097FF04C085B3877F8460E3CF44\r
+:1002800085B38B7F8860DFCF8091E8009091E90093\r
+:100290008115904831F485B3806385BB8FB580624A\r
+:1002A00060CF8091D600811107C08091E8009091C5\r
+:1002B000E9001816190654F08091D600811551F006\r
+:1002C0008091E8009091E90097FF04C085B38F7D8D\r
+:1002D0008061E3CF85B38F7E8062DFCF8091E600BF\r
+:1002E0009091E7008115904831F485B3806C85BB0F\r
+:1002F00085B5806261CF80916900811107C08091CE\r
+:10030000E6009091E7001816190654F080916900F4\r
+:10031000811551F08091E6009091E70097FF04C0AD\r
+:1003200085B38F778064E3CF85B38F7B8068DFCF21\r
+:100330008091E4009091E5008115904831F482B3FA\r
+:100340008C6082BB83B7806261CF8091680081112D\r
+:1003500007C08091E4009091E5001816190654F04A\r
+:1003600080916800811551F08091E4009091E50042\r
+:1003700097FF04C082B3877F8460E3CF82B38B7F13\r
+:100380008860DFCF1F920F920FB60F9211242F9328\r
+:100390003F934F935F936F937F938F939F93AF930D\r
+:1003A000BF93EF93FF9381B1887F8093A10081B1C8\r
+:1003B000887F803A09F40BC330F48036A9F0803886\r
+:1003C00069F185ED05C3883B21F0883F89F0883AC3\r
+:1003D000C1F7E091ED00E150E43A08F0F5C2F0E039\r
+:1003E000E95CF94E0C945E1385EC86BF1092ED002B\r
+:1003F00080919F00823009F4A895FF91EF91BF9101\r
+:10040000AF919F918F917F916F915F914F913F91AC\r
+:100410002F910F900FBE0F901F901895E091ED0057\r
+:10042000E83570F0E53A88F4E03910F485E825C045\r
+:10043000E059E531D8F7F0E0E552F94E0C945E133F\r
+:10044000F0E0E051F94E0C945E13E13B09F43FC13A\r
+:10045000EF3F09F460C1E03B49F783B190E090932E\r
+:1004600095008093940008C083B18093ED00809143\r
+:10047000ED0081508093ED0085EC86BF8091ED000A\r
+:100480008F3F09F4B5CF8091ED008F5F8093ED0031\r
+:10049000AFCF83B190919E009093EB008093EA00E0\r
+:1004A0001092DB00E9CF83B190919E009093E90018\r
+:1004B0008093E8001092DA00DFCF83B190919E0024\r
+:1004C0009093E7008093E6001092D900D5CF83B1D6\r
+:1004D00090919E009093E5008093E4001092D800E4\r
+:1004E000CBCF83B190919E009093D5008093D400A0\r
+:1004F00081E08093DB00C0CF83B190919E00909308\r
+:10050000D3008093D20081E08093DA00B5CF83B12D\r
+:1005100090919E009093D1008093D00081E08093D1\r
+:10052000D900AACF83B180939E00A6CF83B19091CA\r
+:100530009E009093CF008093CE0081E08093D800FE\r
+:100540009BCF23B180919A0090919B00A0919C0039\r
+:10055000B0919D00BA2FA92F982F8827822B8093C6\r
+:100560009A0090939B00A0939C00B0939D0084CF31\r
+:1005700023B160919A0070919B0080919C009091B2\r
+:100580009D00982F872F762F6627622B60939A0005\r
+:1005900070939B0080939C0090939D002AE939E41E\r
+:1005A0004BE955E40E946D120E944111462F572FCE\r
+:1005B0005093D5004093D4005093D3004093D20081\r
+:1005C00081E08093DB00A0CF83B190E0A0E0B0E0B9\r
+:1005D000C6CF23B180919A0090919B00A0919C007E\r
+:1005E000B0919D00BA2FA92F982F8827822B809336\r
+:1005F0009A0090939B00A0939C00B0939D008093E1\r
+:10060000EE009093EF00A093F000B093F10034CF90\r
+:1006100083B190E0A0E0B0E08093960090939700C3\r
+:10062000A0939800B093990027CF23B180919600B2\r
+:1006300090919700A0919800B0919900BA2FA92F9E\r
+:10064000982F8827822BE8CF23B180919600909134\r
+:100650009700A0919800B0919900BA2FA92F982FD8\r
+:100660008827822B8093960090939700A093980000\r
+:10067000B09399008093F2009093F300A093F4005C\r
+:10068000B093F50081E08093F6001092A200F4CEC2\r
+:1006900083B18093D700F0CE83B18093D600ECCEA7\r
+:1006A00083B180936900E8CE83B180936800E4CE83\r
+:1006B00083B18093A50081E08093F600DDCE83B105\r
+:1006C0008093A400F8CF83B180939F00D5CE83B1EF\r
+:1006D00060E070E070919400682B709395006093D7\r
+:1006E00094006115710579F08CE496E20E942B1359\r
+:1006F0006093C3001092C40089B7816089BF8FEFF7\r
+:100700008093A200B9CE89B78E7F89BF1092E50091\r
+:100710001092E400B1CE83B18093EC0087CE8AB50D\r
+:100720009BB583B908C088B599B5FBCF83B5F9CF20\r
+:100730008CB7F7CF13B885EC86BFA5CE8091E200C9\r
+:100740009091E300909395008093940093B9F3CF38\r
+:100750008091E0009091E100F5CF8091DE009091D2\r
+:10076000DF00F0CF8091DC009091DD00EBCF809135\r
+:100770009400D7CF8091D4009091D500E7CF80919D\r
+:10078000D4009091D500CDCF8091D2009091D3002C\r
+:10079000DDCF8091D2009091D300C3CF8091D00063\r
+:1007A0009091D100D3CF8091D0009091D100B9CF5A\r
+:1007B0008091CE009091CF00C9CF8091CE009091D2\r
+:1007C000CF00AFCF8091C1009091C200BFCF809188\r
+:1007D000C1009091C200A5CF8091BF009091C00050\r
+:1007E000B5CF8091BF009091C0009BCF8091BD009C\r
+:1007F0009091BE00ABCF8091BD009091BE0091CF93\r
+:100800008091BB009091BC00A1CF8091BB009091E2\r
+:10081000BC0087CF8091AB009091AC00A091AD005F\r
+:10082000B091AE0080939A0090939B00A0939C009F\r
+:10083000B0939D008B2F9927AA27BB2772CF809159\r
+:100840009A0090919B00A0919C00B0919D00CD01D9\r
+:10085000AA27BB2766CF80919A0090919B00A09118\r
+:100860009C00B0919D00892F9A2FAB2FBB2759CFA9\r
+:1008700080919A0056CF8091A7009091A800A091F6\r
+:10088000A900B091AA008093960090939700A0933E\r
+:100890009800B0939900CECF8091960090919700E8\r
+:1008A000A0919800B0919900D2CF8091960090913C\r
+:1008B0009700A0919800B0919900D5CF80919600B3\r
+:1008C00030CF8091B7009091B800A091B900B0915D\r
+:1008D000BA00B0CF8091B7009091B800A091B90054\r
+:1008E000B091BA00B4CF8091B7009091B800A091B8\r
+:1008F000B900B091BA00B7CF8091B7009091B8001D\r
+:10090000A091B900B091BA000CCF8091B300909142\r
+:10091000B400A091B500B091B6008CCF8091B30027\r
+:100920009091B400A091B500B091B60090CF8091A5\r
+:10093000B3009091B400A091B500B091B60093CFF0\r
+:100940008091B3009091B400A091B500B091B60031\r
+:10095000E8CE8091AF009091B000A091B100B0912D\r
+:10096000B20068CF8091AF009091B000A091B1002B\r
+:10097000B091B2006CCF8091AF009091B000A09187\r
+:10098000B100B091B2006FCF8091AF009091B000F4\r
+:10099000A091B100B091B200C4CE84B78F7083B97A\r
+:1009A00014BEC9CE8091A300BCCE8091A60083B9AD\r
+:1009B00085EC86BF8091A00083B985EC86BF8091CD\r
+:1009C0009F0083B985EC86BF13B885E8B5CE85EC6A\r
+:1009D00086BF0ECD1F921FB61F9211242F933F93F7\r
+:1009E0008F939F9389B3982F917080FD93E081FF3F\r
+:1009F00002C081E0982780919300891B80FF14C07A\r
+:100A0000909393002091D70082708150990B21150B\r
+:100A100009F48BC02091E2003091E300281B390BD0\r
+:100A20003093E3002093E20089B3282F22952F70A2\r
+:100A300084FB992790F984FD93E021FF02C081E0B7\r
+:100A4000982780919200891B80FF14C09093920098\r
+:100A50002091D60082708150990B211509F46CC049\r
+:100A60002091E0003091E100280F391F3093E10020\r
+:100A70002093E00089B3282F2695269582FB99279D\r
+:100A800090F982FD93E021FF02C081E098278091D8\r
+:100A90009100891B80FF14C0909391002091690000\r
+:100AA00082708150990B211509F44DC02091DE0010\r
+:100AB0003091DF00280F391F3093DF002093DE00D4\r
+:100AC00029B3822F829586958695837026FB992778\r
+:100AD00090F926FD93E08270811511F081E098274E\r
+:100AE00080919000891B80FF13C09093900020910B\r
+:100AF000680082708150990B211561F12091DC0012\r
+:100B00003091DD00281B390B3093DD002093DC0091\r
+:100B10008091C5008F5F8093C5009F918F913F9119\r
+:100B20002F911F901FBE1F9018952091E2003091C9\r
+:100B3000E300280F391F74CF2091E0003091E100CD\r
+:100B4000281B390B93CF2091DE003091DF00281B4A\r
+:100B5000390BB2CF2091DC003091DD00280F391F16\r
+:100B6000D3CF1F920F920FB60F9211242F933F9362\r
+:100B70004F935F936F937F938F939F93AF93BF93A5\r
+:100B8000EF93FF938091C4008F5F8093C400909196\r
+:100B9000C4008091C300981778F01092C40080912F\r
+:100BA000E4009091E5009195819591099093E5007D\r
+:100BB0008093E4000E945A00FF91EF91BF91AF91A2\r
+:100BC0009F918F917F916F915F914F913F912F9165\r
+:100BD0000F900FBE0F901F9018958F938FB78F9324\r
+:100BE0009F93EF93FF935F9B0FC0E091F900F0910B\r
+:100BF000FA008CB1808381E0EE35F80760F431961D\r
+:100C0000F093FA00E093F900FF91EF919F918F919B\r
+:100C10008FBF8F9118958BEF90E09093FA0080939F\r
+:100C2000F900F2CF2AB128612AB9579A20B5266077\r
+:100C300020BDA0E0B0E09C01AD0184E0220F331F95\r
+:100C4000441F551F8A95D1F760E47BE48CE490E063\r
+:100C50000E943F13DA01C9010197A109B109892F47\r
+:100C60009A2FAB2FBB27A7FDBA9580BD215029B97C\r
+:100C70008BEF90E09093F8008093F7009093FA0048\r
+:100C80008093F9000895FC015D9BFECF80818CB9B3\r
+:100C90000895CF93DF930F92CDB7DEB78983CE014E\r
+:100CA00001960E94430680E090E00F90DF91CF9183\r
+:100CB0000895CF93DF93EC01CE012196FC012081B2\r
+:100CC000211103C0DF91CF9108950E944306F4CF14\r
+:100CD0005F9BFECF8CB190E008950C9468066DE6A2\r
+:100CE00076E089E496E00C94C813CF93DF939C01DF\r
+:100CF000FC01E253FF4FA081B181EC01C453DF4FEF\r
+:100D000088819981A817B90781F05D9B0EC08C91ED\r
+:100D10008CB980819181A90145535F4F8417950754\r
+:100D200008F040C0019691838083E091F700F09134\r
+:100D3000F8008091F9009091FA008E179F0779F1E1\r
+:100D4000B9016C597F4FDB01CD91DC911197808106\r
+:100D50008883CD91DC91C9018B529F4FDC019C911E\r
+:100D60008881981307C0A9014C525F4FDA018C911A\r
+:100D70008F5F8C93C9018D599F4FC817D90718F4FD\r
+:100D80009E012F5F3F4FDB012D933C93B1E0EE3589\r
+:100D9000FB0760F43196F093F800E093F700DF91E1\r
+:100DA000CF910895C90188599F4FBDCF8BEF90E037\r
+:100DB0009093F8008093F700F2CFCDB7DEB76E972F\r
+:100DC0000FB6F894DEBF0FBECDBF88E087BB8CEFB7\r
+:100DD00084BB8CEB81BB87E088BB80E482BB109234\r
+:100DE000EC0080E895E20E9412060E946F0680E502\r
+:100DF00082B985EC86BF81E08FBD8AE08EBD1BBCC9\r
+:100E00001ABC19BC18BC8AE485BD13BC83BF1CBEC8\r
+:100E100084E590E09F938F930E94421481E090E0DC\r
+:100E20000E9411168093A00085B78F7885BF84E05B\r
+:100E300089BF78940F900F9084E6282E312C8091F2\r
+:100E4000ED00843A09F479C38F3F09F496C3809189\r
+:100E5000F600811509F4CFC0F894C090EE00D09050\r
+:100E6000EF00E090F000F090F1006091F2007091DE\r
+:100E7000F3008091F4009091F5001092F6007894C0\r
+:100E80002AEC37E34BEF5EE30E946D1220E030E086\r
+:100E900040E05FE30E946D12A70196010E94591085\r
+:100EA0008B018D839983A7019601C701B6010E942A\r
+:100EB000591098014D8159810E9458102AE939E44E\r
+:100EC0004BE955E40E946D126B017C012AE939E47B\r
+:100ED0004BE955E4B8018D8199810E946D128B0117\r
+:100EE0008D8399838091A400811509F45CC360917E\r
+:100EF000A4009101261B3109B901072E000C880BB3\r
+:100F0000990B0E9479119601A7010E946D1220E0B1\r
+:100F100030E048EC52E40E94CF100E9441117093DF\r
+:100F2000D5006093D4006091A400C101861B910993\r
+:100F3000BC01072E000C880B990B0E9479119801B7\r
+:100F40004D8159810E946D1220E030E048EC52E45E\r
+:100F50000E94CF100E9441117093D1006093D00085\r
+:100F60008091A500811509F42AC36091A500D101E3\r
+:100F7000A61BB109BD01072E000C880B990B0E941E\r
+:100F800079119601A7010E946D1220E030E048EC33\r
+:100F900052E40E94CF100E944111462F572F5093C8\r
+:100FA000D3004093D2006091A5009101261B310926\r
+:100FB000B901072E000C880B990B0E94791198013A\r
+:100FC0004D8159810E946D1220E030E048EC52E4DE\r
+:100FD0000E94CF100E944111462F572F5093CF00EF\r
+:100FE0004093CE0031E03093DB003093DA00309351\r
+:100FF000D9003093D8008091C5008E3408F47AC6A9\r
+:101000001092C500F8948091E2009091E3009A8BD1\r
+:10101000898BA091E000B091E100BC8BAB8B20915B\r
+:10102000DE003091DF003E8B2D8B8091DC009091B3\r
+:10103000DD00988F8F8B789480918E0090918F0037\r
+:10104000C988DA88C81AD90A80918C0090918D00DD\r
+:101050007D01E81AF90A80918A0090918B00281B83\r
+:10106000390B3A8F298F80918800909189000F89E0\r
+:10107000188D081B190BB6010D2C000C880B990B51\r
+:101080000E9479112AE037ED43E25CE30E94CF1021\r
+:101090000E94411169837A838B839C83B7010F2C53\r
+:1010A000000C880B990B0E9479112AE037ED43E27E\r
+:1010B0005CE30E94CF100E9441116D837E838F8379\r
+:1010C0009887A98DBA8DBD01BB0F880B990B0E9423\r
+:1010D00079112AE037ED43E25CE30E94CF100E94D1\r
+:1010E000411169877A878B879C87B801012E000C94\r
+:1010F000880B990B0E9479112AE037ED43E25CE3FB\r
+:101100000E94CF100E9441116D877E878F87988B38\r
+:10111000B6016E0D7F1D072E000C880B990B0E94E7\r
+:1011200079112AE939E44BE156E40E94CF104B01D2\r
+:101130005C01698D7A8D600F711F072E000C880B82\r
+:10114000990B0E9479112AE939E44BE156E40E9497\r
+:10115000CF102B013C019401A5010E9458102AECEC\r
+:1011600037E34BEF5EE30E94CF106D8F7E8F182F19\r
+:10117000092F2091AF003091B0004091B100509103\r
+:10118000B2000E9459106B017C012BED3FE049EC4D\r
+:1011900050E4C701B6010E94681218160CF01DC277\r
+:1011A0002BED3FE049EC50E4C701B6010E94581016\r
+:1011B0006B017C019201A301B401C5010E94591089\r
+:1011C00020E030E040E05FE30E946D124B015C01E3\r
+:1011D0004090B7005090B8006090B9007090BA008D\r
+:1011E000C701B6010E94CA109B01AC01B401C50140\r
+:1011F0000E946D12A30192010E945910AB01BC0123\r
+:10120000498F5A8F6B8F7C8F4090B3005090B40001\r
+:101210006090B5007090B600C701B6010E94DA1266\r
+:101220009B01AC01B401C5010E946D12A3019201A2\r
+:101230000E9459102B013C012AE037ED43E25CE3A8\r
+:10124000B401C5010E94CF104B015C012AE037EDCB\r
+:1012500043E25CE36D8D7E8D812F902F0E94CF1035\r
+:10126000AB01BC01F894C092AF00D092B000E09204\r
+:10127000B100F092B200898D9A8DAB8DBC8D8093B8\r
+:10128000B7009093B800A093B900B093BA00409211\r
+:10129000B3005092B4006092B5007092B600A9817C\r
+:1012A000BA81B093C200A093C1002D813E813093DA\r
+:1012B000C0002093BF0089859A859093BE008093DB\r
+:1012C000BD00AD85BE85B093BC00A093BB008092ED\r
+:1012D000AB009092AC00A092AD00B092AE004093F3\r
+:1012E000A7005093A8006093A9007093AA00789477\r
+:1012F00029893A8930938F0020938E008B899C89AD\r
+:1013000090938D0080938C00AD89BE89B0938B0043\r
+:10131000A0938A002F89388D30938900209388000C\r
+:101320008091840090918500A0918600B091870003\r
+:10133000B7FF07C0B095A095909581959F4FAF4F8F\r
+:10134000BF4F813E9242A240B105CCF08091C100D6\r
+:101350009091C200892B99F41092EB001092EA0050\r
+:101360001092DB008091A30080618093A300109213\r
+:101370008400109285001092860010928700809160\r
+:10138000800090918100A0918200B0918300B7FF0E\r
+:1013900007C0B095A095909581959F4FAF4FBF4FD7\r
+:1013A000813E9242A240B105CCF08091BF00909165\r
+:1013B000C000892B99F41092E9001092E800109275\r
+:1013C000DA008091A30080628093A30010928000D5\r
+:1013D00010928100109282001092830080917C0014\r
+:1013E00090917D00A0917E00B0917F00B7FF07C073\r
+:1013F000B095A095909581959F4FAF4FBF4F813E7F\r
+:101400009242A240B105CCF08091BD009091BE0007\r
+:10141000892B99F41092E7001092E6001092D900FF\r
+:101420008091A30080648093A30010927C001092AE\r
+:101430007D0010927E0010927F0080917800909144\r
+:101440007900A0917A00B0917B00B7FF07C0B095FA\r
+:10145000A095909581959F4FAF4FBF4F813E92428F\r
+:10146000A240B105CCF08091BB009091BC00892BCB\r
+:1014700099F41092E5001092E4001092D800809147\r
+:10148000A30080688093A3001092780010927900E6\r
+:1014900010927A0010927B008091DB00813009F07D\r
+:1014A000C6C02091D4003091D5008091CC0090919D\r
+:1014B000CD002817390719F12091D4003091D500BB\r
+:1014C0004091CC005091CD00C901841B950B97FF32\r
+:1014D00003C0CA01821B930B853F914044F01092D8\r
+:1014E00084001092850010928600109287008091EF\r
+:1014F000D4009091D5009093CD008093CC00809142\r
+:10150000D4009091D5001816190634F48091C100CA\r
+:101510009091C20097FD74C08091D4009091D50045\r
+:1015200097FF09C02091C1003091C20081E01216DE\r
+:1015300013060CF466C080E064C080919F00813087\r
+:1015400061F428E13FE00FB6F894A89521BD0FBEE5\r
+:1015500031BD82E080939F007ACC80919F00811101\r
+:1015600076CC0FB6F894A89581B5886181BD11BC81\r
+:101570000FBE93E090939F006ACC8091EC00853A77\r
+:1015800009F065CCF8946BE780E090E00E941916B2\r
+:10159000E199FECFA8E1B8E00FB6F894A895A1BDF7\r
+:1015A0000FBEB1BD54CCB601C7010E94411170936A\r
+:1015B000D5006093D400B8018D819981CBCCB60160\r
+:1015C000C7010E944111462F572F5093D3004093DB\r
+:1015D000D200B8018D819981FDCC2BED3FE049EC23\r
+:1015E00050ECC701B6010E94C51087FFE3CD2BED7B\r
+:1015F0003FE049EC50E4C701B6010E945910D8CD34\r
+:1016000081E02091D4003091D500232B19F0811571\r
+:1016100009F45EC01092EB001092EA00109277007D\r
+:10162000109276008091A3008F7E8093A30080911A\r
+:10163000DA00813009F008C12091D2003091D30046\r
+:101640008091CA009091CB002817390719F1209199\r
+:10165000D2003091D3004091CA005091CB00C90113\r
+:10166000841B950B97FF03C0CA01821B930B853F18\r
+:10167000914044F0109280001092810010928200FC\r
+:10168000109283008091D2009091D3009093CB0070\r
+:101690008093CA008091D2009091D3001816190649\r
+:1016A00034F48091BF009091C00097FDB6C0809146\r
+:1016B000D2009091D30097FF09C02091BF003091D4\r
+:1016C000C00081E0121613060CF4A8C080E0A6C08A\r
+:1016D0008091D4009091D5009A8389838091C10034\r
+:1016E0009091C200A981BA81A81BB90BBA83A983C2\r
+:1016F000BD01BB0F880B990BC0908400D090850072\r
+:10170000E0908600F0908700C60ED71EE81EF91EF6\r
+:10171000C0928400D0928500E0928600F09287000B\r
+:101720000E9479112EE237E04CE25CE30E946D12D8\r
+:101730005B018C01C701B6010E9479112EE237E0EE\r
+:101740004CE25CE30E946D129B01AC01B501C80143\r
+:101750000E9459107B018C01809176009091770056\r
+:1017600069817A81681B790B072E000C880B990B15\r
+:101770000E94791120E030E0A9010E946D129B01C6\r
+:10178000AC01B701C8010E9459100E944111462FB7\r
+:10179000572F5093EB004093EA0029813A81309310\r
+:1017A0007700209376008091EA009091EB00181664\r
+:1017B00019066CF48091D4009091D50097FF07C072\r
+:1017C00080E090E89093EB008093EA0030CF809126\r
+:1017D000EA009091EB0097FF07C08091D4009091B0\r
+:1017E000D5001816190664F38091EA009091EB0079\r
+:1017F000811591403CF02FEF30E03093EB002093C7\r
+:10180000EA0015CF8091EA009091EB0081309F4F64\r
+:101810000CF00DCF81E09FEFD5CF81E02091D20079\r
+:101820003091D300232B19F0811509F45EC010927A\r
+:10183000E9001092E80010927500109274008091F7\r
+:10184000A3008F7D8093A3008091D900813009F09F\r
+:101850000BC12091D0003091D1008091C8009091AF\r
+:10186000C9002817390719F12091D0003091D10013\r
+:101870004091C8005091C900C901841B950B97FF86\r
+:1018800003C0CA01821B930B853F914044F0109224\r
+:101890007C0010927D0010927E0010927F0080915B\r
+:1018A000D0009091D1009093C9008093C80080919E\r
+:1018B000D0009091D1001816190634F48091BD0023\r
+:1018C0009091BE0097FDB9C08091D0009091D10059\r
+:1018D00097FF09C02091BD003091BE0081E0121633\r
+:1018E00013060CF4ABC080E0A9C02091D200309167\r
+:1018F000D3008091BF009091C000281B390B3A8320\r
+:101900002983B901330F880B990BC0908000D090C8\r
+:101910008100E0908200F0908300C60ED71EE81E82\r
+:10192000F91EC0928000D0928100E0928200F09275\r
+:1019300083000E9479112EE237E04CE25CE30E94C2\r
+:101940006D125B018C01C701B6010E9479112EE274\r
+:1019500037E04CE25CE30E946D129B01AC01B501E3\r
+:10196000C8010E9459107B018C01809174009091F4\r
+:10197000750069817A81681B790B072E000C880B32\r
+:10198000990B0E94791120E030E0A9010E946D12AC\r
+:101990009B01AC01B701C8010E9459100E9441117E\r
+:1019A000462F572F5093E9004093E80089819A8190\r
+:1019B00090937500809374008091E8009091E90005\r
+:1019C000181619066CF48091D2009091D30097FFFD\r
+:1019D00007C0A0E0B0E8B093E900A093E80034CFDE\r
+:1019E0008091E8009091E90097FF0EC08091D200AD\r
+:1019F0009091D300181619063CF480E090E890937B\r
+:101A0000E9008093E80020CF8091E8009091E90000\r
+:101A1000811591403CF02FEF30E03093E9002093A6\r
+:101A2000E80012CF8091E8009091E90081309F4F4B\r
+:101A30000CF00ACF81E09FEFE2CF81E02091D0004F\r
+:101A40003091D100232B19F0811509F45EC010925A\r
+:101A5000E7001092E60010927300109272008091DD\r
+:101A6000A3008F7B8093A3008091D800813009F080\r
+:101A70000BC12091CE003091CF008091C600909193\r
+:101A8000C7002817390719F12091CE003091CF00F7\r
+:101A90004091C6005091C700C901841B950B97FF68\r
+:101AA00003C0CA01821B930B853F914044F0109202\r
+:101AB00078001092790010927A0010927B00809149\r
+:101AC000CE009091CF009093C7008093C600809184\r
+:101AD000CE009091CF001816190634F48091BB0007\r
+:101AE0009091BC0097FDB9C08091CE009091CF003D\r
+:101AF00097FF09C02091BB003091BC0081E0121615\r
+:101B000013060CF4ABC080E0A9C02091D000309146\r
+:101B1000D1008091BD009091BE00281B390B3A8303\r
+:101B20002983B901330F880B990BC0907C00D090AA\r
+:101B30007D00E0907E00F0907F00C60ED71EE81E6C\r
+:101B4000F91EC0927C00D0927D00E0927E00F0925F\r
+:101B50007F000E9479112EE237E04CE25CE30E94A4\r
+:101B60006D125B018C01C701B6010E9479112EE252\r
+:101B700037E04CE25CE30E946D129B01AC01B501C1\r
+:101B8000C8010E9459107B018C01809172009091D4\r
+:101B9000730069817A81681B790B072E000C880B12\r
+:101BA000990B0E94791120E030E0A9010E946D128A\r
+:101BB0009B01AC01B701C8010E9459100E9441115C\r
+:101BC000462F572F5093E7004093E60089819A8172\r
+:101BD00090937300809372008091E6009091E700EB\r
+:101BE000181619066CF48091D0009091D10097FFDF\r
+:101BF00007C0A0E0B0E8B093E700A093E60034CFC0\r
+:101C00008091E6009091E70097FF0EC08091D00090\r
+:101C10009091D100181619063CF480E090E890935A\r
+:101C2000E7008093E60020CF8091E6009091E700E6\r
+:101C3000811591403CF02FEF30E03093E700209386\r
+:101C4000E60012CF8091E6009091E70081309F4F2F\r
+:101C50000CF00ACF81E09FEFE2CF81E02091CE002F\r
+:101C60003091CF00232B19F0811509F44BC010924D\r
+:101C7000E5001092E40010927100109270008091C3\r
+:101C8000A3008F778093A3000E945A008091A60042\r
+:101C90008F5F8093A6008091A2008F3F59F18091C1\r
+:101CA000A2008F5F8093A2008091A200843610F181\r
+:101CB0001092EE001092EF001092F0001092F100DE\r
+:101CC0001092F2001092F3001092F4001092F500BE\r
+:101CD00081E08093F6008091A200843641F4609107\r
+:101CE000A10081E090E00E941916E199FECF9FEFDC\r
+:101CF0009093A20085B7806885BF889585B78F7758\r
+:101D000085BF9DC82091CE003091CF008091BB004F\r
+:101D10009091BC00281B390B3A832983B901330FFA\r
+:101D2000880B990BC0907800D0907900E0907A00F1\r
+:101D3000F0907B00C60ED71EE81EF91EC0927800F8\r
+:101D4000D0927900E0927A00F0927B000E947911A3\r
+:101D50002EE237E04CE25CE30E946D125B018C01E5\r
+:101D6000C701B6010E9479112EE237E04CE25CE334\r
+:101D70000E946D129B01AC01B501C8010E9459106F\r
+:101D80007B018C01809170009091710069817A8152\r
+:101D9000681B790B072E000C880B990B0E94791198\r
+:101DA00020E030E0A9010E946D129B01AC01B70157\r
+:101DB000C8010E9459100E944111462F572F50937D\r
+:101DC000E5004093E40089819A81909371008093AB\r
+:101DD00070008091E4009091E500181619066CF4EB\r
+:101DE0008091CE009091CF0097FF07C0A0E0B0E8AF\r
+:101DF000B093E500A093E40047CF8091E400909178\r
+:101E0000E50097FF0EC08091CE009091CF0018168C\r
+:101E100019063CF480E090E89093E5008093E4009C\r
+:101E200033CF8091E4009091E500811591403CF022\r
+:101E30002FEF30E03093E5002093E40025CF809130\r
+:101E4000E4009091E50081309F4F0CF01DCF81E0C0\r
+:101E50009FEFE2CFABE0B0E0E0E3FFE00C9468136B\r
+:101E60005C016B013A01FC0117821682838181FFBC\r
+:101E70001CC1AE014F5F5F4F4A01F5010381F601BE\r
+:101E800003FD859103FF81916F01811509F4B8C0AD\r
+:101E9000853239F403FD159103FF11916F0115325D\r
+:101EA00039F4B50190E00E94121483013801E5CFA6\r
+:101EB000F12CE12C111509F4A3C0612F70E085E627\r
+:101EC00090E00E94BD13892B21F4133209F5689428\r
+:101ED000F4F8F60103FD159103FF11916F01EF2C4A\r
+:101EE000F7FEE8CF111509F48BC0612F70E08EE585\r
+:101EF00090E00E94BD13892BC1F08301F3FE12C054\r
+:101F0000085F1F4FB5018FE390E00E941214CECFFF\r
+:101F10001C3619F46894F7F8DCCF1C3431F7689458\r
+:101F2000F3F8D7CF0C5F1F4FEDCF1336E9F113371E\r
+:101F300009F442C01335F1F1143619F0193609F0DD\r
+:101F400055C0F301E7FE4AC0619171918191919171\r
+:101F50008F01E894F4F897FF09C0909580957095EB\r
+:101F600061957F4F8F4F9F4F6894F6F82AE030E0DD\r
+:101F7000AE014F5F5F4F3A010E946914E82EF6FCF4\r
+:101F80008EC0E618EA94F12C5FEFE51AF50AE80C2A\r
+:101F9000F91CF70182917F01B50190E00E941214B3\r
+:101FA0008E149F04B1F782CFB501F3018191919115\r
+:101FB0008F01ABCF6894F0F8F301619071908F01BD\r
+:101FC000F301F0FC8591F0FE81913F01811509F448\r
+:101FD0006DCFB50190E00E941214F2CF6191719122\r
+:101FE0008F01072E000C880B990BB3CF103709F423\r
+:101FF00045C068F4183509F444C01F3609F446C0DA\r
+:10200000F501868197812B96EEE00C9484131537A9\r
+:1020100041F01837A9F75F2D5462F52E20E130E02A\r
+:1020200004C0E894F4F82AE030E0F301F7FE31C090\r
+:1020300061917191819191918F01AE014F5F5F4FDD\r
+:102040003A010E94691486199709E82EF4FE9ACF86\r
+:10205000FE01E80FF11D8081803309F493CFB501B3\r
+:1020600080E390E00E941214F2FE8CCF8F2D8072DC\r
+:10207000885AB50190E00E94121484CF6894F4F855\r
+:10208000CACF6894F2F820E132E0CFCF28E030E008\r
+:10209000CCCF619171918F0180E090E0CECFB501FE\r
+:1020A0008DE290E00E9412146CCF8FEF9FEFABCFC8\r
+:1020B0005058BB27AA270E9470100C941A120E9435\r
+:1020C000E11138F00E94E81120F039F49F3F19F433\r
+:1020D00026F40C94DE110EF4E095E7FB0C94D81175\r
+:1020E000E92F0E943F1258F3BA176207730784075B\r
+:1020F000950720F079F4A6F50C9461120EF4E095A2\r
+:102100000B2EBA2FA02D0B01B90190010C01CA01B1\r
+:10211000A0011124FF27591B99F0593F50F4503E5C\r
+:1021200068F11A16F040A22F232F342F4427585F4E\r
+:10213000F3CF469537952795A795F0405395C9F766\r
+:102140007EF41F16BA0B620B730B840BBAF091501E\r
+:10215000A1F0FF0FBB1F661F771F881FC2F70EC0BD\r
+:10216000BA0F621F731F841F48F487957795679590\r
+:10217000B795F7959E3F08F0B0CF9395880F08F07C\r
+:102180009927EE0F9795879508950E94B41108F44A\r
+:1021900081E008950E94F111E3950C942B120E94A6\r
+:1021A000E3100C941A120E94E81158F00E94E111F9\r
+:1021B00040F029F45F3F29F00C94D81151110C9490\r
+:1021C00062120C94DE110E943F1268F39923B1F35E\r
+:1021D000552391F3951B550BBB27AA27621773074D\r
+:1021E000840738F09F5F5F4F220F331F441FAA1FE1\r
+:1021F000A9F335D00E2E3AF0E0E832D0915050409D\r
+:10220000E695001CCAF72BD0FE2F29D0660F771F4A\r
+:10221000881FBB1F261737074807AB07B0E809F030\r
+:10222000BB0B802DBF01FF2793585F4F3AF09E3FB5\r
+:10223000510578F00C94D8110C9462125F3FE4F3CE\r
+:10224000983ED4F3869577956795B795F7959F5FF8\r
+:10225000C9F7880F911D9695879597F90895E1E044\r
+:10226000660F771F881FBB1F621773078407BA07A3\r
+:1022700020F0621B730B840BBA0BEE1F88F7E095FE\r
+:1022800008950E9448116894B1110C946212089547\r
+:102290000E94471288F09F5798F0B92F9927B7519D\r
+:1022A000B0F0E1F0660F771F881F991F1AF0BA95FA\r
+:1022B000C9F714C0B13091F00E946112B1E00895E5\r
+:1022C0000C946112672F782F8827B85F39F0B93FD7\r
+:1022D000CCF3869577956795B395D9F73EF49095AD\r
+:1022E0008095709561957F4F8F4F9F4F0895E8942B\r
+:1022F00009C097FB3EF490958095709561957F4F4E\r
+:102300008F4F9F4F9923A9F0F92F96E9BB279395FB\r
+:10231000F695879577956795B795F111F8CFFAF40B\r
+:10232000BB0F11F460FF1BC06F5F7F4F8F4F9F4F3C\r
+:1023300016C0882311F096E911C0772321F09EE89A\r
+:10234000872F762F05C0662371F096E8862F70E000\r
+:1023500060E02AF09A95660F771F881FDAF7880FDA\r
+:102360009695879597F90895990F0008550FAA0B30\r
+:10237000E0E8FEEF16161706E807F907C0F0121698\r
+:102380001306E407F50798F0621B730B840B950B9B\r
+:1023900039F40A2661F0232B242B252B21F40895F0\r
+:1023A0000A2609F4A140A6958FEF811D811D08958D\r
+:1023B00097F99F6780E870E060E008959FEF80ECF8\r
+:1023C000089500240A94161617061806090608959B\r
+:1023D00000240A94121613061406050608950C9498\r
+:1023E000DE110E944712D8F3E894E0E0BB279F5724\r
+:1023F000F0F02AED3FE049EC06C0EE0FBB0F661F80\r
+:10240000771F881F28F0B23A62077307840728F005\r
+:10241000B25A620B730B840BE3959A9572F780386E\r
+:1024200030F49A95BB0F661F771F881FD2F790482C\r
+:102430000C94E412092E0394000C11F4882352F03A\r
+:10244000BB0F40F4BF2B11F460FF04C06F5F7F4FE0\r
+:102450008F4F9F4F0895EF93E0FF07C0A2EA2AED48\r
+:102460003FE049EC5FEB0E9470100E941A120F903F\r
+:10247000039401FC9058E0E5FCE20C94F01257FD47\r
+:102480009058440F551F59F05F3F71F04795880FE2\r
+:1024900097FB991F61F09F3F79F087950895121679\r
+:1024A00013061406551FF2CF4695F1DF08C0161625\r
+:1024B00017061806991FF1CF8695710561050894D6\r
+:1024C0000895E894BB2766277727CB0197F90895ED\r
+:1024D0000E94B41108F48FEF08950E9480120C94AA\r
+:1024E0001A120E94E11138F00E94E81120F09523A1\r
+:1024F00011F00C94D8110C94DE1111240C9462127A\r
+:102500000E943F1270F3959FC1F3950F50E0551F45\r
+:10251000629FF001729FBB27F00DB11D639FAA2738\r
+:10252000F00DB11DAA1F649F6627B00DA11D661F87\r
+:10253000829F2227B00DA11D621F739FB00DA11DA8\r
+:10254000621F839FA00D611D221F749F3327A00D62\r
+:10255000611D231F849F600D211D822F762F6A2FFE\r
+:1025600011249F5750409AF0F1F088234AF0EE0F63\r
+:10257000FF1FBB1F661F771F881F91505040A9F790\r
+:102580009E3F510580F00C94D8110C9462125F3F6D\r
+:10259000E4F3983ED4F3869577956795B795F795CC\r
+:1025A000E7959F5FC1F7FE2B880F911D9695879544\r
+:1025B00097F908959F930E94F1110F9007FCEE5F29\r
+:1025C0000C942B129F3F31F0915020F48795779512\r
+:1025D0006795B795880F911D9695879597F90895FA\r
+:1025E0009F938F937F936F93FF93EF939B01AC0126\r
+:1025F0000E946D12EF91FF910E9404132F913F9161\r
+:102600004F915F910C946D12DF93CF931F930F93B3\r
+:10261000FF92EF92DF927B018C01689406C0DA2E64\r
+:10262000EF010E948012FE01E894A5912591359159\r
+:1026300045915591A6F3EF010E947010FE0197019C\r
+:10264000A801DA9469F7DF90EF90FF900F911F9146\r
+:10265000CF91DF910895AA1BBB1B51E107C0AA1FB0\r
+:10266000BB1FA617B70710F0A61BB70B881F991F33\r
+:102670005A95A9F780959095BC01CD010895052E36\r
+:1026800097FB1EF400940E94561357FD07D00E943A\r
+:102690009B1307FC03D04EF40C94561350954095B1\r
+:1026A000309521953F4F4F4F5F4F089590958095FE\r
+:1026B000709561957F4F8F4F9F4F0895EE0FFF1FCD\r
+:1026C0000590F491E02D09942F923F924F925F92E2\r
+:1026D0006F927F928F929F92AF92BF92CF92DF9232\r
+:1026E000EF92FF920F931F93CF93DF93CDB7DEB797\r
+:1026F000CA1BDB0B0FB6F894DEBF0FBECDBF09942B\r
+:102700002A88398848885F846E847D848C849B8481\r
+:10271000AA84B984C884DF80EE80FD800C811B818F\r
+:10272000AA81B981CE0FD11D0FB6F894DEBF0FBEBE\r
+:10273000CDBFED010895A1E21A2EAA1BBB1BFD011E\r
+:102740000DC0AA1FBB1FEE1FFF1FA217B307E40790\r
+:10275000F50720F0A21BB30BE40BF50B661F771FE8\r
+:10276000881F991F1A9469F76095709580959095C8\r
+:102770009B01AC01BD01CF010895FC010590061637\r
+:1027800021F00020D9F7C00108953197CF010895B5\r
+:102790000F931F93CF93DF93EC018B01892B51F49F\r
+:1027A000672B41F4E0E0F0E0CF01DF91CF911F9182\r
+:1027B0000F9108956EE070E081E090E00E94C714F0\r
+:1027C000FC01892B79F380E883830115110571F0F1\r
+:1027D0001387028781E8838380916201909163016E\r
+:1027E000892B21F4F0936301E09362012097E1F2D9\r
+:1027F000D187C087838182608383809164019091B7\r
+:102800006501892B89F6F0936501E093640180915D\r
+:10281000660190916701892B39F6F0936701E09387\r
+:102820006601C2CF0F931F93CF93DF93182F092F09\r
+:10283000EB018B8181FD09C01FEF0FEF812F902FDE\r
+:10284000DF91CF911F910F91089582FF14C02E81C7\r
+:102850003F818C819D81281739073CF4E881F981FB\r
+:10286000CF0101969983888310838E819F81019681\r
+:102870009F838E83E3CFE885F985812F0995892B86\r
+:10288000A1F3DACFA0E0B0E0E8E4F4E10C94721335\r
+:10289000AE01495F5F4FDA016D917D91AD0102E6B6\r
+:1028A00011E0F80182819381DC0113962C9113973A\r
+:1028B000286013962C930E942A0F9C01D80112962F\r
+:1028C000ED91FC919381977F9383C901E4E00C948F\r
+:1028D0008E13FA01AA27283051F1203181F1E894B2\r
+:1028E0006F936E7F6E5F7F4F8F4F9F4FAF4FB1E003\r
+:1028F0003ED0B4E03CD0670F781F891F9A1FA11DFE\r
+:10290000680F791F8A1F911DA11D6A0F711D811DFE\r
+:10291000911DA11D20D009F468943F912AE0269FC3\r
+:1029200011243019305D3193DEF6CF010895462F22\r
+:102930004770405D4193B3E00FD0C9F7F6CF462F03\r
+:102940004F70405D4A3318F0495D31FD405241936C\r
+:1029500002D0A9F7EACFB4E0A69597958795779529\r
+:102960006795BA95C9F700976105710508959B01B0\r
+:10297000AC010A2E06945795479537952795BA9539\r
+:10298000C9F7620F731F841F951FA01D08950F9331\r
+:102990001F93CF93DF93869F8001879F100D969F93\r
+:1029A000100D1124C8010E94E314EC01009729F0D6\r
+:1029B000A80160E070E00E940A16CE01DF91CF917D\r
+:1029C0001F910F9108950F931F93CF93DF939C0155\r
+:1029D000029710F422E030E0E0916801F091690183\r
+:1029E00080E090E0A0E0B0E0309791F4009709F427\r
+:1029F00048C0821B930B84309105D0F58A819B815E\r
+:102A00006115710581F1FB0193838283FE0111C081\r
+:102A1000408151810281138142175307D8F0421738\r
+:102A2000530799F4109761F012960C93129713962E\r
+:102A30001C933296CF01DF91CF911F910F91089592\r
+:102A40000093680110936901F4CF009751F0481783\r
+:102A5000590738F0AC01DB01CA01BD01DF01F80103\r
+:102A6000C3CFEF01F9CF9093690180936801CECF76\r
+:102A7000FE01E80FF91F2193319302979983888310\r
+:102A8000D9CF40916A0150916B014115510541F434\r
+:102A900040916C0050916D0050936B0140936A011E\r
+:102AA00080916A0090916B00009741F48DB79EB7BA\r
+:102AB00060916E0070916F00861B970B4817590745\r
+:102AC00008F0B8CF841B950B8217930708F4B2CF98\r
+:102AD000B9016E5F7F4F8617970708F4ABCF640F7D\r
+:102AE000751F70936B0160936A01FA012193319312\r
+:102AF000A1CFCF93DF939C010097E9F0FC013297BF\r
+:102B000013821282C0916801D0916901DE0180E0D8\r
+:102B100090E02097A1F480819181280F391F809146\r
+:102B20006A0190916B018217930709F068C0F093D6\r
+:102B30006B01E0936A01DF91CF910895DA01AE173E\r
+:102B4000BF0708F042C012964D915C911397CD01DA\r
+:102B50004115510599F7DC011396FC93EE931297FA\r
+:102B60004D915D919D01240F351FE217F30781F40C\r
+:102B7000208131812E5F3F4F240F351FDC011196DC\r
+:102B80003C932E932281338113963C932E9312977C\r
+:102B9000E0E0F0E08A819B810097B1F5DE018D9144\r
+:102BA0009D918A0F9B1F20916A0130916B0128171C\r
+:102BB000390709F6309759F51092690110926801AA\r
+:102BC000D0936B01C0936A01B6CFB383A2834081D7\r
+:102BD0005181240F351FA217B30771F44E5F5F4F69\r
+:102BE0002D913C911197240F351F3183208312962C\r
+:102BF0002D913C9133832283009709F0ACCFF09361\r
+:102C00006901E093680197CFFE01EC01C3CF138205\r
+:102C10001282D6CFDC0101C06D9341505040E0F7E5\r
+:102C20000895E199FECF9FBB8EBBE09A99278DB3A3\r
+:102C30000895262FE199FECF9FBB8EBB2DBB0FB60B\r
+:102C4000F894E29AE19A0FBE01960895F894FFCFA6\r
+:102C500005A84CCDB2D44EB93836A9020C50B99162\r
+:102C60008688083CA6AAAA2ABE000000803F9A03D4\r
+:102C70008F039A0393039A0396039A0398039A0384\r
+:102C8000E404E404E404E404E404E4049E03B70379\r
+:102C9000A803B703AD03B703B203B703E404E40426\r
+:102CA000E404E404E404E404E404E404BA03BF0335\r
+:102CB000C403C903CE03D303D803DD03E404E4044F\r
+:102CC000E404E404E404E404E404E404E203E703C5\r
+:102CD000EC03F103F603FB03000405040A041F04DC\r
+:102CE0002B0438043B044C0455045E0461046A045C\r
+:102CF00073047C0485048E049704A004A904B20420\r
+:102D0000BB04C404E404E404E404E404E404E404CC\r
+:102D1000E404E404E404E404E404E404E404E40473\r
+:102D2000E404E404E404E404E404E404E404E40463\r
+:102D3000E404E404E404E404E404E404E404E40453\r
+:102D4000E404E404E404E404E404E404E404E40443\r
+:102D5000E404E404E404E404E404E404E404E40433\r
+:102D6000E404E404E404E404E404E404E404E40423\r
+:102D7000E404E404E404E404E404E404E404E40413\r
+:102D8000E404E404E404E404E404E404E404E40403\r
+:102D9000E404E404E404E404E404E404E404E404F3\r
+:102DA000E404E404E404E404E404E404CD04D2040C\r
+:102DB000D504DA04DF0448034C03500354035803DA\r
+:102DC0005F031602160216021602160216021602F9\r
+:102DD00016021602160216021602160216026303E5\r
+:102DE0003402920249029202530292025D0292025E\r
+:102DF00067021602160216021602160216021602C2\r
+:102E00001602160216021602160216021602160202\r
+:102E100016021602160216021602160216021602F2\r
+:102E20009202710292027C02920287029202960240\r
+:102E3000E402A102A102B802E402A102A102B802C6\r
+:102E400016021602160216021602160216021602C2\r
+:102E500016021602160216021602160216021602B2\r
+:102E600016021602160216021602160216021602A2\r
+:102E70001602160216021602160216021602160292\r
+:102E8000E402A102A102E9020803150315032403C9\r
+:102E90000080008000800080010100006C012000A3\r
+:00000001FF\r
--- /dev/null
+#include "ringbuffer.h"
+
+ringbuffer_t *ringbuffers[RINGBUFFER_MAX_NUM];
+uint8_t num_ringbuffers;
--- /dev/null
+#ifndef RINGBUFFER_H
+#define RINGBUFFER_H
+
+#include <stdio.h>
+
+#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
+
--- /dev/null
+#include <stdlib.h>
+#include "uart.h"
+#include <avr/interrupt.h>
+
+#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<<RXC)) {
+ *in_read_ptr = UDR;
+
+ if (in_read_ptr < read + RINGBUFFER_LEN - 1) in_read_ptr++;
+ else in_read_ptr = (char *)read;
+ }
+}
+
+
+void setup_uart(unsigned int rate) {
+ UCSRB |= (1<<TXEN) | (1<<RXEN); // UART TX & RX
+ UCSRB |= (1<<RXCIE); // RX Interrupt
+ UCSRC |= (3<<UCSZ0); // Asynchron 8N1
+
+ UBRRH = (uint8_t)(UART_UBRR_CALC(rate, F_CPU) >> 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<<UDRE)));
+ UDR = *c;
+}
+
+
+int uart_putchar(char c, FILE *stream)
+{
+ uart_putc(&c);
+ return 0;
+}
+
+
+void uart_puts(char *s) {
+ char *c;
+
+ for (c = s; *c != '\0'; c++)
+ uart_putc(c);
+}
+
+
+int uart_getchar(FILE *stream)
+{
+ return uart_getc();
+}
+
+
+int uart_getc() {
+ while (!(UCSRA & (1<<RXC)));
+ return UDR;
+}
+
+
+void uart_setup_stdout() {
+ fdevopen(uart_putchar, uart_getchar);
+}
+
+
+void uart_stream_update(ringbuffer_t *buffer) {
+ // update write to uart
+ if (buffer->out_write_ptr != buffer->in_write_ptr) {
+ if (UCSRA & (1<<UDRE)) {
+ UDR = *buffer->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;
+ }
+}
+
--- /dev/null
+#ifndef UART_H
+#define UART_H
+
+#include <stdio.h>
+#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
+
--- /dev/null
+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
--- /dev/null
+#!/bin/sh
+
+docker pull ros:jazzy-ros-base
+
+docker build -t ros2 .
+
+docker image prune
--- /dev/null
+#!/bin/bash
+set -e
+
+# setup ros2 environment
+source /root/ros2_ws/install/setup.bash
+exec "$@"
--- /dev/null
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>a4wd3</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="root@todo.todo">root</maintainer>
+ <license>MIT</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <depend>rclcpp</depend>
+ <depend>std_msgs</depend>
+ <depend>sensor_msgs</depend>
+ <depend>geometry_msgs</depend>
+ <depend>std_srvs</depend>
+ <depend>nav_msgs</depend>
+ <depend>tf2_geometry_msgs</depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
+
+ <exec_depend>ros2launch</exec_depend>
+ <exec_depend>bno055</exec_depend>
+ <exec_depend>tf2_ros</exec_depend>
+ <exec_depend>robot_localization</exec_depend>
+ <exec_depend>twist_mux</exec_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
--- /dev/null
+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
--- /dev/null
+#!/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)
--- /dev/null
+#!/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)
--- /dev/null
+#!/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])
--- /dev/null
+Subproject commit 5a2e8766d52bbd4251cfa784ca71e087673789ad
--- /dev/null
+#!/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)
--- /dev/null
+#include <cstdio>
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <linux/i2c-dev.h>
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/battery_state.hpp"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <vector>
+#include <math.h>
+#include <byteswap.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+extern "C" {
+#include <i2c/smbus.h>
+}
+
+/**
+ * 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<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_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<nav_msgs::msg::Odometry>("odom", 10);
+ pub_bat = this->create_publisher<sensor_msgs::msg::BatteryState>("battery", 10);
+ sub_cmd_vel = this->create_subscription<geometry_msgs::msg::Twist>("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<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
+ rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pub_bat;
+ rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_vel;
+ std::vector<double> cmd_vel;
+ double pos_x=0, pos_y=0, angle=0;
+
+ void set_speed(float trans, float rot) {
+ rot*=-1;
+ std::vector<uint8_t> 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<uint8_t> 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<A4wd3>());
+ rclcpp::shutdown();
+
+ return 0;
+}
projects / a4wd3.git / commitdiff