mirror of https://github.com/ArduPilot/ardupilot
124 lines
3.5 KiB
C++
124 lines
3.5 KiB
C++
/*
|
|
* AP_Gripper_EPM.cpp
|
|
*
|
|
* Created on: DEC 6, 2013
|
|
* Author: Andreas Jochum
|
|
* Pavel Kirienko <pavel.kirienko@zubax.com> - UAVCAN support
|
|
* Peter Barker - moved into AP_Gripper_EPM
|
|
*/
|
|
|
|
#include "AP_Gripper_EPM.h"
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <cstdio>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
AP_Gripper_EPM::AP_Gripper_EPM(struct AP_Gripper::Backend_Config &_config) :
|
|
AP_Gripper_Backend(_config) { }
|
|
|
|
void AP_Gripper_EPM::init_gripper()
|
|
{
|
|
#ifdef UAVCAN_NODE_FILE
|
|
_uavcan_fd = ::open(UAVCAN_NODE_FILE, O_CLOEXEC);
|
|
// http://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html
|
|
::printf("EPM: UAVCAN fd %d\n", _uavcan_fd);
|
|
#endif
|
|
|
|
// initialise the EPM to the neutral position
|
|
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()
|
|
{
|
|
// 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
|
|
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 to the release position
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.grab_pwm);
|
|
}
|
|
}
|
|
|
|
// release - move epm pwm output to the release position
|
|
void AP_Gripper_EPM::release()
|
|
{
|
|
// 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
|
|
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
|
|
{
|
|
// move the servo to the release position
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.release_pwm);
|
|
}
|
|
}
|
|
|
|
// neutral - return the epm pwm output to the neutral position
|
|
void AP_Gripper_EPM::neutral()
|
|
{
|
|
if (!should_use_uavcan()) {
|
|
// move the servo to the off position
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.neutral_pwm);
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|