From 0536594c6562f8c313dbbd2daaba62d7fb01eae2 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Thu, 19 Oct 2023 15:59:11 +0200 Subject: [PATCH] Fix and update biclops control thread --- .../robot/include/visp3/robot/vpRobotBiclops.h | 2 +- .../src/real-robot/biclops/vpRobotBiclops.cpp | 15 ++++----------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/modules/robot/include/visp3/robot/vpRobotBiclops.h b/modules/robot/include/visp3/robot/vpRobotBiclops.h index 44aba86a72..356e3c4c94 100644 --- a/modules/robot/include/visp3/robot/vpRobotBiclops.h +++ b/modules/robot/include/visp3/robot/vpRobotBiclops.h @@ -410,7 +410,7 @@ class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot * speed/power can damage the unit, damage due to velocity mode commanding is * under user responsibility. */ - static void *vpRobotBiclopsSpeedControlLoop(void *arg); + static void vpRobotBiclopsSpeedControlLoop(void *arg); private: std::thread m_control_thread; diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp index 68b6aa2ff2..288e6ec518 100644 --- a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp +++ b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp @@ -51,8 +51,8 @@ #include "private/vpRobotBiclopsController_impl.h" -//#define VP_DEBUG // Activate the debug mode -//#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2 +//#define VP_DEBUG // Activate the debug mode +//#define VP_DEBUG_MODE 12 // Activate debug level up to 12 #include /* ---------------------------------------------------------------------- */ @@ -140,11 +140,10 @@ void vpRobotBiclops::init() return; } -void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) +void vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) { vpRobotBiclopsController *controller = static_cast(arg); - int iter = 0; // PMDAxisControl *m_panAxis = controller->getPanAxis(); // PMDAxisControl *m_tiltAxis = controller->getTiltAxis(); vpRobotBiclopsController::shmType shm; @@ -363,12 +362,8 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) for (unsigned int i = 0; i < vpBiclops::ndof; i++) prev_q_dot[i] = shm.q_dot[i]; - vpDEBUG_TRACE(12, "iter: %d", iter); - // wait 5 ms vpTime::wait(5.0); - - iter++; } controller->stopRequest(false); // Stop the robot @@ -382,8 +377,6 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) delete[] enable_limit; vpDEBUG_TRACE(11, "unlock vpEndThread_mutex"); m_mutex_end_thread.unlock(); - - return NULL; } vpRobot::vpRobotStateType vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState) @@ -410,7 +403,7 @@ vpRobot::vpRobotStateType vpRobotBiclops::setRobotState(vpRobot::vpRobotStateTyp m_mutex_end_thread.lock(); vpDEBUG_TRACE(12, "Create speed control thread"); - m_control_thread = std::thread(&vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &m_controller); + m_control_thread = std::thread(&vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, m_controller); vpTime::wait(100.0); vpDEBUG_TRACE(12, "Speed control thread created");