full spectrum of data read/write (needs testing), feedback checkbox activates position streaming for the 5 test motors
parent
fb38ca9bdb
commit
13ae278505
152
HansonServo.ino
152
HansonServo.ino
|
|
@ -20,6 +20,9 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
|
|||
#define CMD_SET_POSITION 0x07
|
||||
#define CMD_PLAY_FILE 0x08
|
||||
#define CMD_SCAN_CHANNEL 0x09
|
||||
#define CMD_WRITE_DATA 0x10
|
||||
#define CMD_START_POSITION_STREAM 0x14
|
||||
#define POSITION_STREAM 0x15
|
||||
|
||||
|
||||
// ESP32 S2 PINOUT
|
||||
|
|
@ -37,6 +40,9 @@ Animation anim;
|
|||
Animation sweep;
|
||||
Animation stagger;
|
||||
|
||||
bool streamPositions = false;
|
||||
unsigned long lastStreamPositions = 0;
|
||||
|
||||
Feetech* servos[2];
|
||||
|
||||
uint16_t flipBytes(uint16_t value) {
|
||||
|
|
@ -137,7 +143,7 @@ void setup() {
|
|||
|
||||
void SetID(uint8_t oldID, uint8_t newID) {
|
||||
Serial.println("Setting Lock to 0");
|
||||
Serial.println(servos[0]->setLockSTS(oldID, 0));
|
||||
Serial.println(servos[0]->setLock(oldID, 0));
|
||||
delay(1000);
|
||||
Serial.print("Changing ID ");
|
||||
Serial.print(oldID);
|
||||
|
|
@ -146,7 +152,7 @@ void SetID(uint8_t oldID, uint8_t newID) {
|
|||
Serial.println(servos[0]->setID(oldID, newID));
|
||||
delay(1000);
|
||||
Serial.println("Setting Lock to 1");
|
||||
Serial.println(servos[0]->setLockSTS(newID, 1));
|
||||
Serial.println(servos[0]->setLock(newID, 1));
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
|
|
@ -254,6 +260,14 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
|
|||
handleScanChannel(payload, length);
|
||||
break;
|
||||
|
||||
case CMD_WRITE_DATA:
|
||||
handleDataWrite(payload, length);
|
||||
break;
|
||||
|
||||
case CMD_START_POSITION_STREAM:
|
||||
startPositionStream(payload, length);
|
||||
break;
|
||||
|
||||
default:
|
||||
Serial.print("Unknown command: ");
|
||||
Serial.println(command, HEX);
|
||||
|
|
@ -262,6 +276,40 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
|
|||
}
|
||||
|
||||
|
||||
void startPositionStream(const uint8_t* payload, uint16_t length) {
|
||||
bool start = payload[0] != 0;
|
||||
servos[0]->disableTorque(10);
|
||||
servos[0]->waitOnData1Byte(10);
|
||||
servos[0]->disableTorque(11);
|
||||
servos[0]->waitOnData1Byte(10);
|
||||
servos[0]->disableTorque(12);
|
||||
servos[0]->waitOnData1Byte(10);
|
||||
servos[0]->disableTorque(13);
|
||||
servos[0]->waitOnData1Byte(10);
|
||||
servos[0]->disableTorque(14);
|
||||
servos[0]->waitOnData1Byte(10);
|
||||
streamPositions = start;
|
||||
}
|
||||
|
||||
|
||||
void handleDataWrite(const uint8_t* payload, uint16_t length) {
|
||||
uint8_t channel = payload[0];
|
||||
uint8_t id = payload[1];
|
||||
uint8_t writeByte = payload[2];
|
||||
|
||||
if (payload[4] == 1) {
|
||||
servos[channel]->write1Byte(id, writeByte, payload[4]);
|
||||
uint8_t response = servos[channel]->waitOnData1Byte(10);
|
||||
sendMessage(&response, 1, CMD_WRITE_DATA);
|
||||
} else {
|
||||
servos[channel]->write2Bytes(id, writeByte, payload[4] | (payload[5] << 8));
|
||||
uint16_t response = servos[channel]->waitOnData2Bytes(10);
|
||||
uint8_t buffer[2];
|
||||
buffer[0] = (response >> 8) & 0xFF; // high byte
|
||||
buffer[1] = response & 0xFF; // low byte
|
||||
sendMessage(buffer, 2, CMD_WRITE_DATA);
|
||||
}
|
||||
}
|
||||
|
||||
void handleIdRequest() {
|
||||
String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION;
|
||||
|
|
@ -589,15 +637,34 @@ void handleScanChannel(const uint8_t* payload, uint16_t length) {
|
|||
uint8_t val = servos[payload[0]]->waitOnData1Byte(10);
|
||||
|
||||
if (val != 0) {
|
||||
uint8_t response[4];
|
||||
uint8_t response[44]; // Adjusted size to fit all values
|
||||
response[0] = payload[0]; // channel
|
||||
response[1] = i; // responding address
|
||||
uint16_t mode = servos[payload[0]]->getModel(i);
|
||||
|
||||
uint16_t model = servos[payload[0]]->getModel(i);
|
||||
uint16_t minAngleLimit = servos[payload[0]]->getMinAngleLimit(i);
|
||||
uint16_t maxAngleLimit = servos[payload[0]]->getMaxAngleLimit(i);
|
||||
uint16_t position = servos[payload[0]]->getPosition(i);
|
||||
response[2] = (mode >> 8) & 0xFF; // high byte
|
||||
response[3] = mode & 0xFF; // low byte
|
||||
uint8_t CWDeadZone = servos[payload[0]]->getCWDeadZone(i);
|
||||
uint8_t CCWDeadZone = servos[payload[0]]->getCCWDeadZone(i);
|
||||
uint16_t offset = servos[payload[0]]->getOffset(i);
|
||||
uint8_t mode = servos[payload[0]]->getMode(i);
|
||||
uint8_t torqueEnable = servos[payload[0]]->getTorqueEnable(i);
|
||||
uint8_t acceleration = servos[payload[0]]->getAcceleration(i);
|
||||
uint16_t goalPosition = servos[payload[0]]->getGoalPosition(i);
|
||||
uint16_t goalTime = servos[payload[0]]->getGoalTime(i);
|
||||
uint16_t goalSpeed = servos[payload[0]]->getGoalSpeed(i);
|
||||
uint8_t lock = servos[payload[0]]->getLock(i);
|
||||
int16_t speed = servos[payload[0]]->getSpeed(i);
|
||||
uint16_t load = servos[payload[0]]->getLoad(i);
|
||||
uint8_t temperature = servos[payload[0]]->getTemperature(i);
|
||||
uint8_t moving = servos[payload[0]]->getMoving(i);
|
||||
uint8_t current = servos[payload[0]]->getCurrent(i);
|
||||
uint8_t voltage = servos[payload[0]]->getVoltage(i);
|
||||
|
||||
// Pack values into response
|
||||
response[2] = (model >> 8) & 0xFF;
|
||||
response[3] = model & 0xFF;
|
||||
|
||||
response[4] = (minAngleLimit >> 8) & 0xFF;
|
||||
response[5] = minAngleLimit & 0xFF;
|
||||
|
|
@ -608,8 +675,44 @@ void handleScanChannel(const uint8_t* payload, uint16_t length) {
|
|||
response[8] = (position >> 8) & 0xFF;
|
||||
response[9] = position & 0xFF;
|
||||
|
||||
response[10] = CWDeadZone;
|
||||
response[11] = CCWDeadZone;
|
||||
|
||||
sendMessage(response, 10, CMD_SCAN_CHANNEL); // send all 4 bytes
|
||||
response[12] = (offset >> 8) & 0xFF;
|
||||
response[13] = offset & 0xFF;
|
||||
|
||||
response[14] = mode;
|
||||
response[15] = torqueEnable;
|
||||
response[16] = acceleration;
|
||||
|
||||
response[17] = (goalPosition >> 8) & 0xFF;
|
||||
response[18] = goalPosition & 0xFF;
|
||||
|
||||
response[19] = (goalTime >> 8) & 0xFF;
|
||||
response[20] = goalTime & 0xFF;
|
||||
|
||||
response[21] = (goalSpeed >> 8) & 0xFF;
|
||||
response[22] = goalSpeed & 0xFF;
|
||||
|
||||
response[23] = lock;
|
||||
|
||||
response[24] = (speed >> 8) & 0xFF;
|
||||
response[25] = speed & 0xFF;
|
||||
|
||||
response[26] = (load >> 8) & 0xFF;
|
||||
response[27] = load & 0xFF;
|
||||
|
||||
response[28] = temperature;
|
||||
response[29] = moving;
|
||||
response[30] = (current >> 8) & 0xFF;
|
||||
response[31] = current & 0xFF;
|
||||
|
||||
response[32] = voltage;
|
||||
|
||||
|
||||
// You can continue adding more fields here if needed...
|
||||
|
||||
sendMessage(response, 33, CMD_SCAN_CHANNEL); // updated length
|
||||
}
|
||||
}
|
||||
uint8_t r[2];
|
||||
|
|
@ -638,6 +741,12 @@ bool flip = false;
|
|||
unsigned long lastSend = 0;
|
||||
void loop() {
|
||||
|
||||
// for (int i = 0; i < 1023; i++) {
|
||||
// servos[0]->sendWritePos(10, i);
|
||||
// Serial.println(servos[0]->waitOnData1Byte(10));
|
||||
// delay(20);
|
||||
// }
|
||||
|
||||
|
||||
HandleSerialRequests();
|
||||
// for (int i = 0; i < 500; i+=32) {
|
||||
|
|
@ -694,9 +803,38 @@ void loop() {
|
|||
//PrintFileList();
|
||||
lastSend = millis();
|
||||
}
|
||||
|
||||
if (streamPositions) {
|
||||
if (millis() - lastStreamPositions > 50) {
|
||||
lastStreamPositions = millis();
|
||||
SendMotorPositions();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SendMotorPositions() {
|
||||
|
||||
uint16_t p0 = servos[0]->getPosition(10);
|
||||
uint16_t p1 = servos[0]->getPosition(11);
|
||||
uint16_t p2 = servos[0]->getPosition(12);
|
||||
uint16_t p3 = servos[0]->getPosition(13);
|
||||
uint16_t p4 = servos[0]->getPosition(14);
|
||||
|
||||
uint8_t payload[10];
|
||||
|
||||
payload[0] = p0 >> 8;
|
||||
payload[1] = p0 & 0xFF;
|
||||
payload[2] = p1 >> 8;
|
||||
payload[3] = p1 & 0xFF;
|
||||
payload[4] = p2 >> 8;
|
||||
payload[5] = p2 & 0xFF;
|
||||
payload[6] = p3 >> 8;
|
||||
payload[7] = p3 & 0xFF;
|
||||
payload[8] = p4 >> 8;
|
||||
payload[9] = p4 & 0xFF;
|
||||
|
||||
sendMessage(payload, 10, POSITION_STREAM);
|
||||
}
|
||||
|
||||
void PrintFileList() {
|
||||
File root = FFat.open("/");
|
||||
|
|
|
|||
59
feetech.cpp
59
feetech.cpp
|
|
@ -308,24 +308,27 @@ uint16_t Feetech::getGoalSpeed(uint8_t id) {
|
|||
}
|
||||
|
||||
uint8_t Feetech::getLock(uint8_t id) {
|
||||
sendRequest(id, REQUEST_LOCK, 1);
|
||||
if (feetechMode == MODE_SCS) {
|
||||
sendRequest(id, REQUEST_LOCK_SCS, 1);
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
sendRequest(id, REQUEST_LOCK_SMS_STS, 1);
|
||||
}
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
uint8_t Feetech::setLock(uint8_t id, uint8_t lockEnabled) {
|
||||
write1Byte(id, REQUEST_LOCK, lockEnabled);
|
||||
if (feetechMode == MODE_SCS) {
|
||||
write1Byte(id, REQUEST_LOCK_SCS, lockEnabled);
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
write1Byte(id, REQUEST_LOCK_SMS_STS, lockEnabled);
|
||||
}
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
uint8_t Feetech::setLockSTS(uint8_t id, uint8_t lockEnabled) {
|
||||
write1Byte(id, 0x37, lockEnabled);
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
float Feetech::getVoltage(uint8_t id) {
|
||||
uint8_t Feetech::getVoltage(uint8_t id) {
|
||||
sendRequest(id, REQUEST_VOLTAGE, 1);
|
||||
float voltage = waitOnData1Byte(10) * 0.1;
|
||||
return voltage;
|
||||
//float voltage = waitOnData1Byte(10) * 0.1;
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
uint16_t Feetech::getPosition(uint8_t id) {
|
||||
|
|
@ -364,9 +367,10 @@ uint8_t Feetech::getMoving(uint8_t id) {
|
|||
}
|
||||
|
||||
// Multiplier could be wrong
|
||||
float Feetech::getCurrent(uint8_t id) {
|
||||
uint16_t Feetech::getCurrent(uint8_t id) {
|
||||
sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
|
||||
return waitOnData2Bytes(10) * 0.01;
|
||||
//return waitOnData2Bytes(10) * 0.01; FLOAT
|
||||
return waitOnData2Bytes(10);
|
||||
}
|
||||
|
||||
void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
|
||||
|
|
@ -418,36 +422,33 @@ void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) {
|
|||
void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
|
||||
uint8_t packet[9];
|
||||
|
||||
packet[0] = 0xFF; // Header
|
||||
packet[1] = 0xFF; // Header
|
||||
packet[2] = id; // Servo ID
|
||||
packet[3] = 5; // Length = instruction + address + 2 bytes + checksum
|
||||
packet[4] = WRITE_DATA; // Instruction: WRITE
|
||||
packet[5] = instruction; // Address: Goal Position
|
||||
packet[0] = 0xFF; // Header
|
||||
packet[1] = 0xFF; // Header
|
||||
packet[2] = id; // Servo ID
|
||||
packet[3] = 5; // Length = instruction + address + 2 bytes + checksum
|
||||
packet[4] = WRITE_DATA; // Instruction: WRITE
|
||||
packet[5] = instruction; // Address: Goal Position
|
||||
if (feetechMode == MODE_SCS) {
|
||||
packet[6] = (data >> 8) & 0xFF; // High byte
|
||||
packet[7] = data & 0xFF; // Low byte
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
|
||||
packet[6] = data & 0xFF; // Low byte first for STS
|
||||
packet[7] = (data >> 8) & 0xFF; // High byte second
|
||||
}
|
||||
|
||||
// Calculate checksum
|
||||
uint8_t sum = 0;
|
||||
for (int i = 2; i <= 7; i++) {
|
||||
sum += packet[i];
|
||||
}
|
||||
for (int i = 2; i < 8; i++) sum += packet[i];
|
||||
packet[8] = ~sum;
|
||||
for (int i = 0; i < 9; i++) {
|
||||
Serial.print(packet[i], HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println();
|
||||
|
||||
// Send packet
|
||||
setModeTransmit();
|
||||
serial.write(packet, 9);
|
||||
serial.write(packet, sizeof(packet));
|
||||
serial.flush();
|
||||
setModeReceive();
|
||||
clearEcho(sizeof(packet));
|
||||
|
||||
// Should later recieve a 1 byte response code packet
|
||||
//return waitOnData2Bytes(10);
|
||||
}
|
||||
|
||||
void Feetech::pingAll(std::vector<uint8_t>& successfulAddresses) {
|
||||
|
|
|
|||
11
feetech.h
11
feetech.h
|
|
@ -40,7 +40,6 @@ public:
|
|||
uint16_t getGoalSpeed(uint8_t id);
|
||||
uint8_t getLock(uint8_t id);
|
||||
uint8_t setLock(uint8_t id, uint8_t lockEnabled);
|
||||
uint8_t setLockSTS(uint8_t id, uint8_t lockEnabled);
|
||||
uint16_t getPosition(uint8_t id);
|
||||
|
||||
int16_t getSpeed(uint8_t id);
|
||||
|
|
@ -49,9 +48,9 @@ public:
|
|||
|
||||
|
||||
uint8_t getMoving(uint8_t id);
|
||||
float getCurrent(uint8_t id);
|
||||
uint16_t getCurrent(uint8_t id);
|
||||
|
||||
float getVoltage(uint8_t id);
|
||||
uint8_t getVoltage(uint8_t id);
|
||||
void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount);
|
||||
uint8_t sendWritePos(uint8_t id, uint16_t position);
|
||||
void syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count);
|
||||
|
|
@ -90,6 +89,8 @@ public:
|
|||
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
|
||||
|
||||
// REQUEST OFFSET AND MODE NOT IN SCS?????
|
||||
static const byte REQUEST_OFFSET = 0x1F; // 2 bytes
|
||||
static const byte REQUEST_MODE = 0x21; // 1 byte
|
||||
|
||||
|
|
@ -98,7 +99,9 @@ public:
|
|||
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 = 0x30; // 1 byte
|
||||
|
||||
static const byte REQUEST_LOCK_SCS = 0x30; // 1 byte
|
||||
static const byte REQUEST_LOCK_SMS_STS = 0x37; // 1 byte
|
||||
|
||||
static const byte REQUEST_POSITION = 0x38; // 2 bytes
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue