diff --git a/libraries/AP_EPM/AP_EPM.cpp b/libraries/AP_EPM/AP_EPM.cpp index 81ce5ba582..c7ec41d275 100644 --- a/libraries/AP_EPM/AP_EPM.cpp +++ b/libraries/AP_EPM/AP_EPM.cpp @@ -15,11 +15,39 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_EPM::var_info[] PROGMEM = { // @Param: ENABLE // @DisplayName: EPM Enable/Disable - // @Description: EPM enable/disable. Note enabling will disable the external LEDs on the APM2 + // @Description: EPM enable/disable // @User: Standard // @Values: 0:Disabled, 1:Enabled AP_GROUPINFO("ENABLE", 0, AP_EPM, _enabled, 0), + // @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 weakend + // @User: Advanced + // @Values: 0:Never, 15:every 15 seconds, 30:every 30 seconds, 60:once per minute + AP_GROUPINFO("REGRAB", 4, AP_EPM, _regrab_interval, EPM_REGRAB_DEFAULT), + AP_GROUPEND }; @@ -27,6 +55,11 @@ 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() @@ -48,11 +81,15 @@ void AP_EPM::grab() return; } + // flag we are active and grabbing cargo + _flags.grab = true; + _flags.active = true; + // capture time _last_grab_or_release = hal.scheduler->millis(); // move the servo to the release position - RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, EPM_GRAB_PWM); + RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _grab_pwm); } // release - move the epm pwm output to the release position @@ -63,11 +100,15 @@ void AP_EPM::release() return; } + // flag we are active and releasing cargo + _flags.grab = false; + _flags.active = true; + // capture time _last_grab_or_release = hal.scheduler->millis(); // move the servo to the release position - RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, EPM_RELEASE_PWM); + RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _release_pwm); } // neutral - return the epm pwm output to the neutral position @@ -78,8 +119,11 @@ void AP_EPM::neutral() return; } + // flag we are inactive (i.e. not grabbing or releasing cargo) + _flags.active = false; + // move the servo to the off position - RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, EPM_NEUTRAL_PWM); + RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _neutral_pwm); } // update - moves the pwm back to neutral after the timeout has passed @@ -87,8 +131,12 @@ void AP_EPM::neutral() void AP_EPM::update() { // move EPM PWM output back to neutral 2 seconds after the last grab or release - if ((_last_grab_or_release != 0) && (hal.scheduler->millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS)) { + if (_flags.active && (hal.scheduler->millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS)) { neutral(); } -} + // re-grab the cargo intermittently + if (_flags.grab && (_regrab_interval > 0) && (hal.scheduler->millis() - _last_grab_or_release > ((uint32_t)_regrab_interval * 1000))) { + grab(); + } +} diff --git a/libraries/AP_EPM/AP_EPM.h b/libraries/AP_EPM/AP_EPM.h index db13abc8f1..23e47d8e1e 100644 --- a/libraries/AP_EPM/AP_EPM.h +++ b/libraries/AP_EPM/AP_EPM.h @@ -5,6 +5,8 @@ * * Created on: DEC 06, 2013 * Author: Andreas Jochum + * + * Set-up Wiki: http://copter.ardupilot.com/wiki/common-electro-permanent-magnet-gripper/ */ /// @file AP_EPM.h @@ -18,12 +20,11 @@ #include // EPM PWM definitions -#define EPM_GRAB_PWM 1900 -#define EPM_RELEASE_PWM 1100 -#define EPM_NEUTRAL_PWM 1500 - -// EPM PWM returns to neutral position this many milliseconds after grab or release -#define EPM_RETURN_TO_NEUTRAL_MS 500 +#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 /// @brief Class to manage the EPM_CargoGripper @@ -31,21 +32,18 @@ class AP_EPM { public: AP_EPM(); - // setup the epm + // initialise the EPM void init(); - // enabled - true if the epm is enabled + // 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(); - // release - move the epm pwm output to the release position + // release - move the EPM pwm output to the release position void release(); - // neutral - return the epm pwm output to the neutral position - void neutral(); - // update - moves the pwm back to neutral after the timeout has passed // should be called at at least 10hz void update(); @@ -53,8 +51,22 @@ public: static const struct AP_Param::GroupInfo var_info[]; private: + + // neutral - return the EPM pwm output to the neutral position + void neutral(); + + // 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; + // parameters - AP_Int8 _enabled; + 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 // internal variables uint32_t _last_grab_or_release;