fallensigh/craic
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WIP: snapshot before repo migration

Browse Source
This commit is contained in:
FallenSigh
2026-05-27 02:14:33 +08:00
parent aa7ed7cf51
commit ec41ed8a48
51 changed files with 3119 additions and 2015 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
jxbeye/.clangd
View File

@@ -15,6 +15,7 @@ CompileFlags:
- -isystem/home/fallensigh/.platformio/packages/toolchain-xtensa-esp32s3/lib/gcc/xtensa-esp32s3-elf/8.4.0/include-fixed
- -isystem/home/fallensigh/.platformio/packages/toolchain-xtensa-esp32s3/xtensa-esp32s3-elf/sys-include
- -isystem/home/fallensigh/.platformio/packages/toolchain-xtensa-esp32s3/xtensa-esp32s3-elf/include
- -Ilib/FTServo/src
Remove:
- -mlongcalls
- -fstrict-volatile-bitfields

21
jxbeye/lib/FTServo/LICENSE Normal file
View File

@@ -0,0 +1,21 @@
MIT License
Copyright (c) 2024 ftservo
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

44
jxbeye/lib/FTServo/README.md Normal file
View File

@@ -0,0 +1,44 @@
# FEETECH BUS Servo
> FEETECH BUS Servo library for Arduino and ESP32
## Table of Contents
<!-- TOC -->
* [FEETECH BUS Servo](#ft-series-servo)
* [Table of Contents](#table-of-contents)
* [Requirements](#requirements)
* [Usage](#usage)
* [Notes](#notes)
* [Release](#release)
<!-- TOC -->
## Requirements
* arduino-1.6.10 or newer(https://www.arduino.cc/).
## Usage
For usage examples, see the [examples](./examples) directory.
## Notes
The code for the `SCServo` library is divided into the following
parts:
* communication layer: [src/SCS.cpp](src/SCS.cpp)
* hardware interface layer: [src/SCSerial.cpp](src/SCSerial.cpp)
* application layer:
* corresponds to the three series of FEETECH BUS Servo
* `SCSCL` application layer program: [src/SCSCL.h](src/SCSCL.h) and [src/SCSCL.cpp](src/SCSCL.cpp)
* `SMS` and `STS` application layer program: [src/SMS_STS.h](src/SMS_STS.h) and [src/SMS_STS.cpp](src/SMS_STS.cpp)
* `HLS` application layer program: [src/HLSCL.h](src/HLSCL.h) and [src/HLSCL.cpp](src/HLSCL.cpp)
* instruction definition header file: [src/INST.h](src/INST.h)
* communication layer program: [src/SCS.h](src/SCS.h) and [src/SCS.cpp](src/SCS.cpp)
* hardware interface program: [src/SCSerial.h](src/SCSerial.h) and [src/SCSerial.cpp](src/SCSerial.cpp)
Note that there are differences in the memory table definitions of different series of FEETECH BUS servos.
## Release
New (GitHub) releases of this library are automatically ingressed by the Arduino Library Manager.

22
jxbeye/lib/FTServo/examples/HLSCL/Broadcast/Broadcast.ino Normal file
View File

@@ -0,0 +1,22 @@
#include <SCServo.h>
HLSCL hlscl;
void setup()
{
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(广播)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P1=4095位置
hlscl.WritePosEx(0xfe, 4095, 60, 50, 500);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
//舵机(广播)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P0=0位置
hlscl.WritePosEx(0xfe, 0, 60, 50, 500);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
}

28
jxbeye/lib/FTServo/examples/HLSCL/CalibrationOfs/CalibrationOfs.ino Normal file
View File

@@ -0,0 +1,28 @@
/*
中位校准例子
*/
#include <SCServo.h>
HLSCL hlscl;
void setup()
{
Serial.begin(115200);
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
hlscl.CalibrationOfs(1);
delay(10);
}
void loop()
{
int pos = hlscl.ReadPos(1);
if(!hlscl.getLastError()){
Serial.print("mid pos:");
Serial.println(pos);
}
delay(1000);
}

148
jxbeye/lib/FTServo/examples/HLSCL/FeedBack/FeedBack.ino Normal file
View File

@@ -0,0 +1,148 @@
/*
回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流；
FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态；
函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态,
无需调用FeedBack函数。
*/
#include <SCServo.h>
HLSCL hlscl;
int LEDpin = 13;
void setup()
{
pinMode(LEDpin,OUTPUT);
digitalWrite(LEDpin, HIGH);
Serial1.begin(1000000);//mega2560
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s32
Serial.begin(115200);
hlscl.pSerial = &Serial1;
delay(1000);
}
void loop()
{
int Pos;
int Speed;
int Load;
int Voltage;
int Temper;
int Move;
int Current;
hlscl.FeedBack(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Pos = hlscl.ReadPos(-1);
Speed = hlscl.ReadSpeed(-1);
Load = hlscl.ReadLoad(-1);
Voltage = hlscl.ReadVoltage(-1);
Temper = hlscl.ReadTemper(-1);
Move = hlscl.ReadMove(-1);
Current = hlscl.ReadCurrent(-1);
Serial.print("Position:");
Serial.println(Pos);
Serial.print("Speed:");
Serial.println(Speed);
Serial.print("Load:");
Serial.println(Load);
Serial.print("Voltage:");
Serial.println(Voltage);
Serial.print("Temper:");
Serial.println(Temper);
Serial.print("Move:");
Serial.println(Move);
Serial.print("Current:");
Serial.println(Current);
delay(10);
}else{
digitalWrite(LEDpin, HIGH);
Serial.println("FeedBack err");
delay(500);
}
Pos = hlscl.ReadPos(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo position:");
Serial.println(Pos, DEC);
delay(10);
}else{
Serial.println("read position err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Voltage = hlscl.ReadVoltage(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Voltage:");
Serial.println(Voltage, DEC);
delay(10);
}else{
Serial.println("read Voltage err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Temper = hlscl.ReadTemper(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo temperature:");
Serial.println(Temper, DEC);
delay(10);
}else{
Serial.println("read temperature err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Speed = hlscl.ReadSpeed(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Speed:");
Serial.println(Speed, DEC);
delay(10);
}else{
Serial.println("read Speed err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Load = hlscl.ReadLoad(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Load:");
Serial.println(Load, DEC);
delay(10);
}else{
Serial.println("read Load err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Current = hlscl.ReadCurrent(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Current:");
Serial.println(Current, DEC);
delay(10);
}else{
Serial.println("read Current err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Move = hlscl.ReadMove(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Move:");
Serial.println(Move, DEC);
delay(10);
}else{
Serial.println("read Move err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Serial.println();
}

34
jxbeye/lib/FTServo/examples/HLSCL/Ping/Ping.ino Normal file
View File

@@ -0,0 +1,34 @@
/*
Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况
*/
#include <SCServo.h>
HLSCL hlscl;
int LEDpin = 13;
void setup()
{
pinMode(LEDpin,OUTPUT);
digitalWrite(LEDpin, HIGH);
Serial.begin(115200);
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
}
void loop()
{
int ID = hlscl.Ping(1);
if(!hlscl.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo ID:");
Serial.println(ID, DEC);
delay(100);
}else{
Serial.println("Ping servo ID error!");
digitalWrite(LEDpin, HIGH);
delay(2000);
}
}

27
jxbeye/lib/FTServo/examples/HLSCL/ProgramEprom/ProgramEprom.ino Normal file
View File

@@ -0,0 +1,27 @@
/*
舵机参数编程
*/
#include <SCServo.h>
int LEDpin = 13;
HLSCL hlscl;
void setup()
{
pinMode(LEDpin, OUTPUT);
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
digitalWrite(LEDpin, LOW);
hlscl.unLockEprom(1);//打开EPROM保存功能
hlscl.writeByte(1, HLSCL_ID, 2);//ID
hlscl.LockEprom(2);//关闭EPROM保存功能
digitalWrite(LEDpin, HIGH);
}
void loop()
{
}

26
jxbeye/lib/FTServo/examples/HLSCL/RegWritePos/RegWritePos.ino Normal file
View File

@@ -0,0 +1,26 @@
#include <SCServo.h>
HLSCL hlscl;
void setup()
{
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=3500*6.5=3250mA，运行至P1=4095位置
hlscl.RegWritePosEx(1, 4095, 60, 50, 500);
hlscl.RegWritePosEx(2, 4095, 60, 50, 500);
hlscl.RegWriteAction();
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P1=4095位置
hlscl.RegWritePosEx(1, 0, 60, 50, 500);
hlscl.RegWritePosEx(2, 0, 60, 50, 500);
hlscl.RegWriteAction();
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
}

45
jxbeye/lib/FTServo/examples/HLSCL/SyncRead/SyncRead.ino Normal file
View File

@@ -0,0 +1,45 @@
/*
同步读指令，回读ID1与ID2两个舵机的位置与速度信息
*/
#include <SCServo.h>
HLSCL hlscl;
uint8_t ID[] = {1, 2};
uint8_t rxPacket[4];
int16_t Position;
int16_t Speed;
void setup()
{
Serial.begin(115200);
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
hlscl.syncReadBegin(sizeof(ID), sizeof(rxPacket), 5);//10*10*2=200us<5ms
delay(1000);
}
void loop()
{
hlscl.syncReadPacketTx(ID, sizeof(ID), HLSCL_PRESENT_POSITION_L, sizeof(rxPacket));//同步读指令包发送
for(uint8_t i=0; i<sizeof(ID); i++){
//接收ID[i]同步读返回包
if(!hlscl.syncReadPacketRx(ID[i], rxPacket)){
Serial.print("ID:");
Serial.println(ID[i]);
Serial.println("sync read error!");
continue;//接收解码失败
}
Position = hlscl.syncReadRxPacketToWrod(15);//解码两个字节 bit15为方向位,参数=0表示无方向位
Speed = hlscl.syncReadRxPacketToWrod(15);//解码两个字节 bit15为方向位,参数=0表示无方向位
Serial.print("ID:");
Serial.println(ID[i]);
Serial.print("Position:");
Serial.println(Position);
Serial.print("Speed:");
Serial.println(Speed);
}
delay(10);
}

39
jxbeye/lib/FTServo/examples/HLSCL/SyncWritePos/SyncWritePos.ino Normal file
View File

@@ -0,0 +1,39 @@
#include <SCServo.h>
HLSCL hlscl;
byte ID[2];
s16 Position[2];
u16 Speed[2];
byte ACC[2];
u16 Torque[2];
void setup()
{
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
ID[0] = 1;//舵机ID1
ID[1] = 2;//舵机ID2
Speed[0] = 60;//最高速度V=60*0.732=43.92rpm
Speed[1] = 60;//最高速度V=60*0.732=43.92rpm
ACC[0] = 50;//加速度A=50*8.7deg/s^2
ACC[1] = 50;//加速度A=50*8.7deg/s^2
Torque[0] = 300;//最大扭矩电流T=500*6.5=3250mA
Torque[1] = 300;//最大扭矩电流T=500*6.5=3250mA
}
void loop()
{
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P1=4095位置
Position[0] = 4095;
Position[1] = 4095;
hlscl.SyncWritePosEx(ID, 2, Position, Speed, ACC, Torque);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P0=0位置
Position[0] = 0;
Position[1] = 0;
hlscl.SyncWritePosEx(ID, 2, Position, Speed, ACC, Torque);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
}

50
jxbeye/lib/FTServo/examples/HLSCL/SyncWriteSpe/SyncWriteSpe.ino Normal file
View File

@@ -0,0 +1,50 @@
#include <SCServo.h>
HLSCL hlscl;
byte ID[2];
s16 Speed[2];
byte ACC[2];
u16 Torque[2];
void setup()
{
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
hlscl.WheelMode(1);//舵机ID1切换至恒速模式
hlscl.WheelMode(2);//舵机ID2切换至恒速模式
ID[0] = 1;//舵机ID1
ID[1] = 2;//舵机ID2
ACC[0] = 50;//加速度A=50*8.7deg/s^2
ACC[1] = 50;//加速度A=50*8.7deg/s^2
Torque[0] = 500;//最大扭矩电流T=500*6.5=3250mA
Torque[1] = 500;//最大扭矩电流T=500*6.5=3250mA
}
void loop()
{
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，加速至最高速度V=60*0.732=43.92rpm，并保持恒速运行，最大扭矩电流T=500*6.5=3250mA
Speed[0] = 60;
Speed[1] = 60;
hlscl.SyncWriteSpe(ID, 2, Speed, ACC, Torque);
delay(5000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，减速至速度0停止运行
Speed[0] = 0;
Speed[1] = 0;
hlscl.SyncWriteSpe(ID, 2, Speed, ACC, Torque);
delay(2000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，加速至最高速度V=-60*0.732=-43.92rpm，并保持恒速运行，最大扭矩电流T=500*6.5=3250mA
Speed[0] = -60;
Speed[1] = -60;
hlscl.SyncWriteSpe(ID, 2, Speed, ACC, Torque);
delay(5000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，减速至速度0停止运行
Speed[0] = 0;
Speed[1] = 0;
hlscl.SyncWriteSpe(ID, 2, Speed, ACC, Torque);
delay(2000);
}

31
jxbeye/lib/FTServo/examples/HLSCL/WriteEle/WriteEle.ino Normal file
View File

@@ -0,0 +1,31 @@
#include <SCServo.h>
HLSCL hlscl;
void setup()
{
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
hlscl.EleMode(1);//舵机ID1切换至电机恒力模式
}
void loop()
{
//舵机(ID1)以最大扭矩电流T=300*6.5=1950mA，正向旋转
hlscl.WriteEle(1, 300);
delay(5000);
//舵机(ID1/ID2)以扭矩0停止旋转
hlscl.WriteEle(1, 0);
delay(2000);
//舵机(ID1)以最大扭矩电流T=300*6.5=1950mA，反向旋转
hlscl.WriteEle(1, -300);
delay(5000);
//舵机(ID1)以扭矩0停止旋转
hlscl.WriteEle(1, 0);
delay(2000);
}

22
jxbeye/lib/FTServo/examples/HLSCL/WritePos/WritePos.ino Normal file
View File

@@ -0,0 +1,22 @@
#include <SCServo.h>
HLSCL hlscl;
void setup()
{
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P1=4095位置
hlscl.WritePosEx(1, 4095, 60, 50, 500);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
//舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P0=0位置
hlscl.WritePosEx(1, 0, 60, 50, 500);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
}

31
jxbeye/lib/FTServo/examples/HLSCL/WriteSpe/WriteSpe.ino Normal file
View File

@@ -0,0 +1,31 @@
#include <SCServo.h>
HLSCL hlscl;
void setup()
{
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
Serial1.begin(1000000);//mega2560
hlscl.pSerial = &Serial1;
delay(1000);
hlscl.WheelMode(1);//舵机ID1切换至电机恒速模式
}
void loop()
{
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，加速至最高速度V=60*0.732=43.92rpm，并保持恒速正向旋转，最大扭矩电流T=500*6.5=3250mA
hlscl.WriteSpe(1, 60, 50, 500);
delay(5000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，减速至速度0停止旋转
hlscl.WriteSpe(1, 0, 50, 500);
delay(2000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，加速至最高速度V=-60*0.732=-43.92rpm，并保持恒速反向旋转，最大扭矩电流T=500*6.5=3250mA
hlscl.WriteSpe(1, -60, 50, 500);
delay(5000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，减速至速度0停止旋转
hlscl.WriteSpe(1, 0, 50, 500);
delay(2000);
}

25
jxbeye/lib/FTServo/examples/SCSCL/Broadcast/Broadcast.ino Normal file
View File

@@ -0,0 +1,25 @@
/*
广播写例子在SCS15中测试通过，如果测试其它型号SCS系列舵机请更改合适的位置、速度与延时参数。
*/
#include <SCServo.h>
SCSCL sc;
void setup()
{
Serial1.begin(1000000);
sc.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(广播)以最高速度V=1500*0.059=88.5rpm，运行至P1=1000位置
sc.WritePos(0xfe, 1000, 0, 1500);
delay((1000-20)*1000/(1500) + 100);//[(P1-P0)/(V)]*1000 + 100(误差)
//舵机(广播)以最高速度V=1500*0.059=88.5rpm，运行至P0=20位置
sc.WritePos(0xfe, 20, 0, 1500);
delay((1000-20)*1000/(1500) + 100);//[(P1-P0)/(V)]*1000 + 100(误差)
}

147
jxbeye/lib/FTServo/examples/SCSCL/FeedBack/FeedBack.ino Normal file
View File

@@ -0,0 +1,147 @@
/*
回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流；
FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态；
函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态,
无需调用FeedBack函数。
*/
#include <SCServo.h>
SCSCL sc;
int LEDpin = 13;
void setup()
{
pinMode(LEDpin,OUTPUT);
digitalWrite(LEDpin, HIGH);
Serial1.begin(1000000);
Serial.begin(115200);
sc.pSerial = &Serial1;
delay(1000);
}
void loop()
{
int Pos;
int Speed;
int Load;
int Voltage;
int Temper;
int Move;
int Current;
sc.FeedBack(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Pos = sc.ReadPos(-1);
Speed = sc.ReadSpeed(-1);
Load = sc.ReadLoad(-1);
Voltage = sc.ReadVoltage(-1);
Temper = sc.ReadTemper(-1);
Move = sc.ReadMove(-1);
Current = sc.ReadCurrent(-1);
Serial.print("Position:");
Serial.println(Pos);
Serial.print("Speed:");
Serial.println(Speed);
Serial.print("Load:");
Serial.println(Load);
Serial.print("Voltage:");
Serial.println(Voltage);
Serial.print("Temper:");
Serial.println(Temper);
Serial.print("Move:");
Serial.println(Move);
Serial.print("Current:");
Serial.println(Current);
delay(10);
}else{
digitalWrite(LEDpin, HIGH);
Serial.println("FeedBack err");
delay(500);
}
Pos = sc.ReadPos(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo position:");
Serial.println(Pos, DEC);
delay(10);
}else{
Serial.println("read position err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Voltage = sc.ReadVoltage(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Voltage:");
Serial.println(Voltage, DEC);
delay(10);
}else{
Serial.println("read Voltage err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Temper = sc.ReadTemper(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo temperature:");
Serial.println(Temper, DEC);
delay(10);
}else{
Serial.println("read temperature err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Speed = sc.ReadSpeed(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Speed:");
Serial.println(Speed, DEC);
delay(10);
}else{
Serial.println("read Speed err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Load = sc.ReadLoad(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Load:");
Serial.println(Load, DEC);
delay(10);
}else{
Serial.println("read Load err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Current = sc.ReadCurrent(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Current:");
Serial.println(Current, DEC);
delay(10);
}else{
Serial.println("read Current err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Move = sc.ReadMove(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Move:");
Serial.println(Move, DEC);
delay(10);
}else{
Serial.println("read Move err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Serial.println();
}

33
jxbeye/lib/FTServo/examples/SCSCL/Ping/Ping.ino Normal file
View File

@@ -0,0 +1,33 @@
/*
Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况
*/
#include <SCServo.h>
SCSCL sc;
int LEDpin = 13;
void setup()
{
pinMode(LEDpin,OUTPUT);
digitalWrite(LEDpin, HIGH);
Serial.begin(115200);
Serial1.begin(1000000);
sc.pSerial = &Serial1;
delay(1000);
}
void loop()
{
int ID = sc.Ping(1);
if(!sc.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo ID:");
Serial.println(ID, DEC);
delay(100);
}else{
Serial.println("Ping servo ID error!");
digitalWrite(LEDpin, HIGH);
delay(2000);
}
}

28
jxbeye/lib/FTServo/examples/SCSCL/ProgramEprom/ProgramEprom.ino Normal file
View File

@@ -0,0 +1,28 @@
/*
舵机参数编程
*/
#include <SCServo.h>
int LEDpin = 13;
SCSCL sc;
void setup()
{
pinMode(LEDpin, OUTPUT);
Serial1.begin(1000000);
sc.pSerial = &Serial1;
delay(1000);
digitalWrite(LEDpin, LOW);
sc.unLockEprom(1);//打开EPROM保存功能
sc.writeByte(1, SCSCL_ID, 2);//ID
sc.writeWord(2, SCSCL_MIN_ANGLE_LIMIT_L, 20);
sc.writeWord(2, SCSCL_MAX_ANGLE_LIMIT_L, 1000);
sc.LockEprom(2);////关闭EPROM保存功能
digitalWrite(LEDpin, HIGH);
}
void loop()
{
}

29
jxbeye/lib/FTServo/examples/SCSCL/RegWritePos/RegWritePos.ino Normal file
View File

@@ -0,0 +1,29 @@
/*
异步写例子在SCS15中测试通过，如果测试其它型号SCS系列舵机请更改合适的位置、速度与延时参数。
*/
#include <SCServo.h>
SCSCL sc;
void setup()
{
Serial1.begin(1000000);
sc.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(ID1/ID2)以最高速度V=1500*0.059=88.5rpm，运行至P1=1000位置
sc.RegWritePos(1, 1000, 0, 1500);
sc.RegWritePos(2, 1000, 0, 1500);
sc.RegWriteAction();
delay((1000-20)*1000/(1500) + 100);//[(P1-P0)/(V)]*1000 + 100(误差)
//舵机(ID1/ID2)以最高速度V=1500*0.059=88.5rpm，运行至P0=20位置
sc.RegWritePos(1, 20, 0, 1500);
sc.RegWritePos(2, 20, 0, 1500);
sc.RegWriteAction();
delay((1000-20)*1000/(1500) + 100);//[(P1-P0)/(V)]*1000 + 100(误差)
}

37
jxbeye/lib/FTServo/examples/SCSCL/SyncWritePos/SyncWritePos.ino Normal file
View File

@@ -0,0 +1,37 @@
/*
同步写例子在SCS15中测试通过，如果测试其它型号SCS系列舵机请更改合适的位置、速度与延时参数。
*/
#include <SCServo.h>
SCSCL sc;
byte ID[2];
u16 Position[2];
u16 Speed[2];
void setup()
{
Serial1.begin(1000000);
sc.pSerial = &Serial1;
delay(1000);
ID[0] = 1;//舵机ID1
ID[1] = 2;//舵机ID2
Speed[0] = 1500;//最高速度V=1500*0.059=88.5rpm
Speed[1] = 1500;//最高速度V=1500*0.059=88.5rpm
}
void loop()
{
//舵机(ID1/ID2)以最高速度V=1500*0.059=88.5rpm，运行至P1=1000位置
Position[0] = 1000;
Position[1] = 1000;
sc.SyncWritePos(ID, 2, Position, 0, Speed);
delay((1000-20)*1000/(1500) + 100);//[(P1-P0)/(V)]*1000 + 100(误差)
//舵机(ID1/ID2)以最高速度V=1500*0.059=88.5rpm，运行至P0=20位置
Position[0] = 20;
Position[1] = 20;
sc.SyncWritePos(ID, 2, Position, 0, Speed);
delay((1000-20)*1000/(1500) + 100);//[(P1-P0)/(V)]*1000 + 100(误差)
}

34
jxbeye/lib/FTServo/examples/SCSCL/WritePWM/WritePWM.ino Normal file
View File

@@ -0,0 +1,34 @@
/*
电机模式例子
*/
#include <SCServo.h>
SCSCL sc;
void setup()
{
Serial1.begin(1000000);
sc.pSerial = &Serial1;
delay(1000);
sc.PWMMode(1);//舵机切换到PWM开环调速度模式
}
void loop()
{
//舵机(ID1)以最大50%扭矩正向旋转
sc.WritePWM(1, 500);
delay(2000);
//舵机(ID1)停止旋转
sc.WritePWM(1, 0);
delay(2000);
//舵机(ID1)以最大50%扭矩反向旋转
sc.WritePWM(1, -500);
delay(2000);
//舵机(ID1)停止旋转
sc.WritePWM(1,0);
delay(2000);
}

25
jxbeye/lib/FTServo/examples/SCSCL/WritePos/WritePos.ino Normal file
View File

@@ -0,0 +1,25 @@
/*
普通写例子在SCS15中测试通过，如果测试其它型号SCS系列舵机请更改合适的位置、速度与延时参数。
*/
#include <SCServo.h>
SCSCL sc;
void setup()
{
Serial1.begin(1000000);
sc.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(ID1)以最高速度V=1500*0.059=88.5rpm，运行至P1=1000位置
sc.WritePos(1, 1000, 0, 1500);
delay((1000-20)*1000/(1500) + 100);//[(P1-P0)/(V)]*1000 + 100(误差)
//舵机(ID1)以最高速度V=1500*0.059=88.5rpm，运行至P0=20位置
sc.WritePos(1, 20, 0, 1500);
delay((1000-20)*1000/(1500) + 100);//[(P1-P0)/(V)]*1000 + 100(误差)
}

22
jxbeye/lib/FTServo/examples/SMS_STS/Broadcast/Broadcast.ino Normal file
View File

@@ -0,0 +1,22 @@
#include <SCServo.h>
SMS_STS sms_sts;
void setup()
{
//Serial1.begin(115200);//sms舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(广播)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置
sms_sts.WritePosEx(0xfe, 4095, 60, 50);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
//舵机(广播)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置
sms_sts.WritePosEx(0xfe, 0, 60, 50);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
}

25
jxbeye/lib/FTServo/examples/SMS_STS/CalibrationOfs/CalibrationOfs.ino Normal file
View File

@@ -0,0 +1,25 @@
/*
中位校准例子
*/
#include <SCServo.h>
int LEDpin = 13;
SMS_STS sm_st;
void setup()
{
pinMode(LEDpin, OUTPUT);
//Serial1.begin(115200);//sms舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sm_st.pSerial = &Serial1;
delay(1000);
}
void loop()
{
digitalWrite(LEDpin, LOW);
sm_st.CalibrationOfs(1);
digitalWrite(LEDpin, HIGH);
while(1);
}

148
jxbeye/lib/FTServo/examples/SMS_STS/FeedBack/FeedBack.ino Normal file
View File

@@ -0,0 +1,148 @@
/*
回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流；
FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态；
函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态,
无需调用FeedBack函数。
*/
#include <SCServo.h>
SMS_STS sms_sts;
int LEDpin = 13;
void setup()
{
pinMode(LEDpin,OUTPUT);
digitalWrite(LEDpin, HIGH);
//Serial1.begin(115200);//sms舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
Serial.begin(115200);
sms_sts.pSerial = &Serial1;
delay(1000);
}
void loop()
{
int Pos;
int Speed;
int Load;
int Voltage;
int Temper;
int Move;
int Current;
sms_sts.FeedBack(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Pos = sms_sts.ReadPos(-1);
Speed = sms_sts.ReadSpeed(-1);
Load = sms_sts.ReadLoad(-1);
Voltage = sms_sts.ReadVoltage(-1);
Temper = sms_sts.ReadTemper(-1);
Move = sms_sts.ReadMove(-1);
Current = sms_sts.ReadCurrent(-1);
Serial.print("Position:");
Serial.println(Pos);
Serial.print("Speed:");
Serial.println(Speed);
Serial.print("Load:");
Serial.println(Load);
Serial.print("Voltage:");
Serial.println(Voltage);
Serial.print("Temper:");
Serial.println(Temper);
Serial.print("Move:");
Serial.println(Move);
Serial.print("Current:");
Serial.println(Current);
delay(10);
}else{
digitalWrite(LEDpin, HIGH);
Serial.println("FeedBack err");
delay(500);
}
Pos = sms_sts.ReadPos(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo position:");
Serial.println(Pos, DEC);
delay(10);
}else{
Serial.println("read position err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Voltage = sms_sts.ReadVoltage(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Voltage:");
Serial.println(Voltage, DEC);
delay(10);
}else{
Serial.println("read Voltage err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Temper = sms_sts.ReadTemper(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo temperature:");
Serial.println(Temper, DEC);
delay(10);
}else{
Serial.println("read temperature err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Speed = sms_sts.ReadSpeed(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Speed:");
Serial.println(Speed, DEC);
delay(10);
}else{
Serial.println("read Speed err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Load = sms_sts.ReadLoad(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Load:");
Serial.println(Load, DEC);
delay(10);
}else{
Serial.println("read Load err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Current = sms_sts.ReadCurrent(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Current:");
Serial.println(Current, DEC);
delay(10);
}else{
Serial.println("read Current err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Move = sms_sts.ReadMove(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo Move:");
Serial.println(Move, DEC);
delay(10);
}else{
Serial.println("read Move err");
digitalWrite(LEDpin, HIGH);
delay(500);
}
Serial.println();
}

33
jxbeye/lib/FTServo/examples/SMS_STS/Ping/Ping.ino Normal file
View File

@@ -0,0 +1,33 @@
/*
Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况
*/
#include <SCServo.h>
SMS_STS sms_sts;
int LEDpin = 13;
void setup()
{
pinMode(LEDpin,OUTPUT);
digitalWrite(LEDpin, HIGH);
Serial.begin(115200);
Serial1.begin(1000000);
sms_sts.pSerial = &Serial1;
delay(1000);
}
void loop()
{
int ID = sms_sts.Ping(1);
if(!sms_sts.getLastError()){
digitalWrite(LEDpin, LOW);
Serial.print("Servo ID:");
Serial.println(ID, DEC);
delay(100);
}else{
Serial.println("Ping servo ID error!");
digitalWrite(LEDpin, HIGH);
delay(2000);
}
}

27
jxbeye/lib/FTServo/examples/SMS_STS/ProgramEprom/ProgramEprom.ino Normal file
View File

@@ -0,0 +1,27 @@
/*
舵机参数编程
*/
#include <SCServo.h>
int LEDpin = 13;
SMS_STS sms_sts;
void setup()
{
pinMode(LEDpin, OUTPUT);
//Serial1.begin(115200);//sms舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
delay(1000);
digitalWrite(LEDpin, LOW);
sms_sts.unLockEprom(1);//打开EPROM保存功能
sms_sts.writeByte(1, SMS_STS_ID, 2);//ID
sms_sts.LockEprom(2);//关闭EPROM保存功能
digitalWrite(LEDpin, HIGH);
}
void loop()
{
}

26
jxbeye/lib/FTServo/examples/SMS_STS/RegWritePos/RegWritePos.ino Normal file
View File

@@ -0,0 +1,26 @@
#include <SCServo.h>
SMS_STS sms_sts;
void setup()
{
//Serial1.begin(115200);//sms舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置
sms_sts.RegWritePosEx(1, 4095, 60, 50);
sms_sts.RegWritePosEx(2, 4095, 60, 50);
sms_sts.RegWriteAction();
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置
sms_sts.RegWritePosEx(1, 0, 60, 50);
sms_sts.RegWritePosEx(2, 0, 60, 50);
sms_sts.RegWriteAction();
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
}

45
jxbeye/lib/FTServo/examples/SMS_STS/SyncRead/SyncRead.ino Normal file
View File

@@ -0,0 +1,45 @@
/*
同步读指令，回读ID1与ID2两个舵机的位置与速度信息
*/
#include <SCServo.h>
SMS_STS sms_sts;
uint8_t ID[] = {1, 2};
uint8_t rxPacket[4];
int16_t Position;
int16_t Speed;
void setup()
{
Serial.begin(115200);
//Serial1.begin(115200);//sms舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
sms_sts.syncReadBegin(sizeof(ID), sizeof(rxPacket), 5);//10*10*2=200us<5ms
delay(1000);
}
void loop()
{
sms_sts.syncReadPacketTx(ID, sizeof(ID), SMS_STS_PRESENT_POSITION_L, sizeof(rxPacket));//同步读指令包发送
for(uint8_t i=0; i<sizeof(ID); i++){
//接收ID[i]同步读返回包
if(!sms_sts.syncReadPacketRx(ID[i], rxPacket)){
Serial.print("ID:");
Serial.println(ID[i]);
Serial.println("sync read error!");
continue;//接收解码失败
}
Position = sms_sts.syncReadRxPacketToWrod(15);//解码两个字节 bit15为方向位,参数=0表示无方向位
Speed = sms_sts.syncReadRxPacketToWrod(15);//解码两个字节 bit15为方向位,参数=0表示无方向位
Serial.print("ID:");
Serial.println(ID[i]);
Serial.print("Position:");
Serial.println(Position);
Serial.print("Speed:");
Serial.println(Speed);
}
delay(10);
}

36
jxbeye/lib/FTServo/examples/SMS_STS/SyncWritePos/SyncWritePos.ino Normal file
View File

@@ -0,0 +1,36 @@
#include <SCServo.h>
SMS_STS sms_sts;
byte ID[2];
s16 Position[2];
u16 Speed[2];
byte ACC[2];
void setup()
{
//Serial1.begin(115200);//sms_stss舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
delay(1000);
ID[0] = 1;//舵机ID1
ID[1] = 2;//舵机ID2
Speed[0] = 60;//最高速度V=60*0.732=43.92rpm
Speed[1] = 60;//最高速度V=60*0.732=43.92rpm
ACC[0] = 50;//加速度A=50*8.7deg/s^2
ACC[1] = 50;//加速度A=50*8.7deg/s^2
}
void loop()
{
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置
Position[0] = 4095;
Position[1] = 4095;
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置
Position[0] = 0;
Position[1] = 0;
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
}

47
jxbeye/lib/FTServo/examples/SMS_STS/SyncWriteSpe/SyncWriteSpe.ino Normal file
View File

@@ -0,0 +1,47 @@
#include <SCServo.h>
SMS_STS sms_sts;
byte ID[2];
s16 Speed[2];
byte ACC[2];
void setup()
{
//Serial1.begin(115200);//sms_stss舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
delay(1000);
sms_sts.WheelMode(1);//舵机ID1切换至恒速模式
sms_sts.WheelMode(2);//舵机ID2切换至恒速模式
ID[0] = 1;//舵机ID1
ID[1] = 2;//舵机ID2
ACC[0] = 50;//加速度A=50*8.7deg/s^2
ACC[1] = 50;//加速度A=50*8.7deg/s^2
}
void loop()
{
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，加速至最高速度V=60*0.732=43.92rpm，并保持恒速正向旋转
Speed[0] = 60;
Speed[1] = 60;
sms_sts.SyncWriteSpe(ID, 2, Speed, ACC);
delay(5000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，减速至速度0停止旋转
Speed[0] = 0;
Speed[1] = 0;
sms_sts.SyncWriteSpe(ID, 2, Speed, ACC);
delay(2000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，加速至最高速度V=-60*0.732=-43.92rpm，并保持恒速反向旋转
Speed[0] = -60;
Speed[1] = -60;
sms_sts.SyncWriteSpe(ID, 2, Speed, ACC);
delay(5000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，减速至速度0停止运行
Speed[0] = 0;
Speed[1] = 0;
sms_sts.SyncWriteSpe(ID, 2, Speed, ACC);
delay(2000);
}

22
jxbeye/lib/FTServo/examples/SMS_STS/WritePos/WritePos.ino Normal file
View File

@@ -0,0 +1,22 @@
#include <SCServo.h>
SMS_STS sms_sts;
void setup()
{
//Serial1.begin(115200);//sms舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
delay(1000);
}
void loop()
{
//舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置
sms_sts.WritePosEx(1, 4095, 2400, 50);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
//舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置
sms_sts.WritePosEx(1, 0, 2400, 50);
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
}

31
jxbeye/lib/FTServo/examples/SMS_STS/WriteSpe/WriteSpe.ino Normal file
View File

@@ -0,0 +1,31 @@
#include <SCServo.h>
SMS_STS sms_sts;
void setup()
{
//Serial1.begin(115200);//sms舵机波特率115200
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
delay(1000);
sms_sts.WheelMode(1);//舵机ID1切换至电机恒速模式
}
void loop()
{
//舵机(ID1)以加速度A=50*8.7deg/s^2，加速至最高速度V=60*0.732=43.92rpm，并保持恒速正向旋转
sms_sts.WriteSpe(1, 60, 50);
delay(5000);
//舵机(ID1)以加速度A=50*8.7deg/s^2，减速至速度0停止旋转
sms_sts.WriteSpe(1, 0, 50);
delay(2000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，加速至最高速度V=-60*0.732=-43.92rpm，并保持恒速反向旋转
sms_sts.WriteSpe(1, -60, 50);
delay(5000);
//舵机(ID1/ID2)以加速度A=50*8.7deg/s^2，减速至速度0停止旋转
sms_sts.WriteSpe(1, 0, 50);
delay(2000);
}

11
jxbeye/lib/FTServo/library.properties Normal file
View File

@@ -0,0 +1,11 @@
# see https://arduino.github.io/arduino-cli/1.0/library-specification/#library-metadata
name=FTServo
version=2.0.0
author=ftservo
maintainer=FEETECH <github.com/ftservo>
sentence=FEETECH BUS Servo library for Arduino and ESP32
paragraph=This library is compatible with all series of FEETECH BUS Servo
category=Other
url=https://github.com/ftservo/FTServo_Arduino
architectures=*

260
jxbeye/lib/FTServo/src/HLSCL.cpp Normal file
View File

@@ -0,0 +1,260 @@
/*
* HLSCL.cpp
* 飞特HTS/HLS系列串行舵机应用层程序
* 日期: 2024.11.21
* 作者: txl
*/
#include "HLSCL.h"
HLSCL::HLSCL()
{
End = 0;
}
HLSCL::HLSCL(u8 End):SCSerial(End)
{
}
HLSCL::HLSCL(u8 End, u8 Level):SCSerial(End, Level)
{
}
int HLSCL::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC, u16 Torque)
{
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, Torque);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, HLSCL_ACC, bBuf, 7);
}
int HLSCL::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC, u16 Torque)
{
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, Torque);
Host2SCS(bBuf+5, bBuf+6, Speed);
return regWrite(ID, HLSCL_ACC, bBuf, 7);
}
void HLSCL::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[], u16 Torque[])
{
u8 offbuf[7*IDN];
for(u8 i = 0; i<IDN; i++){
if(Position[i]<0){
Position[i] = -Position[i];
Position[i] |= (1<<15);
}
if(ACC){
offbuf[i*7] = ACC[i];
}else{
offbuf[i*7] = 0;
}
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, Position[i]);
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, Torque[i]);
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
}
syncWrite(ID, IDN, HLSCL_ACC, offbuf, 7);
}
void HLSCL::SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[], u16 Torque[])
{
u8 offbuf[7*IDN];
for(u8 i = 0; i<IDN; i++){
if(Speed[i]<0){
Speed[i] = -Speed[i];
Speed[i] |= (1<<15);
}
if(ACC){
offbuf[i*7] = ACC[i];
}else{
offbuf[i*7] = 0;
}
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, 0);
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, Torque[i]);
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
}
syncWrite(ID, IDN, HLSCL_ACC, offbuf, 7);
}
int HLSCL::WheelMode(u8 ID)
{
return writeByte(ID, HLSCL_MODE, 1);
}
int HLSCL::EleMode(u8 ID)
{
return writeByte(ID, HLSCL_MODE, 2);
}
int HLSCL::WriteSpe(u8 ID, s16 Speed, u8 ACC, u16 Torque)
{
if(Speed<0){
Speed = -Speed;
Speed |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, 0);
Host2SCS(bBuf+3, bBuf+4, Torque);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, HLSCL_ACC, bBuf, 7);
}
int HLSCL::WriteEle(u8 ID, s16 Torque)
{
if(Torque<0){
Torque = -Torque;
Torque |= (1<<15);
}
return writeWord(ID, HLSCL_GOAL_TORQUE_L, Torque);
}
int HLSCL::EnableTorque(u8 ID, u8 Enable)
{
return writeByte(ID, HLSCL_TORQUE_ENABLE, Enable);
}
int HLSCL::unLockEprom(u8 ID)
{
EnableTorque(ID, 0);
return writeByte(ID, HLSCL_LOCK, 0);
}
int HLSCL::LockEprom(u8 ID)
{
return writeByte(ID, HLSCL_LOCK, 1);
}
int HLSCL::CalibrationOfs(u8 ID)
{
EnableTorque(ID, 0);
unLockEprom(ID);
return Recal(ID);
}
int HLSCL::FeedBack(int ID)
{
int nLen = Read(ID, HLSCL_PRESENT_POSITION_L, Mem, sizeof(Mem));
if(nLen!=sizeof(Mem)){
return -1;
}
return nLen;
}
int HLSCL::ReadPos(int ID)
{
int Pos = -1;
if(ID==-1){
Pos = Mem[HLSCL_PRESENT_POSITION_H-HLSCL_PRESENT_POSITION_L];
Pos <<= 8;
Pos |= Mem[HLSCL_PRESENT_POSITION_L-HLSCL_PRESENT_POSITION_L];
}else{
Pos = readWord(ID, HLSCL_PRESENT_POSITION_L);
}
if(Pos&(1<<15)){
Pos = -(Pos&~(1<<15));
}
return Pos;
}
int HLSCL::ReadSpeed(int ID)
{
int Speed = -1;
if(ID==-1){
Speed = Mem[HLSCL_PRESENT_SPEED_H-HLSCL_PRESENT_POSITION_L];
Speed <<= 8;
Speed |= Mem[HLSCL_PRESENT_SPEED_L-HLSCL_PRESENT_POSITION_L];
}else{
Speed = readWord(ID, HLSCL_PRESENT_SPEED_L);
}
if(Speed&(1<<15)){
Speed = -(Speed&~(1<<15));
}
return Speed;
}
int HLSCL::ReadLoad(int ID)
{
int Load = -1;
if(ID==-1){
Load = Mem[HLSCL_PRESENT_LOAD_H-HLSCL_PRESENT_POSITION_L];
Load <<= 8;
Load |= Mem[HLSCL_PRESENT_LOAD_L-HLSCL_PRESENT_POSITION_L];
}else{
Load = readWord(ID, HLSCL_PRESENT_LOAD_L);
}
if(Load&(1<<10)){
Load = -(Load&~(1<<10));
}
return Load;
}
int HLSCL::ReadVoltage(int ID)
{
int Voltage = -1;
if(ID==-1){
Voltage = Mem[HLSCL_PRESENT_VOLTAGE-HLSCL_PRESENT_POSITION_L];
}else{
Voltage = readByte(ID, HLSCL_PRESENT_VOLTAGE);
}
return Voltage;
}
int HLSCL::ReadTemper(int ID)
{
int Temper = -1;
if(ID==-1){
Temper = Mem[HLSCL_PRESENT_TEMPERATURE-HLSCL_PRESENT_POSITION_L];
}else{
Temper = readByte(ID, HLSCL_PRESENT_TEMPERATURE);
}
return Temper;
}
int HLSCL::ReadMove(int ID)
{
int Move = -1;
if(ID==-1){
Move = Mem[HLSCL_MOVING-HLSCL_PRESENT_POSITION_L];
}else{
Move = readByte(ID, HLSCL_MOVING);
}
return Move;
}
int HLSCL::ReadCurrent(int ID)
{
int Current = -1;
if(ID==-1){
Current = Mem[HLSCL_PRESENT_CURRENT_H-HLSCL_PRESENT_POSITION_L];
Current <<= 8;
Current |= Mem[HLSCL_PRESENT_CURRENT_L-HLSCL_PRESENT_POSITION_L];
}else{
Current = readWord(ID, HLSCL_PRESENT_CURRENT_L);
}
if(Current&(1<<15)){
Current = -(Current&~(1<<15));
}
return Current;
}
int HLSCL::ServoMode(u8 ID)
{
return writeByte(ID, HLSCL_MODE, 0);
}

89
jxbeye/lib/FTServo/src/HLSCL.h Normal file
View File

@@ -0,0 +1,89 @@
/*
* HLSCL.h
* 飞特HLS系列串行舵机应用层程序
* 日期: 2024.11.21
* 作者: txl
*/
#ifndef _HLSCL_H
#define _HLSCL_H
//内存表定义
//-------EPROM(只读)--------
#define HLSCL_MODEL_L 3
#define HLSCL_MODEL_H 4
//-------EPROM(读写)--------
#define HLSCL_ID 5
#define HLSCL_BAUD_RATE 6
#define HLSCL_SECOND_ID 7
#define HLSCL_MIN_ANGLE_LIMIT_L 9
#define HLSCL_MIN_ANGLE_LIMIT_H 10
#define HLSCL_MAX_ANGLE_LIMIT_L 11
#define HLSCL_MAX_ANGLE_LIMIT_H 12
#define HLSCL_CW_DEAD 26
#define HLSCL_CCW_DEAD 27
#define HLSCL_OFS_L 31
#define HLSCL_OFS_H 32
#define HLSCL_MODE 33
//-------SRAM(读写)--------
#define HLSCL_TORQUE_ENABLE 40
#define HLSCL_ACC 41
#define HLSCL_GOAL_POSITION_L 42
#define HLSCL_GOAL_POSITION_H 43
#define HLSCL_GOAL_TORQUE_L 44
#define HLSCL_GOAL_TORQUE_H 45
#define HLSCL_GOAL_SPEED_L 46
#define HLSCL_GOAL_SPEED_H 47
#define HLSCL_TORQUE_LIMIT_L 48
#define HLSCL_TORQUE_LIMIT_H 49
#define HLSCL_LOCK 55
//-------SRAM(只读)--------
#define HLSCL_PRESENT_POSITION_L 56
#define HLSCL_PRESENT_POSITION_H 57
#define HLSCL_PRESENT_SPEED_L 58
#define HLSCL_PRESENT_SPEED_H 59
#define HLSCL_PRESENT_LOAD_L 60
#define HLSCL_PRESENT_LOAD_H 61
#define HLSCL_PRESENT_VOLTAGE 62
#define HLSCL_PRESENT_TEMPERATURE 63
#define HLSCL_MOVING 66
#define HLSCL_PRESENT_CURRENT_L 69
#define HLSCL_PRESENT_CURRENT_H 70
#include "SCSerial.h"
class HLSCL : public SCSerial
{
public:
HLSCL();
HLSCL(u8 End);
HLSCL(u8 End, u8 Level);
int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0, u16 Torque = 0);//普通写单个舵机位置指令
int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0, u16 Torque = 0);//异步写单个舵机位置指令(RegWriteAction生效)
void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[], u16 Torque[]);//同步写多个舵机位置指令
void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[], u16 Torque[]);//同步写多个舵机速度指令
int ServoMode(u8 ID);//Servo模式
int WheelMode(u8 ID);//恒速模式
int EleMode(u8 ID);//恒力模式
int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0, u16 Torque = 0);//恒速模式控制指令
int WriteEle(u8 ID, s16 Torque);//恒力模式控制指令
int EnableTorque(u8 ID, u8 Enable);//扭力控制指令
int unLockEprom(u8 ID);//eprom解锁
int LockEprom(u8 ID);//eprom加锁
int CalibrationOfs(u8 ID);//中位校准
int FeedBack(int ID);//反馈舵机信息
int ReadPos(int ID);//读位置
int ReadSpeed(int ID);//读速度
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
int ReadVoltage(int ID);//读电压
int ReadTemper(int ID);//读温度
int ReadMove(int ID);//读移动状态
int ReadCurrent(int ID);//读电流
private:
u8 Mem[HLSCL_PRESENT_CURRENT_H-HLSCL_PRESENT_POSITION_L+1];
};
#endif

51
jxbeye/lib/FTServo/src/INST.h Normal file
View File

@@ -0,0 +1,51 @@
/*
* INST.h
* 飞特串行舵机协议指令定义
* 日期: 2024.11.24
* 作者: txl
*/
#ifndef _INST_H
#define _INST_H
typedef char s8;
typedef unsigned char u8;
typedef unsigned short u16;
typedef short s16;
typedef unsigned long u32;
typedef long s32;
enum SCS_ERR_LIST
{
ERR_NO_REPLY = 1,
ERR_CRC_CMP = 2,
ERR_SLAVE_ID = 3,
ERR_BUFF_LEN = 4,
};
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_REG_ACTION 0x05
#define INST_SYNC_READ 0x82
#define INST_SYNC_WRITE 0x83
#define INST_RECOVERY 0x06
#define INST_RESET 0x0A
#define INST_CAL 0x0B
//波特率定义
#define _1M 0
#define _0_5M 1
#define _250K 2
#define _128K 3
#define _115200 4
#define _76800 5
#define _57600 6
#define _38400 7
#define _19200 8
#define _14400 9
#define _9600 10
#define _4800 11
#endif

450
jxbeye/lib/FTServo/src/SCS.cpp Normal file
View File

@@ -0,0 +1,450 @@
/*
* SCS.cpp
* 飞特串行舵机通信层协议程序
* 日期: 2024.12.21
* 作者: txl
*/
#include <stddef.h>
#include "SCS.h"
SCS::SCS()
{
Level = 1;//除广播指令所有指令返回应答
u8Status = 0;
}
SCS::SCS(u8 End)
{
Level = 1;
this->End = End;
u8Status = 0;
}
SCS::SCS(u8 End, u8 Level)
{
this->Level = Level;
this->End = End;
u8Status = 0;
}
//1个16位数拆分为2个8位数
//DataL为低位，DataH为高位
void SCS::Host2SCS(u8 *DataL, u8* DataH, u16 Data)
{
if(End){
*DataL = (Data>>8);
*DataH = (Data&0xff);
}else{
*DataH = (Data>>8);
*DataL = (Data&0xff);
}
}
//2个8位数组合为1个16位数
//DataL为低位，DataH为高位
u16 SCS::SCS2Host(u8 DataL, u8 DataH)
{
u16 Data;
if(End){
Data = DataL;
Data<<=8;
Data |= DataH;
}else{
Data = DataH;
Data<<=8;
Data |= DataL;
}
return Data;
}
void SCS::writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun)
{
u8 msgLen = 2;
u8 bBuf[6];
u8 CheckSum = 0;
bBuf[0] = 0xff;
bBuf[1] = 0xff;
bBuf[2] = ID;
bBuf[4] = Fun;
if(nDat){
msgLen += nLen + 1;
bBuf[3] = msgLen;
bBuf[5] = MemAddr;
writeSCS(bBuf, 6);
}else{
bBuf[3] = msgLen;
writeSCS(bBuf, 5);
}
CheckSum = ID + msgLen + Fun + MemAddr;
u8 i = 0;
if(nDat){
for(i=0; i<nLen; i++){
CheckSum += nDat[i];
}
writeSCS(nDat, nLen);
}
writeSCS(~CheckSum);
}
//普通写指令
//舵机ID，MemAddr内存表地址，写入数据，写入长度
int SCS::genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
//异步写指令
//舵机ID，MemAddr内存表地址，写入数据，写入长度
int SCS::regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, nDat, nLen, INST_REG_WRITE);
wFlushSCS();
return Ack(ID);
}
//异步写执行指令
//舵机ID
int SCS::RegWriteAction(u8 ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_REG_ACTION);
wFlushSCS();
return Ack(ID);
}
//同步写指令
//舵机ID[]数组，IDN数组长度，MemAddr内存表地址，写入数据，写入长度
void SCS::syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen)
{
rFlushSCS();
u8 mesLen = ((nLen+1)*IDN+4);
u8 Sum = 0;
u8 bBuf[7];
bBuf[0] = 0xff;
bBuf[1] = 0xff;
bBuf[2] = 0xfe;
bBuf[3] = mesLen;
bBuf[4] = INST_SYNC_WRITE;
bBuf[5] = MemAddr;
bBuf[6] = nLen;
writeSCS(bBuf, 7);
Sum = 0xfe + mesLen + INST_SYNC_WRITE + MemAddr + nLen;
u8 i, j;
for(i=0; i<IDN; i++){
writeSCS(ID[i]);
writeSCS(nDat+i*nLen, nLen);
Sum += ID[i];
for(j=0; j<nLen; j++){
Sum += nDat[i*nLen+j];
}
}
writeSCS(~Sum);
wFlushSCS();
}
int SCS::writeByte(u8 ID, u8 MemAddr, u8 bDat)
{
rFlushSCS();
writeBuf(ID, MemAddr, &bDat, 1, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
int SCS::writeWord(u8 ID, u8 MemAddr, u16 wDat)
{
u8 bBuf[2];
Host2SCS(bBuf+0, bBuf+1, wDat);
rFlushSCS();
writeBuf(ID, MemAddr, bBuf, 2, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
//读指令
//舵机ID，MemAddr内存表地址，返回数据nData，数据长度nLen
int SCS::Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
wFlushSCS();
u8Error = 0;
if(!checkHead()){
u8Error = ERR_NO_REPLY;
return 0;
}
u8 bBuf[4];
u8Status = 0;
if(readSCS(bBuf, 3)!=3){
u8Error = ERR_NO_REPLY;
return 0;
}
if(bBuf[0]!=ID && ID!=0xfe){
u8Error = ERR_SLAVE_ID;
return 0;
}
if(bBuf[1]!=(nLen+2)){
u8Error = ERR_BUFF_LEN;
return 0;
}
int Size = readSCS(nData, nLen);
if(Size!=nLen){
u8Error = ERR_NO_REPLY;
return 0;
}
if(readSCS(bBuf+3, 1)!=1){
u8Error = ERR_NO_REPLY;
return 0;
}
u8 calSum = bBuf[0]+bBuf[1]+bBuf[2];
u8 i;
for(i=0; i<Size; i++){
calSum += nData[i];
}
calSum = ~calSum;
if(calSum!=bBuf[3]){
u8Error = ERR_CRC_CMP;
return 0;
}
u8Status = bBuf[2];
return Size;
}
//读1字节，超时返回-1
int SCS::readByte(u8 ID, u8 MemAddr)
{
u8 bDat;
int Size = Read(ID, MemAddr, &bDat, 1);
if(Size!=1){
return -1;
}else{
return bDat;
}
}
//读2字节，超时返回-1
int SCS::readWord(u8 ID, u8 MemAddr)
{
u8 nDat[2];
int Size;
u16 wDat;
Size = Read(ID, MemAddr, nDat, 2);
if(Size!=2)
return -1;
wDat = SCS2Host(nDat[0], nDat[1]);
return wDat;
}
//Ping指令，返回舵机ID，超时返回-1
int SCS::Ping(u8 ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_PING);
wFlushSCS();
u8Status = 0;
if(!checkHead()){
u8Error = ERR_NO_REPLY;
return -1;
}
u8 bBuf[4];
u8Error = 0;
if(readSCS(bBuf, 4)!=4){
u8Error = ERR_NO_REPLY;
return -1;
}
if(bBuf[0]!=ID && ID!=0xfe){
u8Error = ERR_SLAVE_ID;
return -1;
}
if(bBuf[1]!=2){
u8Error = ERR_BUFF_LEN;
return -1;
}
u8 calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
if(calSum!=bBuf[3]){
u8Error = ERR_CRC_CMP;
return -1;
}
u8Status = bBuf[2];
return bBuf[0];
}
int SCS::checkHead()
{
u8 bDat;
u8 bBuf[] = {0, 0};
u8 Cnt = 0;
while(1){
if(!readSCS(&bDat, 1)){
return 0;
}
bBuf[1] = bBuf[0];
bBuf[0] = bDat;
if(bBuf[0]==0xff && bBuf[1]==0xff){
break;
}
Cnt++;
if(Cnt>10){
return 0;
}
}
return 1;
}
int SCS::Ack(u8 ID)
{
u8Error = 0;
if(ID!=0xfe && Level){
if(!checkHead()){
u8Error = ERR_NO_REPLY;
return 0;
}
u8Status = 0;
u8 bBuf[4];
if(readSCS(bBuf, 4)!=4){
u8Error = ERR_NO_REPLY;
return 0;
}
if(bBuf[0]!=ID){
u8Error = ERR_SLAVE_ID;
return 0;
}
if(bBuf[1]!=2){
u8Error = ERR_BUFF_LEN;
return 0;
}
u8 calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
if(calSum!=bBuf[3]){
u8Error = ERR_CRC_CMP;
return 0;
}
u8Status = bBuf[2];
}
return 1;
}
int SCS::syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen)
{
rFlushSCS();
syncReadRxPacketLen = nLen;
u8 checkSum = (4+0xfe) + IDN + MemAddr + nLen + INST_SYNC_READ;
u8 i;
writeSCS(0xff);
writeSCS(0xff);
writeSCS(0xfe);
writeSCS(IDN+4);
writeSCS(INST_SYNC_READ);
writeSCS(MemAddr);
writeSCS(nLen);
for(i=0; i<IDN; i++){
writeSCS(ID[i]);
checkSum += ID[i];
}
checkSum = ~checkSum;
writeSCS(checkSum);
wFlushSCS();
syncReadRxBuffLen = readSCS(syncReadRxBuff, syncReadRxBuffMax, syncTimeOut);
return syncReadRxBuffLen;
}
void SCS::syncReadBegin(u8 IDN, u8 rxLen, u32 TimeOut)
{
syncReadRxBuffMax = IDN*(rxLen+6);
syncReadRxBuff = new u8[syncReadRxBuffMax];
syncTimeOut = TimeOut;
}
void SCS::syncReadEnd()
{
if(syncReadRxBuff){
delete syncReadRxBuff;
syncReadRxBuff = NULL;
}
}
int SCS::syncReadPacketRx(u8 ID, u8 *nDat)
{
u16 syncReadRxBuffIndex = 0;
syncReadRxPacket = nDat;
syncReadRxPacketIndex = 0;
u8Error = 0;
while((syncReadRxBuffIndex+6+syncReadRxPacketLen)<=syncReadRxBuffLen){
u8 bBuf[] = {0, 0, 0};
u8 calSum = 0;
while(syncReadRxBuffIndex<syncReadRxBuffLen){
bBuf[0] = bBuf[1];
bBuf[1] = bBuf[2];
bBuf[2] = syncReadRxBuff[syncReadRxBuffIndex++];
if(bBuf[0]==0xff && bBuf[1]==0xff && bBuf[2]!=0xff){
break;
}
}
if(bBuf[2]!=ID){
continue;
}
if(syncReadRxBuff[syncReadRxBuffIndex++]!=(syncReadRxPacketLen+2)){
continue;
}
u8Status = syncReadRxBuff[syncReadRxBuffIndex++];
calSum = ID + (syncReadRxPacketLen+2) + u8Status;
for(u8 i=0; i<syncReadRxPacketLen; i++){
syncReadRxPacket[i] = syncReadRxBuff[syncReadRxBuffIndex++];
calSum += syncReadRxPacket[i];
}
calSum = ~calSum;
if(calSum!=syncReadRxBuff[syncReadRxBuffIndex++]){
u8Error = ERR_CRC_CMP;
return 0;
}
return syncReadRxPacketLen;
}
return 0;
}
int SCS::syncReadRxPacketToByte()
{
if(syncReadRxPacketIndex>=syncReadRxPacketLen){
u8Error = ERR_BUFF_LEN;
return -1;
}
return syncReadRxPacket[syncReadRxPacketIndex++];
}
int SCS::syncReadRxPacketToWrod(u8 negBit)
{
if((syncReadRxPacketIndex+1)>=syncReadRxPacketLen){
u8Error = ERR_BUFF_LEN;
return -1;
}
int Word = SCS2Host(syncReadRxPacket[syncReadRxPacketIndex], syncReadRxPacket[syncReadRxPacketIndex+1]);
syncReadRxPacketIndex += 2;
if(negBit){
if(Word&(1<<negBit)){
Word = -(Word & ~(1<<negBit));
}
}
return Word;
}
int SCS::Reset(u8 ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_RESET);
wFlushSCS();
return Ack(ID);
}
int SCS::Recal(u8 ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_CAL);
wFlushSCS();
return Ack(ID);
}

