102 lines
3.7 KiB
C++
102 lines
3.7 KiB
C++
#ifndef FEETECH_H
|
|
#define FEETECH_H
|
|
|
|
#include <Arduino.h>
|
|
#include <HardwareSerial.h>
|
|
|
|
class Feetech {
|
|
public:
|
|
Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN);
|
|
void begin();
|
|
void sendPing(uint8_t id);
|
|
uint16_t getModel(uint8_t id);
|
|
uint8_t getID(uint8_t id);
|
|
uint8_t getBaudRate(uint8_t id);
|
|
uint16_t getMinAngleLimit(uint8_t id);
|
|
uint16_t getMaxAngleLimit(uint8_t id);
|
|
uint8_t getCWDeadZone(uint8_t id);
|
|
uint8_t getCCWDeadZone(uint8_t id);
|
|
uint16_t getOffset(uint8_t id);
|
|
uint8_t getMode(uint8_t id);
|
|
uint8_t getTorqueEnable(uint8_t id);
|
|
uint8_t getAcceleration(uint8_t id);
|
|
uint16_t getGoalPosition(uint8_t id);
|
|
uint16_t getGoalTime(uint8_t id);
|
|
uint16_t getGoalSpeed(uint8_t id);
|
|
uint8_t getLock(uint8_t id);
|
|
uint16_t getPosition(uint8_t id);
|
|
float getVoltage(uint8_t id);
|
|
void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount);
|
|
void sendWritePos(uint8_t id, uint16_t position);
|
|
void pingAll();
|
|
void waitOnReply(unsigned long timeout);
|
|
uint8_t waitOnData1Byte(unsigned long timeout);
|
|
uint16_t waitOnData2Bytes(unsigned long timeout);
|
|
void sendData(const byte* data, size_t length);
|
|
size_t receiveData(byte* buffer, size_t bufferSize);
|
|
void setModeReceive();
|
|
void setModeTransmit();
|
|
void update();
|
|
void testRequest();
|
|
void enableTorque(uint8_t id);
|
|
|
|
static const byte PING = 0x01; // QUERY THE WORKING STATUS
|
|
static const byte READ_DATA = 0x02; // READ DATA
|
|
static const byte WRITE_DATA = 0x03; // WRITE DATA
|
|
static const byte REGWRITE_DATA = 0x04; // QUEUES MOVES FOR ACTION COMMAND
|
|
static const byte ACTION = 0x05; // TRIGGERS REG WRITE WRITES
|
|
static const byte SYNCWRITE_DATA = 0x83; // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
|
|
static const byte RESET = 0x06; // RESET TO FACTORY DEFAULT
|
|
|
|
|
|
// MEMORY TABLE LOCATIONS SMS-STS
|
|
static const byte REQUEST_MODEL = 0x03; // 2 bytes
|
|
static const byte REQUEST_ID = 0x05; // 1 byte
|
|
static const byte REQUEST_BAUD_RATE = 0x06; // 1 byte
|
|
static const byte REQUEST_MIN_ANGLE_LIMIT = 0x09; // 2 bytes
|
|
static const byte REQUEST_MAX_ANGLE_LIMIT = 0x0B; // 2 bytes
|
|
static const byte REQUEST_CW_DEAD_ZONE = 0x1A; // 1 byte
|
|
static const byte REQUEST_CCW_DEAD_ZONE = 0x1B; // 1 byte
|
|
static const byte REQUEST_OFFSET = 0x1F; // 2 bytes
|
|
static const byte REQUEST_MODE = 0x21; // 1 byte
|
|
|
|
static const byte REQUEST_TORQUE_ENABLE = 0x28; // 1 byte
|
|
static const byte REQUEST_ACCELERATION = 0x29; // 1 byte
|
|
static const byte REQUEST_GOAL_POSITION = 0x2A; // 2 byte
|
|
static const byte REQUEST_GOAL_TIME = 0x2C; // 2 byte
|
|
static const byte REQUEST_GOAL_SPEED = 0x2E; // 2 byte
|
|
static const byte REQUEST_LOCK = 0x37; // 1 byte
|
|
|
|
static const byte REQUEST_POSITION = 0x38; // 2 bytes
|
|
static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte
|
|
|
|
// TODO
|
|
// SMS_STS_PRESENT_SPEED_L = 0x3A
|
|
// SMS_STS_PRESENT_SPEED_H = 0x3B
|
|
// SMS_STS_PRESENT_LOAD_L = 0x3C
|
|
// SMS_STS_PRESENT_LOAD_H = 0x3D
|
|
// SMS_STS_PRESENT_VOLTAGE = 0x3E
|
|
// SMS_STS_PRESENT_TEMPERATURE = 0x3F
|
|
// SMS_STS_MOVING = 0x42
|
|
// SMS_STS_PRESENT_CURRENT_L = 0x45
|
|
// SMS_STS_PRESENT_CURRENT_H = 0x46
|
|
|
|
// BAUD RATES (stored as 1 byte)
|
|
static const byte SMS_STS_1M = 0;
|
|
static const byte SMS_STS_0_5M = 1;
|
|
static const byte SMS_STS_250K = 2;
|
|
static const byte SMS_STS_128K = 3;
|
|
static const byte SMS_STS_115200 = 4;
|
|
static const byte SMS_STS_76800 = 5;
|
|
static const byte SMS_STS_57600 = 6;
|
|
static const byte SMS_STS_38400 = 7;
|
|
|
|
private:
|
|
HardwareSerial& serial; // Reference to the HardwareSerial object
|
|
uint8_t DE_PIN = 20;
|
|
uint8_t RE_PIN = 10;
|
|
uint8_t TX_PIN = 21;
|
|
uint8_t RX_PIN = 9;
|
|
};
|
|
|
|
#endif // FEETECH_H
|