From 76c9d3982a71a5c1482d6324c8c6395fa490a9ec Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 6 Jul 2017 12:07:12 +0900 Subject: [PATCH] AP_MotorsUGV: motors lib for Rover --- APMrover2/AP_MotorsUGV.cpp | 256 +++++++++++++++++++++++++++++++++++++ APMrover2/AP_MotorsUGV.h | 70 ++++++++++ 2 files changed, 326 insertions(+) create mode 100644 APMrover2/AP_MotorsUGV.cpp create mode 100644 APMrover2/AP_MotorsUGV.h diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp new file mode 100644 index 0000000000..d0e9cdb756 --- /dev/null +++ b/APMrover2/AP_MotorsUGV.cpp @@ -0,0 +1,256 @@ +/* + 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 . + */ + +#include +#include "SRV_Channel/SRV_Channel.h" +#include "AP_MotorsUGV.h" +#include "Rover.h" + +extern const AP_HAL::HAL& hal; + +// parameters for the motor class +const AP_Param::GroupInfo AP_MotorsUGV::var_info[] = { + // @Param: PWM_TYPE + // @DisplayName: Output PWM type + // @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot or brushed motor output + // @Values: 0:Normal,1:OneShot,2:OneShot125,3:Brushed + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("PWM_TYPE", 1, AP_MotorsUGV, _pwm_type, PWM_TYPE_NORMAL), + + // @Param: PWM_FREQ + // @DisplayName: Output PWM freq for brushed motors + // @Description: Output PWM freq for brushed motors + // @Units: kHz + // @Range: 1 20 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("PWM_FREQ", 2, AP_MotorsUGV, _pwm_freq, 16), + + // @Param: SAFE_DISARM + // @DisplayName: Motor PWM output disabled when disarmed + // @Description: Disables motor PWM output when disarmed + // @Values: 0:PWM enabled while disarmed, 1:PWM disabled while disarmed + // @User: Advanced + AP_GROUPINFO("SAFE_DISARM", 3, AP_MotorsUGV, _disarm_disable_pwm, 0), + + // @Param: THR_SLEWRATE + // @DisplayName: Throttle slew rate + // @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit. A value of 100 means the throttle can change over its full range in one second. Note that for some NiMH powered rovers setting a lower value like 40 or 50 may be worthwhile as the sudden current demand on the battery of a big rise in throttle may cause a brownout. + // @Units: %/s + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("SLEWRATE", 4, AP_MotorsUGV, _slew_rate, 100), + + AP_GROUPEND +}; + +AP_MotorsUGV::AP_MotorsUGV() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void AP_MotorsUGV::init() +{ + // k_steering are limited to -45;45 degree + SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX); + + // k_throttle are in power percent so -100 ... 100 + SRV_Channels::set_angle(SRV_Channel::k_throttle, 100); + + // skid steering left/right throttle as -1000 to 1000 values + SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 1000); + SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000); + + // setup pwm type + setup_pwm_type(); + + // set safety output + setup_safety_output(); +} + +/* + work out if skid steering is available + */ +bool AP_MotorsUGV::have_skid_steering() const +{ + if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) && + SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) { + return true; + } + return false; +} + +void AP_MotorsUGV::output(bool armed, float dt) +{ + // soft-armed overrides passed in armed status + if (!hal.util->get_soft_armed()) { + armed = false; + } + + // ensure steering and throttle are within limits + _steering = constrain_float(_steering, -4500.0f, 4500.0f); + _throttle = constrain_float(_throttle, -100.0f, 100.0f); + + slew_limit_throttle(dt); + + // output for regular steering/throttle style frames + output_regular(armed, _steering, _throttle); + + // output for skid steering style frames + output_skid_steering(armed, _steering, _throttle); + + // send values to the PWM timers for output + SRV_Channels::calc_pwm(); + hal.rcout->cork(); + SRV_Channels::output_ch_all(); + hal.rcout->push(); + _last_throttle = _throttle; +} + +// output to regular steering and throttle channels +void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle) +{ + // always allow steering to move + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering); + + // output to throttle channels + if (armed) { + // handle armed case + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); + } else { + // handle disarmed case + if (_disarm_disable_pwm) { + SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + } else { + SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + } + } +} + +// output to skid steering channels +void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle) +{ + // handle simpler disarmed case + if (!armed) { + if (_disarm_disable_pwm) { + SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + } else { + SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + } + return; + } + + // skid steering mixer + float steering_scaled = steering / 4500.0f; // steering scaled -1 to +1 + float throttle_scaled = throttle / 100.0f; // throttle scaled -1 to +1 + + // apply constraints + steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f); + throttle_scaled = constrain_float(throttle_scaled, -1.0f, 1.0f); + + // check for saturation and scale back throttle and steering proportionally + const float saturation_value = fabsf(steering_scaled) + fabsf(throttle_scaled); + if (saturation_value > 1.0f) { + steering_scaled = steering_scaled / saturation_value; + throttle_scaled = throttle_scaled / saturation_value; + } + + // add in throttle + float motor_left = throttle_scaled; + float motor_right = throttle_scaled; + + // deal with case of turning on the spot + if (is_zero(throttle_scaled)) { + // full possible range is not used to keep response equivalent to non-zero throttle case + motor_left += steering_scaled * 0.5f; + motor_right -= steering_scaled * 0.5f; + } else { + // add in steering + const float dir = is_positive(throttle_scaled) ? 1.0f : -1.0f; + if (is_negative(steering_scaled)) { + // moving left all steering to right wheel + motor_right -= dir * steering_scaled; + } else { + // turning right, all steering to left wheel + motor_left += dir * steering_scaled; + } + } + + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000.0f * motor_left); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motor_right); +} + +// slew limit throttle for one iteration +void AP_MotorsUGV::slew_limit_throttle(float dt) +{ + if (_use_slew_rate && (_slew_rate > 0)) { + float temp = _slew_rate * dt * 0.01f * 100; // TODO : get THROTTLE MIN and THROTTLE MAX + if (temp < 1) { + temp = 1; + } + _throttle = constrain_int16(_throttle, _last_throttle - temp, _last_throttle + temp); + } +} + +// setup pwm output type +void AP_MotorsUGV::setup_pwm_type() +{ + switch (_pwm_type) { + case PWM_TYPE_ONESHOT: + case PWM_TYPE_ONESHOT125: + // tell HAL to do immediate output + hal.rcout->set_output_mode(AP_HAL::RCOutput::MODE_PWM_ONESHOT); + break; + case PWM_TYPE_BRUSHED: + hal.rcout->set_output_mode(AP_HAL::RCOutput::MODE_PWM_BRUSHED); + /* + * Group 0: channels 0 1 + * Group 1: channels 4 5 6 7 + * Group 2: channels 2 3 + */ + // TODO : See if we can seperate frequency between groups + hal.rcout->set_freq((1UL << 0), static_cast(_pwm_freq * 1000)); // Steering group + hal.rcout->set_freq((1UL << 2), static_cast(_pwm_freq * 1000)); // Throttle group + break; + default: + // do nothing + break; + } +} + +// setup output in case of main CPU failure +void AP_MotorsUGV::setup_safety_output() +{ + if (_pwm_type == PWM_TYPE_BRUSHED) { + // set trim to min to set duty cycle range (0 - 100%) to servo range + SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleLeft); + SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleRight); + SRV_Channels::setup_failsafe_trim_all(); + } + if (_disarm_disable_pwm) { + // throttle channels output zero pwm (i.e. no signal) + SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + SRV_Channels::set_safety_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + SRV_Channels::set_safety_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + } else { + // throttle channels output trim values (because rovers will go backwards if set to MIN) + SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_safety_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_safety_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + } +} diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h new file mode 100644 index 0000000000..fa5c03a1df --- /dev/null +++ b/APMrover2/AP_MotorsUGV.h @@ -0,0 +1,70 @@ +#pragma once + +#include "defines.h" +#include "AP_Arming.h" + +class AP_MotorsUGV { +public: + + // Constructor + AP_MotorsUGV(); + + enum pwm_type { + PWM_TYPE_NORMAL = 0, + PWM_TYPE_ONESHOT = 1, + PWM_TYPE_ONESHOT125 = 2, + PWM_TYPE_BRUSHED = 3 + }; + + // initialise motors + void init(); + + // setup output in case of main CPU failure + void setup_safety_output(); + + // set steering as a value from -4500 to +4500 + float get_steering() const { return _steering; } + void set_steering(float steering) { _steering = steering; } + + // get or set throttle as a value from 0 to 100 + float get_throttle() const { return _throttle; } + void set_throttle(float throttle) { _throttle = throttle; } + + // true if vehicle is capable of skid steering + bool have_skid_steering() const; + + // output to motors and steering servos + void output(bool armed, float dt); + + // set when to use slew rate limiter + void slew_limit_throttle(bool value) { _use_slew_rate = value; } + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + // setup pwm output type + void setup_pwm_type(); + + // output to regular steering and throttle channels + void output_regular(bool armed, float steering, float throttle); + + // output to skid steering channels + void output_skid_steering(bool armed, float steering, float throttle); + + // slew limit throttle for one iteration + void slew_limit_throttle(float dt); + + // parameters + AP_Int8 _pwm_type; // PWM output type + AP_Int8 _pwm_freq; // PWM output freq for brushed motors + AP_Int8 _disarm_disable_pwm; // disable PWM output while disarmed + AP_Int8 _slew_rate; // slew rate expressed as a percentage / second + + // internal variables + float _steering; // requested steering as a value from -4500 to +4500 + float _throttle; // requested throttle as a value from 0 to 100 + float _last_throttle; + bool _use_slew_rate; // true if we should slew limit the throttle for one interation +};