--- /dev/null
+# Hey Emacs, this is a -*- makefile -*-\r
+#\r
+# WinAVR makefile written by Eric B. Weddington, Jörg Wunsch, et al.\r
+# Released to the Public Domain\r
+# Please read the make user manual!\r
+#\r
+# Additional material for this makefile was submitted by:\r
+# Tim Henigan\r
+# Peter Fleury\r
+# Reiner Patommel\r
+# Sander Pool\r
+# Frederik Rouleau\r
+# Markus Pfaff\r
+#\r
+# On command line:\r
+#\r
+# make all = Make software.\r
+#\r
+# make clean = Clean out built project files.\r
+#\r
+# make coff = Convert ELF to AVR COFF (for use with AVR Studio 3.x or VMLAB).\r
+#\r
+# make extcoff = Convert ELF to AVR Extended COFF (for use with AVR Studio\r
+# 4.07 or greater).\r
+#\r
+# make program = Download the hex file to the device, using avrdude. Please\r
+# customize the avrdude settings below first!\r
+#\r
+# make filename.s = Just compile filename.c into the assembler code only\r
+#\r
+# To rebuild project do "make clean" then "make all".\r
+#\r
+\r
+# mth 2004/09 \r
+# Differences from WinAVR 20040720 sample:\r
+# - DEPFLAGS according to Eric Weddingtion's fix (avrfreaks/gcc-forum)\r
+# - F_OSC Define in CFLAGS and AFLAGS\r
+\r
+\r
+# MCU name\r
+MCU = atmega32\r
+\r
+# Main Oscillator Frequency\r
+# This is only used to define F_OSC in all assembler and c-sources.\r
+F_OSC = 3686400\r
+\r
+# Output format. (can be srec, ihex, binary)\r
+FORMAT = ihex\r
+\r
+# Target file name (without extension).\r
+TARGET = main\r
+\r
+\r
+# List C source files here. (C dependencies are automatically generated.)\r
+SRC = $(TARGET).c uart.c\r
+\r
+\r
+# List Assembler source files here.\r
+# Make them always end in a capital .S. Files ending in a lowercase .s\r
+# will not be considered source files but generated files (assembler\r
+# output from the compiler), and will be deleted upon "make clean"!\r
+# Even though the DOS/Win* filesystem matches both .s and .S the same,\r
+# it will preserve the spelling of the filenames, and gcc itself does\r
+# care about how the name is spelled on its command-line.\r
+ASRC = \r
+\r
+\r
+\r
+# Optimization level, can be [0, 1, 2, 3, s]. \r
+# 0 = turn off optimization. s = optimize for size.\r
+# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)\r
+OPT = s\r
+\r
+# Debugging format.\r
+# Native formats for AVR-GCC's -g are stabs [default], or dwarf-2.\r
+# AVR (extended) COFF requires stabs, plus an avr-objcopy run.\r
+DEBUG = stabs\r
+#DEBUG = dwarf-2\r
+\r
+# List any extra directories to look for include files here.\r
+# Each directory must be seperated by a space.\r
+EXTRAINCDIRS = \r
+\r
+\r
+# Compiler flag to set the C Standard level.\r
+# c89 - "ANSI" C\r
+# gnu89 - c89 plus GCC extensions\r
+# c99 - ISO C99 standard (not yet fully implemented)\r
+# gnu99 - c99 plus GCC extensions\r
+CSTANDARD = -std=gnu99\r
+\r
+# Place -D or -U options here\r
+CDEFS = -DF_CPU=4000000\r
+\r
+# Place -I options here\r
+CINCS = -Ii2c/\r
+\r
+\r
+# Compiler flags.\r
+# -g*: generate debugging information\r
+# -O*: optimization level\r
+# -f...: tuning, see GCC manual and avr-libc documentation\r
+# -Wall...: warning level\r
+# -Wa,...: tell GCC to pass this to the assembler.\r
+# -adhlns...: create assembler listing\r
+CFLAGS = -g$(DEBUG)\r
+CFLAGS += $(CDEFS) $(CINCS)\r
+CFLAGS += -O$(OPT)\r
+CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums\r
+CFLAGS += -Wall -Wstrict-prototypes\r
+CFLAGS += -Wa,-adhlns=$(<:.c=.lst)\r
+CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))\r
+CFLAGS += $(CSTANDARD)\r
+CFLAGS += -DF_OSC=$(F_OSC)\r
+\r
+\r
+\r
+# Assembler flags.\r
+# -Wa,...: tell GCC to pass this to the assembler.\r
+# -ahlms: create listing\r
+# -gstabs: have the assembler create line number information; note that\r
+# for use in COFF files, additional information about filenames\r
+# and function names needs to be present in the assembler source\r
+# files -- see avr-libc docs [FIXME: not yet described there]\r
+ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs \r
+ASFLAGS += -DF_OSC=$(F_OSC)\r
+\r
+\r
+#Additional libraries.\r
+\r
+# Minimalistic printf version\r
+PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min\r
+\r
+# Floating point printf version (requires MATH_LIB = -lm below)\r
+PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt -lm\r
+\r
+PRINTF_LIB = \r
+\r
+# Minimalistic scanf version\r
+SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min\r
+\r
+# Floating point + %[ scanf version (requires MATH_LIB = -lm below)\r
+SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt\r
+\r
+SCANF_LIB = \r
+\r
+MATH_LIB = -lm\r
+\r
+# External memory options\r
+\r
+# 64 KB of external RAM, starting after internal RAM (ATmega128!),\r
+# used for variables (.data/.bss) and heap (malloc()).\r
+#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff\r
+\r
+# 64 KB of external RAM, starting after internal RAM (ATmega128!),\r
+# only used for heap (malloc()).\r
+#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff\r
+\r
+EXTMEMOPTS =\r
+\r
+# Linker flags.\r
+# -Wl,...: tell GCC to pass this to linker.\r
+# -Map: create map file\r
+# --cref: add cross reference to map file\r
+LDFLAGS = -Wl,-Map=$(TARGET).map,--cref\r
+LDFLAGS += $(EXTMEMOPTS)\r
+LDFLAGS += $(PRINTF_LIB_MIN) $(SCANF_LIB) $(MATH_LIB)\r
+\r
+\r
+\r
+\r
+# Programming support using avrdude. Settings and variables.\r
+\r
+# Programming hardware: alf avr910 avrisp bascom bsd \r
+# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500\r
+#\r
+# Type: avrdude -c ?\r
+# to get a full listing.\r
+#\r
+#AVRDUDE_PROGRAMMER = avr911\r
+AVRDUDE_PROGRAMMER = avrisp2\r
+\r
+# com1 = serial port. Use lpt1 to connect to parallel port.\r
+#AVRDUDE_PORT = /dev/ttyUSB0 # programmer connected to serial device\r
+AVRDUDE_PORT = usb # programmer connected to serial device\r
+\r
+AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex\r
+#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep\r
+\r
+\r
+# Uncomment the following if you want avrdude's erase cycle counter.\r
+# Note that this counter needs to be initialized first using -Yn,\r
+# see avrdude manual.\r
+#AVRDUDE_ERASE_COUNTER = -y\r
+\r
+# Uncomment the following if you do /not/ wish a verification to be\r
+# performed after programming the device.\r
+#AVRDUDE_NO_VERIFY = -V\r
+\r
+# Increase verbosity level. Please use this when submitting bug\r
+# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude> \r
+# to submit bug reports.\r
+#AVRDUDE_VERBOSE = -v -v\r
+\r
+AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)\r
+AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY)\r
+AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE)\r
+AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER)\r
+\r
+\r
+\r
+# ---------------------------------------------------------------------------\r
+\r
+# Define directories, if needed.\r
+DIRAVR = c:/winavr\r
+DIRAVRBIN = $(DIRAVR)/bin\r
+DIRAVRUTILS = $(DIRAVR)/utils/bin\r
+DIRINC = .\r
+DIRLIB = $(DIRAVR)/avr/lib\r
+\r
+\r
+# Define programs and commands.\r
+SHELL = sh\r
+CC = avr-gcc\r
+OBJCOPY = avr-objcopy\r
+OBJDUMP = avr-objdump\r
+SIZE = avr-size\r
+NM = avr-nm\r
+AVRDUDE = avrdude\r
+REMOVE = rm -f\r
+COPY = cp\r
+\r
+\r
+\r
+\r
+# Define Messages\r
+# English\r
+MSG_ERRORS_NONE = Errors: none\r
+MSG_BEGIN = -------- begin --------\r
+MSG_END = -------- end --------\r
+MSG_SIZE_BEFORE = Size before: \r
+MSG_SIZE_AFTER = Size after:\r
+MSG_COFF = Converting to AVR COFF:\r
+MSG_EXTENDED_COFF = Converting to AVR Extended COFF:\r
+MSG_FLASH = Creating load file for Flash:\r
+MSG_EEPROM = Creating load file for EEPROM:\r
+MSG_EXTENDED_LISTING = Creating Extended Listing:\r
+MSG_SYMBOL_TABLE = Creating Symbol Table:\r
+MSG_LINKING = Linking:\r
+MSG_COMPILING = Compiling:\r
+MSG_ASSEMBLING = Assembling:\r
+MSG_CLEANING = Cleaning project:\r
+\r
+\r
+\r
+\r
+# Define all object files.\r
+OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) \r
+\r
+# Define all listing files.\r
+LST = $(ASRC:.S=.lst) $(SRC:.c=.lst)\r
+\r
+\r
+# Compiler flags to generate dependency files.\r
+### GENDEPFLAGS = -Wp,-M,-MP,-MT,$(*F).o,-MF,.dep/$(@F).d\r
+GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d\r
+\r
+# Combine all necessary flags and optional flags.\r
+# Add target processor to flags.\r
+ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS)\r
+ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)\r
+\r
+\r
+\r
+\r
+\r
+# Default target.\r
+all: begin gccversion sizebefore build sizeafter finished end\r
+\r
+build: elf hex eep lss sym\r
+\r
+elf: $(TARGET).elf\r
+hex: $(TARGET).hex\r
+eep: $(TARGET).eep\r
+lss: $(TARGET).lss \r
+sym: $(TARGET).sym\r
+\r
+\r
+\r
+# Eye candy.\r
+# AVR Studio 3.x does not check make's exit code but relies on\r
+# the following magic strings to be generated by the compile job.\r
+begin:\r
+ @echo\r
+ @echo $(MSG_BEGIN)\r
+\r
+finished:\r
+ @echo $(MSG_ERRORS_NONE)\r
+\r
+end:\r
+ @echo $(MSG_END)\r
+ @echo\r
+\r
+\r
+# Display size of file.\r
+HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex\r
+ELFSIZE = $(SIZE) -A $(TARGET).elf\r
+sizebefore:\r
+ @if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi\r
+\r
+sizeafter:\r
+ @if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi\r
+\r
+\r
+\r
+# Display compiler version information.\r
+gccversion : \r
+ @$(CC) --version\r
+\r
+\r
+\r
+# Program the device. \r
+program: $(TARGET).hex $(TARGET).eep\r
+ $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)\r
+\r
+\r
+\r
+\r
+# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB.\r
+COFFCONVERT=$(OBJCOPY) --debugging \\r
+--change-section-address .data-0x800000 \\r
+--change-section-address .bss-0x800000 \\r
+--change-section-address .noinit-0x800000 \\r
+--change-section-address .eeprom-0x810000 \r
+\r
+\r
+coff: $(TARGET).elf\r
+ @echo\r
+ @echo $(MSG_COFF) $(TARGET).cof\r
+ $(COFFCONVERT) -O coff-avr $< $(TARGET).cof\r
+\r
+\r
+extcoff: $(TARGET).elf\r
+ @echo\r
+ @echo $(MSG_EXTENDED_COFF) $(TARGET).cof\r
+ $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof\r
+\r
+\r
+\r
+# Create final output files (.hex, .eep) from ELF output file.\r
+%.hex: %.elf\r
+ @echo\r
+ @echo $(MSG_FLASH) $@\r
+ $(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@\r
+\r
+%.eep: %.elf\r
+ @echo\r
+ @echo $(MSG_EEPROM) $@\r
+ -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \\r
+ --change-section-lma .eeprom=0 -O $(FORMAT) $< $@\r
+\r
+# Create extended listing file from ELF output file.\r
+%.lss: %.elf\r
+ @echo\r
+ @echo $(MSG_EXTENDED_LISTING) $@\r
+ $(OBJDUMP) -h -S $< > $@\r
+\r
+# Create a symbol table from ELF output file.\r
+%.sym: %.elf\r
+ @echo\r
+ @echo $(MSG_SYMBOL_TABLE) $@\r
+ $(NM) -n $< > $@\r
+\r
+\r
+\r
+# Link: create ELF output file from object files.\r
+.SECONDARY : $(TARGET).elf\r
+.PRECIOUS : $(OBJ)\r
+%.elf: $(OBJ)\r
+ @echo\r
+ @echo $(MSG_LINKING) $@\r
+ $(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS)\r
+\r
+\r
+# Compile: create object files from C source files.\r
+%.o : %.c\r
+ @echo\r
+ @echo $(MSG_COMPILING) $<\r
+ $(CC) -c $(ALL_CFLAGS) $< -o $@ \r
+\r
+\r
+# Compile: create assembler files from C source files.\r
+%.s : %.c\r
+ $(CC) -S $(ALL_CFLAGS) $< -o $@\r
+\r
+\r
+# Assemble: create object files from assembler source files.\r
+%.o : %.S\r
+ @echo\r
+ @echo $(MSG_ASSEMBLING) $<\r
+ $(CC) -c $(ALL_ASFLAGS) $< -o $@\r
+\r
+\r
+\r
+# Target: clean project.\r
+clean: begin clean_list finished end\r
+\r
+clean_list :\r
+ @echo\r
+ @echo $(MSG_CLEANING)\r
+ $(REMOVE) $(TARGET).hex\r
+ $(REMOVE) $(TARGET).eep\r
+ $(REMOVE) $(TARGET).obj\r
+ $(REMOVE) $(TARGET).cof\r
+ $(REMOVE) $(TARGET).elf\r
+ $(REMOVE) $(TARGET).map\r
+ $(REMOVE) $(TARGET).obj\r
+ $(REMOVE) $(TARGET).a90\r
+ $(REMOVE) $(TARGET).sym\r
+ $(REMOVE) $(TARGET).lnk\r
+ $(REMOVE) $(TARGET).lss\r
+ $(REMOVE) $(OBJ)\r
+ $(REMOVE) $(LST)\r
+ $(REMOVE) $(SRC:.c=.s)\r
+ $(REMOVE) $(SRC:.c=.d)\r
+ $(REMOVE) .dep/*\r
+\r
+\r
+\r
+# Include the dependency files.\r
+-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)\r
+\r
+\r
+# Listing of phony targets.\r
+.PHONY : all begin finish end sizebefore sizeafter gccversion \\r
+build elf hex eep lss sym coff extcoff \\r
+clean clean_list program\r
+\r
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <limits.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <avr/sleep.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
+ * free
+ * 0x90 Motor 1 switch
+ * 0x91 Motor 2 switch
+ * 0x92 Motor 3 switch
+ * 0x93 Motor 4 switch
+ * free
+ * 0xff Bootloader
+ */
+
+
+#define TWI_ACK TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
+#define TWI_RESET TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
+#define TWI_NAK TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
+
+#define KP 10.0
+#define KI 0.0
+#define KD 0.0
+#define TIMER0_T 0.020
+
+enum mode {
+ MOTOR_MANUAL,
+ MOTOR_PID
+};
+
+static volatile uint8_t ireg=0;
+static volatile uint8_t bootloader=0;
+static volatile int16_t motor1=0;
+static volatile int16_t motor2=0;
+static volatile int16_t motor3=0;
+static volatile int16_t motor4=0;
+static volatile int16_t pos1=0;
+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=0;
+static volatile uint8_t motor4_switch=0;
+static volatile int16_t speed1_wish=0;
+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=0;
+static volatile int16_t speed2=0;
+static volatile int16_t speed3=0;
+static volatile int16_t speed4=0;
+static volatile int16_t eold1=0;
+static volatile int16_t eold2=0;
+static volatile int16_t eold3=0;
+static volatile int16_t eold4=0;
+static volatile int32_t esum1=0;
+static volatile int32_t esum2=0;
+static volatile int32_t esum3=0;
+static volatile int32_t esum4=0;
+static volatile int16_t pos1_last=0;
+static volatile int16_t pos2_last=0;
+static volatile int16_t pos3_last=0;
+static volatile int16_t pos4_last=0;
+
+ISR(TWI_vect)
+{
+ static uint8_t tmp=0;
+ static int16_t tmp16=0;
+
+ switch (TWSR & 0xF8)
+ {
+ case 0x60: // start write
+ TWI_ACK;
+ ireg = 0;
+ break;
+ case 0x80: // 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 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 0xff: // bootloader
+ bootloader = TWDR;
+ default:
+ TWI_NAK;
+ }
+ ireg++;
+ break;
+ case 0xA8: // start read
+ case 0xB8: // read
+ switch(ireg) {
+ case 0x02: // Motor 1 PWM
+ TWDR = OCR1A;
+ TWI_ACK;
+ break;
+ case 0x04: // Motor 2 PWM
+ TWDR = OCR1B;
+ TWI_ACK;
+ break;
+ case 0x06: // Motor 3 PWM
+ TWDR = OCR2;
+ TWI_ACK;
+ break;
+ case 0x08: // Motor 4 PWM
+ TWDR = OCR0;
+ 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;
+ default:
+ TWDR = 0;
+ TWI_NAK;
+ }
+ ireg++;
+ break;
+ default:
+ TWI_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 >> 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 (motor2_switch) pos2 += (diff & 2) - 1; // bit 1 = direction (+/-)
+ else pos2 -= (diff & 2) - 1;
+ }
+}
+
+
+static void update_hall3(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 (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;
+
+ if (m1_old != motor1) { // update only when changed
+ if (motor1 == 0) {
+ // stop
+ PORTC |= (1 << 3) | (1 << 2);
+ } else if ((!motor1_switch && motor1 > 0) || (motor1_switch && motor1 < 0)) {
+ // forward
+ PORTC &= ~(1 << 3) & ~(1 << 2);
+ } else { // motor1 < 0
+ // backward
+ PORTC &= ~(1 << 2);
+ PORTC |= (1 << 3);
+ }
+
+ m1_old = motor1;
+ OCR1A = abs(motor1);
+ }
+
+ if (m2_old != motor2) { // update only when changed
+ if (motor2 == 0) {
+ // stop
+ PORTC |= (1 << 5) | (1 << 4);
+ } else if ((!motor2_switch && motor2 > 0) || (motor2_switch && motor2 < 0)) {
+ // forward
+ PORTC &= ~(1 << 5) & ~(1 << 4);
+ } else { // motor2 < 0
+ // backward
+ PORTC &= ~(1 << 4);
+ PORTC |= (1 << 5);
+ }
+
+ m2_old = motor2;
+ OCR1B = abs(motor2);
+ }
+
+ if (m3_old != motor3) { // update only when changed
+ if (motor3 == 0) {
+ // stop
+ PORTC |= (1 << 7) | (1 << 6);
+ } else if ((!motor3_switch && motor3 > 0) || (motor3_switch && motor3 < 0)) {
+ // forward
+ PORTC &= ~(1 << 7) & ~(1 << 6);
+ } else { // motor3 < 0
+ // backward
+ PORTC &= ~(1 << 6);
+ PORTC |= (1 << 7);
+ }
+
+ m3_old = motor3;
+ OCR2 = abs(motor3);
+ }
+
+ if (m4_old != motor4) { // update only when changed
+ if (motor4 == 0) {
+ // stop
+ PORTD |= (1 << 3) | (1 << 2);
+ } else if ((!motor4_switch && motor4 > 0) || (motor4_switch && motor4 < 0)) {
+ // forward
+ PORTD &= ~(1 << 3) & ~(1 << 2);
+ } else { // motor4 < 0
+ // backward
+ PORTD &= ~(1 << 2);
+ PORTD |= (1 << 3);
+ }
+
+ m4_old = motor4;
+ OCR0 = abs(motor4);
+ }
+}
+
+
+ISR(TIMER0_OVF_vect) {
+ update_hall1();
+ update_hall2();
+ update_hall3();
+ update_hall4();
+
+ // PID control
+ if (motor1_mode == MOTOR_PID) {
+ speed1 = (pos1 - pos1_last)/TIMER0_T;
+
+ if (speed1_wish == 0) {
+ motor1 = 0;
+ } else {
+ int16_t e = speed1_wish - speed1;
+ esum1+=e;
+ motor1 += KP*e + KI*TIMER0_T*esum1 + KD/TIMER0_T*(e - eold1);
+ eold1 = e;
+
+ if (motor1 > 255) motor1 = 255;
+ else if (motor1 < -255) motor1 = -255;
+ }
+
+ pos1_last = pos1;
+ }
+ if (motor2_mode == MOTOR_PID) {
+ speed2 = (pos2 - pos2_last)/TIMER0_T;
+
+ if (speed2_wish == 0) {
+ motor2 = 0;
+ } else {
+ int16_t e = speed2_wish - speed2;
+ esum2+=e;
+ motor2 += KP*e + KI*TIMER0_T*esum2 + KD/TIMER0_T*(e - eold2);
+ eold2 = e;
+
+ if (motor2 > 255) motor2 = 255;
+ else if (motor2 < -255) motor2 = -255;
+ }
+
+ pos2_last = pos2;
+ }
+ if (motor3_mode == MOTOR_PID) {
+ speed3 = (pos3 - pos3_last)/TIMER0_T;
+
+ if (speed3_wish == 0) {
+ motor3 = 0;
+ } else {
+ int16_t e = speed3_wish - speed3;
+ esum3+=e;
+ motor3 += KP*e + KI*TIMER0_T*esum3 + KD/TIMER0_T*(e - eold3);
+ eold3 = e;
+
+ if (motor3 > 255) motor3 = 255;
+ else if (motor3 < -255) motor3 = -255;
+ }
+
+ pos3_last = pos3;
+ }
+ if (motor4_mode == MOTOR_PID) {
+ speed4 = (pos4 - pos4_last)/TIMER0_T;
+
+ if (speed4_wish == 0) {
+ motor4 = 0;
+ } else {
+ int16_t e = speed4_wish - speed4;
+ esum4+=e;
+ motor4 += KP*e + KI*TIMER0_T*esum4 + KD/TIMER0_T*(e - eold4);
+ eold4 = e;
+
+ if (motor4 > 255) motor4 = 255;
+ else if (motor4 < -255) motor4 = -255;
+ }
+
+ pos4_last = pos4;
+ }
+}
+
+
+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);
+
+ bootloader = 0x00;
+ setup_uart(9600);
+ uart_setup_stdout();
+
+ // I2C
+ TWAR = 0x50;
+ TWI_RESET;
+
+ // Motor 1 & 2
+ // Timer 1: Fast PWM inverting mode, Top=256 => 15.625Hz
+ // Prescaler=1
+ TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM10);
+ TCCR1B = (1 << WGM12) | (1 << CS10);
+ OCR1A = 0;
+ OCR1B = 0;
+
+ // Motor 3
+ // Timer 2: Fast PWM, Top=256 => 15.625Hz
+ // Prescaler=1
+ TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << COM21) | (1 << CS20);
+ OCR2 = 0;
+
+ // Motor 4
+ // Timer 0: Fast PWM, Top=256 => 15.625Hz
+ // Prescaler=1
+ TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << COM01) | (1 << CS00);
+ OCR0 = 0;
+
+ printf("\r\nStart\r\n");
+
+ set_sleep_mode(SLEEP_MODE_IDLE);
+ // Enable Timer 1 Overflow Interrupt
+ TIMSK = (1 << TOIE1);
+ sei();
+
+ while(1) {
+ switch(ireg) {
+ case 0xff: // Magic reg that starts the bootloader
+ if (bootloader == 0xa5) {
+ cli();
+ {
+ void (*start)(void) = (void*)0x1800;
+ start();
+ }
+ }
+ break;
+ }
+ update_motor();
+
+ sleep_mode();
+ }
+
+ return 0;
+}
--- /dev/null
+:100000000C945C000C9479000C9479000C947900A9\r
+:100010000C9479000C9479000C9479000C9479007C\r
+:100020000C9479000C9479000C9479000C94E30101\r
+:100030000C9479000C9405060C9479000C947900CA\r
+:100040000C9479000C9479000C9479000C947B004A\r
+:100050000C9479007901C9017C01C9017F01C901B2\r
+:100060008101C901C901C901C901C901C901C90188\r
+:1000700083019C0188019C0192019C0197019C01D4\r
+:10008000C901C901C901C901C901C901C901C90120\r
+:100090009F01A401A901AE01B301B801BD01C201D4\r
+:1000A00045464765666700202B2D2E3031323334AC\r
+:1000B000353637383968000011241FBECFE5D8E047\r
+:1000C000DEBFCDBF10E0A0E6B0E0E6E4FCE102C098\r
+:1000D00005900D92A837B107D9F721E0A8E7B0E065\r
+:1000E00001C01D92AE33B207E1F70E94FA060C94EC\r
+:1000F000210E0C9400001F920F920FB60F92112444\r
+:100100002F938F939F93AF93BF93EF93FF9381B1FF\r
+:10011000887F803881F040F4803609F043C185EC57\r
+:1001200086BF1092C80043C1883A09F4D5C0883B05\r
+:1001300009F4D2C037C18091C800813209F495C05A\r
+:1001400068F5843009F479C060F4813009F4A7C0FF\r
+:1001500008F45EC0823009F464C0833009F49FC0A3\r
+:1001600019C1873009F49BC090F4853009F497C019\r
+:10017000863009F00FC183B120917E0090E0922B70\r
+:100180009093C2008093C1001092B40000C18830E7\r
+:1001900009F45FC0803209F482C0FCC0873209F4E0\r
+:1001A00082C0F0F4843209F47AC098F4823209F4FF\r
+:1001B00076C0833209F0EEC083B120917E0090E0DA\r
+:1001C000922B9093AC008093AB0081E08093B500BC\r
+:1001D000DEC0853209F456C0863209F460C0DAC048\r
+:1001E000823909F471C050F4803909F469C0813949\r
+:1001F00009F0D0C083B18093B100C9C0833909F43C\r
+:1002000067C08F3F09F0C6C083B18093C700C2C0EA\r
+:1002100083B18093C8008091C80081508093C8004A\r
+:10022000B6C083B120917E0090E0922B9093C600DF\r
+:100230008093C5001092B600AAC083B120917E00C1\r
+:1002400090E0922B9093C4008093C3001092B5006D\r
+:100250009EC083B120917E0090E0922B9093C000CD\r
+:100260008093BF001092B30092C083B120917E00B2\r
+:1002700090E0922B9093AE008093AD0081E080934C\r
+:10028000B60085C083B120917E0090E0922B9093C0\r
+:10029000AA008093A90081E08093B40078C083B164\r
+:1002A00080937E0074C083B120917E0090E0922BF9\r
+:1002B0009093A8008093A70081E08093B30067C06B\r
+:1002C00083B18093B20063C083B18093B0005FC0FC\r
+:1002D00083B18093AF005BC08091C80090E0FC01C7\r
+:1002E0003297E632F10508F054C0E65DFF4F0C94FA\r
+:1002F0009C0B8AB59BB54AC088B599B547C083B5F4\r
+:1003000045C08CB743C08091BD009091BE0004C031\r
+:100310008091BB009091BC0090937D0080937C0005\r
+:1003200093B935C08091B9009091BA00F5CF809112\r
+:10033000B7009091B800F0CF80917C0027C08091E9\r
+:10034000AD009091AE00ECCF8091AD009091AE00E9\r
+:100350001DC08091AB009091AC00E2CF8091AB00CA\r
+:100360009091AC0013C08091A9009091AA00D8CFC1\r
+:100370008091A9009091AA0009C08091A700909156\r
+:10038000A800CECF8091A7009091A80083B985ECFA\r
+:1003900002C013B885E886BF8091C8008F5F809344\r
+:1003A000C80005C086B78B7E86BF85EC86BFFF91EF\r
+:1003B000EF91BF91AF919F918F912F910F900FBEB1\r
+:1003C0000F901F9018951F920F920FB60F92112445\r
+:1003D0004F925F926F927F928F929F92AF92BF9255\r
+:1003E000CF92DF92EF92FF920F931F932F933F9341\r
+:1003F0004F935F936F937F938F939F93AF93BF932D\r
+:10040000CF93DF93EF93FF9399B390FD02C080E009\r
+:1004100001C083E091FF02C091E0892720917B0019\r
+:10042000281B20FF1FC080937B008091B2002270A8\r
+:10043000882359F08091BD009091BE0041E050E0CA\r
+:10044000421B5109840F951F09C08091BD009091F6\r
+:10045000BE0030E021503109820F931F9093BE00FF\r
+:100460008093BD0099B39695969590FD02C080E06B\r
+:1004700001C083E091FF02C091E0892720917A00BA\r
+:10048000281B20FF1FC080937A008091B100422F6B\r
+:100490004270882349F08091BB009091BC00242FCA\r
+:1004A00030E02150310908C08091BB009091BC0020\r
+:1004B00021E030E0241B3109820F931F9093BC0090\r
+:1004C0008093BB0099B392959F7090FD02C080E02D\r
+:1004D00001C083E091FF02C091E08927209179005B\r
+:1004E000281B20FF1FC0809379008091B000422F0D\r
+:1004F0004270882349F08091B9009091BA00242F6E\r
+:1005000030E02150310908C08091B9009091BA00C3\r
+:1005100021E030E0241B3109820F931F9093BA0031\r
+:100520008093B90099B3929596959695937090FDA6\r
+:1005300002C080E001C083E0927011F091E0892751\r
+:1005400020917800281B20FF1FC0809378008091A5\r
+:10055000AF00422F4270882349F08091B7009091FC\r
+:10056000B800242F30E02150310908C08091B70035\r
+:100570009091B80021E030E0241B3109820F931FD5\r
+:100580009093B8008093B7008091B600813009F055\r
+:10059000C3C06091BD007091BE00809185009091B4\r
+:1005A0008600681B790B882777FD8095982F0E941D\r
+:1005B0008E0A2AE037ED43EA5CE30E94F3090E94C9\r
+:1005C0005B0A7093A6006093A5008091AD009091A6\r
+:1005D000AE00892B29F41092C6001092C50094C079\r
+:1005E000C091AD00D091AE008091A5009091A60081\r
+:1005F000C81BD90B8091930090919400A091950015\r
+:10060000B09196004E01AA2497FCA094BA2C880DB4\r
+:10061000991DAA1DBB1D8093930090939400A093F5\r
+:100620009500B09396006091C5007091C60040900F\r
+:100630009300509094006090950070909600009107\r
+:100640009D0010919E00882777FD8095982F0E942D\r
+:100650008E0A6B017C01C501B4010E948E0A20E064\r
+:1006600030E040E251E40E941A0B4B015C01C301EF\r
+:10067000B2010E948E0A20E030E0A9010E941A0B0C\r
+:100680009B01AC01C501B4010E948F094B015C01C3\r
+:10069000BE01601B710B882777FD8095982F0E9403\r
+:1006A0008E0A20E030E0A9010E941A0B9B01AC01E8\r
+:1006B000C501B4010E948F099B01AC01C701B601BD\r
+:1006C0000E948F090E945B0A7093C6006093C50068\r
+:1006D000D0939E00C0939D008091C5009091C6006C\r
+:1006E0008F3F910521F01CF08FEF90E009C08091C1\r
+:1006F000C5009091C60081309F4F34F481E09FEF98\r
+:100700009093C6008093C5008091BD009091BE007B\r
+:1007100090938600809385008091B500813009F028\r
+:10072000C3C06091BB007091BC0080918300909128\r
+:100730008400681B790B882777FD8095982F0E948D\r
+:100740008E0A2AE037ED43EA5CE30E94F3090E9437\r
+:100750005B0A7093A4006093A3008091AB0090911A\r
+:10076000AC00892B29F41092C4001092C30094C0ED\r
+:10077000C091AB00D091AC008091A3009091A400F7\r
+:10078000C81BD90B80918F0090919000A09191008F\r
+:10079000B09192004E01AA2497FCA094BA2C880D27\r
+:1007A000991DAA1DBB1D80938F0090939000A0936C\r
+:1007B0009100B09392006091C3007091C40040908A\r
+:1007C0008F00509090006090910070909200009186\r
+:1007D0009B0010919C00882777FD8095982F0E94A0\r
+:1007E0008E0A6B017C01C501B4010E948E0A20E0D3\r
+:1007F00030E040E251E40E941A0B4B015C01C3015E\r
+:10080000B2010E948E0A20E030E0A9010E941A0B7A\r
+:100810009B01AC01C501B4010E948F094B015C0131\r
+:10082000BE01601B710B882777FD8095982F0E9471\r
+:100830008E0A20E030E0A9010E941A0B9B01AC0156\r
+:10084000C501B4010E948F099B01AC01C701B6012B\r
+:100850000E948F090E945B0A7093C4006093C300DA\r
+:10086000D0939C00C0939B008091C3009091C400E2\r
+:100870008F3F910521F01CF08FEF90E009C080912F\r
+:10088000C3009091C40081309F4F34F481E09FEF0A\r
+:100890009093C4008093C3008091BB009091BC00F2\r
+:1008A00090938400809383008091B400813009F09C\r
+:1008B000C3C06091B9007091BA008091810090919D\r
+:1008C0008200681B790B882777FD8095982F0E94FE\r
+:1008D0008E0A2AE037ED43EA5CE30E94F3090E94A6\r
+:1008E0005B0A7093A2006093A1008091A90090918F\r
+:1008F000AA00892B29F41092C2001092C10094C062\r
+:10090000C091A900D091AA008091A1009091A2006D\r
+:10091000C81BD90B80918B0090918C00A0918D0009\r
+:10092000B0918E004E01AA2497FCA094BA2C880D99\r
+:10093000991DAA1DBB1D80938B0090938C00A093E2\r
+:100940008D00B0938E006091C1007091C200409004\r
+:100950008B0050908C0060908D0070908E00009104\r
+:10096000990010919A00882777FD8095982F0E9412\r
+:100970008E0A6B017C01C501B4010E948E0A20E041\r
+:1009800030E040E251E40E941A0B4B015C01C301CC\r
+:10099000B2010E948E0A20E030E0A9010E941A0BE9\r
+:1009A0009B01AC01C501B4010E948F094B015C01A0\r
+:1009B000BE01601B710B882777FD8095982F0E94E0\r
+:1009C0008E0A20E030E0A9010E941A0B9B01AC01C5\r
+:1009D000C501B4010E948F099B01AC01C701B6019A\r
+:1009E0000E948F090E945B0A7093C2006093C1004D\r
+:1009F000D0939A00C09399008091C1009091C20059\r
+:100A00008F3F910521F01CF08FEF90E009C080919D\r
+:100A1000C1009091C20081309F4F34F481E09FEF7C\r
+:100A20009093C2008093C1008091B9009091BA0068\r
+:100A300090938200809381008091B300813009F00F\r
+:100A4000C3C06091B7007091B80080917F00909111\r
+:100A50008000681B790B882777FD8095982F0E946E\r
+:100A60008E0A2AE037ED43EA5CE30E94F3090E9414\r
+:100A70005B0A7093A00060939F008091A700909103\r
+:100A8000A800892B29F41092C0001092BF0094C0D6\r
+:100A9000C091A700D091A80080919F009091A000E4\r
+:100AA000C81BD90B8091870090918800A091890084\r
+:100AB000B0918A004E01AA2497FCA094BA2C880D0C\r
+:100AC000991DAA1DBB1D8093870090938800A09359\r
+:100AD0008900B0938A006091BF007091C00040907F\r
+:100AE0008700509088006090890070908A00009183\r
+:100AF000970010919800882777FD8095982F0E9485\r
+:100B00008E0A6B017C01C501B4010E948E0A20E0AF\r
+:100B100030E040E251E40E941A0B4B015C01C3013A\r
+:100B2000B2010E948E0A20E030E0A9010E941A0B57\r
+:100B30009B01AC01C501B4010E948F094B015C010E\r
+:100B4000BE01601B710B882777FD8095982F0E944E\r
+:100B50008E0A20E030E0A9010E941A0B9B01AC0133\r
+:100B6000C501B4010E948F099B01AC01C701B60108\r
+:100B70000E948F090E945B0A7093C0006093BF00BF\r
+:100B8000D0939800C09397008091BF009091C000CF\r
+:100B90008F3F910521F01CF08FEF90E009C080910C\r
+:100BA000BF009091C00081309F4F34F481E09FEFEF\r
+:100BB0009093C0008093BF008091B7009091B800DF\r
+:100BC0009093800080937F00FF91EF91DF91CF9110\r
+:100BD000BF91AF919F918F917F916F915F914F9155\r
+:100BE0003F912F911F910F91FF90EF90DF90CF9049\r
+:100BF000BF90AF909F908F907F906F905F904F903D\r
+:100C00000F900FBE0F901F9018951F920F920FB666\r
+:100C10000F9211248F939F93EF93FF935F9B16C0C6\r
+:100C2000E091CB00F091CC008CB1808381E0E03387\r
+:100C3000F80730F43196F093CC00E093CB0006C077\r
+:100C40008DEC90E09093CC008093CB00FF91EF91DE\r
+:100C50009F918F910F900FBE0F901F9018959C0140\r
+:100C60008AB188618AB9579A80B5866080BD40E0B4\r
+:100C700050E084E0220F331F441F551F8A95D1F79F\r
+:100C800060E079E08DE390E00E947D0BDA01C9011C\r
+:100C90000197A109B109892F9A2FAB2FBB27A7FD77\r
+:100CA000BA9580BD215029B98DEC90E09093CA008F\r
+:100CB0008093C9009093CC008093CB0008955D9BF6\r
+:100CC000FECFFC0180818CB90895CF93DF931F92F2\r
+:100CD000CDB7DEB78983CE0101960E945F0680E022\r
+:100CE00090E00F90DF91CF910895CF93DF93EC01C7\r
+:100CF000CE012196FC012081222319F00E945F067B\r
+:100D0000F7CFDF91CF9108955F9BFECF8CB190E03C\r
+:100D100008950C94840669E876E085E696E00C94E4\r
+:100D2000CF0BCF93DF93FC01E253FF4FA081B18142\r
+:100D3000EC01C453DF4F28813981A217B307A1F01A\r
+:100D40005D9B12C02C912CB920813181AC0145539F\r
+:100D50005F4F2417350718F42F5F3F4F03C09C01E6\r
+:100D600028593F4F31832083E091C900F091CA0098\r
+:100D70002091CB003091CC002E173F07C1F1DC0150\r
+:100D8000AC59BF4FCD91DC911197208128836D9193\r
+:100D90007C9111979C012B523F4FE9013881EB0167\r
+:100DA0002881321307C09C012C523F4FE901488132\r
+:100DB0004F5F48839C012D593F4F6217730728F4FA\r
+:100DC000EB012196CD93DC9302C08D939C93D1E0EF\r
+:100DD000E033FD0730F43196F093CA00E093C90088\r
+:100DE00006C08DEC90E09093CA008093C900DF911B\r
+:100DF000CF91089588E087BB8CEF84BB8CEB81BBDF\r
+:100E00001092C70080E895E20E942F060E948B0690\r
+:100E100080E582B986B78B7E86BF85EC86BF81EA86\r
+:100E20008FBD89E08EBD1BBC1ABC19BC18BC89E6FD\r
+:100E300085BD13BC83BF1CBE8EE690E00E944C0CA7\r
+:100E400085B78F7885BF84E089BF7894C0E0D8E10A\r
+:100E50008091C8008F3F39F48091C700853A19F41A\r
+:100E6000F894FE0109958091C5009091C6002091EB\r
+:100E700066003091670028173907B9F18091C500E5\r
+:100E80009091C600892B19F485B38C6017C08091AE\r
+:100E9000B200811107C08091C5009091C60018165C\r
+:100EA000190654F08091B200882351F08091C5005A\r
+:100EB0009091C60097FF04C085B3837F85BB02C0B5\r
+:100EC000AA98AB9A8091C5009091C6009093670054\r
+:100ED000809366008091C5009091C60097FF03C083\r
+:100EE0009195819591099BBD8ABD8091C300909198\r
+:100EF000C400209164003091650028173907B9F1CA\r
+:100F00008091C3009091C400892B19F485B380634C\r
+:100F100017C08091B100811107C08091C3009091EA\r
+:100F2000C4001816190654F08091B100882351F0BE\r
+:100F30008091C3009091C40097FF04C085B38F7C5B\r
+:100F400085BB02C0AC98AD9A8091C3009091C4005B\r
+:100F500090936500809364008091C3009091C400D9\r
+:100F600097FF03C091958195910999BD88BD8091A6\r
+:100F7000C1009091C2002091620030916300281757\r
+:100F80003907B1F18091C1009091C200892B19F409\r
+:100F900085B3806C17C08091B000811107C080912B\r
+:100FA000C1009091C2001816190654F08091B0004B\r
+:100FB000882351F08091C1009091C20097FF04C036\r
+:100FC00085B38F7385BB02C0AE98AF9A8091C10084\r
+:100FD0009091C20090936300809362008091C10061\r
+:100FE0009091C20097FF03C091958195910983BDAF\r
+:100FF0008091BF009091C00020916000309161000D\r
+:1010000028173907B1F18091BF009091C000892B5A\r
+:1010100019F482B38C6017C08091AF00811107C0B2\r
+:101020008091BF009091C0001816190654F080916D\r
+:10103000AF00882351F08091BF009091C00097FFCE\r
+:1010400004C082B3837F82BB02C09298939A80913E\r
+:10105000BF009091C00090936100809360008091E8\r
+:10106000BF009091C00097FF03C0919581959109B1\r
+:101070008CBF85B7806885BF889585B78F7785BF1A\r
+:10108000E7CE6F927F929F92AF92BF92CF92DF9204\r
+:10109000EF92FF920F931F93CF93DF93CDB7DEB7FD\r
+:1010A0002C970FB6F894DEBF0FBECDBF8C017B012D\r
+:1010B0003A01FC0117821682838181FF19C1CE019A\r
+:1010C00001965C01F801D380F701D3FC2591D3FE92\r
+:1010D00021917F01222309F407C1253249F4D3FC71\r
+:1010E0002591D3FE21917F01253211F0912C1BC057\r
+:1010F000B801822F90E00E941A0C63013601E2CF02\r
+:10110000222309F4F1C0622F70E087EA90E02C8777\r
+:101110000E94C40B2C85892B49F0F701D3FC259143\r
+:10112000D3FE21917F0197FEEBCF0CC0233221F437\r
+:10113000F92DF0619F2EF1CF2C3639F4292D20683E\r
+:10114000922EEBCF222309F4CFC0622F70E080EA09\r
+:1011500090E02C870E94C40B2C85892B41F0630101\r
+:10116000F4E0CF0ED11CB8018FE390E012C023361B\r
+:1011700041F0233789F0233521F5F92DF1609F2EB9\r
+:101180000BC0630122E0C20ED11CB801F3018081C3\r
+:1011900091810E941A0CB2CF630122E0C20ED11CD1\r
+:1011A000F30160807180F30190FC859190FE819144\r
+:1011B0003F01882309F4A2CFB80190E00E941A0CE5\r
+:1011C000F2CF243611F0293639F5630197FE09C0B4\r
+:1011D000F4E0CF0ED11CF301608171818281938193\r
+:1011E0000AC0F2E0CF0ED11CF30160817181882723\r
+:1011F00077FD8095982FF92DFF7E9F2E97FF09C0D0\r
+:1012000090958095709561957F4F8F4F9F4FF064BB\r
+:101210009F2E2AE030E038C02037A1F040F4283576\r
+:10122000D1F02F3609F060C028E030E019C0253732\r
+:1012300019F0283751F058C0292D2F7E922E2AE020\r
+:1012400030E00EC0892D8061982E992D9462992EE0\r
+:1012500020E130E005C0E92DE4609E2E20E132E07F\r
+:10126000630197FE09C0F4E0CF0ED11CF301608149\r
+:1012700071818281938108C0F2E0CF0ED11CF3010D\r
+:101280006081718180E090E0A5010E94790C782E48\r
+:101290007A1896FE05C0B8018DE290E00E941A0C03\r
+:1012A00094FE16C0FE01E70DF11D8081803381F0B0\r
+:1012B000B80180E390E00E941A0C92FE09C0292D2B\r
+:1012C0002072822F90E0B801885A9F4F0E941A0C1A\r
+:1012D0007A94F501E70DF11D8081B80190E00E943C\r
+:1012E0001A0C7110F5CF0ACFF8018681978102C0E0\r
+:1012F0008FEF9FEF2C960FB6F894DEBF0FBECDBFD9\r
+:10130000DF91CF911F910F91FF90EF90DF90CF90E1\r
+:10131000BF90AF909F907F906F9008955058BB27DB\r
+:10132000AA270ED04DC13ED130F043D120F031F488\r
+:101330009F3F11F41EF433C10EF4E095E7FB29C181\r
+:10134000E92F4FD180F3BA17620773078407950717\r
+:1013500018F071F49EF567C10EF4E0950B2EBA2FCC\r
+:10136000A02D0B01B90190010C01CA01A0011124AB\r
+:10137000FF27591B99F0593F50F4503E68F11A1657\r
+:10138000F040A22F232F342F4427585FF3CF4695E8\r
+:1013900037952795A795F0405395C9F77EF41F160A\r
+:1013A000BA0B620B730B840BBAF09150A1F0FF0FD4\r
+:1013B000BB1F661F771F881FC2F70EC0BA0F621FC0\r
+:1013C000731F841F48F4879577956795B795F795B0\r
+:1013D0009E3F08F0B3CF9395880F08F09927EE0F42\r
+:1013E0009795879508950CD0EBC0E3D040F0DAD004\r
+:1013F00030F021F45F3F19F0CCC0511115C1CFC0BE\r
+:10140000F0D098F39923C9F35523B1F3951B550BED\r
+:10141000BB27AA2762177307840738F09F5F5F4FC7\r
+:10142000220F331F441FAA1FA9F333D00E2E3AF008\r
+:10143000E0E830D091505040E695001CCAF729D022\r
+:10144000FE2F27D0660F771F881FBB1F2617370771\r
+:101450004807AB07B0E809F0BB0B802DBF01FF27A1\r
+:1014600093585F4F2AF09E3F510568F092C0DCC050\r
+:101470005F3FECF3983EDCF3869577956795B795DB\r
+:10148000F7959F5FC9F7880F911D9695879597F9F6\r
+:101490000895E1E0660F771F881FBB1F621773076F\r
+:1014A0008407BA0720F0621B730B840BBA0BEE1F84\r
+:1014B00088F7E095089504D06894B111B5C00895F7\r
+:1014C00098D088F09F5790F0B92F9927B751A0F086\r
+:1014D000D1F0660F771F881F991F1AF0BA95C9F7C8\r
+:1014E00012C0B13081F09FD0B1E008959CC0672F49\r
+:1014F000782F8827B85F39F0B93FCCF38695779578\r
+:101500006795B395D9F73EF4909580957095619560\r
+:101510007F4F8F4F9F4F0895E89409C097FB3EF48B\r
+:1015200090958095709561957F4F8F4F9F4F992330\r
+:10153000A9F0F92F96E9BB279395F69587957795AE\r
+:101540006795B795F111F8CFFAF4BB0F11F460FF6E\r
+:101550001BC06F5F7F4F8F4F9F4F16C0882311F0C6\r
+:1015600096E911C0772321F09EE8872F762F05C0DA\r
+:10157000662371F096E8862F70E060E02AF09A9575\r
+:10158000660F771F881FDAF7880F9695879597F96A\r
+:10159000089597F99F6780E870E060E008959FEFF5\r
+:1015A00080EC089500240A941616170618060906FA\r
+:1015B000089500240A9412161306140605060895C9\r
+:1015C000092E0394000C11F4882352F0BB0F40F451\r
+:1015D000BF2B11F460FF04C06F5F7F4F8F4F9F4F91\r
+:1015E000089557FD9058440F551F59F05F3F71F013\r
+:1015F0004795880F97FB991F61F09F3F79F087957A\r
+:101600000895121613061406551FF2CF4695F1DF02\r
+:1016100008C0161617061806991FF1CF8695710592\r
+:10162000610508940895E894BB2766277727CB01C6\r
+:1016300097F908950BD0C4CFB5DF28F0BADF18F0C2\r
+:10164000952309F0A6CFABCF1124EECFCADFA0F3CC\r
+:10165000959FD1F3950F50E0551F629FF001729F47\r
+:10166000BB27F00DB11D639FAA27F00DB11DAA1F66\r
+:10167000649F6627B00DA11D661F829F2227B00DB3\r
+:10168000A11D621F739FB00DA11D621F839FA00D3E\r
+:10169000611D221F749F3327A00D611D231F849F8E\r
+:1016A000600D211D822F762F6A2F11249F575040E5\r
+:1016B0008AF0E1F088234AF0EE0FFF1FBB1F661F80\r
+:1016C000771F881F91505040A9F79E3F510570F039\r
+:1016D00060CFAACF5F3FECF3983EDCF38695779519\r
+:1016E0006795B795F795E7959F5FC1F7FE2B880F34\r
+:1016F000911D9695879597F90895052E97FB1EF4F1\r
+:1017000000940E94940B57FD07D00E94A20B07FC87\r
+:1017100003D04EF40C94940B509540953095219540\r
+:101720003F4F4F4F5F4F089590958095709561950D\r
+:101730007F4F8F4F9F4F0895EE0FFF1F0590F4913D\r
+:10174000E02D0994A1E21A2EAA1BBB1BFD010DC0BE\r
+:10175000AA1FBB1FEE1FFF1FA217B307E407F50761\r
+:1017600020F0A21BB30BE40BF50B661F771F881F3D\r
+:10177000991F1A9469F760957095809590959B01D3\r
+:10178000AC01BD01CF010895FC010590061621F0C2\r
+:101790000020D9F7C00108953197CF0108950F9324\r
+:1017A0001F93CF93DF93EC018B01009731F4611508\r
+:1017B000710519F480E090E038C06EE070E081E0DF\r
+:1017C00090E00E94D70CFC010097A1F380E883838E\r
+:1017D0000115110571F01387028781E883838091D9\r
+:1017E000340190913501892B21F4F0933501E09378\r
+:1017F00034012097C9F0D187C087838182608383B9\r
+:101800008091360190913701892B71F4F093370163\r
+:10181000E09336018091380190913901892B21F4B0\r
+:10182000F0933901E0933801CF01DF91CF911F91FF\r
+:101830000F9108950F931F93CF93DF93182F092FC4\r
+:10184000EB018B8181FD03C08FEF9FEF20C082FFF2\r
+:1018500010C04E815F812C813D81421753077CF47B\r
+:10186000E881F9819F012F5F3F4F398328831083DF\r
+:1018700006C0E885F985812F0995892B29F72E81E6\r
+:101880003F812F5F3F4F3F832E83812F902FDF912A\r
+:10189000CF911F910F9108950F931F93CF93DF93D3\r
+:1018A000E0913601F0913701238121FF1BC0EC014B\r
+:1018B00000E010E089916091360170913701DB0101\r
+:1018C0001896ED91FC911997882331F00995892B91\r
+:1018D00089F30FEF1FEFEECF8AE00995892B11F402\r
+:1018E000C80102C08FEF9FEFDF91CF911F910F9141\r
+:1018F0000895FA01AA27283051F1203181F1E894A6\r
+:101900006F936E7F6E5F7F4F8F4F9F4FAF4FB1E0F2\r
+:101910003ED0B4E03CD0670F781F891F9A1FA11DED\r
+:10192000680F791F8A1F911DA11D6A0F711D811DEE\r
+:10193000911DA11D20D009F468943F912AE0269FB3\r
+:1019400011243019305D3193DEF6CF010895462F12\r
+:101950004770405D4193B3E00FD0C9F7F6CF462FF3\r
+:101960004F70405D4A3318F0495D31FD405241935C\r
+:1019700002D0A9F7EACFB4E0A69597958795779519\r
+:101980006795BA95C9F700976105710508959B01A0\r
+:10199000AC010A2E06945795479537952795BA9529\r
+:1019A000C9F7620F731F841F951FA01D08950F9321\r
+:1019B0001F93CF93DF93869F8001879F100D969F83\r
+:1019C000100D1124C8010E94F30CEC01009729F0BE\r
+:1019D000A80160E070E00E941A0ECE01DF91CF9165\r
+:1019E0001F910F910895CF93DF938230910510F4EA\r
+:1019F00082E090E0E0913C01F0913D0120E030E098\r
+:101A0000A0E0B0E0309739F1408151814817590783\r
+:101A1000B8F04817590771F482819381109729F023\r
+:101A200013969C938E9312972CC090933D018093B4\r
+:101A30003C0127C02115310531F04217530718F03A\r
+:101A4000A901DB0101C0EF019A01BD01DF010280A4\r
+:101A5000F381E02DD7CF21153105F9F0281B390B83\r
+:101A60002430310580F48A819B816115710521F054\r
+:101A7000FB019383828304C090933D0180933C01DA\r
+:101A8000FE01329644C0FE01E20FF31F8193919351\r
+:101A900022503109398328833AC020913A0130918C\r
+:101AA0003B01232B41F420916A0030916B0030936D\r
+:101AB0003B0120933A012091680030916900211583\r
+:101AC000310541F42DB73EB740916C0050916D0047\r
+:101AD000241B350BE0913A01F0913B01E217F3072B\r
+:101AE000A0F42E1B3F0B2817390778F0AC014E5F8E\r
+:101AF0005F4F2417350748F04E0F5F1F50933B018F\r
+:101B000040933A018193919302C0E0E0F0E0CF016D\r
+:101B1000DF91CF910895CF93DF93009709F487C0A9\r
+:101B2000FC01329713821282C0913C01D0913D0199\r
+:101B3000209781F420813181280F391F80913A014B\r
+:101B400090913B018217930779F5F0933B01E09365\r
+:101B50003A016DC0DE0120E030E0AE17BF0750F45F\r
+:101B600012964D915C9113979D014115510509F114\r
+:101B7000DA01F3CFB383A28340815181840F951F93\r
+:101B80008A179B0771F48D919C911197840F951F73\r
+:101B900002969183808312968D919C9113979383E3\r
+:101BA00082832115310529F4F0933D01E0933C0136\r
+:101BB0003EC0D9011396FC93EE9312974D915D911F\r
+:101BC000A40FB51FEA17FB0779F480819181840F78\r
+:101BD000951F0296D90111969C938E9382819381D1\r
+:101BE00013969C938E931297E0E0F0E08A819B819C\r
+:101BF000009719F0FE01EC01F9CFCE010296288181\r
+:101C00003981820F931F20913A0130913B012817AF\r
+:101C1000390769F4309729F410923D0110923C0184\r
+:101C200002C013821282D0933B01C0933A01DF912C\r
+:101C3000CF910895DC0101C06D9341505040E0F711\r
+:061C40000895F894FFCFA7\r
+:101C4600008000800080008000003E0120000D0A18\r
+:081C560053746172740D00006B\r
+:00000001FF\r
--- /dev/null
+#ifndef RINGBUFFER_H
+#define RINGBUFFER_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;
+
+ringbuffer_t *ringbuffers[RINGBUFFER_MAX_NUM];
+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
+
projects / ros_wild_thumper.git / commitdiff