From 485eb3fa88698665ced8ff7105269724b5e4bbf3 Mon Sep 17 00:00:00 2001 From: ashvath100 Date: Wed, 22 Jul 2020 19:12:56 +0530 Subject: [PATCH] Rover: roll and pitch input sent to motors added get desired roll and pitch Motors library adds set and get methods for roll and pitch added roll and pitch to get_control_output get_control_output returns roll and pitch values --- Rover/AP_MotorsUGV.cpp | 12 ++++++++++++ Rover/AP_MotorsUGV.h | 10 ++++++++++ Rover/Rover.cpp | 6 ++++++ Rover/Rover.h | 2 ++ Rover/mode.cpp | 18 ++++++++++++++++++ Rover/mode.h | 6 ++++++ Rover/mode_manual.cpp | 6 ++++++ Rover/radio.cpp | 12 ++++++++++++ 8 files changed, 72 insertions(+) diff --git a/Rover/AP_MotorsUGV.cpp b/Rover/AP_MotorsUGV.cpp index 33b8717d8e..51c1b478a2 100644 --- a/Rover/AP_MotorsUGV.cpp +++ b/Rover/AP_MotorsUGV.cpp @@ -210,6 +210,18 @@ void AP_MotorsUGV::set_lateral(float lateral) _lateral = constrain_float(lateral, -100.0f, 100.0f); } +// set roll input as a value from -1 to +1 +void AP_MotorsUGV::set_roll(float roll) +{ + _roll = constrain_float(roll, -1.0f, 1.0f); +} + +// set pitch input as a value from -1 to +1 +void AP_MotorsUGV::set_pitch(float pitch) +{ + _pitch = constrain_float(pitch, -1.0f, 1.0f); +} + // set mainsail input as a value from 0 to 100 void AP_MotorsUGV::set_mainsail(float mainsail) { diff --git a/Rover/AP_MotorsUGV.h b/Rover/AP_MotorsUGV.h index 4dd8a5a948..12da6507ff 100644 --- a/Rover/AP_MotorsUGV.h +++ b/Rover/AP_MotorsUGV.h @@ -60,6 +60,14 @@ public: float get_throttle() const { return _throttle; } void set_throttle(float throttle); + // get or set roll as a value from -1 to 1 + float get_roll() const { return _roll; } + void set_roll(float roll); + + // get or set pitch as a value from -1 to 1 + float get_pitch() const { return _pitch; } + void set_pitch(float pitch); + // get or set lateral input as a value from -100 to +100 float get_lateral() const { return _lateral; } void set_lateral(float lateral); @@ -182,6 +190,8 @@ protected: float _throttle_prev; // throttle input from previous iteration bool _scale_steering = true; // true if we should scale steering by speed or angle float _lateral; // requested lateral input as a value from -100 to +100 + float _roll; // requested roll as a value from -1 to +1 + float _pitch; // requested pitch as a value from -1 to +1 float _mainsail; // requested mainsail input as a value from 0 to 100 float _wingsail; // requested wing sail input as a value in the range +- 100 diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index a24a36bcd8..6f3d4049b7 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -185,6 +185,12 @@ bool Rover::set_steering_and_throttle(float steering, float throttle) bool Rover::get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) { switch (control_output) { + case AP_Vehicle::ControlOutput::Roll: + control_value = constrain_float(g2.motors.get_roll(), -1.0f, 1.0f); + return true; + case AP_Vehicle::ControlOutput::Pitch: + control_value = constrain_float(g2.motors.get_pitch(), -1.0f, 1.0f); + return true; case AP_Vehicle::ControlOutput::Throttle: control_value = constrain_float(g2.motors.get_throttle() / 100.0f, -1.0f, 1.0f); return true; diff --git a/Rover/Rover.h b/Rover/Rover.h index e4e8ef9add..e6f587fb73 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -143,6 +143,8 @@ private: RC_Channel *channel_steer; RC_Channel *channel_throttle; RC_Channel *channel_lateral; + RC_Channel *channel_roll; + RC_Channel *channel_pitch; AP_Logger logger; diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 5ad962eea4..9354e3aa5b 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -8,6 +8,8 @@ Mode::Mode() : channel_steer(rover.channel_steer), channel_throttle(rover.channel_throttle), channel_lateral(rover.channel_lateral), + channel_roll(rover.channel_roll), + channel_pitch(rover.channel_pitch), attitude_control(rover.g2.attitude_control) { } @@ -171,6 +173,22 @@ void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_ speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); } +// decode pilot roll and pitch inputs and return in roll_out and pitch_out arguments +// outputs are in the range -1 to +1 +void Mode::get_pilot_desired_roll_and_pitch(float &roll_out, float &pitch_out) +{ + if (channel_roll != nullptr) { + roll_out = channel_roll->norm_input(); + } else { + roll_out = 0.0f; + } + if (channel_pitch != nullptr) { + pitch_out = channel_pitch->norm_input(); + } else { + pitch_out = 0.0f; + } +} + // return heading (in degrees) to target destination (aka waypoint) float Mode::wp_bearing() const { diff --git a/Rover/mode.h b/Rover/mode.h index acf86e6e03..eb8a1b8332 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -153,6 +153,10 @@ protected: // decode pilot's input and return heading_out (in cd) and speed_out (in m/s) void get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out); + // decode pilot roll and pitch inputs and return in roll_out and pitch_out arguments + // outputs are in the range -1 to +1 + void get_pilot_desired_roll_and_pitch(float &roll_out, float &pitch_out); + // high level call to navigate to waypoint void navigate_to_waypoint(); @@ -197,6 +201,8 @@ protected: class RC_Channel *&channel_steer; class RC_Channel *&channel_throttle; class RC_Channel *&channel_lateral; + class RC_Channel *&channel_roll; + class RC_Channel *&channel_pitch; class AR_AttitudeControl &attitude_control; // private members for waypoint navigation diff --git a/Rover/mode_manual.cpp b/Rover/mode_manual.cpp index 81959317bf..00eafa3b67 100644 --- a/Rover/mode_manual.cpp +++ b/Rover/mode_manual.cpp @@ -18,6 +18,12 @@ void ModeManual::update() rover.balancebot_pitch_control(desired_throttle); } + // walking robots support roll and pitch + float desired_roll, desired_pitch; + get_pilot_desired_roll_and_pitch(desired_roll, desired_pitch); + g2.motors.set_roll(desired_roll); + g2.motors.set_pitch(desired_pitch); + // set sailboat sails float desired_mainsail; float desired_wingsail; diff --git a/Rover/radio.cpp b/Rover/radio.cpp index 80f43b4766..c839ababeb 100644 --- a/Rover/radio.cpp +++ b/Rover/radio.cpp @@ -17,6 +17,18 @@ void Rover::set_control_channels(void) channel_lateral->set_angle(100); } + // walking robots rc input init + channel_roll = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ROLL); + channel_pitch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::PITCH); + if (channel_roll != nullptr) { + channel_roll->set_angle(SERVO_MAX); + channel_roll->set_default_dead_zone(30); + } + if (channel_pitch != nullptr) { + channel_pitch->set_angle(SERVO_MAX); + channel_pitch->set_default_dead_zone(30); + } + // sailboat rc input init g2.sailboat.init_rc_in();