ardupilot/libraries/AP_Gripper/AP_Gripper_Servo.cpp

98 lines
2.5 KiB
C++
Raw Normal View History

2016-10-28 19:05:19 -03:00
#include <AP_Gripper/AP_Gripper_Servo.h>
2018-08-09 14:08:55 -03:00
#include <GCS_MAVLink/GCS.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#endif
2016-10-28 19:05:19 -03:00
extern const AP_HAL::HAL& hal;
void AP_Gripper_Servo::init_gripper()
{
// move the servo to the release position
release();
2016-10-28 19:05:19 -03:00
}
void AP_Gripper_Servo::grab()
{
// move the servo to the grab position
2017-01-06 21:06:40 -04:00
SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
action_timestamp = AP_HAL::millis();
2018-08-09 14:08:55 -03:00
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
is_releasing = false;
is_released = true;
#endif
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
2016-10-28 19:05:19 -03:00
}
void AP_Gripper_Servo::release()
{
// move the servo to the release position
2017-01-06 21:06:40 -04:00
SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm);
action_timestamp = AP_HAL::millis();
2018-08-09 14:08:55 -03:00
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
is_releasing = true;
is_released = false;
#endif
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing");
}
bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const
{
// return true if servo is in position represented by pwm
2017-01-06 21:06:40 -04:00
uint16_t current_pwm;
if (!SRV_Channels::get_output_pwm(SRV_Channel::k_gripper, current_pwm)) {
// function not assigned to a channel, perhaps?
return false;
}
if (current_pwm != pwm) {
// last action did not set pwm to the current value
2016-12-18 23:14:19 -04:00
// (e.g. last action was a grab not a release)
return false;
}
if (AP_HAL::millis() - action_timestamp < action_time) {
// servo still moving....
return false;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (is_releasing) {
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load released");
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbed");
}
2018-08-09 14:08:55 -03:00
#endif
return true;
}
bool AP_Gripper_Servo::released() const
{
return has_state_pwm(config.release_pwm);
}
bool AP_Gripper_Servo::grabbed() const
{
return has_state_pwm(config.grab_pwm);
2016-10-28 19:05:19 -03:00
}
// type-specific periodic updates:
2018-08-09 14:08:55 -03:00
void AP_Gripper_Servo::update_gripper() {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (is_releasing && !is_released) {
is_released = released();
} else if (!is_releasing && is_released) {
is_released = !grabbed();
}
#endif
};
bool AP_Gripper_Servo::valid() const
{
if (!AP_Gripper_Backend::valid()) {
return false;
}
2017-01-06 21:06:40 -04:00
if (!SRV_Channels::function_assigned(SRV_Channel::k_gripper)) {
return false;
}
return true;
}