Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 08:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: gripper closes faster, vary load carried by gripper

Browse Source 
If gripper is closed on ground it is considered to be carrying
something
This commit is contained in:
Peter Barker 2016-11-23 16:15:12 +11:00
parent b751f836bb
commit 20b86605fa
2 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/SITL/SIM_Gripper_Servo.cpp
View File

@ -39,13 +39,21 @@ void Gripper_Servo::update(const Aircraft::sitl_input &input)
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);
if (should_report()) {
::fprintf(stderr, "position_demand=%f\n", position_demand);
printf("Position: %f mm\n", gap*position);
::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass);
last_report_us = now;
reported_position = position;
}
if (jaw_gap < 5) {
if (aircraft->on_ground()) {
load_mass = 1.0f; // attach the load
}
} else if (jaw_gap > 10) {
load_mass = 0.0f; // detach the load
}
last_update_us = now;
return;
}

4
libraries/SITL/SIM_Gripper_Servo.h
View File

@ -47,7 +47,7 @@ private:
const float gap = 30; // mm
float position; // percentage
float position_slew_rate = 20; // percentage
float position_slew_rate = 35; // percentage
float reported_position = -1; // unlikely
uint64_t last_update_us;
@ -57,7 +57,7 @@ private:
// dangle load from a string:
const float string_length = 2.0f; // metres
const float load_mass = 0.0f; // kilograms
float load_mass = 0.0f; // kilograms
};
}