|
|
|
@ -11,22 +11,22 @@ void AP_Gripper_Servo::init_gripper()
@@ -11,22 +11,22 @@ void AP_Gripper_Servo::init_gripper()
|
|
|
|
|
void AP_Gripper_Servo::grab() |
|
|
|
|
{ |
|
|
|
|
// move the servo to the grab position
|
|
|
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.grab_pwm); |
|
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm); |
|
|
|
|
action_timestamp = AP_HAL::millis(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AP_Gripper_Servo::release() |
|
|
|
|
{ |
|
|
|
|
// move the servo to the release position
|
|
|
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.release_pwm); |
|
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm); |
|
|
|
|
action_timestamp = AP_HAL::millis(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const |
|
|
|
|
{ |
|
|
|
|
// return true if servo is in position represented by pwm
|
|
|
|
|
int16_t current_pwm; |
|
|
|
|
if (!RC_Channel_aux::get_radio(RC_Channel_aux::k_gripper, current_pwm)) { |
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
@ -61,7 +61,7 @@ bool AP_Gripper_Servo::valid() const
@@ -61,7 +61,7 @@ bool AP_Gripper_Servo::valid() const
|
|
|
|
|
if (!AP_Gripper_Backend::valid()) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_gripper)) { |
|
|
|
|
if (!SRV_Channels::function_assigned(SRV_Channel::k_gripper)) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
return true; |
|
|
|
|