Implementation of gamepad client

include/qrw/Joystick.hpp

@@ -9,6 +9,10 @@
 #ifndef JOYSTICK_H_INCLUDED
 #define JOYSTICK_H_INCLUDED
 
+#include <fcntl.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <linux/joystick.h>
 #include <iostream>
 #include <fstream>
 #include <cmath>
@@ -17,6 +21,15 @@
 #include "qrw/Types.h"
 #include "qrw/Params.hpp"
 
+struct gamepad_struct
+{
+	double v_x = 0.0;
+	double v_y = 0.0;
+	double w_yaw = 0.0;
+	int start = 0;
+	int select = 0;
+};
+
 class Joystick {
  public:
   ////////////////////////////////////////////////////////////////////////////////////////////////
@@ -40,7 +53,7 @@ class Joystick {
   /// \brief Destructor.
   ///
   ////////////////////////////////////////////////////////////////////////////////////////////////
-  ~Joystick() {}
+  ~Joystick() { if (js != -1) {close(js);} }
 
   ////////////////////////////////////////////////////////////////////////////////////////////////
   ///
@@ -55,6 +68,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);
+  int read_event(int fd, struct js_event *event);
   void update_v_ref_gamepad();
 
   Vector6 getVRef() { return v_ref_; }
@@ -78,6 +92,12 @@ class Joystick {
   double vYScale = 1.0;  // Forward
   double vYawScale = 1.2;  // Rotation
 
+  // Gamepad client variables
+  struct gamepad_struct gamepad;
+  const char *device;
+  int js;
+  struct js_event event;
+
 };
 
 #endif  // JOYSTICK_H_INCLUDED

src/Controller.cpp

@@ -39,6 +39,7 @@ void Controller::initialize(Params& params) {
   init_robot();
 
   // Initialization of the control blocks
+  joystick.initialize(params);
   statePlanner.initialize(params);
   gait.initialize(params);
   footstepPlanner.initialize(params, gait);
@@ -218,7 +219,7 @@ void Controller::init_robot()
 
 void Controller::security_check()
 {
-  if (error_flag == 0 && !error && !joystick.getStop())
+  if (error_flag == 0 && !error)
   {
     error_flag = estimator.security_check(tau_ff);
     if (error_flag != 0)
@@ -238,8 +239,14 @@ void Controller::security_check()
     }
   }
 
+  if(joystick.getStop())
+  {
+    error = true;
+    error_flag = -1;
+  }
+
   // If something wrong happened in the controller we stick to a security controller
-  if (error || joystick.getStop())
+  if (error)
   {
     // Quantities sent to the control board
     P = Vector12::Zero();

src/Joystick.cpp

@@ -6,6 +6,13 @@ Joystick::Joystick() : A3_(Vector6::Zero()), A2_(Vector6::Zero()),
 void Joystick::initialize(Params &params)
 {
   predefined = params.predefined_vel;
+
+  // Gamepad initialisation
+  device = "/dev/input/js0";
+  js = open(device, O_RDONLY | O_NONBLOCK);
+  if (js == -1) {
+    perror("Could not open joystick");
+  }
 }
 
 VectorN Joystick::handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch) {
@@ -34,15 +41,39 @@ void Joystick::update_v_ref(int k, int velID)
   update_v_ref_gamepad();
 }
 
+int Joystick::read_event(int fd, struct js_event *event)
+{
+    ssize_t bytes;
+    bytes = read(fd, event, sizeof(*event));
+    if (bytes == sizeof(*event))
+        return 0;
+    /* Error, could not read full event. */
+    return -1;
+}
+
 void Joystick::update_v_ref_gamepad()
 {
-  // Create the gamepad client
-  // TODO
+  // Read information from gamepad client
+  if (read_event(js, &event) == 0)
+  {
+    if (event.type == JS_EVENT_BUTTON)
+    {
+      if    (event.number == 9) gamepad.start = event.value;
+      else if(event.number == 8) gamepad.select = event.value;
+    }
+    else if (event.type == JS_EVENT_AXIS)
+    {
+      if     (event.number == 0) gamepad.v_y   = + event.value / 32767.0;
+      else if(event.number == 1) gamepad.v_x   = - event.value / 32767.0;
+      else if(event.number == 3) gamepad.w_yaw = + event.value / 32767.0;
+    }
+  }
+  // printf("Start:%d  Stop:%d  Vx:%f \tVy:%f \tWyaw:%f\n",gamepad.start,gamepad.select,gamepad.v_x,gamepad.v_y,gamepad.w_yaw);
 
   // Retrieve data from gamepad
-  double vX = 0.0 * vXScale;
-  double vY = 0.0 * vYScale;
-  double vYaw = 0.0 * vYawScale;
+  double vX = gamepad.v_x * vXScale;
+  double vY = gamepad.v_y * vYScale;
+  double vYaw = gamepad.w_yaw * 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
@@ -52,6 +83,6 @@ void Joystick::update_v_ref_gamepad()
   }
   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
+  // Switch to safety controller if the select key is pressed
+  if (gamepad.select == 1) { stop_ = true; }
 }
\ No newline at end of file

src/control_solo12.cpp

@@ -69,7 +69,7 @@ int main()
     // Initialization of variables
     Controller controller; // Main controller
     controller.initialize(params);  // Update urdf dependent parameters (mass, inertia, ...)
-    std::thread parallel_thread(parallel_loop); // spawn new thread that calls check_memory()
+    std::thread parallel_thread(parallel_loop); // spawn new thread that runs MPC in parallel
     int k_loop = 0;
 
     // Initialize the communication, session, joints, wait for motors to be ready
@@ -123,22 +123,6 @@ int main()
             robot->joints->PrintVector(robot->joints->GetPositions());
             std::cout << std::endl;
         }
-
-    }
-
-    // Close parallel thread
-    stop_thread();
-    parallel_thread.join();
-    std::cout << "Parallel thread closed" << std::endl ;
-
-    int duration_log [params.N_SIMULATION-2];
-    for (int i = 0; i < params.N_SIMULATION-3; i++)
-    {
-        duration_log[i] = static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>(t_log[i+1] - t_log[i]).count());
-    }
-    for (int i = 0; i < params.N_SIMULATION-3; i++)
-    {
-        std::cout << std::chrono::duration_cast<std::chrono::microseconds>(t_log[i+1] - t_log[i]).count() << ", ";
     }
 
     // DAMPING TO GET ON THE GROUND PROGRESSIVELY *********************
@@ -175,5 +159,21 @@ int main()
         printf("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.");
     }
 
+    // Close parallel thread
+    stop_thread();
+    parallel_thread.join();
+    std::cout << "Parallel thread closed" << std::endl;
+
+    int duration_log [params.N_SIMULATION-2];
+    for (int i = 0; i < params.N_SIMULATION-3; i++)
+    {
+        duration_log[i] = static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>(t_log[i+1] - t_log[i]).count());
+    }
+    for (int i = 0; i < params.N_SIMULATION-3; i++)
+    {
+        std::cout << std::chrono::duration_cast<std::chrono::microseconds>(t_log[i+1] - t_log[i]).count() << ", ";
+    }
+    std::cout << std::endl;
+
     return 0;
 }