Add the uC sources

.gitmodules

+[submodule "src/libopencm3"]
+	path = src/libopencm3
+	url = https://github.com/libopencm3/libopencm3.git

src/.gitignore

+*.elf
+*.o
+*.hex
+*.map
+*.d

src/Makefile

+##
+## This file is part of the libopencm3 project.
+##
+## Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>
+##
+## This library is free software: you can redistribute it and/or modify
+## it under the terms of the GNU Lesser General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This library is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU Lesser General Public License for more details.
+##
+## You should have received a copy of the GNU Lesser General Public License
+## along with this library.  If not, see <http://www.gnu.org/licenses/>.
+##
+
+BINARY = robo
+
+LDSCRIPT = robo.ld
+
+include libopencm3.target.mk

src/i2c.c

+#include <libopencm3/stm32/rcc.h>
+#include <libopencm3/stm32/i2c.h>
+
+void i2c_reset(uint32_t i2c)
+{
+	switch (i2c) {
+	case I2C1:
+		rcc_periph_reset_pulse(RST_I2C1);
+		break;
+#if defined(I2C2_BASE)
+	case I2C2:
+		rcc_periph_reset_pulse(RST_I2C2);
+		break;
+#endif
+#if defined(I2C3_BASE)
+	case I2C3:
+		rcc_periph_reset_pulse(RST_I2C3);
+		break;
+#endif
+	default:
+		break;
+	}
+}
+
+void i2c_peripheral_enable(uint32_t i2c)
+{
+	I2C_CR1(i2c) |= I2C_CR1_PE;
+}
+void i2c_peripheral_disable(uint32_t i2c)
+{
+	I2C_CR1(i2c) &= ~I2C_CR1_PE;
+}
+
+void i2c_send_start(uint32_t i2c)
+{
+	I2C_CR2(i2c) |= I2C_CR2_START;
+}
+void i2c_send_stop(uint32_t i2c)
+{
+	I2C_CR2(i2c) |= I2C_CR2_STOP;
+}
+void i2c_clear_stop(uint32_t i2c)
+{
+	I2C_CR2(i2c) &= ~I2C_CR2_STOP;
+}
+
+void i2c_set_slave_address(uint32_t i2c, uint8_t slave)
+{
+        uint32_t reg32 = I2C_CR2(i2c) & ~I2C_CR2_SADD;
+        reg32 |= I2C_CR2_SADD_VAL(slave);
+        I2C_CR2(i2c) = reg32;
+}
+
+void i2c_enable_stretch(uint32_t i2c)
+{
+        I2C_CR1(i2c) |= I2C_CR1_NOSTRETCH;
+}
+void i2c_disable_stretch(uint32_t i2c)
+{
+        I2C_CR1(i2c) &= ~I2C_CR1_NOSTRETCH;
+}
+
+void i2c_set_standard_mode(uint32_t i2c)
+{
+        uint32_t reg32 = I2C_TIMINGR(i2c);
+
+        reg32 &= ~I2C_TIMINGR_PRESC;
+
+        reg32 &= ~I2C_TIMINGR_SCLL;
+        reg32 &= ~I2C_TIMINGR_SCLH;
+
+        reg32 &= ~I2C_TIMINGR_SDADEL;
+        reg32 &= ~I2C_TIMINGR_SCLDEL;
+
+        /* Page 642, RM0091, assuming 8Mhz HSI clock source */
+
+        /* TODO: not correct when SYSCLK is 8MHz, no easy way to get SYSCLK
+         * clock though, only ahb apb are kept track of in rcc */
+        assert(~RCC_CFGR3 & RCC_CFGR3_I2C1SW); /* 0-> 8MHz (def), 1 -> SCLK */
+
+        reg32 |= I2C_TIMINGR_PRESC_VAL(1)
+              |  I2C_TIMINGR_SCLL_VAL(0xc7)
+              |  I2C_TIMINGR_SCLH_VAL(0xc3)
+              |  I2C_TIMINGR_SDADEL_VAL(0x2)
+              |  I2C_TIMINGR_SCLDEL_VAL(0x4);
+}
+
+I2C_CR2_RD_WRN
+
+void i2c_begin_transfer(uint32_t i2c, uint8_t nbytes, bool autoend, bool readwrite)
+{
+        uint32_t reg32 = I2C_CR2(i2c);
+
+        reg32 &= ~I2C_CR2_NBYTES;
+        reg32 &= ~I2C_CR2_AUTOEND;
+        reg32 &= ~I2C_CR2_RD_WRN;
+
+        reg32 |= I2C_CR2_NBYTES_VAL(nbytes);
+        reg32 |= autoend ? I2C_CR2_AUTOEND : 0;
+        reg32 |= readwrite ? I2C_CR2_RD_WRN : 0;
+
+        I2C_CR2(i2c) = reg32;
+
+        /* TODO: NBYTES must be written 0xff initially?? */
+}
+
+void i2c_send_start(void)
+{
+
+}
+
+bool i2c_send_blocking(uint32_t i2c, uint8_t byte)
+{
+        assert(~I2C_ISR(i2c) & I2C_ISR_NACKF);
+
+        /* wait until tx empty */
+        while (~I2C_ISR(i2c) & I2C_ISR_TXE)
+        {
+                I2C_TXDR(i2c) = byte;
+                I2C2->CR2 |= I2C_CR2_START; /* Go */
+        }
+}
+
+void i2c_init(void)
+{
+        uint8_t addr = 0;
+        rcc_periph_clock_enable(RCC_I2C1);
+
+	i2c_reset(I2C1);
+
+	gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO10 | GPIO11);
+	gpio_set_af(GPIOB, GPIO_AF1, GPIO10 | GPIO11);
+
+	i2c_peripheral_disable(I2C1);
+}

