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.
177 lines
7.6 KiB
177 lines
7.6 KiB
#include "main.hpp"
|
|
|
|
#include <stddef.h>
|
|
#include <stdio.h>
|
|
|
|
#include "main.h"
|
|
#include "project.hpp"
|
|
//
|
|
#include "sdk/hal/zhal.hpp"
|
|
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
|
|
#include "sdk\components\tmc\ic\ztmc4361A.hpp"
|
|
|
|
#define TAG "main"
|
|
namespace iflytop {
|
|
Main gmain;
|
|
};
|
|
|
|
using namespace iflytop;
|
|
|
|
IflytopCanProtocolStackProcesser m_protocolStack;
|
|
TMC4361A m_motora;
|
|
TMC4361A m_motorb;
|
|
|
|
ZGPIO arm_sensor1_gpio;
|
|
ZGPIO arm_sensor2_gpio;
|
|
ZGPIO arm_sensor3_gpio;
|
|
ZGPIO arm_sensor4_gpio;
|
|
ZGPIO arm_sensor5_gpio;
|
|
ZGPIO arm_sensor6_gpio;
|
|
ZGPIO arm_sensor7_gpio;
|
|
ZGPIO arm_sensor8_gpio;
|
|
|
|
ZGPIO debuglight;
|
|
|
|
ZGPIO tmc_motor1_spi_select1_io;
|
|
ZGPIO tmc_motor1_nfreeze_io;
|
|
ZGPIO tmc_motor1_nreset_io;
|
|
ZGPIO tmc_motor1_sub_ic_enn_io;
|
|
ZGPIO tmc_motor2_spi_select1_io;
|
|
ZGPIO tmc_motor2_nfreeze_io;
|
|
ZGPIO tmc_motor2_nreset_io;
|
|
ZGPIO tmc_motor2_sub_ic_enn_io;
|
|
|
|
void input_sensors_init() {
|
|
arm_sensor1_gpio.initAsInput(ARM_SENSOR1_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true);
|
|
arm_sensor2_gpio.initAsInput(ARM_SENSOR2_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true);
|
|
arm_sensor3_gpio.initAsInput(ARM_SENSOR3_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
arm_sensor4_gpio.initAsInput(ARM_SENSOR4_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
arm_sensor5_gpio.initAsInput(ARM_SENSOR5_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
arm_sensor6_gpio.initAsInput(ARM_SENSOR6_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
arm_sensor7_gpio.initAsInput(ARM_SENSOR7_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true);
|
|
arm_sensor8_gpio.initAsInput(ARM_SENSOR8_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true);
|
|
}
|
|
uint32_t intput_sensors_get_table0() {
|
|
uint32_t val = 0;
|
|
val |= arm_sensor1_gpio.getStateUint32() << 1;
|
|
val |= arm_sensor2_gpio.getStateUint32() << 2;
|
|
val |= arm_sensor3_gpio.getStateUint32() << 3;
|
|
val |= arm_sensor4_gpio.getStateUint32() << 4;
|
|
val |= arm_sensor5_gpio.getStateUint32() << 5;
|
|
val |= arm_sensor6_gpio.getStateUint32() << 6;
|
|
val |= arm_sensor7_gpio.getStateUint32() << 7;
|
|
val |= arm_sensor8_gpio.getStateUint32() << 8;
|
|
return val;
|
|
}
|
|
|
|
ZGPIO *input_sensors_get_arm1_homegpio() { return &arm_sensor8_gpio; }
|
|
ZGPIO *input_sensors_get_arm2_homegpio() { return &arm_sensor7_gpio; }
|
|
|
|
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() {
|
|
iflytop_no_os_cfg_t oscfg = {
|
|
.delayhtim = &DELAY_US_TIMER,
|
|
.debuguart = &DEBUG_UART,
|
|
};
|
|
iflytop_no_os_init(&oscfg);
|
|
|
|
ZLOGI(TAG, "robotic_core_xy:%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();
|
|
|
|
/*******************************************************************************
|
|
* 电机初始化 *
|
|
*******************************************************************************/
|
|
tmc_motor1_spi_select1_io.initAsOutput(TMC_MOTOR1_SPI_SELECT1_IO, ZGPIO::kMode_nopull, false, true);
|
|
tmc_motor1_nfreeze_io.initAsOutput(TMC_MOTOR1_nFREEZE_IO, ZGPIO::kMode_nopull, false, true);
|
|
tmc_motor1_nreset_io.initAsOutput(TMC_MOTOR1_nRESET_IO, ZGPIO::kMode_nopull, false, true);
|
|
tmc_motor1_sub_ic_enn_io.initAsOutput(TMC_MOTOR1_SUB_IC_ENN_IO, ZGPIO::kMode_nopull, false, true);
|
|
tmc_motor2_spi_select1_io.initAsOutput(TMC_MOTOR2_SPI_SELECT1_IO, ZGPIO::kMode_nopull, false, true);
|
|
tmc_motor2_nfreeze_io.initAsOutput(TMC_MOTOR2_nFREEZE_IO, ZGPIO::kMode_nopull, false, true);
|
|
tmc_motor2_nreset_io.initAsOutput(TMC_MOTOR2_nRESET_IO, ZGPIO::kMode_nopull, false, true);
|
|
tmc_motor2_sub_ic_enn_io.initAsOutput(TMC_MOTOR2_SUB_IC_ENN_IO, ZGPIO::kMode_nopull, false, true);
|
|
|
|
{
|
|
TMC4361A::cfg_t motora_cfg = {.spi = &TMC_MOTOR_SPI,
|
|
.csgpio = &tmc_motor1_spi_select1_io,
|
|
.resetPin = &tmc_motor1_nreset_io,
|
|
.fREEZEPin = &tmc_motor1_nfreeze_io,
|
|
.ennPin = NULL,
|
|
.driverIC_ennPin = &tmc_motor1_sub_ic_enn_io,
|
|
.driverIC_resetPin = NULL};
|
|
|
|
m_motora.initialize(&motora_cfg);
|
|
int32_t ic4361Version = m_motora.readICVersion();
|
|
int32_t ic2160Version = m_motora.readSubICVersion();
|
|
ZLOGI(TAG, "m_motora:TMC4361Version:%lx TMC2160VERSION:%lx", ic4361Version, ic2160Version);
|
|
}
|
|
|
|
{
|
|
TMC4361A::cfg_t motorb_cfg = {.spi = &TMC_MOTOR_SPI,
|
|
.csgpio = &tmc_motor2_spi_select1_io,
|
|
.resetPin = &tmc_motor2_nreset_io,
|
|
.fREEZEPin = &tmc_motor2_nfreeze_io,
|
|
.ennPin = NULL,
|
|
.driverIC_ennPin = &tmc_motor2_sub_ic_enn_io,
|
|
.driverIC_resetPin = NULL};
|
|
|
|
m_motorb.initialize(&motorb_cfg);
|
|
int32_t ic4361Version = m_motorb.readICVersion();
|
|
int32_t ic2160Version = m_motorb.readSubICVersion();
|
|
ZLOGI(TAG, "m_motorb:TMC4361Version:%lx TMC2160VERSION:%lx", ic4361Version, ic2160Version);
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* 扫码枪初始化 *
|
|
*******************************************************************************/
|
|
{
|
|
// 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_motora.setIHOLD_IRUN(1, 12, 0); // 小臂
|
|
m_motora.setMotorShaft(0);
|
|
|
|
m_motorb.setIHOLD_IRUN(1, 12, 0); // 小臂
|
|
m_motorb.setMotorShaft(0);
|
|
|
|
m_motora.rotate(1200000);
|
|
m_motorb.rotate(1200000);
|
|
|
|
while (1) {
|
|
ZHALCORE::getInstance()->loop();
|
|
}
|
|
}
|