murmur/injectorCTL
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update functions

Browse Source
This commit is contained in:
murmur 2024-12-22 22:29:17 +08:00
parent ce8530303e
commit 90d7bed5b4
2 changed files with 99 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
protocol.c
View File

@ -12,10 +12,10 @@ DeviceStatus deviceStatus = {
}; };
DeviceParam dp = {{ DeviceParam dp = {{
{"pump1", 4, 100, 100, 100}, {"pump1", 4, 60, 100, 100},
{"pump2", 4, 100, 100, 100}, {"pump2", 3, 10, 100, 100},
{"valve1", 3, 100, 100, 100}, {"valve1", 3, 10, 100, 100},
{"valve2", 4, 100, 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 CalculateCRC16(uint8_t *data, uint16_t length) {
uint16_t crc = 0xFFFF; uint16_t crc = 0xFFFF;
if (!data) return 0;
for (uint16_t i = 0; i < length; i++) { for (uint16_t i = 0; i < length; i++) {
uint8_t index = (crc ^ data[i]) & 0xFF; uint8_t index = (crc ^ data[i]) & 0xFF;
crc = (crc >> 8) ^ crcTable[index]; crc = (crc >> 8) ^ crcTable[index];
@ -172,6 +174,7 @@ void updateInitStatus(InitStatus_t status) {
* @param txLen 发送数据长度 * @param txLen 发送数据长度
*/ */
void writeCMD(uint8_t *txBuf, uint16_t txLen) { void writeCMD(uint8_t *txBuf, uint16_t txLen) {
printf("writeCMD: ");
for(uint16_t i = 0; i < txLen; i++) { for(uint16_t i = 0; i < txLen; i++) {
printf("%02X ", txBuf[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); // HAL_GPIO_WritePin(DIR2_GPIO_Port, DIR2_Pin, GPIO_PIN_RESET);
transDataToMotorValve(txBuf, txLen); transDataToMotorValve(txBuf, txLen);
uint8_t rxBuf[30] = {0}; uint8_t rxBuf[30] = {0};
uint16_t rxLen = 0; uint16_t rxLen = txLen;
HAL_UART_Receive(&huart2, rxBuf, rxLen, 100); uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, 50);
for(uint16_t i = 0; i < rxLen; i++) { if(memcmp(rxBuf, txBuf, 2) != 0) {
printf("%02X ", rxBuf[i]); 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 读寄存器 // pump 读寄存器
uint8_t ReadPump1Reg(uint8_t index, uint16_t reg) { uint16_t ReadPump1Reg(uint8_t index, uint16_t reg) {
uint8_t data[8] = {0}; uint8_t data[8] = {0};
data[0] = index; data[0] = index;
@ -258,6 +264,10 @@ uint8_t ReadPump1Reg(uint8_t index, uint16_t reg) {
memcpy(&data[6], &crc, 2); memcpy(&data[6], &crc, 2);
writeCMD(data, 8); 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) { uint8_t ReadPump2Reg(uint8_t index, uint16_t reg) {
@ -282,10 +292,10 @@ uint8_t ReadPump2Reg(uint8_t index, uint16_t reg) {
+----------+--------+------------+----------+------------+ +----------+--------+------------+----------+------------+
*/ */
// 写泵1个寄存器 // 写泵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}; uint8_t data[8] = {0};
data[0] = index; data[0] = id;
data[1] = RTU_FUNC_WRITE_REG; data[1] = RTU_FUNC_WRITE_REG;
FillBigEndian16(&data[2], reg); FillBigEndian16(&data[2], reg);
FillBigEndian16(&data[4], value); FillBigEndian16(&data[4], value);
@ -304,14 +314,14 @@ uint8_t WritePump1Reg(uint8_t index, uint16_t reg, uint16_t value) {
+----------+--------+------------+------------+----------+----------+------------+ +----------+--------+------------+------------+----------+----------+------------+
*/ */
// 写泵2个寄存器 // 写泵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个寄存器需要指定寄存器长度 // 写2个寄存器需要指定寄存器长度
uint8_t data[13] = {0}; uint8_t data[13] = {0};
data[0] = index; data[0] = id;
data[1] = RTU_FUNC_WRITE_MULTI_REG; data[1] = RTU_FUNC_WRITE_MULTI_REG;
FillBigEndian16(&data[2], reg); FillBigEndian16(&data[2], reg);
FillBigEndian16(&data[4], 2); FillBigEndian16(&data[4], 2);
data[6] = 8; data[6] = 4;
FillBigEndian32(&data[7], value); FillBigEndian32(&data[7], value);
uint16_t crc = CalculateCRC16(data, 11); uint16_t crc = CalculateCRC16(data, 11);
// 小端序填充 // 小端序填充
@ -340,7 +350,7 @@ uint8_t WritePump2Reg(uint8_t index, uint16_t reg, uint32_t value) {
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetPumpJogAcc(uint8_t index, uint16_t acc) { 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:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetPumpJogDec(uint8_t index, uint16_t dec) { 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:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetPumpJogSpeed(uint8_t index, uint16_t speed) { 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=慢跑 // Jog=慢跑
@ -401,7 +411,7 @@ static uint8_t StopPumpJog(uint8_t index) {
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetPumpStepAcc(uint8_t index, uint16_t acc) { 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:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetPumpStepDec(uint8_t index, uint16_t dec) { 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 index 泵索引
* @param speed 速度百分比(0-100) * @param speed 用户速度
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetPumpStepSpeed(uint8_t index, uint16_t speed) { static uint8_t SetPumpStepSpeed(uint8_t index, uint16_t speed) {
// 目标速度转换为实际速度 // 速度类寄存器参数设定值单位为 1/240 rps
speed = (uint16_t)(speed * dp.pump[index].maxSpeed / 100); speed = speed * PUMP_SPEED_RPS;
WritePump1Reg(index, RTU_PUMP_CMD_VE, speed); 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 目标位置值 * @param target 目标位置值
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetPumpStepTarget(uint8_t index, uint32_t target) { static uint8_t SetPumpStepTarget(uint8_t index, int32_t target) {
WritePump2Reg(index, RTU_PUMP_CMD_DI, 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:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t StartPumpRelativeMove(uint8_t index) { 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:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t StopPump(uint8_t index) { 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:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
uint8_t SetPumpBR(uint8_t index, uint16_t br) { 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:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
uint8_t SetPumpPR(uint8_t index, uint16_t pr) { 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 泵索引 * @param index 泵索引
*/ */
void ReadPumpStatus(uint8_t index) { uint16_t ReadPumpStatus(uint8_t index) {
ReadPump1Reg(index, RTU_PUMP_CMD_SC); return ReadPump1Reg(dp.pump[index].id, RTU_PUMP_CMD_SC);
} }
/** /**
@ -514,8 +525,8 @@ void ReadPumpStatus(uint8_t index) {
* *
* @param index 泵索引 * @param index 泵索引
*/ */
void ReadPumpAlarm(uint8_t index) { uint16_t ReadPumpAlarm(uint8_t index) {
ReadPump1Reg(index, RTU_PUMP_CMD_AL); return ReadPump1Reg(dp.pump[index].id, RTU_PUMP_CMD_AL);
} }
/** /**
@ -524,6 +535,7 @@ void ReadPumpAlarm(uint8_t index) {
* @param reg4001 告警寄存器值 * @param reg4001 告警寄存器值
*/ */
void DecodePumpAlarmMsg(uint16_t reg4001) { void DecodePumpAlarmMsg(uint16_t reg4001) {
printf("reg4001: %x\r\n", reg4001);
static AlarmCode_t alarmCode = {0}; static AlarmCode_t alarmCode = {0};
// 与上次告警信息相同，则不更新，仅打印一次 // 与上次告警信息相同，则不更新，仅打印一次
if (alarmCode.all == reg4001) if (alarmCode.all == reg4001)
@ -615,6 +627,16 @@ uint8_t InitPump(void) {
SetPumpStepAcc(dp.pump[1].id, dp.pump[1].maxAccel); SetPumpStepAcc(dp.pump[1].id, dp.pump[1].maxAccel);
SetPumpStepDec(dp.pump[1].id, dp.pump[1].maxDecel); SetPumpStepDec(dp.pump[1].id, dp.pump[1].maxDecel);
SetPumpStepSpeed(dp.pump[1].id, dp.pump[1].maxSpeed); 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 index 阀门索引
* @param torque 力矩值 * @param torque 力矩值，百分比，太低容易误判，默认为60即60%
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetValveHomeTorque(uint8_t index, uint16_t torque) { 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 index 阀门索引
* @param time 检测时间(ms) * @param time 检测时间(ms)，太短容易误判，默认为1000ms
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetValveHomeTime(uint8_t index, uint16_t time) { 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 阀门索引 * @param index 阀门索引
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
uint8_t ValveBackToOrigin(uint8_t index) { uint8_t ValveBackToOrigin(uint8_t index,uint8_t direction) {
// 1.设置原点回归方式 // 1.设置原点回归方式
// (0416h)=37;17=负限位，18=正限位 // (0416h)=37;17=负限位，18=正限位
SetValveHomeDetectMode(index, 37); if(direction) {
SetValveHomeDetectMode(index, 37);//正方向堵转
}
else {
SetValveHomeDetectMode(index, 38);//反方向堵转
}
// 2.设置堵转检测力矩和堵转检测时间 // 2.设置堵转检测力矩和堵转检测时间
// (0170h)=300，(0172h)=50 // (0170h)=300，(0172h)=50
SetValveHomeTorque(index, 300);//30% SetValveHomeTorque(index, 600);//60%
SetValveHomeTime(index, 50);//50ms SetValveHomeTime(index, 10000);//1000ms
// 3.写 (00B1h)=0、运行模式 (03C2h)=0x06，使其工作在原点回归模式； // 3.写 (00B1h)=0、运行模式 (03C2h)=0x06，使其工作在原点回归模式；
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402); // SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
SetValveRunMode(index, RTU_VALVE_CFG_MODE_HM); // SetValveRunMode(index, RTU_VALVE_CFG_MODE_HM);
// 4.写寻找限位开关速度和寻找原点信号速度(0417h)= 10000 (0419h)=1000; // 4.写寻找限位开关速度和寻找原点信号速度(0417h)= 10000 (0419h)=1000;
SetValveHomeSwtSpeed(index, 10000); // SetValveHomeSwtSpeed(index, 20000);
SetValveHomeOriSpeed(index, 1000); SetValveHomeOriSpeed(index, 20000);
// 5.设置回零加速度 // 5.设置回零加速度
SetValveHomeAcc(index, 200000); SetValveHomeAcc(index, 200000);
// 6.写控制字 // 6.写控制字
// (0380h)= 0x06→0x07→0x0F→0x1F，电机运行 // (0380h)= 0x06→0x07→0x0F→0x1F，电机运行
SetValveFunc(index, RTU_VALVE_CFG_PREPARE); // SetValveFunc(index, RTU_VALVE_CFG_PREPARE);
SetValveFunc(index, RTU_VALVE_CFG_DISABLE); // SetValveFunc(index, RTU_VALVE_CFG_DISABLE);
SetValveFunc(index, RTU_VALVE_CFG_ENABLE); // SetValveFunc(index, RTU_VALVE_CFG_ENABLE);
SetValveFunc(index, RTU_VALVE_CFG_RUN_ORIGIN); SetValveFunc(index, RTU_VALVE_CFG_RUN_ORIGIN);
} }
@ -1224,22 +1251,31 @@ CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen) {
void runPumpDemo(void) { void runPumpDemo(void) {
printf("runPumpDemo\n"); printf("runPumpDemo\r\n");
printf("InitPump\n"); // printf("InitPump\n");
InitPump(); InitPump();
// 泵1正转100步 // 泵1正转100步
printf("SetPumpStepTarget(0, 100)\n"); printf("SetPumpStepTarget(0, 100)\n");
SetPumpStepTarget(0, 100); SetPumpStepTarget(0, 200000);
printf("StartPumpRelativeMove(0)\n"); printf("StartPumpRelativeMove(0)\n");
StartPumpRelativeMove(0); StartPumpRelativeMove(0);
HAL_Delay(1000); HAL_Delay(1000);
// 泵1反转100步 // 泵1反转100步
printf("SetPumpStepTarget(0, -100)\n"); printf("SetPumpStepTarget(0, -100)\n");
SetPumpStepTarget(0, -100); SetPumpStepTarget(1, -300000);
printf("StartPumpRelativeMove(0)\n"); printf("StartPumpRelativeMove(0)\n");
StartPumpRelativeMove(0); StartPumpRelativeMove(1);
HAL_Delay(1000); HAL_Delay(1000);
printf("StopPump(0)\n"); printf("StopPump(0)\n");
StopPump(0); StopPump(0);
HAL_Delay(1000); 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
View File

@ -29,6 +29,12 @@
#define HOST_CMD_SYSTEM_INIT 0x0007 // 系统初始化 #define HOST_CMD_SYSTEM_INIT 0x0007 // 系统初始化
#define CMD_BR_SET 0x0008 // 波特率设置 #define CMD_BR_SET 0x0008 // 波特率设置
#define CMD_PR_SET 0x0009 // 通信协议设置 #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 { typedef enum {
CMD_FRAME_OK = 0, 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_CO 0x007C // 操作码，Command Opcode
#define RTU_PUMP_CMD_SC 0x4002 // 状态寄存器 #define RTU_PUMP_CMD_SC 0x0002 // 状态寄存器
#define RTU_PUMP_CMD_AL 0x4001 // 告警寄存器 #define RTU_PUMP_CMD_AL 0x0001 // 告警寄存器