libopencm3 @ 6b5150a4

+Subproject commit 6b5150a4dc54c4f5b2f8a98317ec963ea7fbc864

src/libopencm3.rules.mk

+##
+## This file is part of the libopencm3 project.
+##
+## Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>
+## Copyright (C) 2010 Piotr Esden-Tempski <piotr@esden.net>
+## Copyright (C) 2013 Frantisek Burian <BuFran@seznam.cz>
+##
+## This library is free software: you can redistribute it and/or modify
+## it under the terms of the GNU Lesser General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This library is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU Lesser General Public License for more details.
+##
+## You should have received a copy of the GNU Lesser General Public License
+## along with this library.  If not, see <http://www.gnu.org/licenses/>.
+##
+
+# Be silent per default, but 'make V=1' will show all compiler calls.
+ifneq ($(V),1)
+Q		:= @
+NULL		:= 2>/dev/null
+endif
+
+###############################################################################
+# Executables
+
+PREFIX		?= arm-none-eabi
+
+CC		:= $(PREFIX)-gcc
+CXX		:= $(PREFIX)-g++
+LD		:= $(PREFIX)-gcc
+AR		:= $(PREFIX)-ar
+AS		:= $(PREFIX)-as
+OBJCOPY		:= $(PREFIX)-objcopy
+OBJDUMP		:= $(PREFIX)-objdump
+GDB		:= $(PREFIX)-gdb
+STFLASH		= $(shell which st-flash)
+
+
+###############################################################################
+# Source files
+
+LDSCRIPT	?= $(BINARY).ld
+
+OBJS		+= $(BINARY).o
+
+OPENCM3_DIR     = ./libopencm3
+INCLUDE_DIR	= $(OPENCM3_DIR)/include
+LIB_DIR		= $(OPENCM3_DIR)/lib
+SCRIPT_DIR	= $(OPENCM3_DIR)/scripts
+
+###############################################################################
+# C flags
+
+CFLAGS		+= -Os -g
+CFLAGS		+= -Wextra -Wshadow -Wimplicit-function-declaration
+CFLAGS		+= -Wredundant-decls -Wstrict-prototypes # -Wmissing-prototypes
+CFLAGS		+= -fno-common -ffunction-sections -fdata-sections
+CFLAGS          += -std=gnu99
+
+###############################################################################
+# C++ flags
+
+CXXFLAGS	+= -Os -g
+CXXFLAGS	+= -Wextra -Wshadow -Wredundant-decls  -Weffc++
+CXXFLAGS	+= -fno-common -ffunction-sections -fdata-sections
+
+###############################################################################
+# C & C++ preprocessor common flags
+
+CPPFLAGS	+= -MD
+CPPFLAGS	+= -Wall -Wundef
+CPPFLAGS	+= -I$(INCLUDE_DIR) $(DEFS)
+
+###############################################################################
+# Linker flags
+
+LDFLAGS		+= --static -nostartfiles
+LDFLAGS		+= -L$(LIB_DIR)
+LDFLAGS		+= -T$(LDSCRIPT)
+LDFLAGS		+= -Wl,-Map=$(*).map
+LDFLAGS		+= -Wl,--gc-sections
+ifeq ($(V),99)
+LDFLAGS		+= -Wl,--print-gc-sections
+endif
+
+###############################################################################
+# Used libraries
+
+LDLIBS		+= -l$(LIBNAME)
+LDLIBS		+= -Wl,--start-group -lc -lgcc -lnosys -Wl,--end-group
+
+###############################################################################
+###############################################################################
+###############################################################################
+
+.SUFFIXES: .elf .bin .hex .srec .list .map .images
+.SECONDEXPANSION:
+.SECONDARY:
+
+all: elf
+
+elf: $(BINARY).elf
+bin: $(BINARY).bin
+hex: $(BINARY).hex
+srec: $(BINARY).srec
+list: $(BINARY).list
+
+images: $(BINARY).images
+flash: $(BINARY).flash
+
+%.images: %.bin %.hex %.srec %.list %.map
+	@#printf "*** $* images generated ***\n"
+
+%.bin: %.elf
+	@#printf "  OBJCOPY $(*).bin\n"
+	$(Q)$(OBJCOPY) -Obinary $(*).elf $(*).bin
+
+%.hex: %.elf
+	@#printf "  OBJCOPY $(*).hex\n"
+	$(Q)$(OBJCOPY) -Oihex $(*).elf $(*).hex
+
+%.srec: %.elf
+	@#printf "  OBJCOPY $(*).srec\n"
+	$(Q)$(OBJCOPY) -Osrec $(*).elf $(*).srec
+
+%.list: %.elf
+	@#printf "  OBJDUMP $(*).list\n"
+	$(Q)$(OBJDUMP) -S $(*).elf > $(*).list
+
+%.elf %.map: $(OBJS) $(LDSCRIPT) $(LIB_DIR)/lib$(LIBNAME).a
+	@#printf "  LD      $(*).elf\n"
+	$(Q)$(LD) $(LDFLAGS) $(ARCH_FLAGS) $(OBJS) $(LDLIBS) -o $(*).elf
+
+%.o: %.c
+	@#printf "  CC      $(*).c\n"
+	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -o $(*).o -c $(*).c
+
+%.o: %.cxx
+	@#printf "  CXX     $(*).cxx\n"
+	$(Q)$(CXX) $(CXXFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -o $(*).o -c $(*).cxx
+
+%.o: %.cpp
+	@#printf "  CXX     $(*).cpp\n"
+	$(Q)$(CXX) $(CXXFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -o $(*).o -c $(*).cpp
+
+clean:
+	@#printf "  CLEAN\n"
+	$(Q)$(RM) *.o *.d *.elf *.bin *.hex *.srec *.list *.map
+
+%.stlink-flash: %.bin
+	@printf "  FLASH  $<\n"
+	$(Q)$(STFLASH) write $(*).bin 0x8000000
+
+%.flash: %.hex
+	@printf "  FLASH   $<\n"
+# IMPORTANT: Dont use "resume", only "reset" will work correctly!
+	$(Q)$(OOCD) -f interface/$(OOCD_INTERFACE).cfg \
+		    -f board/$(OOCD_BOARD).cfg \
+		    -c "init" -c "reset init" \
+		    -c "flash write_image erase $(*).hex" \
+		    -c "reset" \
+		    -c "shutdown" $(NULL)
+
+.PHONY: images clean stylecheck styleclean elf bin hex srec list
+
+-include $(OBJS:.o=.d)

src/libopencm3.target.mk

+##
+## This file is part of the libopencm3 project.
+##
+## Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>
+## Copyright (C) 2010 Piotr Esden-Tempski <piotr@esden.net>
+##
+## This library is free software: you can redistribute it and/or modify
+## it under the terms of the GNU Lesser General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This library is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU Lesser General Public License for more details.
+##
+## You should have received a copy of the GNU Lesser General Public License
+## along with this library.  If not, see <http://www.gnu.org/licenses/>.
+##
+
+LIBNAME		= opencm3_stm32f0
+DEFS		+= -DSTM32F0
+
+FP_FLAGS	?= -msoft-float
+ARCH_FLAGS	= -mthumb -mcpu=cortex-m0 $(FP_FLAGS)
+
+################################################################################
+# OpenOCD specific variables
+
+OOCD		?= openocd
+OOCD_INTERFACE	?= stlink-v2
+OOCD_BOARD	?= stm32f0discovery
+
+include libopencm3.rules.mk

src/pid.c

+struct pid_parameters {
+        float kP, kI, kD;
+} pid_params;
+
+struct pid_state {
+        unsigned int psT;
+        float psE, psI;
+} pid_state;
+
+void pid_iteration(float set,
+                   float current,
+                   unsigned int t)
+{
+        unsigned int dt = t - pid_state.psT;
+        float e = set - current;
+        float i = pid_state.psI + e * dt;
+        float d = (e - pid_state.psE) / dt;
+        float o = pid_params.kP * e + pid_params.kI * i + pid_params.kD * d;
+        pid_state.psT = t;
+        pid_state.psE = e;
+        pid_state.psI = i;
+
+
+}
+
+int main()
+{
+        pid_iteration(1.123, 10, 100);
+}

src/robo.c

+#include <libopencm3/stm32/rcc.h>
+#include <libopencm3/stm32/gpio.h>
+#include <libopencm3/stm32/usart.h>
+#include <libopencm3/stm32/adc.h>
+#include <libopencm3/stm32/timer.h>
+#include <libopencm3/stm32/syscfg.h>
+#include <libopencm3/stm32/exti.h>
+#include <libopencm3/cm3/nvic.h>
+#include <libopencm3/cm3/systick.h>
+
+
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+
+typedef int16_t mc_t;
+
+void mc_init(void);
+void mc_pwm_set(mc_t drive[2]);
+void me_init(void);
+void sens_init(void);
+
+/* L -> M2, ME1
+ * R -> M1, ME0
+ */
+
+#define MC_L_TIM TIM1
+#define MC_R_TIM TIM3
+
+#define MC_L_RCC_TIM RCC_TIM1
+#define MC_R_RCC_TIM RCC_TIM3
+
+#define MC_R_OCA TIM_OC3
+#define MC_R_OCB TIM_OC4
+
+#define MC_L_RCC_GPIO RCC_GPIOA
+#define MC_R_RCC_GPIO RCC_GPIOB
+
+#define MC_L_GPIO GPIOA
+#define MC_R_GPIO GPIOB
+
+#define MC_L_OC_AF GPIO_AF1
+#define MC_R_OC_AF GPIO_AF1
+
+#define MC_L_GPIO_PINS GPIO8 | GPIO9
+#define MC_R_GPIO_PINS GPIO0 | GPIO1
+
+/* Motor encoder */
+#define ME_RCC RCC_GPIOB
+#define ME_PORT GPIOB
+
+#define ME0A GPIO4
+#define ME0B GPIO5
+
+#define ME1A GPIO5 // PORTA !!
+#define ME1B GPIO3
+
+#define MC_L_MEA ME1A
+#define MC_L_MEB ME1B
+#define MC_R_MEA ME0A
+#define MC_R_MEB ME0B
+
+#define SENS_RCC_GPIO RCC_GPIOA
+#define SENS_GPIO GPIOA
+#define SENS_GPIO_PINS (GPIO0 | GPIO1 | GPIO2 | GPIO3 | GPIO5)
+
+const uint16_t MC_PER = 0x100;
+
+void mc_init(void)
+{
+        rcc_periph_clock_enable(MC_L_RCC_TIM);
+        rcc_periph_clock_enable(MC_R_RCC_TIM);
+        rcc_periph_clock_enable(MC_L_RCC_GPIO);
+        rcc_periph_clock_enable(MC_R_RCC_GPIO);
+
+        gpio_mode_setup(
+                MC_R_GPIO,
+                GPIO_MODE_AF,
+                GPIO_PUPD_PULLDOWN,
+                MC_R_GPIO_PINS);
+
+        gpio_set_af(MC_R_GPIO, MC_R_OC_AF, MC_R_GPIO_PINS);
+
+        gpio_set_output_options(
+                MC_R_GPIO,
+                GPIO_OTYPE_PP,
+                GPIO_OSPEED_25MHZ,
+                MC_R_GPIO_PINS);
+
+	rcc_periph_clock_enable(MC_R_RCC_TIM); // not sure if this is necessary
+        timer_reset(MC_R_TIM);
+
+        timer_set_mode(
+                MC_R_TIM,
+                TIM_CR1_CKD_CK_INT,
+                TIM_CR1_CMS_CENTER_1,
+                TIM_CR1_DIR_UP);
+
+        timer_set_prescaler(MC_R_TIM, 0);
+
+        timer_set_oc_mode(MC_R_TIM, MC_R_OCA, TIM_OCM_PWM2);
+        timer_set_oc_mode(MC_R_TIM, MC_R_OCB, TIM_OCM_PWM2);
+
+        timer_enable_oc_output(MC_R_TIM, MC_R_OCA);
+        timer_enable_oc_output(MC_R_TIM, MC_R_OCB);
+
+        timer_enable_break_main_output(MC_R_TIM);
+
+        timer_set_period(MC_R_TIM, MC_PER);
+        timer_set_oc_value(MC_R_TIM, MC_R_OCA, MC_PER / 2);
+        timer_set_oc_value(MC_R_TIM, MC_R_OCB, MC_PER / 2);
+        timer_set_oc_polarity_low(MC_R_TIM, MC_R_OCA);
+        timer_set_oc_polarity_high(MC_R_TIM, MC_R_OCB);
+
+        timer_enable_counter(MC_R_TIM);
+}
+
+void mc_pwm_set(mc_t drive[2])
+{
+        int32_t drv1 = (int32_t)MC_PER/2 + (drive[1] * (int32_t)MC_PER/2) / (0xffff/2);
+
+        // todo: figure out forwards backwards
+        timer_set_oc_value(MC_R_TIM, MC_R_OCA, drv1);
+        timer_set_oc_value(MC_R_TIM, MC_R_OCB, drv1);
+}
+
+volatile int8_t MC_encDelta[2] = {0};
+
+/* untested */
+void me_irq(void)
+{
+        usart_send_blocking(USART1, 'Q');
+        return;
+        static int8_t last[2];
+
+        /* stolen from iRobot */
+        const uint16_t pins[][2] = {
+                { MC_L_MEA, MC_L_MEB },
+                { MC_R_MEA, MC_R_MEB },
+        };
+
+        for(int i=0; i < 2; i++) {
+                int8_t new, diff;
+
+                new = 0;
+                if(gpio_get(ME_PORT, pins[i][0])) /*GET(MC_S0A)*/
+                        new = 3;
+                if(gpio_get(ME_PORT, pins[i][1])) /*GET(MC_S0B)*/
+                        new ^= 1;
+                diff = last[i] - new;           		// something changed?
+                if( diff & 1 )
+                {
+                        last[i] = new; // store new as next last
+                        MC_encDelta[i] += (diff & 2) - 1;
+                }
+        }
+}
+
+volatile unsigned int cnt = 0;
+void exti4_15_isr()
+{
+        exti_reset_request(EXTI4);
+        exti_reset_request(EXTI5);
+	/* exti_set_trigger(EXTI4, EXTI_TRIGGER_BOTH); */
+
+        cnt += 100;
+}
+
+/* untested */
+void me_init()
+{
+        rcc_periph_clock_enable(RCC_SYSCFG_COMP);
+        rcc_periph_clock_enable(RCC_GPIOB);
+
+	gpio_mode_setup(ME_PORT,
+                        GPIO_MODE_INPUT,
+                        GPIO_PUPD_NONE,
+                        /* MC_L_MEA | MC_L_MEB |  */MC_R_MEA | MC_R_MEB);
+
+	exti_enable_request(EXTI4);
+	exti_enable_request(EXTI5);
+
+	exti_set_trigger(EXTI3, EXTI_TRIGGER_RISING);
+	exti_set_trigger(EXTI4, EXTI_TRIGGER_RISING);
+
+
+        // PB 3,4  10,11
+        // ME B,A   A,B
+	exti_select_source(EXTI4, ME_PORT);
+	exti_select_source(EXTI5, ME_PORT);
+
+        exti_reset_request(EXTI4);
+        exti_reset_request(EXTI5);
+
+        nvic_enable_irq(NVIC_EXTI4_15_IRQ);
+        nvic_set_priority(NVIC_EXTI4_15_IRQ, 0);
+}
+
+
+void sens_init(void)
+{
+	rcc_periph_clock_enable(RCC_ADC);
+	rcc_periph_clock_enable(SENS_RCC_GPIO);
+
+	gpio_mode_setup(SENS_GPIO,
+                        GPIO_MODE_ANALOG,
+                        GPIO_PUPD_NONE,
+                        SENS_GPIO_PINS);
+
+	adc_power_off(ADC);
+
+	adc_set_clk_source(ADC, ADC_CLKSOURCE_ADC);
+	adc_calibrate_start(ADC);
+	adc_calibrate_wait_finish(ADC);
+	adc_set_operation_mode(ADC, ADC_MODE_SEQUENTIAL);
+	adc_disable_external_trigger_regular(ADC);
+	adc_set_right_aligned(ADC);
+
+	adc_set_sample_time_on_all_channels(ADC, ADC_SMPTIME_071DOT5);
+
+        uint8_t channel_array[] = {
+                ADC_CHANNEL0,
+                ADC_CHANNEL1,
+                ADC_CHANNEL2,
+                ADC_CHANNEL3,
+                ADC_CHANNEL5
+        };
+	adc_set_regular_sequence(ADC, 1, channel_array);