Skip to content

Commit beea82d

Browse files
ftservoftservo
committed
examples
1 parent 7c0f719 commit beea82d

31 files changed

Lines changed: 1323 additions & 0 deletions

File tree

‎examples/HLSCL/Broadcast/Broadcast.ino‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#include <SCServo.h>
2+
3+
HLSCL hlscl;
4+
5+
void setup()
6+
{
7+
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
8+
Serial1.begin(1000000);//mega2560
9+
hlscl.pSerial = &Serial1;
10+
delay(1000);
11+
}
12+
13+
void loop()
14+
{
15+
//舵机(广播)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P1=4095位置
16+
hlscl.WritePosEx(0xfe, 4095, 60, 50, 500);
17+
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
18+
19+
//舵机(广播)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P0=0位置
20+
hlscl.WritePosEx(0xfe, 0, 60, 50, 500);
21+
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
22+
}

‎examples/HLSCL/CalibrationOfs/CalibrationOfs.ino‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
/*
2+
中位校准例子
3+
*/
4+
5+
#include <SCServo.h>
6+
7+
HLSCL hlscl;
8+
9+
void setup()
10+
{
11+
Serial.begin(115200);
12+
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
13+
Serial1.begin(1000000);//mega2560
14+
hlscl.pSerial = &Serial1;
15+
delay(1000);
16+
hlscl.CalibrationOfs(1);
17+
delay(10);
18+
}
19+
20+
void loop()
21+
{
22+
int pos = hlscl.ReadPos(1);
23+
if(!hlscl.getLastError()){
24+
Serial.print("mid pos:");
25+
Serial.println(pos);
26+
}
27+
delay(1000);
28+
}

‎examples/HLSCL/FeedBack/FeedBack.ino‎

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
/*
2+
回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流；
3+
FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态；
4+
函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态,
5+
无需调用FeedBack函数。
6+
*/
7+
8+
#include <SCServo.h>
9+
10+
HLSCL hlscl;
11+
int LEDpin = 13;
12+
13+
void setup()
14+
{
15+
pinMode(LEDpin,OUTPUT);
16+
digitalWrite(LEDpin, HIGH);
17+
Serial1.begin(1000000);//mega2560
18+
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s32
19+
Serial.begin(115200);
20+
hlscl.pSerial = &Serial1;
21+
delay(1000);
22+
}
23+
24+
void loop()
25+
{
26+
int Pos;
27+
int Speed;
28+
int Load;
29+
int Voltage;
30+
int Temper;
31+
int Move;
32+
int Current;
33+
hlscl.FeedBack(1);
34+
if(!hlscl.getLastError()){
35+
digitalWrite(LEDpin, LOW);
36+
Pos = hlscl.ReadPos(-1);
37+
Speed = hlscl.ReadSpeed(-1);
38+
Load = hlscl.ReadLoad(-1);
39+
Voltage = hlscl.ReadVoltage(-1);
40+
Temper = hlscl.ReadTemper(-1);
41+
Move = hlscl.ReadMove(-1);
42+
Current = hlscl.ReadCurrent(-1);
43+
Serial.print("Position:");
44+
Serial.println(Pos);
45+
Serial.print("Speed:");
46+
Serial.println(Speed);
47+
Serial.print("Load:");
48+
Serial.println(Load);
49+
Serial.print("Voltage:");
50+
Serial.println(Voltage);
51+
Serial.print("Temper:");
52+
Serial.println(Temper);
53+
Serial.print("Move:");
54+
Serial.println(Move);
55+
Serial.print("Current:");
56+
Serial.println(Current);
57+
delay(10);
58+
}else{
59+
digitalWrite(LEDpin, HIGH);
60+
Serial.println("FeedBack err");
61+
delay(500);
62+
}
63+
64+
Pos = hlscl.ReadPos(1);
65+
if(!hlscl.getLastError()){
66+
digitalWrite(LEDpin, LOW);
67+
Serial.print("Servo position:");
68+
Serial.println(Pos, DEC);
69+
delay(10);
70+
}else{
71+
Serial.println("read position err");
72+
digitalWrite(LEDpin, HIGH);
73+
delay(500);
74+
}
75+
76+
Voltage = hlscl.ReadVoltage(1);
77+
if(!hlscl.getLastError()){
78+
digitalWrite(LEDpin, LOW);
79+
Serial.print("Servo Voltage:");
80+
Serial.println(Voltage, DEC);
81+
delay(10);
82+
}else{
83+
Serial.println("read Voltage err");
84+
digitalWrite(LEDpin, HIGH);
85+
delay(500);
86+
}
87+
88+
Temper = hlscl.ReadTemper(1);
89+
if(!hlscl.getLastError()){
90+
digitalWrite(LEDpin, LOW);
91+
Serial.print("Servo temperature:");
92+
Serial.println(Temper, DEC);
93+
delay(10);
94+
}else{
95+
Serial.println("read temperature err");
96+
digitalWrite(LEDpin, HIGH);
97+
delay(500);
98+
}
99+
100+
Speed = hlscl.ReadSpeed(1);
101+
if(!hlscl.getLastError()){
102+
digitalWrite(LEDpin, LOW);
103+
Serial.print("Servo Speed:");
104+
Serial.println(Speed, DEC);
105+
delay(10);
106+
}else{
107+
Serial.println("read Speed err");
108+
digitalWrite(LEDpin, HIGH);
109+
delay(500);
110+
}
111+
112+
Load = hlscl.ReadLoad(1);
113+
if(!hlscl.getLastError()){
114+
digitalWrite(LEDpin, LOW);
115+
Serial.print("Servo Load:");
116+
Serial.println(Load, DEC);
117+
delay(10);
118+
}else{
119+
Serial.println("read Load err");
120+
digitalWrite(LEDpin, HIGH);
121+
delay(500);
122+
}
123+
124+
Current = hlscl.ReadCurrent(1);
125+
if(!hlscl.getLastError()){
126+
digitalWrite(LEDpin, LOW);
127+
Serial.print("Servo Current:");
128+
Serial.println(Current, DEC);
129+
delay(10);
130+
}else{
131+
Serial.println("read Current err");
132+
digitalWrite(LEDpin, HIGH);
133+
delay(500);
134+
}
135+
136+
Move = hlscl.ReadMove(1);
137+
if(!hlscl.getLastError()){
138+
digitalWrite(LEDpin, LOW);
139+
Serial.print("Servo Move:");
140+
Serial.println(Move, DEC);
141+
delay(10);
142+
}else{
143+
Serial.println("read Move err");
144+
digitalWrite(LEDpin, HIGH);
145+
delay(500);
146+
}
147+
Serial.println();
148+
}

