Skip to content

Fix: The maximum achievable angle in servo mode is 315°, not 360°. #27

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 6 commits into from
Jan 19, 2022
Merged

Fix: The maximum achievable angle in servo mode is 315°, not 360°. #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
92a2ad0
The maximum achievable angle is 315.0 degrees, not 360.0 degrees.
aentinger Jan 19, 2022
dea7437
Implement function to verify if a given angle is within the allowed r…
aentinger Jan 19, 2022
fd8b8ea
Fix setMinAngle/setMaxAngle by limiting positions rather than angles.
aentinger Jan 19, 2022
500419c
Revamping example to steer every servo through the full range of motion.
aentinger Jan 19, 2022
1e0f5cd
Use constant for max achievable angle instead of magic number.
aentinger Jan 19, 2022
da8e36d
Delete not-implemented function prototypes.
aentinger Jan 19, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
67 changes: 48 additions & 19 deletions examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,18 +3,59 @@
* angle for each smart servo motor.
*/

/**************************************************************************************
* INCLUDE
**************************************************************************************/

#include <Braccio++.h>

/**************************************************************************************
* FUNCTIONS
**************************************************************************************/

void test_motor(int const id)
{
Serial.print("Connecting ..... ");
for (; !Braccio.get(id).connected(); delay(10)) { }
Serial.println("OK.");
delay(500);

Serial.print("Disengaging .... ");
Braccio.get(id).disengage();
Serial.println("OK.");
delay(500);

Serial.print("Engaging ....... ");
Braccio.get(id).engage();
Serial.println("OK.");
delay(500);

Serial.print("Drive to start . ");
Braccio.get(id).move().to(0.0f).in(1s);
Serial.println("OK.");
delay(1500);

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

char msg[64] = {0};
snprintf(msg, sizeof(msg), "Angle (Target | Current) = %.2f | %.2f", target_angle, Braccio.get(id).position());
Serial.println(msg);
}
}

/**************************************************************************************
* SETUP/LOOP
**************************************************************************************/

void setup()
{
Serial.begin(115200);
while(!Serial){}

/* Call Braccio.begin() for default menu
* or pass a function for custom menu.
*/
Braccio.begin();

Serial.println("Testing motor angular movement!");
}

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

while((Serial.available() <= 0)) { }
int const selected_motor = Serial.parseInt();
int const motor_id = Serial.parseInt();
while(Serial.read() != '\n') { }

if (selected_motor < 1 || selected_motor > 6) {
if (motor_id < 1 || motor_id > 6) {
Serial.println("Error, wrong motor id, choose motor id between 1 and 6");
return;
}

float const ANGLE_START = 0.0;
float const ANGLE_STOP = 180.0;
float const ANGLE_INCREMENT = 10.0;

for (float angle = ANGLE_START; angle <= ANGLE_STOP; angle += ANGLE_INCREMENT)
{
Braccio.move(selected_motor).to(angle);
Serial.print("Target angle: " + String(angle));
Serial.print(" | ");
Serial.print("Current angle: " + String(Braccio.get(selected_motor).position()));
Serial.println();
delay(100);
}
test_motor(motor_id);
}
2 changes: 1 addition & 1 deletion src/Braccio++.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -255,4 +255,4 @@ inline void attachInterrupt(pin_size_t interruptNum, mbed::Callback<void()> func
attachInterruptParam(interruptNum, callback, mode, (void*)a);
}

#endif //__BRACCIO_PLUSPLUS_H__
#endif //__BRACCIO_PLUSPLUS_H__
28 changes: 18 additions & 10 deletions src/lib/motors/SmartServo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,11 @@ int SmartServoClass::begin() {
}
}

void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) {
void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed)
{
if (!isValidAngle(angle))
return;

mutex.lock();
if (isValidId(id))
{
Expand Down Expand Up @@ -295,27 +299,31 @@ void SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const ti
mutex.unlock();
}

void SmartServoClass::setMinAngle(float const angle) {
void SmartServoClass::setMinAngle(uint16_t const min_angle)
{
mutex.lock();
writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle);
mutex.unlock();
}

void SmartServoClass::setMinAngle(uint8_t const id, float const angle) {
void SmartServoClass::setMinAngle(uint8_t const id, uint16_t const min_angle)
{
mutex.lock();
writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle);
mutex.unlock();
}

void SmartServoClass::setMaxAngle(float const angle) {
void SmartServoClass::setMaxAngle(uint16_t const max_angle)
{
mutex.lock();
writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle);
mutex.unlock();
}

void SmartServoClass::setMaxAngle(uint8_t const id, float const angle) {
void SmartServoClass::setMaxAngle(uint8_t const id, uint16_t const max_angle)
{
mutex.lock();
writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle);
mutex.unlock();
}

Expand All @@ -336,4 +344,4 @@ void SmartServoClass::getInfo(Stream & stream, uint8_t const id) {
for (i = 0; i < sizeof(regs); i++) {
stream.println(String(i, HEX) + " : " + String(regs[i], HEX));
}
}
}
22 changes: 9 additions & 13 deletions src/lib/motors/SmartServo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,13 +50,10 @@ class SmartServoClass

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

void setMinAngle(float const angle);

void setMinAngle(uint8_t const id, float const angle);

void setMaxAngle(float const angle);

void setMaxAngle(uint8_t const id, float const angle);
void setMinAngle(uint16_t const min_angle);
void setMinAngle(uint8_t const id, uint16_t const min_angle);
void setMaxAngle(uint16_t const max_angle);
void setMaxAngle(uint8_t const id, uint16_t const max_angle);

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

Expand All @@ -68,15 +65,14 @@ class SmartServoClass

bool isEngaged(uint8_t const id);

void printTimestamps();

void getInfo(Stream & stream, uint8_t const id);

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

inline int getErrors() const { return _errors; }

static const int BROADCAST = 0xFE;
static float constexpr MAX_ANGLE = 315.0f;

private:

Expand All @@ -88,6 +84,7 @@ class SmartServoClass
static int constexpr MIN_MOTOR_ID = 1;
static int constexpr MAX_MOTOR_ID = 6;

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

Expand All @@ -101,10 +98,9 @@ class SmartServoClass
int readWordCmd (uint8_t const id, uint8_t const address);
int readByteCmd (uint8_t const id, uint8_t const address);
void action (uint8_t const id);
void writeSyncCmd (uint8_t *id, uint8_t const num, uint8_t const address, uint8_t const len, uint8_t const * data);

inline uint16_t angleToPosition(float const angle) { return (angle*MAX_POSITION)/360.0; }
inline float positionToAngle(uint16_t const position) { return (360.0*position)/MAX_POSITION; }
inline uint16_t angleToPosition(float const angle) { return (angle*MAX_POSITION)/MAX_ANGLE; }
inline float positionToAngle(uint16_t const position) { return (MAX_ANGLE*position)/MAX_POSITION; }


RS485Class& _RS485;
Expand All @@ -129,4 +125,4 @@ class SmartServoClass
rtos::Mutex mutex;
};

#endif // _SMARTMOTOR_H_
#endif // _SMARTMOTOR_H_