realrobotshk
/
rs485transmitter
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

moved everything to feetech.c

master
Jake 2025-09-16 17:32:59 +08:00
parent df4f7de0ca
commit 07c78bc2a0
6 changed files with 524 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/arduino.json vendored Normal file
View File

@ -0,0 +1,3 @@
{
"port": "COM78"
}

35
.vscode/tasks.json vendored Normal file
View File

@ -0,0 +1,35 @@
{
"version": "2.0.0",
"tasks": [
{
"label": "Arduino: Compile",
"type": "shell",
"command": "arduino-cli compile --fqbn esp32:esp32:esp32c3 --build-property build.usb_cdc_on_boot=1 --verbose .",
"group": "build",
"problemMatcher": []
},
{
"label": "Arduino: Upload",
"type": "shell",
"command": "arduino-cli upload -p COM78 --fqbn esp32:esp32:esp32c3 .",
"group": "test",
"problemMatcher": []
},
{
"label": "Arduino: Compile & Upload",
"type": "shell",
"command": "arduino-cli compile --fqbn esp32:esp32:esp32c3 --build-property build.usb_cdc_on_boot=1 --verbose .; if ($?) { arduino-cli upload -p COM78 --fqbn esp32:esp32:esp32c3 --verbose . }",
"group": {
"kind": "build",
"isDefault": true
},
"problemMatcher": []
},
{
"label": "Arduino: Set Board",
"type": "shell",
"command": "arduino-cli board attach --port COM78 --fqbn esp32:esp32:esp32c3",
"problemMatcher": []
}
]
}

257
feetech.cpp Normal file
View File