64
jxbeye/lib/FTServo/src/SCS.h Normal file
View File

@@ -0,0 +1,64 @@
/*
* SCS.h
* 飞特串行舵机通信层协议程序
* 日期: 2024.11.21
* 作者: txl
*/
#ifndef _SCS_H
#define _SCS_H
#include "INST.h"
class SCS{
public:
SCS();
SCS(u8 End);
SCS(u8 End, u8 Level);
int genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//普通写指令
int regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//异步写指令
int RegWriteAction(u8 ID = 0xfe);//异步写执行指令
void syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen);//同步写指令
int writeByte(u8 ID, u8 MemAddr, u8 bDat);//写1个字节
int writeWord(u8 ID, u8 MemAddr, u16 wDat);//写2个字节
int Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen);//读指令
int readByte(u8 ID, u8 MemAddr);//读1个字节
int readWord(u8 ID, u8 MemAddr);//读2个字节
int Ping(u8 ID);//Ping指令
int syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen);//同步读指令包发送
int syncReadPacketRx(u8 ID, u8 *nDat);//同步读返回包解码，成功返回内存字节数，失败返回0
int syncReadRxPacketToByte();//解码一个字节
int syncReadRxPacketToWrod(u8 negBit=0);//解码两个字节，negBit为方向为，negBit=0表示无方向
void syncReadBegin(u8 IDN, u8 rxLen, u32 TimeOut);//同步读开始
void syncReadEnd();//同步读结束
int Reset(u8 ID);//重置舵机状态
int Recal(u8 ID);//重置舵机中位
u8 getState() { return u8Status; }
u8 getLastError() { return u8Error; }
public:
u8 Level;//舵机返回等级
u8 End;//处理器大小端结构
u8 u8Status;//舵机状态
u8 u8Error;//通信状态
u8 syncReadRxPacketIndex;
u8 syncReadRxPacketLen;
u8 *syncReadRxPacket;
u8 *syncReadRxBuff;
u16 syncReadRxBuffLen;
u16 syncReadRxBuffMax;
u32 syncTimeOut;
protected:
virtual int writeSCS(unsigned char *nDat, int nLen) = 0;
virtual int readSCS(unsigned char *nDat, int nLen) = 0;
virtual int readSCS(unsigned char *nDat, int nLen, unsigned long TimeOut) = 0;
virtual int writeSCS(unsigned char bDat) = 0;
virtual void rFlushSCS() = 0;
virtual void wFlushSCS() = 0;
protected:
void writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun);
void Host2SCS(u8 *DataL, u8* DataH, u16 Data);//1个16位数拆分为2个8位数
u16 SCS2Host(u8 DataL, u8 DataH);//2个8位数组合为1个16位数
int Ack(u8 ID);//返回应答
int checkHead();//帧头检测
};
#endif

