You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
465 lines
14 KiB
465 lines
14 KiB
#include "hardware.hpp"
|
|
|
|
#include <stddef.h>
|
|
#include <stdio.h>
|
|
|
|
#include "main.h"
|
|
#include "project.hpp"
|
|
#include <string.h>
|
|
//
|
|
// #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp"
|
|
|
|
#include "sdk/hal/zhal.hpp"
|
|
|
|
#include "sdk\components\m3078\m3078_code_scaner.hpp"
|
|
#include "sdk\components\tmc\ic\ztmc4361A.hpp"
|
|
#include "sdk\components\tmc\ic\ztmc5130.hpp"
|
|
//
|
|
#include "driver/preportional_valve_ctrl.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"
|
|
|
|
using namespace iflytop;
|
|
#define TAG "main"
|
|
|
|
TMC5130 m_motor1;
|
|
TMC5130 m_motor2;
|
|
|
|
ZGPIO triLight_R;
|
|
ZGPIO triLight_G;
|
|
ZGPIO triLight_Y;
|
|
ZGPIO triLight_BEEP;
|
|
|
|
ZCanBasicOrderModule m_basicOrderModule;
|
|
ZCanPumpCtrlModule m_pumpCtrlModule;
|
|
ZCanTrigleWarningLightCtlModule m_warningLightCtlModule;
|
|
HuachengPressureSensor m_huachengPressureSensor;
|
|
|
|
DP600PressureSensor m_dp600PressureSensor2;
|
|
DP600PressureSensor m_dp600PressureSensor3;
|
|
DP600PressureSensor m_dp600PressureSensor4;
|
|
|
|
ZGPIO IO_PD13_IN;
|
|
ZGPIO IO_PC7_IN;
|
|
|
|
ZGPIO OUT_PD14;
|
|
ZGPIO OUT_PD15;
|
|
|
|
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);
|
|
}
|
|
|
|
PreportionalValveCtrl m_PreportionalValveHost;
|
|
|
|
int32_t preportional_valve_is_busy(int32_t *busy) {
|
|
int32_t valve1state = 0;
|
|
int32_t valve2state = 0;
|
|
int32_t err = 0;
|
|
*busy = 1;
|
|
|
|
err = m_PreportionalValveHost.isBusy(1, &valve1state);
|
|
if (err != 0) return err;
|
|
err = m_PreportionalValveHost.isBusy(2, &valve2state);
|
|
if (err != 0) return err;
|
|
|
|
if (valve1state == 0 && valve2state == 0) {
|
|
*busy = 0;
|
|
} else {
|
|
*busy = 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void air_compressor_read_pressure(int32_t *ack) { //
|
|
DP600PressureSensor::sensor_data_t d = m_huachengPressureSensor.readsensordata(2);
|
|
*ack = d.value;
|
|
}
|
|
|
|
void Hardware::initialize(int deviceId) {
|
|
m_device_id = deviceId;
|
|
IO_PD13_IN.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
|
|
IO_PC7_IN.initAsInput(PC7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
|
|
{
|
|
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);
|
|
|
|
while (!m_motor1.ping()) {
|
|
ZLOGI(TAG, "waitting for motor1 become ready");
|
|
HAL_Delay(100);
|
|
}
|
|
m_motor1.reInitialize();
|
|
|
|
m_motor1.setIHOLD_IRUN(1, 20, 0);
|
|
m_motor1.setMotorShaft(true);
|
|
|
|
m_motor1.setAcceleration(300000);
|
|
m_motor1.setDeceleration(300000);
|
|
// m_motor1.rotate(1000000);
|
|
|
|
auto gstate = m_motor1.getGState();
|
|
ZLOGI(TAG, "motor1: reset:%d drv_err:%d uv_cp:%d", gstate.reset, gstate.drv_err, gstate.uv_cp);
|
|
}
|
|
|
|
{
|
|
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);
|
|
|
|
while (!m_motor2.ping()) {
|
|
ZLOGI(TAG, "waitting for motor2 become ready");
|
|
HAL_Delay(100);
|
|
}
|
|
|
|
m_motor2.reInitialize();
|
|
|
|
m_motor2.setIHOLD_IRUN(1, 20, 0); // 5W
|
|
m_motor2.setMotorShaft(true);
|
|
|
|
m_motor2.setAcceleration(300000);
|
|
m_motor2.setDeceleration(300000);
|
|
|
|
auto gstate = m_motor2.getGState();
|
|
ZLOGI(TAG, "motor2: reset:%d drv_err:%d uv_cp:%d", gstate.reset, gstate.drv_err, gstate.uv_cp);
|
|
// m_motor2.rotate(1000000);
|
|
}
|
|
|
|
triLight_R.initAsOutput(PD8, ZGPIO::kMode_nopull, true, false);
|
|
triLight_G.initAsOutput(PD7, ZGPIO::kMode_nopull, true, false);
|
|
triLight_Y.initAsOutput(PD9, ZGPIO::kMode_nopull, true, false);
|
|
triLight_BEEP.initAsOutput(PD10, ZGPIO::kMode_nopull, true, false);
|
|
|
|
m_PreportionalValveHost.initialize(&huart2);
|
|
|
|
#if 1
|
|
m_dp600PressureSensor2.initialize(&huart3, 2);
|
|
m_dp600PressureSensor3.initialize(&huart3, 3);
|
|
m_dp600PressureSensor4.initialize(&huart3, 4);
|
|
OUT_PD14.initAsOutput(PD14, ZGPIO::kMode_nopull, false, true);
|
|
OUT_PD15.initAsOutput(PD15, ZGPIO::kMode_nopull, true, false);
|
|
#endif
|
|
}
|
|
|
|
void air_compressor_ch_select(int32_t val) {
|
|
if (val == 2) { // 内管路
|
|
OUT_PD15.setState(1);
|
|
} else if (val == 1) { // 空气
|
|
OUT_PD15.setState(0);
|
|
}
|
|
}
|
|
|
|
void dumpdp600data(DP600PressureSensor::sensor_data_t *data) {
|
|
ZLOGI(TAG, "value:%d", data->value);
|
|
ZLOGI(TAG, "zero_point:%d", data->zero_point);
|
|
ZLOGI(TAG, "range_full_point:%d", data->range_full_point);
|
|
ZLOGI(TAG, "precision:%d", data->precision);
|
|
ZLOGI(TAG, "pressure_unit:%d", data->pressure_unit);
|
|
}
|
|
|
|
void packet_kcmd_read_huacheng_pressure_sensor_data(int id, DP600PressureSensor::sensor_data_t *dp600data, uint8_t *receipt, int32_t &receiptsize) {
|
|
receipt[0] = id;
|
|
receipt[1] = 0;
|
|
receipt[2] = dp600data->precision;
|
|
receipt[3] = dp600data->pressure_unit;
|
|
memcpy(receipt + 4, &dp600data->value, 2);
|
|
memcpy(receipt + 6, &dp600data->zero_point, 2);
|
|
memcpy(receipt + 8, &dp600data->range_full_point, 2);
|
|
receiptsize = 10;
|
|
}
|
|
|
|
int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int32_t len, uint8_t *receipt, int32_t &receiptsize, bool &matching) {
|
|
Cmdheader_t *cmdheader = (Cmdheader_t *)packet;
|
|
if (fromwhere == kuart) printf("rx: cmdid:%d subcmdid:%d id:%d\n", cmdheader->cmdid, cmdheader->subcmdid, cmdheader->data[0]);
|
|
|
|
/**
|
|
* @brief Ping
|
|
*/
|
|
PROCESS_CMD(kcmd_ping, 0, m_device_id) {
|
|
receipt[0] = cmdheader->data[0];
|
|
receiptsize = 1;
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief 控制加液泵
|
|
*/
|
|
PROCESS_CMD(kcmd_peristaltic_pump_ctl, 1, 1) {
|
|
int16_t acc = *(int16_t *)(&cmdheader->data[2]);
|
|
int16_t rpm = *(int16_t *)(&cmdheader->data[4]);
|
|
int16_t idlepower = cmdheader->data[6];
|
|
int16_t power = cmdheader->data[7];
|
|
|
|
printf("kcmd_peristaltic_pump_ctl 1 acc:%d rpm:%d idlepower:%d power:%d\n", acc, rpm, idlepower, power);
|
|
|
|
setmotor1(acc, rpm, idlepower, power);
|
|
receipt[0] = cmdheader->data[0];
|
|
receiptsize = 1;
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief 控制-喷液泵
|
|
*/
|
|
PROCESS_CMD(kcmd_peristaltic_pump_ctl, 1, 2) {
|
|
int16_t acc = *(int16_t *)(&cmdheader->data[2]);
|
|
int16_t rpm = *(int16_t *)(&cmdheader->data[4]);
|
|
int16_t idlepower = cmdheader->data[6];
|
|
int16_t power = cmdheader->data[7];
|
|
printf("kcmd_peristaltic_pump_ctl 2 acc:%d rpm:%d idlepower:%d power:%d\n", acc, rpm, idlepower, power);
|
|
|
|
setmotor2(acc, rpm, idlepower, power);
|
|
receipt[0] = cmdheader->data[0];
|
|
receiptsize = 1;
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief 三色指示灯控制
|
|
*/
|
|
PROCESS_CMD(kcmd_triple_warning_light_ctl, 0, 1) {
|
|
/**
|
|
* @brief 0:设置状态
|
|
* cmd:
|
|
* [0]:SENSORID
|
|
* [2]:红色灯状态
|
|
* [3]:黄色灯状态
|
|
* [4]:绿色灯状态
|
|
* [5]:蜂鸣器状态
|
|
* ack : b0:id
|
|
* ack_datalen : 1
|
|
*/
|
|
uint8_t id = cmdheader->data[0];
|
|
uint8_t r = cmdheader->data[2];
|
|
uint8_t g = cmdheader->data[3];
|
|
uint8_t b = cmdheader->data[4];
|
|
uint8_t beep = cmdheader->data[5];
|
|
|
|
triLight_R.setState(r != 0);
|
|
triLight_G.setState(g != 0);
|
|
triLight_Y.setState(b != 0);
|
|
triLight_BEEP.setState(beep != 0);
|
|
|
|
receipt[0] = cmdheader->data[0];
|
|
receiptsize = 1;
|
|
}
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_air_compressor_valve1_set) && (cmdheader->subcmdid == 0)) {
|
|
uint32_t val = *(uint32_t *)(&cmdheader->data[0]);
|
|
ZLOGI(TAG, "kcmd_air_compressor_valve1_set:%d", val);
|
|
OUT_PD14.setState(val != 0);
|
|
matching = true;
|
|
}
|
|
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_air_compressor_valve2_set) && (cmdheader->subcmdid == 0)) {
|
|
uint32_t val = *(uint32_t *)(&cmdheader->data[0]);
|
|
ZLOGI(TAG, "kcmd_air_compressor_valve2_set:%d", val);
|
|
OUT_PD14.setState(val != 0);
|
|
matching = true;
|
|
}
|
|
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_proportional_read_state) && (cmdheader->subcmdid == 0)) {
|
|
matching = true;
|
|
|
|
int32_t ack = 0;
|
|
int32_t ecode = preportional_valve_is_busy(&ack);
|
|
|
|
int32_t *p_receipt_32 = (int32_t *)receipt;
|
|
*p_receipt_32 = ack;
|
|
receiptsize = 4;
|
|
}
|
|
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_proportional_set_valve) && (cmdheader->subcmdid == 0)) {
|
|
int32_t para0 = *(int32_t *)(&cmdheader->data[0]);
|
|
int32_t para1 = *(int32_t *)(&cmdheader->data[4]);
|
|
|
|
if (para0 == 1) {
|
|
matching = true;
|
|
m_PreportionalValveHost.setValvePos(1, para1);
|
|
} else if (para0 == 2) {
|
|
matching = true;
|
|
m_PreportionalValveHost.setValvePos(2, para1);
|
|
}
|
|
}
|
|
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_air_compressor_ch_select) && (cmdheader->subcmdid == 0)) {
|
|
int32_t para0 = *(int32_t *)(&cmdheader->data[0]);
|
|
air_compressor_ch_select(para0);
|
|
}
|
|
static DP600PressureSensor::sensor_data_t dp600data;
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_air_compressor_read_pressure) && (cmdheader->subcmdid == 0)) {
|
|
matching = true;
|
|
bool suc = m_dp600PressureSensor2.readVal(&dp600data);
|
|
if (!suc) return 1002;
|
|
int32_t *p_receipt_32 = (int32_t *)receipt;
|
|
*p_receipt_32 = dp600data.value;
|
|
receiptsize = 4;
|
|
}
|
|
|
|
#if 1
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_read_evaporation_bin_water_detection)) {
|
|
matching = true;
|
|
|
|
((int32_t *)receipt)[0] = IO_PC7_IN.getState(); // 高有效
|
|
receiptsize = 4;
|
|
return 0;
|
|
}
|
|
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_read_device_bottom_water_detection_sensor)) {
|
|
matching = true;
|
|
((int32_t *)receipt)[0] = !IO_PD13_IN.getState(); // 低有效
|
|
receiptsize = 4;
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
/**
|
|
* @brief 液位测量压力传感器
|
|
*/
|
|
PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 0, 1) {
|
|
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 1002;
|
|
dp600data.precision = 3;
|
|
dp600data.pressure_unit = 1;
|
|
dp600data.value = val[0];
|
|
dp600data.zero_point = 0;
|
|
dp600data.range_full_point = 0;
|
|
packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
|
|
if (fromwhere == kuart) {
|
|
dumpdp600data(&dp600data);
|
|
}
|
|
return 0;
|
|
}
|
|
/**
|
|
* @brief 空压机压力传感器
|
|
*/
|
|
PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 0, 2) {
|
|
bool suc = m_dp600PressureSensor2.readVal(&dp600data);
|
|
if (!suc) return 1002;
|
|
packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
|
|
if (fromwhere == kuart) {
|
|
dumpdp600data(&dp600data);
|
|
}
|
|
return 0;
|
|
}
|
|
/**
|
|
* @brief 加液蠕动泵压力传感器
|
|
*/
|
|
PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 0, 3) {
|
|
bool suc = m_dp600PressureSensor3.readVal(&dp600data);
|
|
if (!suc) return 1002;
|
|
packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
|
|
if (fromwhere == kuart) {
|
|
dumpdp600data(&dp600data);
|
|
}
|
|
return 0;
|
|
}
|
|
/**
|
|
* @brief 喷射蠕动泵压力传感器
|
|
*/
|
|
PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 0, 4) {
|
|
bool suc = m_dp600PressureSensor4.readVal(&dp600data);
|
|
if (!suc) return 1002;
|
|
packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
|
|
if (fromwhere == kuart) {
|
|
dumpdp600data(&dp600data);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
void Hardware::loop() {}
|
|
|
|
TMC5130 *Hardware::getMotor1() { return &m_motor1; }
|
|
TMC5130 *Hardware::getMotor2() { return &m_motor2; }
|
|
|
|
typedef struct motor_state_cache_t {
|
|
int16_t acc_rpm2;
|
|
int16_t rpm;
|
|
int16_t idlepower;
|
|
int16_t power;
|
|
};
|
|
|
|
motor_state_cache_t m_motor1_cache;
|
|
motor_state_cache_t m_motor2_cache;
|
|
|
|
void Hardware::setmotor1(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { //
|
|
setmotor(&m_motor1, acc_rpm2, rpm, idlepower, power);
|
|
|
|
m_motor1_cache.acc_rpm2 = acc_rpm2;
|
|
m_motor1_cache.rpm = rpm;
|
|
m_motor1_cache.idlepower = idlepower;
|
|
m_motor1_cache.power = power;
|
|
}
|
|
void Hardware::recovermotor1() {
|
|
m_motor1.reInitialize();
|
|
setmotor(&m_motor1, m_motor1_cache.acc_rpm2, m_motor1_cache.rpm, m_motor1_cache.idlepower, m_motor1_cache.power);
|
|
}
|
|
bool Hardware::ismotor1error() {
|
|
DevStatusReg_t sreg1 = m_motor1.getDevStatus();
|
|
GState_t gstate1 = m_motor1.getGState();
|
|
|
|
if (gstate1.reset) {
|
|
ZLOGE(TAG, "motor1 driver trigger illegal reset");
|
|
return true;
|
|
}
|
|
|
|
if (sreg1.ola || sreg1.olb) {
|
|
ZLOGE(TAG, "motor1 driver trigger ola olb");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void Hardware::setmotor2(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { //
|
|
setmotor(&m_motor2, acc_rpm2, rpm, idlepower, power);
|
|
|
|
m_motor2_cache.acc_rpm2 = acc_rpm2;
|
|
m_motor2_cache.rpm = rpm;
|
|
m_motor2_cache.idlepower = idlepower;
|
|
m_motor2_cache.power = power;
|
|
}
|
|
void Hardware::recovermotor2() {
|
|
m_motor2.reInitialize();
|
|
setmotor(&m_motor2, m_motor2_cache.acc_rpm2, m_motor2_cache.rpm, m_motor2_cache.idlepower, m_motor2_cache.power);
|
|
}
|
|
bool Hardware::ismotor2error() {
|
|
DevStatusReg_t sreg2 = m_motor2.getDevStatus();
|
|
GState_t gstate2 = m_motor2.getGState();
|
|
|
|
if (gstate2.reset) {
|
|
ZLOGE(TAG, "motor2 driver trigger illegal reset");
|
|
return true;
|
|
}
|
|
|
|
if (sreg2.ola || sreg2.olb) {
|
|
ZLOGE(TAG, "motor2 driver trigger ola olb");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|