mia_hand_driver  rel 1.0.0
cpp_driver.cpp
Go to the documentation of this file.
2 
3 namespace mia_hand
4 {
6  serial_port_(&finger_data_mtx_, &reply_mtx_, &is_connected_),
7  connection_trd_on_(true),
8  serial_trd_on_(true),
9  is_checking_on_(false)
10 {
11  /* Starting serial polling detached thread.
12  */
13  serial_poll_trd_ = std::thread(&CppDriver::pollSerialPort, this);
14  serial_poll_trd_.detach();
15 
16  /* Starting connection checking detached thread.
17  */
19  check_connection_trd_.detach();
20 }
21 
23 {
24  /* Ending detached threads by clearing related flags.
25  */
26  connection_trd_on_ = false;
27  serial_trd_on_ = false;
28 }
29 
30 bool CppDriver::connectToPort(uint16_t port_num)
31 {
32  bool is_port_opened = serial_port_.open(port_num);
33 
34  return is_port_opened;
35 }
36 
38 {
39  bool is_port_closed = serial_port_.close();
40 
41  if (is_port_closed)
42  {
43  connection_mtx_.lock();
44  is_connected_ = false;
45  connection_mtx_.unlock();
46  }
47 
48  return is_port_closed;
49 }
50 
52 {
53  connection_mtx_.lock();
54  bool is_connected = is_connected_;
55  connection_mtx_.unlock();
56 
57  return is_connected;
58 }
59 
60 void CppDriver::setMotorPos(uint8_t mot_id, int16_t mot_pos)
61 {
62  std::string pos_cmd = "@" + std::to_string(mot_id + 1) + "P+000040......";
63 
64  if (mot_pos < 0)
65  {
66  pos_cmd.replace(3, 1, "-");
67  mot_pos = -mot_pos;
68  }
69 
70  if (mot_pos > 999)
71  {
72  pos_cmd.replace(5, 3, "999");
73  }
74  else
75  {
76  pos_cmd.replace(5, 3, numToStr(mot_pos, 3));
77  }
78 
79  serial_port_.sendCommand(pos_cmd);
80 
81  return;
82 }
83 
84 void CppDriver::setMotorSpe(uint8_t mot_id, int16_t mot_spe)
85 {
86  std::string spe_cmd = "@" + std::to_string(mot_id + 1) + "S+....0080....";
87 
88  if (mot_spe < 0)
89  {
90  spe_cmd.replace(3, 1, "-");
91  mot_spe = -mot_spe;
92  }
93 
94  if (mot_spe > 99)
95  {
96  spe_cmd.replace(8, 2, "99");
97  }
98  else
99  {
100  spe_cmd.replace(8, 2, numToStr(mot_spe, 2));
101  }
102 
103  serial_port_.sendCommand(spe_cmd);
104 
105  return;
106 }
107 
108 void CppDriver::setFingerFor(uint8_t mot_id, int16_t fin_for)
109 {
110  std::string for_cmd = "@" + std::to_string(mot_id + 1) + "F+....0080....";
111 
112  if (fin_for > 99)
113  {
114  for_cmd.replace(8, 2, "99");
115  }
116  else if (fin_for > 0)
117  {
118  for_cmd.replace(8, 2, numToStr(fin_for, 2));
119  }
120  else
121  {
122  // Default empty case
123  }
124 
125  serial_port_.sendCommand(for_cmd);
126 
127  return;
128 }
129 
130 int16_t CppDriver::getMotorPos(uint8_t fin_id)
131 {
132  int16_t mot_pos;
133 
134  finger_data_mtx_.lock();
135 
136  switch (fin_id)
137  {
138  case 0:
139 
140  mot_pos = thumb_info_.mot_pos;
141 
142  break;
143 
144  case 1:
145 
146  mot_pos = mrl_info_.mot_pos;
147 
148  break;
149 
150  case 2:
151 
152  mot_pos = index_info_.mot_pos;
153 
154  break;
155 
156  default:
157 
158  break;
159  }
160 
161  finger_data_mtx_.unlock();
162 
163  return mot_pos;
164 }
165 
166 int16_t CppDriver::getMotorSpe(uint8_t fin_id)
167 {
168  int16_t mot_spe;
169 
170  finger_data_mtx_.lock();
171 
172  switch (fin_id)
173  {
174  case 0:
175 
176  mot_spe = thumb_info_.mot_spe;
177 
178  break;
179 
180  case 1:
181 
182  mot_spe = mrl_info_.mot_spe;
183 
184  break;
185 
186  case 2:
187 
188  mot_spe = index_info_.mot_spe;
189 
190  break;
191 
192  default:
193 
194  break;
195  }
196 
197  finger_data_mtx_.unlock();
198 
199  return mot_spe;
200 }
201 
202 int16_t CppDriver::getMotorCur(uint8_t fin_id)
203 {
204  int16_t mot_cur;
205 
206  finger_data_mtx_.lock();
207 
208  switch (fin_id)
209  {
210  case 0:
211 
212  mot_cur = thumb_info_.mot_cur;
213 
214  break;
215 
216  case 1:
217 
218  mot_cur = mrl_info_.mot_cur;
219 
220  break;
221 
222  case 2:
223 
224  mot_cur = index_info_.mot_cur;
225 
226  break;
227 
228  default:
229 
230  break;
231  }
232 
233  finger_data_mtx_.unlock();
234 
235  return mot_cur;
236 }
237 
238 void CppDriver::getFingerSgRaw(uint8_t fin_id, int16_t& nor_for,
239  int16_t& tan_for)
240 {
241  finger_data_mtx_.lock();
242 
243  switch (fin_id)
244  {
245  case 0:
246 
247  nor_for = thumb_info_.fin_sg_raw[0];
248  tan_for = thumb_info_.fin_sg_raw[1];
249 
250  break;
251 
252  case 1:
253 
254  nor_for = mrl_info_.fin_sg_raw[0];
255  tan_for = mrl_info_.fin_sg_raw[1];
256 
257  break;
258 
259  case 2:
260 
261  nor_for = index_info_.fin_sg_raw[0];
262  tan_for = index_info_.fin_sg_raw[1];
263 
264  break;
265 
266  default:
267 
268  break;
269  }
270 
271  finger_data_mtx_.unlock();
272 
273  return;
274 }
275 
276 void CppDriver::openGrasp(char grasp_id)
277 {
278  std::string grasp_cmd = "@AG" + std::string(1, grasp_id) + "a10050......";
279  serial_port_.sendCommand(grasp_cmd);
280 
281  return;
282 }
283 
284 void CppDriver::closeGrasp(char grasp_id)
285 {
286  std::string grasp_cmd = "@AG" + std::string(1, grasp_id) + "A10050......";
287  serial_port_.sendCommand(grasp_cmd);
288 
289  return;
290 }
291 
292 void CppDriver::closeGrasp(char grasp_id, int16_t close_percent)
293 {
294  std::string grasp_cmd = "@AG" + std::string(1, grasp_id) + "M00050......";
295 
296  if (close_percent > 99)
297  {
298  grasp_cmd.replace(5, 3, "100");
299  }
300  else if (close_percent > 0)
301  {
302  grasp_cmd.replace(6, 2, numToStr(close_percent, 2));
303  }
304  else
305  {
306 
307  }
308 
309  serial_port_.sendCommand(grasp_cmd);
310 
311  return;
312 }
313 
314 void CppDriver::setThuGraspRef(char grasp_id, int16_t rest, int16_t pos,
315  int16_t delay)
316 {
317  std::string grasp_ref_cmd = "@1G" + std::string(1, grasp_id) + "+000+000+000";
318 
319  if (rest > 0)
320  {
321  grasp_ref_cmd.replace(5, 3, numToStr(rest, 3));
322  }
323 
324  if (pos > 0)
325  {
326  grasp_ref_cmd.replace(9, 3, numToStr(pos, 3));
327  }
328 
329  if (delay > 99)
330  {
331  grasp_ref_cmd.replace(13, 1, "1");
332  }
333  else if (delay > 0)
334  {
335  grasp_ref_cmd.replace(13, 3, numToStr(delay, 3));
336  }
337  else
338  {
339 
340  }
341 
342  serial_port_.sendCommand(grasp_ref_cmd);
343 
344  return;
345 }
346 
347 void CppDriver::setIndGraspRef(char grasp_id, int16_t rest, int16_t pos,
348  int16_t delay)
349 {
350  std::string grasp_ref_cmd = "@3G" + std::string(1, grasp_id) + "+000+000+000";
351 
352  if (rest > 0)
353  {
354  grasp_ref_cmd.replace(5, 3, numToStr(rest, 3));
355  }
356 
357  if (pos > 0)
358  {
359  grasp_ref_cmd.replace(9, 3, numToStr(pos, 3));
360  }
361 
362  if (delay > 99)
363  {
364  grasp_ref_cmd.replace(13, 1, "1");
365  }
366  else if (delay > 0)
367  {
368  grasp_ref_cmd.replace(13, 3, numToStr(delay, 3));
369  }
370  else
371  {
372 
373  }
374 
375  serial_port_.sendCommand(grasp_ref_cmd);
376 
377  return;
378 }
379 
380 void CppDriver::setMrlGraspRef(char grasp_id, int16_t rest, int16_t pos,
381  int16_t delay)
382 {
383  std::string grasp_ref_cmd = "@2G" + std::string(1, grasp_id) + "+000+000+000";
384 
385  if (rest > 0)
386  {
387  grasp_ref_cmd.replace(5, 3, numToStr(rest, 3));
388  }
389 
390  if (pos > 0)
391  {
392  grasp_ref_cmd.replace(9, 3, numToStr(pos, 3));
393  }
394 
395  if (delay > 99)
396  {
397  grasp_ref_cmd.replace(13, 1, "1");
398  }
399  else if (delay > 0)
400  {
401  grasp_ref_cmd.replace(13, 3, numToStr(delay, 3));
402  }
403  else
404  {
405 
406  }
407 
408  serial_port_.sendCommand(grasp_ref_cmd);
409 
410  return;
411 }
412 
413 void CppDriver::switchPosStream(bool b_on_off)
414 {
415  std::string stream_cmd = "@ADP" + std::to_string(b_on_off ? 1 : 0)
416  + "...........";
417 
418  serial_port_.sendCommand(stream_cmd);
419 
420  return;
421 }
422 
423 void CppDriver::switchSpeStream(bool b_on_off)
424 {
425  std::string stream_cmd = "@ADS" + std::to_string(b_on_off ? 1 : 0)
426  + "...........";
427 
428  serial_port_.sendCommand(stream_cmd);
429 
430  return;
431 }
432 
433 void CppDriver::switchAnaStream(bool b_on_off)
434 {
435  std::string stream_cmd = "@ADA" + std::to_string(b_on_off ? 1 : 0)
436  + "...........";
437 
438  serial_port_.sendCommand(stream_cmd);
439 
440  return;
441 }
442 
443 void CppDriver::switchCurStream(bool b_on_off)
444 {
445  std::string stream_cmd = "@ADC" + std::to_string(b_on_off ? 1 : 0)
446  + "...........";
447 
448  serial_port_.sendCommand(stream_cmd);
449 
450  return;
451 }
452 
453 std::string CppDriver::numToStr(int16_t num, int8_t n_digits)
454 {
455  char num_ascii[n_digits];
456 
457  for (int8_t ascii_it = n_digits - 1; ascii_it > -1; --ascii_it)
458  {
459  num_ascii[ascii_it] = num % 10 + 48;
460  num /= 10;
461  }
462 
463  std::string num_str(num_ascii, n_digits);
464 
465  return num_str;
466 }
467 
469 {
470  while (serial_trd_on_)
471  {
474  }
475 
476  return;
477 }
478 
480 {
481  while (connection_trd_on_)
482  {
483  connection_mtx_.lock();
484  is_checking_on_ = true;
485  connection_mtx_.unlock();
486 
487  serial_port_.sendCommand("@?AreYouPlugged?");
488 
489  std::this_thread::sleep_for(std::chrono::milliseconds(500));
490  }
491 
492  return;
493 }
494 } // namespace
mia_hand::CppDriver::getFingerSgRaw
void getFingerSgRaw(uint8_t fin_id, int16_t &nor_for, int16_t &tan_for)
Get the current output of the force sensor of a specific finger of the Mia hand.
Definition: cpp_driver.cpp:238
mia_hand::CppDriver::is_connected_
bool is_connected_
True if the Mia hand is conncted, False otherwise.
Definition: cpp_driver.h:244
mia_hand::CppDriver::connection_trd_on_
bool connection_trd_on_
True if check_connection_trd_ thread is running.
Definition: cpp_driver.h:234
mia_hand::CppDriver::switchSpeStream
void switchSpeStream(bool b_on_off)
Manage the streaming of the motors velocity data.
Definition: cpp_driver.cpp:423
mia_hand::CppDriver::CppDriver
CppDriver()
Class constructor.
Definition: cpp_driver.cpp:5
mia_hand::CppDriver::numToStr
std::string numToStr(int16_t num, int8_t n_digits)
Convert integer number into string.
Definition: cpp_driver.cpp:453
mia_hand::FingerSerialInfo::fin_sg_raw
int16_t fin_sg_raw[2]
Definition: serial_port.h:42
mia_hand::CppDriver::pollSerialPort
void pollSerialPort()
Function of the serial_poll_trd_ thread.
Definition: cpp_driver.cpp:468
mia_hand::CppDriver::openGrasp
void openGrasp(char grasp_id)
Go to rest pose of a grasp type Move all the Mia hand fingers to the rest pose of a specific grasp ty...
Definition: cpp_driver.cpp:276
mia_hand::SerialPort::close
bool close()
Close the serial port.
Definition: serial_port.cpp:65
mia_hand::CppDriver::setThuGraspRef
void setThuGraspRef(char grasp_id, int16_t rest, int16_t pos, int16_t delay)
Set grasp parameter for thumb flexion motor.
Definition: cpp_driver.cpp:314
mia_hand::CppDriver::serial_port_
SerialPort serial_port_
Hanlder of the serial port_num.
Definition: cpp_driver.h:193
mia_hand::CppDriver::setMotorSpe
void setMotorSpe(uint8_t fin_id, int16_t mot_spe)
Set the target velocity of a specific motor of the Mia hand.
Definition: cpp_driver.cpp:84
mia_hand::CppDriver::connection_mtx_
std::mutex connection_mtx_
Definition: cpp_driver.h:238
mia_hand::SerialPort::sendCommand
void sendCommand(const std::string &command)
Send a command to the Mia hand attached to the serial port.
Definition: serial_port.cpp:85
mia_hand::CppDriver::is_checking_on_
bool is_checking_on_
Definition: cpp_driver.h:235
mia_hand::CppDriver::getMotorPos
int16_t getMotorPos(uint8_t fin_id)
Get the current position of a specific motor of the Mia hand.
Definition: cpp_driver.cpp:130
mia_hand::CppDriver::disconnect
bool disconnect()
Disconnect the Mia hand closing the serial port.
Definition: cpp_driver.cpp:37
mia_hand::SerialPort::open
bool open(uint16_t port_num)
Open a serial port.
Definition: serial_port.cpp:31
mia_hand::CppDriver::check_connection_trd_
std::thread check_connection_trd_
Thread to execute the check of the Mia hand connection.
Definition: cpp_driver.h:232
mia_hand::FingerSerialInfo::mot_cur
int16_t mot_cur
Definition: serial_port.h:41
mia_hand::CppDriver::finger_data_mtx_
std::mutex finger_data_mtx_
Mutex to handle the reading of the motors finger data.
Definition: cpp_driver.h:225
mia_hand::CppDriver::switchAnaStream
void switchAnaStream(bool b_on_off)
Manage the streaming of analog input data (as the straing gauge force sensors).
Definition: cpp_driver.cpp:433
mia_hand::FingerSerialInfo::mot_pos
int16_t mot_pos
Definition: serial_port.h:39
mia_hand::CppDriver::getMotorSpe
int16_t getMotorSpe(uint8_t fin_id)
Get the current velocity of a specific motor of the Mia hand.
Definition: cpp_driver.cpp:166
mia_hand::FingerSerialInfo::mot_spe
int8_t mot_spe
Definition: serial_port.h:40
mia_hand::SerialPort::parseStream
void parseStream(FingerSerialInfo &thumb, FingerSerialInfo &index, FingerSerialInfo &mrl, bool &is_checking_on)
Parse message received from the Mia hand.
Definition: serial_port.cpp:109
mia_hand::CppDriver::serial_poll_trd_
std::thread serial_poll_trd_
Thread to execute the parse of the data received by the Mia hand.
Definition: cpp_driver.h:222
mia_hand
Definition: cpp_driver.h:16
mia_hand::CppDriver::switchPosStream
void switchPosStream(bool b_on_off)
Manage the streaming of the motors position data.
Definition: cpp_driver.cpp:413
mia_hand::CppDriver::isConnected
bool isConnected()
Function returning the connection status of the Mia hand.
Definition: cpp_driver.cpp:51
mia_hand::CppDriver::setMrlGraspRef
void setMrlGraspRef(char grasp_id, int16_t rest, int16_t pos, int16_t delay)
Set grasp parameter for MRL flexion motor.
Definition: cpp_driver.cpp:380
mia_hand::CppDriver::mrl_info_
FingerSerialInfo mrl_info_
Info about the mrl flexion motor.
Definition: cpp_driver.h:242
mia_hand::CppDriver::getMotorCur
int16_t getMotorCur(uint8_t fin_id)
Get the current currently absorbed by a specific motor of the Mia hand.
Definition: cpp_driver.cpp:202
mia_hand::CppDriver::index_info_
FingerSerialInfo index_info_
Info about the index flexion motor.
Definition: cpp_driver.h:241
mia_hand::CppDriver::checkConnection
void checkConnection()
Function of the check_connection_trd_ thread.
Definition: cpp_driver.cpp:479
mia_hand::CppDriver::connectToPort
bool connectToPort(uint16_t port_num)
Open serial port used to plug Mia hand.
Definition: cpp_driver.cpp:30
mia_hand::CppDriver::closeGrasp
void closeGrasp(char grasp_id)
Go to closed pose of a grasp type Move all the Mia hand fingers to the closed pose of a specific gras...
Definition: cpp_driver.cpp:284
mia_hand::CppDriver::setMotorPos
void setMotorPos(uint8_t fin_id, int16_t mot_pos)
Set the target position of a specific motor of the Mia hand.
Definition: cpp_driver.cpp:60
mia_hand::CppDriver::setIndGraspRef
void setIndGraspRef(char grasp_id, int16_t rest, int16_t pos, int16_t delay)
Set grasp parameter for Index flexion + thumb abduction motor.
Definition: cpp_driver.cpp:347
mia_hand::CppDriver::switchCurStream
void switchCurStream(bool b_on_off)
Manage the streaming of the current absorbed by Mi hand motors.
Definition: cpp_driver.cpp:443
cpp_driver.h
mia_hand::CppDriver::~CppDriver
~CppDriver()
Class destructor.
Definition: cpp_driver.cpp:22
mia_hand::CppDriver::setFingerFor
void setFingerFor(uint8_t fin_id, int16_t fin_for)
Set the target force of a specific motor of the Mia hand.
Definition: cpp_driver.cpp:108
mia_hand::CppDriver::thumb_info_
FingerSerialInfo thumb_info_
Info about the thumb flexion motor.
Definition: cpp_driver.h:240
mia_hand::CppDriver::serial_trd_on_
bool serial_trd_on_
True if serial_poll_trd_ thread is running.
Definition: cpp_driver.h:224