Skip to content

Commit 24c10bf

Browse files
aentingeraentinger
authored
Merge pull request #88 from arduino-libraries/limit-angular-velocity
Add configuration of joint angular velocity and replace the outdated speed API.
2 parents 5353987 + 8a5cc94 commit 24c10bf

File tree

15 files changed

+65
-58
lines changed

15 files changed

+65
-58
lines changed

‎examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,11 @@ RecordAndReplayApp app;
1818

1919
void setup()
2020
{
21-
if (Braccio.begin(custom_main_menu)) {
21+
if (Braccio.begin(custom_main_menu))
22+
{
2223
app.enableButtons();
24+
/* Allow greater angular velocity than the default one. */
25+
Braccio.setAngularVelocity(45.0f);
2326
}
2427
}
2528

‎examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -54,8 +54,6 @@ void setup()
5454

5555
if (Braccio.begin(directionScreen))
5656
{
57-
/* Configure Braccio. */
58-
Braccio.speed(speed_grade_t(120)/*MEDIUM*/);
5957
/* Move to home position. */
6058
Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);
6159
delay(500);

‎examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,9 +18,6 @@ void setup() {
1818
Braccio.begin();
1919
delay(500); // Waits for the Braccio initialization
2020

21-
// You can choose the speed beforehand with
22-
Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW
23-
2421
// Send motors initial angle
2522
gripper.move().to(initialGripper);
2623
delay(100);

‎examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,6 @@ void setup() {
2121
Braccio.begin();
2222
delay(500); // Waits for the Braccio initialization
2323

24-
// You can choose the speed beforehand with
25-
Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW
26-
2724
// Set motors initial angle
2825
// Should move all the motors at once
2926
Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);

‎examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -20,9 +20,6 @@ void setup() {
2020
Braccio.begin();
2121
delay(500); // Waits for the Braccio initialization
2222

23-
// You can choose the speed beforehand with
24-
Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW
25-
2623
// Set motors initial angle
2724
// Should move all the motors at once
2825
Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);

‎examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -54,8 +54,6 @@ void setup()
5454

5555
if (Braccio.begin(directionScreen))
5656
{
57-
/* Configure Braccio. */
58-
Braccio.speed(speed_grade_t(120)/*MEDIUM*/);
5957
/* Move to home position. */
6058
Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);
6159
delay(500);

‎examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -54,8 +54,6 @@ void setup()
5454

5555
if (Braccio.begin(directionScreen))
5656
{
57-
/* Configure Braccio. */
58-
Braccio.speed(speed_grade_t(120)/*MEDIUM*/);
5957
/* Move to home position. */
6058
Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);
6159
delay(500);

‎examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,8 @@ void setup()
2020
{
2121
if (Braccio.begin(custom_main_menu)) {
2222
app.enableButtons();
23+
/* Allow greater angular velocity than the default one. */
24+
Braccio.setAngularVelocity(45.0f);
2325
}
2426
}
2527

‎examples/Tools/Braccio_Basic/Braccio_Basic.ino

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -79,15 +79,16 @@ void setup()
7979

8080
Braccio.moveTo(home_position[0], home_position[1], home_position[2], home_position[3], home_position[4], home_position[5]);
8181
delay(1000);
82+
Braccio.setAngularVelocity(45.0f); /* 45 deg/sec */
8283
}
8384

8485
void loop()
8586
{
8687
if (move_joint)
8788
{
88-
Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f).in(1s);
89-
delay(1000);
90-
Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f).in(1s);
91-
delay(1000);
89+
Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f);
90+
delay(2000);
91+
Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f);
92+
delay(2000);
9293
}
9394
}

