From a0b9b4ebd0abd4b114f912ca6faa98bd1969f93b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 30 May 2023 19:59:01 +0900 Subject: [PATCH] Rover: implement get_steering_and_throttle --- Rover/Rover.cpp | 8 ++++++++ Rover/Rover.h | 1 + 2 files changed, 9 insertions(+) diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index f8a3ef7800..ec8f77801d 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -204,6 +204,14 @@ bool Rover::set_steering_and_throttle(float steering, float throttle) return true; } +// get steering and throttle (-1 to +1) (for use by scripting) +bool Rover::get_steering_and_throttle(float& steering, float& throttle) +{ + steering = g2.motors.get_steering() / 4500.0; + throttle = g2.motors.get_throttle() * 0.01; + return true; +} + // set desired turn rate (degrees/sec) and speed (m/s). Used for scripting bool Rover::set_desired_turn_rate_and_speed(float turn_rate, float speed) { diff --git a/Rover/Rover.h b/Rover/Rover.h index 3310809815..4c127dc351 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -257,6 +257,7 @@ private: bool set_target_location(const Location& target_loc) override; bool set_target_velocity_NED(const Vector3f& vel_ned) override; bool set_steering_and_throttle(float steering, float throttle) override; + bool get_steering_and_throttle(float& steering, float& throttle) override; // set desired turn rate (degrees/sec) and speed (m/s). Used for scripting bool set_desired_turn_rate_and_speed(float turn_rate, float speed) override; bool set_desired_speed(float speed) override;