Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   simple Gripper (Servo) simulation class
      17             : */
      18             : 
      19             : #include "SIM_Gripper_Servo.h"
      20             : #include "AP_HAL/AP_HAL.h"
      21             : #include "AP_Math/AP_Math.h"
      22             : #include <stdio.h>
      23             : 
      24             : extern const AP_HAL::HAL& hal;
      25             : 
      26             : using namespace SITL;
      27             : 
      28             : // table of user settable parameters
      29             : const AP_Param::GroupInfo Gripper_Servo::var_info[] = {
      30             : 
      31             :     // @Param: ENABLE
      32             :     // @DisplayName: Gripper servo Sim enable/disable
      33             :     // @Description: Allows you to enable (1) or disable (0) the gripper servo simulation
      34             :     // @Values: 0:Disabled,1:Enabled
      35             :     // @User: Advanced
      36             :     AP_GROUPINFO("ENABLE", 0, Gripper_Servo, gripper_enable, 0),
      37             : 
      38             :     // @Param: PIN
      39             :     // @DisplayName: Gripper servo pin
      40             :     // @Description: The pin number that the gripper servo is connected to. (start at 1)
      41             :     // @Range: 0 15
      42             :     // @User: Advanced
      43             :     AP_GROUPINFO("PIN", 1, Gripper_Servo, gripper_servo_pin, -1),
      44             : 
      45             :     // @Param: GRAB
      46             :     // @DisplayName: Gripper Grab PWM
      47             :     // @Description: PWM value in microseconds sent to Gripper to initiate grabbing the cargo
      48             :     // @User: Advanced
      49             :     // @Range: 1000 2000
      50             :     // @Units: PWM
      51             :     AP_GROUPINFO("GRAB", 2, Gripper_Servo, grab_pwm, SIM_GRIPPER_GRAB_PWM_DEFAULT),
      52             : 
      53             :     // @Param: RELEASE
      54             :     // @DisplayName: Gripper Release PWM
      55             :     // @Description: PWM value in microseconds sent to Gripper to release the cargo
      56             :     // @User: Advanced
      57             :     // @Range: 1000 2000
      58             :     // @Units: PWM
      59             :     AP_GROUPINFO("RELEASE", 3, Gripper_Servo, release_pwm, SIM_GRIPPER_RELEASE_PWM_DEFAULT),
      60             : 
      61             :     // @Param: REVERSE
      62             :     // @DisplayName: Gripper close direction
      63             :     // @Description: Reverse the closing direction.
      64             :     // @User: Advanced
      65             :     // @Values: 0:Normal,1:Reverse
      66             :     AP_GROUPINFO("REVERSE", 4, Gripper_Servo, reverse, 0),
      67             : 
      68             :     AP_GROUPEND
      69             : };
      70             : 
      71             : /*
      72             :   update gripper state
      73             :  */
      74      810128 : void Gripper_Servo::update(const struct sitl_input &input)
      75             : {
      76      810128 :     const int16_t gripper_pwm = gripper_servo_pin >= 1 ? input.servos[gripper_servo_pin-1] : -1;
      77             : 
      78      810128 :     const uint64_t now = AP_HAL::micros64();
      79      810128 :     const float dt = (now - last_update_us) * 1.0e-6f;
      80             : 
      81             :     // update gripper position
      82      810128 :     if (gripper_pwm < 0) {
      83         462 :         last_update_us = now;
      84         462 :         return;
      85             :     }
      86      809666 :     const int16_t diff_pwm = abs(grab_pwm - release_pwm);
      87      809666 :     float position_demand = (gripper_pwm - diff_pwm) * 0.001f;
      88      809666 :     if (gripper_pwm < MIN(grab_pwm, release_pwm) || position_demand > 1.0f) { // never updated
      89       40646 :         position_demand = position;
      90             :     }
      91             : 
      92      809666 :     const float position_max_change = position_slew_rate * 0.01f * dt;
      93      809666 :     position = constrain_float(position_demand, position - position_max_change, position + position_max_change);
      94             :     float jaw_gap;
      95      809666 :     if ((release_pwm < grab_pwm && reverse) || (release_pwm > grab_pwm && !reverse)) {
      96           0 :         jaw_gap = gap * position;
      97             :     } else {
      98      809666 :         jaw_gap = gap * (1.0f - position);
      99             :     }
     100      809666 :     if (should_report()) {
     101          59 :         hal.console->printf("position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass);
     102          59 :         last_report_us = now;
     103          59 :         reported_position = position;
     104             :     }
     105             : 
     106      809666 :     if (jaw_gap < 5) {
     107           0 :         if (altitude <= 0.0f) {
     108           0 :             load_mass = 1.0f; // attach the load
     109           0 :             jaw_open = false;
     110             :         }
     111      809666 :     } else if (jaw_gap > 10) {
     112      809666 :         load_mass = 0.0f; // detach the load
     113      809666 :         jaw_open = true;
     114             :     }
     115             : 
     116      809666 :     last_update_us = now;
     117             : }
     118             : 
     119      809666 : bool Gripper_Servo::should_report() const
     120             : {
     121      809666 :     if (AP_HAL::micros64() - last_report_us < report_interval) {
     122       78641 :         return false;
     123             :     }
     124             : 
     125      731025 :     if (!is_equal(reported_position, position)) {
     126          59 :         return true;
     127             :     }
     128             : 
     129      730966 :     return false;
     130             : }
     131             : 
     132             : 
     133      810128 : float Gripper_Servo::payload_mass() const
     134             : {
     135      810128 :     if (altitude < string_length) {
     136      502996 :         return 0.0f;
     137             :     }
     138      307132 :     return load_mass;
     139             : }