Skip to content

Commit 61674d1

Browse files
aentingeraentinger
authored
Merge pull request #27 from bcmi-labs/fix-angle
Fix: The maximum achievable angle in servo mode is 315°, not 360°.
2 parents 0f3d34c + da8e36d commit 61674d1

File tree

4 files changed

+76
-43
lines changed

4 files changed

+76
-43
lines changed

‎examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino

Lines changed: 48 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -3,18 +3,59 @@
33
* angle for each smart servo motor.
44
*/
55

6+
/**************************************************************************************
7+
* INCLUDE
8+
**************************************************************************************/
9+
610
#include <Braccio++.h>
711

12+
/**************************************************************************************
13+
* FUNCTIONS
14+
**************************************************************************************/
15+
16+
void test_motor(int const id)
17+
{
18+
Serial.print("Connecting ..... ");
19+
for (; !Braccio.get(id).connected(); delay(10)) { }
20+
Serial.println("OK.");
21+
delay(500);
22+
23+
Serial.print("Disengaging .... ");
24+
Braccio.get(id).disengage();
25+
Serial.println("OK.");
26+
delay(500);
27+
28+
Serial.print("Engaging ....... ");
29+
Braccio.get(id).engage();
30+
Serial.println("OK.");
31+
delay(500);
32+
33+
Serial.print("Drive to start . ");
34+
Braccio.get(id).move().to(0.0f).in(1s);
35+
Serial.println("OK.");
36+
delay(1500);
37+
38+
for (float target_angle = 0.0f; target_angle < SmartServoClass::MAX_ANGLE; target_angle += 1.0f)
39+
{
40+
Braccio.get(id).move().to(target_angle).in(200ms);
41+
delay(250);
42+
43+
char msg[64] = {0};
44+
snprintf(msg, sizeof(msg), "Angle (Target | Current) = %.2f | %.2f", target_angle, Braccio.get(id).position());
45+
Serial.println(msg);
46+
}
47+
}
48+
49+
/**************************************************************************************
50+
* SETUP/LOOP
51+
**************************************************************************************/
52+
853
void setup()
954
{
1055
Serial.begin(115200);
1156
while(!Serial){}
1257

13-
/* Call Braccio.begin() for default menu
14-
* or pass a function for custom menu.
15-
*/
1658
Braccio.begin();
17-
1859
Serial.println("Testing motor angular movement!");
1960
}
2061

@@ -24,25 +65,13 @@ void loop()
2465
Serial.println(">> ");
2566

2667
while((Serial.available() <= 0)) { }
27-
int const selected_motor = Serial.parseInt();
68+
int const motor_id = Serial.parseInt();
2869
while(Serial.read() != '\n') { }
2970

30-
if (selected_motor < 1 || selected_motor > 6) {
71+
if (motor_id < 1 || motor_id > 6) {
3172
Serial.println("Error, wrong motor id, choose motor id between 1 and 6");
3273
return;
3374
}
3475

35-
float const ANGLE_START = 0.0;
36-
float const ANGLE_STOP = 180.0;
37-
float const ANGLE_INCREMENT = 10.0;
38-
39-
for (float angle = ANGLE_START; angle <= ANGLE_STOP; angle += ANGLE_INCREMENT)
40-
{
41-
Braccio.move(selected_motor).to(angle);
42-
Serial.print("Target angle: " + String(angle));
43-
Serial.print(" | ");
44-
Serial.print("Current angle: " + String(Braccio.get(selected_motor).position()));
45-
Serial.println();
46-
delay(100);
47-
}
76+
test_motor(motor_id);
4877
}

‎src/Braccio++.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -255,4 +255,4 @@ inline void attachInterrupt(pin_size_t interruptNum, mbed::Callback<void()> func
255255
attachInterruptParam(interruptNum, callback, mode, (void*)a);
256256
}
257257

258-
#endif //__BRACCIO_PLUSPLUS_H__
258+
#endif //__BRACCIO_PLUSPLUS_H__

‎src/lib/motors/SmartServo.cpp

Lines changed: 18 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,11 @@ int SmartServoClass::begin() {
180180
}
181181
}
182182