203
jxbeye/lib/FTServo/src/SCSCL.cpp Normal file
View File

@@ -0,0 +1,203 @@
/*
* SCSCL.cpp
* 飞特SCSCL系列串行舵机应用层程序
* 日期: 2024.4.2
* 作者:
*/
#include "SCSCL.h"
SCSCL::SCSCL()
{
End = 1;
}
SCSCL::SCSCL(u8 End):SCSerial(End)
{
}
SCSCL::SCSCL(u8 End, u8 Level):SCSerial(End, Level)
{
}
int SCSCL::WritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
{
u8 bBuf[6];
Host2SCS(bBuf+0, bBuf+1, Position);
Host2SCS(bBuf+2, bBuf+3, Time);
Host2SCS(bBuf+4, bBuf+5, Speed);
return genWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6);
}
int SCSCL::RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
{
u8 bBuf[6];
Host2SCS(bBuf+0, bBuf+1, Position);
Host2SCS(bBuf+2, bBuf+3, Time);
Host2SCS(bBuf+4, bBuf+5, Speed);
return regWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6);
}
void SCSCL::SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[])
{
u8 offbuf[6*IDN];
for(u8 i = 0; i<IDN; i++){
u16 T, V;
if(Time){
T = Time[i];
}else{
T = 0;
}
if(Speed){
V = Speed[i];
}else{
V = 0;
}
Host2SCS(offbuf+i*6+0, offbuf+i*6+1, Position[i]);
Host2SCS(offbuf+i*6+2, offbuf+i*6+3, T);
Host2SCS(offbuf+i*6+4, offbuf+i*6+5, V);
}
syncWrite(ID, IDN, SCSCL_GOAL_POSITION_L, offbuf, 6);
}
int SCSCL::EnableTorque(u8 ID, u8 Enable)
{
return writeByte(ID, SCSCL_TORQUE_ENABLE, Enable);
}
int SCSCL::unLockEprom(u8 ID)
{
return writeByte(ID, SCSCL_LOCK, 0);
}
int SCSCL::LockEprom(u8 ID)
{
return writeByte(ID, SCSCL_LOCK, 1);
}
int SCSCL::FeedBack(int ID)
{
int nLen = Read(ID, SCSCL_PRESENT_POSITION_L, Mem, sizeof(Mem));
if(nLen!=sizeof(Mem)){
return -1;
}
return nLen;
}
int SCSCL::ReadPos(int ID)
{
int Pos = -1;
if(ID==-1){
Pos = Mem[SCSCL_PRESENT_POSITION_L-SCSCL_PRESENT_POSITION_L];
Pos <<= 8;
Pos |= Mem[SCSCL_PRESENT_POSITION_H-SCSCL_PRESENT_POSITION_L];
}else{
Pos = readWord(ID, SCSCL_PRESENT_POSITION_L);
}
return Pos;
}
int SCSCL::ReadSpeed(int ID)
{
int Speed = -1;
if(ID==-1){
Speed = Mem[SCSCL_PRESENT_SPEED_L-SCSCL_PRESENT_POSITION_L];
Speed <<= 8;
Speed |= Mem[SCSCL_PRESENT_SPEED_H-SCSCL_PRESENT_POSITION_L];
}else{
Speed = readWord(ID, SCSCL_PRESENT_SPEED_L);
}
if(Speed&(1<<15)){
Speed = -(Speed&~(1<<15));
}
return Speed;
}
int SCSCL::ReadLoad(int ID)
{
int Load = -1;
if(ID==-1){
Load = Mem[SCSCL_PRESENT_LOAD_L-SCSCL_PRESENT_POSITION_L];
Load <<= 8;
Load |= Mem[SCSCL_PRESENT_LOAD_H-SCSCL_PRESENT_POSITION_L];
}else{
Load = readWord(ID, SCSCL_PRESENT_LOAD_L);
}
if(Load&(1<<10)){
Load = -(Load&~(1<<10));
}
return Load;
}
int SCSCL::ReadVoltage(int ID)
{
int Voltage = -1;
if(ID==-1){
Voltage = Mem[SCSCL_PRESENT_VOLTAGE-SCSCL_PRESENT_POSITION_L];
}else{
Voltage = readByte(ID, SCSCL_PRESENT_VOLTAGE);
}
return Voltage;
}
int SCSCL::ReadTemper(int ID)
{
int Temper = -1;
if(ID==-1){
Temper = Mem[SCSCL_PRESENT_TEMPERATURE-SCSCL_PRESENT_POSITION_L];
}else{
Temper = readByte(ID, SCSCL_PRESENT_TEMPERATURE);
}
return Temper;
}
int SCSCL::ReadMove(int ID)
{
int Move = -1;
if(ID==-1){
Move = Mem[SCSCL_MOVING-SCSCL_PRESENT_POSITION_L];
}else{
Move = readByte(ID, SCSCL_MOVING);
}
return Move;
}
int SCSCL::ReadCurrent(int ID)
{
int Current = -1;
if(ID==-1){
Current = Mem[SCSCL_PRESENT_CURRENT_L-SCSCL_PRESENT_POSITION_L];
Current <<= 8;
Current |= Mem[SCSCL_PRESENT_CURRENT_H-SCSCL_PRESENT_POSITION_L];
}else{
Current = readWord(ID, SCSCL_PRESENT_CURRENT_L);
}
if(Current&(1<<15)){
Current = -(Current&~(1<<15));
}
return Current;
}
int SCSCL::PWMMode(u8 ID)
{
u8 bBuf[4];
bBuf[0] = 0;
bBuf[1] = 0;
bBuf[2] = 0;
bBuf[3] = 0;
return genWrite(ID, SCSCL_MIN_ANGLE_LIMIT_L, bBuf, 4);
}
int SCSCL::WritePWM(u8 ID, s16 pwmOut)
{
if(pwmOut<0){
pwmOut = -pwmOut;
pwmOut |= (1<<10);
}
u8 bBuf[2];
Host2SCS(bBuf+0, bBuf+1, pwmOut);
return genWrite(ID, SCSCL_GOAL_TIME_L, bBuf, 2);
}

