Work on Joystick Class

include/qrw/Joystick.hpp

@@ -15,6 +15,7 @@
 #include <Eigen/Core>
 #include <Eigen/Dense>
 #include "qrw/Types.h"
+#include "qrw/Params.hpp"
 
 class Joystick {
  public:
@@ -25,6 +26,15 @@ class Joystick {
   ////////////////////////////////////////////////////////////////////////////////////////////////
   Joystick();
 
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Initialize with given data
+  ///
+  /// \param[in] params Object that stores parameters
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void initialize(Params& params);
+
   ////////////////////////////////////////////////////////////////////////////////////////////////
   ///
   /// \brief Destructor.
@@ -45,6 +55,7 @@ class Joystick {
   VectorN handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch);
 
   void update_v_ref(int k, int velID);
+  void update_v_ref_gamepad();
 
   Vector6 getVRef() { return v_ref_; }
   int getJoystickCode() { return joystick_code_; }
@@ -54,8 +65,19 @@ class Joystick {
   Vector6 A3_;     // Third order coefficient of the polynomial that generates the velocity profile
   Vector6 A2_;     // Second order coefficient of the polynomial that generates the velocity profile
   Vector6 v_ref_;  // Reference velocity resulting of the polynomial interpolation
+  Vector6 v_gp_;
   int joystick_code_ = 0;
   bool stop_ = false;
+  bool predefined = false;
+
+  // How much the gamepad velocity is filtered to avoid sharp changes
+  double alpha = 0.001;
+
+  // Maximum velocity values
+  double vXScale = 0.6;  // Lateral
+  double vYScale = 1.0;  // Forward
+  double vYawScale = 1.2;  // Rotation
+
 };
 
 #endif  // JOYSTICK_H_INCLUDED

src/Joystick.cpp

 #include "qrw/Joystick.hpp"
 
-Joystick::Joystick() : A3_(Vector6::Zero()), A2_(Vector6::Zero()), v_ref_(Vector6::Zero()) {}
+Joystick::Joystick() : A3_(Vector6::Zero()), A2_(Vector6::Zero()),
+                       v_ref_(Vector6::Zero()), v_gp_(Vector6::Zero()) {}
+
+void Joystick::initialize(Params &params)
+{
+  predefined = params.predefined_vel;
+}
 
 VectorN Joystick::handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch) {
   int i = 1;
@@ -19,5 +25,33 @@ VectorN Joystick::handle_v_switch(double k, VectorN const& k_switch, MatrixN con
 
 void Joystick::update_v_ref(int k, int velID)
 {
-  // TODO
+  /* ONLY GAMEPAD CONTROL FOR NOW
+  if (predefined):
+    update_v_ref_predefined(k, velID);
+  else:
+  */
+  
+  update_v_ref_gamepad();
 }
+
+void Joystick::update_v_ref_gamepad()
+{
+  // Create the gamepad client
+  // TODO
+
+  // Retrieve data from gamepad
+  double vX = 0.0 * vXScale;
+  double vY = 0.0 * vYScale;
+  double vYaw = 0.0 * vYawScale;
+  v_gp_ << vY, vX, 0.0, 0.0, 0.0, vYaw;
+
+  // Low pass filter to slow down the changes of velocity when moving the joysticks
+  double dead_zone = 0.004;
+  for (int i = 0; i < 6; i++) {
+    if (v_gp_(i, 0) > -dead_zone && v_gp_(i, 0) < dead_zone) { v_gp_(i, 0) = 0.0; }
+  }
+  v_ref_ = alpha * v_gp_ + (1 - alpha) * v_ref_;
+
+  // Switch to safety controller if the Back key is pressed
+  // if (gp.backButton.value) { stop = true; }  // TODO
+}
\ No newline at end of file