1 |
|
|
/** |
2 |
|
|
* @file AtiFTSensor.h |
3 |
|
|
* @author Ludovic Righetti |
4 |
|
|
* @license License BSD-3-Clause |
5 |
|
|
* @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft. |
6 |
|
|
* @date 2013-10-22 |
7 |
|
|
*/ |
8 |
|
|
|
9 |
|
|
#ifndef ATIFTSENSOR_H_ |
10 |
|
|
#define ATIFTSENSOR_H_ |
11 |
|
|
#include <netinet/in.h> |
12 |
|
|
#include <arpa/inet.h> |
13 |
|
|
#include <errno.h> |
14 |
|
|
#ifdef XENOMAI |
15 |
|
|
#include <native/task.h> |
16 |
|
|
#include <native/mutex.h> |
17 |
|
|
#include <native/pipe.h> |
18 |
|
|
#else |
19 |
|
|
#include <mutex> // std::mutex |
20 |
|
|
#endif |
21 |
|
|
#include <real_time_tools/thread.hpp> |
22 |
|
|
#include <boost/thread.hpp> |
23 |
|
|
#include <boost/shared_ptr.hpp> |
24 |
|
|
#include <boost/bind.hpp> |
25 |
|
|
|
26 |
|
|
|
27 |
|
|
namespace ati_ft_sensor |
28 |
|
|
{ |
29 |
|
|
|
30 |
|
|
class AtiFTSensor{ |
31 |
|
|
public: |
32 |
|
|
AtiFTSensor(); |
33 |
|
|
~AtiFTSensor(); |
34 |
|
|
|
35 |
|
|
bool initialize(); |
36 |
|
|
|
37 |
|
|
void getStatus(uint32_t& rdt_seq, uint32_t& ft_seq, uint32_t& status); |
38 |
|
|
void getFT(double* force, double* torque); |
39 |
|
|
void stream(bool stream); |
40 |
|
|
void stop(); |
41 |
|
|
void setBias(); |
42 |
|
|
void setBias(double* force, double* torque); |
43 |
|
|
void resetBias(); |
44 |
|
|
|
45 |
|
|
double F_[3]; |
46 |
|
|
double T_[3]; |
47 |
|
|
uint32_t rdt_sequence_; |
48 |
|
|
uint32_t ft_sequence_; |
49 |
|
|
uint32_t status_; |
50 |
|
|
|
51 |
|
|
// Streaming support is only available on xenomai. |
52 |
|
|
#ifdef XENOMAI |
53 |
|
|
struct steaming_msg |
54 |
|
|
{ |
55 |
|
|
uint32_t rdt_seq; |
56 |
|
|
uint32_t ft_seq; |
57 |
|
|
uint32_t status; |
58 |
|
|
double time; |
59 |
|
|
double F[3]; |
60 |
|
|
double T[3]; |
61 |
|
|
}; |
62 |
|
|
#endif |
63 |
|
|
|
64 |
|
|
private: |
65 |
|
|
|
66 |
|
|
static constexpr double count_per_force_ = 1000000.0; |
67 |
|
|
static constexpr double count_per_torque_ = 1000000.0; |
68 |
|
|
|
69 |
|
|
struct received_msg |
70 |
|
|
{ |
71 |
|
|
uint32_t rdt_sequence; // RDT sequence number of this packet. |
72 |
|
|
uint32_t ft_sequence; // The record���s internal sequence number |
73 |
|
|
uint32_t status; // System status code |
74 |
|
|
// Force and torque readings use counts values |
75 |
|
|
int32_t Fx; // X-axis force |
76 |
|
|
int32_t Fy; // Y-axis force |
77 |
|
|
int32_t Fz; // Z-axis force |
78 |
|
|
int32_t Tx; // X-axis torque |
79 |
|
|
int32_t Ty; // Y-axis torque |
80 |
|
|
int32_t Tz; // Z-axis torque |
81 |
|
|
}; |
82 |
|
|
|
83 |
|
|
struct send_msg |
84 |
|
|
{ |
85 |
|
|
uint16_t command_header; // = 0x1234 // Required |
86 |
|
|
uint16_t command; // Command to execute |
87 |
|
|
uint32_t sample_count; // Samples to output (0 = infinite) |
88 |
|
|
|
89 |
|
|
}; |
90 |
|
|
|
91 |
|
|
static THREAD_FUNCTION_RETURN_TYPE read_ft(void* instance_pointer) |
92 |
|
|
{ |
93 |
|
|
((AtiFTSensor*)(instance_pointer))->read_ft(); |
94 |
|
|
return THREAD_FUNCTION_RETURN_VALUE; |
95 |
|
|
} |
96 |
|
|
|
97 |
|
|
void read_ft(); |
98 |
|
|
|
99 |
|
|
sockaddr_in local_address_, remote_address_; |
100 |
|
|
int socket_; |
101 |
|
|
real_time_tools::RealTimeThread reading_thread_; |
102 |
|
|
|
103 |
|
|
double F_bias_[3], T_bias_[3]; |
104 |
|
|
|
105 |
|
|
#ifdef XENOMAI |
106 |
|
|
RT_MUTEX mutex_; |
107 |
|
|
RT_PIPE stream_pipe_; |
108 |
|
|
#else |
109 |
|
|
std::mutex mutex_; |
110 |
|
|
#endif |
111 |
|
|
|
112 |
|
|
bool initialized_; |
113 |
|
|
bool going_; |
114 |
|
|
bool streaming_; |
115 |
|
|
}; |
116 |
|
|
|
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
|
120 |
|
|
#endif /* ATIFTSENSOR_H_ */ |