iCub-main
velControlThread.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 // Copyright (C) 2008 RobotCub Consortium
3 // Author: Lorenzo Natale
4 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
5 
6 #ifndef __VELCONTROLTHREAD__
7 #define __VELCONTROLTHREAD__
8 
9 #include <mutex>
10 #include <string>
11 
12 #include <yarp/os/PeriodicThread.h>
13 #include <yarp/os/Bottle.h>
14 #include <yarp/dev/ControlBoardInterfaces.h>
15 #include <yarp/dev/PolyDriver.h>
16 #include <yarp/sig/Vector.h>
17 #include <yarp/os/Time.h>
18 
19 //class yarp::dev::PolyDriver;
20 
21 class velControlThread: public yarp::os::PeriodicThread
22 {
23 private:
24  char robotName[255];
25  yarp::dev::IVelocityControl *ivel;
26  yarp::dev::IPositionControl *ipos;
27  yarp::dev::IEncoders *ienc;
28  yarp::dev::IControlMode *imod;
29  yarp::dev::IPidControl *ipid;
30  int nJoints;
31  yarp::dev::PolyDriver *driver;
32 
33  yarp::sig::Vector encoders;
34  yarp::sig::Vector encoders_speed;
35  yarp::sig::Vector Kp;
36  yarp::sig::Vector Kd; //derivative term
37 
38  yarp::sig::Vector targets;
39  yarp::sig::Vector ffVelocities;
40  yarp::sig::Vector command;
41 
42  yarp::sig::Vector error;
43  yarp::sig::Vector error_d;
44 
45  yarp::sig::Vector maxVel; //added ludo
46 
47  bool suspended;
48 
49  int nb_void_loops;
50 
51  std::mutex _mutex;
52 
53  int control_rate; //in ms
54 
55  yarp::os::BufferedPort<yarp::os::Bottle> command_port; //deprecated
56  yarp::os::BufferedPort<yarp::os::Bottle> command_port2; //new
57 
58  double time_watch;
59  double time_loop;
60  int count;
61 
62  FILE *currentSpeedFile;
63  FILE *targetSpeedFile;
64 
65 public:
66  velControlThread(int rate);
68 
69  bool init(yarp::dev::PolyDriver *d, std::string partName,
70  std::string robotName);
71 
72  void halt();
73  void go();
74  void setRef(int i, double pos);
75 
76  void setVel(int i, double vel); //added ludovic to set max velocity
77  void setGain(int i, double gain); //to set the Kp gains
78 
79  void run();
80  bool threadInit();
81  void threadRelease();
82 
83  void limitSpeed(yarp::sig::Vector &command);
84 };
85 
86 #endif
87 
velControlThread::setVel
void setVel(int i, double vel)
Definition: velControlThread.cpp:303
velControlThread::go
void go()
Definition: velControlThread.cpp:276
velControlThread::setGain
void setGain(int i, double gain)
Definition: velControlThread.cpp:316
velControlThread::limitSpeed
void limitSpeed(yarp::sig::Vector &command)
Definition: velControlThread.cpp:333
pos
const dReal * pos
Definition: iCub_Sim.cpp:62
velControlThread::halt
void halt()
Definition: velControlThread.cpp:267
velControlThread::velControlThread
velControlThread(int rate)
Definition: velControlThread.cpp:25
velControlThread::~velControlThread
~velControlThread()
Definition: velControlThread.cpp:32
velControlThread::init
bool init(yarp::dev::PolyDriver *d, std::string partName, std::string robotName)
Definition: velControlThread.cpp:191
velControlThread::run
void run()
Definition: velControlThread.cpp:35
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
velControlThread::threadInit
bool threadInit()
Definition: velControlThread.cpp:158
velControlThread
Definition: velControlThread.h:21
velControlThread::setRef
void setRef(int i, double pos)
Definition: velControlThread.cpp:294
velControlThread::threadRelease
void threadRelease()
Definition: velControlThread.cpp:172