Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Gripper: make EPM a subclass of AP_Gripper_Backend

Browse Source
This commit is contained in:
Peter Barker 2016-10-29 22:01:18 +11:00 committed by Randy Mackay
parent 15b271ec13
commit 48cd35609c
4 changed files with 103 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_Gripper/AP_Gripper.cpp
View File

@ -1,11 +1,15 @@
#include "AP_Gripper.h" #include "AP_Gripper.h"
#include "AP_Gripper_Servo.h" #include "AP_Gripper_Servo.h"
#include "AP_Gripper_EPM.h"
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define GRIPPER_GRAB_PWM_DEFAULT 1900 #define GRIPPER_GRAB_PWM_DEFAULT 1900
#define GRIPPER_RELEASE_PWM_DEFAULT 1100 #define GRIPPER_RELEASE_PWM_DEFAULT 1100
// EPM PWM definitions
#define GRIPPER_NEUTRAL_PWM_DEFAULT 1500
#define GRIPPER_REGRAB_DEFAULT 0 // default re-grab interval (in seconds) to ensure cargo is securely held
const AP_Param::GroupInfo AP_Gripper::var_info[] = { const AP_Param::GroupInfo AP_Gripper::var_info[] = {
// @Param: ENABLE // @Param: ENABLE
@ -36,6 +40,27 @@ const AP_Param::GroupInfo AP_Gripper::var_info[] = {
// @Range: 1000 2000 // @Range: 1000 2000
AP_GROUPINFO("RELEASE", 3, AP_Gripper, config.release_pwm, GRIPPER_RELEASE_PWM_DEFAULT), AP_GROUPINFO("RELEASE", 3, AP_Gripper, config.release_pwm, GRIPPER_RELEASE_PWM_DEFAULT),
// @Param: NEUTRAL
// @DisplayName: Neutral PWM
// @Description: PWM value sent to grabber when not grabbing or releasing
// @User: Advanced
// @Range: 1000 2000
AP_GROUPINFO("NEUTRAL", 4, AP_Gripper, config.neutral_pwm, GRIPPER_NEUTRAL_PWM_DEFAULT),
// @Param: REGRAB
// @DisplayName: Gripper Regrab interval
// @Description: Time in seconds that gripper will regrab the cargo to ensure grip has not weakened; 0 to disable
// @User: Advanced
// @Values: 0 255
AP_GROUPINFO("REGRAB", 5, AP_Gripper, config.regrab_interval, GRIPPER_REGRAB_DEFAULT),
// @Param: UAVCAN_ID
// @DisplayName: EPM UAVCAN Hardpoint ID
// @Description: Refer to https://docs.zubax.com/opengrab_epm_v3#UAVCAN_interface
// @User: Standard
// @Range: 0 255
AP_GROUPINFO("UAVCAN_ID", 6, AP_Gripper, config.uavcan_hardpoint_id, 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -57,6 +82,9 @@ void AP_Gripper::init()
case 1: case 1:
backend = new AP_Gripper_Servo(config); backend = new AP_Gripper_Servo(config);
break; break;
case 2:
backend = new AP_Gripper_EPM(config);
break;
default: default:
break; break;
} }
@ -69,6 +97,9 @@ void AP_Gripper::init()
#define PASS_TO_BACKEND(function_name) \ #define PASS_TO_BACKEND(function_name) \
void AP_Gripper::function_name() \ void AP_Gripper::function_name() \
{ \ { \
if (!enabled()) { \
return; \
} \
if (backend != nullptr) { \ if (backend != nullptr) { \
backend->function_name(); \ backend->function_name(); \
} \ } \

15
libraries/AP_Gripper/AP_Gripper.h
View File

@ -23,6 +23,9 @@ class AP_Gripper {
public: public:
AP_Gripper(); AP_Gripper();
// indicate whether this module is enabled or not
bool enabled() const { return _enabled; }
// initialise the gripper // initialise the gripper
void init(); void init();
@ -40,10 +43,22 @@ public:
// parameters // parameters
AP_Int8 _enabled; // grabber enable/disable AP_Int8 _enabled; // grabber enable/disable
typedef enum {
STATE_GRABBING,
STATE_RELEASING,
STATE_GRABBED,
STATE_RELEASED,
} gripper_state;
struct Backend_Config { struct Backend_Config {
AP_Int8 type; // grabber type (e.g. EPM or servo) AP_Int8 type; // grabber type (e.g. EPM or servo)
AP_Int16 grab_pwm; // PWM value sent to Gripper to initiate grabbing the cargo AP_Int16 grab_pwm; // PWM value sent to Gripper to initiate grabbing the cargo
AP_Int16 release_pwm; // PWM value sent to Gripper to release the cargo AP_Int16 release_pwm; // PWM value sent to Gripper to release the cargo
AP_Int16 neutral_pwm; // PWM value sent to gripper when not grabbing or releasing
AP_Int8 regrab_interval; // Time in seconds that gripper will regrab the cargo to ensure grip has not weakend
AP_Int16 uavcan_hardpoint_id;
gripper_state state = STATE_RELEASED;
} config; } config;
private: private:

147
libraries/AP_Gripper/AP_Gripper_EPM.cpp
View File

@ -1,12 +1,13 @@
/* /*
* AP_EPM.cpp * AP_Gripper_EPM.cpp
* *
* Created on: DEC 6, 2013 * Created on: DEC 6, 2013
* Author: Andreas Jochum * Author: Andreas Jochum
* Pavel Kirienko <pavel.kirienko@zubax.com> - UAVCAN support * Pavel Kirienko <pavel.kirienko@zubax.com> - UAVCAN support
* Peter Barker - moved into AP_Gripper_EPM
*/ */
#include "AP_EPM.h" #include "AP_Gripper_EPM.h"
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <fcntl.h> #include <fcntl.h>
@ -15,71 +16,13 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_EPM::var_info[] = { AP_Gripper_EPM::AP_Gripper_EPM(struct AP_Gripper::Backend_Config &_config) :
// @Param: ENABLE AP_Gripper_Backend(_config) { }
// @DisplayName: EPM Enable/Disable
// @Description: EPM enable/disable
// @User: Standard
// @Values: 0:Disabled, 1:Enabled
AP_GROUPINFO_FLAGS("ENABLE", 0, AP_EPM, _enabled, 0, AP_PARAM_FLAG_ENABLE),
// @Param: GRAB void AP_Gripper_EPM::init_gripper()
// @DisplayName: EPM Grab PWM
// @Description: PWM value sent to EPM to initiate grabbing the cargo
// @User: Advanced
// @Range: 1000 2000
AP_GROUPINFO("GRAB", 1, AP_EPM, _grab_pwm, EPM_GRAB_PWM_DEFAULT),
// @Param: RELEASE
// @DisplayName: EPM Release PWM
// @Description: PWM value sent to EPM to release the cargo
// @User: Advanced
// @Range: 1000 2000
AP_GROUPINFO("RELEASE", 2, AP_EPM, _release_pwm, EPM_RELEASE_PWM_DEFAULT),
// @Param: NEUTRAL
// @DisplayName: EPM Neutral PWM
// @Description: PWM value sent to EPM when not grabbing or releasing
// @User: Advanced
// @Range: 1000 2000
AP_GROUPINFO("NEUTRAL", 3, AP_EPM, _neutral_pwm, EPM_NEUTRAL_PWM_DEFAULT),
// @Param: REGRAB
// @DisplayName: EPM Regrab interval
// @Description: Time in seconds that gripper will regrab the cargo to ensure grip has not weakened; 0 to disable
// @User: Advanced
// @Values: 0 255
AP_GROUPINFO("REGRAB", 4, AP_EPM, _regrab_interval, EPM_REGRAB_DEFAULT),
// @Param: REGRAB
// @DisplayName: EPM UAVCAN Hardpoint ID
// @Description: Refer to https://docs.zubax.com/opengrab_epm_v3#UAVCAN_interface
// @User: Standard
// @Range: 0 255
AP_GROUPINFO("UAVCAN_ID", 5, AP_EPM, _uavcan_hardpoint_id, 0),
AP_GROUPEND
};
AP_EPM::AP_EPM(void) :
_last_grab_or_release(0)
{ {
AP_Param::setup_object_defaults(this, var_info);
// initialise flags
_flags.grab = false;
_flags.active = false;
}
void AP_EPM::init()
{
// return immediately if not enabled
if (!_enabled) {
return;
}
#ifdef UAVCAN_NODE_FILE #ifdef UAVCAN_NODE_FILE
_uavcan_fd = open(UAVCAN_NODE_FILE, 0); _uavcan_fd = ::open(UAVCAN_NODE_FILE, 0);
// http://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html // http://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html
::printf("EPM: UAVCAN fd %d\n", _uavcan_fd); ::printf("EPM: UAVCAN fd %d\n", _uavcan_fd);
#endif #endif
@ -88,17 +31,17 @@ void AP_EPM::init()
neutral(); neutral();
} }
// grab - move the epm pwm output to the grab position // Refer to http://uavcan.org/Specification/7._List_of_standard_data_types/#uavcanequipmenthardpoint
void AP_EPM::grab() struct UAVCANCommand {
{ uint8_t hardpoint_id = 0;
// return immediately if not enabled uint16_t command = 0;
if (!_enabled) { };
return;
}
// grab - move the epm pwm output to the grab position
void AP_Gripper_EPM::grab()
{
// flag we are active and grabbing cargo // flag we are active and grabbing cargo
_flags.grab = true; config.state = AP_Gripper::STATE_GRABBING;
_flags.active = true;
// capture time // capture time
_last_grab_or_release = AP_HAL::millis(); _last_grab_or_release = AP_HAL::millis();
@ -113,21 +56,15 @@ void AP_EPM::grab()
#endif #endif
{ {
// move the servo to the release position // move the servo to the release position
RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _grab_pwm); RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.grab_pwm);
} }
} }
// release - move the epm pwm output to the release position // release - move epm pwm output to the release position
void AP_EPM::release() void AP_Gripper_EPM::release()
{ {
// return immediately if not enabled // flag we are releasing cargo
if (!_enabled) { config.state = AP_Gripper::STATE_RELEASING;
return;
}
// flag we are active and releasing cargo
_flags.grab = false;
_flags.active = true;
// capture time // capture time
_last_grab_or_release = AP_HAL::millis(); _last_grab_or_release = AP_HAL::millis();
@ -142,39 +79,47 @@ void AP_EPM::release()
#endif #endif
{ {
// move the servo to the release position // move the servo to the release position
RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _release_pwm); RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.release_pwm);
} }
} }
// neutral - return the epm pwm output to the neutral position // neutral - return the epm pwm output to the neutral position
void AP_EPM::neutral() void AP_Gripper_EPM::neutral()
{ {
// return immediately if not enabled
if (!_enabled) {
return;
}
// flag we are inactive (i.e. not grabbing or releasing cargo)
_flags.active = false;
if (!should_use_uavcan()) { if (!should_use_uavcan()) {
// move the servo to the off position // move the servo to the off position
RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _neutral_pwm); RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.neutral_pwm);
} }
} }
// update - moves the pwm back to neutral after the timeout has passed // update - moves the pwm back to neutral after the timeout has passed
// should be called at at least 10hz // should be called at at least 10hz
void AP_EPM::update() void AP_Gripper_EPM::update_gripper()
{ {
// move EPM PWM output back to neutral 2 seconds after the last grab or release // move EPM PWM output back to neutral after the last grab or release
if (_flags.active && (AP_HAL::millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS)) { if (AP_HAL::millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS) {
neutral(); if (config.state == AP_Gripper::STATE_GRABBING) {
neutral();
config.state = AP_Gripper::STATE_GRABBED;
} else if (config.state == AP_Gripper::STATE_RELEASING) {
neutral();
config.state = AP_Gripper::STATE_RELEASED;
}
} }
// re-grab the cargo intermittently // re-grab the cargo intermittently
if (_flags.grab && (_regrab_interval > 0) && if (config.state == AP_Gripper::STATE_GRABBED &&
(AP_HAL::millis() - _last_grab_or_release > ((uint32_t)_regrab_interval * 1000))) { (config.regrab_interval > 0) &&
(AP_HAL::millis() - _last_grab_or_release > ((uint32_t)config.regrab_interval * 1000))) {
grab(); grab();
} }
} }
UAVCANCommand AP_Gripper_EPM::make_uavcan_command(uint16_t command) const
{
UAVCANCommand cmd;
cmd.hardpoint_id = config.uavcan_hardpoint_id;
cmd.command = command;
return cmd;
}

