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.
255 lines
8.3 KiB
255 lines
8.3 KiB
#include "main.hpp"
|
|
|
|
#include <stddef.h>
|
|
#include <stdio.h>
|
|
|
|
#include "main.h"
|
|
#include "project.hpp"
|
|
//
|
|
// #include "one_dimensional_code_laser_scanner.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"
|
|
|
|
#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 m_input6;
|
|
|
|
ZGPIO output1;
|
|
ZGPIO output2;
|
|
|
|
ZCanReceiver m_canReceiver;
|
|
ZCanBasicOrderModule m_basicOrderModule;
|
|
ZCanPumpCtrlModule m_pumpCtrlModule;
|
|
ZCanTrigleWarningLightCtlModule m_warningLightCtlModule;
|
|
HuachengPressureSensor m_huachengPressureSensor;
|
|
|
|
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 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::run() {
|
|
ZHALCORE::cfg_t oscfg = {
|
|
.delayhtim = &DELAY_US_TIMER,
|
|
.debuguart = &DEBUG_UART,
|
|
};
|
|
ZHALCORE::getInstance()->initialize(oscfg);
|
|
|
|
ZLOGI(TAG, "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, false /*mirror*/);
|
|
m_input2.initAsInput(PC5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
|
|
m_input3.initAsInput(PD12, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
|
|
m_input4.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
|
|
m_input5.initAsInput(PC6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
|
|
m_input6.initAsInput(PC7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*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;
|
|
}
|
|
if (id == 6) {
|
|
val = m_input6.getState();
|
|
return true;
|
|
}
|
|
return false;
|
|
});
|
|
|
|
// output1.reg
|
|
// output2
|
|
m_basicOrderModule.regOutCtl([this](uint8_t id, bool val) { return false; });
|
|
ZHAL_CORE_REG(3000, {
|
|
// ZLOGI(TAG, "IO1:%d IO2:%d", m_input1.getState(), m_input2.getState());
|
|
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());
|
|
});
|
|
|
|
/*******************************************************************************
|
|
* 蠕动泵驱动 *
|
|
*******************************************************************************/
|
|
|
|
{
|
|
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, false, false);
|
|
triLight_G.initAsOutput(PD7, ZGPIO::kMode_nopull, false, false);
|
|
triLight_B.initAsOutput(PD9, ZGPIO::kMode_nopull, false, false);
|
|
triLight_BEEP.initAsOutput(PD10, ZGPIO::kMode_nopull, false, 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);
|
|
});
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* 压力传感器 *
|
|
*******************************************************************************/
|
|
|
|
// while (true) {
|
|
// static ModbusBlockHost modbusBlockHost;
|
|
// modbusBlockHost.initialize(&huart3);
|
|
// int16_t val[1] = {0};
|
|
// bool suc = modbusBlockHost.readReg03Muti(1, 0x00, (uint16_t *)val, 1, 50);
|
|
// printf("suc:%d val:%d\n", suc, val[0]);
|
|
// chip_delay_ms(1000);
|
|
// }
|
|
|
|
{
|
|
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);
|
|
}
|
|
|
|
ZLOGI(TAG, "init done");
|
|
while (1) {
|
|
ZHALCORE::getInstance()->loop();
|
|
}
|
|
}
|