Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Gripper: apply auto close to all backends.

Browse Source
This commit is contained in:
Iampete1 2022-08-06 14:35:17 +01:00 committed by Peter Barker
parent 4bf622dbaf
commit 2d06261be3
5 changed files with 17 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Gripper/AP_Gripper_Backend.cpp
View File

@ -1,4 +1,5 @@
#include "AP_Gripper_Backend.h" #include "AP_Gripper_Backend.h"
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -11,4 +12,11 @@ void AP_Gripper_Backend::init()
void AP_Gripper_Backend::update() void AP_Gripper_Backend::update()
{ {
update_gripper(); update_gripper();
// close the gripper again if autoclose_time > 0.0
if (config.state == AP_Gripper::STATE_RELEASED && (_last_grab_or_release > 0) &&
(is_positive(config.autoclose_time)) &&
(AP_HAL::millis() - _last_grab_or_release > (config.autoclose_time * 1000.0))) {
grab();
}
} }

2
libraries/AP_Gripper/AP_Gripper_Backend.h
View File

@ -51,5 +51,7 @@ public:
protected: protected:
uint32_t _last_grab_or_release; // ms; time last grab or release happened
struct AP_Gripper::Backend_Config &config; struct AP_Gripper::Backend_Config &config;
}; };

3
libraries/AP_Gripper/AP_Gripper_EPM.h
View File

@ -56,7 +56,4 @@ private:
// UAVCAN driver fd // UAVCAN driver fd
int _uavcan_fd = -1; int _uavcan_fd = -1;
// internal variables
uint32_t _last_grab_or_release;
}; };

18
libraries/AP_Gripper/AP_Gripper_Servo.cpp
View File

@ -25,7 +25,7 @@ void AP_Gripper_Servo::grab()
// check if we are already grabbed // check if we are already grabbed
if (config.state == AP_Gripper::STATE_GRABBED) { if (config.state == AP_Gripper::STATE_GRABBED) {
// inform user that we are already grabbed // inform user that we are already grabbed
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing"); gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbed");
return; return;
} }
@ -34,7 +34,7 @@ void AP_Gripper_Servo::grab()
// move the servo to the grab position // move the servo to the grab position
SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm); SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
action_timestamp = AP_HAL::millis(); _last_grab_or_release = AP_HAL::millis();
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing"); gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
AP::logger().Write_Event(LogEvent::GRIPPER_GRAB); AP::logger().Write_Event(LogEvent::GRIPPER_GRAB);
} }
@ -59,7 +59,7 @@ void AP_Gripper_Servo::release()
// move the servo to the release position // move the servo to the release position
SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm); SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm);
action_timestamp = AP_HAL::millis(); _last_grab_or_release = AP_HAL::millis();
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing"); gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing");
AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE); AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE);
} }
@ -77,7 +77,7 @@ bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const
// (e.g. last action was a grab not a release) // (e.g. last action was a grab not a release)
return false; return false;
} }
if (AP_HAL::millis() - action_timestamp < SERVO_ACTUATION_TIME) { if (AP_HAL::millis() - _last_grab_or_release < SERVO_ACTUATION_TIME) {
// servo still moving.... // servo still moving....
return false; return false;
} }
@ -95,21 +95,15 @@ bool AP_Gripper_Servo::grabbed() const
} }
// type-specific periodic updates: // type-specific periodic updates:
void AP_Gripper_Servo::update_gripper() { void AP_Gripper_Servo::update_gripper()
{
// Check for successful grabbed or released // Check for successful grabbed or released
if (config.state == AP_Gripper::STATE_GRABBING && has_state_pwm(config.grab_pwm)) { if (config.state == AP_Gripper::STATE_GRABBING && has_state_pwm(config.grab_pwm)) {
config.state = AP_Gripper::STATE_GRABBED; config.state = AP_Gripper::STATE_GRABBED;
} else if (config.state == AP_Gripper::STATE_RELEASING && has_state_pwm(config.release_pwm)) { } else if (config.state == AP_Gripper::STATE_RELEASING && has_state_pwm(config.release_pwm)) {
config.state = AP_Gripper::STATE_RELEASED; config.state = AP_Gripper::STATE_RELEASED;
} }
// close the gripper again if autoclose_time > 0.0
if (config.state == AP_Gripper::STATE_RELEASED &&
(is_positive(config.autoclose_time)) &&
(AP_HAL::millis() - action_timestamp > (config.autoclose_time * 1000.0))) {
grab();
} }
};
bool AP_Gripper_Servo::valid() const bool AP_Gripper_Servo::valid() const
{ {

2
libraries/AP_Gripper/AP_Gripper_Servo.h
View File

@ -51,7 +51,5 @@ protected:
private: private:
uint32_t action_timestamp; // ms; time grab or release happened
bool has_state_pwm(const uint16_t pwm) const; bool has_state_pwm(const uint16_t pwm) const;
}; };