‎examples/HLSCL/Ping/Ping.ino‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
/*
2+
Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况
3+
*/
4+
5+
#include <SCServo.h>
6+
7+
HLSCL hlscl;
8+
9+
int LEDpin = 13;
10+
void setup()
11+
{
12+
pinMode(LEDpin,OUTPUT);
13+
digitalWrite(LEDpin, HIGH);
14+
Serial.begin(115200);
15+
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
16+
Serial1.begin(1000000);//mega2560
17+
hlscl.pSerial = &Serial1;
18+
delay(1000);
19+
}
20+
21+
void loop()
22+
{
23+
int ID = hlscl.Ping(1);
24+
if(!hlscl.getLastError()){
25+
digitalWrite(LEDpin, LOW);
26+
Serial.print("Servo ID:");
27+
Serial.println(ID, DEC);
28+
delay(100);
29+
}else{
30+
Serial.println("Ping servo ID error!");
31+
digitalWrite(LEDpin, HIGH);
32+
delay(2000);
33+
}
34+
}

‎examples/HLSCL/ProgramEprom/ProgramEprom.ino‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
/*
2+
舵机参数编程
3+
*/
4+
5+
#include <SCServo.h>
6+
7+
int LEDpin = 13;
8+
HLSCL hlscl;
9+
10+
void setup()
11+
{
12+
pinMode(LEDpin, OUTPUT);
13+
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
14+
Serial1.begin(1000000);//mega2560
15+
hlscl.pSerial = &Serial1;
16+
delay(1000);
17+
digitalWrite(LEDpin, LOW);
18+
hlscl.unLockEprom(1);//打开EPROM保存功能
19+
hlscl.writeByte(1, HLSCL_ID, 2);//ID
20+
hlscl.LockEprom(2);//关闭EPROM保存功能
21+
digitalWrite(LEDpin, HIGH);
22+
}
23+
24+
void loop()
25+
{
26+
27+
}

