From d439bbe5bfdaacce6e3236c2e2b76f516428154f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 29 Oct 2016 09:06:52 +1100 Subject: [PATCH] SITL: SIM_Gripper_Servo, a simulated servo gripper --- libraries/SITL/SIM_Gripper_Servo.cpp | 65 ++++++++++++++++++++++++++++ libraries/SITL/SIM_Gripper_Servo.h | 53 +++++++++++++++++++++++ libraries/SITL/SIM_Multicopter.cpp | 3 ++ libraries/SITL/SIM_Multicopter.h | 4 ++ 4 files changed, 125 insertions(+) create mode 100644 libraries/SITL/SIM_Gripper_Servo.cpp create mode 100644 libraries/SITL/SIM_Gripper_Servo.h diff --git a/libraries/SITL/SIM_Gripper_Servo.cpp b/libraries/SITL/SIM_Gripper_Servo.cpp new file mode 100644 index 0000000000..1227f9b2fa --- /dev/null +++ b/libraries/SITL/SIM_Gripper_Servo.cpp @@ -0,0 +1,65 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simple Gripper (Servo) simulation class +*/ + +#include "SIM_Gripper_Servo.h" +#include + +using namespace SITL; + +/* + update gripper state + */ +void Gripper_Servo::update(const Aircraft::sitl_input &input) +{ + const uint64_t now = AP_HAL::micros64(); + const float dt = (now - last_update_us) * 1.0e-6f; + + // update gripper position + + float position_demand = (input.servos[gripper_servo]-1000) * 0.001f; + if (position_demand < 0) { // never updated + position_demand = 0; + } + + const float position_max_change = position_slew_rate/100.0f * dt; + position = constrain_float(position_demand, position-position_max_change, position+position_max_change); + + if (should_report()) { + ::fprintf(stderr, "position_demand=%f\n", position_demand); + printf("Position: %f mm\n", gap*position); + last_report_us = now; + reported_position = position; + } + + last_update_us = now; + return; +} + +bool Gripper_Servo::should_report() +{ + if (AP_HAL::micros64() - last_report_us < report_interval) { + return false; + } + + if (reported_position != position) { + return true; + } + + return false; +} + diff --git a/libraries/SITL/SIM_Gripper_Servo.h b/libraries/SITL/SIM_Gripper_Servo.h new file mode 100644 index 0000000000..713af8cc7e --- /dev/null +++ b/libraries/SITL/SIM_Gripper_Servo.h @@ -0,0 +1,53 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simple Gripper (Servo) simulation class +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +namespace SITL { + +class Gripper_Servo { +public: + const uint8_t gripper_servo; + + Gripper_Servo(const uint8_t _gripper_servo) : + gripper_servo(_gripper_servo) + {} + + // update Gripper state + void update(const struct Aircraft::sitl_input &input); + +private: + + const uint32_t report_interval = 1000000; // microseconds + uint64_t last_report_us; + + const float gap = 30; // mm + + float position; // percentage + float position_slew_rate = 20; // percentage + float reported_position = -1; // unlikely + + uint64_t last_update_us; + + bool should_report(); + bool zero_report_done = false; +}; + +} diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 1f4e980fdf..ad0821ef69 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -73,6 +73,9 @@ void MultiCopter::update(const struct sitl_input &input) // update sprayer sprayer.update(input); + + // update gripper + gripper.update(input); } float MultiCopter::gross_mass() const diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index 6b3752b273..0a9f4c5884 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -22,6 +22,7 @@ #include "SIM_Motor.h" #include "SIM_Frame.h" #include "SIM_Sprayer.h" +#include "SIM_Gripper_Servo.h" namespace SITL { @@ -48,8 +49,11 @@ protected: // The numbers here are offsets into the input servos array // (generally output-servo-number-1 e.g. 2 for throttle) Sprayer sprayer{6, 7}; + Gripper_Servo gripper{6}; + float gross_mass() const override; + }; }