61 lines
1.2 KiB
C++
61 lines
1.2 KiB
C++
#pragma once
|
|
#include <Arduino.h>
|
|
#include "feetech.h"
|
|
#include "RobotConfig.h"
|
|
|
|
// Pin definitions
|
|
namespace Pins {
|
|
// Channel 0 (SCS servos)
|
|
constexpr int CH0_RX = 13;
|
|
constexpr int CH0_TX = 12;
|
|
|
|
// Channel 1 (STS servos)
|
|
constexpr int CH1_RX = 11;
|
|
constexpr int CH1_TX = 10;
|
|
|
|
// RS485 control
|
|
constexpr int DE = 7; // Driver Enable
|
|
constexpr int RE = 8; // Receiver Enable
|
|
}
|
|
|
|
// Servo model constants
|
|
constexpr uint8_t MODEL_STS = 9;
|
|
|
|
// Utility functions
|
|
uint16_t flipBytes(uint16_t value);
|
|
|
|
// Prepare motor positions for transmission (handles SCS vs STS byte ordering)
|
|
void prepareMotorPositions(
|
|
const uint8_t* ids,
|
|
uint16_t* positions,
|
|
uint16_t* speeds,
|
|
uint8_t count,
|
|
RobotConfig& config
|
|
);
|
|
|
|
// Servo manager class for cleaner access
|
|
class ServoManager {
|
|
public:
|
|
void init();
|
|
|
|
Feetech* channel(uint8_t ch) { return servos[ch]; }
|
|
Feetech* operator[](uint8_t ch) { return servos[ch]; }
|
|
|
|
// Convenience methods
|
|
void syncWritePositions(
|
|
const uint8_t* ids,
|
|
uint16_t* positions,
|
|
uint16_t* speeds,
|
|
uint8_t count,
|
|
RobotConfig& config,
|
|
uint8_t channel = 0
|
|
);
|
|
|
|
private:
|
|
Feetech* servos[2] = {nullptr, nullptr};
|
|
};
|
|
|
|
// Global servo manager
|
|
extern ServoManager servoManager;
|
|
|