@ -0,0 +1,257 @@
#include "feetech.h"
Feetech::Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN)
: serial(serial), DE_PIN(DE_PIN), RE_PIN(RE_PIN), TX_PIN(TX_PIN), RX_PIN(RX_PIN) {
}
void Feetech::begin() {
serial.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
pinMode(DE_PIN, OUTPUT);
pinMode(RE_PIN, OUTPUT);
setModeReceive();
}
// Send move command to servo id:0-255, position:0-4095
void Feetech::sendWritePos(uint8_t id, uint16_t position) {
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] = 0x2A; // Address: Goal Position
packet[7] = (position >> 8) & 0xFF; // High byte
packet[6] = position & 0xFF; // Low byte
// Calculate checksum
uint8_t sum = 0;
for (int i = 2; i < 8; i++) sum += packet[i];
packet[8] = ~sum;
// Send packet
setModeTransmit();
serial.write(packet, sizeof(packet));
serial.flush();
setModeReceive();
}
void Feetech::sendPing(uint8_t id) {
uint8_t packet[6];
packet[0] = 0xFF; // Header
packet[1] = 0xFF; // Header
packet[2] = id; // Servo ID
packet[3] = 0x02; // Length = instruction + address + checksum
packet[4] = PING; // Instruction: PING
// Calculate checksum
uint8_t sum = 0;
for (int i = 2; i < 5; i++) sum += packet[i];
packet[5] = ~sum; // Checksum
// Send packet
setModeTransmit();
serial.write(packet, sizeof(packet));
serial.flush();
setModeReceive();
}
void Feetech::testRequest() {
uint8_t packet[6] = { 0XFF, 0XFF, 0X00, 0X02, 0X06, 0XF7 };
uint8_t sum = 0;
for (int i = 2; i < 5; i++) sum += packet[i]; // Include all data bytes for checksum
packet[5] = ~sum; // Checksum
setModeTransmit();
serial.write(packet, sizeof(packet));
serial.flush();
setModeReceive();
waitOnReply(100);
}
void Feetech::enableTorque(uint8_t id) {
uint8_t packet[8]; // Adjust size based on your packet structure
packet[0] = 255; // Header
packet[1] = 255; // Header
packet[2] = id; // Servo ID
packet[3] = 4; // Length (instruction + parameters + checksum)
packet[4] = 3; // Instruction to enable torque
packet[5] = 1; // Parameter to enable torque
// Calculate checksum
uint8_t sum = 0;
for (int i = 2; i < 6; i++) {
sum += packet[i];
}
packet[6] = ~sum; // Checksum (bitwise NOT)
setModeTransmit();
serial.write(packet, sizeof(packet)); // Send the packet
serial.flush();
//delay(10); // Short delay
setModeReceive();
}
uint16_t Feetech::getPosition(uint8_t id){
sendRequest(id, 0x38);
waitOnReply(10);
return 0;
}
void Feetech::sendRequest(uint8_t id, byte instruction) {
uint8_t packet[8];
packet[0] = 0xFF; // Header
packet[1] = 0xFF; // Header
packet[2] = id; // Servo ID (ensure this matches the expected ID, e.g., 0x02)
packet[3] = 0x04; // Length (4 bytes of following data)
packet[4] = READ_DATA; // Instruction
packet[5] = instruction; // Write first address
packet[6] = 0x02; // Number of bytes to read
// Calculate checksum
uint8_t sum = 0;
for (int i = 2; i < 7; i++) sum += packet[i]; // Include all data bytes for checksum
packet[7] = ~sum; // Checksum
setModeTransmit();
serial.write(packet, sizeof(packet));
serial.flush();
setModeReceive();
}
void Feetech::pingAll() {
Serial.println("PINGING ALL 0-255");
for (int i = 0; i < 255; i++) {
sendPing(i);
waitOnReply(100);
}
Serial.println("PINGING COMPLETE");
}
void Feetech::waitOnReply(unsigned long timeout) {
unsigned long startTime = millis(); // Record the start time
while (millis() - startTime < timeout) { // Loop until timeout
if (serial.available()) {
Serial.println("RECV");
uint8_t buffer[32];
int count = 0;
// Read all available bytes
while (serial.available() && count < sizeof(buffer)) {
buffer[count++] = serial.read();
}
// Display on Serial
Serial.print("recv: ");
Serial.print(buffer[2]);
Serial.print(" ");
if (buffer[4] == 0x00) {
Serial.print("OK");
} else {
Serial.print("NOK");
}
Serial.print("\t");
for (int i = 0; i < count; i++) {
Serial.print("0x");
if (buffer[i] < 0x10) Serial.print("0");
Serial.print(buffer[i], HEX);
Serial.print(" ");
}
Serial.println();
break; // Exit the loop after processing the reply
}
delay(10); // Small delay to prevent busy-waiting
}
}
void Feetech::waitOnReply(unsigned long timeout, uint8_t expectedReturnType) {
unsigned long startTime = millis(); // Record the start time
while (millis() - startTime < timeout) { // Loop until timeout
if (serial.available()) {
Serial.println("RECV");
uint8_t buffer[32];
int count = 0;
// Read all available bytes
while (serial.available() && count < sizeof(buffer)) {
buffer[count++] = serial.read();
}
// Display on Serial
Serial.print("recv: ");
Serial.print(buffer[2]);
Serial.print(" ");
if (buffer[4] == 0x00) {
Serial.print("OK");
} else {
Serial.print("NOK");
}
Serial.print("\t");
for (int i = 0; i < count; i++) {
Serial.print("0x");
if (buffer[i] < 0x10) Serial.print("0");
Serial.print(buffer[i], HEX);
Serial.print(" ");
}
Serial.println();
break; // Exit the loop after processing the reply
}
delay(10); // Small delay to prevent busy-waiting
}
}
void Feetech::sendData(const byte* data, size_t length) {
// digitalWrite(_transmitPin, HIGH); // Enable transmit mode
// _serial.write(data, length);
// _serial.flush(); // Wait for transmission to complete
// digitalWrite(_transmitPin, LOW); // Disable transmit mode
}
size_t Feetech::receiveData(byte* buffer, size_t bufferSize) {
size_t bytesRead = 0;
while (serial.available() > 0 && bytesRead < bufferSize) {
buffer[bytesRead++] = serial.read();
}
return bytesRead;
}
void Feetech::setModeTransmit() {
digitalWrite(DE_PIN, HIGH);
digitalWrite(RE_PIN, HIGH);
delay(10);
}
void Feetech::setModeReceive() {
digitalWrite(DE_PIN, LOW);
digitalWrite(RE_PIN, LOW);
delay(10);
}
void Feetech::update() {
if (Serial.available()) {
setModeTransmit();
while (Serial.available()) {
char incomingByte = Serial.read(); // Read from USB Serial
serial.write(incomingByte); // Send to Serial1
}
setModeReceive();
}
// Pass data from Serial1 (Feetech) to Serial (USB)
if (serial.available()) {
while (serial.available()) {
char incomingByte = serial.read(); // Read from Serial1
Serial.write(incomingByte); // Send to USB Serial
}
}
}

