|
|
@ -43,8 +43,8 @@ void nvs_init_cb() { |
|
|
|
} |
|
|
|
static void initsubmodule() { |
|
|
|
static XYRobotCtrlModule xyRobotCtrlModule; |
|
|
|
static TMC4361A motora; |
|
|
|
static TMC4361A motorb; |
|
|
|
static TMC4361A motora; // 1
|
|
|
|
static TMC4361A motorb; // 2
|
|
|
|
{ |
|
|
|
TMC4361A::cfg_t cfg = { |
|
|
|
.spi = &TMC_MOTOR_SPI, //
|
|
|
@ -57,6 +57,7 @@ static void initsubmodule() { |
|
|
|
}; |
|
|
|
motora.initialize(&cfg); |
|
|
|
motora.setMotorShaft(false); |
|
|
|
motora.setGLOBAL_SCALER(32); |
|
|
|
ZLOGI(TAG, "motora initialize TMC4361A:%x DriverIC:%x", motora.readICVersion(), motora.readSubICVersion()); |
|
|
|
} |
|
|
|
|
|
|
@ -72,6 +73,7 @@ static void initsubmodule() { |
|
|
|
}; |
|
|
|
motorb.initialize(&cfg); |
|
|
|
motorb.setMotorShaft(false); |
|
|
|
motorb.setGLOBAL_SCALER(32); |
|
|
|
ZLOGI(TAG, "motorb initialize TMC4361A:%x DriverIC:%x", motorb.readICVersion(), motorb.readSubICVersion()); |
|
|
|
} |
|
|
|
|
|
|
@ -89,17 +91,45 @@ static void initsubmodule() { |
|
|
|
/*******************************************************************************
|
|
|
|
* zcanXYRobotCtrlModule * |
|
|
|
*******************************************************************************/ |
|
|
|
ZGPIO input[8]; |
|
|
|
input[0].initAsInput(ARM_SENSOR1_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[1].initAsInput(ARM_SENSOR2_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[2].initAsInput(ARM_SENSOR3_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[3].initAsInput(ARM_SENSOR4_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[4].initAsInput(ARM_SENSOR5_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[5].initAsInput(ARM_SENSOR6_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[6].initAsInput(ARM_SENSOR7_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[7].initAsInput(ARM_SENSOR8_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
|
|
|
|
xyRobotCtrlModule.initialize(initer.get_module_id(1), &motora, &motorb, &input[0], 8, FLASH_MASK_XYRobotCtrlModule1); |
|
|
|
static ZGPIO input[4]; |
|
|
|
input[0].initAsInput(ARM_X_ZERO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[1].initAsInput(ARM_Y_ZERO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[2].initAsInput(ARM_X_LIMIT, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
input[3].initAsInput(ARM_Y_LIMIT, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); |
|
|
|
|
|
|
|
static XYRobotCtrlModule::flash_config_t xy_defaultcfg; |
|
|
|
XYRobotCtrlModule::create_default_cfg(xy_defaultcfg); |
|
|
|
xy_defaultcfg.configInited = true; |
|
|
|
xy_defaultcfg.basecfg.robot_type = 1; |
|
|
|
|
|
|
|
xy_defaultcfg.basecfg.robot_type = I_XYRobotCtrlModule::hbot; |
|
|
|
xy_defaultcfg.basecfg.x_shaft = 0; |
|
|
|
xy_defaultcfg.basecfg.y_shaft = 1; |
|
|
|
xy_defaultcfg.basecfg.ihold = 1; |
|
|
|
xy_defaultcfg.basecfg.irun = 16; |
|
|
|
xy_defaultcfg.basecfg.iholddelay = 100; |
|
|
|
xy_defaultcfg.basecfg.distance_scale = 7344; |
|
|
|
xy_defaultcfg.basecfg.shift_x = 0; |
|
|
|
xy_defaultcfg.basecfg.shift_y = 0; |
|
|
|
|
|
|
|
// limit
|
|
|
|
xy_defaultcfg.basecfg.acc = 800; |
|
|
|
xy_defaultcfg.basecfg.dec = 800; |
|
|
|
xy_defaultcfg.basecfg.breakdec = 1600; |
|
|
|
xy_defaultcfg.basecfg.maxspeed = 600; |
|
|
|
xy_defaultcfg.basecfg.min_x = 0; |
|
|
|
xy_defaultcfg.basecfg.max_x = 0; |
|
|
|
xy_defaultcfg.basecfg.min_y = 0; |
|
|
|
xy_defaultcfg.basecfg.max_y = 0; |
|
|
|
|
|
|
|
xy_defaultcfg.basecfg.run_to_zero_max_d = 10000 * 100; |
|
|
|
xy_defaultcfg.basecfg.run_to_zero_speed = 80; |
|
|
|
xy_defaultcfg.basecfg.run_to_zero_dec = 1600; |
|
|
|
xy_defaultcfg.basecfg.look_zero_edge_max_d = 10000 * 3; |
|
|
|
xy_defaultcfg.basecfg.look_zero_edge_speed = 10; |
|
|
|
xy_defaultcfg.basecfg.look_zero_edge_dec = 1600; |
|
|
|
|
|
|
|
xyRobotCtrlModule.initialize(initer.get_module_id(1), &motora, &motorb, &input[0], 4, xy_defaultcfg, nullptr); |
|
|
|
xyRobotCtrlModule.dumpcfg(); |
|
|
|
|
|
|
|
initer.register_module(&xyRobotCtrlModule); |
|
|
@ -111,7 +141,10 @@ void umain() { |
|
|
|
.deviceId = getDeviceId(), |
|
|
|
.input_gpio = |
|
|
|
{ |
|
|
|
// {PC6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true},
|
|
|
|
{ARM_SENSOR5_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true}, |
|
|
|
{ARM_SENSOR6_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true}, |
|
|
|
{ARM_SENSOR7_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true}, |
|
|
|
{ARM_SENSOR8_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true}, |
|
|
|
}, |
|
|
|
.output_gpio = |
|
|
|
{ |
|
|
@ -119,5 +152,6 @@ void umain() { |
|
|
|
}, |
|
|
|
}; |
|
|
|
initer.init(&cfg); |
|
|
|
initsubmodule(); |
|
|
|
initer.loop(); |
|
|
|
} |