summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/common-cc/asserv.hh
blob: 07d5f8ef5923413d60cdf694d5c1f87937d1ec78 (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
129
130
131
132
133
#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.
    const Position &get_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_;
    Position position_;
    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_;
}

inline const Position &
Asserv::get_position () const
{
    return position_;
}

#endif // asserv_hh