|
|
楼主 |
发表于 2022-8-26 22:36:36
|
显示全部楼层
本帖最后由 往前走 于 2022-8-28 22:48 编辑
主要功能程序说明：2.舵机内存表宏定义（节选）
#define SCSCL_ID 5
#define SCSCL_BAUD_RATE 6
#define SCSCL_RETURN_DELAY_TIME 7
#define SCSCL_RETURN_LEVEL 8
#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_LIMIT_TEMPERATURE 13
#define SCSCL_MAX_LIMIT_VOLTAGE 14
#define SCSCL_MIN_LIMIT_VOLTAGE 15
#define SCSCL_MAX_TORQUE_L 16
#define SCSCL_MAX_TORQUE_H 17
#define SCSCL_ALARM_LED 19
#define SCSCL_ALARM_SHUTDOWN 20
#define SCSCL_COMPLIANCE_P 21
#define SCSCL_COMPLIANCE_D 22
#define SCSCL_COMPLIANCE_I 23
#define SCSCL_PUNCH_L 24
#define SCSCL_PUNCH_H 25
#define SCSCL_CW_DEAD 26
#define SCSCL_CCW_DEAD 27
#define SCSCL_PROTECT_TORQUE 37
#define SCSCL_PROTECT_TIME 38
#define SCSCL_OVLOAD_TORQUE 39
#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
3.舵机服务子程序(其他子程序，由于篇幅有限，这里就不写）
int writePos(uint8_t ID, uint16_t Position, uint16_t Time, uint16_t Speed, uint8_t Fun)
{
uint8_t buf[6];
flushSCS();//清空缓冲区
u16_to_u8(buf+0, buf+1, Position);
u16_to_u8(buf+2, buf+3, Time);
u16_to_u8(buf+4, buf+5, Speed);
writeBuf(ID, SCSCL_GOAL_POSITION_L, buf, 6, Fun);
return Ack(ID);
}
//写位置指令
//舵机ID，Position位置，运行时间Time，速度Speed
int WritePos(uint8_t ID, uint16_t Position, uint16_t Time, uint16_t Speed)
{
return writePos(ID, Position, Time, Speed, INST_WRITE);
}
//异步写位置指令
//舵机ID，Position位置，运行时间Time，速度Speed
int RegWritePos(uint8_t ID, uint16_t Position, uint16_t Time, uint16_t Speed)
{
return writePos(ID, Position, Time, Speed, INST_REG_WRITE);
}
void RegWriteAction(void)
{
writeBuf(0xfe, 0, NULL, 0, INST_ACTION);
}
//写位置指令
//舵机ID[]数组，IDN数组长度，Position位置，运行时间Time，速度Speed
void SyncWritePos(uint8_t ID[], uint8_t IDN, uint16_t Position, uint16_t Time, uint16_t Speed)
{
uint8_t buf[6];
u16_to_u8(buf+0, buf+1, Position);
u16_to_u8(buf+2, buf+3, 0);
u16_to_u8(buf+4, buf+5, Speed);
snycWrite(ID, IDN, SCSCL_GOAL_POSITION_L, buf, 6);
}
//读位置，超时返回-1
int ReadPos(uint8_t ID)
{
return readWord(ID, SCSCL_PRESENT_POSITION_L);
}
//速度控制模式
int WriteSpe(uint8_t ID, int16_t Speed)
{
if(Speed<0){
Speed = -Speed;
Speed |= (1<<10);
}
return writeWord(ID, SCSCL_GOAL_TIME_L, Speed);
}
//读负载，超时返回-1
int ReadLoad(uint8_t ID)
{
return readWord(ID, SCSCL_PRESENT_LOAD_L);
}
//读电压，超时返回-1
int ReadVoltage(uint8_t ID)
{
return readByte(ID, SCSCL_PRESENT_VOLTAGE);
}
//读温度，超时返回-1
int ReadTemper(uint8_t ID)
{
return readByte(ID, SCSCL_PRESENT_TEMPERATURE);
}
//Ping指令，返回舵机ID，超时返回-1
int Ping(uint8_t ID)
{
int Size;
uint8_t bBuf[6];
flushSCS();
writeBuf(ID, 0, NULL, 0, INST_PING);
Size = readSCS(bBuf, 6);
if(Size==6){
return bBuf[2];
}else{
return -1;
}
}
void writeBuf(uint8_t ID, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen, uint8_t Fun)
{
uint8_t i;
uint8_t msgLen = 2;
uint8_t bBuf[6];
uint8_t 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);//MemAddr保存的是目标位置在舵机SRAM中的保存地址，如果有目标参数（判断nDat==1），则会从这一位开始输出
}else{
bBuf[3] = msgLen;
writeSCS(bBuf, 5);//如果没有目标参数（判断nDat==0）则不输出目标地址，也就只有五位数
}
CheckSum = ID + msgLen + Fun + MemAddr;
if(nDat){
for(i=0; i<nLen; i++){
CheckSum += nDat;
}
writeSCS(nDat, nLen);
}
CheckSum = ~CheckSum;
writeSCS(&CheckSum, 1);
}
//普通写指令
//舵机ID，MemAddr内存表地址，写入数据，写入长度
int genWrite(uint8_t ID, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen)
{
flushSCS();
writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
return Ack(ID);
}
//读指令
//舵机ID，MemAddr内存表地址，返回数据nData，数据长度nLen
int Read(uint8_t ID, uint8_t MemAddr, uint8_t *nData, uint8_t nLen)
{
int Size;
uint8_t bBuf[5];
flushSCS();
writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
if(readSCS(bBuf, 5)!=5){
return 0;
}
Size = readSCS(nData, nLen);
if(readSCS(bBuf, 1)){
return Size;
}
return 0;
}
|
|
|Archiver|手机版|小黑屋|迪文科技论坛
( 京ICP备05033781号-1 )
发表于 2022-8-20 23:24:22
