Skip to content

Unify API for setting the run speed of a servo #32

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 2 commits into from
Jan 20, 2022
Merged

Unify API for setting the run speed of a servo #32

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3f757d3
Use speed() API to directly write desired speed to servos.
aentinger Jan 20, 2022
ac5ec75
Consolidating different APIs writing to the RUN_TIME register within …
aentinger Jan 20, 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
14 changes: 7 additions & 7 deletions src/Braccio++.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ BraccioClass::BraccioClass()
, _is_motor_connected{false}
, _motors_connected_mtx{}
, _motors_connected_thd{}
, runTime{SLOW}
, _customMenu{nullptr}
{

Expand Down Expand Up @@ -163,6 +162,7 @@ bool BraccioClass::begin(voidFuncPtr customMenu)
}

servos.begin();
servos.setTime(SmartServoClass::BROADCAST, SLOW);
servos.setPositionMode(PositionMode::IMMEDIATE);

_motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));
Expand Down Expand Up @@ -202,12 +202,12 @@ Servo BraccioClass::get(int const id)
void BraccioClass::moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6)
{
servos.setPositionMode(PositionMode::SYNC);
servos.setPosition(1, a1, runTime);
servos.setPosition(2, a2, runTime);
servos.setPosition(3, a3, runTime);
servos.setPosition(4, a4, runTime);
servos.setPosition(5, a5, runTime);
servos.setPosition(6, a6, runTime);
servos.setPosition(1, a1);
servos.setPosition(2, a2);
servos.setPosition(3, a3);
servos.setPosition(4, a4);
servos.setPosition(5, a5);
servos.setPosition(6, a6);
servos.synchronize();
servos.setPositionMode(PositionMode::IMMEDIATE);
}
Expand Down
10 changes: 4 additions & 6 deletions src/Braccio++.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,9 @@ class BraccioClass
void positions(float * buffer);
void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);

inline void speed (speed_grade_t const speed_grade) { runTime = speed_grade; }
inline void speed(speed_grade_t const speed_grade) { servos.setTime(SmartServoClass::BROADCAST, speed_grade); }
inline void speed(int const id, speed_grade_t const speed_grade) { servos.setTime(id, speed_grade); }

inline void disengage(int const id = SmartServoClass::BROADCAST) { servos.disengage(id); }
inline void engage (int const id = SmartServoClass::BROADCAST) { servos.engage(id); }

Expand Down Expand Up @@ -112,8 +114,6 @@ class BraccioClass
void setMotorConnectionStatus(int const id, bool const is_connected);
void motorConnectedThreadFunc();

speed_grade_t runTime; //ms

voidFuncPtr _customMenu;

const int BTN_LEFT = 3;
Expand Down Expand Up @@ -175,7 +175,7 @@ class Servo
inline bool engaged() { return _servos.isEngaged(_id); }

inline Servo & move() { return *this; }
inline Servo & to (float const angle) { _servos.setPosition(_id, angle, _speed); return *this; }
inline Servo & to (float const angle) { _servos.setPosition(_id, angle); return *this; }
inline Servo & in (std::chrono::milliseconds const len) { _servos.setTime(_id, len.count()); return *this; }

inline float position() { return _servos.getPosition(_id); }
Expand All @@ -189,8 +189,6 @@ class Servo

SmartServoClass & _servos;
int const _id;
int const _speed = 100;

};

struct __callback__container__ {
Expand Down
7 changes: 2 additions & 5 deletions src/lib/motors/SmartServo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,7 @@ void SmartServoClass::begin()
_positionMode = PositionMode::IMMEDIATE;
}

void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed)
void SmartServoClass::setPosition(uint8_t const id, float const angle)
{
if (!isValidAngle(angle))
return;
Expand All @@ -184,7 +184,6 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t
if (isValidId(id))
{
_targetPosition[idToArrayIndex(id)] = angleToPosition(angle);
_targetSpeed[idToArrayIndex(id)] = speed;
if (_positionMode==PositionMode::IMMEDIATE) {
writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle));
}
Expand Down Expand Up @@ -212,15 +211,13 @@ void SmartServoClass::synchronize()
_txPacket.length = MAX_TX_PAYLOAD_LEN;
_txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE);
_txPacket.payload[0] = REG(SmartServoRegister::TARGET_POSITION_H);
_txPacket.payload[1] = 4;
_txPacket.payload[1] = 2;
int index = 2;

for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++) {
_txPacket.payload[index++] = i;
_txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)] >>8;
_txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)];
_txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]>>8;
_txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)];
}
sendPacket();
}
Expand Down
3 changes: 1 addition & 2 deletions src/lib/motors/SmartServo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ class SmartServoClass

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

void setPosition(uint8_t const id, float const angle, uint16_t const speed);
void setPosition(uint8_t const id, float const angle);

float getPosition(uint8_t const id);

Expand Down Expand Up @@ -120,7 +120,6 @@ class SmartServoClass
uint8_t _rxBuf[MAX_RX_PAYLOAD_LEN];
uint8_t _rxLen;
uint16_t _targetPosition[NUM_MOTORS];
uint16_t _targetSpeed[NUM_MOTORS];
PositionMode _positionMode;
};

Expand Down