61 lines
1.5 KiB
C++
61 lines
1.5 KiB
C++
#include "motorcontrol.h"
|
|
|
|
// Global servo manager instance
|
|
ServoManager servoManager;
|
|
|
|
uint16_t flipBytes(uint16_t value) {
|
|
return (value >> 8) | (value << 8);
|
|
}
|
|
|
|
void prepareMotorPositions(
|
|
const uint8_t* ids,
|
|
uint16_t* positions,
|
|
uint16_t* speeds,
|
|
uint8_t count,
|
|
RobotConfig& config
|
|
) {
|
|
for (uint8_t i = 0; i < count; i++) {
|
|
if (config.getMotorModel(ids[i]) == MODEL_STS) {
|
|
// STS servos: flip bytes, different speed range
|
|
positions[i] = flipBytes(positions[i]);
|
|
speeds[i] = map(speeds[i], 0, 4095, 0, 254);
|
|
speeds[i] = flipBytes(speeds[i]);
|
|
} else {
|
|
// SCS servos: map to 10-bit range
|
|
positions[i] = map(positions[i], 0, 4095, 0, 1023);
|
|
speeds[i] = map(speeds[i], 0, 4095, 0, 1000);
|
|
}
|
|
}
|
|
}
|
|
|
|
void ServoManager::init() {
|
|
servos[0] = new Feetech(Serial1, Pins::DE, Pins::RE, Pins::CH0_TX, Pins::CH0_RX);
|
|
servos[0]->setFeetechMode(Feetech::MODE_SCS);
|
|
servos[0]->begin();
|
|
|
|
servos[1] = new Feetech(Serial2, Pins::DE, Pins::RE, Pins::CH1_TX, Pins::CH1_RX);
|
|
servos[1]->setFeetechMode(Feetech::MODE_SCS);
|
|
// servos[1]->begin(); // Uncomment when using channel 1
|
|
}
|
|
|
|
void ServoManager::syncWritePositions(
|
|
const uint8_t* ids,
|
|
uint16_t* positions,
|
|
uint16_t* speeds,
|
|
uint8_t count,
|
|
RobotConfig& config,
|
|
uint8_t channel
|
|
) {
|
|
// Prepare positions (handles SCS vs STS conversion)
|
|
prepareMotorPositions(ids, positions, speeds, count, config);
|
|
|
|
// Send to servos
|
|
servos[channel]->syncWritePos(
|
|
const_cast<uint8_t*>(ids),
|
|
positions,
|
|
speeds,
|
|
count
|
|
);
|
|
}
|
|
|