mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-26 01:33:56 -04:00
SITL: Better gripper sim options
This commit is contained in:
parent
f258a25138
commit
a5e2f96a59
@ -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;
|
||||
|
@ -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<bool>(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
|
||||
|
Loading…
Reference in New Issue
Block a user