5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

SITL: a simulator for the EPM grabber

This commit is contained in:
Peter Barker 2016-10-30 17:00:04 +11:00 committed by Randy Mackay
parent 7200692137
commit d9cbcd9487
4 changed files with 167 additions and 2 deletions

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
simple Gripper (OpenGrab EPM) simulation class
*/
#include "SIM_Gripper_EPM.h"
#include <stdio.h>
#include <AP_Common/AP_Common.h>
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;
}

View File

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

View File

@ -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

View File

@ -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;