zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EPM: add params for pwm levels add re-grab feature 

Also include some minor commenting changes
master
Randy Mackay 11 years ago
parent
14d80910ec
commit
d09bbd1d91
2 changed files with 80 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 60
      libraries/AP_EPM/AP_EPM.cpp
  2. 40
      libraries/AP_EPM/AP_EPM.h

60
libraries/AP_EPM/AP_EPM.cpp
Unescape View File

@ -15,11 +15,39 @@ extern const AP_HAL::HAL& hal; @@ -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) : @@ -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() @@ -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() @@ -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() @@ -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() @@ -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();
}
}

40
libraries/AP_EPM/AP_EPM.h
Unescape View File

@ -5,6 +5,8 @@ @@ -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 @@ @@ -18,12 +20,11 @@
#include <RC_Channel.h>
// 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 { @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save