44
feetech.h Normal file
View File

@ -0,0 +1,44 @@
#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 getPosition(uint8_t id);
void sendRequest(uint8_t id, uint8_t instruction);
void sendWritePos(uint8_t id, uint16_t position);
void pingAll();
void waitOnReply(unsigned long timeout);
void waitOnReply(unsigned long timeout, uint8_t expectedReturnType);
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
static const byte REQUEST_POSITION = 0x38; // Current Position request
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

238
rs485transmitter.ino
View File

@ -1,10 +1,26 @@
#include "feetech.h"
bool PRINT_SENT_PACKETS = false; bool PRINT_SENT_PACKETS = false;
// S3 setup
// #define RX_PIN 4 // DI
// #define DE_PIN 5 // Driver Enable
// #define RE_PIN 6 // Receiver Enable
// #define TX_PIN 7 // RO
// C3 setup
#define DE_PIN 20 #define DE_PIN 20
#define RE_PIN 10 #define RE_PIN 10
#define TX_PIN 21 #define TX_PIN 21
#define RX_PIN 9 #define RX_PIN 9
Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);
// SCS are BIG endian (high byte first)
// SMS are little endian (low byte first)
#define PING 0x01 // QUERY THE WORKING STATUS #define PING 0x01 // QUERY THE WORKING STATUS
#define READ_DATA 0x02 // READ DATA #define READ_DATA 0x02 // READ DATA
@ -16,11 +32,20 @@ bool PRINT_SENT_PACKETS = false;
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN); servos.begin();
pinMode(DE_PIN, OUTPUT);
pinMode(RE_PIN, OUTPUT);
SetModeReceive(); // // while (true){
delay(1000); // // digitalWrite(TX_PIN, !digitalRead(TX_PIN));
// // Serial.println(digitalRead(TX_PIN));
// // delay(2000);
// // }
// Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
// pinMode(DE_PIN, OUTPUT);
// pinMode(RE_PIN, OUTPUT);
// SetModeReceive();
// delay(1000);
} }
// Send move command to servo id:0-255, position:0-4095 // Send move command to servo id:0-255, position:0-4095
@ -33,8 +58,8 @@ void sendWritePos(uint8_t id, uint16_t position) {
packet[3] = 5; // Length = instruction + address + 2 bytes + checksum packet[3] = 5; // Length = instruction + address + 2 bytes + checksum
packet[4] = WRITE_DATA; // Instruction: WRITE packet[4] = WRITE_DATA; // Instruction: WRITE
packet[5] = 0x2A; // Address: Goal Position packet[5] = 0x2A; // Address: Goal Position
packet[6] = position & 0xFF; // Low byte
packet[7] = (position >> 8) & 0xFF; // High byte packet[7] = (position >> 8) & 0xFF; // High byte
packet[6] = position & 0xFF; // Low byte
// Calculate checksum // Calculate checksum
uint8_t sum = 0; uint8_t sum = 0;
@ -44,59 +69,71 @@ void sendWritePos(uint8_t id, uint16_t position) {
// Send packet // Send packet
SetModeTransmit(); SetModeTransmit();
Serial1.write(packet, sizeof(packet)); Serial1.write(packet, sizeof(packet));
Serial1.flush(); delay(10);
//Serial1.flush();
SetModeReceive(); SetModeReceive();
digitalWrite(DE_PIN, LOW); // Back to receive
if (PRINT_SENT_PACKETS) {
Serial.print("Sent packet: ");
for (int i = 0; i < 9; i++) {
Serial.print("0x");
if (packet[i] < 0x10) Serial.print("0"); // Leading zero for single-digit hex
Serial.print(packet[i], HEX);
Serial.print(" ");
}
Serial.println();
}
} }
void syncWriteTwoServos(uint8_t id1, uint16_t pos1, uint8_t id2, uint16_t pos2) {
const uint8_t SYNC_WRITE = 0x83; void syncWriteTwoServos(uint16_t pos1, uint16_t pos2) {
Serial.println(pos1);
const uint8_t START_BYTE = 0xFF; const uint8_t START_BYTE = 0xFF;
const uint8_t DATA_LEN = 0x05; // 2 bytes for position + 1 byte for speed (optional) const uint8_t BROADCAST_ID = 0xFE;
const uint8_t SYNC_WRITE = 0x83;
const uint8_t START_ADDR = 0x2A;
const uint8_t DATA_LEN_PER_SERVO = 6; // pos(2) + time(2) + speed(2)
const uint8_t NUM_SERVOS = 2;
// Build packet uint8_t packetLen = 4 + (NUM_SERVOS * (1 + DATA_LEN_PER_SERVO)); // instruction + addr + len + servo data
uint8_t packet[] = { uint8_t packet[3 + packetLen]; // header + payload
START_BYTE, START_BYTE, 0xFE, // Broadcast ID uint8_t index = 0;
0x0F, // Length = 15 bytes total
SYNC_WRITE,
0x2A, // Starting address for Goal Position (0x2A)
0x04, // Data length per servo (2 bytes pos + 2 bytes speed)
id1,
(uint8_t)(pos1 & 0xFF), // Position low byte
(uint8_t)((pos1 >> 8) & 0xFF),// Position high byte
0x00, 0x00, // Speed (optional, set to 0)
id2,
(uint8_t)(pos2 & 0xFF),
(uint8_t)((pos2 >> 8) & 0xFF),
0x00, 0x00
};
// Calculate checksum // Header
packet[index++] = START_BYTE;
packet[index++] = START_BYTE;
packet[index++] = BROADCAST_ID;
packet[index++] = packetLen;
packet[index++] = SYNC_WRITE;
packet[index++] = START_ADDR;
packet[index++] = DATA_LEN_PER_SERVO;
// Servo 1
packet[index++] = 0x01; // ID
packet[index++] = (pos1 >> 8) & 0xFF; // Position high
packet[index++] = pos1 & 0xFF; // Position low
packet[index++] = 0x00; // Time high
packet[index++] = 0x00; // Time low
packet[index++] = 0x07; // High byte
packet[index++] = 0xD0; // Low byte 2000
// Servo 2
packet[index++] = 0x02; // ID
packet[index++] = pos2 & 0xFF;
packet[index++] = (pos2 >> 8) & 0xFF;
packet[index++] = 0x00;
packet[index++] = 0x00;
packet[index++] = 0xE8;
packet[index++] = 0x03;
// Checksum
uint8_t checksum = 0; uint8_t checksum = 0;
for (int i = 2; i < sizeof(packet); i++) { for (uint8_t i = 2; i < index; i++) {
checksum += packet[i]; checksum += packet[i];
} }
checksum = ~checksum; checksum = ~checksum & 0xFF;
// Send packet // Send packet
SetModeTransmit(); SetModeTransmit();
Serial1.write(packet, sizeof(packet)); Serial1.write(packet, index);
Serial1.write(checksum); Serial1.write(checksum);
Serial1.flush(); Serial1.flush();
delay(2);
SetModeReceive(); SetModeReceive();
} }
void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) { void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
const uint8_t SYNC_WRITE = 0x83; const uint8_t SYNC_WRITE = 0x83;
const uint8_t START_BYTE = 0xFF; const uint8_t START_BYTE = 0xFF;
@ -123,10 +160,10 @@ void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
// Servo data // Servo data
for (uint8_t i = 0; i < count; i++) { for (uint8_t i = 0; i < count; i++) {
packet[index++] = ids[i]; packet[index++] = ids[i];
packet[index++] = (uint8_t)(positions[i] & 0xFF); // Low byte
packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte
packet[index++] = 0x00; // Speed low byte packet[index++] = (uint8_t)(positions[i] & 0xFF); // Low byte
packet[index++] = 0x00; // Speed high byte packet[index++] = 0x00; // Speed high byte
packet[index++] = 0x00; // Speed low byte
} }
// Checksum // Checksum
@ -229,7 +266,7 @@ void SetModeReceive() {
void PingAll() { void PingAll() {
Serial.println("PINGING ALL 0-255"); Serial.println("PINGING ALL 0-255");
for (int i = 0; i < 255; i++) { for (int i = 180; i < 255; i++) {
sendPing(i); sendPing(i);
@ -252,30 +289,105 @@ void TestRange(uint8_t id) {
} }
} }
uint8_t ids[] = { 0, 4, 10, 15, 40, 41, 43, 44}; uint8_t ids[] = { 1, 2 }; //, 10, 15, 40, 41, 43, 44};
uint16_t pos[8]; uint16_t pos[2];
void loop() { void loop() {
//PingAll(); //PingAll();
for (int i = 0; i < 4; i += 1) {
for (int x = 0; x < 8; x++){
pos[x] = i;
} if (true) {
syncWriteServos(ids, pos, 8); for (int i = 0; i < 4096; i += 128) {
servos.sendWritePos(103, i);
servos.waitOnReply(10);
servos.getPosition(103);
Serial.println(i); Serial.println(i);
//syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]); delay(2000);
delay(100);
} }
for (int i = 4; i > 0; i -= 8) { for (int i = 4095; i > 0; i -= 128) {
for (int x = 0; x < 8; x++){ servos.sendWritePos(103, i);
pos[x] = i; servos.waitOnReply(10);
} servos.getPosition(103);
syncWriteServos(ids, pos, 2);
Serial.println(i); Serial.println(i);
//syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]); delay(2000);
delay(100);
} }
// TestRange(0); }
//servos.testRequest();
//servos.update();
//servos.enableTorque(1);
//servos.enableTorque(2);
// servos.pingAll();
// delay(2000);
// for (int i = 0; i < 4096; i += 32) {
// servos.sendWritePos(103, i);
// servos.waitOnReply(10);
// delay(100);
// }
return;
//servos.update();
// //servos.pingAll();
// //servos.sendWritePos(1, 100);
//delay(2000);
// servos.waitOnReply(10);
// delay(2000);
// servos.sendRequest(103, 0x02);
// servos.waitOnReply(10);
// delay(2000);
// for (int i = 0; i < 1024; i++){
// servos.sendWritePos(103, i);
// servos.waitOnReply(10);
// Serial.println(i);
// delay(50);
// }
// servos.waitOnReply(10);
// delay(2000);
// servos.sendWritePos(103, 500);
// servos.waitOnReply(10);
// delay(2000);
// servos.sendWritePos(103, 1000);
// //servos.sendRequest(103, 0x02);
// servos.waitOnReply(10);
// delay(2000);
return;
//WaitOnReply(100);
//PingAll();
//return;
// for (uint16_t pos = 0; pos <= 1024; pos += 16) {
// syncWriteTwoServos(pos, 1024 - pos);
// delay(500);
// }
// for (int i = 0; i < 1024; i += 8) {
// for (int x = 0; x < 2; x++) {
// pos[x] = i;
// }
// //sendWritePos(1, i);
// sendWritePos(103, i);
// //syncWriteServos(ids, pos, 2);
// Serial.println(i);
// //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
// delay(10);
// }
// for (int i = 1025; i > 0; i -= 8) {
// for (int x = 0; x < 2; x++) {
// pos[x] = i;
// }
// //sendWritePos(1, i);
// sendWritePos(103, i);
// //syncWriteServos(ids, pos, 2);
// Serial.println(i);
// //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
// delay(10);
// }
//TestRange(1);
// TestRange(4); // TestRange(4);
// TestRange(10); // TestRange(10);
// TestRange(15); // TestRange(15);

2
sketch.yaml Normal file
View File

@ -0,0 +1,2 @@
default_fqbn: arduino:avr:uno
default_port: COM3