‎examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,7 @@ bool set_initial_servo_position(int const id, float const target_angle)
8585
for ( float current_angle = Braccio.get(id).position();
8686
!isTargetAngleReached(EPSILON) && !isTimeout(start);)
8787
{
88-
Braccio.get(id).move().to(target_angle).in(200ms);
88+
Braccio.get(id).move().to(target_angle);
8989
delay(250);
9090

9191
char msg[64] = {0};

‎examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,13 +31,13 @@ void test_motor(int const id)
3131
delay(500);
3232

3333
Serial.print("Drive to start . ");
34-
Braccio.get(id).move().to(0.0f).in(1s);
34+
Braccio.get(id).move().to(0.0f);
3535
Serial.println("OK.");
3636
delay(1500);
3737

3838
for (float target_angle = 0.0f; target_angle < SmartServoClass::MAX_ANGLE; target_angle += 1.0f)
3939
{
40-
Braccio.get(id).move().to(target_angle).in(200ms);
40+
Braccio.get(id).move().to(target_angle);
4141
delay(250);
4242

4343
char msg[64] = {0};

‎src/Braccio++.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -136,7 +136,8 @@ bool BraccioClass::begin(voidFuncPtr custom_menu, bool const wait_for_all_motor_
136136
lvgl_defaultMenu();
137137

138138
_servos.begin();
139-
_servos.setTime(SLOW);
139+
_servos.setMaxTorque(SmartServoClass::TORQUE_MAX);
140+
_servos.setAngularVelocity(SmartServoClass::DEFAULT_ANGULAR_VELOCITY_deg_per_sec);
140141
_servos.setPositionMode(PositionMode::IMMEDIATE);
141142

142143
_motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));

‎src/Braccio++.h

Lines changed: 4 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -38,16 +38,6 @@
3838
#include <chrono>
3939
using namespace std::chrono;
4040

41-
/**************************************************************************************
42-
* TYPEDEF
43-
**************************************************************************************/
44-
45-
enum speed_grade_t {
46-
FAST = 10,
47-
MEDIUM = 100,
48-
SLOW = 1000,
49-
};
50-
5141
/**************************************************************************************
5242
* FORWARD DECLARATION
5343
**************************************************************************************/
@@ -83,8 +73,8 @@ class BraccioClass
8373
inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(max_torque); }
8474
inline void setMaxTorque(int const id, uint16_t const max_torque) { _servos.setMaxTorque(id, max_torque); }
8575

86-
inline void speed(speed_grade_t const speed_grade) { _servos.setTime(speed_grade); }
87-
inline void speed(int const id, speed_grade_t const speed_grade) { _servos.setTime(id, speed_grade); }
76+
inline void setAngularVelocity(float const angular_velocity_deg_per_sec) { _servos.setAngularVelocity(angular_velocity_deg_per_sec); }
77+
inline float getAngularVelocity() const { return _servos.getAngularVelocity(); }
8878

8979
inline void disengage(int const id = SmartServoClass::BROADCAST) { _servos.disengage(id); }
9080
inline void engage (int const id = SmartServoClass::BROADCAST) { _servos.engage(id); }
@@ -189,15 +179,12 @@ class Servo
189179
inline bool engaged() { return _servos.isEngaged(_id); }
190180
inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(_id, max_torque); }
191181

192-
inline Servo & move() { return *this; }
193-
inline Servo & to (float const angle) { _servos.setPosition(_id, angle); return *this; }
194-
inline Servo & in (std::chrono::milliseconds const len) { _servos.setTime(_id, len.count()); return *this; }
182+
inline Servo & move() { return *this; }
183+
inline Servo & to (float const angle) { _servos.setPosition(_id, angle); return *this; }
195184

196185
inline float position() { return _servos.getPosition(_id); }
197186
inline bool connected() { return Braccio.connected(_id); }
198187
inline void info(Stream & stream) { _servos.getInfo(stream, _id); }
199-
inline uint16_t runtime() { return _servos.getTime(_id); }
200-
inline void setRuntime(uint16_t const time) { _servos.setTime(_id, time); }
201188

202189
operator bool() { return connected(); }
203190

‎src/lib/motors/SmartServo.cpp

Lines changed: 39 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131
SmartServoClass::SmartServoClass(RS485Class & RS485)
3232
: _RS485{RS485}
3333
, _errors{0}
34+
, _angular_velocity_deg_per_sec{DEFAULT_ANGULAR_VELOCITY_deg_per_sec}
3435
, _onError{}
3536
, _mtx{}
3637
{
@@ -198,27 +199,43 @@ void SmartServoClass::begin()
198199
_targetSpeed[idToArrayIndex(i)] = 1000;
199200
}
200201

