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.
162 lines
6.4 KiB
162 lines
6.4 KiB
#include <stddef.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include "board.h"
|
|
#include "sdk\components\subcanmodule\zcancmder_subboard_initer.hpp"
|
|
/*******************************************************************************
|
|
* PROJECT_INCLUDE *
|
|
*******************************************************************************/
|
|
#include "sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp"
|
|
#include "sdk\components\mini_servo_motor\feite_servo_motor.hpp"
|
|
#include "sdk\components\mini_servo_motor\mini_servo_motor_ctrl_module.hpp"
|
|
#include "sdk\components\step_motor_ctrl_module\tmc51x0_motor.hpp"
|
|
#include "sdk\components\tmc\ic\ztmc4361A.hpp"
|
|
#include "sdk\components\tmc\ic\ztmc5130.hpp"
|
|
|
|
#define TAG "main"
|
|
using namespace iflytop;
|
|
using namespace std;
|
|
|
|
static ZCancmderSubboardIniter initer;
|
|
|
|
extern void umain();
|
|
extern "C" {
|
|
void StartDefaultTask(void const* argument) { umain(); }
|
|
|
|
extern DMA_HandleTypeDef hdma_usart3_rx;
|
|
extern DMA_HandleTypeDef hdma_usart3_tx;
|
|
}
|
|
/*******************************************************************************
|
|
* GET_DEVICE_ID *
|
|
*******************************************************************************/
|
|
static int32_t getDeviceId() {
|
|
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;
|
|
}
|
|
/*******************************************************************************
|
|
* INIT_SUBMODULE *
|
|
*******************************************************************************/
|
|
void nvs_init_cb() {}
|
|
static void initsubmodule() {
|
|
osDelay(1000);
|
|
{
|
|
static TMC5130 g_motor;
|
|
static TMC51X0Motor g_stepMotorCtrlModule;
|
|
TMC5130::cfg_t cfg = {
|
|
.spi = &TMC_MOTOR_SPI, //
|
|
.csgpio = MOTOR0_CSN, //
|
|
.ennPin = MOTOR0_ENN, //
|
|
.spi_mode_select = MOTOR0_SPI_MODE_SELECT, //
|
|
};
|
|
g_motor.initialize(&cfg);
|
|
ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
|
|
g_motor.enable(false);
|
|
g_motor.setMotorShaft(false);
|
|
|
|
#if 0
|
|
// g_motor.setMotorShaft(true);
|
|
|
|
// void writeInt(uint8_t address, int32_t value);
|
|
// int32_t readInt(uint8_t address);
|
|
|
|
int32_t enc_const_integral = 12;
|
|
float enc_const_fractional = 0.8;
|
|
|
|
int32_t setval = (enc_const_integral << 16) + enc_const_fractional * 10000;
|
|
setval = -setval;
|
|
ZLOGI(TAG, "setval:%d", setval);
|
|
g_motor.writeInt(TMC5130_ENC_CONST, setval);
|
|
g_motor.writeInt(TMC5130_ENCMODE, 0x1 << 10);
|
|
|
|
g_motor.setIHOLD_IRUN(2, 6, 1000);
|
|
g_motor.setENCVAL(0);
|
|
g_motor.enable(true);
|
|
osDelay(100);
|
|
g_motor.setENCVAL(0);
|
|
// g_motor.moveBy(51200, 100000);
|
|
g_motor.moveBy(51200, 100000);
|
|
|
|
while (true) {
|
|
int32_t enc = g_motor.readInt(TMC5130_XENC);
|
|
int32_t xactual = g_motor.readInt(TMC5130_XACTUAL);
|
|
ZLOGI(TAG, "enc:%d %d %d", enc, xactual, enc - xactual);
|
|
osDelay(1000);
|
|
}
|
|
#endif
|
|
|
|
static ZGPIO input[10];
|
|
input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
|
|
|
|
TMC51X0Motor::flash_config_t smcm_cfg = {0};
|
|
TMC51X0Motor::create_default_cfg(smcm_cfg);
|
|
smcm_cfg.base_param.motor_one_circle_pulse = 1000;
|
|
smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
|
|
smcm_cfg.base_param.stepmotor_ihold = 1;
|
|
smcm_cfg.base_param.stepmotor_irun = 1;
|
|
smcm_cfg.base_param.motor_shaft = 0;
|
|
smcm_cfg.base_param.motor_default_velocity = 1;
|
|
smcm_cfg.base_param.motor_run_to_zero_speed = 0;
|
|
smcm_cfg.base_param.max_d = 0;
|
|
smcm_cfg.base_param.min_d = 0;
|
|
smcm_cfg.base_param.motor_run_to_zero_max_d = 0;
|
|
smcm_cfg.base_param.motor_run_to_zero_speed = 1;
|
|
smcm_cfg.base_param.motor_run_to_zero_dec = 0;
|
|
smcm_cfg.base_param.motor_look_zero_edge_max_d = 0;
|
|
smcm_cfg.base_param.motor_look_zero_edge_speed = 1;
|
|
smcm_cfg.base_param.motor_look_zero_edge_dec = 0;
|
|
|
|
g_stepMotorCtrlModule.initialize(initer.get_module_id(1), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
|
|
g_stepMotorCtrlModule.enable(false);
|
|
initer.register_module(&g_stepMotorCtrlModule);
|
|
}
|
|
|
|
{
|
|
// 115200
|
|
/*******************************************************************************
|
|
* 试管帽夹舵机 *
|
|
*******************************************************************************/
|
|
static FeiTeServoMotor feiteservomotor_bus; // 飞特舵机总线
|
|
static MiniRobotCtrlModule mini_servo;
|
|
|
|
// 飞特舵机总线
|
|
ZASSERT(huart3.Init.BaudRate == 115200);
|
|
feiteservomotor_bus.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx);
|
|
|
|
static MiniRobotCtrlModule::flash_config_t cfg = {0};
|
|
cfg.default_torque = 330;
|
|
mini_servo.initialize(initer.get_module_id(2), &feiteservomotor_bus, 1, &cfg);
|
|
initer.register_module(&mini_servo);
|
|
}
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* MAIN *
|
|
*******************************************************************************/
|
|
void umain() {
|
|
ZCancmderSubboardIniter::cfg_t cfg = //
|
|
{
|
|
.deviceId = getDeviceId(),
|
|
.input_gpio = {},
|
|
.output_gpio = {},
|
|
};
|
|
initer.init(&cfg);
|
|
initsubmodule();
|
|
initer.loop();
|
|
}
|