‎examples/HLSCL/RegWritePos/RegWritePos.ino‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
#include <SCServo.h>
2+
3+
HLSCL hlscl;
4+
5+
void setup()
6+
{
7+
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
8+
Serial1.begin(1000000);//mega2560
9+
hlscl.pSerial = &Serial1;
10+
delay(1000);
11+
}
12+
13+
void loop()
14+
{
15+
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=3500*6.5=3250mA，运行至P1=4095位置
16+
hlscl.RegWritePosEx(1, 4095, 60, 50, 500);
17+
hlscl.RegWritePosEx(2, 4095, 60, 50, 500);
18+
hlscl.RegWriteAction();
19+
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
20+
21+
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P1=4095位置
22+
hlscl.RegWritePosEx(1, 0, 60, 50, 500);
23+
hlscl.RegWritePosEx(2, 0, 60, 50, 500);
24+
hlscl.RegWriteAction();
25+
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
26+
}

‎examples/HLSCL/SyncRead/SyncRead.ino‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
/*
2+
同步读指令，回读ID1与ID2两个舵机的位置与速度信息
3+
*/
4+
5+
#include <SCServo.h>
6+
7+
HLSCL hlscl;
8+
9+
uint8_t ID[] = {1, 2};
10+
uint8_t rxPacket[4];
11+
int16_t Position;
12+
int16_t Speed;
13+
14+
void setup()
15+
{
16+
Serial.begin(115200);
17+
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
18+
Serial1.begin(1000000);//mega2560
19+
hlscl.pSerial = &Serial1;
20+
hlscl.syncReadBegin(sizeof(ID), sizeof(rxPacket), 5);//10*10*2=200us<5ms
21+
delay(1000);
22+
}
23+
24+
void loop()
25+
{
26+
hlscl.syncReadPacketTx(ID, sizeof(ID), HLSCL_PRESENT_POSITION_L, sizeof(rxPacket));//同步读指令包发送
27+
for(uint8_t i=0; i<sizeof(ID); i++){
28+
//接收ID[i]同步读返回包
29+
if(!hlscl.syncReadPacketRx(ID[i], rxPacket)){
30+
Serial.print("ID:");
31+
Serial.println(ID[i]);
32+
Serial.println("sync read error!");
33+
continue;//接收解码失败
34+
}
35+
Position = hlscl.syncReadRxPacketToWrod(15);//解码两个字节 bit15为方向位,参数=0表示无方向位
36+
Speed = hlscl.syncReadRxPacketToWrod(15);//解码两个字节 bit15为方向位,参数=0表示无方向位
37+
Serial.print("ID:");
38+
Serial.println(ID[i]);
39+
Serial.print("Position:");
40+
Serial.println(Position);
41+
Serial.print("Speed:");
42+
Serial.println(Speed);
43+
}
44+
delay(10);
45+
}

‎examples/HLSCL/SyncWritePos/SyncWritePos.ino‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
#include <SCServo.h>
2+
HLSCL hlscl;
3+
4+
byte ID[2];
5+
s16 Position[2];
6+
u16 Speed[2];
7+
byte ACC[2];
8+
u16 Torque[2];
9+
10+
void setup()
11+
{
12+
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
13+
Serial1.begin(1000000);//mega2560
14+
hlscl.pSerial = &Serial1;
15+
delay(1000);
16+
ID[0] = 1;//舵机ID1
17+
ID[1] = 2;//舵机ID2
18+
Speed[0] = 60;//最高速度V=60*0.732=43.92rpm
19+
Speed[1] = 60;//最高速度V=60*0.732=43.92rpm
20+
ACC[0] = 50;//加速度A=50*8.7deg/s^2
21+
ACC[1] = 50;//加速度A=50*8.7deg/s^2
22+
Torque[0] = 300;//最大扭矩电流T=500*6.5=3250mA
23+
Torque[1] = 300;//最大扭矩电流T=500*6.5=3250mA
24+
}
25+
26+
void loop()
27+
{
28+
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P1=4095位置
29+
Position[0] = 4095;
30+
Position[1] = 4095;
31+
hlscl.SyncWritePosEx(ID, 2, Position, Speed, ACC, Torque);
32+
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
33+
34+
//舵机(ID1/ID2)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，最大扭矩电流T=500*6.5=3250mA，运行至P0=0位置
35+
Position[0] = 0;
36+
Position[1] = 0;
37+
hlscl.SyncWritePosEx(ID, 2, Position, Speed, ACC, Torque);
38+
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
39+
}

0 commit comments

Comments
 (0)