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

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