Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Gripper: make EPM a subclass of AP_Gripper_Backend

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_Servo.h"
#include "AP_Gripper_EPM.h"
extern const AP_HAL::HAL& hal;
#define GRIPPER_GRAB_PWM_DEFAULT 1900
#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[] = {
// @Param: ENABLE
@ -36,6 +40,27 @@ const AP_Param::GroupInfo AP_Gripper::var_info[] = {
// @Range: 1000 2000
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
};
@ -57,6 +82,9 @@ void AP_Gripper::init()
case 1:
backend = new AP_Gripper_Servo(config);
break;
case 2:
backend = new AP_Gripper_EPM(config);
break;
default:
break;
}
@ -69,6 +97,9 @@ void AP_Gripper::init()
#define PASS_TO_BACKEND(function_name) \
void AP_Gripper::function_name() \
{ \
if (!enabled()) { \
return; \
} \
if (backend != nullptr) { \
backend->function_name(); \
} \

15
libraries/AP_Gripper/AP_Gripper.h
View File

@ -23,6 +23,9 @@ class AP_Gripper {
public:
AP_Gripper();
// indicate whether this module is enabled or not
bool enabled() const { return _enabled; }
// initialise the gripper
void init();
@ -40,10 +43,22 @@ public:
// parameters
AP_Int8 _enabled; // grabber enable/disable
typedef enum {
STATE_GRABBING,
STATE_RELEASING,
STATE_GRABBED,
STATE_RELEASED,
} gripper_state;
struct Backend_Config {
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 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;
private:

145
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
* Author: Andreas Jochum
* 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_BoardConfig/AP_BoardConfig.h>
#include <fcntl.h>
@ -15,71 +16,13 @@
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_EPM::var_info[] = {
// @Param: ENABLE
// @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),
AP_Gripper_EPM::AP_Gripper_EPM(struct AP_Gripper::Backend_Config &_config) :
AP_Gripper_Backend(_config) { }
// @Param: GRAB
// @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)
void AP_Gripper_EPM::init_gripper()
{
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
_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
::printf("EPM: UAVCAN fd %d\n", _uavcan_fd);
#endif
@ -88,17 +31,17 @@ void AP_EPM::init()
neutral();
}
// grab - move the epm pwm output to the grab position
void AP_EPM::grab()
{
// return immediately if not enabled
if (!_enabled) {
return;
}
// Refer to http://uavcan.org/Specification/7._List_of_standard_data_types/#uavcanequipmenthardpoint
struct UAVCANCommand {
uint8_t hardpoint_id = 0;
uint16_t command = 0;
};
// grab - move the epm pwm output to the grab position
void AP_Gripper_EPM::grab()
{
// flag we are active and grabbing cargo
_flags.grab = true;
_flags.active = true;
config.state = AP_Gripper::STATE_GRABBING;
// capture time
_last_grab_or_release = AP_HAL::millis();
@ -113,21 +56,15 @@ void AP_EPM::grab()
#endif
{
// 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
void AP_EPM::release()
// release - move epm pwm output to the release position
void AP_Gripper_EPM::release()
{
// return immediately if not enabled
if (!_enabled) {
return;
}
// flag we are active and releasing cargo
_flags.grab = false;
_flags.active = true;
// flag we are releasing cargo
config.state = AP_Gripper::STATE_RELEASING;
// capture time
_last_grab_or_release = AP_HAL::millis();
@ -142,39 +79,47 @@ void AP_EPM::release()
#endif
{
// 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
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()) {
// 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
// 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
if (_flags.active && (AP_HAL::millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS)) {
// move EPM PWM output back to neutral after the last grab or release
if (AP_HAL::millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS) {
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
if (_flags.grab && (_regrab_interval > 0) &&
(AP_HAL::millis() - _last_grab_or_release > ((uint32_t)_regrab_interval * 1000))) {
if (config.state == AP_Gripper::STATE_GRABBED &&
(config.regrab_interval > 0) &&
(AP_HAL::millis() - _last_grab_or_release > ((uint32_t)config.regrab_interval * 1000))) {
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
#pragma once
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include "AP_Gripper.h"
#include "AP_Gripper_Backend.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_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
class AP_EPM {
class AP_Gripper_EPM : public AP_Gripper_Backend {
public:
AP_EPM();
AP_Gripper_EPM(struct AP_Gripper::Backend_Config &_config);
// initialise the EPM
void init();
// enabled - true if the EPM is enabled
bool enabled() { return _enabled; }
void init_gripper() override;
// 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
void release();
void release() override;
// update - moves the pwm back to neutral after the timeout has passed
// should be called at at least 10hz
void update();
static const struct AP_Param::GroupInfo var_info[];
void update_gripper() override;
private:
@ -55,37 +46,11 @@ private:
bool should_use_uavcan() const { return _uavcan_fd >= 0; }
// Refer to http://uavcan.org/Specification/7._List_of_standard_data_types/#uavcanequipmenthardpoint
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;
struct UAVCANCommand make_uavcan_command(uint16_t command) const;
// UAVCAN driver fd
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
uint32_t _last_grab_or_release;
};