added serial passthrough define option at astart of HansonServo.ino for using feetech app
parent
4375fb283f
commit
d603ef2e18
161
HansonServo.ino
161
HansonServo.ino
|
|
@ -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
|
||||
|
|
@ -37,7 +42,7 @@ 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;
|
||||
|
|
@ -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 &&
|
||||
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);
|
||||
|
|
@ -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