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

added serial passthrough define option at astart of HansonServo.ino for using feetech app

protocolv2
Jake 2025-12-13 13:22:24 +08:00
parent 4375fb283f
commit d603ef2e18
1 changed files with 107 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
HansonServo.ino
View File

@ -1,24 +1,29 @@
/** /**
* HansonServo Firmware * HansonServo Firmware
* *
* Unified robot controller with: * Unified robot controller with:
* - Feetech servo control (SCS/STS) * - Feetech servo control (SCS/STS)
* - Animation playback via node graphs * - Animation playback via node graphs
* - BNO055 IMU * - ADXL345 IMU
* - RD-03D mmWave radar * - RD-03D mmWave radar
* - CRC16 tagged packet protocol * - CRC16 tagged packet protocol
* *
* Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B] * Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B]
*/ */
#include <FFat.h> // Configuration defines
#include "protocol.h" #define ENABLE_SERIAL_PASSTHROUGH \
false // Set true to use Feetech app comms straight to servos
#include "RobotConfig.h"
#include "animation.h"
#include "commands.h" #include "commands.h"
#include "motorcontrol.h" #include "motorcontrol.h"
#include "animation.h"
#include "nodegraph.h" #include "nodegraph.h"
#include "RobotConfig.h" #include "protocol.h"
#include "sensors.h" #include "sensors.h"
#include <FFat.h>
// ============================================================================ // ============================================================================
// Global State // Global State
@ -37,9 +42,9 @@ constexpr uint32_t HEARTBEAT_INTERVAL_MS = 1000;
// ============================================================================ // ============================================================================
uint16_t getSineWaveValue(unsigned long centiseconds) { uint16_t getSineWaveValue(unsigned long centiseconds) {
constexpr uint16_t WAVE_PERIOD_CS = 400; // 4 seconds constexpr uint16_t WAVE_PERIOD_CS = 400; // 4 seconds
constexpr uint16_t WAVE_MAX = 4095; constexpr uint16_t WAVE_MAX = 4095;
float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS; float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS;
float sine = sin(theta); float sine = sin(theta);
float scaled = (sine + 1.0) * (WAVE_MAX / 2.0); float scaled = (sine + 1.0) * (WAVE_MAX / 2.0);
@ -56,6 +61,24 @@ void handleProtocol() {
} }
} }
// ============================================================================
// Serial Passthrough (USB ↔ Servo Serial)
// ============================================================================
#if ENABLE_SERIAL_PASSTHROUGH
void handleSerialPassthrough() {
// Forward USB Serial → Servo Serial1
while (Serial.available()) {
Serial1.write(Serial.read());
}
// Forward Servo Serial1 → USB Serial
while (Serial1.available()) {
Serial.write(Serial1.read());
}
}
#endif
// ============================================================================ // ============================================================================
// Animation Playback // Animation Playback
// ============================================================================ // ============================================================================
@ -80,28 +103,29 @@ void runNodeAnimation() {
config.enableAllMotors(); config.enableAllMotors();
uint32_t now = millis(); uint32_t now = millis();
if (now - lastTickTime < FRAME_INTERVAL_MS) return; if (now - lastTickTime < FRAME_INTERVAL_MS)
lastTickTime = now; return;
lastTickTime = now;
// Tick the node graph // Tick the node graph
animState.current->nodeGraph.tick(currentTick, *animState.current); animState.current->nodeGraph.tick(currentTick, *animState.current);
auto outputs = animState.current->nodeGraph.getServoOutputs(); auto outputs = animState.current->nodeGraph.getServoOutputs();
// Collect motor commands // Collect motor commands
std::vector<uint8_t> motorIDs; std::vector<uint8_t> motorIDs;
std::vector<uint16_t> positions; std::vector<uint16_t> positions;
std::vector<uint16_t> speeds; std::vector<uint16_t> speeds;
for (const auto& [motorID, value] : outputs) { for (const auto &[motorID, value] : outputs) {
if (value != 65535) { if (value != 65535) {
motorIDs.push_back(motorID); motorIDs.push_back(motorID);
positions.push_back(value); positions.push_back(value);
speeds.push_back(0); speeds.push_back(0);
config.setMotorPosition(motorID, value); config.setMotorPosition(motorID, value);
config.setMotorEnabled(motorID, true); config.setMotorEnabled(motorID, true);
} else { } else {
// Only disable torque for motors that should be limp // Only disable torque for motors that should be limp
if (config.setMotorEnabled(motorID, false)) { if (config.setMotorEnabled(motorID, false)) {
servoManager[0]->disableTorque(motorID); servoManager[0]->disableTorque(motorID);
} }
} }
@ -109,14 +133,8 @@ void runNodeAnimation() {
// Send all positions in one sync write - motors move together! // Send all positions in one sync write - motors move together!
if (!motorIDs.empty()) { if (!motorIDs.empty()) {
servoManager.syncWritePositions( servoManager.syncWritePositions(motorIDs.data(), positions.data(),
motorIDs.data(), speeds.data(), motorIDs.size(), config, 0);
positions.data(),
speeds.data(),
motorIDs.size(),
config,
0
);
} }
// Emit per-frame event: [frameLo, frameHi, playMode, status=0] // Emit per-frame event: [frameLo, frameHi, playMode, status=0]
@ -125,45 +143,45 @@ void runNodeAnimation() {
payload[0] = currentTick & 0xFF; payload[0] = currentTick & 0xFF;
payload[1] = (currentTick >> 8) & 0xFF; payload[1] = (currentTick >> 8) & 0xFF;
payload[2] = static_cast<uint8_t>(animState.playMode); payload[2] = static_cast<uint8_t>(animState.playMode);
payload[3] = 0; // in-progress payload[3] = 0; // in-progress
sendPacket(Tag::FRAME, payload, 4); sendPacket(Tag::FRAME, payload, 4);
} }
currentTick++; currentTick++;
// Handle animation end (0 = run indefinitely for variable-only animations) // Handle animation end (0 = run indefinitely for variable-only animations)
if (animState.current->getFrameCount() > 0 && if (animState.current->getFrameCount() > 0 &&
currentTick > animState.current->getFrameCount()) { currentTick > animState.current->getFrameCount()) {
switch (animState.playMode) { switch (animState.playMode) {
case PLAY_ONCE: case PLAY_ONCE:
animState.stop(); animState.stop();
{ {
uint8_t done[4]; uint8_t done[4];
done[0] = currentTick & 0xFF; done[0] = currentTick & 0xFF;
done[1] = (currentTick >> 8) & 0xFF; done[1] = (currentTick >> 8) & 0xFF;
done[2] = static_cast<uint8_t>(animState.playMode); done[2] = static_cast<uint8_t>(animState.playMode);
done[3] = 1; // complete done[3] = 1; // complete
sendPacket(Tag::FRAME, done, 4); sendPacket(Tag::FRAME, done, 4);
}
break;
case PLAY_LOOP:
// Continue looping
break;
case PLAY_REPEAT:
if (--animState.repeatsRemaining == 0) {
animState.stop();
uint8_t done[4];
done[0] = currentTick & 0xFF;
done[1] = (currentTick >> 8) & 0xFF;
done[2] = static_cast<uint8_t>(animState.playMode);
done[3] = 1; // complete
sendPacket(Tag::FRAME, done, 4);
}
break;
default:
break;
} }
currentTick = 0; break;
case PLAY_LOOP:
// Continue looping
break;
case PLAY_REPEAT:
if (--animState.repeatsRemaining == 0) {
animState.stop();
uint8_t done[4];
done[0] = currentTick & 0xFF;
done[1] = (currentTick >> 8) & 0xFF;
done[2] = static_cast<uint8_t>(animState.playMode);
done[3] = 1; // complete
sendPacket(Tag::FRAME, done, 4);
}
break;
default:
break;
}
currentTick = 0;
} }
} }
@ -174,10 +192,11 @@ void runNodeAnimation() {
void updateMotorPositions() { void updateMotorPositions() {
static unsigned long lastUpdate = 0; static unsigned long lastUpdate = 0;
if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS) return; if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
return;
lastUpdate = millis(); lastUpdate = millis();
for (const Motor& motor : config.motors) { for (const Motor &motor : config.motors) {
servoManager[0]->setFeetechMode(motor.servoModel.major); servoManager[0]->setFeetechMode(motor.servoModel.major);
uint16_t position = servoManager[0]->getPosition(motor.motorID); uint16_t position = servoManager[0]->getPosition(motor.motorID);
config.setMotorPosition(motor.motorID, position); config.setMotorPosition(motor.motorID, position);
@ -196,21 +215,27 @@ void handleMotorStreaming() {
void sendHeartbeat() { void sendHeartbeat() {
static unsigned long lastHeartbeat = 0; static unsigned long lastHeartbeat = 0;
if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS) return; if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS)
return;
lastHeartbeat = millis(); lastHeartbeat = millis();
// Pack state: uptime(4) + flags(2) // Pack state: uptime(4) + flags(2)
uint8_t payload[6]; uint8_t payload[6];
uint32_t uptime = millis() / 1000; uint32_t uptime = millis() / 1000;
uint16_t flags = 0; uint16_t flags = 0;
// Build flags // Build flags
if (adxl.isReady()) flags |= 0x01; if (adxl.isReady())
if (animState.current && animState.current->isActive()) flags |= 0x02; flags |= 0x01;
if (motorStream.active) flags |= 0x04; if (animState.current && animState.current->isActive())
if (sensors.isADXLStreamEnabled()) flags |= 0x08; flags |= 0x02;
if (sensors.isRadarStreamEnabled()) flags |= 0x10; if (motorStream.active)
flags |= 0x04;
if (sensors.isADXLStreamEnabled())
flags |= 0x08;
if (sensors.isRadarStreamEnabled())
flags |= 0x10;
payload[0] = uptime & 0xFF; payload[0] = uptime & 0xFF;
payload[1] = (uptime >> 8) & 0xFF; payload[1] = (uptime >> 8) & 0xFF;
@ -230,7 +255,7 @@ void setup() {
// Serial setup (buffer size must be set before begin) // Serial setup (buffer size must be set before begin)
Serial.setRxBufferSize(8192); Serial.setRxBufferSize(8192);
Serial.begin(1000000); Serial.begin(1000000);
// Startup delay // Startup delay
delay(500); delay(500);
Serial.println("\n[HansonServo] Starting..."); Serial.println("\n[HansonServo] Starting...");
@ -265,19 +290,25 @@ void setup() {
// ============================================================================ // ============================================================================
void loop() { void loop() {
// Serial passthrough (when enabled)
#if ENABLE_SERIAL_PASSTHROUGH
handleSerialPassthrough();
return;
#endif
// Protocol handling // Protocol handling
handleProtocol(); handleProtocol();
// Animation playback // Animation playback
runNodeAnimation(); runNodeAnimation();
// Motor position updates // Motor position updates
updateMotorPositions(); updateMotorPositions();
handleMotorStreaming(); handleMotorStreaming();
// Sensor updates and streaming // Sensor updates and streaming
sensors.update(); sensors.update();
// Heartbeat // Heartbeat
sendHeartbeat(); sendHeartbeat();
} }