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 ++ digital/io-hub/src/common-cc/asserv.cc | 193 ++++++++++++++++++++++++ digital/io-hub/src/common-cc/asserv.hh | 124 +++++++++++++++ digital/io-hub/src/common-cc/motor.hh | 40 +++++ 9 files changed, 406 insertions(+), 2 deletions(-) create mode 100644 digital/io-hub/src/common-cc/asserv.cc create mode 100644 digital/io-hub/src/common-cc/asserv.hh create mode 100644 digital/io-hub/src/common-cc/motor.hh (limited to 'digital/io-hub') 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; diff --git a/digital/io-hub/src/common-cc/asserv.cc b/digital/io-hub/src/common-cc/asserv.cc new file mode 100644 index 00000000..c216678f --- /dev/null +++ b/digital/io-hub/src/common-cc/asserv.cc @@ -0,0 +1,193 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "asserv.hh" + +#include "ucoolib/utils/bytes.hh" + +Asserv::Asserv (I2cQueue &queue, float scale) + : I2cQueue::Slave (queue, 0x04, 10 + aux_nb * 2), + status_flag_ (0), + input_port_ (0), + position_x_ (0), position_y_ (0), position_a_ (0), + last_moving_direction_ (DIRECTION_NONE), + scale_ (scale), scale_inv_ (1 / scale) +{ +} + +void +Asserv::recv_status (const uint8_t *status) +{ + status_flag_ = status[0]; + input_port_ = status[1]; + position_x_ = ucoo::bytes_pack (0, status[2], status[3], status[4]); + position_y_ = ucoo::bytes_pack (0, status[5], status[6], status[7]); + position_a_ = ucoo::bytes_pack (status[8], status[9]); + Direction d = get_moving_direction (); + if (d != DIRECTION_NONE) + last_moving_direction_ = d; +} + +void +Asserv::get_position (Position &position) const +{ + position.v.x = position_x_ * scale_; + position.v.y = position_y_ * scale_; + position.a = position_a_; +} + +void +Asserv::free () +{ + uint8_t buf[] = { 'w' }; + send (buf, sizeof (buf)); +} + +void +Asserv::stop () +{ + uint8_t buf[] = { 's' }; + send (buf, sizeof (buf)); +} + +void +Asserv::move_distance (int distance) +{ + distance *= scale_inv_; + uint8_t buf[] = { 'l', + ucoo::bytes_unpack (distance, 2), + ucoo::bytes_unpack (distance, 1), + ucoo::bytes_unpack (distance, 0), + }; + send (buf, sizeof (buf)); +} + +void +Asserv::move_angle (int16_t angle) +{ + uint8_t buf[] = { 'a', + ucoo::bytes_unpack (angle, 1), + ucoo::bytes_unpack (angle, 0), + }; + send (buf, sizeof (buf)); +} + +void +Asserv::goto_angle (uint16_t angle) +{ + uint8_t buf[] = { 'y', + ucoo::bytes_unpack (angle, 1), + ucoo::bytes_unpack (angle, 0), + }; + send (buf, sizeof (buf)); +} + +void +Asserv::goto_xya (const Position &pos, + DirectionConsign direction_consign) +{ + int x = pos.v.x * scale_inv_; + int y = pos.v.y * scale_inv_; + uint8_t buf[] = { 'X', + ucoo::bytes_unpack (x, 2), + ucoo::bytes_unpack (x, 1), + ucoo::bytes_unpack (x, 0), + ucoo::bytes_unpack (y, 2), + ucoo::bytes_unpack (y, 1), + ucoo::bytes_unpack (y, 0), + ucoo::bytes_unpack (pos.a, 1), + ucoo::bytes_unpack (pos.a, 0), + direction_consign, + }; + send (buf, sizeof (buf)); +} + +void +Asserv::goto_xy (const vect_t &vect, + DirectionConsign direction_consign) +{ + int x = vect.x * scale_inv_; + int y = vect.y * scale_inv_; + uint8_t buf[] = { 'x', + ucoo::bytes_unpack (x, 2), + ucoo::bytes_unpack (x, 1), + ucoo::bytes_unpack (x, 0), + ucoo::bytes_unpack (y, 2), + ucoo::bytes_unpack (y, 1), + ucoo::bytes_unpack (y, 0), + direction_consign, + }; + send (buf, sizeof (buf)); +} + +void +Asserv::push_wall (DirectionConsign direction_consign, int init_x, int init_y, + int16_t init_a) +{ + if (init_x != -1) + init_x *= scale_inv_; + if (init_y != -1) + init_y *= scale_inv_; + uint8_t buf[] = { 'G', + direction_consign, + ucoo::bytes_unpack (init_x, 2), + ucoo::bytes_unpack (init_x, 1), + ucoo::bytes_unpack (init_x, 0), + ucoo::bytes_unpack (init_y, 2), + ucoo::bytes_unpack (init_y, 1), + ucoo::bytes_unpack (init_y, 0), + ucoo::bytes_unpack (init_a, 1), + ucoo::bytes_unpack (init_a, 0), + }; + send (buf, sizeof (buf)); +} + +void +Asserv::set_speed (uint16_t linear_hi, uint16_t angular_hi, uint16_t + linear_lo, uint16_t angular_lo) +{ + uint8_t buf[] = { 'p', 's', + ucoo::bytes_unpack (linear_hi, 1), + ucoo::bytes_unpack (linear_hi, 0), + ucoo::bytes_unpack (angular_hi, 1), + ucoo::bytes_unpack (angular_hi, 0), + ucoo::bytes_unpack (linear_lo, 1), + ucoo::bytes_unpack (linear_lo, 0), + ucoo::bytes_unpack (angular_lo, 1), + ucoo::bytes_unpack (angular_lo, 0), + }; + send (buf, sizeof (buf)); +} + +void +Asserv::set_acceleration (uint16_t linear, uint16_t angular) +{ + uint8_t buf[] = { 'p', 'a', + ucoo::bytes_unpack (linear, 1), + ucoo::bytes_unpack (linear, 0), + ucoo::bytes_unpack (angular, 1), + ucoo::bytes_unpack (angular, 0), + }; + send (buf, sizeof (buf)); +} + diff --git a/digital/io-hub/src/common-cc/asserv.hh b/digital/io-hub/src/common-cc/asserv.hh new file mode 100644 index 00000000..09f364f4 --- /dev/null +++ b/digital/io-hub/src/common-cc/asserv.hh @@ -0,0 +1,124 @@ +#ifndef asserv_hh +#define asserv_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "i2c_queue.hh" +#include "defs.hh" +#include "motor.hh" + +/// Interface to asserv board. +class Asserv : public I2cQueue::Slave +{ + public: + /// No support for auxiliary motors for the moment. + static const int aux_nb = 0; + public: + /// Direction consigns, can be or'ed together. + enum DirectionConsign + { + /// Use forward movements. + FORWARD = 0, + /// Use backward movements. + BACKWARD = 1, + /// Authorise to reverse the direction consign + REVERT_OK = 2, + /// Combination. + BACKWARD_REVERT_OK = BACKWARD | REVERT_OK, + }; + public: + /// Constructor. + Asserv (I2cQueue &queue, float scale); + /// See I2cQueue::Slave::recv_status. + void recv_status (const uint8_t *status); + /// Get status of last command. + Motor::Status get_status () const; + /// Get current moving direction (or NONE). + Direction get_moving_direction () const; + /// Get last moving direction. + Direction get_last_moving_direction () const; + /// Get current position. + void get_position (Position &position) const; + /// Release motors (zero torque). + void free (); + /// Stop moving, applying acceleration constraints. + void stop (); + /// Move linearly on a given distance (mm). + void move_distance (int distance); + /// Move angularly by a given angle. + void move_angle (int16_t angle); + /// Face an absolute angle. + void goto_angle (uint16_t angle); + /// Go to an absolute position and angle. + void goto_xya (const Position &pos, + DirectionConsign direction_consign = FORWARD); + /// Go to an absolute position, any angle. + void goto_xy (const vect_t &vect, + DirectionConsign direction_consign = FORWARD); + /// Push the wall and initialise position. Use -1 for coordinates to keep + /// unchanged. + void push_wall (DirectionConsign direction_consign, + int init_x, int init_y, int16_t init_a); + /// Set movement speeds. + void set_speed (uint16_t linear_hi, uint16_t angular_hi, + uint16_t linear_lo, uint16_t angular_lo); + /// Set acceleration. + void set_acceleration (uint16_t linear, uint16_t angular); + private: + uint8_t status_flag_; + uint8_t input_port_; + int position_x_, position_y_; + uint16_t position_a_; + Direction last_moving_direction_; + float scale_, scale_inv_; +}; + +inline Motor::Status +Asserv::get_status () const +{ + if (status_flag_ & 1) + return Motor::SUCCESS; + else if (status_flag_ & 2) + return Motor::FAILURE; + else + return Motor::RUNNING; +} + +inline Direction +Asserv::get_moving_direction () const +{ + if (status_flag_ & 4) + return DIRECTION_FORWARD; + else if (status_flag_ & 8) + return DIRECTION_BACKWARD; + else + return DIRECTION_NONE; +} + +inline Direction +Asserv::get_last_moving_direction () const +{ + return last_moving_direction_; +} + +#endif // asserv_hh diff --git a/digital/io-hub/src/common-cc/motor.hh b/digital/io-hub/src/common-cc/motor.hh new file mode 100644 index 00000000..c9896057 --- /dev/null +++ b/digital/io-hub/src/common-cc/motor.hh @@ -0,0 +1,40 @@ +#ifndef motor_hh +#define motor_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +namespace Motor +{ + enum Status + { + /// Running, last command is not finished yet. + RUNNING, + /// Last command succeed. + SUCCESS, + /// Last command failed, motor is blocked. + FAILURE, + }; +} + +#endif // motor_hh -- cgit v1.2.3