update functions
This commit is contained in:
murmur
parent
ce8530303e
commit
90d7bed5b4
146
protocol.c
146
protocol.c
@ -12,10 +12,10 @@ DeviceStatus deviceStatus = {
|
||||
};
|
||||
|
||||
DeviceParam dp = {{
|
||||
{"pump1", 4, 100, 100, 100},
|
||||
{"pump2", 4, 100, 100, 100},
|
||||
{"valve1", 3, 100, 100, 100},
|
||||
{"valve2", 4, 100, 100, 100}
|
||||
{"pump1", 4, 60, 100, 100},
|
||||
{"pump2", 3, 10, 100, 100},
|
||||
{"valve1", 3, 10, 100, 100},
|
||||
{"valve2", 4, 10, 100, 100}
|
||||
}};
|
||||
|
||||
|
||||
@ -66,6 +66,8 @@ static const uint16_t crcTable[] = {
|
||||
uint16_t CalculateCRC16(uint8_t *data, uint16_t length) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
|
||||
if (!data) return 0;
|
||||
|
||||
for (uint16_t i = 0; i < length; i++) {
|
||||
uint8_t index = (crc ^ data[i]) & 0xFF;
|
||||
crc = (crc >> 8) ^ crcTable[index];
|
||||
@ -172,6 +174,7 @@ void updateInitStatus(InitStatus_t status) {
|
||||
* @param txLen 发送数据长度
|
||||
*/
|
||||
void writeCMD(uint8_t *txBuf, uint16_t txLen) {
|
||||
printf("writeCMD: ");
|
||||
for(uint16_t i = 0; i < txLen; i++) {
|
||||
printf("%02X ", txBuf[i]);
|
||||
}
|
||||
@ -181,13 +184,16 @@ void writeCMD(uint8_t *txBuf, uint16_t txLen) {
|
||||
// HAL_GPIO_WritePin(DIR2_GPIO_Port, DIR2_Pin, GPIO_PIN_RESET);
|
||||
transDataToMotorValve(txBuf, txLen);
|
||||
uint8_t rxBuf[30] = {0};
|
||||
uint16_t rxLen = 0;
|
||||
HAL_UART_Receive(&huart2, rxBuf, rxLen, 100);
|
||||
for(uint16_t i = 0; i < rxLen; i++) {
|
||||
printf("%02X ", rxBuf[i]);
|
||||
uint16_t rxLen = txLen;
|
||||
uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, 50);
|
||||
if(memcmp(rxBuf, txBuf, 2) != 0) {
|
||||
log_e("writeCMD error");
|
||||
printf("%d readCMD: ", rst);
|
||||
for(uint16_t i = 0; i < rxLen; i++) {
|
||||
printf("%02X ", rxBuf[i]);
|
||||
}
|
||||
printf("\r\n");
|
||||
}
|
||||
printf("\r\n");
|
||||
// transDataToMotorValve(txBuf, txLen);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -245,7 +251,7 @@ static void FillBigEndian16(uint8_t *data, uint16_t value) {
|
||||
+----------+--------+------------+------------+------------+
|
||||
*/
|
||||
// pump 读寄存器
|
||||
uint8_t ReadPump1Reg(uint8_t index, uint16_t reg) {
|
||||
uint16_t ReadPump1Reg(uint8_t index, uint16_t reg) {
|
||||
|
||||
uint8_t data[8] = {0};
|
||||
data[0] = index;
|
||||
@ -258,6 +264,10 @@ uint8_t ReadPump1Reg(uint8_t index, uint16_t reg) {
|
||||
memcpy(&data[6], &crc, 2);
|
||||
|
||||
writeCMD(data, 8);
|
||||
uint8_t rxBuf[30] = {0};
|
||||
uint16_t rxLen = 8;
|
||||
uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, 50);
|
||||
return rxBuf[3]<<8|rxBuf[4];
|
||||
}
|
||||
|
||||
uint8_t ReadPump2Reg(uint8_t index, uint16_t reg) {
|
||||
@ -282,10 +292,10 @@ uint8_t ReadPump2Reg(uint8_t index, uint16_t reg) {
|
||||
+----------+--------+------------+----------+------------+
|
||||
*/
|
||||
// 写泵1个寄存器
|
||||
uint8_t WritePump1Reg(uint8_t index, uint16_t reg, uint16_t value) {
|
||||
uint8_t WritePump1Reg(uint8_t id, uint16_t reg, int16_t value) {
|
||||
// 写一个寄存器不需要指定寄存器长度
|
||||
uint8_t data[8] = {0};
|
||||
data[0] = index;
|
||||
data[0] = id;
|
||||
data[1] = RTU_FUNC_WRITE_REG;
|
||||
FillBigEndian16(&data[2], reg);
|
||||
FillBigEndian16(&data[4], value);
|
||||
@ -304,14 +314,14 @@ uint8_t WritePump1Reg(uint8_t index, uint16_t reg, uint16_t value) {
|
||||
+----------+--------+------------+------------+----------+----------+------------+
|
||||
*/
|
||||
// 写泵2个寄存器
|
||||
uint8_t WritePump2Reg(uint8_t index, uint16_t reg, uint32_t value) {
|
||||
uint8_t WritePump2Reg(uint8_t id , uint16_t reg, int32_t value) {
|
||||
// 写2个寄存器需要指定寄存器长度
|
||||
uint8_t data[13] = {0};
|
||||
data[0] = index;
|
||||
data[0] = id;
|
||||
data[1] = RTU_FUNC_WRITE_MULTI_REG;
|
||||
FillBigEndian16(&data[2], reg);
|
||||
FillBigEndian16(&data[4], 2);
|
||||
data[6] = 8;
|
||||
data[6] = 4;
|
||||
FillBigEndian32(&data[7], value);
|
||||
uint16_t crc = CalculateCRC16(data, 11);
|
||||
// 小端序填充
|
||||
@ -340,7 +350,7 @@ uint8_t WritePump2Reg(uint8_t index, uint16_t reg, uint32_t value) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetPumpJogAcc(uint8_t index, uint16_t acc) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_JA, acc);
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_JA, acc*PUMP_ACCEL_RPS);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -351,7 +361,7 @@ static uint8_t SetPumpJogAcc(uint8_t index, uint16_t acc) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetPumpJogDec(uint8_t index, uint16_t dec) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_JL, dec);
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_JL, dec*PUMP_DECEL_RPS);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -362,7 +372,7 @@ static uint8_t SetPumpJogDec(uint8_t index, uint16_t dec) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetPumpJogSpeed(uint8_t index, uint16_t speed) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_JS, speed);
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_JS, speed*PUMP_SPEED_RPS);
|
||||
}
|
||||
|
||||
// Jog=慢跑
|
||||
@ -401,7 +411,7 @@ static uint8_t StopPumpJog(uint8_t index) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetPumpStepAcc(uint8_t index, uint16_t acc) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_AC, acc);
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_AC, acc*PUMP_ACCEL_RPS);
|
||||
}
|
||||
/**
|
||||
* 设置泵的步进减速度
|
||||
@ -411,20 +421,21 @@ static uint8_t SetPumpStepAcc(uint8_t index, uint16_t acc) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetPumpStepDec(uint8_t index, uint16_t dec) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_DE, dec);
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_DE, dec*PUMP_DECEL_RPS);
|
||||
}
|
||||
/**
|
||||
* 设置泵的步进速度
|
||||
* 将百分比速度转换为实际速度值
|
||||
*
|
||||
* @param index 泵索引
|
||||
* @param speed 速度百分比(0-100)
|
||||
* @param speed 用户速度
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetPumpStepSpeed(uint8_t index, uint16_t speed) {
|
||||
// 目标速度转换为实际速度
|
||||
speed = (uint16_t)(speed * dp.pump[index].maxSpeed / 100);
|
||||
// 速度类寄存器参数设定值单位为 1/240 rps
|
||||
speed = speed * PUMP_SPEED_RPS;
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_VE, speed);
|
||||
return 0;
|
||||
}
|
||||
/**
|
||||
* 设置泵的步进目标位置
|
||||
@ -433,8 +444,8 @@ static uint8_t SetPumpStepSpeed(uint8_t index, uint16_t speed) {
|
||||
* @param target 目标位置值
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetPumpStepTarget(uint8_t index, uint32_t target) {
|
||||
WritePump2Reg(index, RTU_PUMP_CMD_DI, target);
|
||||
static uint8_t SetPumpStepTarget(uint8_t index, int32_t target) {
|
||||
WritePump2Reg(dp.pump[index].id, RTU_PUMP_CMD_DI, target);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -445,7 +456,7 @@ static uint8_t SetPumpStepTarget(uint8_t index, uint32_t target) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t StartPumpRelativeMove(uint8_t index) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0066);
|
||||
WritePump1Reg(dp.pump[index].id, RTU_PUMP_CMD_CO, 0x0066);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -467,7 +478,7 @@ static uint8_t StartPumpAbsoluteMove(uint8_t index) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t StopPump(uint8_t index) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x00E1);
|
||||
WritePump1Reg(dp.pump[index].id, RTU_PUMP_CMD_CO, 0x00E1);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -487,7 +498,7 @@ uint8_t ReadPumpHWReg(uint8_t index) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
uint8_t SetPumpBR(uint8_t index, uint16_t br) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_BR, br);
|
||||
WritePump1Reg(dp.pump[index].id, RTU_PUMP_CMD_BR, br);
|
||||
}
|
||||
/**
|
||||
* 设置泵的通信协议
|
||||
@ -497,7 +508,7 @@ uint8_t SetPumpBR(uint8_t index, uint16_t br) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
uint8_t SetPumpPR(uint8_t index, uint16_t pr) {
|
||||
WritePump1Reg(index, RTU_PUMP_CMD_PR, pr);
|
||||
WritePump1Reg(dp.pump[index].id, RTU_PUMP_CMD_PR, pr);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -505,8 +516,8 @@ uint8_t SetPumpPR(uint8_t index, uint16_t pr) {
|
||||
*
|
||||
* @param index 泵索引
|
||||
*/
|
||||
void ReadPumpStatus(uint8_t index) {
|
||||
ReadPump1Reg(index, RTU_PUMP_CMD_SC);
|
||||
uint16_t ReadPumpStatus(uint8_t index) {
|
||||
return ReadPump1Reg(dp.pump[index].id, RTU_PUMP_CMD_SC);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -514,8 +525,8 @@ void ReadPumpStatus(uint8_t index) {
|
||||
*
|
||||
* @param index 泵索引
|
||||
*/
|
||||
void ReadPumpAlarm(uint8_t index) {
|
||||
ReadPump1Reg(index, RTU_PUMP_CMD_AL);
|
||||
uint16_t ReadPumpAlarm(uint8_t index) {
|
||||
return ReadPump1Reg(dp.pump[index].id, RTU_PUMP_CMD_AL);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -524,6 +535,7 @@ void ReadPumpAlarm(uint8_t index) {
|
||||
* @param reg4001 告警寄存器值
|
||||
*/
|
||||
void DecodePumpAlarmMsg(uint16_t reg4001) {
|
||||
printf("reg4001: %x\r\n", reg4001);
|
||||
static AlarmCode_t alarmCode = {0};
|
||||
// 与上次告警信息相同，则不更新，仅打印一次
|
||||
if (alarmCode.all == reg4001)
|
||||
@ -615,6 +627,16 @@ uint8_t InitPump(void) {
|
||||
SetPumpStepAcc(dp.pump[1].id, dp.pump[1].maxAccel);
|
||||
SetPumpStepDec(dp.pump[1].id, dp.pump[1].maxDecel);
|
||||
SetPumpStepSpeed(dp.pump[1].id, dp.pump[1].maxSpeed);
|
||||
|
||||
uint16_t reg4001 = ReadPumpAlarm(0);
|
||||
DecodePumpAlarmMsg(reg4001);
|
||||
uint16_t reg4002 = ReadPumpStatus(0);
|
||||
DecodePumpStatusMsg(reg4002);
|
||||
|
||||
reg4001 = ReadPumpAlarm(1);
|
||||
DecodePumpAlarmMsg(reg4001);
|
||||
reg4002 = ReadPumpStatus(1);
|
||||
DecodePumpStatusMsg(reg4002);
|
||||
}
|
||||
|
||||
|
||||
@ -785,22 +807,22 @@ static uint8_t SetValveFunc(uint8_t index, uint16_t func) {
|
||||
* 设置阀门原点回归堵转检测力矩
|
||||
*
|
||||
* @param index 阀门索引
|
||||
* @param torque 力矩值
|
||||
* @param torque 力矩值，百分比，太低容易误判，默认为60即60%
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetValveHomeTorque(uint8_t index, uint16_t torque) {
|
||||
writeValve1Reg(index, RTU_VALVE_CMD_HOME_TORQUE, torque);
|
||||
writeValve1Reg(index, RTU_VALVE_CMD_HOME_TORQUE, torque*10);
|
||||
}
|
||||
|
||||
/**
|
||||
* 设置阀门原点回归堵转检测时间
|
||||
*
|
||||
* @param index 阀门索引
|
||||
* @param time 检测时间(ms)
|
||||
* @param time 检测时间(ms)，太短容易误判，默认为1000ms
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t SetValveHomeTime(uint8_t index, uint16_t time) {
|
||||
writeValve1Reg(index, RTU_VALVE_CMD_HOME_TIME, time);
|
||||
writeValve1Reg(index, RTU_VALVE_CMD_HOME_TIME, time*10);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -810,27 +832,32 @@ static uint8_t SetValveHomeTime(uint8_t index, uint16_t time) {
|
||||
* @param index 阀门索引
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
uint8_t ValveBackToOrigin(uint8_t index) {
|
||||
uint8_t ValveBackToOrigin(uint8_t index,uint8_t direction) {
|
||||
// 1.设置原点回归方式
|
||||
// (0416h)=37;17=负限位，18=正限位
|
||||
SetValveHomeDetectMode(index, 37);
|
||||
if(direction) {
|
||||
SetValveHomeDetectMode(index, 37);//正方向堵转
|
||||
}
|
||||
else {
|
||||
SetValveHomeDetectMode(index, 38);//反方向堵转
|
||||
}
|
||||
// 2.设置堵转检测力矩和堵转检测时间
|
||||
// (0170h)=300，(0172h)=50
|
||||
SetValveHomeTorque(index, 300);//30%
|
||||
SetValveHomeTime(index, 50);//50ms
|
||||
SetValveHomeTorque(index, 600);//60%
|
||||
SetValveHomeTime(index, 10000);//1000ms
|
||||
// 3.写 (00B1h)=0、运行模式 (03C2h)=0x06，使其工作在原点回归模式；
|
||||
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
|
||||
SetValveRunMode(index, RTU_VALVE_CFG_MODE_HM);
|
||||
// SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
|
||||
// SetValveRunMode(index, RTU_VALVE_CFG_MODE_HM);
|
||||
// 4.写寻找限位开关速度和寻找原点信号速度(0417h)= 10000 (0419h)=1000;
|
||||
SetValveHomeSwtSpeed(index, 10000);
|
||||
SetValveHomeOriSpeed(index, 1000);
|
||||
// SetValveHomeSwtSpeed(index, 20000);
|
||||
SetValveHomeOriSpeed(index, 20000);
|
||||
// 5.设置回零加速度
|
||||
SetValveHomeAcc(index, 200000);
|
||||
// 6.写控制字
|
||||
// (0380h)= 0x06→0x07→0x0F→0x1F，电机运行
|
||||
SetValveFunc(index, RTU_VALVE_CFG_PREPARE);
|
||||
SetValveFunc(index, RTU_VALVE_CFG_DISABLE);
|
||||
SetValveFunc(index, RTU_VALVE_CFG_ENABLE);
|
||||
// SetValveFunc(index, RTU_VALVE_CFG_PREPARE);
|
||||
// SetValveFunc(index, RTU_VALVE_CFG_DISABLE);
|
||||
// SetValveFunc(index, RTU_VALVE_CFG_ENABLE);
|
||||
SetValveFunc(index, RTU_VALVE_CFG_RUN_ORIGIN);
|
||||
}
|
||||
|
||||
@ -1224,22 +1251,31 @@ CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen) {
|
||||
|
||||
|
||||
void runPumpDemo(void) {
|
||||
printf("runPumpDemo\n");
|
||||
printf("InitPump\n");
|
||||
printf("runPumpDemo\r\n");
|
||||
// printf("InitPump\n");
|
||||
InitPump();
|
||||
// 泵1正转100步
|
||||
printf("SetPumpStepTarget(0, 100)\n");
|
||||
SetPumpStepTarget(0, 100);
|
||||
SetPumpStepTarget(0, 200000);
|
||||
printf("StartPumpRelativeMove(0)\n");
|
||||
StartPumpRelativeMove(0);
|
||||
HAL_Delay(1000);
|
||||
// 泵1反转100步
|
||||
printf("SetPumpStepTarget(0, -100)\n");
|
||||
SetPumpStepTarget(0, -100);
|
||||
SetPumpStepTarget(1, -300000);
|
||||
printf("StartPumpRelativeMove(0)\n");
|
||||
StartPumpRelativeMove(0);
|
||||
StartPumpRelativeMove(1);
|
||||
HAL_Delay(1000);
|
||||
printf("StopPump(0)\n");
|
||||
StopPump(0);
|
||||
HAL_Delay(1000);
|
||||
}
|
||||
}
|
||||
|
||||
void runValveDemo(void) {
|
||||
InitValve();
|
||||
ValveBackToOrigin(0);
|
||||
printf("runValveDemo\r\n");
|
||||
// 阀门1正转120度
|
||||
printf("ValveRunToAngle(0, 120)\n");
|
||||
ValveRunToAngle(0, 120);
|
||||
}
|
||||
|
||||
10
protocol.h
10
protocol.h
@ -29,6 +29,12 @@
|
||||
#define HOST_CMD_SYSTEM_INIT 0x0007 // 系统初始化
|
||||
#define CMD_BR_SET 0x0008 // 波特率设置
|
||||
#define CMD_PR_SET 0x0009 // 通信协议设置
|
||||
|
||||
#define PUMP_SPEED_RPS 240//速度单位为1/240 rps
|
||||
#define PUMP_ACCEL_RPS 6//加速度单位为1/6 rps/s
|
||||
#define PUMP_DECEL_RPS 6//减速度单位为1/6 rps/s
|
||||
#define PUMP_SPEED_RPS_MAX 50//
|
||||
|
||||
// 命令帧错误码定义
|
||||
typedef enum {
|
||||
CMD_FRAME_OK = 0,
|
||||
@ -248,8 +254,8 @@ static const uint8_t statusInfo[16][32]={
|
||||
*/
|
||||
#define RTU_PUMP_CMD_CO 0x007C // 操作码，Command Opcode
|
||||
|
||||
#define RTU_PUMP_CMD_SC 0x4002 // 状态寄存器
|
||||
#define RTU_PUMP_CMD_AL 0x4001 // 告警寄存器
|
||||
#define RTU_PUMP_CMD_SC 0x0002 // 状态寄存器
|
||||
#define RTU_PUMP_CMD_AL 0x0001 // 告警寄存器
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user