103 lines
2.7 KiB
C
103 lines
2.7 KiB
C
#include "motor.h"
|
||
#include "gpio.h"
|
||
#include "tim.h"
|
||
|
||
#define MAX_SPEED 100
|
||
#define MIN_SPEED 5
|
||
#define MOTOR_L 1
|
||
#define MOTOR_R 2
|
||
#define MOTOR_TIM (&htim4)
|
||
#define MOTOR_L_CHAN TIM_CHANNEL_2
|
||
#define MOTOR_R_CHAN TIM_CHANNEL_1
|
||
|
||
int8_t preProcess(int8_t duty, uint8_t motor);
|
||
|
||
void MOTOR_Init(void)
|
||
{
|
||
HAL_TIM_PWM_Start_IT(MOTOR_TIM, MOTOR_L_CHAN);
|
||
HAL_TIM_PWM_Start_IT(MOTOR_TIM, MOTOR_R_CHAN);
|
||
}
|
||
|
||
/// @brief 自由滑行
|
||
/// @param
|
||
void MOTOR_Slip(void)
|
||
{
|
||
HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin | L_BACKWARD_Pin | R_FORWARD_Pin | R_BACKWARD_Pin, GPIO_PIN_RESET);
|
||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, MAX_SPEED);
|
||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, MAX_SPEED);
|
||
}
|
||
|
||
/// @brief 刹车
|
||
/// @param
|
||
void MOTOR_Stop(void)
|
||
{
|
||
HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin | L_BACKWARD_Pin | R_FORWARD_Pin | R_BACKWARD_Pin, GPIO_PIN_RESET);
|
||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, 0);
|
||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, 0);
|
||
}
|
||
|
||
/// @brief 控制电机占空比
|
||
|
||
/// @param leftDuty 绝对值 0或20-100, 支持正负
|
||
/// @param rightDuty 绝对值 0或20-100，支持正负
|
||
void MOTOR_SetDuty(int8_t leftDuty, int8_t rightDuty)
|
||
{
|
||
leftDuty = preProcess(leftDuty, MOTOR_L);
|
||
rightDuty = preProcess(rightDuty, MOTOR_R);
|
||
|
||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, leftDuty);
|
||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, rightDuty);
|
||
}
|
||
|
||
int8_t preProcess(int8_t duty, uint8_t motor)
|
||
{
|
||
if (duty > 0)
|
||
{
|
||
if (motor & MOTOR_L)
|
||
{
|
||
HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_SET);
|
||
HAL_GPIO_WritePin(L_BACKWARD_GPIO_Port, L_BACKWARD_Pin, GPIO_PIN_RESET);
|
||
}
|
||
if (motor & MOTOR_R)
|
||
{
|
||
HAL_GPIO_WritePin(R_FORWARD_GPIO_Port, R_FORWARD_Pin, GPIO_PIN_SET);
|
||
HAL_GPIO_WritePin(R_BACKWARD_GPIO_Port, R_BACKWARD_Pin, GPIO_PIN_RESET);
|
||
}
|
||
|
||
}
|
||
else if (duty < 0)
|
||
{
|
||
duty = -duty;
|
||
if (motor & MOTOR_L)
|
||
{
|
||
HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_RESET);
|
||
HAL_GPIO_WritePin(L_BACKWARD_GPIO_Port, L_BACKWARD_Pin, GPIO_PIN_SET);
|
||
}
|
||
if (motor & MOTOR_R)
|
||
{
|
||
HAL_GPIO_WritePin(R_FORWARD_GPIO_Port, R_FORWARD_Pin, GPIO_PIN_RESET);
|
||
HAL_GPIO_WritePin(R_BACKWARD_GPIO_Port, R_BACKWARD_Pin, GPIO_PIN_SET);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
duty = MAX_SPEED;
|
||
if (motor & MOTOR_L)
|
||
{
|
||
HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_RESET);
|
||
HAL_GPIO_WritePin(L_BACKWARD_GPIO_Port, L_BACKWARD_Pin, GPIO_PIN_RESET);
|
||
}
|
||
if (motor & MOTOR_R)
|
||
{
|
||
HAL_GPIO_WritePin(R_FORWARD_GPIO_Port, R_FORWARD_Pin, GPIO_PIN_RESET);
|
||
HAL_GPIO_WritePin(R_BACKWARD_GPIO_Port, R_BACKWARD_Pin, GPIO_PIN_RESET);
|
||
}
|
||
}
|
||
|
||
if (duty > MAX_SPEED)
|
||
duty = MAX_SPEED;
|
||
if (duty < MIN_SPEED)
|
||
duty = MIN_SPEED;
|
||
return duty;
|
||
}
|