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

AP_Gripper: apply auto close to all backends.

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_Math/AP_Math.h>
extern const AP_HAL::HAL& hal;
@ -11,4 +12,11 @@ void AP_Gripper_Backend::init()
void AP_Gripper_Backend::update()
{
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:
uint32_t _last_grab_or_release; // ms; time last grab or release happened
struct AP_Gripper::Backend_Config &config;
};

3
libraries/AP_Gripper/AP_Gripper_EPM.h
View File

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

20
libraries/AP_Gripper/AP_Gripper_Servo.cpp
View File

@ -25,7 +25,7 @@ void AP_Gripper_Servo::grab()
// check if we are already grabbed
if (config.state == AP_Gripper::STATE_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;
}
@ -34,7 +34,7 @@ void AP_Gripper_Servo::grab()
// move the servo to the grab position
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");
AP::logger().Write_Event(LogEvent::GRIPPER_GRAB);
}
@ -59,7 +59,7 @@ void AP_Gripper_Servo::release()
// move the servo to the release position
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");
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)
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....
return false;
}
@ -95,21 +95,15 @@ bool AP_Gripper_Servo::grabbed() const
}
// type-specific periodic updates:
void AP_Gripper_Servo::update_gripper() {
void AP_Gripper_Servo::update_gripper()
{
// Check for successful grabbed or released
if (config.state == AP_Gripper::STATE_GRABBING && has_state_pwm(config.grab_pwm)) {
config.state = AP_Gripper::STATE_GRABBED;
} else if (config.state == AP_Gripper::STATE_RELEASING && has_state_pwm(config.release_pwm)) {
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
{

2
libraries/AP_Gripper/AP_Gripper_Servo.h
View File

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