Skip to content

Fix: Also write run time when doing sync action (not only position). #48

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 1 commit into from
Feb 9, 2022
Merged

Fix: Also write run time when doing sync action (not only position). #48

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
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 @@ -80,7 +80,7 @@ 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) { _servos.setTime(SmartServoClass::BROADCAST, speed_grade); }
inline void speed(speed_grade_t const speed_grade) { _servos.setTime(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); }
Expand Down
18 changes: 16 additions & 2 deletions src/lib/motors/SmartServo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -193,6 +193,9 @@ void SmartServoClass::begin()
writeByteCmd(BROADCAST, REG(SmartServoRegister::SERVO_MOTOR_MODE), 1);
writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH) ,1);
_positionMode = PositionMode::IMMEDIATE;

for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++)
_targetSpeed[idToArrayIndex(i)] = 1000;
}

void SmartServoClass::setPosition(uint8_t const id, float const angle)
Expand Down Expand Up @@ -231,13 +234,15 @@ 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] = 2;
_txPacket.payload[1] = 4;
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)]>>8;
_txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)];
_txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]>>8;
_txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)];
}
sendPacket();
}
Expand All @@ -254,9 +259,18 @@ void SmartServoClass::setTorque(uint8_t const id, bool const torque)
writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0);
}

void SmartServoClass::setTime(uint16_t const time)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++)
_targetSpeed[idToArrayIndex(i)] = time;
writeWordCmd(BROADCAST, REG(SmartServoRegister::RUN_TIME_H), time);
}

void SmartServoClass::setTime(uint8_t const id, uint16_t const time)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
if ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)) _targetSpeed[idToArrayIndex(id)] = time;
writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time);
}

Expand Down
5 changes: 4 additions & 1 deletion src/lib/motors/SmartServo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ class SmartServoClass
void setMaxAngle(uint16_t const max_angle);
void setMaxAngle(uint8_t const id, uint16_t const max_angle);

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

void center(uint8_t const id, uint16_t const position);
Expand All @@ -97,7 +98,7 @@ class SmartServoClass
static int constexpr NUM_MOTORS = 6;
static float constexpr MAX_ANGLE = 315.0f;

static int idToArrayIndex(int const id) { return (id - 1); }
static size_t idToArrayIndex(size_t const id) { return (id - 1); }

private:

Expand Down Expand Up @@ -140,6 +141,8 @@ 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