77
jxbeye/lib/FTServo/src/SCSCL.h Normal file
View File

@@ -0,0 +1,77 @@
/*
* SCSCL.h
* 飞特SCSCL系列串行舵机应用层程序
* 日期: 2024.4.2
* 作者:
*/
#ifndef _SCSCL_H
#define _SCSCL_H
//内存表定义
//-------EPROM(只读)--------
#define SCSCL_VERSION_L 3
#define SCSCL_VERSION_H 4
//-------EPROM(读写)--------
#define SCSCL_ID 5
#define SCSCL_BAUD_RATE 6
#define SCSCL_MIN_ANGLE_LIMIT_L 9
#define SCSCL_MIN_ANGLE_LIMIT_H 10
#define SCSCL_MAX_ANGLE_LIMIT_L 11
#define SCSCL_MAX_ANGLE_LIMIT_H 12
#define SCSCL_CW_DEAD 26
#define SCSCL_CCW_DEAD 27
//-------SRAM(读写)--------
#define SCSCL_TORQUE_ENABLE 40
#define SCSCL_GOAL_POSITION_L 42
#define SCSCL_GOAL_POSITION_H 43
#define SCSCL_GOAL_TIME_L 44
#define SCSCL_GOAL_TIME_H 45
#define SCSCL_GOAL_SPEED_L 46
#define SCSCL_GOAL_SPEED_H 47
#define SCSCL_LOCK 48
//-------SRAM(只读)--------
#define SCSCL_PRESENT_POSITION_L 56
#define SCSCL_PRESENT_POSITION_H 57
#define SCSCL_PRESENT_SPEED_L 58
#define SCSCL_PRESENT_SPEED_H 59
#define SCSCL_PRESENT_LOAD_L 60
#define SCSCL_PRESENT_LOAD_H 61
#define SCSCL_PRESENT_VOLTAGE 62
#define SCSCL_PRESENT_TEMPERATURE 63
#define SCSCL_MOVING 66
#define SCSCL_PRESENT_CURRENT_L 69
#define SCSCL_PRESENT_CURRENT_H 70
#include "SCSerial.h"
class SCSCL : public SCSerial
{
public:
SCSCL();
SCSCL(u8 End);
SCSCL(u8 End, u8 Level);
int WritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//普通写单个舵机位置指令
int RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//异步写单个舵机位置指令(RegWriteAction生效)
void SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[]);//同步写多个舵机位置指令
int PWMMode(u8 ID);//PWM模式
int WritePWM(u8 ID, s16 pwmOut);//PWM输出模式指令
int EnableTorque(u8 ID, u8 Enable);//扭矩控制指令
int unLockEprom(u8 ID);//eprom解锁
int LockEprom(u8 ID);//eprom加锁
int FeedBack(int ID);//反馈舵机信息
int ReadPos(int ID);//读位置
int ReadSpeed(int ID);//读速度
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
int ReadVoltage(int ID);//读电压
int ReadTemper(int ID);//读温度
int ReadMove(int ID);//读移动状态
int ReadCurrent(int ID);//读电流
private:
u8 Mem[SCSCL_PRESENT_CURRENT_H-SCSCL_PRESENT_POSITION_L+1];
};
#endif

99
jxbeye/lib/FTServo/src/SCSerial.cpp Normal file
View File

@@ -0,0 +1,99 @@
/*
* SCSerial.h
* 飞特串行舵机硬件接口层程序
* 日期: 2024.4.2
* 作者:
*/
#include "SCSerial.h"
SCSerial::SCSerial()
{
IOTimeOut = 10;
pSerial = NULL;
}
SCSerial::SCSerial(u8 End):SCS(End)
{
IOTimeOut = 10;
pSerial = NULL;
}
SCSerial::SCSerial(u8 End, u8 Level):SCS(End, Level)
{
IOTimeOut = 10;
pSerial = NULL;
}
int SCSerial::readSCS(unsigned char *nDat, int nLen, unsigned long TimeOut)
{
int Size = 0;
int ComData;
unsigned long t_begin = millis();
unsigned long t_user;
while(1){
ComData = pSerial->read();
if(ComData!=-1){
if(nDat){
nDat[Size] = ComData;
}
Size++;
}
if(Size>=nLen){
break;
}
t_user = millis() - t_begin;
if(t_user>TimeOut){
break;
}
}
return Size;
}
int SCSerial::readSCS(unsigned char *nDat, int nLen)
{
int Size = 0;
int ComData;
unsigned long t_begin = millis();
unsigned long t_user;
while(1){
ComData = pSerial->read();
if(ComData!=-1){
if(nDat){
nDat[Size] = ComData;
}
Size++;
t_begin = millis();
}
if(Size>=nLen){
break;
}
t_user = millis() - t_begin;
if(t_user>IOTimeOut){
break;
}
}
return Size;
}
int SCSerial::writeSCS(unsigned char *nDat, int nLen)
{
if(nDat==NULL){
return 0;
}
return pSerial->write(nDat, nLen);
}
int SCSerial::writeSCS(unsigned char bDat)
{
return pSerial->write(&bDat, 1);
}
void SCSerial::rFlushSCS()
{
while(pSerial->read()!=-1);
}
void SCSerial::wFlushSCS()
{
}

38
jxbeye/lib/FTServo/src/SCSerial.h Normal file
View File

@@ -0,0 +1,38 @@
/*
* SCSerial.h
* 飞特串行舵机硬件接口层程序
* 日期: 2024.11.22
* 作者: txl
*/
#ifndef _SCSERIAL_H
#define _SCSERIAL_H
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include "SCS.h"
class SCSerial : public SCS
{
public:
SCSerial();
SCSerial(u8 End);
SCSerial(u8 End, u8 Level);
protected:
int writeSCS(unsigned char *nDat, int nLen);//输出nLen字节
int readSCS(unsigned char *nDat, int nLen);//输入nLen字节
int readSCS(unsigned char *nDat, int nLen, unsigned long TimeOut);
int writeSCS(unsigned char bDat);//输出1字节
void rFlushSCS();//
void wFlushSCS();//
public:
unsigned long IOTimeOut;//输入输出超时
HardwareSerial *pSerial;//串口指针
};
#endif

15
jxbeye/lib/FTServo/src/SCServo.h Normal file
View File

@@ -0,0 +1,15 @@
/*
* SCServo.h
* 飞特串行舵机接口
* 日期: 2024.11.24
* 作者: txl
*/
#ifndef _SCSERVO_H
#define _SCSERVO_H
#include "SCSCL.h"
#include "SMS_STS.h"
#include "HLSCL.h"
#endif

249
jxbeye/lib/FTServo/src/SMS_STS.cpp Normal file
View File

@@ -0,0 +1,249 @@
/*
* SMS_STS.cpp
* 飞特SMS_STS系列串行舵机应用层程序
* 日期: 2024.11.21
* 作者: txl
*/
#include "SMS_STS.h"
SMS_STS::SMS_STS()
{
End = 0;
}
SMS_STS::SMS_STS(u8 End):SCSerial(End)
{
}
SMS_STS::SMS_STS(u8 End, u8 Level):SCSerial(End, Level)
{
}
int SMS_STS::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC)
{
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, 0);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, SMS_STS_ACC, bBuf, 7);
}
int SMS_STS::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC)
{
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, 0);
Host2SCS(bBuf+5, bBuf+6, Speed);
return regWrite(ID, SMS_STS_ACC, bBuf, 7);
}
void SMS_STS::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[])
{
u8 offbuf[7*IDN];
for(u8 i = 0; i<IDN; i++){
if(Position[i]<0){
Position[i] = -Position[i];
Position[i] |= (1<<15);
}
u16 V;
if(Speed){
V = Speed[i];
}else{
V = 0;
}
if(ACC){
offbuf[i*7] = ACC[i];
}else{
offbuf[i*7] = 0;
}
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, Position[i]);
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, 0);
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, V);
}
syncWrite(ID, IDN, SMS_STS_ACC, offbuf, 7);
}
void SMS_STS::SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[])
{
u8 offbuf[7*IDN];
for(u8 i = 0; i<IDN; i++){
if(Speed[i]<0){
Speed[i] = -Speed[i];
Speed[i] |= (1<<15);
}
if(ACC){
offbuf[i*7] = ACC[i];
}else{
offbuf[i*7] = 0;
}
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, 0);
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, 0);
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
}
syncWrite(ID, IDN, SMS_STS_ACC, offbuf, 7);
}
int SMS_STS::WheelMode(u8 ID)
{
return writeByte(ID, SMS_STS_MODE, 1);
}
int SMS_STS::WriteSpe(u8 ID, s16 Speed, u8 ACC)
{
if(Speed<0){
Speed = -Speed;
Speed |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, 0);
Host2SCS(bBuf+3, bBuf+4, 0);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, SMS_STS_ACC, bBuf, 7);
}
int SMS_STS::EnableTorque(u8 ID, u8 Enable)
{
return writeByte(ID, SMS_STS_TORQUE_ENABLE, Enable);
}
int SMS_STS::unLockEprom(u8 ID)
{
return writeByte(ID, SMS_STS_LOCK, 0);
}
int SMS_STS::LockEprom(u8 ID)
{
return writeByte(ID, SMS_STS_LOCK, 1);
}
int SMS_STS::CalibrationOfs(u8 ID)
{
return writeByte(ID, SMS_STS_TORQUE_ENABLE, 128);
}
int SMS_STS::FeedBack(int ID)
{
int nLen = Read(ID, SMS_STS_PRESENT_POSITION_L, Mem, sizeof(Mem));
if(nLen!=sizeof(Mem)){
return -1;
}
return nLen;
}
int SMS_STS::ReadPos(int ID)
{
int Pos = -1;
if(ID==-1){
Pos = Mem[SMS_STS_PRESENT_POSITION_H-SMS_STS_PRESENT_POSITION_L];
Pos <<= 8;
Pos |= Mem[SMS_STS_PRESENT_POSITION_L-SMS_STS_PRESENT_POSITION_L];
}else{
Pos = readWord(ID, SMS_STS_PRESENT_POSITION_L);
}
if(Pos&(1<<15)){
Pos = -(Pos&~(1<<15));
}
return Pos;
}
int SMS_STS::ReadSpeed(int ID)
{
int Speed = -1;
if(ID==-1){
Speed = Mem[SMS_STS_PRESENT_SPEED_H-SMS_STS_PRESENT_POSITION_L];
Speed <<= 8;
Speed |= Mem[SMS_STS_PRESENT_SPEED_L-SMS_STS_PRESENT_POSITION_L];
}else{
Speed = readWord(ID, SMS_STS_PRESENT_SPEED_L);
}
if(Speed&(1<<15)){
Speed = -(Speed&~(1<<15));
}
return Speed;
}
int SMS_STS::ReadLoad(int ID)
{
int Load = -1;
if(ID==-1){
Load = Mem[SMS_STS_PRESENT_LOAD_H-SMS_STS_PRESENT_POSITION_L];
Load <<= 8;
Load |= Mem[SMS_STS_PRESENT_LOAD_L-SMS_STS_PRESENT_POSITION_L];
}else{
Load = readWord(ID, SMS_STS_PRESENT_LOAD_L);
}
if(Load&(1<<10)){
Load = -(Load&~(1<<10));
}
return Load;
}
int SMS_STS::ReadVoltage(int ID)
{
int Voltage = -1;
if(ID==-1){
Voltage = Mem[SMS_STS_PRESENT_VOLTAGE-SMS_STS_PRESENT_POSITION_L];
}else{
Voltage = readByte(ID, SMS_STS_PRESENT_VOLTAGE);
}
return Voltage;
}
int SMS_STS::ReadTemper(int ID)
{
int Temper = -1;
if(ID==-1){
Temper = Mem[SMS_STS_PRESENT_TEMPERATURE-SMS_STS_PRESENT_POSITION_L];
}else{
Temper = readByte(ID, SMS_STS_PRESENT_TEMPERATURE);
}
return Temper;
}
int SMS_STS::ReadMove(int ID)
{
int Move = -1;
if(ID==-1){
Move = Mem[SMS_STS_MOVING-SMS_STS_PRESENT_POSITION_L];
}else{
Move = readByte(ID, SMS_STS_MOVING);
}
return Move;
}
int SMS_STS::ReadCurrent(int ID)
{
int Current = -1;
if(ID==-1){
Current = Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L];
Current <<= 8;
Current |= Mem[SMS_STS_PRESENT_CURRENT_L-SMS_STS_PRESENT_POSITION_L];
}else{
Current = readWord(ID, SMS_STS_PRESENT_CURRENT_L);
}
if(Current&(1<<15)){
Current = -(Current&~(1<<15));
}
return Current;
}
int SMS_STS::ServoMode(u8 ID)
{
return writeByte(ID, SMS_STS_MODE, 0);
}

