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.
145 lines
6.0 KiB
145 lines
6.0 KiB
#include <stddef.h>
|
|
#include <stdio.h>
|
|
|
|
#include "sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp"
|
|
#include "sdk/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp"
|
|
#include "sdk/os/zos.hpp"
|
|
#include "sdk\components\flash\zsimple_flash.hpp"
|
|
#include "sdk\components\mini_servo_motor\feite_servo_motor.hpp"
|
|
#include "sdk\components\tmc\ic\ztmc5130.hpp"
|
|
#include "sdk\components\zcancmder\zcanreceiver.hpp"
|
|
#include "sdk\components\zcancmder_module\zcan_basic_order_module.hpp"
|
|
#include "sdk\components\zprotocols\zcancmder_v2\protocol_parser.hpp"
|
|
//
|
|
#include "sdk\components\flash\znvs.hpp"
|
|
//
|
|
|
|
#define TAG "main"
|
|
using namespace iflytop;
|
|
using namespace std;
|
|
|
|
extern void umain();
|
|
extern "C" {
|
|
void StartDefaultTask(void const* argument) { umain(); }
|
|
}
|
|
|
|
static TMC5130 g_motor;
|
|
static ZCanCmder g_zcanCmder;
|
|
static StepMotorCtrlModule g_stepMotorCtrlModule;
|
|
static ZIProtocolParser g_ziProtocolParser;
|
|
|
|
uint8_t getId() {
|
|
static bool init = false;
|
|
static ZGPIO ID0;
|
|
static ZGPIO ID1;
|
|
static ZGPIO ID2;
|
|
static ZGPIO ID3;
|
|
static ZGPIO ID4;
|
|
if (!init) {
|
|
ID0.initAsInput(ID0_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
ID1.initAsInput(ID1_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
ID2.initAsInput(ID2_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
ID3.initAsInput(ID3_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
ID4.initAsInput(ID4_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
init = true;
|
|
}
|
|
uint8_t id = ID0.getState() * 1 + ID1.getState() * 2 + ID2.getState() * 4 + ID3.getState() * 8 + ID4.getState() * 16;
|
|
return id;
|
|
}
|
|
|
|
void umain() {
|
|
chip_cfg_t chipcfg;
|
|
chipcfg.us_dleay_tim = &DELAY_US_TIMER;
|
|
chipcfg.tim_irq_scheduler_tim = &TIM_IRQ_SCHEDULER_TIMER;
|
|
chipcfg.huart = &DEBUG_UART;
|
|
chipcfg.debuglight = DEBUG_LIGHT_GPIO;
|
|
|
|
chip_init(&chipcfg);
|
|
|
|
zos_cfg_t zoscfg;
|
|
zos_init(&zoscfg);
|
|
|
|
uint8_t deviceId = getId();
|
|
ZLOGI(TAG, "motorId:%d", deviceId + STEP_MOTOR_ID_OFF);
|
|
|
|
|
|
/*******************************************************************************
|
|
* NVSINIT *
|
|
*******************************************************************************/
|
|
ZNVS::ins().initialize(IFLYTOP_NVS_CONFIG_FLASH_SECTOR);
|
|
{
|
|
static I_StepMotorCtrlModule::flash_config_t cfg;
|
|
StepMotorCtrlModule::create_default_cfg(cfg);
|
|
ZNVS::ins().alloc_config(MOTOR_CFG_FLASH_MARK, (uint8_t*)&cfg, sizeof(cfg));
|
|
}
|
|
ZNVS::ins().init_config();
|
|
// Pin_t enn_pin;
|
|
// Pin_t csn_pin;
|
|
osDelay(1000);
|
|
{
|
|
TMC5130::cfg_t cfg = {
|
|
.spi = &TMC_MOTOR_SPI, //
|
|
.csgpio = MOTOR0_CSN, //
|
|
.ennPin = MOTOR0_ENN, //
|
|
.spi_mode_select = MOTOR1_SPI_MODE_SELECT, //
|
|
};
|
|
g_motor.initialize(&cfg);
|
|
g_motor.setMotorShaft(false);
|
|
ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
|
|
}
|
|
|
|
g_motor.setAcceleration(100);
|
|
g_motor.setDeceleration(100);
|
|
g_motor.setIHOLD_IRUN(0, 8, 10);
|
|
|
|
g_motor.rotate(0);
|
|
// g_motor.enable(false);
|
|
auto zcanCmder_cfg = g_zcanCmder.createCFG( deviceId);
|
|
g_zcanCmder.init(zcanCmder_cfg);
|
|
|
|
/*******************************************************************************
|
|
* zcanBasicOrderModule *
|
|
*******************************************************************************/
|
|
|
|
/*******************************************************************************
|
|
* zcanXYRobotCtrlModule *
|
|
*******************************************************************************/
|
|
static ZGPIO input[10];
|
|
input[0].initAsInput(SENSOR_INT0, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[1].initAsInput(SENSOR_INT1, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[2].initAsInput(SENSOR_INT2, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[3].initAsInput(SENSOR_INT3, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[4].initAsInput(SENSOR_INT4, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[5].initAsInput(SENSOR_INT5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[6].initAsInput(SENSOR_INT6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[7].initAsInput(SENSOR_INT7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[8].initAsInput(SENSOR_INT8, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[9].initAsInput(SENSOR_INT9, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
|
|
OSDefaultSchduler::getInstance()->regPeriodJob(
|
|
[](OSDefaultSchduler::Context& context) {
|
|
#if 0
|
|
ZLOGI(TAG, "[0]:%d [1]:%d [2]:%d [3]:%d [4]:%d [5]:%d [6]:%d [7]:%d [8]:%d [9]:%d", //
|
|
input[0].getState(), //
|
|
input[1].getState(), //
|
|
input[2].getState(), //
|
|
input[3].getState(), //
|
|
input[4].getState(), //
|
|
input[5].getState(), //
|
|
input[6].getState(), //
|
|
input[7].getState(), //
|
|
input[8].getState(), //
|
|
input[9].getState());
|
|
#endif
|
|
},
|
|
1000);
|
|
|
|
g_stepMotorCtrlModule.initialize(deviceId, &g_motor, input, ZARRAY_SIZE(input), "MOTOR_CFG_FLASH_MARK");
|
|
g_ziProtocolParser.initialize(&g_zcanCmder);
|
|
g_ziProtocolParser.registerModule(&g_stepMotorCtrlModule);
|
|
|
|
while (true) {
|
|
OSDefaultSchduler::getInstance()->loop();
|
|
g_zcanCmder.loop();
|
|
}
|
|
}
|