mia_hand_driver
rel 1.0.0
|
#include <cpp_driver.h>
Public Member Functions | |
CppDriver () | |
Class constructor. More... | |
~CppDriver () | |
Class destructor. More... | |
bool | connectToPort (uint16_t port_num) |
Open serial port used to plug Mia hand. More... | |
bool | disconnect () |
Disconnect the Mia hand closing the serial port. More... | |
bool | isConnected () |
Function returning the connection status of the Mia hand. More... | |
void | setMotorPos (uint8_t fin_id, int16_t mot_pos) |
Set the target position of a specific motor of the Mia hand. More... | |
void | setMotorSpe (uint8_t fin_id, int16_t mot_spe) |
Set the target velocity of a specific motor of the Mia hand. More... | |
void | setFingerFor (uint8_t fin_id, int16_t fin_for) |
Set the target force of a specific motor of the Mia hand. More... | |
int16_t | getMotorPos (uint8_t fin_id) |
Get the current position of a specific motor of the Mia hand. More... | |
int16_t | getMotorSpe (uint8_t fin_id) |
Get the current velocity of a specific motor of the Mia hand. More... | |
int16_t | getMotorCur (uint8_t fin_id) |
Get the current currently absorbed by a specific motor of the Mia hand. More... | |
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. More... | |
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 type. More... | |
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 grasp type. More... | |
void | closeGrasp (char grasp_id, int16_t close_percent) |
Go to intermediate pose of a grasp type Move all the Mia hand fingers to a specific intermediate pose of a specific grasp type. More... | |
void | setThuGraspRef (char grasp_id, int16_t rest, int16_t pos, int16_t delay) |
Set grasp parameter for thumb flexion motor. More... | |
void | setIndGraspRef (char grasp_id, int16_t rest, int16_t pos, int16_t delay) |
Set grasp parameter for Index flexion + thumb abduction motor. More... | |
void | setMrlGraspRef (char grasp_id, int16_t rest, int16_t pos, int16_t delay) |
Set grasp parameter for MRL flexion motor. More... | |
void | switchPosStream (bool b_on_off) |
Manage the streaming of the motors position data. More... | |
void | switchSpeStream (bool b_on_off) |
Manage the streaming of the motors velocity data. More... | |
void | switchAnaStream (bool b_on_off) |
Manage the streaming of analog input data (as the straing gauge force sensors). More... | |
void | switchCurStream (bool b_on_off) |
Manage the streaming of the current absorbed by Mi hand motors. More... | |
Private Member Functions | |
std::string | numToStr (int16_t num, int8_t n_digits) |
Convert integer number into string. More... | |
void | pollSerialPort () |
Function of the serial_poll_trd_ thread. More... | |
void | checkConnection () |
Function of the check_connection_trd_ thread. More... | |
Private Attributes | |
SerialPort | serial_port_ |
Hanlder of the serial port_num. More... | |
std::thread | serial_poll_trd_ |
Thread to execute the parse of the data received by the Mia hand. More... | |
bool | serial_trd_on_ |
True if serial_poll_trd_ thread is running. More... | |
std::mutex | finger_data_mtx_ |
Mutex to handle the reading of the motors finger data. More... | |
std::thread | check_connection_trd_ |
Thread to execute the check of the Mia hand connection. More... | |
bool | connection_trd_on_ |
True if check_connection_trd_ thread is running. More... | |
bool | is_checking_on_ |
std::mutex | reply_mtx_ |
std::mutex | connection_mtx_ |
FingerSerialInfo | thumb_info_ |
Info about the thumb flexion motor. More... | |
FingerSerialInfo | index_info_ |
Info about the index flexion motor. More... | |
FingerSerialInfo | mrl_info_ |
Info about the mrl flexion motor. More... | |
bool | is_connected_ |
True if the Mia hand is conncted, False otherwise. More... | |
Definition at line 24 of file cpp_driver.h.
mia_hand::CppDriver::CppDriver | ( | ) |
Class constructor.
Definition at line 5 of file cpp_driver.cpp.
mia_hand::CppDriver::~CppDriver | ( | ) |
Class destructor.
Definition at line 22 of file cpp_driver.cpp.
|
private |
Function of the check_connection_trd_ thread.
While the check_connection_trd_ thread is active, check if the Mia hand is connected.
Definition at line 479 of file cpp_driver.cpp.
void mia_hand::CppDriver::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 grasp type.
grasp_id | Type of grasp: 'C' cylindical, 'L' lateral, 'P' pinch, 'S' spherical, 'T' tridigital grasp. |
Definition at line 284 of file cpp_driver.cpp.
void mia_hand::CppDriver::closeGrasp | ( | char | grasp_id, |
int16_t | close_percent | ||
) |
Go to intermediate pose of a grasp type Move all the Mia hand fingers to a specific intermediate pose of a specific grasp type.
grasp_id | Type of grasp: 'C' cylindical, 'L' lateral, 'P' pinch, 'S' spherical, 'T' tridigital grasp. |
close_percent | Percentage of closure [0, 100]. |
Definition at line 292 of file cpp_driver.cpp.
bool mia_hand::CppDriver::connectToPort | ( | uint16_t | port_num | ) |
Open serial port used to plug Mia hand.
port_num | number of the serial port to open. |
Definition at line 30 of file cpp_driver.cpp.
bool mia_hand::CppDriver::disconnect | ( | ) |
Disconnect the Mia hand closing the serial port.
Definition at line 37 of file cpp_driver.cpp.
void mia_hand::CppDriver::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.
Get the normal and tangential force signal output of the strain gauge included in the finger of the Mia hand
fin_id | Mia hand finger id. 1 for thumb flexion, 2 for MRL flexion, 3 for index flexion + thumb abduction. |
nor_for | sensor output describing the normal force applied on the finger. |
tan_for | sensor output describing the tangential force applied on the finger. |
Definition at line 238 of file cpp_driver.cpp.
int16_t mia_hand::CppDriver::getMotorCur | ( | uint8_t | fin_id | ) |
Get the current currently absorbed by a specific motor of the Mia hand.
fin_id | Mia hand finger id. 1 for thumb flexion, 2 for MRL flexion, 3 for index flexion + thumb abduction. |
Definition at line 202 of file cpp_driver.cpp.
int16_t mia_hand::CppDriver::getMotorPos | ( | uint8_t | fin_id | ) |
Get the current position of a specific motor of the Mia hand.
fin_id | Mia hand finger id. 1 for thumb flexion, 2 for MRL flexion, 3 for index flexion + thumb abduction. |
Definition at line 130 of file cpp_driver.cpp.
int16_t mia_hand::CppDriver::getMotorSpe | ( | uint8_t | fin_id | ) |
Get the current velocity of a specific motor of the Mia hand.
fin_id | Mia hand finger id. 1 for thumb flexion, 2 for MRL flexion, 3 for index flexion + thumb abduction. |
Definition at line 166 of file cpp_driver.cpp.
bool mia_hand::CppDriver::isConnected | ( | ) |
Function returning the connection status of the Mia hand.
Definition at line 51 of file cpp_driver.cpp.
|
private |
Convert integer number into string.
num | Integer number to be converted. |
n_digits | Number of digits of the integer to be converted. |
Definition at line 453 of file cpp_driver.cpp.
void mia_hand::CppDriver::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 type.
grasp_id | Type of grasp: 'C' cylindical, 'L' lateral, 'P' pinch, 'S' spherical, 'T' tridigital grasp. |
Definition at line 276 of file cpp_driver.cpp.
|
private |
Function of the serial_poll_trd_ thread.
While the serial_poll_trd_ thread is active, parse the data received from the Mia hand.
Definition at line 468 of file cpp_driver.cpp.
void mia_hand::CppDriver::setFingerFor | ( | uint8_t | fin_id, |
int16_t | fin_for | ||
) |
Set the target force of a specific motor of the Mia hand.
This function can be used to move specific Mia hand finger with force control.
fin_id | Mia hand finger id. 1 for thumb flexion, 2 for MRL flexion, 3 for index flexion + thumb abduction. |
fin_for | target finger velocity [0, +1024] |
Definition at line 108 of file cpp_driver.cpp.
void mia_hand::CppDriver::setIndGraspRef | ( | char | grasp_id, |
int16_t | rest, | ||
int16_t | pos, | ||
int16_t | delay | ||
) |
Set grasp parameter for Index flexion + thumb abduction motor.
grasp_id | Type of grasp to set: 'C' cylindical, 'L' lateral, 'P' pinch, 'S' spherical, 'T' tridigital grasp. |
rest | Rest reference position [0, 255]. |
pos | Closure reference position [0, 255]. |
delay | Time delay in milliseconds. Used to delay the movement of the target motor of a percentage of the total time needed to perform the grasp. |
Definition at line 347 of file cpp_driver.cpp.
void mia_hand::CppDriver::setMotorPos | ( | uint8_t | fin_id, |
int16_t | mot_pos | ||
) |
Set the target position of a specific motor of the Mia hand.
This function can be used to move specific Mia hand finger in position.
fin_id | Mia hand finger id. 1 for thumb flexion, 2 for MRL flexion, 3 for index flexion + thumb abduction. |
mot_pos | target finger position [-255, +255] for index and [0, +255] otherwise. |
Definition at line 60 of file cpp_driver.cpp.
void mia_hand::CppDriver::setMotorSpe | ( | uint8_t | fin_id, |
int16_t | mot_spe | ||
) |
Set the target velocity of a specific motor of the Mia hand.
This function can be used to move a specific Mia hand finger in velocity.
fin_id | Mia hand finger id. 1 for thumb flexion, 2 for MRL flexion, 3 for index flexion + thumb abduction. |
mot_spe | target finger velocity [-90, +90]. |
Definition at line 84 of file cpp_driver.cpp.
void mia_hand::CppDriver::setMrlGraspRef | ( | char | grasp_id, |
int16_t | rest, | ||
int16_t | pos, | ||
int16_t | delay | ||
) |
Set grasp parameter for MRL flexion motor.
grasp_id | Type of grasp to set: 'C' cylindical, 'L' lateral, 'P' pinch, 'S' spherical, 'T' tridigital grasp. |
rest | Rest reference position [0, 255]. |
pos | Closure reference position [0, 255]. |
delay | Time delay in milliseconds. Used to delay the movement of the target motor of a percentage of the total time needed to perform the grasp. |
Definition at line 380 of file cpp_driver.cpp.
void mia_hand::CppDriver::setThuGraspRef | ( | char | grasp_id, |
int16_t | rest, | ||
int16_t | pos, | ||
int16_t | delay | ||
) |
Set grasp parameter for thumb flexion motor.
grasp_id | Type of grasp to set: 'C' cylindical, 'L' lateral, 'P' pinch, 'S' spherical, 'T' tridigital grasp. |
rest | Rest reference position [0, 255]. |
pos | Closure reference position [0, 255]. |
delay | Time delay in milliseconds. Used to delay the movement of the target motor of a percentage of the total time needed to perform the grasp. |
Definition at line 314 of file cpp_driver.cpp.
void mia_hand::CppDriver::switchAnaStream | ( | bool | b_on_off | ) |
Manage the streaming of analog input data (as the straing gauge force sensors).
b_on_off | True to enable the streaming, False to disable it. |
Definition at line 433 of file cpp_driver.cpp.
void mia_hand::CppDriver::switchCurStream | ( | bool | b_on_off | ) |
Manage the streaming of the current absorbed by Mi hand motors.
b_on_off | True to enable the streaming, False to disable it. |
Definition at line 443 of file cpp_driver.cpp.
void mia_hand::CppDriver::switchPosStream | ( | bool | b_on_off | ) |
Manage the streaming of the motors position data.
b_on_off | True to enable the streaming of the motors position data, False to disable it. |
Definition at line 413 of file cpp_driver.cpp.
void mia_hand::CppDriver::switchSpeStream | ( | bool | b_on_off | ) |
Manage the streaming of the motors velocity data.
b_on_off | True to enable the streaming, False to disable it. |
Definition at line 423 of file cpp_driver.cpp.
|
private |
Thread to execute the check of the Mia hand connection.
Definition at line 232 of file cpp_driver.h.
|
private |
Definition at line 238 of file cpp_driver.h.
|
private |
True if check_connection_trd_ thread is running.
Definition at line 234 of file cpp_driver.h.
|
private |
Mutex to handle the reading of the motors finger data.
Definition at line 225 of file cpp_driver.h.
|
private |
Info about the index flexion motor.
Definition at line 241 of file cpp_driver.h.
|
private |
Definition at line 235 of file cpp_driver.h.
|
private |
True if the Mia hand is conncted, False otherwise.
Definition at line 244 of file cpp_driver.h.
|
private |
Info about the mrl flexion motor.
Definition at line 242 of file cpp_driver.h.
|
private |
Definition at line 237 of file cpp_driver.h.
|
private |
Thread to execute the parse of the data received by the Mia hand.
Definition at line 222 of file cpp_driver.h.
|
private |
Hanlder of the serial port_num.
Definition at line 193 of file cpp_driver.h.
|
private |
True if serial_poll_trd_ thread is running.
Definition at line 224 of file cpp_driver.h.
|
private |
Info about the thumb flexion motor.
Definition at line 240 of file cpp_driver.h.