diff --git a/HansonServo.ino b/HansonServo.ino index 68415da..b51b111 100644 --- a/HansonServo.ino +++ b/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("/"); diff --git a/feetech.cpp b/feetech.cpp index c36603f..f0ee251 100644 --- a/feetech.cpp +++ b/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& successfulAddresses) { diff --git a/feetech.h b/feetech.h index 1556b46..ad7f67a 100644 --- a/feetech.h +++ b/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