183-
void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) {
183+
void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed)
184+
{
185+
if (!isValidAngle(angle))
186+
return;
187+
184188
mutex.lock();
185189
if (isValidId(id))
186190
{
@@ -295,27 +299,31 @@ void SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const ti
295299
mutex.unlock();
296300
}
297301

298-
void SmartServoClass::setMinAngle(float const angle) {
302+
void SmartServoClass::setMinAngle(uint16_t const min_angle)
303+
{
299304
mutex.lock();
300-
writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
305+
writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle);
301306
mutex.unlock();
302307
}
303308

304-
void SmartServoClass::setMinAngle(uint8_t const id, float const angle) {
309+
void SmartServoClass::setMinAngle(uint8_t const id, uint16_t const min_angle)
310+
{
305311
mutex.lock();
306-
writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
312+
writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle);
307313
mutex.unlock();
308314
}
309315

310-
void SmartServoClass::setMaxAngle(float const angle) {
316+
void SmartServoClass::setMaxAngle(uint16_t const max_angle)
317+
{
311318
mutex.lock();
312-
writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
319+
writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle);
313320
mutex.unlock();
314321
}
315322

316-
void SmartServoClass::setMaxAngle(uint8_t const id, float const angle) {
323+
void SmartServoClass::setMaxAngle(uint8_t const id, uint16_t const max_angle)
324+
{
317325
mutex.lock();
318-
writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
326+
writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle);
319327
mutex.unlock();
320328
}
321329

@@ -336,4 +344,4 @@ void SmartServoClass::getInfo(Stream & stream, uint8_t const id) {
336344
for (i = 0; i < sizeof(regs); i++) {
337345
stream.println(String(i, HEX) + " : " + String(regs[i], HEX));
338346
}
339-
}
347+
}

‎src/lib/motors/SmartServo.h

Lines changed: 9 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -50,13 +50,10 @@ class SmartServoClass
5050

5151
void setStallProtectionTime(uint8_t const id, uint8_t const time);
5252

53-
void setMinAngle(float const angle);
54-
55-
void setMinAngle(uint8_t const id, float const angle);
56-
57-
void setMaxAngle(float const angle);
58-
59-
void setMaxAngle(uint8_t const id, float const angle);
53+
void setMinAngle(uint16_t const min_angle);
54+
void setMinAngle(uint8_t const id, uint16_t const min_angle);
55+
void setMaxAngle(uint16_t const max_angle);
56+
void setMaxAngle(uint8_t const id, uint16_t const max_angle);
6057

6158
void setTime(uint8_t const id, uint16_t const time);
6259

@@ -68,15 +65,14 @@ class SmartServoClass
6865

6966
bool isEngaged(uint8_t const id);
7067

71-
void printTimestamps();
72-
7368
void getInfo(Stream & stream, uint8_t const id);
7469

7570
inline void onErrorCb(mbed::Callback<void()> onError) { _onError = onError; }
7671

7772
inline int getErrors() const { return _errors; }
7873

7974
static const int BROADCAST = 0xFE;
75+
static float constexpr MAX_ANGLE = 315.0f;
8076

8177
private:
8278

@@ -88,6 +84,7 @@ class SmartServoClass
8884
static int constexpr MIN_MOTOR_ID = 1;
8985
static int constexpr MAX_MOTOR_ID = 6;
9086

87+
inline bool isValidAngle(float const angle) { return ((angle >= 0.0f) && (angle <= MAX_ANGLE)); }
9188
inline bool isValidId(int const id) const { return ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)); }
9289
inline int idToArrayIndex(int const id) const { return (id - 1); }
9390

@@ -101,10 +98,9 @@ class SmartServoClass
10198
int readWordCmd (uint8_t const id, uint8_t const address);
10299
int readByteCmd (uint8_t const id, uint8_t const address);
103100
void action (uint8_t const id);
104-
void writeSyncCmd (uint8_t *id, uint8_t const num, uint8_t const address, uint8_t const len, uint8_t const * data);
105101

106-
inline uint16_t angleToPosition(float const angle) { return (angle*MAX_POSITION)/360.0; }
107-
inline float positionToAngle(uint16_t const position) { return (360.0*position)/MAX_POSITION; }
102+
inline uint16_t angleToPosition(float const angle) { return (angle*MAX_POSITION)/MAX_ANGLE; }
103+
inline float positionToAngle(uint16_t const position) { return (MAX_ANGLE*position)/MAX_POSITION; }
108104

109105

110106
RS485Class& _RS485;
@@ -129,4 +125,4 @@ class SmartServoClass
129125
rtos::Mutex mutex;
130126
};
131127

132-
#endif // _SMARTMOTOR_H_
128+
#endif // _SMARTMOTOR_H_

0 commit comments

Comments
 (0)