86
jxbeye/lib/FTServo/src/SMS_STS.h Normal file
View File

@@ -0,0 +1,86 @@
/*
* SMS_STS.h
* 飞特SMS/STS系列串行舵机应用层程序
* 日期: 2024.11.21
* 作者: txl
*/
#ifndef _SMS_STS_H
#define _SMS_STS_H
//内存表定义
//-------EPROM(只读)--------
#define SMS_STS_MODEL_L 3
#define SMS_STS_MODEL_H 4
//-------EPROM(读写)--------
#define SMS_STS_ID 5
#define SMS_STS_BAUD_RATE 6
#define SMS_STS_MIN_ANGLE_LIMIT_L 9
#define SMS_STS_MIN_ANGLE_LIMIT_H 10
#define SMS_STS_MAX_ANGLE_LIMIT_L 11
#define SMS_STS_MAX_ANGLE_LIMIT_H 12
#define SMS_STS_CW_DEAD 26
#define SMS_STS_CCW_DEAD 27
#define SMS_STS_OFS_L 31
#define SMS_STS_OFS_H 32
#define SMS_STS_MODE 33
//-------SRAM(读写)--------
#define SMS_STS_TORQUE_ENABLE 40
#define SMS_STS_ACC 41
#define SMS_STS_GOAL_POSITION_L 42
#define SMS_STS_GOAL_POSITION_H 43
#define SMS_STS_GOAL_TIME_L 44
#define SMS_STS_GOAL_TIME_H 45
#define SMS_STS_GOAL_SPEED_L 46
#define SMS_STS_GOAL_SPEED_H 47
#define SMS_STS_TORQUE_LIMIT_L 48
#define SMS_STS_TORQUE_LIMIT_H 49
#define SMS_STS_LOCK 55
//-------SRAM(只读)--------
#define SMS_STS_PRESENT_POSITION_L 56
#define SMS_STS_PRESENT_POSITION_H 57
#define SMS_STS_PRESENT_SPEED_L 58
#define SMS_STS_PRESENT_SPEED_H 59
#define SMS_STS_PRESENT_LOAD_L 60
#define SMS_STS_PRESENT_LOAD_H 61
#define SMS_STS_PRESENT_VOLTAGE 62
#define SMS_STS_PRESENT_TEMPERATURE 63
#define SMS_STS_MOVING 66
#define SMS_STS_PRESENT_CURRENT_L 69
#define SMS_STS_PRESENT_CURRENT_H 70
#include "SCSerial.h"
class SMS_STS : public SCSerial
{
public:
SMS_STS();
SMS_STS(u8 End);
SMS_STS(u8 End, u8 Level);
int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//普通写单个舵机位置指令
int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//异步写单个舵机位置指令(RegWriteAction生效)
void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//同步写多个舵机位置指令
void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[]);//同步写多个舵机速度指令
int ServoMode(u8 ID);//Servo模式
int WheelMode(u8 ID);//恒速模式
int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0);//恒速模式控制指令
int EnableTorque(u8 ID, u8 Enable);//扭力控制指令
int unLockEprom(u8 ID);//eprom解锁
int LockEprom(u8 ID);//eprom加锁
int CalibrationOfs(u8 ID);//中位校准
int FeedBack(int ID);//反馈舵机信息
int ReadPos(int ID);//读位置
int ReadSpeed(int ID);//读速度
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
int ReadVoltage(int ID);//读电压
int ReadTemper(int ID);//读温度
int ReadMove(int ID);//读移动状态
int ReadCurrent(int ID);//读电流
private:
u8 Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L+1];
};
#endif

410
jxbeye/src/EPSNOW通讯
View File

@@ -1,410 +0,0 @@
#define ESP_NOW_HOST
// ===== 配置宏 =====
#ifndef ESP_NOW_DEFAULT_CHANNEL
#define ESP_NOW_DEFAULT_CHANNEL 6
#endif
#ifndef ESP_NOW_MAX_SLAVES
#define ESP_NOW_MAX_SLAVES 20
#endif
#ifndef ESP_NOW_CFG_INTERVAL
#define ESP_NOW_CFG_INTERVAL 500
#endif
// Flash Key
#define HOST_CFG_NS "host_cfg"
#define HOST_CHANNEL_KEY "channel"
#define SLAVES_NS "slaves"
#define SLAVE_CFG_NS "slave_cfg"
#define SLAVE_ID_KEY "id"
#define SLAVE_HOST_KEY "host"
#define SLAVE_CHANNEL_KEY "channel"
class EspNowPeer {
private:
bool _initialized = false;
bool _ready = false;
uint8_t _myId;
uint8_t _hostMac[6];
uint8_t _currentChannel;
#ifdef ESP_NOW_HOST
uint8_t _slaveMacs[ESP_NOW_MAX_SLAVES + 1][6];
bool _slaveActive[ESP_NOW_MAX_SLAVES + 1];
Preferences _prefs;
#endif
char _recvBuf[256];
bool _hasMsg = false;
String _parsedValue;
volatile bool _waitingReg = false;
volatile uint8_t _waitingId = 0;
volatile bool _regReceived = false;
static bool _str2mac(const char* s, uint8_t* m) {
return sscanf(s, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
&m[0],&m[1],&m[2],&m[3],&m[4],&m[5]) == 6;
}
static void _mac2str(const uint8_t* m, char* s) {
sprintf(s, "%02X:%02X:%02X:%02X:%02X:%02X", m[0],m[1],m[2],m[3],m[4],m[5]);
}
static void _onRecv(const uint8_t* mac, const uint8_t* data, int len) {
if(_instance) _instance->_handleRecv(mac, data, len);
}
void _handleRecv(const uint8_t* mac, const uint8_t* data, int len) {
if(len >= 256) return;
memcpy(_recvBuf, data, len);
_recvBuf[len] = '\0';
_hasMsg = true;
#ifdef ESP_NOW_HOST
if(_waitingReg && getMessageValue("climac")) {
String val = lastValue();
int dashPos = val.indexOf('-');
if(dashPos > 0) {
uint8_t recv_id = val.substring(0, dashPos).toInt();
if(recv_id == _waitingId && recv_id >= 1 && recv_id <= ESP_NOW_MAX_SLAVES) {
String macStr = val.substring(dashPos + 1);
uint8_t recv_mac[6];
if(_str2mac(macStr.c_str(), recv_mac)) {
_regReceived = true;
_registerSlave(recv_id, recv_mac);
}
}
}
}
#endif
}
#ifdef ESP_NOW_HOST
bool _saveChannel(uint8_t ch) {
Preferences prefs;
if(prefs.begin(HOST_CFG_NS, false)) {
prefs.putUChar(HOST_CHANNEL_KEY, ch);
prefs.end();
return true;
}
return false;
}
uint8_t _loadChannel() {
Preferences prefs;
if(prefs.begin(HOST_CFG_NS, true)) {
uint8_t ch = prefs.getUChar(HOST_CHANNEL_KEY, 0);
prefs.end();
if(ch >= 1 && ch <= 13) return ch;
}
return ESP_NOW_DEFAULT_CHANNEL;
}
bool _changeChannel(uint8_t newChannel) {
if(newChannel < 1 || newChannel > 13) return false;
if(newChannel == _currentChannel) return true;
_saveChannel(newChannel);
esp_wifi_set_channel(newChannel, WIFI_SECOND_CHAN_NONE);
_currentChannel = newChannel;
esp_now_deinit();
delay(100);
if(esp_now_init() != ESP_OK) return false;
esp_now_register_recv_cb(_onRecv);
return true;
}
bool _registerSlave(uint8_t id, uint8_t* mac) {
if(id < 1 || id > ESP_NOW_MAX_SLAVES) return false;
if(_slaveActive[id] && memcmp(_slaveMacs[id], mac, 6) == 0) return true;
memcpy(_slaveMacs[id], mac, 6);
_slaveActive[id] = true;
_prefs.begin(SLAVES_NS, false);
_prefs.putBytes((String("s")+String(id)).c_str(), mac, 6);
_prefs.end();
esp_now_peer_info_t p = {};
memcpy(p.peer_addr, mac, 6);
p.channel = _currentChannel;
p.encrypt = false;
esp_now_add_peer(&p);
return true;
}
uint8_t* _findMac(uint8_t id) {
if(id < 1 || id > ESP_NOW_MAX_SLAVES) return nullptr;
return _slaveActive[id] ? _slaveMacs[id] : nullptr;
}
void _loadSlaves() {
for(int i=0; i<=ESP_NOW_MAX_SLAVES; i++) _slaveActive[i] = false;
_prefs.begin(SLAVES_NS, true);
for(uint8_t id=1; id<=ESP_NOW_MAX_SLAVES; id++) {
uint8_t mac[6];
if(_prefs.getBytes((String("s")+String(id)).c_str(), mac, 6) == 6) {
memcpy(_slaveMacs[id], mac, 6);
_slaveActive[id] = true;
esp_now_peer_info_t p = {};
memcpy(p.peer_addr, mac, 6);
p.channel = _currentChannel;
p.encrypt = false;
esp_now_add_peer(&p);
}
}
_prefs.end();
}
#endif
bool _send(const uint8_t* mac, const char* key, const char* value) {
if(!_ready || !mac || !key || !value) return false;
char json[250];
snprintf(json, sizeof(json), "{\"%s\":\"%s\"}", key, value);
int len = strlen(json);
if(len > 250) return false;
return esp_now_send(mac, (uint8_t*)json, len) == ESP_OK;
}
static EspNowPeer* _instance;
public:
EspNowPeer() { _instance = this; }
bool begin(uint8_t myId = 0, const uint8_t* hostMac = nullptr) {
if(_initialized) return true;
_myId = myId;
if(hostMac) memcpy(_hostMac, hostMac, 6);
#ifdef ESP_NOW_HOST
_currentChannel = _loadChannel();
#else
_currentChannel = _loadChannelSlave();
#endif
WiFi.mode(WIFI_STA);
WiFi.disconnect();
esp_wifi_set_channel(_currentChannel, WIFI_SECOND_CHAN_NONE);
if(esp_now_init() != ESP_OK) return false;
esp_now_register_recv_cb(_onRecv);
#ifdef ESP_NOW_HOST
_prefs.begin(SLAVES_NS, false);
_loadSlaves();
_prefs.end();
#endif
_initialized = true;
_ready = true;
return true;
}
// ===== 主机公开接口 =====
#ifdef ESP_NOW_HOST
void setDevice(uint8_t slaveId, uint8_t channel = 0) {
if(!_ready) return;
if(channel == 0) channel = _currentChannel;
if(channel != _currentChannel) _changeChannel(channel);
uint8_t myMac[6];
esp_read_mac(myMac, ESP_MAC_WIFI_STA);
char hostMacStr[18];
_mac2str(myMac, hostMacStr);
_waitingReg = true;
_waitingId = slaveId;
_regReceived = false;
uint32_t lastSend = 0;
while(!_regReceived) {
if(millis() - lastSend >= ESP_NOW_CFG_INTERVAL) {
Serial1.printf("CFG:%d:%s:%dWZHY\n", slaveId, hostMacStr, channel);
Serial.printf("CFG:%d:%s:%dWZHY\n", slaveId, hostMacStr, channel);
lastSend = millis();
}
delay(10);
}
_waitingReg = false;
}
bool sendMessage(uint8_t slaveId, const String& key, const String& value) {
if(!_ready) return false;
uint8_t* mac = _findMac(slaveId);
if(!mac) return false;
return _send(mac, key.c_str(), value.c_str());
}
uint8_t getChannel() { return _currentChannel; }
#endif
// ===== 从机公开接口 =====
#ifdef ESP_NOW_SLAVE
uint8_t _loadChannelSlave() {
Preferences prefs;
if(prefs.begin(SLAVE_CFG_NS, true)) {
uint8_t ch = prefs.getUChar(SLAVE_CHANNEL_KEY, 0);
prefs.end();
if(ch >= 1 && ch <= 13) return ch;
}
return ESP_NOW_DEFAULT_CHANNEL;
}
bool _saveChannelSlave(uint8_t ch) {
Preferences prefs;
if(prefs.begin(SLAVE_CFG_NS, false)) {
prefs.putUChar(SLAVE_CHANNEL_KEY, ch);
prefs.end();
return true;
}
return false;
}
// ✅ 核心接口：解析并应用配置
bool applyConfig(uint8_t id, const uint8_t* hostMac, uint8_t channel) {
bool changed = false;
// 信道变更
if(channel != _currentChannel && channel >= 1 && channel <= 13) {
_saveChannelSlave(channel);
esp_wifi_set_channel(channel, WIFI_SECOND_CHAN_NONE);
_currentChannel = channel;
esp_now_deinit();
delay(100);
esp_now_init();
esp_now_register_recv_cb(_onRecv);
changed = true;
}
// ID 变更
Preferences prefs;
prefs.begin(SLAVE_CFG_NS, false);
if(prefs.getUChar(SLAVE_ID_KEY, 0) != id) {
prefs.putUChar(SLAVE_ID_KEY, id);
_myId = id;
changed = true;
}
// 主机 MAC 变更
uint8_t storedHost[6];
if(prefs.getBytes(SLAVE_HOST_KEY, storedHost, 6) != 6 || memcmp(storedHost, hostMac, 6) != 0) {
prefs.putBytes(SLAVE_HOST_KEY, hostMac, 6);
memcpy(_hostMac, hostMac, 6);
changed = true;
}
prefs.end();
// 发送 climac 确认
_sendRegistration();
return changed;
}
bool loadConfig() {
Preferences prefs;
prefs.begin(SLAVE_CFG_NS, true);
_myId = prefs.getUChar(SLAVE_ID_KEY, 0);
if(_myId == 0) { prefs.end(); return false; }
if(prefs.getBytes(SLAVE_HOST_KEY, _hostMac, 6) != 6) { prefs.end(); return false; }
prefs.end();
return true;
}
bool _sendRegistration() {
uint8_t myMac[6];
esp_read_mac(myMac, ESP_MAC_WIFI_STA);
char macStr[18];
_mac2str(myMac, macStr);
char content[40];
snprintf(content, sizeof(content), "%d-%s", _myId, macStr);
return _send(_hostMac, "climac", content);
}
bool sendMessage(const String& key, const String& value) {
if(!_ready) return false;
char content[220];
snprintf(content, sizeof(content), "cli-%d-%s", _myId, value.c_str());
return _send(_hostMac, key.c_str(), content);
}
uint8_t getChannel() { return _currentChannel; }
uint8_t getMyId() { return _myId; }
#endif
// ===== 通用接口 =====
bool hasMessage() {
bool has = _hasMsg;
_hasMsg = false;
return has;
}
bool getMessageValue(const String& key) {
_parsedValue = "";
if(strlen(_recvBuf) == 0) return false;
String pattern = "\"" + key + "\":\"";
int start = String(_recvBuf).indexOf(pattern);
if(start == -1) return false;
start += pattern.length();
int end = String(_recvBuf).indexOf("\"", start);
if(end == -1) return false;
_parsedValue = String(_recvBuf).substring(start, end);
return true;
}
String lastValue() { return _parsedValue; }
void clearMessage() { _recvBuf[0] = '\0'; _parsedValue = ""; _hasMsg = false; }
bool isReady() { return _ready; }
};
EspNowPeer* EspNowPeer::_instance = nullptr;
EspNowPeer peer;
void setup() {
Serial.begin(115200);
delay(1000);
Serial1.begin(115200, SERIAL_8N1, 1, 2);//K10自身管脚不够利用串口接外部芯片扩展
// ✅ 初始化主机（ID=255）
peer.begin(255);
Serial.printf("📡 主机就绪 信道=%d\n", peer.getChannel());
// ===== 关键函数调用示例 =====
// 示例 1：配置从机#5（使用当前信道）
peer.setDevice(5);
// 示例 2：配置从机#5 并切换到信道 6
// peer.setDevice(5, 6);
// 示例 3：发送消息到从机#5
// peer.sendMessage(5, "c", "GO");
// 示例 4：发送消息到从机#5（带参数）
// peer.sendMessage(5, "set", "speed:100");
}
void loop() {
// ✅ 接收从机消息
if(peer.hasMessage()) {
if(peer.getMessageValue("c")) {
String val = peer.lastValue();
// 解析 cli-<id>-<content> 格式
if(val.startsWith("cli-")) {
int d1 = val.indexOf('-', 4);
if(d1 > 0) {
String idStr = val.substring(4, d1);
String content = val.substring(d1 + 1);
Serial.printf("📥 从机#%s: %s\n", idStr.c_str(), content.c_str());
}
}
}
}
// ✅ 实际使用时，根据业务逻辑调用：
// peer.setDevice(5, 6); // 配置从机
peer.sendMessage(5, "c", "GO"); // 发送命令
delay(100);
}

911
jxbeye/src/bkespnow版本
View File

