AP_Gripper: Add delay before grab

lthall · Feb 12, 2024 · d85840f · d85840f
1 parent d857afc
commit d85840f
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 3 deletions.
diff --git a/libraries/AP_Gripper/AP_Gripper.cpp b/libraries/AP_Gripper/AP_Gripper.cpp
@@ -76,6 +76,14 @@ const AP_Param::GroupInfo AP_Gripper::var_info[] = {
     // @Units: s
     AP_GROUPINFO("AUTOCLOSE",  7, AP_Gripper, config.autoclose_time, GRIPPER_AUTOCLOSE_DEFAULT),
 
+    // @Param: DELAYGRAB
+    // @DisplayName: Gripper delay close time
+    // @Description: Time in seconds that gripper waits before closing, 0 to disable
+    // @User: Advanced
+    // @Range: 0.25 255
+    // @Units: s
+    AP_GROUPINFO("DELAYGRAB",  8, AP_Gripper, config.delaygrab_time, GRIPPER_AUTOCLOSE_DEFAULT),
+
     AP_GROUPEND
 };
 

diff --git a/libraries/AP_Gripper/AP_Gripper.h b/libraries/AP_Gripper/AP_Gripper.h
@@ -77,6 +77,7 @@ class AP_Gripper {
         AP_Int16    neutral_pwm;           // PWM value sent to gripper when not grabbing or releasing
         AP_Int8     regrab_interval;       // Time in seconds that EPM gripper will regrab the cargo to ensure grip has not weakend
         AP_Float    autoclose_time;        // Automatic close time (in seconds)
+        AP_Float    delaygrab_time;        // Delay grab time (in seconds)
         AP_Int16    uavcan_hardpoint_id;
 
         gripper_state state = STATE_NEUTRAL;

diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.cpp b/libraries/AP_Gripper/AP_Gripper_Servo.cpp
@@ -37,7 +37,9 @@ void AP_Gripper_Servo::grab()
     config.state = AP_Gripper::STATE_GRABBING;
 
     // move the servo to the grab position
-    SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
+    if (!is_positive(config.delaygrab_time)) {
+        SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
+    }
     _last_grab_or_release = AP_HAL::millis();
     gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
     AP::logger().Write_Event(LogEvent::GRIPPER_GRAB);
@@ -102,8 +104,15 @@ bool AP_Gripper_Servo::grabbed() const
 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;
+    if (config.state == AP_Gripper::STATE_GRABBING) {
+        if (has_state_pwm(config.grab_pwm)) {
+            config.state = AP_Gripper::STATE_GRABBED;
+        } else {
+            // move the servo to the release position
+            if (is_positive(config.delaygrab_time) && (AP_HAL::millis() - _last_grab_or_release > (config.delaygrab_time * 1000.0))) {
+                SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
+            }
+        }
     } else if (config.state == AP_Gripper::STATE_RELEASING && has_state_pwm(config.release_pwm)) {
         config.state = AP_Gripper::STATE_RELEASED;
     }