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 +++++++++++++++++++++++++++++++++ digital/io-hub/src/common-cc/asserv.hh | 124 +++++++++++++++++++++ digital/io-hub/src/common-cc/motor.hh | 40 +++++++ 3 files changed, 357 insertions(+) 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/src/common-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)); +} + 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