diff --git a/include/qrw/Joystick.hpp b/include/qrw/Joystick.hpp
index 5000e557542bbe547de84decac9a473ffad4ad8b..c4cc8112a201683910447bb2d13a518c7fc5ab87 100644
--- a/include/qrw/Joystick.hpp
+++ b/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
diff --git a/src/Controller.cpp b/src/Controller.cpp
index 7483ad212fd4204a6ca16482046d1e02ad83923a..6b3988ff1c1a8ef7d515d63c118ae441408d1df2 100644
--- a/src/Controller.cpp
+++ b/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();
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index 31b6251fc21563e9430964412b51948006a1f149..8fa224fc61da6b193e23ed536b147137942fafa2 100644
--- a/src/Joystick.cpp
+++ b/src/Joystick.cpp
@@ -6,6 +6,13 @@ Joystick::Joystick() : A3_(Vector6::Zero()), A2_(Vector6::Zero()),
void Joystick::initialize(Params ¶ms)
{
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
diff --git a/src/control_solo12.cpp b/src/control_solo12.cpp
index 03aa98ec14bff5bded9f52de78c5c432a0546263..a81f1e107f5066cd614f9df9262aa416170145dd 100644
--- a/src/control_solo12.cpp
+++ b/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;
}