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.
251 lines
7.7 KiB
251 lines
7.7 KiB
#include "main.hpp"
|
|
|
|
#include <stddef.h>
|
|
#include <stdio.h>
|
|
|
|
//
|
|
#include "feite_servo_motor.hpp"
|
|
#include "sdk/os/zos.hpp"
|
|
#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
|
|
#include "sdk\components\eq_20_asb_motor\eq20_servomotor.hpp"
|
|
#include "sdk\components\iflytop_can_slave_module_master_end\stepmotor.hpp"
|
|
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_master.hpp"
|
|
#include "sdk\components\step_motor_45\step_motor_45.hpp"
|
|
#include "sdk\components\step_motor_45\step_motor_45_scheduler.hpp"
|
|
|
|
#define TAG "main"
|
|
namespace iflytop {
|
|
Main gmain;
|
|
};
|
|
|
|
using namespace iflytop;
|
|
using namespace std;
|
|
#define CHECK_ARGC(n) \
|
|
if (argc != (n + 1)) { \
|
|
ZLOGE(TAG, "argc != %d", n); \
|
|
context->breakflag = true; \
|
|
return; \
|
|
}
|
|
extern "C" {
|
|
void StartDefaultTask(void const* argument) { iflytop::gmain.run(); }
|
|
}
|
|
/*******************************************************************************
|
|
* ÅäÖÃ *
|
|
*******************************************************************************/
|
|
static chip_cfg_t chipcfg = {
|
|
.us_dleay_tim = &DELAY_US_TIMER,
|
|
.tim_irq_scheduler_tim = &TIM_IRQ_SCHEDULER_TIMER,
|
|
.huart = &DEBUG_UART,
|
|
.debuglight = DEBUG_LIGHT_GPIO,
|
|
};
|
|
|
|
static StepMotor45::cfg_t cfg1 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PB15, PD11, PD12, PD13},
|
|
.driverPinMirror = true,
|
|
};
|
|
|
|
static StepMotor45::cfg_t cfg2 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PG2, PG3, PG4, PG5},
|
|
.driverPinMirror = true,
|
|
};
|
|
|
|
static StepMotor45::cfg_t cfg3 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PG6, PG7, PG8, PC6},
|
|
.driverPinMirror = true,
|
|
};
|
|
|
|
static StepMotor45::cfg_t cfg4 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PE0, PE2, PE4, PE6},
|
|
.driverPinMirror = true,
|
|
};
|
|
|
|
static StepMotor45::cfg_t cfg5 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PC13, PE5, PE3, PE1},
|
|
.driverPinMirror = true,
|
|
};
|
|
static StepMotor45::cfg_t cfg6 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PC12, PD3, PD5, PD7},
|
|
.driverPinMirror = true,
|
|
};
|
|
|
|
/*******************************************************************************
|
|
* ´úÂë *
|
|
*******************************************************************************/
|
|
namespace iflytop {
|
|
StepMotor45 g_step_motor45[7];
|
|
StepMotor g_step_motor[10];
|
|
IflytopCanMaster m_IflytopCanMaster;
|
|
CmdScheduler cmdScheduler;
|
|
|
|
ModbusBlockHost g_modbusblockhost;
|
|
Eq20ServoMotor g_eq20servomotor;
|
|
FeiTeServoMotor g_feiteservomotor;
|
|
} // namespace iflytop
|
|
|
|
void regfn() {
|
|
// setzero
|
|
// set4095
|
|
// moveto
|
|
|
|
cmdScheduler.registerCmd("mini_servo_set_zero", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
|
|
CHECK_ARGC(1);
|
|
int id = atoi(argv[1]);
|
|
ZLOGI(TAG, "mini_servo_set_zero %d", id);
|
|
g_feiteservomotor.reCalibration(id, 0);
|
|
ZLOGI(TAG, "mini_servo_set_zero %d done", id);
|
|
});
|
|
cmdScheduler.registerCmd("mini_servo_set_4095", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
|
|
CHECK_ARGC(1);
|
|
int id = atoi(argv[1]);
|
|
ZLOGI(TAG, "mini_servo_set_4095 %d", id);
|
|
g_feiteservomotor.reCalibration(id, 4095);
|
|
ZLOGI(TAG, "mini_servo_set_4095 %d done", id);
|
|
});
|
|
cmdScheduler.registerCmd("mini_servo_move_to", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
|
|
CHECK_ARGC(3);
|
|
int id = atoi(argv[1]);
|
|
int pos = atoi(argv[2]);
|
|
int torque = atoi(argv[3]);
|
|
|
|
ZLOGI(TAG, "mini_servo_move_to %d %d %d %d", id, pos, 2700, torque);
|
|
g_feiteservomotor.moveTo(id, pos, 2700, torque);
|
|
});
|
|
cmdScheduler.registerCmd("mini_servo_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
|
|
CHECK_ARGC(2);
|
|
int id = atoi(argv[1]);
|
|
int speed = atoi(argv[2]);
|
|
// int v = atoi(3000);
|
|
ZLOGI(TAG, "mini_servo_rotate %d %d %d", id, speed, 10, 2700);
|
|
g_feiteservomotor.rotate(id, speed, 10);
|
|
});
|
|
cmdScheduler.registerCmd("mini_servo_move_with_torque", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
|
|
CHECK_ARGC(2);
|
|
int id = atoi(argv[1]);
|
|
int torque = atoi(argv[2]);
|
|
// int v = atoi(3000);
|
|
if (torque == 0) {
|
|
g_feiteservomotor.moveWithTorque(id, 1);
|
|
} else {
|
|
g_feiteservomotor.moveWithTorque(id, torque);
|
|
}
|
|
});
|
|
|
|
cmdScheduler.registerCmd("sleep_ms", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
|
|
CHECK_ARGC(1);
|
|
osDelay(atoi(argv[1]));
|
|
});
|
|
}
|
|
|
|
extern "C" {
|
|
extern DMA_HandleTypeDef hdma_usart3_rx;
|
|
extern DMA_HandleTypeDef hdma_usart3_tx;
|
|
}
|
|
|
|
extern "C" {}
|
|
extern void step_motor_cmd_reg();
|
|
void Main::run() {
|
|
/*******************************************************************************
|
|
* ϵͳ³õʼ»¯ *
|
|
*******************************************************************************/
|
|
|
|
chip_init(&chipcfg);
|
|
|
|
zos_cfg_t zoscfg;
|
|
zos_init(&zoscfg);
|
|
|
|
auto config = m_IflytopCanMaster.createDefaultConfig(1);
|
|
m_IflytopCanMaster.initialize(config);
|
|
|
|
int i = 1;
|
|
g_step_motor[i++].initialize(11, 10000, &m_IflytopCanMaster);
|
|
g_step_motor[i++].initialize(12, 10000, &m_IflytopCanMaster);
|
|
g_step_motor[i++].initialize(13, 10000, &m_IflytopCanMaster);
|
|
g_step_motor[i++].initialize(14, 10000, &m_IflytopCanMaster);
|
|
g_step_motor[i++].initialize(15, 10000, &m_IflytopCanMaster);
|
|
g_step_motor[i++].initialize(16, 10000, &m_IflytopCanMaster);
|
|
|
|
g_step_motor45[0].initialize(cfg1);
|
|
g_step_motor45[1].initialize(cfg1);
|
|
g_step_motor45[2].initialize(cfg2);
|
|
g_step_motor45[3].initialize(cfg3);
|
|
g_step_motor45[4].initialize(cfg4);
|
|
g_step_motor45[5].initialize(cfg5);
|
|
g_step_motor45[6].initialize(cfg6);
|
|
|
|
StepMotor45Scheduler step_motor45_scheduler;
|
|
step_motor45_scheduler.initialize(&htim10, 1000);
|
|
step_motor45_scheduler.addMotor(&g_step_motor45[1]);
|
|
step_motor45_scheduler.addMotor(&g_step_motor45[2]);
|
|
step_motor45_scheduler.addMotor(&g_step_motor45[3]);
|
|
step_motor45_scheduler.addMotor(&g_step_motor45[4]);
|
|
step_motor45_scheduler.addMotor(&g_step_motor45[5]);
|
|
step_motor45_scheduler.addMotor(&g_step_motor45[6]);
|
|
// g_step_motor45_1.rotate(true, 1000);
|
|
|
|
step_motor45_scheduler.start();
|
|
|
|
cmdScheduler.initialize(&DEBUG_UART, 1000);
|
|
|
|
g_modbusblockhost.initialize(&huart2);
|
|
g_eq20servomotor.init(&g_modbusblockhost);
|
|
|
|
g_feiteservomotor.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx);
|
|
|
|
step_motor_cmd_reg();
|
|
regfn();
|
|
while (true) {
|
|
OSDefaultSchduler::getInstance()->loop();
|
|
cmdScheduler.schedule();
|
|
m_IflytopCanMaster.periodicJob();
|
|
// m_IflytopCanMaster.writeReg(1,1,1,10);
|
|
// osDelay(1);
|
|
}
|
|
}
|