Skip to content

Commit e50859e

Browse files
ftservoftservo
committed
src
1 parent c43917a commit e50859e

12 files changed

Lines changed: 1681 additions & 0 deletions

File tree

‎src/HLSCL.cpp‎

Lines changed: 260 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,260 @@
1+
/*
2+
* HLSCL.cpp
3+
* 飞特HTS/HLS系列串行舵机应用层程序
4+
* 日期: 2024.11.21
5+
* 作者: txl
6+
*/
7+
8+
#include "HLSCL.h"
9+
10+
HLSCL::HLSCL()
11+
{
12+
End = 0;
13+
}
14+
15+
HLSCL::HLSCL(u8 End):SCSerial(End)
16+
{
17+
}
18+
19+
HLSCL::HLSCL(u8 End, u8 Level):SCSerial(End, Level)
20+
{
21+
}
22+
23+
int HLSCL::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC, u16 Torque)
24+
{
25+
if(Position<0){
26+
Position = -Position;
27+
Position |= (1<<15);
28+
}
29+
u8 bBuf[7];
30+
bBuf[0] = ACC;
31+
Host2SCS(bBuf+1, bBuf+2, Position);
32+
Host2SCS(bBuf+3, bBuf+4, Torque);
33+
Host2SCS(bBuf+5, bBuf+6, Speed);
34+
35+
return genWrite(ID, HLSCL_ACC, bBuf, 7);
36+
}
37+
38+
int HLSCL::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC, u16 Torque)
39+
{
40+
if(Position<0){
41+
Position = -Position;
42+
Position |= (1<<15);
43+
}
44+
u8 bBuf[7];
45+
bBuf[0] = ACC;
46+
Host2SCS(bBuf+1, bBuf+2, Position);
47+
Host2SCS(bBuf+3, bBuf+4, Torque);
48+
Host2SCS(bBuf+5, bBuf+6, Speed);
49+
50+
return regWrite(ID, HLSCL_ACC, bBuf, 7);
51+
}
52+
53+
void HLSCL::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[], u16 Torque[])
54+
{
55+
u8 offbuf[7*IDN];
56+
for(u8 i = 0; i<IDN; i++){
57+
if(Position[i]<0){
58+
Position[i] = -Position[i];
59+
Position[i] |= (1<<15);
60+
}
61+
if(ACC){
62+
offbuf[i*7] = ACC[i];
63+
}else{
64+
offbuf[i*7] = 0;
65+
}
66+
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, Position[i]);
67+
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, Torque[i]);
68+
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
69+
}
70+
syncWrite(ID, IDN, HLSCL_ACC, offbuf, 7);
71+
}
72+
73+
void HLSCL::SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[], u16 Torque[])
74+
{
75+
u8 offbuf[7*IDN];
76+
for(u8 i = 0; i<IDN; i++){
77+
if(Speed[i]<0){
78+
Speed[i] = -Speed[i];
79+
Speed[i] |= (1<<15);
80+
}
81+
if(ACC){
82+
offbuf[i*7] = ACC[i];
83+
}else{
84+
offbuf[i*7] = 0;
85+
}
86+
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, 0);
87+
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, Torque[i]);
88+
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
89+
}
90+
syncWrite(ID, IDN, HLSCL_ACC, offbuf, 7);
91+
}
92+
93+
int HLSCL::WheelMode(u8 ID)
94+
{
95+
return writeByte(ID, HLSCL_MODE, 1);
96+
}
97+
98+
int HLSCL::EleMode(u8 ID)
99+
{
100+
return writeByte(ID, HLSCL_MODE, 2);
101+
}
102+
103+
int HLSCL::WriteSpe(u8 ID, s16 Speed, u8 ACC, u16 Torque)
104+
{
105+
if(Speed<0){
106+
Speed = -Speed;
107+
Speed |= (1<<15);
108+
}
109+
u8 bBuf[7];
110+
bBuf[0] = ACC;
111+
Host2SCS(bBuf+1, bBuf+2, 0);
112+
Host2SCS(bBuf+3, bBuf+4, Torque);
113+
Host2SCS(bBuf+5, bBuf+6, Speed);
114+
115+
return genWrite(ID, HLSCL_ACC, bBuf, 7);
116+
}
117+
118+
int HLSCL::WriteEle(u8 ID, s16 Torque)
119+
{
120+
if(Torque<0){
121+
Torque = -Torque;
122+
Torque |= (1<<15);
123+
}
124+
return writeWord(ID, HLSCL_GOAL_TORQUE_L, Torque);
125+
}
126+
127+
int HLSCL::EnableTorque(u8 ID, u8 Enable)
128+
{
129+
return writeByte(ID, HLSCL_TORQUE_ENABLE, Enable);
130+
}
131+
132+
int HLSCL::unLockEprom(u8 ID)
133+
{
134+
EnableTorque(ID, 0);
135+
return writeByte(ID, HLSCL_LOCK, 0);
136+
}
137+
138+
int HLSCL::LockEprom(u8 ID)
139+
{
140+
return writeByte(ID, HLSCL_LOCK, 1);
141+
}
142+
143+
int HLSCL::CalibrationOfs(u8 ID)
144+
{
145+
EnableTorque(ID, 0);
146+
unLockEprom(ID);
147+
return Recal(ID);
148+
}
149+
150+
int HLSCL::FeedBack(int ID)
151+
{
152+
int nLen = Read(ID, HLSCL_PRESENT_POSITION_L, Mem, sizeof(Mem));
153+
if(nLen!=sizeof(Mem)){
154+
return -1;
155+
}
156+
return nLen;
157+
}
158+
159+
int HLSCL::ReadPos(int ID)
160+
{
161+
int Pos = -1;
162+
if(ID==-1){
163+
Pos = Mem[HLSCL_PRESENT_POSITION_H-HLSCL_PRESENT_POSITION_L];
164+
Pos <<= 8;
165+
Pos |= Mem[HLSCL_PRESENT_POSITION_L-HLSCL_PRESENT_POSITION_L];
166+
}else{
167+
Pos = readWord(ID, HLSCL_PRESENT_POSITION_L);
168+
}
169+
if(Pos&(1<<15)){
170+
Pos = -(Pos&~(1<<15));
171+
}
172+
173+
return Pos;
174+
}
175+
176+
int HLSCL::ReadSpeed(int ID)
177+
{
178+
int Speed = -1;
179+
if(ID==-1){
180+
Speed = Mem[HLSCL_PRESENT_SPEED_H-HLSCL_PRESENT_POSITION_L];
181+
Speed <<= 8;
182+
Speed |= Mem[HLSCL_PRESENT_SPEED_L-HLSCL_PRESENT_POSITION_L];
183+
}else{
184+
Speed = readWord(ID, HLSCL_PRESENT_SPEED_L);
185+
}
186+
if(Speed&(1<<15)){
187+
Speed = -(Speed&~(1<<15));
188+
}
189+
return Speed;
190+
}
191+
192+
int HLSCL::ReadLoad(int ID)
193+
{
194+
int Load = -1;
195+
if(ID==-1){
196+
Load = Mem[HLSCL_PRESENT_LOAD_H-HLSCL_PRESENT_POSITION_L];
197+
Load <<= 8;
198+
Load |= Mem[HLSCL_PRESENT_LOAD_L-HLSCL_PRESENT_POSITION_L];
199+
}else{
200+
Load = readWord(ID, HLSCL_PRESENT_LOAD_L);
201+
}
202+
if(Load&(1<<10)){
203+
Load = -(Load&~(1<<10));
204+
}
205+
return Load;
206+
}
207+
208+
int HLSCL::ReadVoltage(int ID)
209+
{
210+
int Voltage = -1;
211+
if(ID==-1){
212+
Voltage = Mem[HLSCL_PRESENT_VOLTAGE-HLSCL_PRESENT_POSITION_L];
213+
}else{
214+
Voltage = readByte(ID, HLSCL_PRESENT_VOLTAGE);
215+
}
216+
return Voltage;
217+
}
218+
219+
int HLSCL::ReadTemper(int ID)
220+
{
221+
int Temper = -1;
222+
if(ID==-1){
223+
Temper = Mem[HLSCL_PRESENT_TEMPERATURE-HLSCL_PRESENT_POSITION_L];
224+
}else{
225+
Temper = readByte(ID, HLSCL_PRESENT_TEMPERATURE);
226+
}
227+
return Temper;
228+
}
229+
230+
int HLSCL::ReadMove(int ID)
231+
{
232+
int Move = -1;
233+
if(ID==-1){
234+
Move = Mem[HLSCL_MOVING-HLSCL_PRESENT_POSITION_L];
235+
}else{
236+
Move = readByte(ID, HLSCL_MOVING);
237+
}
238+
return Move;
239+
}
240+
241+
int HLSCL::ReadCurrent(int ID)
242+
{
243+
int Current = -1;
244+
if(ID==-1){
245+
Current = Mem[HLSCL_PRESENT_CURRENT_H-HLSCL_PRESENT_POSITION_L];
246+
Current <<= 8;
247+
Current |= Mem[HLSCL_PRESENT_CURRENT_L-HLSCL_PRESENT_POSITION_L];
248+
}else{
249+
Current = readWord(ID, HLSCL_PRESENT_CURRENT_L);
250+
}
251+
if(Current&(1<<15)){
252+
Current = -(Current&~(1<<15));
253+
}
254+
return Current;
255+
}
256+
257+
int HLSCL::ServoMode(u8 ID)
258+
{
259+
return writeByte(ID, HLSCL_MODE, 0);
260+
}

