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.
241 lines
10 KiB
241 lines
10 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/single_axis_motor_control/single_axis_motor_control.hpp"
|
|
#include "sdk/hal/zhal.hpp"
|
|
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.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;
|
|
|
|
DeviceBaseControlService m_deviceBaseControlService;
|
|
SingleAxisMotorControler m_pyMotorControlService; // 平移电机
|
|
SingleAxisMotorControler m_tjMotorControlService; // 推进电机
|
|
OneDimensionalCodeLaserScanner m_oneDimensionalCodeLaserScanner;
|
|
|
|
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*/);
|
|
}
|
|
uint32_t intput_sensors_get_table0() {
|
|
uint32_t val = 0;
|
|
|
|
val |= io_fybjl_sensor0.getStateUint32() << 0;
|
|
val |= io_fybjl_sensor1.getStateUint32() << 1;
|
|
val |= io_fybjl_sensor2.getStateUint32() << 2;
|
|
val |= io_fybjl_sensor3.getStateUint32() << 3;
|
|
val |= io_fybjl_sensor4.getStateUint32() << 4;
|
|
val |= io_fybjl_sensor5.getStateUint32() << 5;
|
|
val |= io_fybjl_sensor6.getStateUint32() << 6;
|
|
val |= io_fybjl_sensor7.getStateUint32() << 7;
|
|
val |= io_fybjl_sensor8.getStateUint32() << 8;
|
|
|
|
val |= io_fybh_read1.getStateUint32() << 9;
|
|
val |= io_fybh_read2.getStateUint32() << 10;
|
|
val |= io_fybh_read3.getStateUint32() << 11;
|
|
val |= io_fybh_read4.getStateUint32() << 12;
|
|
val |= io_fybh_read5.getStateUint32() << 13;
|
|
val |= io_fybh_read6.getStateUint32() << 14;
|
|
return val;
|
|
}
|
|
|
|
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(); });
|
|
|
|
/*******************************************************************************
|
|
* GPIO输入初始化 *
|
|
*******************************************************************************/
|
|
input_sensors_init();
|
|
intput_sensors_get_table0();
|
|
|
|
/*******************************************************************************
|
|
* 电机初始化 *
|
|
*******************************************************************************/
|
|
{
|
|
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);
|
|
}
|
|
|
|
{
|
|
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);
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* 扫码枪初始化 *
|
|
*******************************************************************************/
|
|
{
|
|
// static ZUART uart;
|
|
// PB6.initAsOutput(STM32_GPIO::kOutput_nopull, true, false);
|
|
// ZUART::cfg_t cfg = {
|
|
// .name = "CODE_SCANER_UART",
|
|
// .huart = &CODE_SCANER_UART,
|
|
// .rxbuffersize = 300,
|
|
// .rxovertime_ms = 10,
|
|
// };
|
|
// uart.initialize(&cfg,[](){})
|
|
// uart.initialize_basic("CODE_SCANER_UART", &m_hardware, &CODE_SCANER_UART);
|
|
// uart.initialize_setRxBuffer(300);
|
|
// uart.initialize_setRxOvertime(33);
|
|
// uart.initialize_finished();
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* 协议栈初始化 *
|
|
*******************************************************************************/
|
|
{ //
|
|
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);
|
|
});
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* 横移电机 *
|
|
*******************************************************************************/
|
|
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);
|
|
|
|
/*******************************************************************************
|
|
* 推杆电机 *
|
|
*******************************************************************************/
|
|
|
|
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);
|
|
|
|
// bwm_gpio
|
|
{
|
|
OneDimensionalCodeLaserScanner::cfg_t cfg;
|
|
cfg.triggerPin = FYBJL_SENSOR0;
|
|
cfg.codestartpos = 0;
|
|
cfg.codeendpos = 0;
|
|
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();
|
|
}
|
|
}
|