@@ -1,911 +0,0 @@
#include <Arduino.h>
#include <WiFi.h>
#include <WebServer.h>
#include "esp_camera.h"
#include "esp_heap_caps.h"
#include <freertos/queue.h>
#include <Preferences.h>
#include <WiFiUdp.h>
#include <stdarg.h>
#include <esp_now.h> // ✅【改动 1】添加 ESP-NOW 头文件
#if 1
// ===== UDP通信类（最小改动）=====
class UDPPeer {
private:
WiFiUDP udp;
bool ready = false;
uint16_t local_port = 8888;
// 双缓存
String receivedRaw;
String parsedValue;
// 连接参数
const char* ssid = nullptr;
const char* password = nullptr;
IPAddress local_ip;
IPAddress gateway;
IPAddress subnet;
// ✅ 监视器IP最后一位
uint8_t monitor_ip_last = 101;
// ✅ 改回普通成员变量（非静态）
bool is_initialized = false;
uint32_t last_maintenance = 0;
const uint32_t MAINTENANCE_INTERVAL = 1000;
// ✅【改动 2】新增：ESP-NOW消息标志
bool espnowready=false;
bool espnowHasMsg=false;
void autoMaintain() {
if (!is_initialized) return;
if (millis() - last_maintenance < MAINTENANCE_INTERVAL) return;
last_maintenance = millis();
if (WiFi.status() != WL_CONNECTED) {
WiFi.config(local_ip, gateway, subnet);
if (!ready)
{
//WiFi.disconnect(true);
//delay(500);
Serial.printf("正在启动网络连接！！！\n");
WiFi.begin(ssid, password);
delay(500);
WiFi.begin(ssid, password);
delay(500);
WiFi.begin(ssid, password);
delay(500);
WiFi.begin(ssid, password);
delay(500);
}
else
{
Serial.printf("网络中断，恢复中！！！\n");
WiFi.reconnect();
//esp_now_deinit();
}
return;
}
if (!ready) {
ready = (udp.begin(local_port) == 1);
if (ready) {
//initEspNowRecv();
Serial.printf("✅ 网络就绪: IP=%s 端口=%d\n",
WiFi.localIP().toString().c_str(), local_port);
} else {
return;
}
}
}
public:
// ✅ 新增：访问 is_initialized 的成员方法
bool isInitialized() {
return is_initialized;
}
bool wifiisready() {
return ready;
}
bool espnowisready(){
return espnowready;
}
void setespnowreadyflg()
{
espnowready=true;
}
void resetespnowreadyflg()
{
espnowready=false;
}
// ✅ 设置监视器IP
void setMonitorIPLast(uint8_t last_octet) {
monitor_ip_last = last_octet;
}
// ✅ 获取完整监视器IP
String getMonitorIP() {
return String(gateway[0]) + "." + String(gateway[1]) + "." +
String(gateway[2]) + "." + String(monitor_ip_last);
}
bool connect(const char* _ssid, const char* _pwd,
uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
uint8_t local_ip_last, uint16_t port = 8888) {
WiFi.disconnect(true);
last_maintenance = 0;
ready = false;
espnowHasMsg = false; // ✅ 重置ESP-NOW标志
ssid = _ssid;
password = _pwd;
local_ip = IPAddress(gw1, gw2, gw3, local_ip_last);
gateway = IPAddress(gw1, gw2, gw3, gw4);
subnet = IPAddress(255, 255, 255, 0);
local_port = port;
is_initialized = true;
Serial.printf("📶 wifi：%s， pwd：%s，配置网络: IP=%d.%d.%d.%d 网关=%d.%d.%d.%d 端口=%d\n",
ssid, password, gw1, gw2, gw3, local_ip_last, gw1, gw2, gw3, gw4, port);
Serial.println("💡 网络将在后台自动连接...");
autoMaintain();
return true;
}
// ✅【改动 3】修改hasMessage()：优先检查ESP-NOW
bool hasMessage() {
autoMaintain();
if (!is_initialized || !ready) return false;
// 优先检查ESP-NOW消息
if (espnowHasMsg) {
return true; // 数据已在receivedRaw中
}
// 再检查UDP消息（原有逻辑）
receivedRaw = "";
parsedValue = "";
int len = udp.parsePacket();
if (len <= 0 || len > 512) return false;
char buf[513];
int read = udp.read((uint8_t*)buf, min(len, 512));
if (read > 0) {
buf[read] = '\0';
receivedRaw = String(buf);
return true;
}
return false;
}
bool getMessageValue(const String& key) {
parsedValue = "";
if (receivedRaw.length() == 0) return false;
String keyPattern = "\"" + key + "\"";
int startIndex = receivedRaw.indexOf(keyPattern);
if (startIndex == -1) return false;
int colonIndex = receivedRaw.indexOf(":", startIndex);
if (colonIndex == -1) return false;
int valueStart = colonIndex + 1;
while (valueStart < receivedRaw.length() &&
receivedRaw.charAt(valueStart) == ' ') valueStart++;
if (valueStart < receivedRaw.length() && receivedRaw.charAt(valueStart) == '"') {
int valueEnd = receivedRaw.indexOf("\"", valueStart + 1);
if (valueEnd != -1) {
parsedValue = receivedRaw.substring(valueStart + 1, valueEnd);
return true;
}
} else {
int valueEnd = valueStart;
while (valueEnd < receivedRaw.length() &&
receivedRaw.charAt(valueEnd) != ',' &&
receivedRaw.charAt(valueEnd) != '}' &&
receivedRaw.charAt(valueEnd) != ' ') valueEnd++;
while (valueEnd > valueStart && receivedRaw.charAt(valueEnd - 1) == ' ') valueEnd--;
if (valueEnd > valueStart) {
parsedValue = receivedRaw.substring(valueStart, valueEnd);
return true;
}
}
return false;
}
bool sendTo(const String& target_ip_str, const String& key, const char* format, ...) {
autoMaintain();
if (!is_initialized || !ready)
{
Serial.println("连接还未完成！！！");
return false;
}
IPAddress target_ip;
if (!target_ip.fromString(target_ip_str)) return false;
char content[256];
va_list args;
va_start(args, format);
vsnprintf(content, sizeof(content) - 1, format, args);
va_end(args);
char json[300];
snprintf(json, sizeof(json), "{\"%s\":\"%s\"}\r\n", key.c_str(), content);
return udp.beginPacket(target_ip, local_port) &&
udp.write((const uint8_t*)json, strlen(json)) &&
udp.endPacket();
}
bool sendTo(const String& target_ip_str, const String& key, const String& value) {
return sendTo(target_ip_str, key, "%s", value.c_str());
}
String lastValue() { return parsedValue; }
String lastMessage() { return receivedRaw; }
String getLocalIP() {
autoMaintain();
return is_initialized ? WiFi.localIP().toString() : "未配置";
}
bool isConnected() {
autoMaintain();
return is_initialized && ready;
}
// ✅【改动 4】新增：获取本机MAC地址
String getMacAddress() {
return WiFi.macAddress();
}
// ✅【改动 5】新增：设置ESP-NOW消息（回调调用）
void setEspNowMessage(const char* data, int len) {
if (len <= 0 || len > 250) return; // ESP-NOW最大250字节
char buf[251];
memcpy(buf, data, len);
buf[len] = '\0';
receivedRaw = String(buf);
espnowHasMsg = true;
}
// ✅【改动 6】新增：清除ESP-NOW标志（解析后调用）
void clearEspNowFlag() {
espnowHasMsg = false;
}
};
extern UDPPeer peer;
UDPPeer peer;
// ✅【改动 7】ESP-NOW接收回调（直接调用peer方法）
static void OnEspNowRecv(const uint8_t *mac, const uint8_t *data, int len) {
peer.setEspNowMessage((const char*)data, len);
}
// ✅【改动 8】初始化ESP-NOW接收
bool initEspNowRecv() {
if (esp_now_init() != ESP_OK) {
Serial.println("❌ ESP-NOW初始化失败");
return 0;
}
esp_now_register_recv_cb(OnEspNowRecv);
Serial.println("✅ ESP-NOW接收已启用");
return 1;
}
// ===== 默认配置 =====
#define DEFAULT_SSID "ESP32-Camera"
#define DEFAULT_PWD "12345678"
#define DEFAULT_GW1 192
#define DEFAULT_GW2 168
#define DEFAULT_GW3 4
#define DEFAULT_GW4 1
#define DEFAULT_LOCAL_LAST 100
#define DEFAULT_MONITOR_LAST 101
// ===== Flash配置管理 =====
void loadConfigFromFlash() {
Preferences prefs;
if (!prefs.begin("cam_cfg", true)) {
Serial.println("⚠️ Flash配置不存在，使用默认值");
peer.connect(DEFAULT_SSID, DEFAULT_PWD,
DEFAULT_GW1, DEFAULT_GW2, DEFAULT_GW3, DEFAULT_GW4,
DEFAULT_LOCAL_LAST);
peer.setMonitorIPLast(DEFAULT_MONITOR_LAST);
prefs.end();
return;
}
String ssid = prefs.getString("ssid", DEFAULT_SSID);
String pwd = prefs.getString("pwd", DEFAULT_PWD);
uint8_t gw1 = prefs.getUChar("gw1", DEFAULT_GW1);
uint8_t gw2 = prefs.getUChar("gw2", DEFAULT_GW2);
uint8_t gw3 = prefs.getUChar("gw3", DEFAULT_GW3);
uint8_t gw4 = prefs.getUChar("gw4", DEFAULT_GW4);
uint8_t local_last = prefs.getUChar("local", DEFAULT_LOCAL_LAST);
uint8_t monitor_last = prefs.getUChar("monitor", DEFAULT_MONITOR_LAST);
prefs.end();
peer.connect(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last);
peer.setMonitorIPLast(monitor_last);
Serial.println("✅ 从Flash加载配置成功");
}
void saveConfigToFlash(const char* ssid, const char* pwd,
uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
uint8_t local_last, uint8_t monitor_last) {
Preferences prefs;
if (!prefs.begin("cam_cfg", false)) {
Serial.println("❌ Flash配置保存失败");
return;
}
prefs.putString("ssid", ssid);
prefs.putString("pwd", pwd);
prefs.putUChar("gw1", gw1);
prefs.putUChar("gw2", gw2);
prefs.putUChar("gw3", gw3);
prefs.putUChar("gw4", gw4);
prefs.putUChar("local", local_last);
prefs.putUChar("monitor", monitor_last);
prefs.end();
Serial.println("✅ 配置已保存到Flash");
}
// ===== 串口指令解析 =====
void handleSerialCommand() {
if (!Serial.available()) return;
String cmd = Serial.readStringUntil('\n');
cmd.trim();
if (!cmd.startsWith("wificar:"))
{
if(cmd.endsWith("LZU")||cmd.endsWith("LZU\r"))
{
peer.sendTo(peer.getMonitorIP(), "f", cmd);
Serial.printf("📤 已发送到 %s: {\"f\":%s}\n", peer.getMonitorIP().c_str(), cmd);
}
// ✅【改动 9】ESPNOW分支：回复MAC+IP给主机
else if(cmd.endsWith("ESPNOW")||cmd.endsWith("ESPNOW\r"))
{
String mac = peer.getMacAddress();
String ip = peer.getLocalIP();
char json[150];
snprintf(json, sizeof(json), "{\"mac\":\"%s\",\"ip\":\"%s\"}\r\n",
mac.c_str(), ip.c_str());
bool ok = peer.sendTo(peer.getMonitorIP(), "ESPNOW_REG", json);
if(ok) {
Serial.printf("✅ ESPNOW注册: MAC=%s IP=%s\n", mac.c_str(), ip.c_str());
} else {
Serial.printf("❌ 发送ESPNOW注册信息失败\n");
}
}
return;
}
int pos[7];
int count = 0;
for (int i = 8; i < cmd.length() && count < 7; i++) {
if (cmd.charAt(i) == ':') {
pos[count++] = i;
}
}
if (count < 7) {
Serial.println("❌ 指令格式错误");
return;
}
String ssid = cmd.substring(8, pos[0]);
String pwd = cmd.substring(pos[0] + 1, pos[1]);
uint8_t gw1 = cmd.substring(pos[1] + 1, pos[2]).toInt();
uint8_t gw2 = cmd.substring(pos[2] + 1, pos[3]).toInt();
uint8_t gw3 = cmd.substring(pos[3] + 1, pos[4]).toInt();
uint8_t gw4 = cmd.substring(pos[4] + 1, pos[5]).toInt();
uint8_t local_last = cmd.substring(pos[5] + 1, pos[6]).toInt();
uint8_t monitor_last = cmd.substring(pos[6] + 1).toInt();
peer.connect(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last);
peer.setMonitorIPLast(monitor_last);
saveConfigToFlash(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last, monitor_last);
Serial.println("✅ WiFi配置已更新并保存");
}
void setup() {
Serial.begin(115200);
delay(1000);
Serial.println("\n╔════════════════════════════════════════════════════════════════╗");
Serial.println("║ ESP32-S3 摄像头 UDP+ESP-NOW 通信系统 v1.3 ║");
Serial.println("║ 支持Flash配置 | 串口配置 | UDP+ESP-NOW双通道统一解析 ║");
Serial.println("╚════════════════════════════════════════════════════════════════╝");
loadConfigFromFlash();
//esp_now_deinit();
//initEspNowRecv(); // ✅【改动 10】初始化ESP-NOW接收
}
void loop() {
handleSerialCommand();
if (peer.isInitialized()) {
// if (peer.wifiisready())
// {
// if(!peer.espnowisready())
// {
// if(initEspNowRecv())
// {
// peer.setespnowreadyflg();
// String mac = peer.getMacAddress();
// String ip = peer.getLocalIP();
// char json[150];
// snprintf(json, sizeof(json), "mac:%s;ip:%s",
// mac.c_str(), ip.c_str());
// bool ok = peer.sendTo(peer.getMonitorIP(), "r", json);//ESPNOW_REG
// if(ok) {
// Serial.printf("✅ ESPNOW注册: MAC=%s IP=%s\n", mac.c_str(), ip.c_str());
// } else {
// Serial.printf("❌ 发送ESPNOW注册信息失败\n");
// }
// }
// }
// }
// else if(peer.espnowisready())
// {
// peer.resetespnowreadyflg();
// esp_now_deinit();
// } //k10的总线扩展机制导espnow优势不能有效发挥，每次查阅按钮时，会直接打断espnow通讯，再启动会延迟将近200ms（感觉是硬件通道重启）；
//若是常规udp通讯，不一定被按钮查阅打断，但是会被此间隔时间冲断，再连接延迟好使120ms左右（感觉是软件握手重启）
//也就是采用k10板子，espnow效果反而更差于udp通讯
if (peer.hasMessage()) {
if (peer.getMessageValue("c")) {
Serial.printf("%sZHY\n", peer.lastValue().c_str());
// Serial.printf("📶 Wi-Fi 模式：%s\n",
// WiFi.getMode() == WIFI_AP ? "AP" :
// WiFi.getMode() == WIFI_STA ? "STA" : "AP+STA");
// Serial.printf("📶 信道：%d\n", WiFi.channel());
// Serial.printf("📶 ESP-NOW：%s\n", peer.espnowisready()? "已启用" : "未启用");
// Serial.printf("📶 状态：%d\n", WiFi.status());
}
// ✅【改动 11】解析后清除ESP-NOW标志（避免重复处理）
peer.clearEspNowFlag();
}
}
delay(1);
}
主机端（espnow版本）
#include <WiFi.h>
#include <WiFiUdp.h>
#include <stdarg.h>
#include <esp_now.h> // ✅ 新增：ESP-NOW 头文件
// ✅ 新增：ESP-NOW 对等节点结构（最多 10 个从机）
#define MAX_SLAVES 10
typedef struct {
uint8_t mac[6];
IPAddress ip;
bool active;
} SlaveNode;
class UDPPeer {
private:
WiFiUDP udp;
bool ready = false;
uint16_t local_port = 8888;
// 双缓存
String receivedRaw;
String parsedValue;
// 连接参数
const char* ssid = nullptr;
const char* password = nullptr;
IPAddress local_ip;
IPAddress gateway;
IPAddress subnet;
// ✅ 新增：AP模式标志
bool is_ap_mode = false;
// ✅ 新增：网络配置缓存
String current_ssid = "";
String current_password = "";
uint8_t cached_gw1 = 0, cached_gw2 = 0, cached_gw3 = 0, cached_gw4 = 0;
// 状态标志
bool is_initialized = false;
uint32_t last_maintenance = 0;
const uint32_t MAINTENANCE_INTERVAL = 1000;
// ✅ 新增：ESP-NOW 相关
bool espnowInited = false;
SlaveNode slaves[MAX_SLAVES]; // 从机注册表
uint8_t slaveCount = 0;
// ✅ 新增：解析 MAC 字符串 "AA:BB:CC:DD:EE:FF" → 字节数组
bool parseMacString(const String& macStr, uint8_t* macBuf) {
int vals[6];
if (sscanf(macStr.c_str(), "%x:%x:%x:%x:%x:%x",
&vals[0], &vals[1], &vals[2], &vals[3], &vals[4], &vals[5]) != 6) {
return false;
}
for (int i = 0; i < 6; i++) macBuf[i] = (uint8_t)vals[i];
return true;
}
// ✅ 新增：初始化 ESP-NOW（网络就绪后调用）
void initEspNow() {
if (espnowInited) return;
if (esp_now_init() != ESP_OK) {
Serial.println("❌ ESP-NOW 初始化失败");
return;
}
// 注册发送回调（可选，用于确认发送状态）
esp_now_register_send_cb([](const uint8_t* mac, esp_now_send_status_t status) {
// Serial.printf("ESP-NOW Send to %02x... %s\n", mac[5],
// status == ESP_NOW_SEND_SUCCESS ? "OK" : "FAIL");
});
espnowInited = true;
Serial.println("✅ ESP-NOW 已启用");
}
// ✅ 新增：注册/更新从机（MAC+IP 映射）
bool registerSlave(const String& macStr, const String& ipStr) {
if (!espnowInited) initEspNow();
uint8_t mac[6];
if (!parseMacString(macStr, mac)) return false;
IPAddress ip;
if (!ip.fromString(ipStr)) return false;
// 检查 MAC 是否已存在 → 更新 IP
for (int i = 0; i < slaveCount; i++) {
if (slaves[i].active && memcmp(slaves[i].mac, mac, 6) == 0) {
slaves[i].ip = ip;
Serial.printf("🔄 更新从机: MAC=%s IP=%s\n", macStr.c_str(), ipStr.c_str());
return true;
}
}
// 新从机 → 添加到列表
if (slaveCount >= MAX_SLAVES) {
Serial.println("⚠️ 从机列表已满，无法添加");
return false;
}
SlaveNode* node = &slaves[slaveCount++];
memcpy(node->mac, mac, 6);
node->ip = ip;
node->active = true;
// 注册到 ESP-NOW 对等列表
esp_now_peer_info_t peerInfo = {};
memcpy(peerInfo.peer_addr, mac, 6);
peerInfo.channel = 0; // 自动使用 Wi-Fi 当前信道
peerInfo.encrypt = false;
if (esp_now_add_peer(&peerInfo) == ESP_OK) {
Serial.printf("✅ 注册从机: MAC=%s IP=%s ( #%d )\n",
macStr.c_str(), ipStr.c_str(), slaveCount);
return true;
} else {
Serial.println("❌ ESP-NOW 添加对等节点失败");
slaveCount--; // 回滚
return false;
}
}
// ✅ 新增：根据 IP 查找从机 MAC
uint8_t* findSlaveByIP(const String& ipStr) {
IPAddress target;
if (!target.fromString(ipStr)) return nullptr;
for (int i = 0; i < slaveCount; i++) {
if (slaves[i].active && slaves[i].ip == target) {
return slaves[i].mac;
}
}
return nullptr;
}
// ✅ 新增：ESP-NOW 发送（内部使用）
bool sendEspNow(const uint8_t* mac, const char* data, int len) {
if (!espnowInited) initEspNow();
if (!mac || !data || len <= 0 || len > 250) return false; // ESP-NOW 最大 250 字节
Serial.printf("📶 信道：%d\n", WiFi.channel());
/* int sendreslut=esp_now_send(mac, (uint8_t*)data, len);
Serial.printf("📶 Wi-Fi 模式：%s\n",
WiFi.getMode() == WIFI_AP ? "AP" :
WiFi.getMode() == WIFI_STA ? "STA" : "AP+STA");
Serial.printf("📶 信道：%d\n", WiFi.channel());
Serial.printf("📶 状态：%d\n", WiFi.status());
Serial.printf("📶 ESP-NOW：%s\n", espnowInited ? "已启用" : "未启用");
Serial.printf("ESP-NOW 发送, 信道：%d, mac:%s:%s, 状态:0x%X\n",WiFi.channel(), mac, data, sendreslut); */
return esp_now_send(mac, (uint8_t*)data, len)== ESP_OK;
}
// ✅ 修正后的轻量维护
void autoMaintain() {
if (!is_initialized) return;
if (millis() - last_maintenance < MAINTENANCE_INTERVAL) return;
last_maintenance = millis();
if (is_ap_mode) {
if (!ready) {
ready = (udp.begin(local_port) == 1);
if (ready) Serial.printf("✅ AP模式UDP就绪: 端口=%d\n", local_port);
}
return;
}
if (WiFi.status() != WL_CONNECTED) {
WiFi.config(local_ip, gateway, subnet);
if (!ready) {
Serial.printf("start local net 正在启动网络连接！！！\n");
WiFi.begin(ssid, password);
delay(1000);
} else {
Serial.printf("reconnect 网络中断，恢复中！！！\n");
WiFi.reconnect();
}
return;
}
if (!ready) {
ready = (udp.begin(local_port) == 1);
if (ready) {
Serial.printf("✅ 网络就绪: IP=%s 端口=%d\n",
WiFi.localIP().toString().c_str(), local_port);
// ✅ 网络就绪后初始化 ESP-NOW
//initEspNow();
} else {
return;
}
}
}
public:
// ✅ STA模式连接
bool connect(const char* _ssid, const char* _pwd,
uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
uint8_t local_ip_last, uint16_t port = 8888) {
WiFi.mode(WIFI_STA);
WiFi.disconnect(true);
last_maintenance = 0;
ready = false;
ssid = _ssid; password = _pwd;
local_ip = IPAddress(gw1, gw2, gw3, local_ip_last);
gateway = IPAddress(gw1, gw2, gw3, gw4);
subnet = IPAddress(255, 255, 255, 0);
local_port = port;
is_initialized = true;
is_ap_mode = false;
current_ssid = String(_ssid);
current_password = String(_pwd);
cached_gw1 = gw1; cached_gw2 = gw2; cached_gw3 = gw3; cached_gw4 = gw4;
Serial.printf("📶 配置STA网络: IP=%d.%d.%d.%d 网关=%d.%d.%d.%d 端口=%d\n",
gw1, gw2, gw3, local_ip_last, gw1, gw2, gw3, gw4, port);
Serial.println("💡 网络将在后台自动连接...");
autoMaintain();
return true;
}
// ✅ AP模式连接
bool connectAP(const char* _ssid, const char* _pwd, uint16_t port = 8888) {
WiFi.disconnect(true);
WiFi.mode(WIFI_AP);
last_maintenance = 0;
ready = false;
WiFi.softAPConfig(IPAddress(192, 168, 4, 1),
IPAddress(192, 168, 4, 1),
IPAddress(255, 255, 255, 0));
if (!WiFi.softAP(_ssid, _pwd, 1, false, 10)) {
Serial.println("❌ AP启动失败");
return false;
}
local_port = port;
ready = (udp.begin(local_port) == 1);
is_initialized = true;
is_ap_mode = true;
current_ssid = String(_ssid);
current_password = String(_pwd);
cached_gw1 = 192; cached_gw2 = 168; cached_gw3 = 4; cached_gw4 = 1;
Serial.printf("✅ AP模式启动: 热点=\"%s\" 密码=\"%s\"\n", _ssid, _pwd);
Serial.printf("🌐 本机IP: %s | 端口: %d | 客户端: %d/%d\n",
WiFi.softAPIP().toString().c_str(), port,
WiFi.softAPgetStationNum(), 10);
// ✅ AP模式也初始化 ESP-NOW
initEspNow();
return true;
}
// ✅ 向小车发送配置
void sendConfigToDevice(uint8_t car_last_octet, uint8_t monitor_last_octet) {
if (!is_initialized) {
Serial.println("❌ 手柄未配置网络，无法发送配置");
return;
}
if (car_last_octet == cached_gw4 || car_last_octet == 0 || car_last_octet == 255) {
Serial.println("❌ 无效的小车编号（需1~254，且不能与网关相同）");
return;
}
uint8_t local_last = is_ap_mode ? 1 : local_ip[3];
if(monitor_last_octet != 255) local_last = monitor_last_octet;
Serial.printf("wificar:%s:%s:%d:%d:%d:%d:%d:%d\nWZHY",
current_ssid.c_str(), current_password.c_str(),
cached_gw1, cached_gw2, cached_gw3, cached_gw4,
car_last_octet, local_last);
Serial1.printf("wificar:%s:%s:%d:%d:%d:%d:%d:%d\nWZHY",
current_ssid.c_str(), current_password.c_str(),
cached_gw1, cached_gw2, cached_gw3, cached_gw4,
car_last_octet, local_last);
Serial.printf("✅ 已配置小车 #%d → IP=%d.%d.%d.%d | 监视器=%d.%d.%d.%d\n",
car_last_octet,
cached_gw1, cached_gw2, cached_gw3, car_last_octet,
cached_gw1, cached_gw2, cached_gw3, local_last);
}
// ✅ 接收：解析 ESPNOW_REG 并自动注册
bool hasMessage() {
autoMaintain();
if (!is_initialized || !ready) return false;
receivedRaw = "";
parsedValue = "";
int len = udp.parsePacket();
if (len <= 0 || len > 512) return false;
char buf[513];
int read = udp.read((uint8_t*)buf, min(len, 512));
if (read > 0) {
buf[read] = '\0';
receivedRaw = String(buf);
// ✅ 解析 ESPNOW_REG 格式: "mac:AA:BB:CC:DD:EE:FF;ip:192.168.4.6"
if (getMessageValue("r")) {
String raw = lastValue(); // "mac:AA:BB:CC:DD:EE:FF;ip:192.168.4.6"
// 提取 MAC
int macStart = raw.indexOf("mac:");
int macEnd = raw.indexOf(";", macStart);
// 提取 IP
int ipStart = raw.indexOf("ip:", macEnd);
if (macStart != -1 && macEnd != -1 && ipStart != -1) {
String macStr = raw.substring(macStart + 4, macEnd); // +4 跳过 "mac:"
String ipStr = raw.substring(ipStart + 3); // +3 跳过 "ip:"
// ✅ 注册/更新从机
registerSlave(macStr, ipStr);
}
}
return true;
}
return false;
}
// ✅ 解析字段（不变）
bool getMessageValue(const String& key) {
parsedValue = "";
if (receivedRaw.length() == 0) return false;
String keyPattern = "\"" + key + "\"";
int startIndex = receivedRaw.indexOf(keyPattern);
if (startIndex == -1) return false;
int colonIndex = receivedRaw.indexOf(":", startIndex);
if (colonIndex == -1) return false;
int valueStart = colonIndex + 1;
while (valueStart < receivedRaw.length() &&
receivedRaw.charAt(valueStart) == ' ') valueStart++;
if (valueStart < receivedRaw.length() && receivedRaw.charAt(valueStart) == '"') {
int valueEnd = receivedRaw.indexOf("\"", valueStart + 1);
if (valueEnd != -1) {
parsedValue = receivedRaw.substring(valueStart + 1, valueEnd);
return true;
}
} else {
int valueEnd = valueStart;
while (valueEnd < receivedRaw.length() &&
receivedRaw.charAt(valueEnd) != ',' &&
receivedRaw.charAt(valueEnd) != '}' &&
receivedRaw.charAt(valueEnd) != ' ') valueEnd++;
while (valueEnd > valueStart && receivedRaw.charAt(valueEnd - 1) == ' ') valueEnd--;
if (valueEnd > valueStart) {
parsedValue = receivedRaw.substring(valueStart, valueEnd);
return true;
}
}
return false;
}
// ✅ 发送：智能路由（UDP/ESP-NOW 自动选择）
bool sendTo(const String& target_ip_str, const String& key, const char* format, ...) {
autoMaintain();
if (!is_initialized || !ready) return false;
IPAddress target_ip;
if (!target_ip.fromString(target_ip_str)) return false;
// ✅ 智能路由：如果目标 IP 是已注册从机，优先使用 ESP-NOW
uint8_t* slaveMac = findSlaveByIP(target_ip_str);
if (slaveMac && espnowInited) {
// 构造 ESP-NOW 消息（保持 JSON 格式，与 UDP 一致）
char content[256];
va_list args;
va_start(args, format);
vsnprintf(content, sizeof(content) - 1, format, args);
va_end(args);
char json[300];
snprintf(json, sizeof(json), "{\"%s\":\"%s\"}", key.c_str(), content);
// ✅ 使用 ESP-NOW 发送（更快、更低延迟）
if (sendEspNow(slaveMac, json, strlen(json))) {
Serial.println("⚠️ ESP-NOW 发送");
delay(10);
return true;
}
// ESP-NOW 失败时，降级使用 UDP（可选）
//Serial.println("⚠️ ESP-NOW 发送失败，降级使用 UDP");
}
// ✅ 默认：使用 UDP 发送
char content[256];
va_list args;
va_start(args, format);
vsnprintf(content, sizeof(content) - 1, format, args);
va_end(args);
char json[300];
snprintf(json, sizeof(json), "{\"%s\":\"%s\"}\r\n", key.c_str(), content);
return udp.beginPacket(target_ip, local_port) &&
udp.write((const uint8_t*)json, strlen(json)) &&
udp.endPacket();
}
// ✅ 积木友好重载
bool sendTo(const String& target_ip_str, const String& key, const String& value) {
return sendTo(target_ip_str, key, "%s", value.c_str());
}
// ✅ 辅助方法
String lastValue() { return parsedValue; }
String lastMessage() { return receivedRaw; }
String getLocalIP() {
autoMaintain();
return is_initialized ?
(is_ap_mode ? WiFi.softAPIP().toString() : WiFi.localIP().toString()) :
"未配置";
}
bool isConnected() {
autoMaintain();
return is_initialized && ready;
}
bool isAPMode() { return is_ap_mode; }
// ✅ 新增：获取已注册从机数量（调试用）
int getRegisteredSlaveCount() { return slaveCount; }
};
extern UDPPeer peer;

