From 73f9b5e5a082dd91b6d96fdddbe953afe4362b9d Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Thu, 7 May 2015 21:32:48 +0200 Subject: [PATCH] added initial avr motor_ctrl --- .gitignore | 1 + avr/motor_ctrl/Makefile | 438 ++++++++++++++++++++++++++ avr/motor_ctrl/main.c | 602 ++++++++++++++++++++++++++++++++++++ avr/motor_ctrl/main.hex | 456 +++++++++++++++++++++++++++ avr/motor_ctrl/ringbuffer.h | 34 ++ avr/motor_ctrl/uart.c | 97 ++++++ avr/motor_ctrl/uart.h | 18 ++ 7 files changed, 1646 insertions(+) create mode 100644 avr/motor_ctrl/Makefile create mode 100644 avr/motor_ctrl/main.c create mode 100644 avr/motor_ctrl/main.hex create mode 100644 avr/motor_ctrl/ringbuffer.h create mode 100644 avr/motor_ctrl/uart.c create mode 100644 avr/motor_ctrl/uart.h diff --git a/.gitignore b/.gitignore index 0d20b64..b948985 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ +*.swp *.pyc diff --git a/avr/motor_ctrl/Makefile b/avr/motor_ctrl/Makefile new file mode 100644 index 0000000..50995aa --- /dev/null +++ b/avr/motor_ctrl/Makefile @@ -0,0 +1,438 @@ +# 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 + + +# 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=4000000 + +# Place -I options here +CINCS = -Ii2c/ + + +# Compiler flags. +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CFLAGS = -g$(DEBUG) +CFLAGS += $(CDEFS) $(CINCS) +CFLAGS += -O$(OPT) +CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums +CFLAGS += -Wall -Wstrict-prototypes +CFLAGS += -Wa,-adhlns=$(<:.c=.lst) +CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +CFLAGS += $(CSTANDARD) +CFLAGS += -DF_OSC=$(F_OSC) + + + +# Assembler flags. +# -Wa,...: tell GCC to pass this to the assembler. +# -ahlms: create listing +# -gstabs: have the assembler create line number information; note that +# for use in COFF files, additional information about filenames +# and function names needs to be present in the assembler source +# files -- see avr-libc docs [FIXME: not yet described there] +ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs +ASFLAGS += -DF_OSC=$(F_OSC) + + +#Additional libraries. + +# Minimalistic printf version +PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min + +# Floating point printf version (requires MATH_LIB = -lm below) +PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt -lm + +PRINTF_LIB = + +# Minimalistic scanf version +SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min + +# Floating point + %[ scanf version (requires MATH_LIB = -lm below) +SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt + +SCANF_LIB = + +MATH_LIB = -lm + +# External memory options + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# used for variables (.data/.bss) and heap (malloc()). +#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# only used for heap (malloc()). +#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff + +EXTMEMOPTS = + +# Linker flags. +# -Wl,...: tell GCC to pass this to linker. +# -Map: create map file +# --cref: add cross reference to map file +LDFLAGS = -Wl,-Map=$(TARGET).map,--cref +LDFLAGS += $(EXTMEMOPTS) +LDFLAGS += $(PRINTF_LIB_MIN) $(SCANF_LIB) $(MATH_LIB) + + + + +# Programming support using avrdude. Settings and variables. + +# Programming hardware: alf avr910 avrisp bascom bsd +# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500 +# +# Type: avrdude -c ? +# to get a full listing. +# +#AVRDUDE_PROGRAMMER = avr911 +AVRDUDE_PROGRAMMER = avrisp2 + +# com1 = serial port. Use lpt1 to connect to parallel port. +#AVRDUDE_PORT = /dev/ttyUSB0 # programmer connected to serial device +AVRDUDE_PORT = usb # programmer connected to serial device + +AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex +#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep + + +# Uncomment the following if you want avrdude's erase cycle counter. +# Note that this counter needs to be initialized first using -Yn, +# see avrdude manual. +#AVRDUDE_ERASE_COUNTER = -y + +# Uncomment the following if you do /not/ wish a verification to be +# performed after programming the device. +#AVRDUDE_NO_VERIFY = -V + +# Increase verbosity level. Please use this when submitting bug +# reports about avrdude. See +# to submit bug reports. +#AVRDUDE_VERBOSE = -v -v + +AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) +AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY) +AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE) +AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER) + + + +# --------------------------------------------------------------------------- + +# Define directories, if needed. +DIRAVR = c:/winavr +DIRAVRBIN = $(DIRAVR)/bin +DIRAVRUTILS = $(DIRAVR)/utils/bin +DIRINC = . +DIRLIB = $(DIRAVR)/avr/lib + + +# Define programs and commands. +SHELL = sh +CC = avr-gcc +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump +SIZE = avr-size +NM = avr-nm +AVRDUDE = avrdude +REMOVE = rm -f +COPY = cp + + + + +# Define Messages +# English +MSG_ERRORS_NONE = Errors: none +MSG_BEGIN = -------- begin -------- +MSG_END = -------- end -------- +MSG_SIZE_BEFORE = Size before: +MSG_SIZE_AFTER = Size after: +MSG_COFF = Converting to AVR COFF: +MSG_EXTENDED_COFF = Converting to AVR Extended COFF: +MSG_FLASH = Creating load file for Flash: +MSG_EEPROM = Creating load file for EEPROM: +MSG_EXTENDED_LISTING = Creating Extended Listing: +MSG_SYMBOL_TABLE = Creating Symbol Table: +MSG_LINKING = Linking: +MSG_COMPILING = Compiling: +MSG_ASSEMBLING = Assembling: +MSG_CLEANING = Cleaning project: + + + + +# Define all object files. +OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) + +# Define all listing files. +LST = $(ASRC:.S=.lst) $(SRC:.c=.lst) + + +# Compiler flags to generate dependency files. +### GENDEPFLAGS = -Wp,-M,-MP,-MT,$(*F).o,-MF,.dep/$(@F).d +GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d + +# Combine all necessary flags and optional flags. +# Add target processor to flags. +ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS) +ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) + + + + + +# Default target. +all: begin gccversion sizebefore build sizeafter finished end + +build: elf hex eep lss sym + +elf: $(TARGET).elf +hex: $(TARGET).hex +eep: $(TARGET).eep +lss: $(TARGET).lss +sym: $(TARGET).sym + + + +# Eye candy. +# AVR Studio 3.x does not check make's exit code but relies on +# the following magic strings to be generated by the compile job. +begin: + @echo + @echo $(MSG_BEGIN) + +finished: + @echo $(MSG_ERRORS_NONE) + +end: + @echo $(MSG_END) + @echo + + +# Display size of file. +HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex +ELFSIZE = $(SIZE) -A $(TARGET).elf +sizebefore: + @if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi + +sizeafter: + @if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi + + + +# Display compiler version information. +gccversion : + @$(CC) --version + + + +# Program the device. +program: $(TARGET).hex $(TARGET).eep + $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) + + + + +# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB. +COFFCONVERT=$(OBJCOPY) --debugging \ +--change-section-address .data-0x800000 \ +--change-section-address .bss-0x800000 \ +--change-section-address .noinit-0x800000 \ +--change-section-address .eeprom-0x810000 + + +coff: $(TARGET).elf + @echo + @echo $(MSG_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-avr $< $(TARGET).cof + + +extcoff: $(TARGET).elf + @echo + @echo $(MSG_EXTENDED_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof + + + +# Create final output files (.hex, .eep) from ELF output file. +%.hex: %.elf + @echo + @echo $(MSG_FLASH) $@ + $(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@ + +%.eep: %.elf + @echo + @echo $(MSG_EEPROM) $@ + -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ + --change-section-lma .eeprom=0 -O $(FORMAT) $< $@ + +# Create extended listing file from ELF output file. +%.lss: %.elf + @echo + @echo $(MSG_EXTENDED_LISTING) $@ + $(OBJDUMP) -h -S $< > $@ + +# Create a symbol table from ELF output file. +%.sym: %.elf + @echo + @echo $(MSG_SYMBOL_TABLE) $@ + $(NM) -n $< > $@ + + + +# Link: create ELF output file from object files. +.SECONDARY : $(TARGET).elf +.PRECIOUS : $(OBJ) +%.elf: $(OBJ) + @echo + @echo $(MSG_LINKING) $@ + $(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS) + + +# Compile: create object files from C source files. +%.o : %.c + @echo + @echo $(MSG_COMPILING) $< + $(CC) -c $(ALL_CFLAGS) $< -o $@ + + +# Compile: create assembler files from C source files. +%.s : %.c + $(CC) -S $(ALL_CFLAGS) $< -o $@ + + +# Assemble: create object files from assembler source files. +%.o : %.S + @echo + @echo $(MSG_ASSEMBLING) $< + $(CC) -c $(ALL_ASFLAGS) $< -o $@ + + + +# Target: clean project. +clean: begin clean_list finished end + +clean_list : + @echo + @echo $(MSG_CLEANING) + $(REMOVE) $(TARGET).hex + $(REMOVE) $(TARGET).eep + $(REMOVE) $(TARGET).obj + $(REMOVE) $(TARGET).cof + $(REMOVE) $(TARGET).elf + $(REMOVE) $(TARGET).map + $(REMOVE) $(TARGET).obj + $(REMOVE) $(TARGET).a90 + $(REMOVE) $(TARGET).sym + $(REMOVE) $(TARGET).lnk + $(REMOVE) $(TARGET).lss + $(REMOVE) $(OBJ) + $(REMOVE) $(LST) + $(REMOVE) $(SRC:.c=.s) + $(REMOVE) $(SRC:.c=.d) + $(REMOVE) .dep/* + + + +# Include the dependency files. +-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*) + + +# Listing of phony targets. +.PHONY : all begin finish end sizebefore sizeafter gccversion \ +build elf hex eep lss sym coff extcoff \ +clean clean_list program + diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c new file mode 100644 index 0000000..0c07d13 --- /dev/null +++ b/avr/motor_ctrl/main.c @@ -0,0 +1,602 @@ +#include +#include +#include +#include +#include +#include +#include "uart.h" + +/* + * I2C Register Map (8 Bit) + * 0x00 Register select + * 0x01 Motor 1 PWM MSB + * 0x02 Motor 1 PWM LSB + * 0x03 Motor 2 PWM MSB + * 0x04 Motor 2 PWM LSB + * 0x05 Motor 3 PWM MSB + * 0x06 Motor 3 PWM LSB + * 0x07 Motor 4 PWM MSB + * 0x08 Motor 4 PWM LSB + * free + * 0x10 Hall 1 MSB + * 0x11 Hall 1 LSB + * 0x12 Hall 2 MSB + * 0x13 Hall 2 LSB + * 0x14 Hall 3 MSB + * 0x15 Hall 3 LSB + * 0x16 Hall 4 MSB + * 0x17 Hall 4 LSB + * free + * 0x20 Motor 1 speed wish MSB + * 0x21 Motor 1 speed wish LSB + * 0x22 Motor 2 speed wish MSB + * 0x23 Motor 2 speed wish LSB + * 0x24 Motor 3 speed wish MSB + * 0x25 Motor 3 speed wish LSB + * 0x26 Motor 4 speed wish MSB + * 0x27 Motor 4 speed wish LSB + * 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<>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; +} diff --git a/avr/motor_ctrl/main.hex b/avr/motor_ctrl/main.hex new file mode 100644 index 0000000..6b1dece --- /dev/null +++ b/avr/motor_ctrl/main.hex @@ -0,0 +1,456 @@ +:100000000C945C000C9479000C9479000C947900A9 +:100010000C9479000C9479000C9479000C9479007C +:100020000C9479000C9479000C9479000C94E30101 +:100030000C9479000C9405060C9479000C947900CA +:100040000C9479000C9479000C9479000C947B004A +:100050000C9479007901C9017C01C9017F01C901B2 +:100060008101C901C901C901C901C901C901C90188 +:1000700083019C0188019C0192019C0197019C01D4 +:10008000C901C901C901C901C901C901C901C90120 +:100090009F01A401A901AE01B301B801BD01C201D4 +:1000A00045464765666700202B2D2E3031323334AC +:1000B000353637383968000011241FBECFE5D8E047 +:1000C000DEBFCDBF10E0A0E6B0E0E6E4FCE102C098 +:1000D00005900D92A837B107D9F721E0A8E7B0E065 +:1000E00001C01D92AE33B207E1F70E94FA060C94EC +:1000F000210E0C9400001F920F920FB60F92112444 +:100100002F938F939F93AF93BF93EF93FF9381B1FF +:10011000887F803881F040F4803609F043C185EC57 +:1001200086BF1092C80043C1883A09F4D5C0883B05 +:1001300009F4D2C037C18091C800813209F495C05A +:1001400068F5843009F479C060F4813009F4A7C0FF +:1001500008F45EC0823009F464C0833009F49FC0A3 +:1001600019C1873009F49BC090F4853009F497C019 +:10017000863009F00FC183B120917E0090E0922B70 +:100180009093C2008093C1001092B40000C18830E7 +:1001900009F45FC0803209F482C0FCC0873209F4E0 +:1001A00082C0F0F4843209F47AC098F4823209F4FF +:1001B00076C0833209F0EEC083B120917E0090E0DA +:1001C000922B9093AC008093AB0081E08093B500BC +:1001D000DEC0853209F456C0863209F460C0DAC048 +:1001E000823909F471C050F4803909F469C0813949 +:1001F00009F0D0C083B18093B100C9C0833909F43C +:1002000067C08F3F09F0C6C083B18093C700C2C0EA +:1002100083B18093C8008091C80081508093C8004A +:10022000B6C083B120917E0090E0922B9093C600DF +:100230008093C5001092B600AAC083B120917E00C1 +:1002400090E0922B9093C4008093C3001092B5006D +:100250009EC083B120917E0090E0922B9093C000CD +:100260008093BF001092B30092C083B120917E00B2 +:1002700090E0922B9093AE008093AD0081E080934C +:10028000B60085C083B120917E0090E0922B9093C0 +:10029000AA008093A90081E08093B40078C083B164 +:1002A00080937E0074C083B120917E0090E0922BF9 +:1002B0009093A8008093A70081E08093B30067C06B +:1002C00083B18093B20063C083B18093B0005FC0FC +:1002D00083B18093AF005BC08091C80090E0FC01C7 +:1002E0003297E632F10508F054C0E65DFF4F0C94FA +:1002F0009C0B8AB59BB54AC088B599B547C083B5F4 +:1003000045C08CB743C08091BD009091BE0004C031 +:100310008091BB009091BC0090937D0080937C0005 +:1003200093B935C08091B9009091BA00F5CF809112 +:10033000B7009091B800F0CF80917C0027C08091E9 +:10034000AD009091AE00ECCF8091AD009091AE00E9 +:100350001DC08091AB009091AC00E2CF8091AB00CA +:100360009091AC0013C08091A9009091AA00D8CFC1 +:100370008091A9009091AA0009C08091A700909156 +:10038000A800CECF8091A7009091A80083B985ECFA +:1003900002C013B885E886BF8091C8008F5F809344 +:1003A000C80005C086B78B7E86BF85EC86BFFF91EF +:1003B000EF91BF91AF919F918F912F910F900FBEB1 +:1003C0000F901F9018951F920F920FB60F92112445 +:1003D0004F925F926F927F928F929F92AF92BF9255 +:1003E000CF92DF92EF92FF920F931F932F933F9341 +:1003F0004F935F936F937F938F939F93AF93BF932D +:10040000CF93DF93EF93FF9399B390FD02C080E009 +:1004100001C083E091FF02C091E0892720917B0019 +:10042000281B20FF1FC080937B008091B2002270A8 +:10043000882359F08091BD009091BE0041E050E0CA +:10044000421B5109840F951F09C08091BD009091F6 +:10045000BE0030E021503109820F931F9093BE00FF +:100460008093BD0099B39695969590FD02C080E06B +:1004700001C083E091FF02C091E0892720917A00BA +:10048000281B20FF1FC080937A008091B100422F6B +:100490004270882349F08091BB009091BC00242FCA +:1004A00030E02150310908C08091BB009091BC0020 +:1004B00021E030E0241B3109820F931F9093BC0090 +:1004C0008093BB0099B392959F7090FD02C080E02D +:1004D00001C083E091FF02C091E08927209179005B +:1004E000281B20FF1FC0809379008091B000422F0D +:1004F0004270882349F08091B9009091BA00242F6E +:1005000030E02150310908C08091B9009091BA00C3 +:1005100021E030E0241B3109820F931F9093BA0031 +:100520008093B90099B3929596959695937090FDA6 +:1005300002C080E001C083E0927011F091E0892751 +:1005400020917800281B20FF1FC0809378008091A5 +:10055000AF00422F4270882349F08091B7009091FC +:10056000B800242F30E02150310908C08091B70035 +:100570009091B80021E030E0241B3109820F931FD5 +:100580009093B8008093B7008091B600813009F055 +:10059000C3C06091BD007091BE00809185009091B4 +:1005A0008600681B790B882777FD8095982F0E941D +:1005B0008E0A2AE037ED43EA5CE30E94F3090E94C9 +:1005C0005B0A7093A6006093A5008091AD009091A6 +:1005D000AE00892B29F41092C6001092C50094C079 +:1005E000C091AD00D091AE008091A5009091A60081 +:1005F000C81BD90B8091930090919400A091950015 +:10060000B09196004E01AA2497FCA094BA2C880DB4 +:10061000991DAA1DBB1D8093930090939400A093F5 +:100620009500B09396006091C5007091C60040900F +:100630009300509094006090950070909600009107 +:100640009D0010919E00882777FD8095982F0E942D +:100650008E0A6B017C01C501B4010E948E0A20E064 +:1006600030E040E251E40E941A0B4B015C01C301EF +:10067000B2010E948E0A20E030E0A9010E941A0B0C +:100680009B01AC01C501B4010E948F094B015C01C3 +:10069000BE01601B710B882777FD8095982F0E9403 +:1006A0008E0A20E030E0A9010E941A0B9B01AC01E8 +:1006B000C501B4010E948F099B01AC01C701B601BD +:1006C0000E948F090E945B0A7093C6006093C50068 +:1006D000D0939E00C0939D008091C5009091C6006C +:1006E0008F3F910521F01CF08FEF90E009C08091C1 +:1006F000C5009091C60081309F4F34F481E09FEF98 +:100700009093C6008093C5008091BD009091BE007B +:1007100090938600809385008091B500813009F028 +:10072000C3C06091BB007091BC0080918300909128 +:100730008400681B790B882777FD8095982F0E948D +:100740008E0A2AE037ED43EA5CE30E94F3090E9437 +:100750005B0A7093A4006093A3008091AB0090911A +:10076000AC00892B29F41092C4001092C30094C0ED +:10077000C091AB00D091AC008091A3009091A400F7 +:10078000C81BD90B80918F0090919000A09191008F +:10079000B09192004E01AA2497FCA094BA2C880D27 +:1007A000991DAA1DBB1D80938F0090939000A0936C +:1007B0009100B09392006091C3007091C40040908A +:1007C0008F00509090006090910070909200009186 +:1007D0009B0010919C00882777FD8095982F0E94A0 +:1007E0008E0A6B017C01C501B4010E948E0A20E0D3 +:1007F00030E040E251E40E941A0B4B015C01C3015E +:10080000B2010E948E0A20E030E0A9010E941A0B7A +:100810009B01AC01C501B4010E948F094B015C0131 +:10082000BE01601B710B882777FD8095982F0E9471 +:100830008E0A20E030E0A9010E941A0B9B01AC0156 +:10084000C501B4010E948F099B01AC01C701B6012B +:100850000E948F090E945B0A7093C4006093C300DA +:10086000D0939C00C0939B008091C3009091C400E2 +:100870008F3F910521F01CF08FEF90E009C080912F +:10088000C3009091C40081309F4F34F481E09FEF0A +:100890009093C4008093C3008091BB009091BC00F2 +:1008A00090938400809383008091B400813009F09C +:1008B000C3C06091B9007091BA008091810090919D +:1008C0008200681B790B882777FD8095982F0E94FE +:1008D0008E0A2AE037ED43EA5CE30E94F3090E94A6 +:1008E0005B0A7093A2006093A1008091A90090918F +:1008F000AA00892B29F41092C2001092C10094C062 +:10090000C091A900D091AA008091A1009091A2006D +:10091000C81BD90B80918B0090918C00A0918D0009 +:10092000B0918E004E01AA2497FCA094BA2C880D99 +:10093000991DAA1DBB1D80938B0090938C00A093E2 +:100940008D00B0938E006091C1007091C200409004 +:100950008B0050908C0060908D0070908E00009104 +:10096000990010919A00882777FD8095982F0E9412 +:100970008E0A6B017C01C501B4010E948E0A20E041 +:1009800030E040E251E40E941A0B4B015C01C301CC +:10099000B2010E948E0A20E030E0A9010E941A0BE9 +:1009A0009B01AC01C501B4010E948F094B015C01A0 +:1009B000BE01601B710B882777FD8095982F0E94E0 +:1009C0008E0A20E030E0A9010E941A0B9B01AC01C5 +:1009D000C501B4010E948F099B01AC01C701B6019A +:1009E0000E948F090E945B0A7093C2006093C1004D +:1009F000D0939A00C09399008091C1009091C20059 +:100A00008F3F910521F01CF08FEF90E009C080919D +:100A1000C1009091C20081309F4F34F481E09FEF7C +:100A20009093C2008093C1008091B9009091BA0068 +:100A300090938200809381008091B300813009F00F +:100A4000C3C06091B7007091B80080917F00909111 +:100A50008000681B790B882777FD8095982F0E946E +:100A60008E0A2AE037ED43EA5CE30E94F3090E9414 +:100A70005B0A7093A00060939F008091A700909103 +:100A8000A800892B29F41092C0001092BF0094C0D6 +:100A9000C091A700D091A80080919F009091A000E4 +:100AA000C81BD90B8091870090918800A091890084 +:100AB000B0918A004E01AA2497FCA094BA2C880D0C +:100AC000991DAA1DBB1D8093870090938800A09359 +:100AD0008900B0938A006091BF007091C00040907F +:100AE0008700509088006090890070908A00009183 +:100AF000970010919800882777FD8095982F0E9485 +:100B00008E0A6B017C01C501B4010E948E0A20E0AF +:100B100030E040E251E40E941A0B4B015C01C3013A +:100B2000B2010E948E0A20E030E0A9010E941A0B57 +:100B30009B01AC01C501B4010E948F094B015C010E +:100B4000BE01601B710B882777FD8095982F0E944E +:100B50008E0A20E030E0A9010E941A0B9B01AC0133 +:100B6000C501B4010E948F099B01AC01C701B60108 +:100B70000E948F090E945B0A7093C0006093BF00BF +:100B8000D0939800C09397008091BF009091C000CF +:100B90008F3F910521F01CF08FEF90E009C080910C +:100BA000BF009091C00081309F4F34F481E09FEFEF +:100BB0009093C0008093BF008091B7009091B800DF +:100BC0009093800080937F00FF91EF91DF91CF9110 +:100BD000BF91AF919F918F917F916F915F914F9155 +:100BE0003F912F911F910F91FF90EF90DF90CF9049 +:100BF000BF90AF909F908F907F906F905F904F903D +:100C00000F900FBE0F901F9018951F920F920FB666 +:100C10000F9211248F939F93EF93FF935F9B16C0C6 +:100C2000E091CB00F091CC008CB1808381E0E03387 +:100C3000F80730F43196F093CC00E093CB0006C077 +:100C40008DEC90E09093CC008093CB00FF91EF91DE +:100C50009F918F910F900FBE0F901F9018959C0140 +:100C60008AB188618AB9579A80B5866080BD40E0B4 +:100C700050E084E0220F331F441F551F8A95D1F79F +:100C800060E079E08DE390E00E947D0BDA01C9011C +:100C90000197A109B109892F9A2FAB2FBB27A7FD77 +:100CA000BA9580BD215029B98DEC90E09093CA008F +:100CB0008093C9009093CC008093CB0008955D9BF6 +:100CC000FECFFC0180818CB90895CF93DF931F92F2 +:100CD000CDB7DEB78983CE0101960E945F0680E022 +:100CE00090E00F90DF91CF910895CF93DF93EC01C7 +:100CF000CE012196FC012081222319F00E945F067B +:100D0000F7CFDF91CF9108955F9BFECF8CB190E03C +:100D100008950C94840669E876E085E696E00C94E4 +:100D2000CF0BCF93DF93FC01E253FF4FA081B18142 +:100D3000EC01C453DF4F28813981A217B307A1F01A +:100D40005D9B12C02C912CB920813181AC0145539F +:100D50005F4F2417350718F42F5F3F4F03C09C01E6 +:100D600028593F4F31832083E091C900F091CA0098 +:100D70002091CB003091CC002E173F07C1F1DC0150 +:100D8000AC59BF4FCD91DC911197208128836D9193 +:100D90007C9111979C012B523F4FE9013881EB0167 +:100DA0002881321307C09C012C523F4FE901488132 +:100DB0004F5F48839C012D593F4F6217730728F4FA +:100DC000EB012196CD93DC9302C08D939C93D1E0EF +:100DD000E033FD0730F43196F093CA00E093C90088 +:100DE00006C08DEC90E09093CA008093C900DF911B +:100DF000CF91089588E087BB8CEF84BB8CEB81BBDF +:100E00001092C70080E895E20E942F060E948B0690 +:100E100080E582B986B78B7E86BF85EC86BF81EA86 +:100E20008FBD89E08EBD1BBC1ABC19BC18BC89E6FD +:100E300085BD13BC83BF1CBE8EE690E00E944C0CA7 +:100E400085B78F7885BF84E089BF7894C0E0D8E10A +:100E50008091C8008F3F39F48091C700853A19F41A +:100E6000F894FE0109958091C5009091C6002091EB +:100E700066003091670028173907B9F18091C500E5 +:100E80009091C600892B19F485B38C6017C08091AE +:100E9000B200811107C08091C5009091C60018165C +:100EA000190654F08091B200882351F08091C5005A +:100EB0009091C60097FF04C085B3837F85BB02C0B5 +:100EC000AA98AB9A8091C5009091C6009093670054 +:100ED000809366008091C5009091C60097FF03C083 +:100EE0009195819591099BBD8ABD8091C300909198 +:100EF000C400209164003091650028173907B9F1CA +:100F00008091C3009091C400892B19F485B380634C +:100F100017C08091B100811107C08091C3009091EA +:100F2000C4001816190654F08091B100882351F0BE +:100F30008091C3009091C40097FF04C085B38F7C5B +:100F400085BB02C0AC98AD9A8091C3009091C4005B +:100F500090936500809364008091C3009091C400D9 +:100F600097FF03C091958195910999BD88BD8091A6 +:100F7000C1009091C2002091620030916300281757 +:100F80003907B1F18091C1009091C200892B19F409 +:100F900085B3806C17C08091B000811107C080912B +:100FA000C1009091C2001816190654F08091B0004B +:100FB000882351F08091C1009091C20097FF04C036 +:100FC00085B38F7385BB02C0AE98AF9A8091C10084 +:100FD0009091C20090936300809362008091C10061 +:100FE0009091C20097FF03C091958195910983BDAF +:100FF0008091BF009091C00020916000309161000D +:1010000028173907B1F18091BF009091C000892B5A +:1010100019F482B38C6017C08091AF00811107C0B2 +:101020008091BF009091C0001816190654F080916D +:10103000AF00882351F08091BF009091C00097FFCE +:1010400004C082B3837F82BB02C09298939A80913E +:10105000BF009091C00090936100809360008091E8 +:10106000BF009091C00097FF03C0919581959109B1 +:101070008CBF85B7806885BF889585B78F7785BF1A +:10108000E7CE6F927F929F92AF92BF92CF92DF9204 +:10109000EF92FF920F931F93CF93DF93CDB7DEB7FD +:1010A0002C970FB6F894DEBF0FBECDBF8C017B012D +:1010B0003A01FC0117821682838181FF19C1CE019A +:1010C00001965C01F801D380F701D3FC2591D3FE92 +:1010D00021917F01222309F407C1253249F4D3FC71 +:1010E0002591D3FE21917F01253211F0912C1BC057 +:1010F000B801822F90E00E941A0C63013601E2CF02 +:10110000222309F4F1C0622F70E087EA90E02C8777 +:101110000E94C40B2C85892B49F0F701D3FC259143 +:10112000D3FE21917F0197FEEBCF0CC0233221F437 +:10113000F92DF0619F2EF1CF2C3639F4292D20683E +:10114000922EEBCF222309F4CFC0622F70E080EA09 +:1011500090E02C870E94C40B2C85892B41F0630101 +:10116000F4E0CF0ED11CB8018FE390E012C023361B +:1011700041F0233789F0233521F5F92DF1609F2EB9 +:101180000BC0630122E0C20ED11CB801F3018081C3 +:1011900091810E941A0CB2CF630122E0C20ED11CD1 +:1011A000F30160807180F30190FC859190FE819144 +:1011B0003F01882309F4A2CFB80190E00E941A0CE5 +:1011C000F2CF243611F0293639F5630197FE09C0B4 +:1011D000F4E0CF0ED11CF301608171818281938193 +:1011E0000AC0F2E0CF0ED11CF30160817181882723 +:1011F00077FD8095982FF92DFF7E9F2E97FF09C0D0 +:1012000090958095709561957F4F8F4F9F4FF064BB +:101210009F2E2AE030E038C02037A1F040F4283576 +:10122000D1F02F3609F060C028E030E019C0253732 +:1012300019F0283751F058C0292D2F7E922E2AE020 +:1012400030E00EC0892D8061982E992D9462992EE0 +:1012500020E130E005C0E92DE4609E2E20E132E07F +:10126000630197FE09C0F4E0CF0ED11CF301608149 +:1012700071818281938108C0F2E0CF0ED11CF3010D +:101280006081718180E090E0A5010E94790C782E48 +:101290007A1896FE05C0B8018DE290E00E941A0C03 +:1012A00094FE16C0FE01E70DF11D8081803381F0B0 +:1012B000B80180E390E00E941A0C92FE09C0292D2B +:1012C0002072822F90E0B801885A9F4F0E941A0C1A +:1012D0007A94F501E70DF11D8081B80190E00E943C +:1012E0001A0C7110F5CF0ACFF8018681978102C0E0 +:1012F0008FEF9FEF2C960FB6F894DEBF0FBECDBFD9 +:10130000DF91CF911F910F91FF90EF90DF90CF90E1 +:10131000BF90AF909F907F906F9008955058BB27DB +:10132000AA270ED04DC13ED130F043D120F031F488 +:101330009F3F11F41EF433C10EF4E095E7FB29C181 +:10134000E92F4FD180F3BA17620773078407950717 +:1013500018F071F49EF567C10EF4E0950B2EBA2FCC +:10136000A02D0B01B90190010C01CA01A0011124AB +:10137000FF27591B99F0593F50F4503E68F11A1657 +:10138000F040A22F232F342F4427585FF3CF4695E8 +:1013900037952795A795F0405395C9F77EF41F160A +:1013A000BA0B620B730B840BBAF09150A1F0FF0FD4 +:1013B000BB1F661F771F881FC2F70EC0BA0F621FC0 +:1013C000731F841F48F4879577956795B795F795B0 +:1013D0009E3F08F0B3CF9395880F08F09927EE0F42 +:1013E0009795879508950CD0EBC0E3D040F0DAD004 +:1013F00030F021F45F3F19F0CCC0511115C1CFC0BE +:10140000F0D098F39923C9F35523B1F3951B550BED +:10141000BB27AA2762177307840738F09F5F5F4FC7 +:10142000220F331F441FAA1FA9F333D00E2E3AF008 +:10143000E0E830D091505040E695001CCAF729D022 +:10144000FE2F27D0660F771F881FBB1F2617370771 +:101450004807AB07B0E809F0BB0B802DBF01FF27A1 +:1014600093585F4F2AF09E3F510568F092C0DCC050 +:101470005F3FECF3983EDCF3869577956795B795DB +:10148000F7959F5FC9F7880F911D9695879597F9F6 +:101490000895E1E0660F771F881FBB1F621773076F +:1014A0008407BA0720F0621B730B840BBA0BEE1F84 +:1014B00088F7E095089504D06894B111B5C00895F7 +:1014C00098D088F09F5790F0B92F9927B751A0F086 +:1014D000D1F0660F771F881F991F1AF0BA95C9F7C8 +:1014E00012C0B13081F09FD0B1E008959CC0672F49 +:1014F000782F8827B85F39F0B93FCCF38695779578 +:101500006795B395D9F73EF4909580957095619560 +:101510007F4F8F4F9F4F0895E89409C097FB3EF48B +:1015200090958095709561957F4F8F4F9F4F992330 +:10153000A9F0F92F96E9BB279395F69587957795AE +:101540006795B795F111F8CFFAF4BB0F11F460FF6E +:101550001BC06F5F7F4F8F4F9F4F16C0882311F0C6 +:1015600096E911C0772321F09EE8872F762F05C0DA +:10157000662371F096E8862F70E060E02AF09A9575 +:10158000660F771F881FDAF7880F9695879597F96A +:10159000089597F99F6780E870E060E008959FEFF5 +:1015A00080EC089500240A941616170618060906FA +:1015B000089500240A9412161306140605060895C9 +:1015C000092E0394000C11F4882352F0BB0F40F451 +:1015D000BF2B11F460FF04C06F5F7F4F8F4F9F4F91 +:1015E000089557FD9058440F551F59F05F3F71F013 +:1015F0004795880F97FB991F61F09F3F79F087957A +:101600000895121613061406551FF2CF4695F1DF02 +:1016100008C0161617061806991FF1CF8695710592 +:10162000610508940895E894BB2766277727CB01C6 +:1016300097F908950BD0C4CFB5DF28F0BADF18F0C2 +:10164000952309F0A6CFABCF1124EECFCADFA0F3CC +:10165000959FD1F3950F50E0551F629FF001729F47 +:10166000BB27F00DB11D639FAA27F00DB11DAA1F66 +:10167000649F6627B00DA11D661F829F2227B00DB3 +:10168000A11D621F739FB00DA11D621F839FA00D3E +:10169000611D221F749F3327A00D611D231F849F8E +:1016A000600D211D822F762F6A2F11249F575040E5 +:1016B0008AF0E1F088234AF0EE0FFF1FBB1F661F80 +:1016C000771F881F91505040A9F79E3F510570F039 +:1016D00060CFAACF5F3FECF3983EDCF38695779519 +:1016E0006795B795F795E7959F5FC1F7FE2B880F34 +:1016F000911D9695879597F90895052E97FB1EF4F1 +:1017000000940E94940B57FD07D00E94A20B07FC87 +:1017100003D04EF40C94940B509540953095219540 +:101720003F4F4F4F5F4F089590958095709561950D +:101730007F4F8F4F9F4F0895EE0FFF1F0590F4913D +:10174000E02D0994A1E21A2EAA1BBB1BFD010DC0BE +:10175000AA1FBB1FEE1FFF1FA217B307E407F50761 +:1017600020F0A21BB30BE40BF50B661F771F881F3D +:10177000991F1A9469F760957095809590959B01D3 +:10178000AC01BD01CF010895FC010590061621F0C2 +:101790000020D9F7C00108953197CF0108950F9324 +:1017A0001F93CF93DF93EC018B01009731F4611508 +:1017B000710519F480E090E038C06EE070E081E0DF +:1017C00090E00E94D70CFC010097A1F380E883838E +:1017D0000115110571F01387028781E883838091D9 +:1017E000340190913501892B21F4F0933501E09378 +:1017F00034012097C9F0D187C087838182608383B9 +:101800008091360190913701892B71F4F093370163 +:10181000E09336018091380190913901892B21F4B0 +:10182000F0933901E0933801CF01DF91CF911F91FF +:101830000F9108950F931F93CF93DF93182F092FC4 +:10184000EB018B8181FD03C08FEF9FEF20C082FFF2 +:1018500010C04E815F812C813D81421753077CF47B +:10186000E881F9819F012F5F3F4F398328831083DF +:1018700006C0E885F985812F0995892B29F72E81E6 +:101880003F812F5F3F4F3F832E83812F902FDF912A +:10189000CF911F910F9108950F931F93CF93DF93D3 +:1018A000E0913601F0913701238121FF1BC0EC014B +:1018B00000E010E089916091360170913701DB0101 +:1018C0001896ED91FC911997882331F00995892B91 +:1018D00089F30FEF1FEFEECF8AE00995892B11F402 +:1018E000C80102C08FEF9FEFDF91CF911F910F9141 +:1018F0000895FA01AA27283051F1203181F1E894A6 +:101900006F936E7F6E5F7F4F8F4F9F4FAF4FB1E0F2 +:101910003ED0B4E03CD0670F781F891F9A1FA11DED +:10192000680F791F8A1F911DA11D6A0F711D811DEE +:10193000911DA11D20D009F468943F912AE0269FB3 +:1019400011243019305D3193DEF6CF010895462F12 +:101950004770405D4193B3E00FD0C9F7F6CF462FF3 +:101960004F70405D4A3318F0495D31FD405241935C +:1019700002D0A9F7EACFB4E0A69597958795779519 +:101980006795BA95C9F700976105710508959B01A0 +:10199000AC010A2E06945795479537952795BA9529 +:1019A000C9F7620F731F841F951FA01D08950F9321 +:1019B0001F93CF93DF93869F8001879F100D969F83 +:1019C000100D1124C8010E94F30CEC01009729F0BE +:1019D000A80160E070E00E941A0ECE01DF91CF9165 +:1019E0001F910F910895CF93DF938230910510F4EA +:1019F00082E090E0E0913C01F0913D0120E030E098 +:101A0000A0E0B0E0309739F1408151814817590783 +:101A1000B8F04817590771F482819381109729F023 +:101A200013969C938E9312972CC090933D018093B4 +:101A30003C0127C02115310531F04217530718F03A +:101A4000A901DB0101C0EF019A01BD01DF010280A4 +:101A5000F381E02DD7CF21153105F9F0281B390B83 +:101A60002430310580F48A819B816115710521F054 +:101A7000FB019383828304C090933D0180933C01DA +:101A8000FE01329644C0FE01E20FF31F8193919351 +:101A900022503109398328833AC020913A0130918C +:101AA0003B01232B41F420916A0030916B0030936D +:101AB0003B0120933A012091680030916900211583 +:101AC000310541F42DB73EB740916C0050916D0047 +:101AD000241B350BE0913A01F0913B01E217F3072B +:101AE000A0F42E1B3F0B2817390778F0AC014E5F8E +:101AF0005F4F2417350748F04E0F5F1F50933B018F +:101B000040933A018193919302C0E0E0F0E0CF016D +:101B1000DF91CF910895CF93DF93009709F487C0A9 +:101B2000FC01329713821282C0913C01D0913D0199 +:101B3000209781F420813181280F391F80913A014B +:101B400090913B018217930779F5F0933B01E09365 +:101B50003A016DC0DE0120E030E0AE17BF0750F45F +:101B600012964D915C9113979D014115510509F114 +:101B7000DA01F3CFB383A28340815181840F951F93 +:101B80008A179B0771F48D919C911197840F951F73 +:101B900002969183808312968D919C9113979383E3 +:101BA00082832115310529F4F0933D01E0933C0136 +:101BB0003EC0D9011396FC93EE9312974D915D911F +:101BC000A40FB51FEA17FB0779F480819181840F78 +:101BD000951F0296D90111969C938E9382819381D1 +:101BE00013969C938E931297E0E0F0E08A819B819C +:101BF000009719F0FE01EC01F9CFCE010296288181 +:101C00003981820F931F20913A0130913B012817AF +:101C1000390769F4309729F410923D0110923C0184 +:101C200002C013821282D0933B01C0933A01DF912C +:101C3000CF910895DC0101C06D9341505040E0F711 +:061C40000895F894FFCFA7 +:101C4600008000800080008000003E0120000D0A18 +:081C560053746172740D00006B +:00000001FF diff --git a/avr/motor_ctrl/ringbuffer.h b/avr/motor_ctrl/ringbuffer.h new file mode 100644 index 0000000..ed8b022 --- /dev/null +++ b/avr/motor_ctrl/ringbuffer.h @@ -0,0 +1,34 @@ +#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 + diff --git a/avr/motor_ctrl/uart.c b/avr/motor_ctrl/uart.c new file mode 100644 index 0000000..24de2f2 --- /dev/null +++ b/avr/motor_ctrl/uart.c @@ -0,0 +1,97 @@ +#include +#include "uart.h" +#include + +#define UART_UBRR_CALC(BAUD_,FREQ_) ((FREQ_)/((BAUD_)*16L)-1) + +static volatile char read[RINGBUFFER_LEN]; +static volatile char *in_read_ptr; +static volatile char *out_read_ptr; + +ISR(USART_RXC_vect) { + // update read from uart + if (UCSRA & (1<> 8); + UBRRL = (uint8_t)UART_UBRR_CALC(rate, F_CPU); + + in_read_ptr = out_read_ptr = read; +} + + +void uart_putc(char *c) { + while (!(UCSRA & (1<out_write_ptr != buffer->in_write_ptr) { + if (UCSRA & (1<out_write_ptr; + if (buffer->out_write_ptr < buffer->write + RINGBUFFER_LEN - 1) buffer->out_write_ptr++; + else buffer->out_write_ptr = buffer->write; + } + } + + // update read from uart + if (in_read_ptr != out_read_ptr) { + *buffer->in_read_ptr = *out_read_ptr; + + // update newline chars + if (buffer->block_read == *buffer->in_read_ptr) buffer->newlines++; + + if (buffer->in_read_ptr < buffer->read + RINGBUFFER_LEN - 1) buffer->in_read_ptr++; + else buffer->in_read_ptr = buffer->read; + + // move pointer + if (out_read_ptr < read + RINGBUFFER_LEN - 1) out_read_ptr++; + else out_read_ptr = read; + } +} + diff --git a/avr/motor_ctrl/uart.h b/avr/motor_ctrl/uart.h new file mode 100644 index 0000000..1225590 --- /dev/null +++ b/avr/motor_ctrl/uart.h @@ -0,0 +1,18 @@ +#ifndef UART_H +#define UART_H + +#include +#include "ringbuffer.h" + +void setup_uart(unsigned int rate); +void uart_putc(char *c); +void uart_puts(char *s); +int uart_getc(void); +void uart_puti(int i); +int uart_putchar(char c, FILE *stream); +int uart_getchar(FILE *stream); +void uart_setup_stdout(void); +void uart_stream_update(ringbuffer_t *buffer); + +#endif + -- 2.39.2