|
|
@ -5,13 +5,12 @@ |
|
|
|
|
|
|
|
#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/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"
|
|
|
@ -68,6 +67,7 @@ void setmotor(TMC5130 *motor, int16_t acc_rpm2, int16_t rpm, int16_t idlepower, |
|
|
|
motor->setDeceleration(acc); |
|
|
|
motor->rotate(ppm); |
|
|
|
} |
|
|
|
|
|
|
|
PreportionalValveCtrl m_PreportionalValveHost; |
|
|
|
|
|
|
|
int32_t preportional_valve_is_busy(int32_t *busy) { |
|
|
@ -96,8 +96,8 @@ void air_compressor_read_pressure(int32_t *ack) { // |
|
|
|
|
|
|
|
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*/);
|
|
|
|
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}; |
|
|
|
|
|
|
@ -214,7 +214,7 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int |
|
|
|
|
|
|
|
printf("kcmd_peristaltic_pump_ctl 1 acc:%d rpm:%d idlepower:%d power:%d\n", acc, rpm, idlepower, power); |
|
|
|
|
|
|
|
setmotor(&m_motor1, acc, rpm, idlepower, power); |
|
|
|
setmotor1(acc, rpm, idlepower, power); |
|
|
|
receipt[0] = cmdheader->data[0]; |
|
|
|
receiptsize = 1; |
|
|
|
return 0; |
|
|
@ -230,7 +230,7 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int |
|
|
|
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); |
|
|
|
|
|
|
|
setmotor(&m_motor2, acc, rpm, idlepower, power); |
|
|
|
setmotor2(acc, rpm, idlepower, power); |
|
|
|
receipt[0] = cmdheader->data[0]; |
|
|
|
receiptsize = 1; |
|
|
|
return 0; |
|
|
@ -290,7 +290,7 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int |
|
|
|
receiptsize = 4; |
|
|
|
} |
|
|
|
|
|
|
|
if ((cmdheader->cmdid == (uint16_t)kcmd_set_proportional_valve) && (cmdheader->subcmdid == 0)) { |
|
|
|
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]); |
|
|
|
|
|
|
@ -317,25 +317,21 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int |
|
|
|
receiptsize = 4; |
|
|
|
} |
|
|
|
|
|
|
|
#if 0
|
|
|
|
/**
|
|
|
|
* @brief 获取蒸发仓水浸状态 |
|
|
|
*/ |
|
|
|
PROCESS_CMD(kcmd_proportional_read_water_immersion_sensor, 0, 0) { |
|
|
|
((int32_t *)receipt)[0] = !IO_PC7_IN.getState(); |
|
|
|
#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; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* @brief 获取设备底盘水浸传感器 |
|
|
|
*/ |
|
|
|
PROCESS_CMD(kcmd_proportional_read_water_immersion_sensor, 0, 1) { |
|
|
|
((int32_t *)receipt)[0] = !IO_PD13_IN.getState(); |
|
|
|
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
|
|
|
|
|
|
|
|
/**
|
|
|
@ -400,4 +396,70 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int |
|
|
|
void Hardware::loop() {} |
|
|
|
|
|
|
|
TMC5130 *Hardware::getMotor1() { return &m_motor1; } |
|
|
|
TMC5130 *Hardware::getMotor2() { return &m_motor2; } |
|
|
|
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; |
|
|
|
} |