From c9f14499885b3c087aa5b107156cc91b48917a7c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 21 Mar 2013 00:35:55 +0100 Subject: digital/io-hub/src/apbirthday: add first I2C slave --- digital/io-hub/src/apbirthday/Makefile | 7 ++++++- digital/io-hub/src/apbirthday/hardware.hh | 3 +++ digital/io-hub/src/apbirthday/hardware.host.cc | 1 + digital/io-hub/src/apbirthday/hardware.stm32.cc | 9 +++++++++ digital/io-hub/src/apbirthday/robot.cc | 21 ++++++++++++++++++++- digital/io-hub/src/apbirthday/robot.hh | 10 ++++++++++ 6 files changed, 49 insertions(+), 2 deletions(-) (limited to 'digital/io-hub/src/apbirthday') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 1e83e9a9..89458a08 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -3,11 +3,12 @@ BASE = ../../../ucoolib TARGETS = host stm32f4 PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ + i2c_queue.cc asserv.cc \ top.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c \ $(AVR_SOURCES) -MODULES = hal/gpio hal/uart hal/usb base/proto +MODULES = utils hal/gpio hal/uart hal/usb hal/i2c base/proto # Hack mode, include old code from AVR modules. AVR_MODULES = math/geometry math/fixed @@ -22,6 +23,10 @@ AVR_SOURCES = $(foreach module,$(AVR_MODULES),$($(subst /,_,$(module))_SOURCES)) INCLUDES += -I$(BASE)/../ai/src/fsm -I. vpath %.c $(BASE)/../ai/src/fsm +# Common files. +INCLUDES += -I../common-cc +vpath %.cc ../common-cc + include $(BASE)/build/top.mk clean: clean.project diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index 148d0727..6660156e 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -26,6 +26,7 @@ #include "ucoolib/hal/uart/uart.hh" #include "ucoolib/hal/gpio/gpio.hh" +#include "ucoolib/hal/i2c/i2c.hh" #ifdef TARGET_stm32 # include "ucoolib/hal/usb/usb.hh" #else @@ -53,8 +54,10 @@ struct Hardware #ifdef TARGET_stm32 ucoo::UsbStreamControl usb_control; ucoo::UsbStream usb; + ucoo::I2cHard main_i2c; #else ucoo::HostStream usb; + ucoo::I2cHost main_i2c; #endif ucoo::Gpio raw_jack, diff --git a/digital/io-hub/src/apbirthday/hardware.host.cc b/digital/io-hub/src/apbirthday/hardware.host.cc index a4210480..d2deecad 100644 --- a/digital/io-hub/src/apbirthday/hardware.host.cc +++ b/digital/io-hub/src/apbirthday/hardware.host.cc @@ -31,6 +31,7 @@ HardwareHost::HardwareHost () Hardware::Hardware () : dev_uart (), zb_uart ("zb_uart"), usb ("usb"), + main_i2c (host, 0), raw_jack (host, "raw_jack"), ihm_color (host, "ihm_color"), ihm_strat (host, "ihm_strat"), diff --git a/digital/io-hub/src/apbirthday/hardware.stm32.cc b/digital/io-hub/src/apbirthday/hardware.stm32.cc index be83905c..53420a45 100644 --- a/digital/io-hub/src/apbirthday/hardware.stm32.cc +++ b/digital/io-hub/src/apbirthday/hardware.stm32.cc @@ -30,6 +30,7 @@ Hardware::Hardware () : dev_uart (4), zb_uart (2), usb_control ("APBTeam", "APBirthday"), usb (usb_control, 0), + main_i2c (2), raw_jack (GPIOD, 12), ihm_color (GPIOD, 14), ihm_strat (GPIOD, 13), @@ -71,6 +72,14 @@ Hardware::Hardware () zb_uart.block (false); // usb usb.block (false); + // main_i2c + gpio_mode_setup (GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8); + gpio_mode_setup (GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9); + gpio_set_output_options (GPIOA, GPIO_OTYPE_OD, GPIO_OSPEED_2MHZ, GPIO8); + gpio_set_output_options (GPIOC, GPIO_OTYPE_OD, GPIO_OSPEED_2MHZ, GPIO9); + gpio_set_af (GPIOA, GPIO_AF4, GPIO8); + gpio_set_af (GPIOC, GPIO_AF4, GPIO9); + main_i2c.enable (); // Cycle timer, 4 ms period. rcc_peripheral_enable_clock (&RCC_APB1ENR, RCC_APB1ENR_TIM3EN); TIM3_CR1 = TIM_CR1_CEN; diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index a2fcfb89..4464013e 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -24,11 +24,14 @@ #include "robot.hh" #include "ucoolib/arch/arch.hh" +#include "ucoolib/utils/bytes.hh" Robot *robot; Robot::Robot () - : dev_proto (*this, hardware.dev_uart), + : main_i2c_queue_ (hardware.main_i2c), + asserv (main_i2c_queue_, scale), + dev_proto (*this, hardware.dev_uart), zb_proto (*this, hardware.zb_uart), usb_proto (*this, hardware.usb) { @@ -42,6 +45,8 @@ Robot::main_loop () { // Wait until next cycle. hardware.wait (); + // Handle communications. + main_i2c_queue_.sync (); // Handle commands. dev_proto.accept (); zb_proto.accept (); @@ -66,6 +71,20 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size // Reset. ucoo::arch_reset (); break; + case c ('m', 5): + // Go to position. + // 2H: x, y. + // 1B: direction_consign. + { + vect_t pos = { + (int16_t) ucoo::bytes_pack (args[0], args[1]), + (int16_t) ucoo::bytes_pack (args[2], args[3]), + }; + asserv.stop (); + // TODO: use move FSM. + asserv.goto_xy (pos, Asserv::DirectionConsign (args[4])); + } + break; default: proto.send ('?'); return; diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index d65b2a3a..e0b50f41 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -24,12 +24,16 @@ // // }}} #include "hardware.hh" +#include "asserv.hh" #include "ucoolib/base/proto/proto.hh" /// Main robot superclass. class Robot : public ucoo::Proto::Handler { + public: + /// Scaling factor, millimeter per step. + static const float scale = 0.0395840674352314; public: /// Initialise robot singleton. Robot (); @@ -42,6 +46,12 @@ class Robot : public ucoo::Proto::Handler public: /// Public access to hardware class. Hardware hardware; + private: + /// Main I2C queue. + I2cQueue main_i2c_queue_; + public: + /// Public access to asserv class. + Asserv asserv; private: /// Proto associated to each serial interface. ucoo::Proto dev_proto, zb_proto, usb_proto; -- cgit v1.2.3