From 5ba95a6d2bbcfe58ef08994b4f292cef7c623968 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 14 Nov 2023 20:13:17 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=AF=94=E4=BE=8B=E9=98=80?= =?UTF-8?q?=E8=AF=BB=E5=8F=96=E7=8A=B6=E6=80=81=E7=9A=84=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .settings/language.settings.xml | 4 +- sdk | 2 +- usrc/can_cmd_reg_fn_reg.cpp | 75 +++++++ usrc/device.cpp | 225 +++++++++++++++++++ usrc/device.hpp | 69 ++++++ usrc/driver/preportional_valve_ctrl.cpp | 29 +++ usrc/driver/preportional_valve_ctrl.hpp | 43 ++++ usrc/main.cpp | 376 +------------------------------- usrc/main.hpp | 30 --- usrc/preportional_valve_ctrl.cpp | 29 --- usrc/preportional_valve_ctrl.hpp | 43 ---- usrc/uart_debug_fn_reg.cpp | 83 +++++++ 12 files changed, 536 insertions(+), 472 deletions(-) create mode 100644 usrc/can_cmd_reg_fn_reg.cpp create mode 100644 usrc/device.cpp create mode 100644 usrc/device.hpp create mode 100644 usrc/driver/preportional_valve_ctrl.cpp create mode 100644 usrc/driver/preportional_valve_ctrl.hpp delete mode 100644 usrc/main.hpp delete mode 100644 usrc/preportional_valve_ctrl.cpp delete mode 100644 usrc/preportional_valve_ctrl.hpp create mode 100644 usrc/uart_debug_fn_reg.cpp diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 6d9848e..1c1c4fc 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/sdk b/sdk index 8ed37f5..9668eec 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 8ed37f58ad5bf01d82f6c8996e866a6872527e3b +Subproject commit 9668eec9f1c1536ed57d72b5a760851455ac5042 diff --git a/usrc/can_cmd_reg_fn_reg.cpp b/usrc/can_cmd_reg_fn_reg.cpp new file mode 100644 index 0000000..bd2f25c --- /dev/null +++ b/usrc/can_cmd_reg_fn_reg.cpp @@ -0,0 +1,75 @@ +#include "device.hpp" + +using namespace iflytop; +using namespace std; + +namespace iflytop { +ZCanTrigleWarningLightCtlModule m_warningLightCtlModule; + +void can_cmd_reg_fn_reg() { + // registerListener + + m_warningLightCtlModule.initialize(&m_canReceiver); + m_warningLightCtlModule.regSubmodule(1, [&](uint8_t r, uint8_t g, uint8_t b, uint8_t beep) { triple_warning_light_ctl(r, g, b, beep); }); + + m_canReceiver.registerListener([](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { + Cmdheader_t *cmdheader = (Cmdheader_t *)packet; + if (IS_CMD(cmdheader, kcmd_set_proportional_valve, 0)) { + int32_t valueid = CMD_GET_PARAM(cmdheader, 0); + int32_t value = CMD_GET_PARAM(cmdheader, 1); + if (valueid == 1) { + m_PreportionalValveHost.setValvePos(1, value); + m_canReceiver.sendAck(cmdheader, NULL, 0); + } else if (valueid == 2) { + m_PreportionalValveHost.setValvePos(2, value); + m_canReceiver.sendAck(cmdheader, NULL, 0); + } + } + }); + + m_canReceiver.registerListener([](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { + Cmdheader_t *cmdheader = (Cmdheader_t *)packet; + if (IS_CMD(cmdheader, kcmd_air_compressor_read_pressure, 0)) { + int32_t ack = 0; + air_compressor_read_pressure(&ack); + m_canReceiver.sendAck(cmdheader, (uint8_t *)&ack, 4); + } + }); + m_canReceiver.registerListener([](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { + Cmdheader_t *cmdheader = (Cmdheader_t *)packet; + if (IS_CMD(cmdheader, kcmd_air_compressor_ch_select, 0)) { + air_compressor_ch_select(CMD_GET_PARAM(cmdheader, 0)); + m_canReceiver.sendAck(cmdheader, NULL, 0); + } + }); + m_canReceiver.registerListener([](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { + Cmdheader_t *cmdheader = (Cmdheader_t *)packet; + if (!IS_CMD(cmdheader, kcmd_air_compressor_valve1_set, 0)) return; + air_compressor_valve1_set(CMD_GET_PARAM(cmdheader, 0)); + m_canReceiver.sendAck(cmdheader, NULL, 0); + }); + + m_canReceiver.registerListener([](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { + Cmdheader_t *cmdheader = (Cmdheader_t *)packet; + if (!IS_CMD(cmdheader, kcmd_air_compressor_valve2_set, 0)) return; + air_compressor_valve2_set(CMD_GET_PARAM(cmdheader, 0)); + m_canReceiver.sendAck(cmdheader, NULL, 0); + }); + m_canReceiver.registerListener([](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { + Cmdheader_t *cmdheader = (Cmdheader_t *)packet; + if (!IS_CMD(cmdheader, kcmd_proportional_read_state, 0)) return; + + int32_t ack = 0; + int32_t ecode = preportional_valve_is_busy(&ack); + if (ecode != 0) { + m_canReceiver.sendErrorAck(cmdheader, ecode); + } else { + m_canReceiver.sendAck(cmdheader, (uint8_t *)&ack, 4); + } + }); + + // int32_t isbusy = 0; + // int32_t ecode = preportional_valve_is_busy(&isbusy); + // ZLOGI(TAG, "ackcode:%d ack:%d", ecode, isbusy); +} +} // namespace iflytop diff --git a/usrc/device.cpp b/usrc/device.cpp new file mode 100644 index 0000000..6304ed1 --- /dev/null +++ b/usrc/device.cpp @@ -0,0 +1,225 @@ +#include "device.hpp" + +#include +#include + +#define TAG "DEV" +namespace iflytop { +IflytopCanProtocolStackProcesser m_protocolStack; + +TMC5130 m_motor1; +TMC5130 m_motor2; + +ZGPIO debuglight; + +ZGPIO triLight_R; +ZGPIO triLight_G; +ZGPIO triLight_B; +ZGPIO triLight_BEEP; + +ZGPIO m_input1; +ZGPIO m_input2; +ZGPIO m_input3; +ZGPIO m_input4; +ZGPIO m_input5; + +ZGPIO OUT_PD14; +ZGPIO OUT_PD15; + +ZCanReceiver m_canReceiver; +ZCanBasicOrderModule m_basicOrderModule; +ZCanPumpCtrlModule m_pumpCtrlModule; +HuachengPressureSensor m_huachengPressureSensor; +PreportionalValveCtrl m_PreportionalValveHost; +CmdSchedulerV2 cmdScheduler; + +void device_init() { + debuglight.initAsOutput(DEBUG_LIGHT_GPIO, ZGPIO::kMode_nopull, false, false); + ZHAL_CORE_REG(200, { debuglight.toggleState(); }); + + ZCanReceiver::CFG *cfg = m_canReceiver.createCFG(DEVICE_ID); + m_canReceiver.init(cfg); + // m_canReceiver.registerListener(this); + + /** + * @brief 基础模块 + */ + m_input1.initAsInput(PD11, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); + m_input2.initAsInput(PC5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); + m_input3.initAsInput(PD12, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); + m_input4.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); + m_input5.initAsInput(PC6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); + m_basicOrderModule.initialize(&m_canReceiver); + m_basicOrderModule.regInputCtl([](uint8_t id, bool &val) { + if (id == 1) { + val = m_input1.getState(); + return true; + } + if (id == 2) { + val = m_input2.getState(); + return true; + } + if (id == 3) { + val = m_input3.getState(); + return true; + } + if (id == 4) { + val = m_input4.getState(); + return true; + } + if (id == 5) { + val = m_input5.getState(); + return true; + } + return false; + }); + + m_PreportionalValveHost.initialize(&huart2); + /******************************************************************************* + * 蠕动泵驱动 * + *******************************************************************************/ + + { + TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR1_ENN, .csn_pin = MOTOR1_CSN}; + + m_motor1.initialize(&cfg); + int32_t chipv = m_motor1.readChipVERSION(); + ZLOGI(TAG, "m_motor1:%lx", chipv); + m_motor1.setIHOLD_IRUN(1, 20, 0); + m_motor1.setMotorShaft(true); + + m_motor1.setAcceleration(300000); + m_motor1.setDeceleration(300000); + // m_motor1.rotate(1000000); + } + + { + TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR2_ENN, .csn_pin = MOTOR2_CSN}; + + m_motor2.initialize(&cfg); + int32_t chipv = m_motor2.readChipVERSION(); + ZLOGI(TAG, "m_motor2:%lx", chipv); + m_motor2.setIHOLD_IRUN(1, 20, 0); // 5W + m_motor2.setMotorShaft(true); + + m_motor2.setAcceleration(300000); + m_motor2.setDeceleration(300000); + // m_motor1.rotate(1000000); + } + + m_pumpCtrlModule.initialize(&m_canReceiver); + m_pumpCtrlModule.regSubmodule(1, [&](int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { + ZLOGI(TAG, "pump1 acc_rpm2:%d rpm:%d", acc_rpm2, rpm); + setmotor(&m_motor1, acc_rpm2, rpm, idlepower, power); + }); + m_pumpCtrlModule.regSubmodule(2, [&](int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { + ZLOGI(TAG, "pump2 acc:%d rpm:%d", acc_rpm2, rpm); + setmotor(&m_motor2, acc_rpm2, rpm, idlepower, power); + }); + + /******************************************************************************* + * 三色指示灯 * + *******************************************************************************/ + + { + triLight_R.initAsOutput(PD8, ZGPIO::kMode_nopull, true, false); + triLight_G.initAsOutput(PD7, ZGPIO::kMode_nopull, true, false); + triLight_B.initAsOutput(PD9, ZGPIO::kMode_nopull, true, false); + triLight_BEEP.initAsOutput(PD10, ZGPIO::kMode_nopull, true, false); + } + + /******************************************************************************* + * 压力传感器 * + *******************************************************************************/ + + { + m_huachengPressureSensor.initialize(&m_canReceiver); + m_huachengPressureSensor.regSubmodule(1, [](DP600PressureSensor::sensor_data_t *data) { // + static ModbusBlockHost modbusBlockHost; + modbusBlockHost.initialize(&huart3); + int16_t val[1] = {0}; + bool suc = modbusBlockHost.readReg03Muti(1, 0x00, (uint16_t *)val, 1, 50); + if (!suc) { + return false; + } + data->precision = 3; + data->pressure_unit = 1; + data->value = val[0]; + data->zero_point = 0; + data->range_full_point = 0; + return true; + }); + m_huachengPressureSensor.regSubmodule(2, &huart3, 2); + m_huachengPressureSensor.regSubmodule(3, &huart3, 3); + m_huachengPressureSensor.regSubmodule(4, &huart3, 4); + } + + OUT_PD14.initAsOutput(PD14, ZGPIO::kMode_nopull, true, false); + OUT_PD15.initAsOutput(PD15, ZGPIO::kMode_nopull, true, false); +} + +void setmotor(TMC5130 *motor, int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { + int32_t ppm = rpm / 60.0 * 51200; + int32_t acc = acc_rpm2 / 60.0 * 51200; + + int16_t _idlepower = 1; + int16_t _power = 31; + + if (idlepower > 0 && idlepower < 31) { + _idlepower = idlepower; + } + if (power > 0 && power < 31) { + _power = power; + } + + motor->setIHOLD_IRUN(_idlepower, _power, 10); // 5W + motor->setAcceleration(acc); + motor->setDeceleration(acc); + motor->rotate(ppm); +} + +void air_compressor_ch_select(int32_t val) { + if (val == 2) { // 内管路 + OUT_PD15.setState(1); + } else if (val == 1) { // 空气 + OUT_PD15.setState(0); + } +} +void air_compressor_valve1_set(int32_t val) { OUT_PD14.setState(val != 0); } +void air_compressor_valve2_set(int32_t val) { OUT_PD14.setState(val != 0); } +void air_compressor_read_pressure(int32_t *ack) { // + DP600PressureSensor::sensor_data_t d = m_huachengPressureSensor.readsensordata(2); + *ack = d.value; +} + +void triple_warning_light_ctl(uint8_t r, uint8_t g, uint8_t b, uint8_t warning) { + triLight_R.setState(r != 0); + triLight_G.setState(g != 0); + triLight_B.setState(b != 0); + triLight_BEEP.setState(warning != 0); +} + +int32_t preportional_valve_is_busy(int32_t *busy) { + int32_t valve1state = 0; + int32_t valve2state = 0; + int32_t err = 0; + *busy = 0; + + err = m_PreportionalValveHost.getValveWorkState(1, &valve1state); + if (err != 0) return err; + err = m_PreportionalValveHost.getValveWorkState(2, &valve2state); + if (err != 0) return err; + if (valve1state == m_PreportionalValveHost.kstate_err_state || valve2state == m_PreportionalValveHost.kstate_err_state) { + *busy = 0; + return 1; + } + + if (valve1state == m_PreportionalValveHost.kstate_stop && valve2state == m_PreportionalValveHost.kstate_stop) { + *busy = 0; + } else { + *busy = 1; + } + return 0; +} + +} // namespace iflytop diff --git a/usrc/device.hpp b/usrc/device.hpp new file mode 100644 index 0000000..9c0caff --- /dev/null +++ b/usrc/device.hpp @@ -0,0 +1,69 @@ +#include +#include +#include + +#include "main.h" +#include "project.hpp" +// +// #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp" +#include "sdk/components/iflytop_can_slave_modules/idcard_reader_service.hpp" +#include "sdk/components/single_axis_motor_control/single_axis_motor_control.hpp" +#include "sdk/hal/zhal.hpp" +#include "sdk\components\iflytop_can_slave_modules\io_control_service.hpp" +#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" +#include "sdk\components\m3078\m3078_code_scaner.hpp" +#include "sdk\components\tmc\ic\ztmc4361A.hpp" +#include "sdk\components\tmc\ic\ztmc5130.hpp" +// +#include "sdk\components\huacheng_sensor\dp600_pressure_sensor.hpp" +#include "sdk\components\zcan_module\huacheng_pressure_sensor.hpp" +#include "sdk\components\zcan_module\zcan_basic_order_module.hpp" +#include "sdk\components\zcan_module\zcan_pump_ctrl_module.hpp" +#include "sdk\components\zcan_module\zcan_trigle_warning_light_ctl_module.hpp" +// + +#include "driver/preportional_valve_ctrl.hpp" +#include "iwdg.h" +#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp" +#include "sdk\hal\zuart.hpp" + +namespace iflytop { +extern IflytopCanProtocolStackProcesser m_protocolStack; + +extern TMC5130 m_motor1; +extern TMC5130 m_motor2; + +extern ZGPIO debuglight; + +extern ZGPIO triLight_R; +extern ZGPIO triLight_G; +extern ZGPIO triLight_B; +extern ZGPIO triLight_BEEP; + +extern ZGPIO m_input1; +extern ZGPIO m_input2; +extern ZGPIO m_input3; +extern ZGPIO m_input4; +extern ZGPIO m_input5; + +extern ZGPIO OUT_PD14; +extern ZGPIO OUT_PD15; + +extern ZCanReceiver m_canReceiver; +extern ZCanBasicOrderModule m_basicOrderModule; +extern ZCanPumpCtrlModule m_pumpCtrlModule; +extern HuachengPressureSensor m_huachengPressureSensor; +extern CmdSchedulerV2 cmdScheduler; +// 比例阀 +extern PreportionalValveCtrl m_PreportionalValveHost; + +void device_init(); +void setmotor(TMC5130 *motor, int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power); +void air_compressor_ch_select(int32_t val); +void air_compressor_valve1_set(int32_t val); +void air_compressor_valve2_set(int32_t val); +void air_compressor_read_pressure(int32_t *ack); +void triple_warning_light_ctl(uint8_t r, uint8_t g, uint8_t b, uint8_t warning); +int32_t preportional_valve_is_busy(int32_t *busy); + +}; // namespace iflytop diff --git a/usrc/driver/preportional_valve_ctrl.cpp b/usrc/driver/preportional_valve_ctrl.cpp new file mode 100644 index 0000000..04d31a8 --- /dev/null +++ b/usrc/driver/preportional_valve_ctrl.cpp @@ -0,0 +1,29 @@ +#include "preportional_valve_ctrl.hpp" +using namespace iflytop; + +#define WORK_STATE_REG 0x0000 +#define CTRL_STATE_REG 0x0001 +#define POS_STATE_REG 0x0013 + +void PreportionalValveCtrl::initialize(UART_HandleTypeDef* huart) { m_modbusBlockHost.initialize(huart); } + +int32_t PreportionalValveCtrl::setValvePos(int32_t valueid, int32_t pos) { // + bool ret = m_modbusBlockHost.writeReg06(valueid, CTRL_STATE_REG, pos, 30); + if (!ret) return 1; + return 0; +} +int32_t PreportionalValveCtrl::getValvePos(int32_t valueid, int32_t* pos) { + // return m_modbusBlockHost.readReg03(valueid, POS_STATE_REG, pos, 30); + uint16_t pos16 = 0; + bool ret = m_modbusBlockHost.readReg03(valueid, POS_STATE_REG, &pos16, 30); + *pos = pos16; + if (!ret) return 1; + return 0; +} +int32_t PreportionalValveCtrl::getValveWorkState(int32_t valueid, int32_t* state) { + uint16_t state16 = 0; + bool ret = m_modbusBlockHost.readReg03(valueid, WORK_STATE_REG, &state16, 30); + *state = state16; + if (!ret) return 1; + return 0; +} \ No newline at end of file diff --git a/usrc/driver/preportional_valve_ctrl.hpp b/usrc/driver/preportional_valve_ctrl.hpp new file mode 100644 index 0000000..061fccd --- /dev/null +++ b/usrc/driver/preportional_valve_ctrl.hpp @@ -0,0 +1,43 @@ +// +// Created by zwsd +// + +#pragma once +#include +#include + +#include "sdk/hal/zhal.hpp" +#include "sdk\components\modbus\modbus_block_host.hpp" + +/** + * @brief + * + * https://iflytop1.feishu.cn/wiki/GQwCwHMqFiaJRwks80ncwaYKnSe + */ + +namespace iflytop { +using namespace std; +class PreportionalValveCtrl { + public: + typedef enum { + kstate_stop = 0x0, + kstate_running_forward = 0xaa, + kstate_running_backward = 0xbb, + kstate_err_state = 0xea, + } work_state_t; + + private: + /* data */ + ModbusBlockHost m_modbusBlockHost; + + public: + PreportionalValveCtrl(){}; + ~PreportionalValveCtrl(){}; + + void initialize(UART_HandleTypeDef* huart); + int32_t setValvePos(int32_t valueid, int32_t pos); + int32_t getValvePos(int32_t valueid, int32_t* pos); + int32_t getValveWorkState(int32_t valueid, int32_t* state); +}; + +} // namespace iflytop \ No newline at end of file diff --git a/usrc/main.cpp b/usrc/main.cpp index f2d15f1..8686adf 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -1,235 +1,13 @@ -#include "main.hpp" - -#include -#include - -#include "main.h" -#include "project.hpp" -// -// #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp" -#include "sdk/components/iflytop_can_slave_modules/idcard_reader_service.hpp" -#include "sdk/components/single_axis_motor_control/single_axis_motor_control.hpp" -#include "sdk/hal/zhal.hpp" -#include "sdk\components\iflytop_can_slave_modules\io_control_service.hpp" -#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" -#include "sdk\components\m3078\m3078_code_scaner.hpp" -#include "sdk\components\tmc\ic\ztmc4361A.hpp" -#include "sdk\components\tmc\ic\ztmc5130.hpp" -// -#include "sdk\components\huacheng_sensor\dp600_pressure_sensor.hpp" -#include "sdk\components\zcan_module\huacheng_pressure_sensor.hpp" -#include "sdk\components\zcan_module\zcan_basic_order_module.hpp" -#include "sdk\components\zcan_module\zcan_pump_ctrl_module.hpp" -#include "sdk\components\zcan_module\zcan_trigle_warning_light_ctl_module.hpp" -// -#include - -#include "preportional_valve_ctrl.hpp" -#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp" -#include "sdk\hal\zuart.hpp" -#include "iwdg.h" - +#include "device.hpp" #define TAG "main" namespace iflytop { -Main gmain; -}; +extern void uart_debug_fn_reg(); +extern void can_cmd_reg_fn_reg(); +}; // namespace iflytop using namespace iflytop; - -IflytopCanProtocolStackProcesser m_protocolStack; - -TMC5130 m_motor1; -TMC5130 m_motor2; - -ZGPIO debuglight; - -ZGPIO triLight_R; -ZGPIO triLight_G; -ZGPIO triLight_B; -ZGPIO triLight_BEEP; - -ZGPIO m_input1; -ZGPIO m_input2; -ZGPIO m_input3; -ZGPIO m_input4; -ZGPIO m_input5; - -ZGPIO OUT_PD14; -ZGPIO OUT_PD15; - -ZCanReceiver m_canReceiver; -ZCanBasicOrderModule m_basicOrderModule; -ZCanPumpCtrlModule m_pumpCtrlModule; -ZCanTrigleWarningLightCtlModule m_warningLightCtlModule; -HuachengPressureSensor m_huachengPressureSensor; -PreportionalValveCtrl m_PreportionalValveHost; -static CmdSchedulerV2 cmdScheduler; - -void setmotor(TMC5130 *motor, int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { - int32_t ppm = rpm / 60.0 * 51200; - int32_t acc = acc_rpm2 / 60.0 * 51200; - - int16_t _idlepower = 1; - int16_t _power = 31; - - if (idlepower > 0 && idlepower < 31) { - _idlepower = idlepower; - } - if (power > 0 && power < 31) { - _power = power; - } - - motor->setIHOLD_IRUN(_idlepower, _power, 10); // 5W - motor->setAcceleration(acc); - motor->setDeceleration(acc); - motor->rotate(ppm); -} - -static void air_compressor_ch_select(int32_t val) { - if (val == 2) { // 内管路 - OUT_PD15.setState(1); - } else if (val == 1) { // 空气 - OUT_PD15.setState(0); - } -} -static void air_compressor_valve1_set(int32_t val) { OUT_PD14.setState(val != 0); } -static void air_compressor_valve2_set(int32_t val) { OUT_PD14.setState(val != 0); } -static void air_compressor_read_pressure(int32_t *ack) { // - DP600PressureSensor::sensor_data_t d = m_huachengPressureSensor.readsensordata(2); - *ack = d.value; -} - -void Main::onRceivePacket(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { - // ZLOGI(TAG, "onRceivePacket from %d %d", rxbuf->id, len); - // for (size_t i = 0; i < len; i++) { - // printf("%02X ", packet[i]); - // } - // printf("\n"); -} -void Main::reg_debug_fn() { - /******************************************************************************* - * 调试方法初始化 * - *******************************************************************************/ - static ZUART uartreceiver; - static ZUART::cfg_t uartreceiver_cfg = { - .name = "uartreceiver", - .huart = &DEBUG_UART, - .rxbuffersize = 512, - .rxovertime_ms = 30, - }; - uartreceiver.initialize(&uartreceiver_cfg); - cmdScheduler.initialize(&uartreceiver); - cmdScheduler.regCMD("setmotor1", "(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power)", 4, - [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - int16_t acc_rpm2 = atoi(paraV[0]); - int16_t rpm = atoi(paraV[1]); - int16_t idlepower = atoi(paraV[2]); - int16_t power = atoi(paraV[3]); - setmotor(&m_motor1, acc_rpm2, rpm, idlepower, power); - ack->setNoneAck(0); - }); - cmdScheduler.regCMD("setmotor2", "(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power)", 4, - [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - int16_t acc_rpm2 = atoi(paraV[0]); - int16_t rpm = atoi(paraV[1]); - int16_t idlepower = atoi(paraV[2]); - int16_t power = atoi(paraV[3]); - setmotor(&m_motor2, acc_rpm2, rpm, idlepower, power); - ack->setNoneAck(0); - }); - cmdScheduler.regCMD("setlight", "(uint8_t r, uint8_t g, uint8_t b, uint8_t beep)", 4, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - uint8_t r = atoi(paraV[0]); - uint8_t g = atoi(paraV[1]); - uint8_t b = atoi(paraV[2]); - uint8_t beep = atoi(paraV[3]); - triLight_R.setState(r != 0); - triLight_G.setState(g != 0); - triLight_B.setState(b != 0); - triLight_BEEP.setState(beep != 0); - ack->setNoneAck(0); - }); - cmdScheduler.regCMD("readio", "()", 0, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - uint8_t sensorid = atoi(paraV[0]); - ZLOGI(TAG, "IO1:%d IO2:%d IO3:%d IO4:%d IO5:%d", m_input1.getState(), m_input2.getState(), m_input3.getState(), m_input4.getState(), m_input5.getState()); - ack->setNoneAck(0); - }); - - cmdScheduler.regCMD("air_compressor_ch_ctrl", "(id,val)", 2, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - air_compressor_ch_select(atoi(paraV[0])); - ack->setNoneAck(0); - }); - - cmdScheduler.regCMD("set_proportional_valve", "(uint8_t id,uint8_t val)", 2, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - uint8_t id = atoi(paraV[0]); - uint8_t val = atoi(paraV[1]); - m_PreportionalValveHost.setValvePos(id, val); - ack->setNoneAck(0); - }); - cmdScheduler.regCMD("pressure_sensor_read", "(id)", 1, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - uint8_t id = atoi(paraV[0]); - DP600PressureSensor::sensor_data_t d = m_huachengPressureSensor.readsensordata(id); - ZLOGI(TAG, "pressure_sensor_read id:%d precision:%d pressure_unit:%d value:%d zero_point:%d range_full_point:%d", // - id, d.precision, d.pressure_unit, d.value, d.zero_point, d.range_full_point); - ack->setNoneAck(0); - }); - cmdScheduler.regCMD("air_compressor_valve1_set", "(value)", 1, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - air_compressor_valve1_set(atoi(paraV[0])); - ack->setNoneAck(0); - }); - cmdScheduler.regCMD("air_compressor_valve2_set", "(value)", 1, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { - air_compressor_valve2_set(atoi(paraV[0])); - ack->setNoneAck(0); - }); -} - -void Main::reg_cancmd_fn() { - // registerListener - m_canReceiver.registerListener([this](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { - Cmdheader_t *cmdheader = (Cmdheader_t *)packet; - if (IS_CMD(cmdheader, kcmd_set_proportional_valve, 0)) { - int32_t valueid = CMD_GET_PARAM(cmdheader, 0); - int32_t value = CMD_GET_PARAM(cmdheader, 1); - if (valueid == 1) { - m_PreportionalValveHost.setValvePos(1, value); - m_canReceiver.sendAck(cmdheader, NULL, 0); - } else if (valueid == 2) { - m_PreportionalValveHost.setValvePos(2, value); - m_canReceiver.sendAck(cmdheader, NULL, 0); - } - } - }); - - m_canReceiver.registerListener([this](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { - Cmdheader_t *cmdheader = (Cmdheader_t *)packet; - if (IS_CMD(cmdheader, kcmd_air_compressor_read_pressure, 0)) { - int32_t ack = 0; - air_compressor_read_pressure(&ack); - m_canReceiver.sendAck(cmdheader, (uint8_t *)&ack, 4); - } - }); - m_canReceiver.registerListener([this](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { - Cmdheader_t *cmdheader = (Cmdheader_t *)packet; - if (IS_CMD(cmdheader, kcmd_air_compressor_ch_select, 0)) { - air_compressor_ch_select(CMD_GET_PARAM(cmdheader, 0)); - m_canReceiver.sendAck(cmdheader, NULL, 0); - } - }); - m_canReceiver.registerListener([this](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { - Cmdheader_t *cmdheader = (Cmdheader_t *)packet; - if (!IS_CMD(cmdheader, kcmd_air_compressor_valve1_set, 0)) return; - air_compressor_valve1_set(CMD_GET_PARAM(cmdheader, 0)); - m_canReceiver.sendAck(cmdheader, NULL, 0); - }); - - m_canReceiver.registerListener([this](CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { - Cmdheader_t *cmdheader = (Cmdheader_t *)packet; - if (!IS_CMD(cmdheader, kcmd_air_compressor_valve2_set, 0)) return; - air_compressor_valve2_set(CMD_GET_PARAM(cmdheader, 0)); - m_canReceiver.sendAck(cmdheader, NULL, 0); - }); -} - -void Main::run() { +extern "C"{ +void umain(void) { ZHALCORE::cfg_t oscfg = { .delayhtim = &DELAY_US_TIMER, .debuguart = &DEBUG_UART, @@ -237,150 +15,14 @@ void Main::run() { ZHALCORE::getInstance()->initialize(oscfg); ZLOGI(TAG, "pipeline_disinfection_liquid_path_control:%s", VERSION); - printf("int32_t %d int %d longint %d\n", sizeof(int32_t), sizeof(int), sizeof(long int)); - - debuglight.initAsOutput(DEBUG_LIGHT_GPIO, ZGPIO::kMode_nopull, false, false); - ZHAL_CORE_REG(200, { debuglight.toggleState(); }); - - ZCanReceiver::CFG *cfg = m_canReceiver.createCFG(DEVICE_ID); - m_canReceiver.init(cfg); - m_canReceiver.registerListener(this); - - /** - * @brief 基础模块 - */ - m_input1.initAsInput(PD11, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); - m_input2.initAsInput(PC5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); - m_input3.initAsInput(PD12, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); - m_input4.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); - m_input5.initAsInput(PC6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); - m_basicOrderModule.initialize(&m_canReceiver); - m_basicOrderModule.regInputCtl([this](uint8_t id, bool &val) { - if (id == 1) { - val = m_input1.getState(); - return true; - } - if (id == 2) { - val = m_input2.getState(); - return true; - } - if (id == 3) { - val = m_input3.getState(); - return true; - } - if (id == 4) { - val = m_input4.getState(); - return true; - } - if (id == 5) { - val = m_input5.getState(); - return true; - } - return false; - }); - - m_PreportionalValveHost.initialize(&huart2); - /******************************************************************************* - * 蠕动泵驱动 * - *******************************************************************************/ - - { - TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR1_ENN, .csn_pin = MOTOR1_CSN}; - - m_motor1.initialize(&cfg); - int32_t chipv = m_motor1.readChipVERSION(); - ZLOGI(TAG, "m_motor1:%lx", chipv); - m_motor1.setIHOLD_IRUN(1, 20, 0); - m_motor1.setMotorShaft(true); - - m_motor1.setAcceleration(300000); - m_motor1.setDeceleration(300000); - // m_motor1.rotate(1000000); - } - - { - TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR2_ENN, .csn_pin = MOTOR2_CSN}; - m_motor2.initialize(&cfg); - int32_t chipv = m_motor2.readChipVERSION(); - ZLOGI(TAG, "m_motor2:%lx", chipv); - m_motor2.setIHOLD_IRUN(1, 20, 0); // 5W - m_motor2.setMotorShaft(true); - - m_motor2.setAcceleration(300000); - m_motor2.setDeceleration(300000); - // m_motor1.rotate(1000000); - } - - m_pumpCtrlModule.initialize(&m_canReceiver); - m_pumpCtrlModule.regSubmodule(1, [&](int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { - ZLOGI(TAG, "pump1 acc_rpm2:%d rpm:%d", acc_rpm2, rpm); - setmotor(&m_motor1, acc_rpm2, rpm, idlepower, power); - }); - m_pumpCtrlModule.regSubmodule(2, [&](int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { - ZLOGI(TAG, "pump2 acc:%d rpm:%d", acc_rpm2, rpm); - setmotor(&m_motor2, acc_rpm2, rpm, idlepower, power); - }); - - /******************************************************************************* - * 三色指示灯 * - *******************************************************************************/ - - { - triLight_R.initAsOutput(PD8, ZGPIO::kMode_nopull, true, false); - triLight_G.initAsOutput(PD7, ZGPIO::kMode_nopull, true, false); - triLight_B.initAsOutput(PD9, ZGPIO::kMode_nopull, true, false); - triLight_BEEP.initAsOutput(PD10, ZGPIO::kMode_nopull, true, false); - - m_warningLightCtlModule.initialize(&m_canReceiver); - m_warningLightCtlModule.regSubmodule(1, [&](uint8_t r, uint8_t g, uint8_t b, uint8_t beep) { - ZLOGI(TAG, "warningLightCtlModule r:%d g:%d b:%d beep:%d", r, g, b, beep); - triLight_R.setState(r != 0); - triLight_G.setState(g != 0); - triLight_B.setState(b != 0); - triLight_BEEP.setState(beep != 0); - }); - } - - /******************************************************************************* - * 压力传感器 * - *******************************************************************************/ - - { - m_huachengPressureSensor.initialize(&m_canReceiver); - m_huachengPressureSensor.regSubmodule(1, [this](DP600PressureSensor::sensor_data_t *data) { // - static ModbusBlockHost modbusBlockHost; - modbusBlockHost.initialize(&huart3); - int16_t val[1] = {0}; - bool suc = modbusBlockHost.readReg03Muti(1, 0x00, (uint16_t *)val, 1, 50); - if (!suc) { - return false; - } - data->precision = 3; - data->pressure_unit = 1; - data->value = val[0]; - data->zero_point = 0; - data->range_full_point = 0; - - return true; - }); - m_huachengPressureSensor.regSubmodule(2, &huart3, 2); - m_huachengPressureSensor.regSubmodule(3, &huart3, 3); - m_huachengPressureSensor.regSubmodule(4, &huart3, 4); - } - - OUT_PD14.initAsOutput(PD14, ZGPIO::kMode_nopull, true, false); - OUT_PD15.initAsOutput(PD15, ZGPIO::kMode_nopull, true, false); - - reg_cancmd_fn(); - reg_debug_fn(); + uart_debug_fn_reg(); + can_cmd_reg_fn_reg(); ZLOGI(TAG, "init done"); while (1) { cmdScheduler.schedule(); ZHALCORE::getInstance()->loop(); HAL_IWDG_Refresh(&hiwdg); - - // uint8_t txbuff[10] = {1,2,3,4,5}; - // m_canReceiver.sendPacket(txbuff,4); } } +} \ No newline at end of file diff --git a/usrc/main.hpp b/usrc/main.hpp deleted file mode 100644 index 5b41c0f..0000000 --- a/usrc/main.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once -#include -#include - -#include - -#include "sdk\components\iflytop_can_slave_modules\device_base_control_service.hpp" -#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" -#include "sdk\components\zcanreceiver\zcanreceiver.hpp" -namespace iflytop { -using namespace std; -class Main : public ZCanRceiverListener { - private: - /* data */ - public: - virtual void onRceivePacket(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len); - - Main(/* args */){}; - ~Main(){}; - void run(); - - void reg_debug_fn(); - void reg_cancmd_fn(); -}; -extern Main gmain; -} // namespace iflytop - -extern "C" { -void umain(void) { iflytop::gmain.run(); } -} diff --git a/usrc/preportional_valve_ctrl.cpp b/usrc/preportional_valve_ctrl.cpp deleted file mode 100644 index 04d31a8..0000000 --- a/usrc/preportional_valve_ctrl.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "preportional_valve_ctrl.hpp" -using namespace iflytop; - -#define WORK_STATE_REG 0x0000 -#define CTRL_STATE_REG 0x0001 -#define POS_STATE_REG 0x0013 - -void PreportionalValveCtrl::initialize(UART_HandleTypeDef* huart) { m_modbusBlockHost.initialize(huart); } - -int32_t PreportionalValveCtrl::setValvePos(int32_t valueid, int32_t pos) { // - bool ret = m_modbusBlockHost.writeReg06(valueid, CTRL_STATE_REG, pos, 30); - if (!ret) return 1; - return 0; -} -int32_t PreportionalValveCtrl::getValvePos(int32_t valueid, int32_t* pos) { - // return m_modbusBlockHost.readReg03(valueid, POS_STATE_REG, pos, 30); - uint16_t pos16 = 0; - bool ret = m_modbusBlockHost.readReg03(valueid, POS_STATE_REG, &pos16, 30); - *pos = pos16; - if (!ret) return 1; - return 0; -} -int32_t PreportionalValveCtrl::getValveWorkState(int32_t valueid, int32_t* state) { - uint16_t state16 = 0; - bool ret = m_modbusBlockHost.readReg03(valueid, WORK_STATE_REG, &state16, 30); - *state = state16; - if (!ret) return 1; - return 0; -} \ No newline at end of file diff --git a/usrc/preportional_valve_ctrl.hpp b/usrc/preportional_valve_ctrl.hpp deleted file mode 100644 index 061fccd..0000000 --- a/usrc/preportional_valve_ctrl.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// -// Created by zwsd -// - -#pragma once -#include -#include - -#include "sdk/hal/zhal.hpp" -#include "sdk\components\modbus\modbus_block_host.hpp" - -/** - * @brief - * - * https://iflytop1.feishu.cn/wiki/GQwCwHMqFiaJRwks80ncwaYKnSe - */ - -namespace iflytop { -using namespace std; -class PreportionalValveCtrl { - public: - typedef enum { - kstate_stop = 0x0, - kstate_running_forward = 0xaa, - kstate_running_backward = 0xbb, - kstate_err_state = 0xea, - } work_state_t; - - private: - /* data */ - ModbusBlockHost m_modbusBlockHost; - - public: - PreportionalValveCtrl(){}; - ~PreportionalValveCtrl(){}; - - void initialize(UART_HandleTypeDef* huart); - int32_t setValvePos(int32_t valueid, int32_t pos); - int32_t getValvePos(int32_t valueid, int32_t* pos); - int32_t getValveWorkState(int32_t valueid, int32_t* state); -}; - -} // namespace iflytop \ No newline at end of file diff --git a/usrc/uart_debug_fn_reg.cpp b/usrc/uart_debug_fn_reg.cpp new file mode 100644 index 0000000..cc127d5 --- /dev/null +++ b/usrc/uart_debug_fn_reg.cpp @@ -0,0 +1,83 @@ +#include "device.hpp" + +#define TAG "UART_D" + +namespace iflytop { +using namespace std; + +void uart_debug_fn_reg() { + static ZUART uartreceiver; + static ZUART::cfg_t uartreceiver_cfg = { + .name = "uartreceiver", + .huart = &DEBUG_UART, + .rxbuffersize = 512, + .rxovertime_ms = 30, + }; + uartreceiver.initialize(&uartreceiver_cfg); + cmdScheduler.initialize(&uartreceiver); + cmdScheduler.regCMD("setmotor1", "(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power)", 4, + [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + int16_t acc_rpm2 = atoi(paraV[0]); + int16_t rpm = atoi(paraV[1]); + int16_t idlepower = atoi(paraV[2]); + int16_t power = atoi(paraV[3]); + setmotor(&m_motor1, acc_rpm2, rpm, idlepower, power); + ack->setNoneAck(0); + }); + cmdScheduler.regCMD("setmotor2", "(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power)", 4, + [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + int16_t acc_rpm2 = atoi(paraV[0]); + int16_t rpm = atoi(paraV[1]); + int16_t idlepower = atoi(paraV[2]); + int16_t power = atoi(paraV[3]); + setmotor(&m_motor2, acc_rpm2, rpm, idlepower, power); + ack->setNoneAck(0); + }); + cmdScheduler.regCMD("setlight", "(uint8_t r, uint8_t g, uint8_t b, uint8_t beep)", 4, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + uint8_t r = atoi(paraV[0]); + uint8_t g = atoi(paraV[1]); + uint8_t b = atoi(paraV[2]); + uint8_t beep = atoi(paraV[3]); + triple_warning_light_ctl(r, g, b, beep); + ack->setNoneAck(0); + }); + cmdScheduler.regCMD("readio", "()", 0, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + uint8_t sensorid = atoi(paraV[0]); + ZLOGI(TAG, "IO1:%d IO2:%d IO3:%d IO4:%d IO5:%d", m_input1.getState(), m_input2.getState(), m_input3.getState(), m_input4.getState(), m_input5.getState()); + ack->setNoneAck(0); + }); + + cmdScheduler.regCMD("air_compressor_ch_ctrl", "(id,val)", 2, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + air_compressor_ch_select(atoi(paraV[0])); + ack->setNoneAck(0); + }); + + cmdScheduler.regCMD("set_proportional_valve", "(uint8_t id,uint8_t val)", 2, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + uint8_t id = atoi(paraV[0]); + uint8_t val = atoi(paraV[1]); + m_PreportionalValveHost.setValvePos(id, val); + ack->setNoneAck(0); + }); + cmdScheduler.regCMD("pressure_sensor_read", "(id)", 1, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + uint8_t id = atoi(paraV[0]); + DP600PressureSensor::sensor_data_t d = m_huachengPressureSensor.readsensordata(id); + ZLOGI(TAG, "pressure_sensor_read id:%d precision:%d pressure_unit:%d value:%d zero_point:%d range_full_point:%d", // + id, d.precision, d.pressure_unit, d.value, d.zero_point, d.range_full_point); + ack->setNoneAck(0); + }); + cmdScheduler.regCMD("air_compressor_valve1_set", "(value)", 1, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + air_compressor_valve1_set(atoi(paraV[0])); + ack->setNoneAck(0); + }); + cmdScheduler.regCMD("air_compressor_valve2_set", "(value)", 1, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + air_compressor_valve2_set(atoi(paraV[0])); + ack->setNoneAck(0); + }); + cmdScheduler.regCMD("preportional_valve_is_busy", "()", 0, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) { + int32_t isbusy = 0; + int32_t ecode = preportional_valve_is_busy(&isbusy); + ZLOGI(TAG, "ackcode:%d ack:%d", ecode, isbusy); + ack->setNoneAck(0); + }); +} +} // namespace iflytop