Ping et lecture des registres lorsqu'un servo avec l'ID 1 est trouvé
This commit is contained in:
commit
fb94f7981b
5
.gitignore
vendored
Normal file
5
.gitignore
vendored
Normal file
@ -0,0 +1,5 @@
|
||||
.pio
|
||||
.vscode/.browse.c_cpp.db*
|
||||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
7
platformio.ini
Normal file
7
platformio.ini
Normal file
@ -0,0 +1,7 @@
|
||||
[env]
|
||||
platform = raspberrypi
|
||||
framework = arduino
|
||||
|
||||
[env:pico]
|
||||
board = pico
|
||||
|
||||
260
src/HLSCL.cpp
Normal file
260
src/HLSCL.cpp
Normal 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
src/HLSCL.h
Normal file
89
src/HLSCL.h
Normal 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
src/INST.h
Normal file
51
src/INST.h
Normal 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
src/SCS.cpp
Normal file
450
src/SCS.cpp
Normal 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
src/SCS.h
Normal file
64
src/SCS.h
Normal 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
src/SCSCL.cpp
Normal file
203
src/SCSCL.cpp
Normal 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
src/SCSCL.h
Normal file
77
src/SCSCL.h
Normal 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
src/SCSerial.cpp
Normal file
99
src/SCSerial.cpp
Normal 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
src/SCSerial.h
Normal file
38
src/SCSerial.h
Normal 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
src/SCServo.h
Normal file
15
src/SCServo.h
Normal 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
src/SMS_STS.cpp
Normal file
249
src/SMS_STS.cpp
Normal 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
src/SMS_STS.h
Normal file
86
src/SMS_STS.h
Normal 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
|
||||
131
src/main.cpp
Normal file
131
src/main.cpp
Normal file
@ -0,0 +1,131 @@
|
||||
#include <Arduino.h>
|
||||
#include "SCServo.h"
|
||||
|
||||
SMS_STS sms_sts;
|
||||
|
||||
void test_ping();
|
||||
void lire_tous_les_registres(int servo_id);
|
||||
|
||||
|
||||
char registre_description[70][100]={
|
||||
"Version majeure du firmware",
|
||||
"Version mineure du firmware",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
""
|
||||
};
|
||||
|
||||
// the setup routine runs once when you press reset:
|
||||
void setup() {
|
||||
// initialize the digital pin as an output.
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
Serial.begin(115200);//sts舵机波特率1000000
|
||||
Serial1.begin(1000000);//sts舵机波特率1000000
|
||||
sms_sts.pSerial = &Serial1;
|
||||
|
||||
}
|
||||
|
||||
// the loop routine runs over and over again forever:
|
||||
void loop() {
|
||||
digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
|
||||
test_ping();
|
||||
digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
|
||||
test_ping();
|
||||
}
|
||||
|
||||
void test_ping()
|
||||
{
|
||||
int ID = sms_sts.Ping(1);
|
||||
static int found = 0;
|
||||
if(ID!=-1){
|
||||
Serial.print("Servo ID:");
|
||||
Serial.println(ID);
|
||||
if(found == 0){
|
||||
found = 1;
|
||||
lire_tous_les_registres(ID);
|
||||
}
|
||||
delay(100);
|
||||
}else{
|
||||
Serial.print("Ping servo ID error!\n");
|
||||
found = 0;
|
||||
delay(2000);
|
||||
}
|
||||
}
|
||||
|
||||
int lire_registre(int servo_id, int registre_adresse){
|
||||
return sms_sts.readByte(servo_id, registre_adresse); // Resolution à 1
|
||||
}
|
||||
|
||||
void lire_tous_les_registres(int servo_id){
|
||||
char message[200]="";
|
||||
for (int i=0; i<0x46; i++){
|
||||
sprintf(message, "registre 0x%x: %d\t\t%s\n", i, lire_registre(servo_id, i), registre_description[i]);
|
||||
Serial.print(message);
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user