ardupilot/libraries/AP_Gripper/AP_Gripper_EPM.cpp

145 lines
4.2 KiB
C++
Raw Normal View History

2013-12-17 00:59:14 -04:00
/*
* AP_Gripper_EPM.cpp
2013-12-17 00:59:14 -04:00
*
* Created on: DEC 6, 2013
* Author: Andreas Jochum
2016-06-23 20:48:29 -03:00
* Pavel Kirienko <pavel.kirienko@zubax.com> - UAVCAN support
* Peter Barker - moved into AP_Gripper_EPM
2013-12-17 00:59:14 -04:00
*/
#include "AP_Gripper_EPM.h"
#include <AP_HAL/AP_HAL.h>
2016-06-23 20:48:29 -03:00
#include <AP_BoardConfig/AP_BoardConfig.h>
2018-08-09 14:08:55 -03:00
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
2018-01-05 03:33:46 -04:00
#ifdef UAVCAN_NODE_FILE
2016-06-23 20:48:29 -03:00
#include <fcntl.h>
2018-01-05 03:33:46 -04:00
#include <stdio.h>
#endif
2013-12-17 00:59:14 -04:00
extern const AP_HAL::HAL& hal;
AP_Gripper_EPM::AP_Gripper_EPM(struct AP_Gripper::Backend_Config &_config) :
AP_Gripper_Backend(_config) { }
2013-12-17 00:59:14 -04:00
void AP_Gripper_EPM::init_gripper()
2013-12-17 00:59:14 -04:00
{
2016-06-26 15:23:34 -03:00
#ifdef UAVCAN_NODE_FILE
_uavcan_fd = ::open(UAVCAN_NODE_FILE, O_CLOEXEC);
// https://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html
2016-06-23 20:48:29 -03:00
::printf("EPM: UAVCAN fd %d\n", _uavcan_fd);
2016-06-26 15:23:34 -03:00
#endif
2016-06-23 20:48:29 -03:00
// initialise the EPM to the neutral position
2013-12-17 00:59:14 -04:00
neutral();
}
// Refer to http://uavcan.org/Specification/7._List_of_standard_data_types/#uavcanequipmenthardpoint
struct UAVCANCommand {
uint8_t hardpoint_id = 0;
uint16_t command = 0;
};
// grab - move the epm pwm output to the grab position
void AP_Gripper_EPM::grab()
2013-12-17 00:59:14 -04:00
{
// flag we are active and grabbing cargo
config.state = AP_Gripper::STATE_GRABBING;
// capture time
_last_grab_or_release = AP_HAL::millis();
#ifdef UAVCAN_IOCS_HARDPOINT_SET
2016-06-23 20:48:29 -03:00
if (should_use_uavcan()) {
const UAVCANCommand cmd = make_uavcan_command(1);
(void)ioctl(_uavcan_fd, UAVCAN_IOCS_HARDPOINT_SET, reinterpret_cast<unsigned long>(&cmd));
}
else
#endif
{
// move the servo output to the grab position
2017-01-06 21:06:40 -04:00
SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
2016-06-23 20:48:29 -03:00
}
2018-08-09 14:08:55 -03:00
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
AP::logger().Write_Event(LogEvent::GRIPPER_GRAB);
2013-12-17 00:59:14 -04:00
}
// release - move epm pwm output to the release position
void AP_Gripper_EPM::release()
2013-12-17 00:59:14 -04:00
{
// flag we are releasing cargo
config.state = AP_Gripper::STATE_RELEASING;
// capture time
_last_grab_or_release = AP_HAL::millis();
#ifdef UAVCAN_IOCS_HARDPOINT_SET
2016-06-23 20:48:29 -03:00
if (should_use_uavcan()) {
const UAVCANCommand cmd = make_uavcan_command(0);
(void)ioctl(_uavcan_fd, UAVCAN_IOCS_HARDPOINT_SET, reinterpret_cast<unsigned long>(&cmd));
}
else
#endif
{
2016-06-23 20:48:29 -03:00
// 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);
2016-06-23 20:48:29 -03:00
}
2018-08-09 14:08:55 -03:00
gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing");
AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE);
2013-12-17 00:59:14 -04:00
}
// neutral - return the epm pwm output to the neutral position
void AP_Gripper_EPM::neutral()
2013-12-17 00:59:14 -04:00
{
2016-06-23 20:48:29 -03:00
if (!should_use_uavcan()) {
// move the servo to the off position
2017-01-06 21:06:40 -04:00
SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.neutral_pwm);
2016-06-23 20:48:29 -03:00
}
}
// update - moves the pwm back to neutral after the timeout has passed
// should be called at at least 10hz
void AP_Gripper_EPM::update_gripper()
{
// move EPM PWM output back to neutral after the last grab or release
if (AP_HAL::millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS) {
if (config.state == AP_Gripper::STATE_GRABBING) {
neutral();
config.state = AP_Gripper::STATE_GRABBED;
} else if (config.state == AP_Gripper::STATE_RELEASING) {
neutral();
config.state = AP_Gripper::STATE_RELEASED;
}
}
// re-grab the cargo intermittently
if (config.state == AP_Gripper::STATE_GRABBED &&
(config.regrab_interval > 0) &&
(AP_HAL::millis() - _last_grab_or_release > ((uint32_t)config.regrab_interval * 1000))) {
grab();
}
}
UAVCANCommand AP_Gripper_EPM::make_uavcan_command(uint16_t command) const
{
UAVCANCommand cmd;
cmd.hardpoint_id = config.uavcan_hardpoint_id;
cmd.command = command;
return cmd;
}
bool AP_Gripper_EPM::released() const
{
// we assume instanteous releasing ATM:
return (config.state == AP_Gripper::STATE_GRABBED ||
config.state == AP_Gripper::STATE_GRABBING);
}
bool AP_Gripper_EPM::grabbed() const
{
// we assume instanteous grabbing ATM:
return (config.state == AP_Gripper::STATE_GRABBED ||
config.state == AP_Gripper::STATE_GRABBING);
}