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/common-cc/asserv.cc | 193 +++++++++++++++++++++++++++++++++ 1 file changed, 193 insertions(+) create mode 100644 digital/io-hub/src/common-cc/asserv.cc (limited to 'digital/io-hub/src/common-cc/asserv.cc') 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)); +} + -- cgit v1.2.3