AP_Gripper: Servo Gipper enhancments

ArduPilot · Aug 1, 2022 · 4752679 · 4752679
1 parent f57e366
commit 4752679
Show file tree

Hide file tree

Showing 4 changed files with 39 additions and 36 deletions.
diff --git a/libraries/AP_Gripper/AP_Gripper.cpp b/libraries/AP_Gripper/AP_Gripper.cpp
@@ -9,7 +9,8 @@ extern const AP_HAL::HAL& hal;
 #define GRIPPER_RELEASE_PWM_DEFAULT     1100
 // EPM PWM definitions
 #define GRIPPER_NEUTRAL_PWM_DEFAULT     1500
-#define GRIPPER_REGRAB_DEFAULT          0           // default re-grab interval (in seconds) to ensure cargo is securely held
+#define GRIPPER_REGRAB_DEFAULT          0           // default EPM re-grab interval (in seconds) to ensure cargo is securely held
+#define GRIPPER_AUTOCLOSE_DEFAULT       0.0         // default automatic close time (in seconds)
 
 const AP_Param::GroupInfo AP_Gripper::var_info[] = {
     // @Param: ENABLE
@@ -51,8 +52,8 @@ const AP_Param::GroupInfo AP_Gripper::var_info[] = {
     AP_GROUPINFO("NEUTRAL", 4, AP_Gripper, config.neutral_pwm, GRIPPER_NEUTRAL_PWM_DEFAULT),
 
     // @Param: REGRAB
-    // @DisplayName: Gripper Regrab interval
-    // @Description: Time in seconds that gripper will regrab the cargo to ensure grip has not weakened; 0 to disable
+    // @DisplayName: EPM Gripper Regrab interval
+    // @Description: Time in seconds that EPM gripper will regrab the cargo to ensure grip has not weakened; 0 to disable
     // @User: Advanced
     // @Range: 0 255
     // @Units: s
@@ -65,6 +66,14 @@ const AP_Param::GroupInfo AP_Gripper::var_info[] = {
     // @Range: 0 255
     AP_GROUPINFO("CAN_ID", 6, AP_Gripper, config.uavcan_hardpoint_id, 0),
 
+    // @Param: AUTOCLOSE
+    // @DisplayName: Gripper Autoclose time
+    // @Description: Time in seconds that gripper close the gripper after opening; 0 to disable
+    // @User: Advanced
+    // @Range: 0.25 255
+    // @Units: s
+    AP_GROUPINFO("AUTOCLOSE",  7, AP_Gripper, config.autoclose_time, GRIPPER_AUTOCLOSE_DEFAULT),
+
     AP_GROUPEND
 };
 

diff --git a/libraries/AP_Gripper/AP_Gripper.h b/libraries/AP_Gripper/AP_Gripper.h
@@ -70,7 +70,8 @@ class AP_Gripper {
         AP_Int16    grab_pwm;              // PWM value sent to Gripper to initiate grabbing the cargo
         AP_Int16    release_pwm;           // PWM value sent to Gripper to release the cargo
         AP_Int16    neutral_pwm;           // PWM value sent to gripper when not grabbing or releasing
-        AP_Int8     regrab_interval;       // Time in seconds that gripper will regrab the cargo to ensure grip has not weakend
+        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_Int16    uavcan_hardpoint_id;
 
         gripper_state state = STATE_RELEASED;

diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.cpp b/libraries/AP_Gripper/AP_Gripper_Servo.cpp
@@ -10,32 +10,30 @@ extern const AP_HAL::HAL& hal;
 
 void AP_Gripper_Servo::init_gripper()
 {
-    // move the servo to the release position
-    release();
+    // move the servo to the neutral position
+    SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.neutral_pwm);
 }
 
 void AP_Gripper_Servo::grab()
 {
+    // flag we are active and grabbing cargo
+    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);
     action_timestamp = AP_HAL::millis();
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    is_releasing = false;
-    is_released = true;
-#endif
     gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
     AP::logger().Write_Event(LogEvent::GRIPPER_GRAB);
 }
 
 void AP_Gripper_Servo::release()
 {
+    // flag we are releasing cargo
+    config.state = AP_Gripper::STATE_RELEASING;
+
     // move the servo to the release position
     SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm);
     action_timestamp = AP_HAL::millis();
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    is_releasing = true;
-    is_released = false;
-#endif
     gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing");
     AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE);
 }
@@ -53,40 +51,38 @@ 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 < action_time) {
+    if (AP_HAL::millis() - action_timestamp < SERVO_ACTUATION_TIME) {
         // servo still moving....
         return false;
     }
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    if (is_releasing) {
-        gcs().send_text(MAV_SEVERITY_INFO, "Gripper load released");
-    } else {
-        gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbed");
-    }
-#endif
     return true;
 }
 
-
 bool AP_Gripper_Servo::released() const
 {
-    return has_state_pwm(config.release_pwm);
+    return (config.state == AP_Gripper::STATE_RELEASED);
 }
 
 bool AP_Gripper_Servo::grabbed() const
 {
-    return has_state_pwm(config.grab_pwm);
+    return (config.state == AP_Gripper::STATE_GRABBED);
 }
 
 // type-specific periodic updates:
 void AP_Gripper_Servo::update_gripper() {
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    if (is_releasing && !is_released) {
-        is_released = released();
-    } else if (!is_releasing && is_released) {
-        is_released = !grabbed();
+    // 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();
     }
-#endif
 };
 
 bool AP_Gripper_Servo::valid() const

diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.h b/libraries/AP_Gripper/AP_Gripper_Servo.h
@@ -18,6 +18,8 @@
 #include <AP_Gripper/AP_Gripper_Backend.h>
 #include <SRV_Channel/SRV_Channel.h>
 
+#define SERVO_ACTUATION_TIME    500         // Time for servo to move to target position during grab or release in milliseconds
+
 class AP_Gripper_Servo : public AP_Gripper_Backend {
 public:
 
@@ -50,11 +52,6 @@ class AP_Gripper_Servo : public AP_Gripper_Backend {
 private:
 
     uint32_t action_timestamp; // ms; time grab or release happened
-    const uint16_t action_time = 3000; // ms; time to grab or release
 
     bool has_state_pwm(const uint16_t pwm) const;
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    bool is_releasing;
-    bool is_released;
-#endif
 };