From d9cbcd948707b9e38a0d12d87e0497f28630fe07 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 30 Oct 2016 17:00:04 +1100 Subject: [PATCH] SITL: a simulator for the EPM grabber --- libraries/SITL/SIM_Gripper_EPM.cpp | 99 ++++++++++++++++++++++++++++++ libraries/SITL/SIM_Gripper_EPM.h | 63 +++++++++++++++++++ libraries/SITL/SIM_Multicopter.cpp | 1 + libraries/SITL/SIM_Multicopter.h | 6 +- 4 files changed, 167 insertions(+), 2 deletions(-) create mode 100644 libraries/SITL/SIM_Gripper_EPM.cpp create mode 100644 libraries/SITL/SIM_Gripper_EPM.h diff --git a/libraries/SITL/SIM_Gripper_EPM.cpp b/libraries/SITL/SIM_Gripper_EPM.cpp new file mode 100644 index 0000000000..2adb28c55a --- /dev/null +++ b/libraries/SITL/SIM_Gripper_EPM.cpp @@ -0,0 +1,99 @@ +/* + 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 (OpenGrab EPM) simulation class +*/ + +#include "SIM_Gripper_EPM.h" +#include +#include + +using namespace SITL; + +/* + update gripper state + */ +void Gripper_EPM::update_servobased(const Aircraft::sitl_input &input) +{ + if (! servo_based) { + return; + } + demand = (input.servos[gripper_servo]-1000) * 0.001f; // 0.0 - 1.0 + if (demand < 0) { // never updated + demand = 0; + } +} + + +void Gripper_EPM::update_from_demand(const Aircraft::sitl_input &input) +{ + const uint64_t now = AP_HAL::micros64(); + const float dt = (now - last_update_us) * 1.0e-6f; + + // decay the field + field_strength = field_strength * (100-field_decay_rate * dt)/100.0f; + + // note that "demand" here is just an on/off switch; we only care + // about which range it falls into + if (demand > 0.6) { + // we are instructed to grip harder + field_strength = field_strength + (100.0f-field_strength) * field_strength_slew_rate/100.0f * dt; + } else if (demand < 0.4) { + // we are instructed to loosen grip + field_strength = field_strength * (100-field_degauss_rate * dt)/100.0f; + } else { + // neutral; no demanded change + } + + if (should_report()) { + ::fprintf(stderr, "demand=%f\n", demand); + printf("Field strength: %f%%\n", field_strength); + printf("Field strength: %f Tesla\n", tesla()); + last_report_us = now; + reported_field_strength = field_strength; + } + + last_update_us = now; + return; +} + +void Gripper_EPM::update(const Aircraft::sitl_input &input) +{ + update_servobased(input); + + update_from_demand(input); +} + + +bool Gripper_EPM::should_report() +{ + if (AP_HAL::micros64() - last_report_us < report_interval) { + return false; + } + + if (fabs(reported_field_strength - field_strength) > 10.0f) { + return true; + } + + return false; +} + +float Gripper_EPM::tesla() +{ + // https://en.wikipedia.org/wiki/Orders_of_magnitude_(magnetic_field) + // 200N lifting capacity ~= 2.5T + const float percentage_to_tesla = 0.25; + return percentage_to_tesla * field_strength/100.0f; +} diff --git a/libraries/SITL/SIM_Gripper_EPM.h b/libraries/SITL/SIM_Gripper_EPM.h new file mode 100644 index 0000000000..028cc1dae5 --- /dev/null +++ b/libraries/SITL/SIM_Gripper_EPM.h @@ -0,0 +1,63 @@ +/* + 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 (EPM) simulation class +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +namespace SITL { + +class Gripper_EPM { +public: + const uint8_t gripper_servo; + + Gripper_EPM(const uint8_t _gripper_servo) : + gripper_servo(_gripper_servo) + {} + + // update field stength + void update(const struct Aircraft::sitl_input &input); + +private: + + const uint32_t report_interval = 100000; // microseconds + uint64_t last_report_us; + + bool servo_based = true; + + double field_strength; // percentage + double reported_field_strength = -1; // unlikely + + // I've a feeling these are probably a higher order than this: + const float field_strength_slew_rate = 400; // (percentage of delta between field strength and 100)/second + const float field_decay_rate = 2; // percent of field strength/second + const float field_degauss_rate = 300; // percent of field strength/second + + uint64_t last_update_us; + + bool should_report(); + + void update_from_demand(const Aircraft::sitl_input &input); + void update_servobased(const struct Aircraft::sitl_input &input); + + float tesla(); + + float demand; +}; + +} diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index ad0821ef69..f1ab2687d3 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -76,6 +76,7 @@ void MultiCopter::update(const struct sitl_input &input) // update gripper gripper.update(input); + gripper_epm.update(input); } float MultiCopter::gross_mass() const diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index 0a9f4c5884..14c0ece500 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -21,8 +21,10 @@ #include "SIM_Aircraft.h" #include "SIM_Motor.h" #include "SIM_Frame.h" + #include "SIM_Sprayer.h" #include "SIM_Gripper_Servo.h" +#include "SIM_Gripper_EPM.h" namespace SITL { @@ -49,8 +51,8 @@ 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}; - + Gripper_Servo gripper{8}; + Gripper_EPM gripper_epm{9}; float gross_mass() const override;