|
|
#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"
#define TAG "main"
namespace iflytop {Main gmain;};
using namespace iflytop;
IflytopCanProtocolStackProcesser m_protocolStack;
TMC5130 m_tj_motor;TMC5130 m_py_motor;
ZGPIO debuglight;
ZGPIO io_fybh_read1;ZGPIO io_fybh_read2;ZGPIO io_fybh_read3;ZGPIO io_fybh_read4;ZGPIO io_fybh_read5;ZGPIO io_fybh_read6;
ZGPIO io_fybjl_sensor8;ZGPIO io_fybjl_sensor7;ZGPIO io_fybjl_sensor6;ZGPIO io_fybjl_sensor5;ZGPIO io_fybjl_sensor4;ZGPIO io_fybjl_sensor3;ZGPIO io_fybjl_sensor2;ZGPIO io_fybjl_sensor1;ZGPIO io_fybjl_sensor0;
M3078CodeScanner m_m3078CodeScanner;
DeviceBaseControlService m_deviceBaseControlService;IOControlService m_ioControlService;SingleAxisMotorControler m_pyMotorControlService; // 平移电机
SingleAxisMotorControler m_tjMotorControlService; // 推进电机
OneDimensionalCodeLaserScanner m_oneDimensionalCodeLaserScanner;IDCardReaderService m_idCardReaderService;
void input_sensors_init() { io_fybh_read1.initAsInput(FYBH_READ1, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); io_fybh_read2.initAsInput(FYBH_READ2, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); io_fybh_read3.initAsInput(FYBH_READ3, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); io_fybh_read4.initAsInput(FYBH_READ4, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); io_fybh_read5.initAsInput(FYBH_READ5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); io_fybh_read6.initAsInput(FYBH_READ6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
io_fybjl_sensor8.initAsInput(FYBJL_SENSOR8, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); io_fybjl_sensor7.initAsInput(FYBJL_SENSOR7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); io_fybjl_sensor6.initAsInput(FYBJL_SENSOR6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); io_fybjl_sensor5.initAsInput(FYBJL_SENSOR5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/); io_fybjl_sensor4.initAsInput(FYBJL_SENSOR4, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/); io_fybjl_sensor3.initAsInput(FYBJL_SENSOR3, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/); io_fybjl_sensor2.initAsInput(FYBJL_SENSOR2, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/); io_fybjl_sensor1.initAsInput(FYBJL_SENSOR1, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/); io_fybjl_sensor0.initAsInput(FYBJL_SENSOR0, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/);}
icps::error_t Main::onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser *processer, icps::WriteEvent *event) { return icps::kSuccess; }icps::error_t Main::onHostRegisterReadEvent(IflytopCanProtocolStackProcesser *processer, icps::ReadEvent *event) { return icps::kSuccess; }void Main::onHostRegisterReportEvent(IflytopCanProtocolStackProcesser *processer, icps::ReportEvent *event) {}
void Main::run() { ZHALCORE::cfg_t oscfg = { .delayhtim = &DELAY_US_TIMER, .debuguart = &DEBUG_UART, }; ZHALCORE::getInstance()->initialize(oscfg);
ZLOGI(TAG, "zapp:%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(); });
/*******************************************************************************
* 协议栈初始化 * *******************************************************************************/ { //
auto *cfg = IflytopCanProtocolStackProcesser::createDefaultConfig(DEVICE_ID, 128); m_protocolStack.initialize(cfg); m_protocolStack.setDumpPacketFlag(false); // m_protocolStack.registerListener(this);
// m_protocolStack.activeReg(REG_GPIO_INPUT0, icps::kwr, 0);
}
{ //
m_deviceBaseControlService.initialize(&m_protocolStack, DEVICE_ID); m_deviceBaseControlService.setListener([](int32_t engineer_mode) { // m_pyMotorControlService.setEngineerMode(engineer_mode);
// m_tjMotorControlService.setEngineerMode(engineer_mode);
}); }
/*******************************************************************************
* GPIO输入初始化 * *******************************************************************************/ input_sensors_init(); { m_ioControlService.initialize(&m_protocolStack, GPIO_INPUT_ADD_BASE, nullptr, //
//
[this](int io_off) { if (io_off == 0) return io_fybjl_sensor0.getState(); if (io_off == 1) return io_fybjl_sensor1.getState(); if (io_off == 2) return io_fybjl_sensor2.getState(); if (io_off == 3) return io_fybjl_sensor3.getState(); if (io_off == 4) return io_fybjl_sensor4.getState(); if (io_off == 5) return io_fybjl_sensor5.getState(); if (io_off == 6) return io_fybjl_sensor6.getState();
if (io_off == 7) return io_fybjl_sensor7.getState(); if (io_off == 8) return io_fybjl_sensor8.getState(); if (io_off == 9) return io_fybh_read1.getState(); if (io_off == 10) return io_fybh_read2.getState(); if (io_off == 11) return io_fybh_read3.getState(); if (io_off == 12) return io_fybh_read4.getState(); if (io_off == 13) return io_fybh_read5.getState(); if (io_off == 14) return io_fybh_read6.getState();
return false; }); }
/*******************************************************************************
* 横移电机 * *******************************************************************************/ { TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR0_ENN, .csn_pin = MOTOR0_CSN};
m_py_motor.initialize(&cfg); int32_t chipv = m_py_motor.readChipVERSION(); ZLOGI(TAG, "m_py_motor:%lx", chipv); m_py_motor.setIHOLD_IRUN(1, 31, 0); // m_py_motor.rotate(500000);
m_py_motor.setIHOLD_IRUN(2, 12, 0); m_py_motor.setMotorShaft(false);
m_pyMotorControlService.initialize( //
"infeedMotor", &m_protocolStack, REG_PY_MOTOR_CTRL_ADD_BASE, &io_fybjl_sensor1, // ZERO_GPIO
NULL, // LGPIO
NULL, // RGPIO
&m_py_motor // motor
);
m_pyMotorControlService.cfg_acc->setVal(30000); m_pyMotorControlService.cfg_dec->setVal(30000); m_pyMotorControlService.cfg_velocity->setVal(1000000); m_pyMotorControlService.cfg_zero_shift->setVal(0); m_pyMotorControlService.cfg_runhome_velocity->setVal(50000); m_pyMotorControlService.cfg_runtohome_dec->setVal(100000); m_pyMotorControlService.cfg_min_pos->setVal(-1000); m_pyMotorControlService.cfg_max_pos->setVal(1144028); m_pyMotorControlService.cfg_runtohome_max_distance->setVal(INT32_MAX); m_pyMotorControlService.cfg_runtohome_leave_zero_point_distance->setVal(256 * 200 * 1); }
/*******************************************************************************
* 推杆电机 * *******************************************************************************/ { TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR1_ENN, .csn_pin = MOTOR1_CSN};
m_tj_motor.initialize(&cfg); int32_t chipv = m_tj_motor.readChipVERSION(); ZLOGI(TAG, "m_tj_motor:%lx", chipv); m_tj_motor.setIHOLD_IRUN(1, 31, 0); // m_tj_motor.rotate(500000);
m_tj_motor.setIHOLD_IRUN(2, 12, 0); m_tj_motor.setMotorShaft(true);
m_tjMotorControlService.initialize( //
"outfeedMotor", &m_protocolStack, REG_TJ_MOTOR_CTRL_ADD_BASE, &io_fybjl_sensor4, // ZERO_GPIO
NULL, // LGPIO
NULL, // RGPIO
&m_tj_motor // motor
); m_tjMotorControlService.cfg_acc->setVal(30000); m_tjMotorControlService.cfg_dec->setVal(30000); m_tjMotorControlService.cfg_velocity->setVal(1000000); m_tjMotorControlService.cfg_zero_shift->setVal(0); m_tjMotorControlService.cfg_runhome_velocity->setVal(50000); m_tjMotorControlService.cfg_runtohome_dec->setVal(100000); m_tjMotorControlService.cfg_min_pos->setVal(-1000); m_tjMotorControlService.cfg_max_pos->setVal(1130108); m_tjMotorControlService.cfg_runtohome_max_distance->setVal(INT32_MAX); m_tjMotorControlService.cfg_runtohome_leave_zero_point_distance->setVal(256 * 200 * 1); }
/*******************************************************************************
* 扫码枪初始化 * *******************************************************************************/ { m_m3078CodeScanner.initialize(&REACTION_BOX_SWEEP_SENSOR_UART, PC11, false /*trigger pin mirror*/); m_idCardReaderService.initialize("idCardReaderService", &m_protocolStack, REG_ID_CARD_READER_CTRL_ADD_BASE, &m_m3078CodeScanner); }
{ // 扫描开始位置:518400
// 扫描结束位置:659200
OneDimensionalCodeLaserScanner::cfg_t cfg; cfg.triggerPin = FYBJL_SENSOR0; cfg.codestartpos = 518500; cfg.codeendpos = 659200; cfg.readder = [this]() { return m_tj_motor.getXACTUAL(); }; m_oneDimensionalCodeLaserScanner.initialize(&m_protocolStack, REG_LARSER_SCANNER_CTRL_ADD_BASE, &cfg); }
ZLOGI(TAG, "init done"); while (1) { ZHALCORE::getInstance()->loop(); }}
|