57
libraries/AP_Gripper/AP_Gripper_EPM.h
View File

@ -13,40 +13,31 @@
/// @brief AP_EPM control class /// @brief AP_EPM control class
#pragma once #pragma once
#include <AP_Math/AP_Math.h> #include "AP_Gripper.h"
#include <AP_Common/AP_Common.h> #include "AP_Gripper_Backend.h"
#include <RC_Channel/RC_Channel.h> #include <RC_Channel/RC_Channel.h>
// EPM PWM definitions
#define EPM_GRAB_PWM_DEFAULT 1900
#define EPM_RELEASE_PWM_DEFAULT 1100
#define EPM_NEUTRAL_PWM_DEFAULT 1500
#define EPM_RETURN_TO_NEUTRAL_MS 500 // EPM PWM returns to neutral position this many milliseconds after grab or release #define EPM_RETURN_TO_NEUTRAL_MS 500 // EPM PWM returns to neutral position this many milliseconds after grab or release
#define EPM_REGRAB_DEFAULT 0 // default re-grab interval (in seconds) to ensure cargo is securely held
/// @class AP_EPM /// @class AP_Gripper_EPM
/// @brief Class to manage the EPM_CargoGripper /// @brief Class to manage the EPM_CargoGripper
class AP_EPM { class AP_Gripper_EPM : public AP_Gripper_Backend {
public: public:
AP_EPM(); AP_Gripper_EPM(struct AP_Gripper::Backend_Config &_config);
// initialise the EPM // initialise the EPM
void init(); void init_gripper() override;
// enabled - true if the EPM is enabled
bool enabled() { return _enabled; }
// grab - move the EPM pwm output to the grab position // grab - move the EPM pwm output to the grab position
void grab(); void grab() override;
// release - move the EPM pwm output to the release position // release - move the EPM pwm output to the release position
void release(); void release() override;
// update - moves the pwm back to neutral after the timeout has passed // update - moves the pwm back to neutral after the timeout has passed
// should be called at at least 10hz // should be called at at least 10hz
void update(); void update_gripper() override;
static const struct AP_Param::GroupInfo var_info[];
private: private:
@ -55,37 +46,11 @@ private:
bool should_use_uavcan() const { return _uavcan_fd >= 0; } bool should_use_uavcan() const { return _uavcan_fd >= 0; }
// Refer to http://uavcan.org/Specification/7._List_of_standard_data_types/#uavcanequipmenthardpoint struct UAVCANCommand make_uavcan_command(uint16_t command) const;
struct UAVCANCommand {
uint8_t hardpoint_id = 0;
uint16_t command = 0;
};
UAVCANCommand make_uavcan_command(uint16_t command) const
{
UAVCANCommand cmd;
cmd.hardpoint_id = _uavcan_hardpoint_id;
cmd.command = command;
return cmd;
}
// EPM flags
struct EPM_Flags {
uint8_t grab : 1; // true if we think we have grabbed onto cargo, false if we think we've released it
uint8_t active : 1; // true if we are actively sending grab or release PWM to EPM to activate grabbing or releasing, false if we are sending neutral pwm
} _flags;
// UAVCAN driver fd // UAVCAN driver fd
int _uavcan_fd = -1; int _uavcan_fd = -1;
// parameters
AP_Int8 _enabled; // EPM enable/disable
AP_Int16 _grab_pwm; // PWM value sent to EPM to initiate grabbing the cargo
AP_Int16 _release_pwm; // PWM value sent to EPM to release the cargo
AP_Int16 _neutral_pwm; // PWM value sent to EPM when not grabbing or releasing
AP_Int8 _regrab_interval; // Time in seconds that gripper will regrab the cargo to ensure grip has not weakend
AP_Int16 _uavcan_hardpoint_id;
// internal variables // internal variables
uint32_t _last_grab_or_release; uint32_t _last_grab_or_release;
}; };