SITL: SIM_Gripper_Servo, a simulated servo gripper

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple Gripper (Servo) simulation class
+*/
+
+#include "SIM_Gripper_Servo.h"
+#include <stdio.h>
+
+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;
+}
+

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  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;
+};
+
+}

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

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;
+
 };
 
 }