added serial passthrough define option at astart of HansonServo.ino for using feetech app
parent
4375fb283f
commit
d603ef2e18
|
|
@ -4,21 +4,26 @@
|
|||
* 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
|
||||
|
|
@ -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,7 +103,8 @@ void runNodeAnimation() {
|
|||
config.enableAllMotors();
|
||||
|
||||
uint32_t now = millis();
|
||||
if (now - lastTickTime < FRAME_INTERVAL_MS) return;
|
||||
if (now - lastTickTime < FRAME_INTERVAL_MS)
|
||||
return;
|
||||
lastTickTime = now;
|
||||
|
||||
// Tick the node graph
|
||||
|
|
@ -92,7 +116,7 @@ void runNodeAnimation() {
|
|||
std::vector<uint16_t> positions;
|
||||
std::vector<uint16_t> speeds;
|
||||
|
||||
for (const auto& [motorID, value] : outputs) {
|
||||
for (const auto &[motorID, value] : outputs) {
|
||||
if (value != 65535) {
|
||||
motorIDs.push_back(motorID);
|
||||
positions.push_back(value);
|
||||
|
|
@ -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]
|
||||
|
|
@ -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);
|
||||
|
|
@ -197,7 +216,8 @@ 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)
|
||||
|
|
@ -206,11 +226,16 @@ void sendHeartbeat() {
|
|||
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;
|
||||
|
|
@ -265,6 +290,12 @@ void setup() {
|
|||
// ============================================================================
|
||||
|
||||
void loop() {
|
||||
// Serial passthrough (when enabled)
|
||||
#if ENABLE_SERIAL_PASSTHROUGH
|
||||
handleSerialPassthrough();
|
||||
return;
|
||||
#endif
|
||||
|
||||
// Protocol handling
|
||||
handleProtocol();
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue