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:
parent
7200692137
commit
d9cbcd9487
99
libraries/SITL/SIM_Gripper_EPM.cpp
Normal file
99
libraries/SITL/SIM_Gripper_EPM.cpp
Normal 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;
|
||||
}
|
63
libraries/SITL/SIM_Gripper_EPM.h
Normal file
63
libraries/SITL/SIM_Gripper_EPM.h
Normal 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;
|
||||
};
|
||||
|
||||
}
|
@ -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
|
||||
|
@ -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;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user