diff --git a/libraries/AP_Gripper/AP_Gripper.cpp b/libraries/AP_Gripper/AP_Gripper.cpp index 833a7626fb112d..83e7f6a0dbdabb 100644 --- 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 index 0a5b7dd54b39ab..3f6036557d572f 100644 --- 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 index 97472221107658..81d8e19dd057d9 100644 --- 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 index 87636f02edee70..bdbc381897b8a2 100644 --- a/libraries/AP_Gripper/AP_Gripper_Servo.h +++ b/libraries/AP_Gripper/AP_Gripper_Servo.h @@ -18,6 +18,8 @@ #include #include +#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 };