1、代码备份
This commit is contained in:
@@ -0,0 +1,553 @@
|
||||
#include "dc_motor.h"
|
||||
#include "hc32_ddl.h"
|
||||
#include "Debug.h"
|
||||
|
||||
|
||||
|
||||
// 全局变量
|
||||
volatile MotorState g_motorState = MOTOR_STOPPED;
|
||||
volatile MotorCommand g_motorCmd = MOTOR_CMD_STOP;
|
||||
volatile MotorDirection g_motorDirection = DIR_STOPPED; // 新增运行方向标志位
|
||||
volatile uint16_t g_motorSpeed = 0;
|
||||
volatile uint16_t g_accelTimer = 0;
|
||||
|
||||
static uint16_t accelCounter = 0;
|
||||
|
||||
|
||||
static uint8_t DcMotorDispEn = true;
|
||||
|
||||
#define DCMOTOR_DBG_LOG(...) { if(DcMotorDispEn) DBG_LOG(__VA_ARGS__);}
|
||||
#define DCMOTOR_DBG_ARRAY(ARRAY, SIZE) { if(DcMotorDispEn) {DBG_ARRAY(ARRAY,SIZE)}}
|
||||
|
||||
|
||||
|
||||
void DCMOTORDBGOnOff(uint8_t OnOff)
|
||||
{
|
||||
DcMotorDispEn = OnOff;
|
||||
if(OnOff)
|
||||
DBG_LOG("\r\nKey Display Enable!\r\n");
|
||||
else
|
||||
DBG_LOG("\r\nKey Display Disable!\r\n");
|
||||
}
|
||||
|
||||
|
||||
void Timer61A_Init(void)
|
||||
{
|
||||
uint16_t u16Period;
|
||||
// uint16_t u16Compare;
|
||||
stc_timer6_basecnt_cfg_t stcTIM6BaseCntCfg;
|
||||
stc_timer6_port_output_cfg_t stcTIM6PWMxCfg;
|
||||
stc_timer6_gcmp_buf_cfg_t stcGCMPBufCfg;
|
||||
stc_port_init_t stcPortInit;
|
||||
stc_irq_regi_conf_t stcIrqRegiConf;
|
||||
stc_timer6_deadtime_cfg_t stcDeadTimeCfg;
|
||||
|
||||
MEM_ZERO_STRUCT(stcTIM6BaseCntCfg);
|
||||
MEM_ZERO_STRUCT(stcTIM6PWMxCfg);
|
||||
MEM_ZERO_STRUCT(stcGCMPBufCfg);
|
||||
MEM_ZERO_STRUCT(stcPortInit);
|
||||
MEM_ZERO_STRUCT(stcIrqRegiConf);
|
||||
MEM_ZERO_STRUCT(stcDeadTimeCfg);
|
||||
|
||||
/* Initialize Clock */
|
||||
// BSP_CLK_Init();
|
||||
|
||||
PWC_Fcg2PeriphClockCmd(PWC_FCG2_PERIPH_TIM61, Enable);
|
||||
|
||||
PORT_SetFunc(PortA, Pin08, Func_Tim6, Disable); //Timer61 PWMA
|
||||
PORT_SetFunc(PortB, Pin13, Func_Tim6, Disable); //Timer61 PWMB
|
||||
|
||||
stcTIM6BaseCntCfg.enCntMode = Timer6CntTriangularModeA; //Triangular wave mode
|
||||
stcTIM6BaseCntCfg.enCntDir = Timer6CntDirUp; //Counter counting up
|
||||
stcTIM6BaseCntCfg.enCntClkDiv = Timer6PclkDiv1; //Count clock: pclk
|
||||
Timer6_Init(M4_TMR61, &stcTIM6BaseCntCfg); //timer6 PWM frequency, count mode and clk config
|
||||
|
||||
u16Period = 10000u;
|
||||
Timer6_SetPeriod(M4_TMR61, Timer6PeriodA, u16Period); //period set
|
||||
|
||||
// u16Compare = 3000u;
|
||||
// Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareA, u16Compare); //Set General Compare RegisterA Value
|
||||
// Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareC, u16Compare); //Set General Compare RegisterC Value as buffer register of GCMAR
|
||||
|
||||
|
||||
/*PWMA/PWMB output buf config*/
|
||||
stcGCMPBufCfg.bEnGcmpTransBuf = true;
|
||||
stcGCMPBufCfg.enGcmpBufTransType = Timer6GcmpPrdSingleBuf; //Single buffer transfer
|
||||
Timer6_SetGeneralBuf(M4_TMR61, Timer6PWMA, &stcGCMPBufCfg); //GCMAR buffer transfer set
|
||||
Timer6_SetGeneralBuf(M4_TMR61, Timer6PWMB, &stcGCMPBufCfg); //GCMBR buffer transfer set
|
||||
|
||||
stcTIM6PWMxCfg.bOutEn = true; //Output enable
|
||||
stcTIM6PWMxCfg.enPerc = Timer6PWMxCompareKeep; //PWMA port output keep former level when CNTER value match PERAR
|
||||
stcTIM6PWMxCfg.enCmpc = Timer6PWMxCompareInv; //PWMA port output inverse level when CNTER value match with GCMAR
|
||||
stcTIM6PWMxCfg.enStaStp = Timer6PWMxStateSelSS; //PWMA output status is decide by STACA STPCA when CNTER start and stop
|
||||
stcTIM6PWMxCfg.enStaOut = Timer6PWMxPortOutLow; //PWMA port output set low level when CNTER start
|
||||
stcTIM6PWMxCfg.enStpOut = Timer6PWMxPortOutLow; //PWMA port output set low level when CNTER stop
|
||||
stcTIM6PWMxCfg.enDisVal = Timer6PWMxDisValLow;
|
||||
Timer6_PortOutputConfig(M4_TMR61, Timer6PWMA, &stcTIM6PWMxCfg);
|
||||
Timer6_SetFunc(M4_TMR61, Timer6PWMA, Timer6ModeCompareOutput); //Compare output function
|
||||
|
||||
stcTIM6PWMxCfg.bOutEn = true; //Output enable
|
||||
stcTIM6PWMxCfg.enPerc = Timer6PWMxCompareKeep; //PWMB port output keep former level when CNTER value match PERAR
|
||||
stcTIM6PWMxCfg.enCmpc = Timer6PWMxCompareHigh; //PWMB port output inverse level when CNTER value match with GCMBR
|
||||
stcTIM6PWMxCfg.enStaStp = Timer6PWMxStateSelSS; //PWMB output status is decide by STACB STPCB when CNTER start and stop
|
||||
stcTIM6PWMxCfg.enStaOut = Timer6PWMxPortOutLow; //PWMB port output set high level when CNTER start
|
||||
stcTIM6PWMxCfg.enStpOut = Timer6PWMxPortOutLow; //PWMB port output set low level when CNTER stop
|
||||
stcTIM6PWMxCfg.enDisVal = Timer6PWMxDisValLow;
|
||||
Timer6_PortOutputConfig(M4_TMR61, Timer6PWMB, &stcTIM6PWMxCfg);
|
||||
Timer6_SetFunc(M4_TMR61, Timer6PWMB, Timer6ModeCompareOutput); //Compare output function
|
||||
|
||||
Timer6_SetDeadTimeValue(M4_TMR61, Timer6DeadTimUpAR, 10u); // Set dead time value (up count)
|
||||
|
||||
stcDeadTimeCfg.bEnDeadtime = true; //Enable Hardware DeadTime
|
||||
stcDeadTimeCfg.bEnDtBufUp = false; //Disable buffer transfer
|
||||
stcDeadTimeCfg.bEnDtBufDwn = false; //Disable buffer transfer
|
||||
stcDeadTimeCfg.bEnDtEqualUpDwn = true; //Make the down count dead time value equal to the up count dead time setting
|
||||
Timer6_ConfigDeadTime(M4_TMR61, &stcDeadTimeCfg); // Hardware dead time function config
|
||||
}
|
||||
|
||||
|
||||
void Timer61B_Init(void)
|
||||
{
|
||||
uint16_t u16Period;
|
||||
// uint16_t u16Compare;
|
||||
stc_timer6_basecnt_cfg_t stcTIM6BaseCntCfg;
|
||||
stc_timer6_port_output_cfg_t stcTIM6PWMxCfg;
|
||||
stc_timer6_gcmp_buf_cfg_t stcGCMPBufCfg;
|
||||
stc_port_init_t stcPortInit;
|
||||
stc_irq_regi_conf_t stcIrqRegiConf;
|
||||
stc_timer6_deadtime_cfg_t stcDeadTimeCfg;
|
||||
|
||||
MEM_ZERO_STRUCT(stcTIM6BaseCntCfg);
|
||||
MEM_ZERO_STRUCT(stcTIM6PWMxCfg);
|
||||
MEM_ZERO_STRUCT(stcGCMPBufCfg);
|
||||
MEM_ZERO_STRUCT(stcPortInit);
|
||||
MEM_ZERO_STRUCT(stcIrqRegiConf);
|
||||
MEM_ZERO_STRUCT(stcDeadTimeCfg);
|
||||
|
||||
|
||||
PWC_Fcg2PeriphClockCmd(PWC_FCG2_PERIPH_TIM61, Enable);
|
||||
|
||||
PORT_SetFunc(PortA, Pin08, Func_Tim6, Disable); //Timer61 PWMA
|
||||
PORT_SetFunc(PortB, Pin13, Func_Tim6, Disable); //Timer61 PWMB
|
||||
|
||||
stcTIM6BaseCntCfg.enCntMode = Timer6CntTriangularModeA; //Triangular wave mode
|
||||
stcTIM6BaseCntCfg.enCntDir = Timer6CntDirUp; //Counter counting up
|
||||
stcTIM6BaseCntCfg.enCntClkDiv = Timer6PclkDiv1; //Count clock: pclk
|
||||
Timer6_Init(M4_TMR61, &stcTIM6BaseCntCfg); //timer6 PWM frequency, count mode and clk config
|
||||
|
||||
u16Period = 10000u;
|
||||
Timer6_SetPeriod(M4_TMR61, Timer6PeriodA, u16Period); //period set
|
||||
|
||||
// u16Compare = 3000u;
|
||||
// Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareA, u16Compare); //Set General Compare RegisterA Value
|
||||
// Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareC, u16Compare); //Set General Compare RegisterC Value as buffer register of GCMAR
|
||||
|
||||
|
||||
/*PWMA/PWMB output buf config*/
|
||||
stcGCMPBufCfg.bEnGcmpTransBuf = true;
|
||||
stcGCMPBufCfg.enGcmpBufTransType = Timer6GcmpPrdSingleBuf; //Single buffer transfer
|
||||
Timer6_SetGeneralBuf(M4_TMR61, Timer6PWMA, &stcGCMPBufCfg); //GCMAR buffer transfer set
|
||||
Timer6_SetGeneralBuf(M4_TMR61, Timer6PWMB, &stcGCMPBufCfg); //GCMBR buffer transfer set
|
||||
|
||||
stcTIM6PWMxCfg.bOutEn = true; //Output enable
|
||||
stcTIM6PWMxCfg.enPerc = Timer6PWMxCompareKeep; //PWMA port output keep former level when CNTER value match PERAR
|
||||
stcTIM6PWMxCfg.enCmpc = Timer6PWMxCompareHigh; //PWMA port output inverse level when CNTER value match with GCMAR
|
||||
stcTIM6PWMxCfg.enStaStp = Timer6PWMxStateSelSS; //PWMA output status is decide by STACA STPCA when CNTER start and stop
|
||||
stcTIM6PWMxCfg.enStaOut = Timer6PWMxPortOutLow; //PWMA port output set low level when CNTER start
|
||||
stcTIM6PWMxCfg.enStpOut = Timer6PWMxPortOutLow; //PWMA port output set low level when CNTER stop
|
||||
stcTIM6PWMxCfg.enDisVal = Timer6PWMxDisValLow;
|
||||
Timer6_PortOutputConfig(M4_TMR61, Timer6PWMA, &stcTIM6PWMxCfg);
|
||||
Timer6_SetFunc(M4_TMR61, Timer6PWMA, Timer6ModeCompareOutput); //Compare output function
|
||||
|
||||
stcTIM6PWMxCfg.bOutEn = true; //Output enable
|
||||
stcTIM6PWMxCfg.enPerc = Timer6PWMxCompareKeep; //PWMB port output keep former level when CNTER value match PERAR
|
||||
stcTIM6PWMxCfg.enCmpc = Timer6PWMxCompareInv; //PWMB port output inverse level when CNTER value match with GCMBR
|
||||
stcTIM6PWMxCfg.enStaStp = Timer6PWMxStateSelSS; //PWMB output status is decide by STACB STPCB when CNTER start and stop
|
||||
stcTIM6PWMxCfg.enStaOut = Timer6PWMxPortOutLow; //PWMB port output set high level when CNTER start
|
||||
stcTIM6PWMxCfg.enStpOut = Timer6PWMxPortOutLow; //PWMB port output set low level when CNTER stop
|
||||
stcTIM6PWMxCfg.enDisVal = Timer6PWMxDisValLow;
|
||||
Timer6_PortOutputConfig(M4_TMR61, Timer6PWMB, &stcTIM6PWMxCfg);
|
||||
Timer6_SetFunc(M4_TMR61, Timer6PWMB, Timer6ModeCompareOutput); //Compare output function
|
||||
|
||||
Timer6_SetDeadTimeValue(M4_TMR61, Timer6DeadTimUpAR, 10u); // Set dead time value (up count)
|
||||
|
||||
stcDeadTimeCfg.bEnDeadtime = true; //Enable Hardware DeadTime
|
||||
stcDeadTimeCfg.bEnDtBufUp = false; //Disable buffer transfer
|
||||
stcDeadTimeCfg.bEnDtBufDwn = false; //Disable buffer transfer
|
||||
stcDeadTimeCfg.bEnDtEqualUpDwn = true; //Make the down count dead time value equal to the up count dead time setting
|
||||
Timer6_ConfigDeadTime(M4_TMR61, &stcDeadTimeCfg); // Hardware dead time function config
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 电机初始化
|
||||
* @param 无
|
||||
* @retval 无
|
||||
*/
|
||||
void dcmotor_init(void)
|
||||
{
|
||||
stc_port_init_t stcPortInit;
|
||||
|
||||
|
||||
stcPortInit.enPinMode = Pin_Mode_Out;
|
||||
stcPortInit.enPinOType = Pin_OType_Cmos;
|
||||
stcPortInit.enPinDrv = Pin_Drv_H;
|
||||
stcPortInit.enPullUp = Disable;
|
||||
|
||||
PORT_Init(SHUTDOWN1_GPIO_Port, SHUTDOWN1_Pin, &stcPortInit);
|
||||
PORT_SetBits(SHUTDOWN1_GPIO_Port, SHUTDOWN1_Pin);
|
||||
|
||||
PORT_Init(PortA, Pin03, &stcPortInit);
|
||||
PORT_SetBits(PortA, Pin03);
|
||||
/* SD拉低，关闭输出 */
|
||||
|
||||
dcmotor_stop(); /* 停止电机 */
|
||||
dcmotor_dir(0); /* 设置正转 */
|
||||
dcmotor_speed(0); /* 速度设置为0 */
|
||||
dcmotor_start(); /* 开启电机 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机开启
|
||||
* @param 无
|
||||
* @retval 无
|
||||
*/
|
||||
void dcmotor_start(void)
|
||||
{
|
||||
ENABLE_MOTOR; /* 拉高SD引脚，开启电机 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机停止
|
||||
* @param 无
|
||||
* @retval 无
|
||||
*/
|
||||
void dcmotor_stop(void)
|
||||
{
|
||||
Timer6_StopCount(M4_TMR61);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机旋转方向设置
|
||||
* @param para:方向 0正转，1反转
|
||||
* @note 以电机正面，顺时针方向旋转为正转
|
||||
* @retval 无
|
||||
*/
|
||||
void dcmotor_dir(uint8_t para)
|
||||
{
|
||||
Timer6_StopCount(M4_TMR61);
|
||||
Timer6_DeInit(M4_TMR61);
|
||||
|
||||
if (para == 0) /* 正转 */
|
||||
{
|
||||
|
||||
|
||||
Timer61A_Init();
|
||||
|
||||
}
|
||||
else if (para == 1) /* 反转 */
|
||||
{
|
||||
Timer61B_Init();
|
||||
}
|
||||
dcmotor_start();
|
||||
Timer6_StartCount(M4_TMR61);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机速度设置
|
||||
* @param para:比较寄存器值
|
||||
* @retval 无
|
||||
*/
|
||||
void dcmotor_speed(uint16_t para)
|
||||
{
|
||||
g_motorSpeed = (para > MAX_SPEED) ? MAX_SPEED : para;
|
||||
|
||||
if (g_motorSpeed == 0) {
|
||||
g_motorDirection = DIR_STOPPED;
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareA, g_motorSpeed);
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareA, g_motorSpeed);
|
||||
dcmotor_stop();
|
||||
return;
|
||||
}
|
||||
|
||||
if(Motor_GetDirection() == DIR_REVERSE) //反转
|
||||
{
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareA, g_motorSpeed); //Set General Compare RegisterA Value
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareC, g_motorSpeed); //Set General Compare RegisterC Value as buffer register of GCMAR
|
||||
DBG_LOG("REVERSE\tnewSpeed = %d\r\n", g_motorSpeed);
|
||||
}
|
||||
else if(Motor_GetDirection() == DIR_FORWARD) //正转
|
||||
{
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareA, 10000-g_motorSpeed); //Set General Compare RegisterA Value
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareC, 10000-g_motorSpeed); //Set General Compare RegisterC Value as buffer register of GCMAR
|
||||
DBG_LOG("FORWARD\tnewSpeed = %d\r\n", g_motorSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机控制
|
||||
* @param para: pwm比较值 ,正数电机为正转，负数为反转
|
||||
* @note 根据传入的参数控制电机的转向和速度
|
||||
* @retval 无
|
||||
*/
|
||||
void motor_pwm_set(float para)
|
||||
{
|
||||
int val = (int)para;
|
||||
|
||||
if(val == 0)
|
||||
{
|
||||
dcmotor_stop();
|
||||
return;
|
||||
}
|
||||
|
||||
if (val > 0)
|
||||
{
|
||||
g_motorDirection = DIR_FORWARD;
|
||||
dcmotor_dir(0); /* 正转 */
|
||||
dcmotor_speed(10000-val);
|
||||
}
|
||||
else
|
||||
{
|
||||
g_motorDirection = DIR_REVERSE;
|
||||
dcmotor_dir(1); /* 反转 */
|
||||
dcmotor_speed(-val);
|
||||
}
|
||||
DCMOTOR_DBG_LOG("dcmotor_speed:%d\r\n", val);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 设置电机方向
|
||||
void Motor_SetDirection(uint8_t dir) {
|
||||
// 更新方向标志位
|
||||
if (dir == 0) {
|
||||
g_motorDirection = DIR_FORWARD;
|
||||
// Up();
|
||||
dcmotor_dir(0);
|
||||
} else {
|
||||
g_motorDirection = DIR_REVERSE;
|
||||
// Down();
|
||||
dcmotor_dir(1);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// 电机控制接口
|
||||
void Motor_Control(MotorCommand cmd) {
|
||||
g_motorCmd = cmd;
|
||||
}
|
||||
|
||||
// 获取电机当前方向
|
||||
MotorDirection Motor_GetDirection(void) {
|
||||
return g_motorDirection;
|
||||
}
|
||||
|
||||
|
||||
// 设置电机速度
|
||||
void Motor_SetSpeed(uint16_t speed) {
|
||||
g_motorSpeed = (speed > MAX_SPEED) ? MAX_SPEED : speed;
|
||||
|
||||
// 当速度为零时，更新方向状态为停止
|
||||
if (g_motorSpeed == 0) {
|
||||
g_motorDirection = DIR_STOPPED;
|
||||
}
|
||||
|
||||
if(Motor_GetDirection() == DIR_REVERSE) //反转
|
||||
{
|
||||
// Adt_SetCompareValue(M0P_ADTIM4, AdtCompareC, PWM_PERIOD - g_motorSpeed);
|
||||
// DBG_LOG("REVERSE\tnewSpeed = %d\r\n", g_motorSpeed);
|
||||
}
|
||||
else if(Motor_GetDirection() == DIR_FORWARD) //正转
|
||||
{
|
||||
// Adt_SetCompareValue(M0P_ADTIM5, AdtCompareC, g_motorSpeed);
|
||||
// DBG_LOG("FORWARD\tnewSpeed = %d\r\n", g_motorSpeed);
|
||||
}
|
||||
else
|
||||
{
|
||||
// StopMotor();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
bool Dcmotor1msStatus = false;
|
||||
void Dcmotor1msRoutine(void)
|
||||
{
|
||||
Dcmotor1msStatus = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
uint8_t KeyASta;
|
||||
uint8_t KeyBSta;
|
||||
uint8_t RunSta;
|
||||
void DcmotorLoopHandler(void)
|
||||
{
|
||||
|
||||
#if(1)
|
||||
|
||||
|
||||
if((RunSta == false) && (KeyASta == true) && (KeyBSta == false))
|
||||
{
|
||||
Motor_Control(MOTOR_CMD_REVERSE);
|
||||
}
|
||||
else if((RunSta == false) && (KeyASta == false) && (KeyBSta == true))
|
||||
{
|
||||
Motor_Control(MOTOR_CMD_FORWARD);
|
||||
}
|
||||
else if((RunSta == true) && (KeyASta == false) && (KeyBSta == false))
|
||||
{
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
RunSta = false;
|
||||
}
|
||||
|
||||
switch (g_motorCmd)
|
||||
{
|
||||
case MOTOR_CMD_REVERSE:
|
||||
if(RunSta == false)
|
||||
{
|
||||
RunSta = true;
|
||||
Timer61B_Init();
|
||||
g_motorSpeed = 8000;
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareA, g_motorSpeed);
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareC, g_motorSpeed);
|
||||
Timer6_StartCount(M4_TMR61);
|
||||
}
|
||||
|
||||
break;
|
||||
case MOTOR_CMD_FORWARD:
|
||||
if(RunSta == false)
|
||||
{
|
||||
RunSta = true;
|
||||
Timer61A_Init();
|
||||
g_motorSpeed = 8000;
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareA, g_motorSpeed);
|
||||
Timer6_SetGeneralCmpValue(M4_TMR61, Timer6GenCompareC, g_motorSpeed);
|
||||
Timer6_StartCount(M4_TMR61);
|
||||
}
|
||||
|
||||
break;
|
||||
default:
|
||||
Timer6_StopCount(M4_TMR61);
|
||||
RunSta = false;
|
||||
break;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
#if(0)
|
||||
|
||||
|
||||
if((RunSta == false) && (KeyASta == true) && (KeyBSta == false))
|
||||
{
|
||||
Motor_Control(MOTOR_CMD_REVERSE);
|
||||
RunSta = true;
|
||||
}
|
||||
else if((RunSta == false) && (KeyASta == false) && (KeyBSta == true))
|
||||
{
|
||||
Motor_Control(MOTOR_CMD_FORWARD);
|
||||
RunSta = true;
|
||||
}
|
||||
else if((RunSta == true) && (KeyASta == false) && (KeyBSta == false))
|
||||
{
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
RunSta = false;
|
||||
}
|
||||
|
||||
if(Dcmotor1msStatus == true)
|
||||
{
|
||||
Dcmotor1msStatus = false;
|
||||
|
||||
// 电机状态机处理
|
||||
switch (g_motorState) {
|
||||
case MOTOR_STOPPED:
|
||||
// Motor_SetSpeed(0);
|
||||
// motor_pwm_set(0);
|
||||
dcmotor_speed(0);
|
||||
|
||||
// 检测新命令
|
||||
if (g_motorCmd == MOTOR_CMD_FORWARD) {
|
||||
Motor_SetDirection(0); // 设置正转方向
|
||||
g_motorState = MOTOR_ACCELERATING;
|
||||
accelCounter = 0;
|
||||
// Motor_SetSpeed(ACCEL_STEP); // 初始速度
|
||||
// motor_pwm_set(ACCEL_STEP);
|
||||
dcmotor_speed(ACCEL_STEP);
|
||||
}
|
||||
else if (g_motorCmd == MOTOR_CMD_REVERSE) {
|
||||
Motor_SetDirection(1); // 设置反转方向
|
||||
g_motorState = MOTOR_ACCELERATING;
|
||||
accelCounter = 0;
|
||||
// Motor_SetSpeed(ACCEL_STEP); // 初始速度
|
||||
// motor_pwm_set(ACCEL_STEP);
|
||||
dcmotor_speed(ACCEL_STEP);
|
||||
}
|
||||
break;
|
||||
|
||||
case MOTOR_ACCELERATING:
|
||||
// 加速过程
|
||||
if (++accelCounter >= ACCEL_INTERVAL) {
|
||||
accelCounter = 0;
|
||||
uint16_t newSpeed = g_motorSpeed + ACCEL_STEP;
|
||||
|
||||
if (newSpeed >= MAX_SPEED) {
|
||||
newSpeed = MAX_SPEED;
|
||||
g_motorState = MOTOR_RUNNING; // 达到最大速度，进入匀速
|
||||
}
|
||||
// Motor_SetSpeed(newSpeed);
|
||||
// motor_pwm_set(newSpeed);
|
||||
dcmotor_speed(newSpeed);
|
||||
}
|
||||
|
||||
// 检查停止命令
|
||||
if (g_motorCmd == MOTOR_CMD_STOP) {
|
||||
g_motorState = MOTOR_DECELERATING;
|
||||
}
|
||||
break;
|
||||
|
||||
case MOTOR_RUNNING:
|
||||
// 检查停止命令
|
||||
if (g_motorCmd == MOTOR_CMD_STOP) {
|
||||
g_motorState = MOTOR_DECELERATING;
|
||||
}
|
||||
// 检查方向改变命令
|
||||
else if ((g_motorCmd == MOTOR_CMD_FORWARD && g_motorDirection != DIR_FORWARD) ||
|
||||
(g_motorCmd == MOTOR_CMD_REVERSE && g_motorDirection != DIR_REVERSE)) {
|
||||
g_motorState = MOTOR_DECELERATING; // 需要先减速停止
|
||||
}
|
||||
break;
|
||||
|
||||
case MOTOR_DECELERATING:
|
||||
// 减速过程
|
||||
if (++accelCounter >= ACCEL_INTERVAL) {
|
||||
accelCounter = 0;
|
||||
// if (g_motorSpeed > DECEL_STEP) {
|
||||
if (g_motorSpeed > DECEL_STEP) {
|
||||
// Motor_SetSpeed(g_motorSpeed - DECEL_STEP);
|
||||
// motor_pwm_set(g_motorSpeed - DECEL_STEP);
|
||||
dcmotor_speed(g_motorSpeed - DECEL_STEP);
|
||||
} else {
|
||||
// Motor_SetSpeed(0);
|
||||
// motor_pwm_set(0);
|
||||
dcmotor_speed(0);
|
||||
g_motorState = MOTOR_STOPPED; // 减速完成，进入停止状态
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
|
||||
#ifndef __DCMOTOR_H
|
||||
#define __DCMOTOR_H
|
||||
|
||||
#include "hc32_ddl.h"
|
||||
|
||||
|
||||
// 电机控制参数
|
||||
#define PWM_PERIOD TIM_PERIOD // PWM周期值 (ARR)
|
||||
#define MAX_SPEED 9900 // 最大速度(PWM占空比)
|
||||
#define ACCEL_STEP 100 // 加速步进值
|
||||
#define DECEL_STEP 100 // 减速步进值
|
||||
#define ACCEL_INTERVAL 5 // 加减速间隔(ms)
|
||||
|
||||
|
||||
|
||||
// 电机控制命令
|
||||
typedef enum {
|
||||
MOTOR_CMD_STOP,
|
||||
MOTOR_CMD_FORWARD,
|
||||
MOTOR_CMD_REVERSE
|
||||
} MotorCommand;
|
||||
|
||||
// 电机运行状态机
|
||||
typedef enum {
|
||||
MOTOR_STOPPED,
|
||||
MOTOR_ACCELERATING,
|
||||
MOTOR_RUNNING,
|
||||
MOTOR_DECELERATING
|
||||
} MotorState;
|
||||
|
||||
|
||||
// 电机转动方向
|
||||
typedef enum {
|
||||
DIR_STOPPED,
|
||||
DIR_FORWARD,
|
||||
DIR_REVERSE
|
||||
} MotorDirection;
|
||||
|
||||
|
||||
|
||||
/* 停止引脚操作宏定义
|
||||
* 此引脚控制H桥是否生效以达到开启和关闭电机的效果
|
||||
*/
|
||||
#define SHUTDOWN1_Pin Pin12
|
||||
#define SHUTDOWN1_GPIO_Port PortB
|
||||
|
||||
//#define SHUTDOWN2_Pin GPIO_PIN_2
|
||||
//#define SHUTDOWN2_GPIO_Port GPIOF
|
||||
#define SHUTDOWN_GPIO_CLK_ENABLE() do{ __HAL_RCC_GPIOF_CLK_ENABLE(); }while(0) /* PF口时钟使能 */
|
||||
|
||||
/* 电机停止引脚定义 这里默认是接口1 */
|
||||
#define ENABLE_MOTOR PORT_SetBits(SHUTDOWN1_GPIO_Port,SHUTDOWN1_Pin)
|
||||
#define DISABLE_MOTOR PORT_ResetBits(SHUTDOWN1_GPIO_Port,SHUTDOWN1_Pin)
|
||||
|
||||
/******************************************************************************************/
|
||||
|
||||
void dcmotor_init(void); /* 直流有刷电机初始化 */
|
||||
void dcmotor_start(void); /* 开启电机 */
|
||||
void dcmotor_stop(void); /* 关闭电机 */
|
||||
void dcmotor_dir(uint8_t para); /* 设置电机方向 */
|
||||
void dcmotor_speed(uint16_t para); /* 设置电机速度 */
|
||||
void motor_pwm_set(float para); /* 电机控制 */
|
||||
|
||||
|
||||
|
||||
|
||||
MotorDirection Motor_GetDirection(void);/* 获取电机转动方向 */
|
||||
void Motor_Control(MotorCommand cmd); /* 电机控制 */
|
||||
void Dcmotor1msRoutine(void);
|
||||
void DcmotorLoopHandler(void);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -88,8 +88,8 @@ extern "C"
|
||||
#define DDL_QSPI_ENABLE (DDL_OFF)
|
||||
#define DDL_RMU_ENABLE (DDL_OFF)
|
||||
#define DDL_RTC_ENABLE (DDL_ON)
|
||||
#define DDL_SDIOC_ENABLE (DDL_ON)
|
||||
#define DDL_SPI_ENABLE (DDL_ON)
|
||||
#define DDL_SDIOC_ENABLE (DDL_OFF)
|
||||
#define DDL_SPI_ENABLE (DDL_OFF)
|
||||
#define DDL_SRAM_ENABLE (DDL_ON)
|
||||
#define DDL_SWDT_ENABLE (DDL_ON)
|
||||
#define DDL_TIMER0_ENABLE (DDL_OFF)
|
||||
@@ -98,7 +98,7 @@ extern "C"
|
||||
#define DDL_TIMER4_OCO_ENABLE (DDL_OFF)
|
||||
#define DDL_TIMER4_PWM_ENABLE (DDL_OFF)
|
||||
#define DDL_TIMER4_SEVT_ENABLE (DDL_OFF)
|
||||
#define DDL_TIMER6_ENABLE (DDL_OFF)
|
||||
#define DDL_TIMER6_ENABLE (DDL_ON)
|
||||
#define DDL_TIMERA_ENABLE (DDL_OFF)
|
||||
#define DDL_TRNG_ENABLE (DDL_OFF)
|
||||
#define DDL_USART_ENABLE (DDL_ON)
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
#include "Encryption.h"
|
||||
#include "Debug.h"
|
||||
#include "Algorithm.h"
|
||||
#include "dc_motor.h"
|
||||
#include "CS1237_LEFTUP.h"
|
||||
#include "CS1237_LEFTLOW.h"
|
||||
#include "CS1237_RIGHTUP.h"
|
||||
@@ -132,8 +133,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
{
|
||||
uint32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]);
|
||||
uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]);
|
||||
uint32_t LeftDiffSum = abs(lu_ad[0]) + abs(ll_ad[0]);
|
||||
uint32_t RightDiffSum = abs(ru_ad[0]) + abs(rl_ad[0]);
|
||||
uint32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]);
|
||||
uint32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]);
|
||||
|
||||
App.LeftRunFalg = false;
|
||||
App.RightRunFalg = false;
|
||||
@@ -154,6 +155,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& (Oll_diff3 <= -App.ADTrack.LeftLowVPT)))
|
||||
{
|
||||
//BDC_UP();
|
||||
Motor_Control(MOTOR_CMD_FORWARD);
|
||||
App.UpFlag = true;
|
||||
App.StopFlag = false;
|
||||
App.DownFlag = false;
|
||||
@@ -172,6 +174,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT)))
|
||||
{
|
||||
//BDC_DOWN();
|
||||
Motor_Control(MOTOR_CMD_REVERSE);
|
||||
App.DownFlag = true;
|
||||
App.StopFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -194,6 +197,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& (Orl_diff3 <= -App.ADTrack.RightLowVPT)))
|
||||
{
|
||||
//BDC_UP();
|
||||
Motor_Control(MOTOR_CMD_FORWARD);
|
||||
App.UpFlag = true;
|
||||
App.StopFlag = false;
|
||||
App.DownFlag = false;
|
||||
@@ -212,6 +216,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& (Orl_diff3 >= App.ADTrack.RightLowVPT)))
|
||||
{
|
||||
//BDC_DOWN();
|
||||
Motor_Control(MOTOR_CMD_REVERSE);
|
||||
App.DownFlag = true;
|
||||
App.StopFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -227,6 +232,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& ll_slope > App.ADTrack.Slope_VPT)
|
||||
{
|
||||
//BDC_STOP();
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -236,6 +242,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& Oll_diff1 < (App.ADTrack.LeftLowVPT / 2))
|
||||
{
|
||||
//BDC_STOP();
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -248,6 +255,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& rl_slope > App.ADTrack.Slope_VPT)
|
||||
{
|
||||
//BDC_STOP();
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -257,6 +265,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& Orl_diff1 < (App.ADTrack.RightLowVPT / 2))
|
||||
{
|
||||
//BDC_STOP();
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -272,6 +281,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& ll_slope < -App.ADTrack.Slope_VPT)
|
||||
{
|
||||
//BDC_STOP();
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -281,6 +291,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& Oll_diff1 > -(App.ADTrack.LeftLowVPT / 2))
|
||||
{
|
||||
//BDC_STOP();
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -293,15 +304,17 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
|
||||
&& rl_slope < -App.ADTrack.Slope_VPT)
|
||||
{
|
||||
//BDC_STOP();
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
App.StopCollDelay1mSCnt = 1000;
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
App.StopCollDelay1mSCnt = 1000;
|
||||
}
|
||||
else if(Oru_diff1 < (App.ADTrack.RightUpVPT / 2)
|
||||
&& Orl_diff1 > -(App.ADTrack.RightLowVPT / 2))
|
||||
{
|
||||
//BDC_STOP();
|
||||
Motor_Control(MOTOR_CMD_STOP);
|
||||
App.StopFlag = true;
|
||||
App.DownFlag = false;
|
||||
App.UpFlag = false;
|
||||
@@ -574,6 +587,7 @@ int main(void)
|
||||
RIGHTUP_CS1237DataLoopCollect();
|
||||
RIGHTLOW_CS1237DataLoopCollect();
|
||||
ReadLoopHandler();
|
||||
DcmotorLoopHandler();
|
||||
AppLoopHandler();
|
||||
}
|
||||
}
|
||||
@@ -593,6 +607,7 @@ void SysTick_IrqHandler(void)
|
||||
LEFTLOW_CS12371mSRoutine();
|
||||
RIGHTUP_CS12371mSRoutine();
|
||||
RIGHTLOW_CS12371mSRoutine();
|
||||
Dcmotor1msRoutine();
|
||||
}
|
||||
|
||||
/*****************************************************************************************
|
||||
|
||||
Reference in New Issue
Block a user