Skip to content

Commit b7477f5

Browse files
slimasziomslimasziom
committed
CHG: remove hardcoded values and handle reigsters without iterators
1 parent 94f83f8 commit b7477f5

File tree

5 files changed

+105
-59
lines changed

5 files changed

+105
-59
lines changed

‎robotiq_hande_driver/include/communication.hpp‎

Lines changed: 52 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,25 @@
1313
namespace hande_driver
1414
{
1515
constexpr auto kRegisterWordLength = 3;
16+
enum OutputBytes{
17+
OUTPUT_BYTES_RESERVED_1,
18+
OUTPUT_BYTES_ACTION_REQUEST,
19+
OUTPUT_BYTES_POSITION_REQUEST,
20+
OUTPUT_BYTES_RESERVED_2,
21+
OUTPUT_BYTES_FORCE,
22+
OUTPUT_BYTES_SPEED,
23+
OUTPUT_BYTES_MAX
24+
};
25+
26+
enum InputBytes{
27+
INPUT_BYTES_RESERVED_1,
28+
INPUT_BYTES_GRIPPER_STATUS,
29+
INPUT_BYTES_POSITION_REQUEST_ECHO,
30+
INPUT_BYTES_FAULT_STATUS,
31+
INPUT_BYTES_CURRENT,
32+
INPUT_BYTES_POSITION,
33+
INPUT_BYTES_MAX
34+
};
1635

1736
class Communication{
1837
public:
@@ -47,12 +66,43 @@ class Communication{
4766
*/
4867
void read_write_registers();
4968

50-
uint16_t input_registers_[kRegisterWordLength];
51-
uint16_t output_registers_[kRegisterWordLength];
69+
/**
70+
* @brief Set output bytes to zeros
71+
*
72+
* @param none
73+
* @return none
74+
* @note see status on success, exception thrown in case of communication issues
75+
*/
76+
void clear_output_bytes();
77+
78+
/**
79+
* @brief Get input byte value
80+
*
81+
* @param index InputBytes byte index
82+
* @return requested byte value
83+
* @note see status on success, exception thrown in case of communication issues
84+
*/
85+
uint8_t get_input_byte(InputBytes index);
86+
87+
/**
88+
* @brief Set output byte value
89+
*
90+
* @param index OutputBytes byte index
91+
* @param value to be set
92+
* @return none
93+
* @note see status on success, exception thrown in case of communication issues
94+
*/
95+
void set_output_byte(OutputBytes index, uint8_t value);
96+
97+
// uint16_t input_registers_[kRegisterWordLength];
98+
// uint16_t output_registers_[kRegisterWordLength];
5299

53100
private:
54101
modbus_t *mb_;
55102

103+
uint8_t input_bytes_[INPUT_BYTES_MAX];
104+
uint8_t output_bytes_[OUTPUT_BYTES_MAX];
105+
56106
};
57107
} // namespace hande_driver
58108
#endif // COMMUNICATION_HPP_

‎robotiq_hande_driver/include/protocol_logic.hpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -232,13 +232,13 @@ class ProtocolLogic{
232232
void refresh_registers();
233233

234234
/**
235-
* @brief Read 8bit registers from 16bit words
235+
* @brief Set n-th bit to action request byte
236236
*
237-
* @param reg pointer to 8bit register
238-
* @param byte 8bit register position in 16bit modbus frame
237+
* @param position_bit n-th bit in byte
238+
* @param value bool value
239239
* @return none
240240
*/
241-
void read_register(uint8_t &reg, uint8_t byte);
241+
void write_action_bit(uint8_t position_bit, bool value);
242242

243243
/**
244244
* @brief Set n-th bit to x value

‎robotiq_hande_driver/src/application.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -44,12 +44,12 @@ void ApplicationLayer::activate(){
4444

4545

4646
void ApplicationLayer::open(){
47-
protocol_logic_.go_to(0, kMaxSpeed, kMaxForce);
47+
set_position(kGripperPositionMax);
4848
}
4949

5050

5151
void ApplicationLayer::close(){
52-
protocol_logic_.go_to(255, kMaxSpeed, kMaxForce);
52+
set_position(kGripperPositionMin);
5353
}
5454

5555
ApplicationLayer::Status ApplicationLayer::get_status(){
@@ -91,7 +91,7 @@ void ApplicationLayer::read(){
9191

9292
//fault_status
9393

94-
requested_position_ = (double)protocol_logic_.get_reg_pos() * kGripperPositionStep + kGripperPositionMin;
94+
requested_position_ = kGripperPositionMax - (double)protocol_logic_.get_reg_pos() * kGripperPositionStep;
9595
position_ = kGripperPositionMax - (double)protocol_logic_.get_pos() * kGripperPositionStep;
9696
current_ = (double)protocol_logic_.get_current() * kGripperCurrentScale;
9797
}

‎robotiq_hande_driver/src/communication.cpp‎

Lines changed: 19 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
#include <cstdio>
44
#include <stdint.h>
5+
#include <string.h>
56

67

78
namespace hande_driver {
@@ -18,8 +19,10 @@ constexpr uint16_t kGripperOutputFirstReg = 0x07D0;
1819
constexpr uint16_t kGripperInputFirstReg = 0x03E8;
1920

2021
Communication::Communication()
21-
: input_registers_{0, 0, 0}
22-
, output_registers_{0, 0, 0}
22+
// : input_registers_{0, 0, 0}
23+
// , output_registers_{0, 0, 0}
24+
: input_bytes_{0, 0, 0, 0, 0, 0}
25+
, output_bytes_{0, 0, 0, 0, 0, 0}
2326
{
2427
printf("Communication constructor\n");
2528
mb_ = modbus_new_rtu(kDeviceName, kBaudrate, kParity, kDataBits, kStopBit);
@@ -49,9 +52,21 @@ void Communication::read_write_registers(){
4952
modbus_write_and_read_registers(mb_,
5053
kGripperInputFirstReg,
5154
kRegisterWordLength,
52-
output_registers_,
55+
(uint16_t *)output_bytes_,
5356
kGripperOutputFirstReg,
5457
kRegisterWordLength,
55-
input_registers_);
58+
(uint16_t *)input_bytes_);
59+
}
60+
61+
void Communication::clear_output_bytes(){
62+
memset(output_bytes_, 0, sizeof(output_bytes_));
63+
}
64+
65+
uint8_t Communication::get_input_byte(InputBytes index){
66+
return input_bytes_[index];
67+
}
68+
69+
void Communication::set_output_byte(OutputBytes index, uint8_t value){
70+
output_bytes_[index] = value;
5671
}
5772
} // namespace hande_driver

‎robotiq_hande_driver/src/protocol_logic.cpp‎

Lines changed: 27 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,9 @@
55

66
namespace hande_driver {
77

8+
constexpr auto kGripperPositionOpenedThreshold = 230;
9+
constexpr auto kGripperPositionClosedThreshold = 13;
10+
811
ProtocolLogic::ProtocolLogic()
912
: status_(0)
1013
, activation_status_(GRIPPER_RESET)
@@ -24,41 +27,24 @@ ProtocolLogic::~ProtocolLogic(){
2427
}
2528

2629
void ProtocolLogic::reset(){
27-
communication_.output_registers_[0] = 0x0000;
28-
communication_.output_registers_[1] = 0x0000;
29-
communication_.output_registers_[2] = 0x0000;
30+
communication_.clear_output_bytes();
3031

31-
communication_.output_registers_[kActionRequestByte / 2] = bit_set_to(
32-
communication_.output_registers_[kActionRequestByte / 2],
33-
(kActionRequestByte % 2 == 0 ? 8 : 0) + kActivatePositionByte,
34-
DEACTIVATE_GRIPPER);
32+
write_action_bit(kActivatePositionByte, DEACTIVATE_GRIPPER);
3533

3634
communication_.read_write_registers();
3735
}
3836

3937
void ProtocolLogic::set(){
40-
communication_.output_registers_[0] = 0x0000;
41-
communication_.output_registers_[1] = 0x0000;
42-
communication_.output_registers_[2] = 0x0000;
38+
communication_.clear_output_bytes();
4339

44-
communication_.output_registers_[kActionRequestByte / 2] = bit_set_to(
45-
communication_.output_registers_[kActionRequestByte / 2],
46-
(kActionRequestByte % 2 == 0 ? 8 : 0) + kActivatePositionByte,
47-
ACTIVATE_GRIPPER);
40+
write_action_bit(kActivatePositionByte, ACTIVATE_GRIPPER);
4841

4942
communication_.read_write_registers();
5043
}
5144

5245
void ProtocolLogic::auto_release(){
53-
communication_.output_registers_[kActionRequestByte / 2] = bit_set_to(
54-
communication_.output_registers_[kActionRequestByte / 2],
55-
(kActionRequestByte % 2 == 0 ? 8 : 0) + kAutomaticReleasePositionByte,
56-
EMERGENCY_AUTO_RELEASE);
57-
58-
communication_.output_registers_[kActionRequestByte / 2] = bit_set_to(
59-
communication_.output_registers_[kActionRequestByte / 2],
60-
(kActionRequestByte % 2 == 0 ? 8 : 0) + kAutomaticReleasePositionByte,
61-
OPENING);
46+
write_action_bit(kAutomaticReleasePositionByte, EMERGENCY_AUTO_RELEASE);
47+
write_action_bit(kAutoReleaseDirectionPositionByte, OPENING);
6248

6349
communication_.read_write_registers();
6450
}
@@ -69,22 +55,17 @@ void ProtocolLogic::activate(){
6955
}
7056

7157
void ProtocolLogic::go_to(uint8_t position, uint8_t velocity, uint8_t force){
72-
communication_.output_registers_[kActionRequestByte / 2] = bit_set_to(
73-
communication_.output_registers_[kActionRequestByte / 2],
74-
(kActionRequestByte % 2 == 0 ? 8 : 0) + kGoToPositionByte,
75-
GO_TO_REQ_POS);
58+
write_action_bit(kGoToPositionByte, GO_TO_REQ_POS);
7659

77-
communication_.output_registers_[1] = uint16_t(0x00 << 8 | position);
78-
communication_.output_registers_[2] = uint16_t(velocity << 8 | force);
60+
communication_.set_output_byte(OUTPUT_BYTES_POSITION_REQUEST, position);
61+
communication_.set_output_byte(OUTPUT_BYTES_SPEED, velocity);
62+
communication_.set_output_byte(OUTPUT_BYTES_FORCE, force);
7963

8064
communication_.read_write_registers();
8165
}
8266

8367
void ProtocolLogic::stop(){
84-
communication_.output_registers_[kActionRequestByte / 2] = bit_set_to(
85-
communication_.output_registers_[kActionRequestByte / 2],
86-
(kActionRequestByte % 2 == 0 ? 8 : 0) + kGoToPositionByte,
87-
STOP);
68+
write_action_bit(kGoToPositionByte, STOP);
8869

8970
communication_.read_write_registers();
9071
}
@@ -109,11 +90,12 @@ bool ProtocolLogic::is_stopped(){
10990
}
11091

11192
bool ProtocolLogic::is_closed(){
112-
return position_ <= 13;
93+
return position_ >= kGripperPositionOpenedThreshold;
94+
11395
}
11496

11597
bool ProtocolLogic::is_opened(){
116-
return position_ >= 230;
98+
return position_ <= kGripperPositionClosedThreshold;
11799
}
118100

119101
bool ProtocolLogic::obj_detected(){
@@ -136,30 +118,29 @@ uint8_t ProtocolLogic::get_current(){
136118
void ProtocolLogic::refresh_registers(){
137119
communication_.read_write_registers();
138120

139-
read_register(status_, kStatusByte);
121+
status_ = communication_.get_input_byte(INPUT_BYTES_GRIPPER_STATUS);
140122

141123
activation_status_ = (ActivationStatus)((status_>>kActivationStatusPositionByte) & 1u);
142124
action_status_ = (ActionStatus)((status_>>kActionStatusPositionByte) & 1u);
143125
gripper_status_ = (GripperStatus)((status_>>kGripperStatusPositionByte) & 3u);
144126
object_detection_status_ = (ObjectDetectionStatus)((status_>>kObjectDetectionStatusPositionByte) & 3u);
145127

146-
read_register(fault_status_, kFaultStatusByte);
128+
fault_status_ = communication_.get_input_byte(INPUT_BYTES_FAULT_STATUS);
147129
//To bo specified
148130

149-
read_register(position_request_echo_, kPositionRequestEchoByte);
150-
read_register(position_, kPositionByte);
151-
read_register(current_, kCurrentByte);
131+
position_request_echo_ = communication_.get_input_byte(INPUT_BYTES_POSITION_REQUEST_ECHO);
132+
position_ = communication_.get_input_byte(INPUT_BYTES_POSITION);
133+
current_ = communication_.get_input_byte(INPUT_BYTES_CURRENT);
152134
}
153135

154-
void ProtocolLogic::read_register(uint8_t &reg, uint8_t byte){
155-
// TODO: add byte references in constructor
156-
reg = byte % 2 == 1 ?
157-
communication_.input_registers_[byte / 2] & 255u :
158-
communication_.input_registers_[byte / 2] >> 8u;
136+
void ProtocolLogic::write_action_bit(uint8_t position_bit, bool value){
137+
// TODO: change byte from uint to &uint
138+
// byte = bit_set_to(byte, position, value)
139+
communication_.output_registers_[0] = bit_set_to(
140+
communication_.output_registers_[0], 8 + position_bit, value);
159141
}
160142

161143
inline uint ProtocolLogic::bit_set_to(uint number, uint n, bool x) {
162-
//TODO: include byte/bit logic
163144
return (number & ~((uint)1 << n)) | ((uint)x << n);
164145
}
165146
} // namespace hande_driver

0 commit comments

Comments
 (0)