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.
206 lines
7.5 KiB
206 lines
7.5 KiB
#include <stddef.h>
|
|
#include <stdio.h>
|
|
|
|
#include "sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp"
|
|
#include "sdk/components/zcancmder_module/zcan_step_motor_ctrl_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\ztmc4361A.hpp"
|
|
#include "sdk\components\xy_robot_ctrl_module\xy_robot_ctrl_module.hpp"
|
|
#include "sdk\components\zcancmder\zcanreceiver.hpp"
|
|
#include "sdk\components\zcancmder_module\zcan_basic_order_module.hpp"
|
|
#include "sdk\components\zcancmder_module\zcan_xy_robot_module.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(); }
|
|
}
|
|
#define TMC_MOTOR_SPI hspi1
|
|
|
|
static TMC4361A motora;
|
|
static TMC4361A motorb;
|
|
static ZCanCmder zcanCmder;
|
|
static ZCanBasicOrderModule zcanBasicOrderModule;
|
|
static ZCANXYRobotCtrlModule zcanXYRobotCtrlModule;
|
|
static XYRobotCtrlModule xyRobotCtrlModule;
|
|
static StepMotorCtrlModule stepMotorCtrlModule;
|
|
static ZCanStepMotorCtrlModule zcanStepMotorCtrlModule;
|
|
|
|
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);
|
|
|
|
ZNVS::ins().initialize();
|
|
// ZNVS::ins().set_config_int32("test", 1234);
|
|
ZNVS::ins().dumpcfg();
|
|
|
|
osDelay(1000);
|
|
|
|
{
|
|
TMC4361A::cfg_t cfg = {
|
|
.spi = &TMC_MOTOR_SPI, //
|
|
.csgpio = TMC_MOTOR1_SPI_SELECT1_IO, //
|
|
.resetPin = TMC_MOTOR1_nRESET_IO, //
|
|
.fREEZEPin = TMC_MOTOR1_nFREEZE_IO, //
|
|
.ennPin = PinNull, //
|
|
.driverIC_ennPin = TMC_MOTOR1_SUB_IC_ENN_IO, //
|
|
.driverIC_resetPin = PinNull, //
|
|
};
|
|
motora.initialize(&cfg);
|
|
motora.setMotorShaft(false);
|
|
ZLOGI(TAG, "motora initialize TMC4361A:%x DriverIC:%x", motora.readICVersion(), motora.readSubICVersion());
|
|
}
|
|
|
|
{
|
|
TMC4361A::cfg_t cfg = {
|
|
.spi = &TMC_MOTOR_SPI, //
|
|
.csgpio = TMC_MOTOR2_SPI_SELECT1_IO, //
|
|
.resetPin = TMC_MOTOR2_nRESET_IO, //
|
|
.fREEZEPin = TMC_MOTOR2_nFREEZE_IO, //
|
|
.ennPin = PinNull, //
|
|
.driverIC_ennPin = TMC_MOTOR2_SUB_IC_ENN_IO, //
|
|
.driverIC_resetPin = PinNull, //
|
|
};
|
|
motorb.initialize(&cfg);
|
|
motorb.setMotorShaft(false);
|
|
ZLOGI(TAG, "motorb initialize TMC4361A:%x DriverIC:%x", motorb.readICVersion(), motorb.readSubICVersion());
|
|
}
|
|
|
|
motora.setAcceleration(300000);
|
|
motora.setDeceleration(300000);
|
|
motora.setIHOLD_IRUN(0, 2, 10);
|
|
|
|
motorb.setAcceleration(300000);
|
|
motorb.setDeceleration(300000);
|
|
motorb.setIHOLD_IRUN(0, 2, 10);
|
|
|
|
motora.rotate(0);
|
|
motorb.rotate(0);
|
|
auto zcanCmder_cfg = zcanCmder.createCFG(DEVICE_ID);
|
|
zcanCmder.init(zcanCmder_cfg);
|
|
|
|
/*******************************************************************************
|
|
* zcanBasicOrderModule *
|
|
*******************************************************************************/
|
|
zcanBasicOrderModule.initialize(&zcanCmder);
|
|
zcanBasicOrderModule.reg_set_io(1, [](bool val) { ZLOGI(TAG, "write io 1:%d", val); });
|
|
zcanBasicOrderModule.reg_read_io(1, []() {
|
|
ZLOGI(TAG, "read io 1");
|
|
return 1;
|
|
});
|
|
zcanBasicOrderModule.reg_read_adc(1, []() {
|
|
ZLOGI(TAG, "read adc 1");
|
|
return 123;
|
|
});
|
|
|
|
/*******************************************************************************
|
|
* zcanXYRobotCtrlModule *
|
|
*******************************************************************************/
|
|
ZGPIO input[8];
|
|
input[0].initAsInput(ARM_SENSOR1_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
input[1].initAsInput(ARM_SENSOR2_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
input[2].initAsInput(ARM_SENSOR3_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
input[3].initAsInput(ARM_SENSOR4_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
input[4].initAsInput(ARM_SENSOR5_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
input[5].initAsInput(ARM_SENSOR6_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
|
|
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);
|
|
|
|
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
|
|
}
|
|
}
|