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

removed DE/RE implementation and delay as no longer required with direct TTL setup

master
Jake 2025-10-01 00:35:00 +08:00
parent 366ba5fae3
commit 91f6e269ed
3 changed files with 74 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
HansonServo.ino
View File

@ -22,10 +22,10 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
// ESP32 S2 PINOUT
#define RX_PIN 17 // DI
#define TX_PIN 18 // RO
#define DE_PIN 33 // Driver Enable
#define RE_PIN 3 // Receiver Enable
#define RX_PIN 18 // RO
#define TX_PIN 17 // DI
#define DE_PIN 7 // Driver Enable
#define RE_PIN 8 // Receiver Enable
Animation anim;
Animation sweep;
@ -49,18 +49,29 @@ void setup() {
delay(500);
}
// pinMode(RX_PIN, OUTPUT);
// pinMode(TX_PIN, OUTPUT);
// pinMode(DE_PIN, OUTPUT);
// while (true){
// Serial.println(!digitalRead(RX_PIN));
// digitalWrite(RX_PIN, !digitalRead(RX_PIN));
// digitalWrite(TX_PIN, !digitalRead(TX_PIN));
// digitalWrite(DE_PIN, !digitalRead(DE_PIN));
// delay(2000);
// }
pos2[3] = flipBytes(pos2[3]);
servos.begin();
if (!FFat.begin()) {
if (!FFat.begin(true)) {
Serial.println("FFat mount failed");
return;
}
anim.loadFromFile("/bob.anim");
anim.printKeyframes();
playAnimation(anim);
// anim.loadFromFile("/bob.anim");
// anim.printKeyframes();
// playAnimation(anim);
// ClearFiles();
// // PrintFileList();
// sweep.clear();
@ -525,8 +536,11 @@ unsigned long lastSend = 0;
void loop() {
HandleSerialRequests();
// put your main code here, to run repeatedly:
//PingAll();
// servos.sendWritePos(10, 0);
// delay(1000);
// servos.sendWritePos(10, 200);
// delay(1000);
//PingAll();
// for (int i = 0; i < 1023; i++) {
// for (int z = 0; z < 5; z++) {
// pos1[z] = i;

80
feetech.cpp
View File

@ -94,6 +94,9 @@ void Feetech::sendPing(uint8_t id) {
for (int i = 2; i < 5; i++) sum += packet[i];
packet[5] = ~sum; // Checksum
memcpy(lastSentPacket, packet, sizeof(packet));
lastSentLength = sizeof(packet);
// Send packet
//Serial.println("PING");
setModeTransmit();
@ -398,57 +401,52 @@ void Feetech::waitOnReply(unsigned long timeout) {
}
uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
unsigned long startTime = millis(); // Record the start time
unsigned long startTime = millis();
while (millis() - startTime < timeout) { // Loop until timeout
while (millis() - startTime < 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();
}
//Serial.println(count);
// if (count != 8) {
// Serial.print("ERROR: Expected 8 byte reply, recieved ");
// Serial.println(count);
// return 0;
// } else {
// }
// Display on Serial
Serial.print("recv: ");
Serial.print(buffer[2]);
Serial.print(" ");
if (buffer[4] == 0x00) {
Serial.print("OK");
} else {
Serial.print("NOK");
// Skip echoed packet if it matches the start of the buffer
int startIndex = 0;
if (count >= lastSentLength && memcmp(buffer, lastSentPacket, lastSentLength) == 0) {
startIndex = lastSentLength;
}
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(" ");
// Check if there's a valid packet after the echo
if (count - startIndex >= 4 && buffer[startIndex] == 0xFF && buffer[startIndex + 1] == 0xFF) {
uint8_t length = buffer[startIndex + 3];
int packetSize = length + 4;
if (startIndex + packetSize <= count) {
Serial.print("recv: ");
Serial.print(buffer[startIndex + 2]); // ID
Serial.print(" ");
Serial.print(buffer[startIndex + 4] == 0x00 ? "OK" : "NOK");
Serial.print("\t");
for (int i = startIndex; i < startIndex + packetSize; i++) {
Serial.print("0x");
if (buffer[i] < 0x10) Serial.print("0");
Serial.print(buffer[i], HEX);
Serial.print(" ");
}
Serial.println();
return buffer[startIndex + 5]; // Return value byte
}
}
Serial.println();
uint8_t val = buffer[5];
return val;
break; // Exit the loop after processing the reply
} else {
//Serial.println(millis() - startTime);
}
delay(1); // Small delay to prevent busy-waiting
delay(1);
}
return 0;
}
uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) {
unsigned long startTime = millis(); // Record the start time
@ -515,15 +513,15 @@ size_t Feetech::receiveData(byte* buffer, size_t bufferSize) {
}
void Feetech::setModeTransmit() {
digitalWrite(DE_PIN, HIGH);
digitalWrite(RE_PIN, HIGH);
delay(10);
// digitalWrite(DE_PIN, HIGH);
// digitalWrite(RE_PIN, HIGH);
//delay(10);
}
void Feetech::setModeReceive() {
digitalWrite(DE_PIN, LOW);
digitalWrite(RE_PIN, LOW);
delay(10);
// digitalWrite(DE_PIN, LOW);
// digitalWrite(RE_PIN, LOW);
//delay(10);
}
void Feetech::update() {

18
feetech.h
View File

@ -1,7 +1,7 @@
#ifndef FEETECH_H
#define FEETECH_H
#include <vector>
#include <vector>
#include <Arduino.h>
#include <HardwareSerial.h>
@ -38,7 +38,7 @@ public:
uint16_t getLoad(uint8_t id);
uint8_t getTemperature(uint8_t id);
uint8_t getMoving(uint8_t id);
float getCurrent(uint8_t id);
@ -91,12 +91,12 @@ public:
static const byte REQUEST_POSITION = 0x38; // 2 bytes
static const byte REQUEST_CURRENT_SPEED = 0x3A; // 2 bytes
static const byte REQUEST_CURRENT_LOAD = 0x3C; // 2 bytes
static const byte REQUEST_CURRENT_LOAD = 0x3C; // 2 bytes
static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte
static const byte REQUEST_TEMPERATURE = 0x3F; // 1 byte
static const byte REQUEST_MOVING = 0x42; // 1 byte
static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte
static const byte REQUEST_TEMPERATURE = 0x3F; // 1 byte
static const byte REQUEST_MOVING = 0x42; // 1 byte
static const byte REQUEST_CURRENT_CURRENT = 0x45; // 2 bytes
@ -110,6 +110,10 @@ public:
static const byte SMS_STS_57600 = 6;
static const byte SMS_STS_38400 = 7;
uint8_t lastSentPacket[32];
int lastSentLength = 0;
private:
HardwareSerial& serial; // Reference to the HardwareSerial object
uint8_t DE_PIN = 20;