Skip to content

Commit 3093132

Browse files
aentingeraentinger
authored
Merge pull request #29 from bcmi-labs/fix-connection-code
Fix: Only ping within the motor connection thread.
2 parents 37a1918 + cac386f commit 3093132

File tree

5 files changed

+138
-87
lines changed

5 files changed

+138
-87
lines changed

‎examples/Braccio__Template/Braccio__Template.ino‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,9 @@ static void event_handler(lv_event_t * e)
1414
Serial.println(txt);
1515
if (strcmp(txt, "Demo") == 0) {
1616
demo_mode = 1;
17-
Braccio.ping_allowed = false;
17+
Braccio.pingOff();
1818
} else {
19-
Braccio.ping_allowed = true;
19+
Braccio.pingOn();
2020
demo_mode = 0;
2121
}
2222
}

‎examples/LCD_Motors/LCD_Motors.ino‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -14,24 +14,24 @@ static void event_handler(lv_event_t * e)
1414
Serial.println(txt);
1515
if (strcmp(txt, "Motor 1") == 0) {
1616
selected_motor = 1;
17-
Braccio.ping_allowed = false;
17+
Braccio.pingOff();
1818
} else if (strcmp(txt, "Motor 2") == 0) {
1919
selected_motor = 2;
20-
Braccio.ping_allowed = false;
20+
Braccio.pingOff();
2121
} else if (strcmp(txt, "Motor 3") == 0) {
2222
selected_motor = 3;
23-
Braccio.ping_allowed = false;
23+
Braccio.pingOff();
2424
} else if (strcmp(txt, "Motor 4") == 0) {
2525
selected_motor = 4;
26-
Braccio.ping_allowed = false;
26+
Braccio.pingOff();
2727
} else if (strcmp(txt, "Motor 5") == 0) {
2828
selected_motor = 5;
29-
Braccio.ping_allowed = false;
29+
Braccio.pingOff();
3030
} else if (strcmp(txt, "Motor 6") == 0) {
3131
selected_motor = 6;
32-
Braccio.ping_allowed = false;
32+
Braccio.pingOff();
3333
} else {
34-
Braccio.ping_allowed = true;
34+
Braccio.pingOn();
3535
selected_motor = 0;
3636
}
3737
}

‎src/Braccio++.cpp‎

Lines changed: 66 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -20,15 +20,19 @@ BraccioClass::BraccioClass()
2020
, PD_UFP{PD_LOG_LEVEL_VERBOSE}
2121
, expander{TCA6424A_ADDRESS_ADDR_HIGH}
2222
, bl{}
23-
, _display_thread{}
23+
, _display_thd{}
24+
, _is_ping_allowed{true}
25+
, _is_motor_connected{false}
26+
, _motors_connected_mtx{}
27+
, _motors_connected_thd{}
2428
, runTime{SLOW}
2529
, _customMenu{nullptr}
2630
{
2731

2832
}
2933

