|
|
|
@ -99,6 +99,7 @@ void umain() { |
|
|
|
* zcanBasicOrderModule * |
|
|
|
*******************************************************************************/ |
|
|
|
zcanBasicOrderModule.initialize(&zcanCmder); |
|
|
|
#if 0
|
|
|
|
zcanBasicOrderModule.reg_set_io(1, [](bool val) { ZLOGI(TAG, "write io 1:%d", val); }); |
|
|
|
zcanBasicOrderModule.reg_read_io(1, []() { |
|
|
|
ZLOGI(TAG, "read io 1"); |
|
|
|
@ -108,6 +109,7 @@ void umain() { |
|
|
|
ZLOGI(TAG, "read adc 1"); |
|
|
|
return 123; |
|
|
|
}); |
|
|
|
#endif
|
|
|
|
|
|
|
|
/*******************************************************************************
|
|
|
|
* zcanXYRobotCtrlModule * |
|
|
|
@ -122,85 +124,13 @@ void umain() { |
|
|
|
input[6].initAsInput(ARM_SENSOR7_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false); |
|
|
|
input[7].initAsInput(ARM_SENSOR8_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false); |
|
|
|
|
|
|
|
#if 0
|
|
|
|
{ |
|
|
|
xyRobotCtrlModule.initialize(&motora, &motorb, &input[6], &input[7], 1.0f); |
|
|
|
XYRobotCtrlModule::run_param_t param; |
|
|
|
xyRobotCtrlModule.read_run_param(param); |
|
|
|
param.robot_type = XYRobotCtrlModule::corexy; |
|
|
|
param.distance_scale = 1000; // = 1.0
|
|
|
|
param.x_shaft = false; |
|
|
|
param.y_shaft = false; |
|
|
|
param.ihold = 1; |
|
|
|
param.irun = 3; |
|
|
|
param.maxspeed = 1000000; |
|
|
|
param.acc = 3000000; |
|
|
|
param.dec = 3000000; |
|
|
|
xyRobotCtrlModule.set_run_param(param); |
|
|
|
|
|
|
|
XYRobotCtrlModule::run_to_zero_param_t run_to_zero_param; |
|
|
|
xyRobotCtrlModule.read_run_to_zero_param(run_to_zero_param); |
|
|
|
run_to_zero_param.move_to_zero_max_d = 5120000; |
|
|
|
run_to_zero_param.leave_from_zero_max_d = 5120000; |
|
|
|
run_to_zero_param.speed = 30000; |
|
|
|
run_to_zero_param.dec = 600000; |
|
|
|
xyRobotCtrlModule.set_run_to_zero_param(run_to_zero_param); |
|
|
|
|
|
|
|
zcanXYRobotCtrlModule.initialize(&zcanCmder, 1, &xyRobotCtrlModule); |
|
|
|
} |
|
|
|
{ |
|
|
|
stepMotorCtrlModule.initialize(1, &motora, &input[6], NULL); |
|
|
|
zcanStepMotorCtrlModule.initialize(&zcanCmder, 1, &stepMotorCtrlModule); |
|
|
|
} |
|
|
|
#endif
|
|
|
|
extern DMA_HandleTypeDef hdma_usart2_rx; |
|
|
|
extern DMA_HandleTypeDef hdma_usart2_tx; |
|
|
|
|
|
|
|
FeiTeServoMotor servo; |
|
|
|
servo.initialize(&huart2, &hdma_usart2_rx, &hdma_usart2_tx); |
|
|
|
OSDefaultSchduler::getInstance()->regPeriodJob( |
|
|
|
[&](OSDefaultSchduler::Context& context) { //
|
|
|
|
// FeiTeServoMotor::status_t status = {0};
|
|
|
|
// servo.read_status(1, &status);
|
|
|
|
// servo.dump_status(&status);
|
|
|
|
|
|
|
|
// FeiTeServoMotor::detailed_status_t detailed_status = {0};
|
|
|
|
// servo.read_detailed_status(1, &detailed_status);
|
|
|
|
// servo.dump_detailed_status(&detailed_status);
|
|
|
|
|
|
|
|
}, |
|
|
|
1000); |
|
|
|
//
|
|
|
|
servo.ping(1); |
|
|
|
// servo.reCalibration(1, 1000);
|
|
|
|
// servo.moveTo(1, 4000, 0, 0);
|
|
|
|
// servo.moveWithTorque(1, -1000);
|
|
|
|
int16_t poscalibration = 0; |
|
|
|
// servo.getServoCalibration(1, poscalibration);
|
|
|
|
// ZLOGI(TAG, "poscalibration:%d", poscalibration);
|
|
|
|
xyRobotCtrlModule.initialize(&motora, &motorb, &input[6], &input[7]); |
|
|
|
I_XYRobotCtrlModule::base_param_t base_param; |
|
|
|
xyRobotCtrlModule.create_default_cfg(base_param); |
|
|
|
xyRobotCtrlModule.set_base_param(base_param); |
|
|
|
|
|
|
|
while (true) { |
|
|
|
OSDefaultSchduler::getInstance()->loop(); |
|
|
|
zcanCmder.loop(); |
|
|
|
|
|
|
|
// zcanCmder.sendPacket(data, 4);
|
|
|
|
#if 0
|
|
|
|
static uint8_t rxbuf[1024]; |
|
|
|
static uint16_t rxlen; |
|
|
|
rxlen = 0; |
|
|
|
HAL_UARTEx_ReceiveToIdle(&huart2, rxbuf, 1000, &rxlen, 1000); |
|
|
|
if (rxlen > 0) { |
|
|
|
for (size_t i = 0; i < rxlen; i++) { |
|
|
|
printf("0x%02x ", rxbuf[i]); |
|
|
|
} |
|
|
|
printf("\n"); |
|
|
|
} |
|
|
|
#endif
|
|
|
|
|
|
|
|
#if 0
|
|
|
|
osDelay(100); |
|
|
|
ZLOGI(TAG, "input:%d %d %d %d %d %d %d %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()); |
|
|
|
#endif
|
|
|
|
} |
|
|
|
} |