60
jxbeye/src/main.cpp
View File

@@ -2,11 +2,11 @@
#include <WiFi.h>
#include <WebServer.h>
#include <AsyncUDP.h> // ✅【改动 1】替换头文件
#include "HardwareSerial.h"
#include "esp_camera.h"
#include "esp_heap_caps.h"
#include <freertos/queue.h>
#include <Preferences.h>
#include <stdarg.h>
#include <esp_now.h>
#include <esp_wifi.h>
@@ -41,8 +41,7 @@ volatile bool has_active_client = false;
// ===== 系统配置 =====
String ssid = "ESP32-S3-Camera";
String password = "12345678";
const uint8_t channel = 4;
const uint8_t max_clients = 10;
const unsigned long WIFI_TIMEOUT_MS = 15000;
const uint16_t WEB_SERVER_PORT = 80;
// ✅【改动 2】修改 UDP 对象定义，移除缓冲区变量 (AsyncUDP 不需要)
@@ -144,7 +143,7 @@ void sendMjpegFrame(WiFiClient &client, camera_fb_t *fb) {
// ✅ 流处理（自适应传输帧率）- 保持阻塞模式
void handleStream() {
has_active_client = true;
Serial.println("✅ 客户端连接，开始推流 (AsyncUDP 后台运行)");
Serial.println("✅ 客户端连接，开始推流");
WiFiClient client = server.client();
if (!client || !client.connected()) {
@@ -313,19 +312,24 @@ bool initCamera() {
void setup() {
Serial.begin(1000000);
delay(1000);
// 锁定 CPU 最高主频，确保流媒体性能
setCpuFrequencyMhz(240);
Serial.printf("⚡ CPU 主频: %d MHz\n", getCpuFrequencyMhz());
Preferences prefs;
prefs.begin("wifi_cfg", false);
String saved_ssid = prefs.getString("ssid", "ESP32-S3-Camera");
String saved_pwd = prefs.getString("pwd", "12345678");
// String saved_ssid = prefs.getString("ssid", "FS");
// String saved_pwd = prefs.getString("pwd", "zcw666666");
String saved_ssid = "FS";
String saved_pwd = "zcw666666";
if (saved_ssid.length() > 0 && saved_ssid.length() <= 32) ssid = saved_ssid;
if (saved_pwd.length() >= 8 && saved_pwd.length() <= 64) password = saved_pwd;
prefs.end();
Serial.println("\n");
Serial.println("╔══════════════════════════════════════════════════════════════════════╗");
Serial.println("║ ESP32-S3 + OV2640 双核并行流系统 v4.0 (AsyncUDP 中断版) ║");
Serial.println("║ ESP32-S3 + OV2640 双核并行流系统 v4.0 (STA 模式) ║");
Serial.println("╚══════════════════════════════════════════════════════════════════════╝");
Serial.println("💡 串口指令: WIFI:名称:密码 (例如: WIFI:LabCam:Secure1234)");
@@ -344,14 +348,26 @@ void setup() {
while (1) delay(1000);
}
Serial.println("\n[2] WiFi 初始化...");
WiFi.softAPConfig(IPAddress(192, 168, 4, 1), IPAddress(192, 168, 4, 1), IPAddress(255, 255, 255, 0));
WiFi.softAP(ssid.c_str(), password.c_str(), channel, false, max_clients);
Serial.print("✅ 热点："); Serial.println(ssid);
Serial.print("🔒 密码："); Serial.println(password);
Serial.print("🌐 IP: "); Serial.println(WiFi.softAPIP());
Serial.printf("🌐 访问：http://%s\n", WiFi.softAPIP().toString().c_str());
Serial.println("\n[2] WiFi 初始化 (STA 模式)...");
WiFi.mode(WIFI_STA);
WiFi.begin(ssid.c_str(), password.c_str());
Serial.printf("📡 连接中: %s", ssid.c_str());
unsigned long startAttempt = millis();
while (WiFi.status() != WL_CONNECTED && millis() - startAttempt < WIFI_TIMEOUT_MS) {
delay(500);
Serial.print(".");
}
if (WiFi.status() == WL_CONNECTED) {
Serial.println("\n✅ WiFi 已连接");
WiFi.setSleep(false);
Serial.print("📶 SSID: "); Serial.println(ssid);
Serial.print("🌐 IP: "); Serial.println(WiFi.localIP());
Serial.printf("🌐 访问：http://%s\n", WiFi.localIP().toString().c_str());
} else {
Serial.println("\n❌ WiFi 连接失败，继续运行 (无网络)");
}
Serial.println("\n[3] 摄像头初始化 (SVGA)...");
if (!initCamera()) {
@@ -387,7 +403,7 @@ void setup() {
"<body>"
"<header>"
"<h1>📸 局域物联网-" + String(ssid) + "</h1>"
"<div class=\"subtitle\">ESP32-S3 双核并行摄像头 (AsyncUDP)</div>"
"<div class=\"subtitle\">ESP32-S3 双核并行摄像头 (STA 模式)</div>"
"</header>"
"<div class=\"camera-container\">"
@@ -395,7 +411,7 @@ void setup() {
"</div>"
"<div class=\"footer\">"
"<p>ESP32-S3 双核流系统 | 推流阻塞不影响 UDP 中断</p>"
"<p>ESP32-S3 双核流系统 | STA 模式</p>"
"</div>"
"<script>"
@@ -418,7 +434,7 @@ void setup() {
char json_buffer[180];
snprintf(json_buffer, sizeof(json_buffer),
"{\"clients\":%d,\"capture_fps\":%.1f,\"transmit_fps\":%.1f,\"failed\":%lu,\"free_psram\":%lu,\"has_client\":%s}",
WiFi.softAPgetStationNum(),
(WiFi.status() == WL_CONNECTED) ? 1 : 0,
capture_fps,
transmit_fps,
failed_frames,
@@ -444,13 +460,13 @@ void setup() {
Serial.println("❌ AsyncUDP 监听失败");
}
Serial.printf("✅ 系统就绪！访问 http://%s\n\n", WiFi.softAPIP().toString().c_str());
Serial.println("💡 提示：UDP 数据现在由中断处理，推流不会卡顿通讯。");
Serial.printf("✅ 系统就绪！访问 http://%s\n\n", WiFi.localIP().toString().c_str());
Serial.println("💡 提示：UDP 数据由中断处理，推流不阻塞通讯。");
}
void loop() {
server.handleClient();
// ✅【改动 5】删除了所有 udpServer.parsePacket() 相关代码
// 现在 UDP 处理完全在 onUdpPacket 回调中自动完成，loop 中无需任何操作

672
jxbeye/src/普通连接版本
View File

@@ -1,672 +0,0 @@
从机端
#include <Arduino.h>
#include <WiFi.h>
#include <WebServer.h>
#include "esp_camera.h"
#include "esp_heap_caps.h"
#include <freertos/queue.h>
#include <Preferences.h>
#include <WiFiUdp.h>
#include <stdarg.h>
#if 1
// ===== UDP通信类（is_initialized 改回普通成员 + 新增访问方法）=====
class UDPPeer {
private:
WiFiUDP udp;
bool ready = false;
uint16_t local_port = 8888;
// 双缓存
String receivedRaw;
String parsedValue;
// 连接参数
const char* ssid = nullptr;
const char* password = nullptr;
IPAddress local_ip;
IPAddress gateway;
IPAddress subnet;
// ✅ 监视器IP最后一位
uint8_t monitor_ip_last = 101;
// ✅ 改回普通成员变量（非静态）
bool is_initialized = false;
uint32_t last_maintenance = 0;
const uint32_t MAINTENANCE_INTERVAL = 1000;
void autoMaintain() {
if (!is_initialized) return;
if (millis() - last_maintenance < MAINTENANCE_INTERVAL) return;
last_maintenance = millis();
//Serial.printf("testA");
if (WiFi.status() != WL_CONNECTED) {
WiFi.config(local_ip, gateway, subnet);
if (!ready)
{
Serial.printf("正在启动网络连接！！！\n");
WiFi.begin(ssid, password);
delay(1000);
}
else
{
Serial.printf("网络中断，恢复中！！！\n");
WiFi.reconnect();
}
//last_maintenance = 0;
return;
}
//Serial.printf("testB");
if (!ready) {
ready = (udp.begin(local_port) == 1);
if (ready) {
Serial.printf("✅ 网络就绪: IP=%s 端口=%d\n",
WiFi.localIP().toString().c_str(), local_port);
} else {
//last_maintenance = 0;
return;
}
}
//last_maintenance = millis();
}
public:
// ✅ 新增：访问 is_initialized 的成员方法
bool isInitialized() {
return is_initialized;
}
// ✅ 设置监视器IP
void setMonitorIPLast(uint8_t last_octet) {
monitor_ip_last = last_octet;
}
// ✅ 获取完整监视器IP
String getMonitorIP() {
return String(gateway[0]) + "." + String(gateway[1]) + "." +
String(gateway[2]) + "." + String(monitor_ip_last);
}
bool connect(const char* _ssid, const char* _pwd,
uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
uint8_t local_ip_last, uint16_t port = 8888) {
WiFi.disconnect(true);
last_maintenance = 0;
ready = false;
ssid = _ssid;
password = _pwd;
local_ip = IPAddress(gw1, gw2, gw3, local_ip_last);
gateway = IPAddress(gw1, gw2, gw3, gw4);
subnet = IPAddress(255, 255, 255, 0);
local_port = port;
is_initialized = true; // ✅ 普通成员赋值
Serial.printf("📶 wifi：%s， pwd：%s，配置网络: IP=%d.%d.%d.%d 网关=%d.%d.%d.%d 端口=%d\n",
ssid, password, gw1, gw2, gw3, local_ip_last, gw1, gw2, gw3, gw4, port);
Serial.println("💡 网络将在后台自动连接...");
autoMaintain();
return true;
}
bool hasMessage() {
autoMaintain();
if (!is_initialized || !ready) return false;
receivedRaw = "";
parsedValue = "";
int len = udp.parsePacket();
if (len <= 0 || len > 512) return false;
char buf[513];
int read = udp.read((uint8_t*)buf, min(len, 512));
if (read > 0) {
buf[read] = '\0';
receivedRaw = String(buf);
return true;
}
return false;
}
bool getMessageValue(const String& key) {
parsedValue = "";
if (receivedRaw.length() == 0) return false;
String keyPattern = "\"" + key + "\"";
int startIndex = receivedRaw.indexOf(keyPattern);
if (startIndex == -1) return false;
int colonIndex = receivedRaw.indexOf(":", startIndex);
if (colonIndex == -1) return false;
int valueStart = colonIndex + 1;
while (valueStart < receivedRaw.length() &&
receivedRaw.charAt(valueStart) == ' ') valueStart++;
if (valueStart < receivedRaw.length() && receivedRaw.charAt(valueStart) == '"') {
int valueEnd = receivedRaw.indexOf("\"", valueStart + 1);
if (valueEnd != -1) {
parsedValue = receivedRaw.substring(valueStart + 1, valueEnd);
return true;
}
} else {
int valueEnd = valueStart;
while (valueEnd < receivedRaw.length() &&
receivedRaw.charAt(valueEnd) != ',' &&
receivedRaw.charAt(valueEnd) != '}' &&
receivedRaw.charAt(valueEnd) != ' ') valueEnd++;
while (valueEnd > valueStart && receivedRaw.charAt(valueEnd - 1) == ' ') valueEnd--;
if (valueEnd > valueStart) {
parsedValue = receivedRaw.substring(valueStart, valueEnd);
return true;
}
}
return false;
}
bool sendTo(const String& target_ip_str, const String& key, const char* format, ...) {
autoMaintain();
if (!is_initialized || !ready) return false;
IPAddress target_ip;
if (!target_ip.fromString(target_ip_str)) return false;
char content[256];
va_list args;
va_start(args, format);
vsnprintf(content, sizeof(content) - 1, format, args);
va_end(args);
char json[300];
snprintf(json, sizeof(json), "{\"%s\":\"%s\"}\r\n", key.c_str(), content);
return udp.beginPacket(target_ip, local_port) &&
udp.write((const uint8_t*)json, strlen(json)) &&
udp.endPacket();
}
bool sendTo(const String& target_ip_str, const String& key, const String& value) {
return sendTo(target_ip_str, key, "%s", value.c_str());
}
String lastValue() { return parsedValue; }
String lastMessage() { return receivedRaw; }
String getLocalIP() {
autoMaintain();
return is_initialized ? WiFi.localIP().toString() : "未配置";
}
bool isConnected() {
autoMaintain();
return is_initialized && ready;
}
};
extern UDPPeer peer;
UDPPeer peer;
// ===== 默认配置 =====
#define DEFAULT_SSID "ESP32-Camera"
#define DEFAULT_PWD "12345678"
#define DEFAULT_GW1 192
#define DEFAULT_GW2 168
#define DEFAULT_GW3 4
#define DEFAULT_GW4 1
#define DEFAULT_LOCAL_LAST 100
#define DEFAULT_MONITOR_LAST 101
// ===== Flash配置管理 =====
void loadConfigFromFlash() {
Preferences prefs;
if (!prefs.begin("cam_cfg", true)) { // 只读模式
Serial.println("⚠️ Flash配置不存在，使用默认值");
peer.connect(DEFAULT_SSID, DEFAULT_PWD,
DEFAULT_GW1, DEFAULT_GW2, DEFAULT_GW3, DEFAULT_GW4,
DEFAULT_LOCAL_LAST);
peer.setMonitorIPLast(DEFAULT_MONITOR_LAST);
prefs.end();
return;
}
// 读取配置
String ssid = prefs.getString("ssid", DEFAULT_SSID);
String pwd = prefs.getString("pwd", DEFAULT_PWD);
uint8_t gw1 = prefs.getUChar("gw1", DEFAULT_GW1);
uint8_t gw2 = prefs.getUChar("gw2", DEFAULT_GW2);
uint8_t gw3 = prefs.getUChar("gw3", DEFAULT_GW3);
uint8_t gw4 = prefs.getUChar("gw4", DEFAULT_GW4);
uint8_t local_last = prefs.getUChar("local", DEFAULT_LOCAL_LAST);
uint8_t monitor_last = prefs.getUChar("monitor", DEFAULT_MONITOR_LAST);
prefs.end();
// 应用配置
peer.connect(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last);
peer.setMonitorIPLast(monitor_last);
Serial.println("✅ 从Flash加载配置成功");
}
void saveConfigToFlash(const char* ssid, const char* pwd,
uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
uint8_t local_last, uint8_t monitor_last) {
Preferences prefs;
if (!prefs.begin("cam_cfg", false)) { // 读写模式
Serial.println("❌ Flash配置保存失败");
return;
}
prefs.putString("ssid", ssid);
prefs.putString("pwd", pwd);
prefs.putUChar("gw1", gw1);
prefs.putUChar("gw2", gw2);
prefs.putUChar("gw3", gw3);
prefs.putUChar("gw4", gw4);
prefs.putUChar("local", local_last);
prefs.putUChar("monitor", monitor_last);
prefs.end();
Serial.println("✅ 配置已保存到Flash");
}
// ===== 串口指令解析 =====
void handleSerialCommand() {
if (!Serial.available()) return;
String cmd = Serial.readStringUntil('\n');
cmd.trim();
if (!cmd.startsWith("wificar:"))
{
if(cmd.endsWith("LZU")||cmd.endsWith("LZU\r"))
{
peer.sendTo(peer.getMonitorIP(), "f", cmd);
Serial.printf("📤 已发送到 %s: {\"f\":%s}\n", peer.getMonitorIP().c_str(), cmd);
}
return;
}
// 解析格式: wificar:ssid:pwd:gw1:gw2:gw3:gw4:local_last:monitor_last
// 示例: wificar:MyCam:secure123:192:168:43:1:100:101
int pos[7]; // 存储6个冒号位置
int count = 0;
for (int i = 8; i < cmd.length() && count < 7; i++) { // 从"wificar:"后开始
if (cmd.charAt(i) == ':') {
pos[count++] = i;
}
}
if (count < 7) {
Serial.println("❌ 指令格式错误，应为: wificar:ssid:pwd:gw1:gw2:gw3:gw4:local_last:monitor_last");
return;
}
String ssid = cmd.substring(8, pos[0]);
String pwd = cmd.substring(pos[0] + 1, pos[1]);
uint8_t gw1 = cmd.substring(pos[1] + 1, pos[2]).toInt();
uint8_t gw2 = cmd.substring(pos[2] + 1, pos[3]).toInt();
uint8_t gw3 = cmd.substring(pos[3] + 1, pos[4]).toInt();
uint8_t gw4 = cmd.substring(pos[4] + 1, pos[5]).toInt();
uint8_t local_last = cmd.substring(pos[5] + 1, pos[6]).toInt();
uint8_t monitor_last = cmd.substring(pos[6] + 1).toInt();
// 应用新配置
peer.connect(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last);
peer.setMonitorIPLast(monitor_last);
// 保存到Flash
saveConfigToFlash(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last, monitor_last);
Serial.println("✅ WiFi配置已更新并保存");
}
void setup() {
Serial.begin(115200);
delay(1000);
Serial.println("\n╔════════════════════════════════════════════════════════════════╗");
Serial.println("║ ESP32-S3 摄像头 UDP 通信系统 v1.0 ║");
Serial.println("║ 支持Flash配置存储 | 串口动态配置 | 自动重连 ║");
Serial.println("╚════════════════════════════════════════════════════════════════╝");
Serial.println("💡 串口指令:wificar:ssid:pwd:gw1:gw2:gw3:gw4:local_last:monitor_last");
Serial.println(" 示例: wificar:MyCam:secure123:192:168:43:1:100:101\n");
// 从Flash加载配置（自动回退到默认值）
loadConfigFromFlash();
}
void loop() {
// 处理串口配置指令
handleSerialCommand();
// ✅ 使用成员方法读取 is_initialized 状态
if (peer.isInitialized()) {
// 每2秒发送一次消息到监视器
// static uint32_t last_send = 0;
// if (millis() - last_send >= 2000) {
// last_send = millis();
// peer.sendTo(peer.getMonitorIP(), "f", "device");
// Serial.printf("📤 已发送到 %s: {\"f\":\"start\"}\n", peer.getMonitorIP().c_str());
// }
// 每5ms检查一次消息
static uint32_t last_check = 0;
if (millis() - last_check >= 6) {
last_check = millis();
if (peer.hasMessage()) {
if (peer.getMessageValue("c")) {
Serial.printf("📥 收到消息: %sZHY\n", peer.lastValue().c_str());
}
}
}
}
delay(1); // 避免忙等待
}
#else
主机端
#include <WiFi.h>
#include <WiFiUdp.h>
#include <stdarg.h>
class UDPPeer {
private:
WiFiUDP udp;
bool ready = false;
uint16_t local_port = 8888;
// 双缓存
String receivedRaw;
String parsedValue;
// 连接参数
const char* ssid = nullptr;
const char* password = nullptr;
IPAddress local_ip;
IPAddress gateway;
IPAddress subnet;
// ✅ 新增：AP模式标志（默认false=STA模式）
bool is_ap_mode = false;
// ✅ ✅ ✅ 新增：网络配置缓存（6行） ✅ ✅ ✅
String current_ssid = "";
String current_password = "";
uint8_t cached_gw1 = 0, cached_gw2 = 0, cached_gw3 = 0, cached_gw4 = 0;
// 状态标志
bool is_initialized = false;
uint32_t last_maintenance = 0;
const uint32_t MAINTENANCE_INTERVAL = 1000;
// ✅ 修正后的轻量维护（AP模式特殊处理）
void autoMaintain() {
if (!is_initialized) return;
if (millis() - last_maintenance < MAINTENANCE_INTERVAL) return;
last_maintenance = millis();
// ✅ AP模式：无需维护WiFi（始终在线），仅确保UDP就绪
if (is_ap_mode) {
if (!ready) {
ready = (udp.begin(local_port) == 1);
if (ready) {
Serial.printf("✅ AP模式UDP就绪: 端口=%d\n", local_port);
}
}
//last_maintenance = millis();
return;
}
if (WiFi.status() != WL_CONNECTED) {
WiFi.config(local_ip, gateway, subnet);
if (!ready)
{
Serial.printf("start local net正在启动网络连接！！！\n");
WiFi.begin(ssid, password);
delay(1000);
}
else
{
Serial.printf("reconnect网络中断，恢复中！！！\n");
WiFi.reconnect();
}
//last_maintenance = 0;
return;
}
if (!ready) {
ready = (udp.begin(local_port) == 1);
if (ready) {
Serial.printf("✅ 网络就绪: IP=%s 端口=%d\n",
WiFi.localIP().toString().c_str(), local_port);
} else {
//last_maintenance = 0;
return;
}
}
//last_maintenance = millis();
}
public:
// ✅ STA模式连接（末尾添加配置缓存 - 6行）
bool connect(const char* _ssid, const char* _pwd,
uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
uint8_t local_ip_last, uint16_t port = 8888) {
// 切换到STA模式
WiFi.mode(WIFI_STA);
WiFi.disconnect(true);
last_maintenance = 0;
ready = false;
ssid = _ssid;
password = _pwd;
local_ip = IPAddress(gw1, gw2, gw3, local_ip_last);
gateway = IPAddress(gw1, gw2, gw3, gw4);
subnet = IPAddress(255, 255, 255, 0);
local_port = port;
is_initialized = true;
is_ap_mode = false;
// ✅ ✅ ✅ 新增：缓存网络配置 ✅ ✅ ✅
current_ssid = String(_ssid);
current_password = String(_pwd);
cached_gw1 = gw1;
cached_gw2 = gw2;
cached_gw3 = gw3;
cached_gw4 = gw4;
Serial.printf("📶 配置STA网络: IP=%d.%d.%d.%d 网关=%d.%d.%d.%d 端口=%d\n",
gw1, gw2, gw3, local_ip_last, gw1, gw2, gw3, gw4, port);
Serial.println("💡 网络将在后台自动连接...");
autoMaintain();
return true;
}
// ✅ ✅ ✅ AP模式连接（末尾添加配置缓存 - 6行）✅ ✅ ✅
bool connectAP(const char* _ssid, const char* _pwd, uint16_t port = 8888) {
WiFi.disconnect(true);
WiFi.mode(WIFI_AP);
last_maintenance = 0;
ready = false;
WiFi.softAPConfig(IPAddress(192, 168, 4, 1),
IPAddress(192, 168, 4, 1),
IPAddress(255, 255, 255, 0));
if (!WiFi.softAP(_ssid, _pwd, 4, false, 4)) {
Serial.println("❌ AP启动失败");
return false;
}
local_port = port;
ready = (udp.begin(local_port) == 1);
is_initialized = true;
is_ap_mode = true;
// ✅ ✅ ✅ 新增：缓存AP模式固定配置 ✅ ✅ ✅
current_ssid = String(_ssid);
current_password = String(_pwd);
cached_gw1 = 192;
cached_gw2 = 168;
cached_gw3 = 4;
cached_gw4 = 1;
Serial.printf("✅ AP模式启动: 热点=\"%s\" 密码=\"%s\"\n", _ssid, _pwd);
Serial.printf("🌐 本机IP: %s | 端口: %d | 客户端: %d/%d\n",
WiFi.softAPIP().toString().c_str(), port,
WiFi.softAPgetStationNum(), 4);
return true;
}
// ✅ ✅ ✅ 核心：向小车发送配置（仅需IP最后1段 - 20行）✅ ✅ ✅
void sendConfigToDevice(uint8_t car_last_octet, uint8_t monitor_last_octet) {
if (!is_initialized) {
Serial.println("❌ 手柄未配置网络，无法发送配置");
return;
}
if (car_last_octet == cached_gw4 || car_last_octet == 0 || car_last_octet == 255) {
Serial.println("❌ 无效的小车编号（需1~254，且不能与网关相同）");
return;
}
// 获取本机IP最后1段（AP模式固定为1，STA模式取local_ip[3]）
uint8_t local_last = is_ap_mode ? 1 : local_ip[3];
if(monitor_last_octet!=255) local_last =monitor_last_octet;
// 发送标准配置指令（与终端handleSerialCommand完全兼容）
Serial.printf("wificar:%s:%s:%d:%d:%d:%d:%d:%d\nWZHY",
current_ssid.c_str(),
current_password.c_str(),
cached_gw1, cached_gw2, cached_gw3, cached_gw4,
car_last_octet, // 小车本机IP最后1段
local_last); // 监视器IP最后1段（指向本机）
Serial1.printf("wificar:%s:%s:%d:%d:%d:%d:%d:%d\nWZHY",
current_ssid.c_str(),
current_password.c_str(),
cached_gw1, cached_gw2, cached_gw3, cached_gw4,
car_last_octet, // 小车本机IP最后1段
local_last); // 监视器IP最后1段（指向本机）
Serial.printf("✅ 已配置小车 #%d → IP=%d.%d.%d.%d | 监视器=%d.%d.%d.%d\n",
car_last_octet,
cached_gw1, cached_gw2, cached_gw3, car_last_octet,
cached_gw1, cached_gw2, cached_gw3, local_last);
}
// ✅ 接收：轻量维护 + 纯检查（自动适配AP/STA）
bool hasMessage() {
autoMaintain();
if (!is_initialized || !ready) return false;
receivedRaw = "";
parsedValue = "";
int len = udp.parsePacket();
if (len <= 0 || len > 512) return false;
char buf[513];
int read = udp.read((uint8_t*)buf, min(len, 512));
if (read > 0) {
buf[read] = '\0';
receivedRaw = String(buf);
// ✅【修复 1】补全右括号 + 简化 this->
// ✅【修复 2】类内调用直接写 lastValue()
if (getMessageValue("ESPNOW_REG")) {
Serial.println(lastValue());
}
return true;
}
return false;
}
// ✅ 解析字段（不变）
bool getMessageValue(const String& key) {
parsedValue = "";
if (receivedRaw.length() == 0) return false;
String keyPattern = "\"" + key + "\"";
int startIndex = receivedRaw.indexOf(keyPattern);
if (startIndex == -1) return false;
int colonIndex = receivedRaw.indexOf(":", startIndex);
if (colonIndex == -1) return false;
int valueStart = colonIndex + 1;
while (valueStart < receivedRaw.length() &&
receivedRaw.charAt(valueStart) == ' ') valueStart++;
if (valueStart < receivedRaw.length() && receivedRaw.charAt(valueStart) == '"') {
int valueEnd = receivedRaw.indexOf("\"", valueStart + 1);
if (valueEnd != -1) {
parsedValue = receivedRaw.substring(valueStart + 1, valueEnd);
return true;
}
} else {
int valueEnd = valueStart;
while (valueEnd < receivedRaw.length() &&
receivedRaw.charAt(valueEnd) != ',' &&
receivedRaw.charAt(valueEnd) != '}' &&
receivedRaw.charAt(valueEnd) != ' ') valueEnd++;
while (valueEnd > valueStart && receivedRaw.charAt(valueEnd - 1) == ' ') valueEnd--;
if (valueEnd > valueStart) {
parsedValue = receivedRaw.substring(valueStart, valueEnd);
return true;
}
}
return false;
}
// ✅ 发送：轻量维护 + 直接发送（自动适配AP/STA）
bool sendTo(const String& target_ip_str, const String& key, const char* format, ...) {
autoMaintain();
if (!is_initialized || !ready) return false;
IPAddress target_ip;
if (!target_ip.fromString(target_ip_str)) return false;
char content[256];
va_list args;
va_start(args, format);
vsnprintf(content, sizeof(content) - 1, format, args);
va_end(args);
char json[300];
snprintf(json, sizeof(json), "{\"%s\":\"%s\"}\r\n", key.c_str(), content);
return udp.beginPacket(target_ip, local_port) &&
udp.write((const uint8_t*)json, strlen(json)) &&
udp.endPacket();
}
// ✅ 积木友好重载
bool sendTo(const String& target_ip_str, const String& key, const String& value) {
return sendTo(target_ip_str, key, "%s", value.c_str());
}
// ✅ 辅助方法：自动返回正确IP（AP/STA自适应）
String lastValue() { return parsedValue; }
String lastMessage() { return receivedRaw; }
String getLocalIP() {
autoMaintain();
return is_initialized ?
(is_ap_mode ? WiFi.softAPIP().toString() : WiFi.localIP().toString()) :
"未配置";
}
bool isConnected() {
autoMaintain();
return is_initialized && ready;
}
bool isAPMode() { return is_ap_mode; }
};
extern UDPPeer peer;