pipeline_disinfection_liquid_path_control/usrc/main.cpp

#include "main.hpp"

#include <stddef.h>
#include <stdio.h>

#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 <stdint.h>

#include "preportional_valve_ctrl.hpp"
#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp"
#include "sdk\hal\zuart.hpp"
#include "iwdg.h"

#define TAG "main"
namespace iflytop {
Main gmain;
};

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() {
  ZHALCORE::cfg_t oscfg = {
      .delayhtim = &DELAY_US_TIMER,
      .debuguart = &DEBUG_UART,
  };
  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();
  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);
  }
}