summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/common-cc/asserv.hh
blob: ea6f5ec4e31ffabd6dc0ce8f327c2cddd75a2dd0 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#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);
    /// Follow external consign.
    void follow (DirectionConsign direction_consign);
    /// Update external consign.
    void follow_update (int16_t consign);
    /// 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