Skip to content

Bugfix: Invalid comparison and assignment due to motor id's ranging from 1-6. #24

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 17, 2022
Merged

Bugfix: Invalid comparison and assignment due to motor id's ranging from 1-6. #24

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
e2f96f2
Fix: Motor IDs go from 1 to 6, but values are stored in arrays with a…
aentinger Jan 14, 2022
6e00d45
Define ID to array index function for better convenience.
aentinger Jan 14, 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
22 changes: 11 additions & 11 deletions src/lib/motors/SmartServo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -182,9 +182,10 @@ int SmartServoClass::begin() {

void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) {
mutex.lock();
if (id<MAX_MOTORS) {
_targetPosition[id] = angleToPosition(angle);
_targetSpeed[id] = speed;
if (isValidId(id))
{
_targetPosition[id-1] = angleToPosition(angle);
_targetSpeed[id-1] = speed;
if (_positionMode==PositionMode::IMMEDIATE) {
writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle));
}
Expand All @@ -195,9 +196,8 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t
float SmartServoClass::getPosition(uint8_t const id) {
mutex.lock();
float ret = -1;
if (id<MAX_MOTORS) {
if (isValidId(id))
ret = positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
}
mutex.unlock();
return ret;
}
Expand All @@ -211,18 +211,18 @@ void SmartServoClass::center(uint8_t const id, uint16_t const position) {
void SmartServoClass::synchronize() {
mutex.lock();
_txPacket.id = 0xFE;
_txPacket.length = (4+1)*MAX_MOTORS +4;
_txPacket.length = MAX_TX_PAYLOAD_LEN;
_txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE);
_txPacket.payload[0] = REG(SmartServoRegister::TARGET_POSITION_H);
_txPacket.payload[1] = 4;
int index = 2;

for (int i=1;i < MAX_MOTORS;i++) {
for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++) {
_txPacket.payload[index++] = i;
_txPacket.payload[index++] = _targetPosition[i] >>8;
_txPacket.payload[index++] = _targetPosition[i];
_txPacket.payload[index++] = _targetSpeed[i]>>8;
_txPacket.payload[index++] = _targetSpeed[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();
mutex.unlock();
Expand Down
14 changes: 10 additions & 4 deletions src/lib/motors/SmartServo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,11 +80,17 @@ class SmartServoClass

private:

static int constexpr MAX_MOTORS = 6;
static int constexpr MAX_TX_PAYLOAD_LEN = (5*MAX_MOTORS+4);
static int constexpr NUM_MOTORS = 6;
static int constexpr MAX_TX_PAYLOAD_LEN = (5*NUM_MOTORS+4);
static int constexpr MAX_RX_PAYLOAD_LEN = 10;
static int constexpr MAX_POSITION = 4000;

static int constexpr MIN_MOTOR_ID = 1;
static int constexpr MAX_MOTOR_ID = 6;

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); }

int calcChecksum ();
void sendPacket ();
void writeCmd (uint8_t const id, SmartServoOperation const instruction);
Expand Down Expand Up @@ -116,8 +122,8 @@ class SmartServoClass

uint8_t _rxBuf[MAX_RX_PAYLOAD_LEN];
uint8_t _rxLen;
uint16_t _targetPosition[MAX_MOTORS];
uint16_t _targetSpeed[MAX_MOTORS];
uint16_t _targetPosition[NUM_MOTORS];
uint16_t _targetSpeed[NUM_MOTORS];
PositionMode _positionMode;

rtos::Mutex mutex;
Expand Down