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

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);
}