@@ -36,9 +36,9 @@ using namespace std::chrono_literals;
36
36
37
37
BraccioClass::BraccioClass ()
38
38
: _i2c_mtx{}
39
- , serial485 {Serial1, 0 , 7 , 8 } /* TX, DE, RE */
40
- , servos{serial485 }
41
- , PD_UFP {PD_LOG_LEVEL_VERBOSE}
39
+ , _serial485 {Serial1, 0 , 7 , 8 } /* TX, DE, RE */
40
+ , _servos{_serial485 }
41
+ , _PD_UFP {PD_LOG_LEVEL_VERBOSE}
42
42
, _expander{TCA6424A_ADDRESS_ADDR_HIGH, _i2c_mtx}
43
43
, _is_ping_allowed{true }
44
44
, _is_motor_connected{false }
@@ -75,13 +75,13 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
75
75
static rtos::Thread th (osPriorityHigh);
76
76
th.start (mbed::callback (this , &BraccioClass::pd_thread));
77
77
attachInterrupt (PIN_FUSB302_INT, mbed::callback (this , &BraccioClass::unlock_pd_semaphore_irq), FALLING);
78
- pd_timer .attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 10ms);
78
+ _pd_timer .attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 10ms);
79
79
80
- PD_UFP .init_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
80
+ _PD_UFP .init_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
81
81
82
82
/*
83
83
while (millis() < 200) {
84
- PD_UFP .run();
84
+ _PD_UFP .run();
85
85
}
86
86
*/
87
87
@@ -97,11 +97,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
97
97
98
98
auto check_power_func = [this ]()
99
99
{
100
- if (!PD_UFP .is_PPS_ready ())
100
+ if (!_PD_UFP .is_PPS_ready ())
101
101
{
102
102
_i2c_mtx.lock ();
103
- PD_UFP .print_status (Serial);
104
- PD_UFP .set_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
103
+ _PD_UFP .print_status (Serial);
104
+ _PD_UFP .set_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
105
105
delay (10 );
106
106
_i2c_mtx.unlock ();
107
107
}
@@ -110,11 +110,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
110
110
lvgl_splashScreen (2000 , check_power_func);
111
111
lv_obj_clean (lv_scr_act ());
112
112
113
- if (!PD_UFP .is_PPS_ready ())
113
+ if (!_PD_UFP .is_PPS_ready ())
114
114
lvgl_pleaseConnectPower ();
115
115
116
116
/* Loop forever, if no power is attached. */
117
- while (!PD_UFP .is_PPS_ready ())
117
+ while (!_PD_UFP .is_PPS_ready ())
118
118
check_power_func ();
119
119
lv_obj_clean (lv_scr_act ());
120
120
@@ -123,9 +123,9 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
123
123
else
124
124
lvgl_defaultMenu ();
125
125
126
- servos .begin ();
127
- servos .setTime (SmartServoClass::BROADCAST, SLOW);
128
- servos .setPositionMode (PositionMode::IMMEDIATE);
126
+ _servos .begin ();
127
+ _servos .setTime (SmartServoClass::BROADCAST, SLOW);
128
+ _servos .setPositionMode (PositionMode::IMMEDIATE);
129
129
130
130
_motors_connected_thd.start (mbed::callback (this , &BraccioClass::motorConnectedThreadFunc));
131
131
@@ -152,7 +152,7 @@ bool BraccioClass::connected(int const id)
152
152
153
153
Servo BraccioClass::move (int const id)
154
154
{
155
- Servo wrapper (servos , id);
155
+ Servo wrapper (_servos , id);
156
156
return wrapper;
157
157
}
158
158
@@ -163,32 +163,32 @@ Servo BraccioClass::get(int const id)
163
163
164
164
void BraccioClass::moveTo (float const a1, float const a2, float const a3, float const a4, float const a5, float const a6)
165
165
{
166
- servos .setPositionMode (PositionMode::SYNC);
167
- servos .setPosition (1 , a1);
168
- servos .setPosition (2 , a2);
169
- servos .setPosition (3 , a3);
170
- servos .setPosition (4 , a4);
171
- servos .setPosition (5 , a5);
172
- servos .setPosition (6 , a6);
173
- servos .synchronize ();
174
- servos .setPositionMode (PositionMode::IMMEDIATE);
166
+ _servos .setPositionMode (PositionMode::SYNC);
167
+ _servos .setPosition (1 , a1);
168
+ _servos .setPosition (2 , a2);
169
+ _servos .setPosition (3 , a3);
170
+ _servos .setPosition (4 , a4);
171
+ _servos .setPosition (5 , a5);
172
+ _servos .setPosition (6 , a6);
173
+ _servos .synchronize ();
174
+ _servos .setPositionMode (PositionMode::IMMEDIATE);
175
175
}
176
176
177
177
void BraccioClass::positions (float * buffer)
178
178
{
179
179
for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
180
- *buffer++ = servos .getPosition (id);
180
+ *buffer++ = _servos .getPosition (id);
181
181
}
182
182
183
183
void BraccioClass::positions (float & a1, float & a2, float & a3, float & a4, float & a5, float & a6)
184
184
{
185
185
// TODO: add check if motors are actually connected
186
- a1 = servos .getPosition (1 );
187
- a2 = servos .getPosition (2 );
188
- a3 = servos .getPosition (3 );
189
- a4 = servos .getPosition (4 );
190
- a5 = servos .getPosition (5 );
191
- a6 = servos .getPosition (6 );
186
+ a1 = _servos .getPosition (1 );
187
+ a2 = _servos .getPosition (2 );
188
+ a3 = _servos .getPosition (3 );
189
+ a4 = _servos .getPosition (4 );
190
+ a5 = _servos .getPosition (5 );
191
+ a6 = _servos .getPosition (6 );
192
192
}
193
193
194
194
int BraccioClass::getKey () {
@@ -313,7 +313,7 @@ void BraccioClass::motorConnectedThreadFunc()
313
313
{
314
314
for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
315
315
{
316
- bool const is_connected = (servos .ping (id) == 0 );
316
+ bool const is_connected = (_servos .ping (id) == 0 );
317
317
setMotorConnectionStatus (id, is_connected);
318
318
}
319
319
@@ -432,23 +432,23 @@ void BraccioClass::pd_thread() {
432
432
start_pd_burst = millis ();
433
433
size_t last_time_ask_pps = 0 ;
434
434
while (1 ) {
435
- auto ret = pd_events .wait_any (0xFF );
435
+ auto ret = _pd_events .wait_any (0xFF );
436
436
if ((ret & 1 ) && (millis () - start_pd_burst > 1000 )) {
437
- pd_timer .detach ();
438
- pd_timer .attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 5s);
437
+ _pd_timer .detach ();
438
+ _pd_timer .attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 5s);
439
439
}
440
440
if (ret & 2 ) {
441
- pd_timer .detach ();
442
- pd_timer .attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 50ms);
441
+ _pd_timer .detach ();
442
+ _pd_timer .attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 50ms);
443
443
}
444
444
_i2c_mtx.lock ();
445
445
if (millis () - last_time_ask_pps > 5000 ) {
446
- PD_UFP .set_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
446
+ _PD_UFP .set_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
447
447
last_time_ask_pps = millis ();
448
448
}
449
- PD_UFP .run ();
449
+ _PD_UFP .run ();
450
450
_i2c_mtx.unlock ();
451
- if (PD_UFP .is_power_ready () && PD_UFP .is_PPS_ready ()) {
451
+ if (_PD_UFP .is_power_ready () && _PD_UFP .is_PPS_ready ()) {
452
452
453
453
}
454
454
}
0 commit comments