diff --git a/jxbeye/.clangd b/jxbeye/.clangd index c0ca8d8..9022a5f 100644 --- a/jxbeye/.clangd +++ b/jxbeye/.clangd @@ -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 diff --git a/jxbeye/lib/FTServo/LICENSE b/jxbeye/lib/FTServo/LICENSE new file mode 100644 index 0000000..f213df3 --- /dev/null +++ b/jxbeye/lib/FTServo/LICENSE @@ -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. diff --git a/jxbeye/lib/FTServo/README.md b/jxbeye/lib/FTServo/README.md new file mode 100644 index 0000000..42fd159 --- /dev/null +++ b/jxbeye/lib/FTServo/README.md @@ -0,0 +1,44 @@ +# FEETECH BUS Servo + +> FEETECH BUS Servo library for Arduino and ESP32 + +## Table of Contents + + +* [FEETECH BUS Servo](#ft-series-servo) + * [Table of Contents](#table-of-contents) + * [Requirements](#requirements) + * [Usage](#usage) + * [Notes](#notes) + * [Release](#release) + + +## 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. diff --git a/jxbeye/lib/FTServo/examples/HLSCL/Broadcast/Broadcast.ino b/jxbeye/lib/FTServo/examples/HLSCL/Broadcast/Broadcast.ino new file mode 100644 index 0000000..a0470ba --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/Broadcast/Broadcast.ino @@ -0,0 +1,22 @@ +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/CalibrationOfs/CalibrationOfs.ino b/jxbeye/lib/FTServo/examples/HLSCL/CalibrationOfs/CalibrationOfs.ino new file mode 100644 index 0000000..6179cf2 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/CalibrationOfs/CalibrationOfs.ino @@ -0,0 +1,28 @@ +/* +中位校准例子 +*/ + +#include + +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); +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/FeedBack/FeedBack.ino b/jxbeye/lib/FTServo/examples/HLSCL/FeedBack/FeedBack.ino new file mode 100644 index 0000000..029352e --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/FeedBack/FeedBack.ino @@ -0,0 +1,148 @@ +/* +回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流； +FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态； +函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态, +无需调用FeedBack函数。 +*/ + +#include + +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(); +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/Ping/Ping.ino b/jxbeye/lib/FTServo/examples/HLSCL/Ping/Ping.ino new file mode 100644 index 0000000..dc01c69 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/Ping/Ping.ino @@ -0,0 +1,34 @@ +/* +Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况 +*/ + +#include + +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); + } +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/ProgramEprom/ProgramEprom.ino b/jxbeye/lib/FTServo/examples/HLSCL/ProgramEprom/ProgramEprom.ino new file mode 100644 index 0000000..6a3d86b --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/ProgramEprom/ProgramEprom.ino @@ -0,0 +1,27 @@ +/* +舵机参数编程 +*/ + +#include + +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() +{ + +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/RegWritePos/RegWritePos.ino b/jxbeye/lib/FTServo/examples/HLSCL/RegWritePos/RegWritePos.ino new file mode 100644 index 0000000..394e0b1 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/RegWritePos/RegWritePos.ino @@ -0,0 +1,26 @@ +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/SyncRead/SyncRead.ino b/jxbeye/lib/FTServo/examples/HLSCL/SyncRead/SyncRead.ino new file mode 100644 index 0000000..e8f9c7b --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/SyncRead/SyncRead.ino @@ -0,0 +1,45 @@ +/* +同步读指令，回读ID1与ID2两个舵机的位置与速度信息 +*/ + +#include + +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 +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/SyncWriteSpe/SyncWriteSpe.ino b/jxbeye/lib/FTServo/examples/HLSCL/SyncWriteSpe/SyncWriteSpe.ino new file mode 100644 index 0000000..cdb59ef --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/SyncWriteSpe/SyncWriteSpe.ino @@ -0,0 +1,50 @@ +#include +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); +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/WriteEle/WriteEle.ino b/jxbeye/lib/FTServo/examples/HLSCL/WriteEle/WriteEle.ino new file mode 100644 index 0000000..1208644 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/WriteEle/WriteEle.ino @@ -0,0 +1,31 @@ +#include + +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); +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/WritePos/WritePos.ino b/jxbeye/lib/FTServo/examples/HLSCL/WritePos/WritePos.ino new file mode 100644 index 0000000..34a872b --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/WritePos/WritePos.ino @@ -0,0 +1,22 @@ +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/HLSCL/WriteSpe/WriteSpe.ino b/jxbeye/lib/FTServo/examples/HLSCL/WriteSpe/WriteSpe.ino new file mode 100644 index 0000000..affe04a --- /dev/null +++ b/jxbeye/lib/FTServo/examples/HLSCL/WriteSpe/WriteSpe.ino @@ -0,0 +1,31 @@ +#include + +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); +} diff --git a/jxbeye/lib/FTServo/examples/SCSCL/Broadcast/Broadcast.ino b/jxbeye/lib/FTServo/examples/SCSCL/Broadcast/Broadcast.ino new file mode 100644 index 0000000..230dafd --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SCSCL/Broadcast/Broadcast.ino @@ -0,0 +1,25 @@ +/* +广播写例子在SCS15中测试通过，如果测试其它型号SCS系列舵机请更改合适的位置、速度与延时参数。 +*/ + +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/SCSCL/FeedBack/FeedBack.ino b/jxbeye/lib/FTServo/examples/SCSCL/FeedBack/FeedBack.ino new file mode 100644 index 0000000..acbcd42 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SCSCL/FeedBack/FeedBack.ino @@ -0,0 +1,147 @@ +/* +回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流； +FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态； +函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态, +无需调用FeedBack函数。 +*/ + +#include + +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(); +} diff --git a/jxbeye/lib/FTServo/examples/SCSCL/Ping/Ping.ino b/jxbeye/lib/FTServo/examples/SCSCL/Ping/Ping.ino new file mode 100644 index 0000000..c2bd887 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SCSCL/Ping/Ping.ino @@ -0,0 +1,33 @@ +/* +Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况 +*/ + +#include + +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); + } +} diff --git a/jxbeye/lib/FTServo/examples/SCSCL/ProgramEprom/ProgramEprom.ino b/jxbeye/lib/FTServo/examples/SCSCL/ProgramEprom/ProgramEprom.ino new file mode 100644 index 0000000..feec65b --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SCSCL/ProgramEprom/ProgramEprom.ino @@ -0,0 +1,28 @@ +/* +舵机参数编程 +*/ + +#include + +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() +{ + +} diff --git a/jxbeye/lib/FTServo/examples/SCSCL/RegWritePos/RegWritePos.ino b/jxbeye/lib/FTServo/examples/SCSCL/RegWritePos/RegWritePos.ino new file mode 100644 index 0000000..b8afec8 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SCSCL/RegWritePos/RegWritePos.ino @@ -0,0 +1,29 @@ +/* +异步写例子在SCS15中测试通过，如果测试其它型号SCS系列舵机请更改合适的位置、速度与延时参数。 +*/ + +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/SCSCL/SyncWritePos/SyncWritePos.ino b/jxbeye/lib/FTServo/examples/SCSCL/SyncWritePos/SyncWritePos.ino new file mode 100644 index 0000000..97b2aec --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SCSCL/SyncWritePos/SyncWritePos.ino @@ -0,0 +1,37 @@ +/* +同步写例子在SCS15中测试通过，如果测试其它型号SCS系列舵机请更改合适的位置、速度与延时参数。 +*/ + +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/SCSCL/WritePWM/WritePWM.ino b/jxbeye/lib/FTServo/examples/SCSCL/WritePWM/WritePWM.ino new file mode 100644 index 0000000..3ffb193 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SCSCL/WritePWM/WritePWM.ino @@ -0,0 +1,34 @@ +/* +电机模式例子 +*/ + +#include + +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); +} diff --git a/jxbeye/lib/FTServo/examples/SCSCL/WritePos/WritePos.ino b/jxbeye/lib/FTServo/examples/SCSCL/WritePos/WritePos.ino new file mode 100644 index 0000000..4270fb0 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SCSCL/WritePos/WritePos.ino @@ -0,0 +1,25 @@ +/* +普通写例子在SCS15中测试通过，如果测试其它型号SCS系列舵机请更改合适的位置、速度与延时参数。 +*/ + +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/Broadcast/Broadcast.ino b/jxbeye/lib/FTServo/examples/SMS_STS/Broadcast/Broadcast.ino new file mode 100644 index 0000000..afdefea --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/Broadcast/Broadcast.ino @@ -0,0 +1,22 @@ +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/CalibrationOfs/CalibrationOfs.ino b/jxbeye/lib/FTServo/examples/SMS_STS/CalibrationOfs/CalibrationOfs.ino new file mode 100644 index 0000000..32985f9 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/CalibrationOfs/CalibrationOfs.ino @@ -0,0 +1,25 @@ +/* +中位校准例子 +*/ + +#include + +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); +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/FeedBack/FeedBack.ino b/jxbeye/lib/FTServo/examples/SMS_STS/FeedBack/FeedBack.ino new file mode 100644 index 0000000..8da3f86 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/FeedBack/FeedBack.ino @@ -0,0 +1,148 @@ +/* +回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流； +FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态； +函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态, +无需调用FeedBack函数。 +*/ + +#include + +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(); +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/Ping/Ping.ino b/jxbeye/lib/FTServo/examples/SMS_STS/Ping/Ping.ino new file mode 100644 index 0000000..b219a50 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/Ping/Ping.ino @@ -0,0 +1,33 @@ +/* +Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况 +*/ + +#include + +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); + } +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/ProgramEprom/ProgramEprom.ino b/jxbeye/lib/FTServo/examples/SMS_STS/ProgramEprom/ProgramEprom.ino new file mode 100644 index 0000000..5233442 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/ProgramEprom/ProgramEprom.ino @@ -0,0 +1,27 @@ +/* +舵机参数编程 +*/ + +#include + +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() +{ + +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/RegWritePos/RegWritePos.ino b/jxbeye/lib/FTServo/examples/SMS_STS/RegWritePos/RegWritePos.ino new file mode 100644 index 0000000..833b30c --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/RegWritePos/RegWritePos.ino @@ -0,0 +1,26 @@ +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/SyncRead/SyncRead.ino b/jxbeye/lib/FTServo/examples/SMS_STS/SyncRead/SyncRead.ino new file mode 100644 index 0000000..9736bed --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/SyncRead/SyncRead.ino @@ -0,0 +1,45 @@ +/* +同步读指令，回读ID1与ID2两个舵机的位置与速度信息 +*/ + +#include + +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 +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/SyncWriteSpe/SyncWriteSpe.ino b/jxbeye/lib/FTServo/examples/SMS_STS/SyncWriteSpe/SyncWriteSpe.ino new file mode 100644 index 0000000..2379daa --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/SyncWriteSpe/SyncWriteSpe.ino @@ -0,0 +1,47 @@ +#include +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); +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/WritePos/WritePos.ino b/jxbeye/lib/FTServo/examples/SMS_STS/WritePos/WritePos.ino new file mode 100644 index 0000000..db13e98 --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/WritePos/WritePos.ino @@ -0,0 +1,22 @@ +#include + +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(误差) +} diff --git a/jxbeye/lib/FTServo/examples/SMS_STS/WriteSpe/WriteSpe.ino b/jxbeye/lib/FTServo/examples/SMS_STS/WriteSpe/WriteSpe.ino new file mode 100644 index 0000000..c47291f --- /dev/null +++ b/jxbeye/lib/FTServo/examples/SMS_STS/WriteSpe/WriteSpe.ino @@ -0,0 +1,31 @@ +#include + +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); +} diff --git a/jxbeye/lib/FTServo/library.properties b/jxbeye/lib/FTServo/library.properties new file mode 100644 index 0000000..a9e2e81 --- /dev/null +++ b/jxbeye/lib/FTServo/library.properties @@ -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 +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=* diff --git a/jxbeye/lib/FTServo/src/HLSCL.cpp b/jxbeye/lib/FTServo/src/HLSCL.cpp new file mode 100644 index 0000000..fe8230a --- /dev/null +++ b/jxbeye/lib/FTServo/src/HLSCL.cpp @@ -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 +#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; i10){ + 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=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<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() +{ +} \ No newline at end of file diff --git a/jxbeye/lib/FTServo/src/SCSerial.h b/jxbeye/lib/FTServo/src/SCSerial.h new file mode 100644 index 0000000..402b262 --- /dev/null +++ b/jxbeye/lib/FTServo/src/SCSerial.h @@ -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 \ No newline at end of file diff --git a/jxbeye/lib/FTServo/src/SCServo.h b/jxbeye/lib/FTServo/src/SCServo.h new file mode 100644 index 0000000..090dea6 --- /dev/null +++ b/jxbeye/lib/FTServo/src/SCServo.h @@ -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 \ No newline at end of file diff --git a/jxbeye/lib/FTServo/src/SMS_STS.cpp b/jxbeye/lib/FTServo/src/SMS_STS.cpp new file mode 100644 index 0000000..d79e919 --- /dev/null +++ b/jxbeye/lib/FTServo/src/SMS_STS.cpp @@ -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_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-- 格式 - 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); -} \ No newline at end of file diff --git a/jxbeye/src/bkespnow版本 b/jxbeye/src/bkespnow版本 deleted file mode 100644 index 45a3471..0000000 --- a/jxbeye/src/bkespnow版本 +++ /dev/null @@ -1,911 +0,0 @@ -#include -#include -#include -#include "esp_camera.h" -#include "esp_heap_caps.h" -#include -#include -#include -#include -#include // ✅【改动 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 -#include -#include -#include // ✅ 新增：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; \ No newline at end of file diff --git a/jxbeye/src/main.cpp b/jxbeye/src/main.cpp index 42e83d4..a718044 100644 --- a/jxbeye/src/main.cpp +++ b/jxbeye/src/main.cpp @@ -2,11 +2,11 @@ #include #include #include // ✅【改动 1】替换头文件 +#include "HardwareSerial.h" #include "esp_camera.h" #include "esp_heap_caps.h" #include #include -#include #include #include @@ -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() { "" "
" "

📸 局域物联网-" + String(ssid) + "

" - "
ESP32-S3 双核并行摄像头 (AsyncUDP)
" + "
ESP32-S3 双核并行摄像头 (STA 模式)
" "
" "
" @@ -395,7 +411,7 @@ void setup() { "
" "
" - "

ESP32-S3 双核流系统 | 推流阻塞不影响 UDP 中断

" + "

ESP32-S3 双核流系统 | STA 模式

" "
" "