201-
void SmartServoClass::setPosition(uint8_t const id, float const angle)
202+
void SmartServoClass::setPosition(uint8_t const id, float const angle_deg)
202203
{
203-
if (!isValidAngle(angle))
204+
if (!isValidAngle(angle_deg))
204205
return;
205206

206-
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
207-
if (isValidId(id))
207+
if (!isValidId(id))
208+
return;
209+
210+
float const target_position_deg = angle_deg;
211+
float const actual_position_deg = getPosition(id);
212+
if (actual_position_deg < 0.0f)
213+
return;
214+
215+
float const abs_position_diff_deg = fabs(target_position_deg - actual_position_deg);
216+
float const limited_runtime_sec = abs_position_diff_deg / _angular_velocity_deg_per_sec;
217+
uint16_t const limited_runtime_ms = static_cast<uint16_t>(limited_runtime_sec * 1000.f);
218+
219+
if (_positionMode == PositionMode::IMMEDIATE)
208220
{
209-
_targetPosition[idToArrayIndex(id)] = angleToPosition(angle);
210-
if (_positionMode==PositionMode::IMMEDIATE) {
211-
writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle));
212-
}
213-
}
221+
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
222+
writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), limited_runtime_ms);
223+
writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(target_position_deg));
224+
}
225+
else if (_positionMode == PositionMode::SYNC)
226+
{
227+
_targetSpeed[idToArrayIndex(id)] = limited_runtime_ms;
228+
_targetPosition[idToArrayIndex(id)] = angleToPosition(target_position_deg);
229+
}
214230
}
215231

216232
float SmartServoClass::getPosition(uint8_t const id)
217233
{
234+
if (!isValidId(id))
235+
return -1.0f;
236+
218237
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
219-
float ret = -1;
220-
if (isValidId(id))
221-
return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
238+
return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
222239
}
223240

224241
void SmartServoClass::center(uint8_t const id, uint16_t const position)
@@ -261,16 +278,24 @@ void SmartServoClass::setTorque(uint8_t const id, bool const torque)
261278

262279
void SmartServoClass::setTime(uint16_t const time)
263280
{
264-
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
265281
for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++)
266282
_targetSpeed[idToArrayIndex(i)] = time;
283+
284+
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
267285
writeWordCmd(BROADCAST, REG(SmartServoRegister::RUN_TIME_H), time);
268286
}
269287

270288
void SmartServoClass::setTime(uint8_t const id, uint16_t const time)
271289
{
290+
if (!isValidId(id))
291+
return;
292+
293+
if (id == BROADCAST)
294+
return;
295+
296+
_targetSpeed[idToArrayIndex(id)] = time;
297+
272298
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
273-
if ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)) _targetSpeed[idToArrayIndex(id)] = time;
274299
writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time);
275300
}
276301

‎src/lib/motors/SmartServo.h

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -46,14 +46,15 @@ class SmartServoClass
4646

4747
inline void setPositionMode(PositionMode const mode) { _positionMode = mode; }
4848

49-
void setPosition(uint8_t const id, float const angle);
50-
49+
void setPosition(uint8_t const id, float const angle_deg);
5150
float getPosition(uint8_t const id);
5251

52+
inline void setAngularVelocity(float const angular_velocity_deg_per_sec) { _angular_velocity_deg_per_sec = angular_velocity_deg_per_sec; }
53+
inline float getAngularVelocity() const { return _angular_velocity_deg_per_sec; }
54+
5355
void synchronize();
5456

5557
void setTorque(bool const torque);
56-
5758
void setTorque(uint8_t const id, bool const torque);
5859

5960
void setMaxTorque(uint8_t const id, uint16_t const max_torque);
@@ -97,6 +98,7 @@ class SmartServoClass
9798
static int constexpr MAX_MOTOR_ID = 6;
9899
static int constexpr NUM_MOTORS = 6;
99100
static float constexpr MAX_ANGLE = 315.0f;
101+
static float constexpr DEFAULT_ANGULAR_VELOCITY_deg_per_sec = 20.0f;
100102

101103
static uint16_t constexpr TORQUE_MIN = 0;
102104
static uint16_t constexpr TORQUE_MAX = 1000;
@@ -129,6 +131,7 @@ class SmartServoClass
129131

130132
RS485Class& _RS485;
131133
int _errors;
134+
float _angular_velocity_deg_per_sec;
132135
mbed::Callback<void()> _onError;
133136
rtos::Mutex _mtx;
134137

0 commit comments

Comments
 (0)