ardupilot/libraries/AP_Winch/AP_Winch.h

94 lines
3.3 KiB
C++

/*
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 3 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, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h>
#include <AC_PID/AC_PID.h>
#include <AP_WheelEncoder/AP_WheelEncoder.h>
// winch rate control default gains
#define AP_WINCH_POS_P 1.00f
#define AP_WINCH_RATE_P 1.00f
#define AP_WINCH_RATE_I 0.50f
#define AP_WINCH_RATE_IMAX 1.00f
#define AP_WINCH_RATE_D 0.00f
#define AP_WINCH_RATE_FILT 5.00f
#define AP_WINCH_RATE_DT 0.10f
class AP_Winch_Backend;
class AP_Winch {
friend class AP_Winch_Backend;
friend class AP_Winch_Servo;
public:
AP_Winch();
// indicate whether this module is enabled
bool enabled() const;
// initialise the winch
void init(const AP_WheelEncoder *wheel_encoder = nullptr);
// update the winch
void update();
// relax the winch so it does not attempt to maintain length or rate
void relax() { config.state = STATE_RELAXED; }
// get current line length
float get_line_length() const { return config.length_curr; }
// release specified length of cable (in meters) at the specified rate
// if rate is zero, the RATE_MAX parameter value will be used
void release_length(float length, float rate = 0.0f);
// deploy line at specified speed in m/s (+ve deploys line, -ve retracts line, 0 stops)
void set_desired_rate(float rate);
// get rate maximum in m/s
float get_rate_max() const { return MAX(config.rate_max, 0.0f); }
static const struct AP_Param::GroupInfo var_info[];
private:
// parameters
AP_Int8 _enabled; // grabber enable/disable
// winch states
typedef enum {
STATE_RELAXED = 0, // winch is not operating
STATE_POSITION, // moving or maintaining a target length
STATE_RATE, // deploying or retracting at a target rate
} winch_state;
struct Backend_Config {
AP_Int8 type; // winch type
AP_Float rate_max; // deploy or retract rate maximum (in m/s).
AP_Float pos_p; // position error P gain
AC_PID rate_pid = AC_PID(AP_WINCH_RATE_P, AP_WINCH_RATE_I, AP_WINCH_RATE_D, AP_WINCH_RATE_IMAX, AP_WINCH_RATE_FILT, AP_WINCH_RATE_DT); // rate control PID
winch_state state; // state of winch control (using target position or target rate)
float length_curr; // current length of the line (in meters) that has been deployed
float length_desired; // target desired length (in meters)
float rate_desired; // target deploy rate (in m/s, +ve = deploying, -ve = retracting)
} config;
AP_Winch_Backend *backend;
};