From a5e2f96a59e9911ed37850866c2e01cbfd0d4f63 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 9 Aug 2018 18:51:17 +0200 Subject: [PATCH] SITL: Better gripper sim options --- libraries/SITL/SIM_Gripper_Servo.cpp | 41 ++++++++++++++++++++++++---- libraries/SITL/SIM_Gripper_Servo.h | 8 +++++- 2 files changed, 42 insertions(+), 7 deletions(-) diff --git a/libraries/SITL/SIM_Gripper_Servo.cpp b/libraries/SITL/SIM_Gripper_Servo.cpp index 5e4ec6ec36..5eafad74ee 100644 --- a/libraries/SITL/SIM_Gripper_Servo.cpp +++ b/libraries/SITL/SIM_Gripper_Servo.cpp @@ -40,6 +40,29 @@ const AP_Param::GroupInfo Gripper_Servo::var_info[] = { // @User: Advanced AP_GROUPINFO("PIN", 1, Gripper_Servo, gripper_servo_pin, -1), + // @Param: GRAB + // @DisplayName: Gripper Grab PWM + // @Description: PWM value in microseconds sent to Gripper to initiate grabbing the cargo + // @User: Advanced + // @Range: 1000 2000 + // @Units: PWM + AP_GROUPINFO("GRAB", 2, Gripper_Servo, grab_pwm, SIM_GRIPPER_GRAB_PWM_DEFAULT), + + // @Param: RELEASE + // @DisplayName: Gripper Release PWM + // @Description: PWM value in microseconds sent to Gripper to release the cargo + // @User: Advanced + // @Range: 1000 2000 + // @Units: PWM + AP_GROUPINFO("RELEASE", 3, Gripper_Servo, release_pwm, SIM_GRIPPER_RELEASE_PWM_DEFAULT), + + // @Param: REVERSE + // @DisplayName: Gripper close direction + // @Description: Reverse the closing direction. + // @User: Advanced + // @Values: 0:Normal,1:Reverse + AP_GROUPINFO("REVERSE", 4, Gripper_Servo, reverse, 0), + AP_GROUPEND }; @@ -58,16 +81,20 @@ void Gripper_Servo::update(const struct sitl_input &input) last_update_us = now; return; } - - float position_demand = (gripper_pwm - 1000) * 0.001f; - if (is_negative(position_demand)) { // never updated - position_demand = 0.0f; + const int16_t diff_pwm = abs(grab_pwm - release_pwm); + float position_demand = (gripper_pwm - diff_pwm) * 0.001f; + if (gripper_pwm < MIN(grab_pwm, release_pwm) || position_demand > 1.0f) { // never updated + position_demand = position; } const float position_max_change = position_slew_rate / 100.0f * dt; position = constrain_float(position_demand, position - position_max_change, position + position_max_change); - - const float jaw_gap = gap * (1.0f - position); + float jaw_gap; + if ((release_pwm < grab_pwm && reverse) || (release_pwm > grab_pwm && !reverse)) { + jaw_gap = gap * position; + } else { + jaw_gap = gap * (1.0f - position); + } if (should_report()) { ::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass); last_report_us = now; @@ -77,9 +104,11 @@ void Gripper_Servo::update(const struct sitl_input &input) if (jaw_gap < 5) { if (altitude <= 0.0f) { load_mass = 1.0f; // attach the load + jaw_open = false; } } else if (jaw_gap > 10) { load_mass = 0.0f; // detach the load + jaw_open = true; } last_update_us = now; diff --git a/libraries/SITL/SIM_Gripper_Servo.h b/libraries/SITL/SIM_Gripper_Servo.h index 1e22a81359..d76697721e 100644 --- a/libraries/SITL/SIM_Gripper_Servo.h +++ b/libraries/SITL/SIM_Gripper_Servo.h @@ -37,13 +37,19 @@ public: void set_alt(float alt) {altitude = alt;}; static const struct AP_Param::GroupInfo var_info[]; bool is_enabled() const {return static_cast(gripper_enable);} + bool is_jaw_open() const {return jaw_open;} private: + static constexpr int16_t SIM_GRIPPER_GRAB_PWM_DEFAULT = 2000; + static constexpr int16_t SIM_GRIPPER_RELEASE_PWM_DEFAULT = 1000; AP_Int8 gripper_enable; // enable gripper sim AP_Int8 gripper_servo_pin; + 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_Int8 reverse; // reverse closing direction const uint32_t report_interval = 1000000; // microseconds uint64_t last_report_us; - + bool jaw_open = false; const float gap = 30; // mm float altitude; float position; // percentage