‎src/HLSCL.h‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
/*
2+
* HLSCL.h
3+
* 飞特HLS系列串行舵机应用层程序
4+
* 日期: 2024.11.21
5+
* 作者: txl
6+
*/
7+
8+
#ifndef _HLSCL_H
9+
#define _HLSCL_H
10+
11+
//内存表定义
12+
//-------EPROM(只读)--------
13+
#define HLSCL_MODEL_L 3
14+
#define HLSCL_MODEL_H 4
15+
16+
//-------EPROM(读写)--------
17+
#define HLSCL_ID 5
18+
#define HLSCL_BAUD_RATE 6
19+
#define HLSCL_SECOND_ID 7
20+
#define HLSCL_MIN_ANGLE_LIMIT_L 9
21+
#define HLSCL_MIN_ANGLE_LIMIT_H 10
22+
#define HLSCL_MAX_ANGLE_LIMIT_L 11
23+
#define HLSCL_MAX_ANGLE_LIMIT_H 12
24+
#define HLSCL_CW_DEAD 26
25+
#define HLSCL_CCW_DEAD 27
26+
#define HLSCL_OFS_L 31
27+
#define HLSCL_OFS_H 32
28+
#define HLSCL_MODE 33
29+
30+
//-------SRAM(读写)--------
31+
#define HLSCL_TORQUE_ENABLE 40
32+
#define HLSCL_ACC 41
33+
#define HLSCL_GOAL_POSITION_L 42
34+
#define HLSCL_GOAL_POSITION_H 43
35+
#define HLSCL_GOAL_TORQUE_L 44
36+
#define HLSCL_GOAL_TORQUE_H 45
37+
#define HLSCL_GOAL_SPEED_L 46
38+
#define HLSCL_GOAL_SPEED_H 47
39+
#define HLSCL_TORQUE_LIMIT_L 48
40+
#define HLSCL_TORQUE_LIMIT_H 49
41+
#define HLSCL_LOCK 55
42+
43+
//-------SRAM(只读)--------
44+
#define HLSCL_PRESENT_POSITION_L 56
45+
#define HLSCL_PRESENT_POSITION_H 57
46+
#define HLSCL_PRESENT_SPEED_L 58
47+
#define HLSCL_PRESENT_SPEED_H 59
48+
#define HLSCL_PRESENT_LOAD_L 60
49+
#define HLSCL_PRESENT_LOAD_H 61
50+
#define HLSCL_PRESENT_VOLTAGE 62
51+
#define HLSCL_PRESENT_TEMPERATURE 63
52+
#define HLSCL_MOVING 66
53+
#define HLSCL_PRESENT_CURRENT_L 69
54+
#define HLSCL_PRESENT_CURRENT_H 70
55+
56+
#include "SCSerial.h"
57+
58+
class HLSCL : public SCSerial
59+
{
60+
public:
61+
HLSCL();
62+
HLSCL(u8 End);
63+
HLSCL(u8 End, u8 Level);
64+
int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0, u16 Torque = 0);//普通写单个舵机位置指令
65+
int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0, u16 Torque = 0);//异步写单个舵机位置指令(RegWriteAction生效)
66+
void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[], u16 Torque[]);//同步写多个舵机位置指令
67+
void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[], u16 Torque[]);//同步写多个舵机速度指令
68+
int ServoMode(u8 ID);//Servo模式
69+
int WheelMode(u8 ID);//恒速模式
70+
int EleMode(u8 ID);//恒力模式
71+
int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0, u16 Torque = 0);//恒速模式控制指令
72+
int WriteEle(u8 ID, s16 Torque);//恒力模式控制指令
73+
int EnableTorque(u8 ID, u8 Enable);//扭力控制指令
74+
int unLockEprom(u8 ID);//eprom解锁
75+
int LockEprom(u8 ID);//eprom加锁
76+
int CalibrationOfs(u8 ID);//中位校准
77+
int FeedBack(int ID);//反馈舵机信息
78+
int ReadPos(int ID);//读位置
79+
int ReadSpeed(int ID);//读速度
80+
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
81+
int ReadVoltage(int ID);//读电压
82+
int ReadTemper(int ID);//读温度
83+
int ReadMove(int ID);//读移动状态
84+
int ReadCurrent(int ID);//读电流
85+
private:
86+
u8 Mem[HLSCL_PRESENT_CURRENT_H-HLSCL_PRESENT_POSITION_L+1];
87+
};
88+
89+
#endif

0 commit comments

Comments
 (0)