|
|
|
@ -57,6 +57,8 @@ SingleAxisMotorControler m_tjMotorControlService; // 推进电机 |
|
|
|
OneDimensionalCodeLaserScanner m_oneDimensionalCodeLaserScanner; |
|
|
|
IDCardReaderService m_idCardReaderService; |
|
|
|
|
|
|
|
ZCanReceiver m_canReceiver; |
|
|
|
|
|
|
|
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*/); |
|
|
|
@ -76,9 +78,13 @@ void input_sensors_init() { |
|
|
|
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::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 = { |
|
|
|
@ -93,142 +99,15 @@ void Main::run() { |
|
|
|
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; |
|
|
|
}); |
|
|
|
} |
|
|
|
ZCanReceiver::CFG *cfg = m_canReceiver.createCFG(DEVICE_ID); |
|
|
|
m_canReceiver.init(cfg); |
|
|
|
m_canReceiver.registerListener(this); |
|
|
|
|
|
|
|
/*******************************************************************************
|
|
|
|
* 横移电机 * |
|
|
|
*******************************************************************************/ |
|
|
|
{ |
|
|
|
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, true /*trigger pin mirror*/); |
|
|
|
// m_m3078CodeScanner.trigger();
|
|
|
|
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); |
|
|
|
} |
|
|
|
// uint8_t packet[255];
|
|
|
|
// for (int i = 0; i < 255; i++) {
|
|
|
|
// packet[i] = i;
|
|
|
|
// }
|
|
|
|
// ZHAL_CORE_REG(200, { m_canReceiver.sendPacket(packet, 255); });
|
|
|
|
|
|
|
|
ZLOGI(TAG, "init done"); |
|
|
|
while (1) { |
|
|
|
|