30-
bool BraccioClass::begin(voidFuncPtr customMenu) {
31-
34+
bool BraccioClass::begin(voidFuncPtr customMenu)
35+
{
3236
Wire.begin();
3337
Serial.begin(115200);
3438

@@ -81,6 +85,11 @@ bool BraccioClass::begin(voidFuncPtr customMenu) {
8185
expander.setPinDirection(18, 0); // P22 = 8 * 2 + 2
8286
expander.writePin(18, 0); // reset LCD
8387
expander.writePin(18, 1); // LCD out of reset
88+
89+
/* Set all motor status LEDs to red. */
90+
for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
91+
setRed(id);
92+
}
8493
i2c_mutex.unlock();
8594

8695
pinMode(BTN_LEFT, INPUT_PULLUP);
@@ -122,7 +131,7 @@ bool BraccioClass::begin(voidFuncPtr customMenu) {
122131
p_objGroup = lv_group_create();
123132
lv_group_set_default(p_objGroup);
124133

125-
_display_thread.start(mbed::callback(this, &BraccioClass::display_thread));
134+
_display_thd.start(mbed::callback(this, &BraccioClass::display_thread_func));
126135

127136
auto check_power_func = [this]()
128137
{
@@ -156,20 +165,40 @@ bool BraccioClass::begin(voidFuncPtr customMenu) {
156165
servos.begin();
157166
servos.setPositionMode(PositionMode::IMMEDIATE);
158167

159-
#ifdef __MBED__
160-
static rtos::Thread connected_th;
161-
connected_th.start(mbed::callback(this, &BraccioClass::motors_connected_thread));
162-
#endif
168+
_motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));
163169

164170
return true;
165171
}
166172

173+
void BraccioClass::pingOn()
174+
{
175+
mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
176+
_is_ping_allowed = true;
177+
}
178+
179+
void BraccioClass::pingOff()
180+
{
181+
mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
182+
_is_ping_allowed = false;
183+
}
184+
185+
bool BraccioClass::connected(int const id)
186+
{
187+
mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
188+
return _is_motor_connected[SmartServoClass::idToArrayIndex(id)];
189+
}
190+
167191
MotorsWrapper BraccioClass::move(int const id)
168192
{
169193
MotorsWrapper wrapper(servos, id);
170194
return wrapper;
171195
}
172196

197+
MotorsWrapper BraccioClass::get(int const id)
198+
{
199+
return move(id);
200+
}
201+
173202
void BraccioClass::moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6)
174203
{
175204
servos.setPositionMode(PositionMode::SYNC);
@@ -231,7 +260,7 @@ void BraccioClass::pd_thread() {
231260
}
232261
}
233262

234-
void BraccioClass::display_thread()
263+
void BraccioClass::display_thread_func()
235264
{
236265
for(;;)
237266
{
@@ -285,21 +314,36 @@ void BraccioClass::defaultMenu()
285314
lv_obj_set_pos(label1, 0, 0);
286315
}
287316

288-
void BraccioClass::motors_connected_thread() {
289-
while (1) {
290-
if (ping_allowed) {
291-
for (int i = 1; i < 7; i++) {
292-
_connected[i] = (servos.ping(i) == 0);
293-
//Serial.print(String(i) + ": ");
294-
//Serial.println(_connected[i]);
317+
bool BraccioClass::isPingAllowed()
318+
{
319+
mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
320+
return _is_ping_allowed;
321+
}
322+
323+
void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connected)
324+
{
325+
mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
326+
_is_motor_connected[SmartServoClass::idToArrayIndex(id)] = is_connected;
327+
}
328+
329+
void BraccioClass::motorConnectedThreadFunc()
330+
{
331+
for (;;)
332+
{
333+
if (isPingAllowed())
334+
{
335+
for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
336+
{
337+
bool const is_connected = (servos.ping(id) == 0);
338+
setMotorConnectionStatus(id, is_connected);
295339
}
340+
296341
i2c_mutex.lock();
297-
for (int i = 1; i < 7; i++) {
298-
if (_connected[i]) {
299-
setGreen(i);
300-
} else {
301-
setRed(i);
302-
}
342+
for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
343+
if (connected(id))
344+
setGreen(id);
345+
else
346+
setRed(id);
303347
}
304348
i2c_mutex.unlock();
305349
}

‎src/Braccio++.h‎

Lines changed: 60 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -36,47 +36,7 @@ enum speed_grade_t {
3636
#include <chrono>
3737
using namespace std::chrono;
3838

39-
class MotorsWrapper {
40-
public:
41-
MotorsWrapper(SmartServoClass & servos, int idx) : _servos(servos), _idx(idx) {}
42-
MotorsWrapper& to(float angle) {
43-
_servos.setPosition(_idx, angle, _speed);
44-
return *this;
45-
}
46-
MotorsWrapper& in(std::chrono::milliseconds len) {
47-
_servos.setTime(_idx, len.count());
48-
return *this;
49-
}
50-
MotorsWrapper& move() {
51-
return *this;
52-
}
53-
float position() {
54-
return _servos.getPosition(_idx);
55-
}
56-
bool connected() {
57-
return _servos.ping(_idx) == 0;
58-
}
59-
operator bool() {
60-
return connected();
61-
}
62-
void info(Stream& stream) {
63-
_servos.getInfo(stream, _idx);
64-
}
65-
void disengage() {
66-
_servos.disengage(_idx);
67-
}
68-
void engage() {
69-
_servos.engage(_idx);
70-
}
71-
bool engaged() {
72-
return _servos.isEngaged(_idx);
73-
}
74-
75-
private:
76-
SmartServoClass & _servos;
77-
int _idx;
78-
int _speed = 100;
79-
};
39+
class MotorsWrapper;
8040

8141
class BraccioClass
8242
{
@@ -88,18 +48,19 @@ class BraccioClass
8848
bool begin(voidFuncPtr customMenu);
8949

9050

91-
MotorsWrapper move(int const id);
92-
inline MotorsWrapper get(int const id) { return move(id); }
51+
void pingOn();
52+
void pingOff();
53+
bool connected(int const id);
54+
55+
56+
MotorsWrapper move(int const id);
57+
MotorsWrapper get (int const id);
9358

9459
void moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6);
9560
void positions(float * buffer);
9661
void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
9762

9863

99-
bool connected(int joint_index) {
100-
return _connected[joint_index];
101-
}
102-
10364
void speed(speed_grade_t speed_grade) {
10465
runTime = speed_grade;
10566
}
@@ -124,8 +85,6 @@ class BraccioClass
12485

12586
TFT_eSPI gfx = TFT_eSPI();
12687

127-
bool ping_allowed = true;
128-
12988
static BraccioClass& get_default_instance() {
13089
static BraccioClass dev;
13190
return dev;
@@ -151,7 +110,16 @@ class BraccioClass
151110
PD_UFP_log_c PD_UFP;
152111
TCA6424A expander;
153112
Backlight bl;
154-
rtos::Thread _display_thread;
113+
rtos::Thread _display_thd;
114+
void display_thread_func();
115+
116+
bool _is_ping_allowed;
117+
bool _is_motor_connected[SmartServoClass::NUM_MOTORS];
118+
rtos::Mutex _motors_connected_mtx;
119+
rtos::Thread _motors_connected_thd;
120+
bool isPingAllowed();
121+
void setMotorConnectionStatus(int const id, bool const is_connected);
122+
void motorConnectedThreadFunc();
155123

156124
speed_grade_t runTime; //ms
157125

@@ -172,8 +140,6 @@ class BraccioClass
172140
lv_indev_t *kb_indev;
173141
lv_style_t _lv_style;
174142

175-
bool _connected[8];
176-
177143
#ifdef __MBED__
178144
rtos::EventFlags pd_events;
179145
rtos::Mutex i2c_mutex;
@@ -201,14 +167,54 @@ class BraccioClass
201167
}
202168

203169
void pd_thread();
204-
void display_thread();
205-
void motors_connected_thread();
206170
#endif
207171

208172
};
209173

210174
#define Braccio BraccioClass::get_default_instance()
211175

176+
class MotorsWrapper {
177+
public:
178+
MotorsWrapper(SmartServoClass & servos, int idx) : _servos(servos), _idx(idx) {}
179+
MotorsWrapper& to(float angle) {
180+
_servos.setPosition(_idx, angle, _speed);
181+
return *this;
182+
}
183+
MotorsWrapper& in(std::chrono::milliseconds len) {
184+
_servos.setTime(_idx, len.count());
185+
return *this;
186+
}
187+
MotorsWrapper& move() {
188+
return *this;
189+
}
190+
float position() {
191+
return _servos.getPosition(_idx);
192+
}
193+
194+
inline bool connected() { return Braccio.connected(_idx); }
195+
196+
operator bool() {
197+
return connected();
198+
}
199+
void info(Stream& stream) {
200+
_servos.getInfo(stream, _idx);
201+
}
202+
void disengage() {
203+
_servos.disengage(_idx);
204+
}
205+
void engage() {
206+
_servos.engage(_idx);
207+
}
208+
bool engaged() {
209+
return _servos.isEngaged(_idx);
210+
}
211+
212+
private:
213+
SmartServoClass & _servos;
214+
int _idx;
215+
int _speed = 100;
216+
};
217+
212218
struct __callback__container__ {
213219
mbed::Callback<void()> fn;
214220
};

‎src/lib/motors/SmartServo.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -74,18 +74,19 @@ class SmartServoClass
7474
static int constexpr BROADCAST = 0xFE;
7575
static int constexpr MIN_MOTOR_ID = 1;
7676
static int constexpr MAX_MOTOR_ID = 6;
77+
static int constexpr NUM_MOTORS = 6;
7778
static float constexpr MAX_ANGLE = 315.0f;
7879

80+
static int idToArrayIndex(int const id) { return (id - 1); }
81+
7982
private:
8083

81-
static int constexpr NUM_MOTORS = 6;
8284
static int constexpr MAX_TX_PAYLOAD_LEN = (5*NUM_MOTORS+4);
8385
static int constexpr MAX_RX_PAYLOAD_LEN = 10;
8486
static int constexpr MAX_POSITION = 4000;
8587

8688
inline bool isValidAngle(float const angle) { return ((angle >= 0.0f) && (angle <= MAX_ANGLE)); }
8789
inline bool isValidId(int const id) const { return ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)); }
88-
inline int idToArrayIndex(int const id) const { return (id - 1); }
8990

9091
int calcChecksum ();
9192
void sendPacket ();

0 commit comments

Comments
 (0)