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