✨ feat: 完善小车巡线运动
This commit is contained in:
parent
0aaaa8f610
commit
a88ab88eb5
|
@ -1,6 +1,10 @@
|
|||
#ifndef __APP_LINE_SEEK_H
|
||||
#define __APP_LINE_SEEK_H
|
||||
|
||||
#include "tim.h"
|
||||
|
||||
void LineSeek_Init(void);
|
||||
void App_LineSeek(void);
|
||||
void Hanlde_Crossroad(void);
|
||||
|
||||
#endif
|
|
@ -4,44 +4,281 @@
|
|||
#include "line_seek.h"
|
||||
#include "main.h"
|
||||
#include "motor.h"
|
||||
#include "path_plan.h"
|
||||
#include "stm32f1xx_hal_tim.h"
|
||||
#include "syscalls.h"
|
||||
#include "tim.h"
|
||||
|
||||
#define LOW_TUNE_SPEED -5
|
||||
#define LOW_SPEED 35
|
||||
#define MID_SPEED 40
|
||||
#define HIGH_SPEED 45
|
||||
#define MIN_TURN_TIME 300 // 最小转直角时间
|
||||
#define MIN_CROSSING_TIME 200 // 最小走十字路口时间
|
||||
#define MIN_CROSSING_STRAIGHT_TIME 50 // 最小走十字路口时间
|
||||
#define MIN_TUNE_TIME 40 // 最小微调时间
|
||||
#define MAX_TURN_TIME 4000 // 最大转弯时间
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MOVE_STRAIGHT, // 直行
|
||||
TURN_LEFT, // 左转
|
||||
TURN_RIGHT, // 右转
|
||||
TUNE_LEFT, // 左微调
|
||||
TUNE_RIGHT, // 右微调
|
||||
TURN_LEFT_BIG,
|
||||
TURN_RIGHT_BIG
|
||||
} MoveState;
|
||||
|
||||
MoveState state, preState; // 运动状态
|
||||
int turn_time; // 转弯时间
|
||||
int beep_time; // 蜂鸣器鸣叫时间
|
||||
int is_crossing; // 是否正在走十字路口
|
||||
|
||||
void LINESEEK_HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim)
|
||||
{
|
||||
if (turn_time < INT16_MAX)
|
||||
{
|
||||
turn_time++; // 每1ms自增一
|
||||
}
|
||||
|
||||
if (beep_time++ > 100)
|
||||
{
|
||||
buzzer(0);
|
||||
}
|
||||
}
|
||||
|
||||
void LineSeek_Init(void)
|
||||
{
|
||||
// 注册定时器中断溢出回调
|
||||
HAL_TIM_RegisterCallback(&htim4, HAL_TIM_PERIOD_ELAPSED_CB_ID, LINESEEK_HAL_TIM_PeriodElapsedCallback);
|
||||
HAL_TIM_Base_Start_IT(&htim4);
|
||||
}
|
||||
|
||||
void App_LineSeek(void)
|
||||
{
|
||||
int LineL1 = 1, LineL2 = 1, LineR1 = 1, LineR2 = 1;
|
||||
|
||||
LineSeek_Get(&LineL1, &LineL2, &LineR1, &LineR2); // 获取黑线检测状态
|
||||
// LineSeek_GetStatus(&LineL1, &LineL2, &LineR1, &LineR2); // 获取黑线检测状态
|
||||
|
||||
if ((LineL1 == LOW || LineL2 == LOW) && LineR2 == LOW) // 左大弯
|
||||
LineSeek_GetStatusStr();
|
||||
// printf("%s %d \n", LineSeek_Status, turn_time);
|
||||
|
||||
// return;
|
||||
|
||||
if (turn_time > MIN_CROSSING_TIME)
|
||||
{
|
||||
MOTOR_SetDuty(-30, 30); // 左旋
|
||||
HAL_Delay(80);
|
||||
is_crossing = 0;
|
||||
}
|
||||
else if (LineL1 == LOW && (LineR1 == LOW || LineR2 == LOW)) // 右大弯
|
||||
|
||||
if (is_crossing && GetCurrentDirection() == STRAIGHT)
|
||||
{
|
||||
MOTOR_SetDuty(30, -30); // 右旋
|
||||
HAL_Delay(80);
|
||||
LineSeek_Status[0] = '1';
|
||||
LineSeek_Status[3] = '1';
|
||||
}
|
||||
else if (LineL1 == LOW) // 左最外侧检测
|
||||
|
||||
switch (state)
|
||||
{
|
||||
MOTOR_SetDuty(-50, 50); // 左旋
|
||||
HAL_Delay(10);
|
||||
}
|
||||
else if (LineR2 == LOW) // 右最外侧检测
|
||||
case MOVE_STRAIGHT: // 直行
|
||||
MOTOR_SetDuty(HIGH_SPEED, HIGH_SPEED);
|
||||
if (LineSeek_Equals("0000")) // 十字路口
|
||||
{
|
||||
MOTOR_SetDuty(50, -50); // 右旋
|
||||
HAL_Delay(10);
|
||||
Hanlde_Crossroad();
|
||||
}
|
||||
else if (LineL2 == LOW && LineR1 == HIGH) // 中间黑线上的传感器微调车左转
|
||||
else if (LineSeek_Equals("1011")) // 左微调
|
||||
{
|
||||
MOTOR_SetDuty(0, 50); // 左转
|
||||
state = TUNE_LEFT;
|
||||
}
|
||||
else if (LineL2 == HIGH && LineR1 == LOW) // 中间黑线上的传感器微调车右转
|
||||
else if (LineSeek_Equals("1101")) // 右微调
|
||||
{
|
||||
MOTOR_SetDuty(50, 0); // 右转
|
||||
state = TUNE_RIGHT;
|
||||
}
|
||||
else if (LineL2 == LOW && LineR1 == LOW) // 都是黑色, 加速前进
|
||||
else if (LineSeek_Equals("0001")) // 左直角
|
||||
{
|
||||
MOTOR_SetDuty(50, 50); // 直走
|
||||
turn_time = 0;
|
||||
state = TURN_LEFT;
|
||||
}
|
||||
else if (LineSeek_Equals("1000")) // 右直角
|
||||
{
|
||||
turn_time = 0;
|
||||
state = TURN_RIGHT;
|
||||
}
|
||||
else if (LineSeek_Equals("0xx1")) // 左锐角
|
||||
{
|
||||
state = TURN_LEFT_BIG;
|
||||
}
|
||||
else if (LineSeek_Equals("1xx0")) // 右锐角
|
||||
{
|
||||
state = TURN_RIGHT_BIG;
|
||||
}
|
||||
else if (LineSeek_Equals("1111")) // 冲出去了,使用上一个转向
|
||||
{
|
||||
state = preState;
|
||||
}
|
||||
break;
|
||||
case TURN_LEFT: // 左直角
|
||||
MOTOR_SetDuty(-HIGH_SPEED, HIGH_SPEED);
|
||||
if (LineSeek_Equals("0000")) // 十字路口
|
||||
{
|
||||
Hanlde_Crossroad();
|
||||
}
|
||||
else if (LineSeek_Equals("1001") &&
|
||||
((preState != TUNE_LEFT && turn_time >= MIN_TURN_TIME - 1) ||
|
||||
(preState == TUNE_LEFT && turn_time >= MIN_TUNE_TIME))) // 直行
|
||||
{
|
||||
state = MOVE_STRAIGHT;
|
||||
preState = TURN_LEFT;
|
||||
is_crossing = 0;
|
||||
}
|
||||
else if (LineSeek_Equals("xxx0") && !is_crossing) // 转过头了
|
||||
{
|
||||
turn_time = MIN_TURN_TIME;
|
||||
state = TURN_RIGHT;
|
||||
preState = TURN_LEFT;
|
||||
}
|
||||
break;
|
||||
case TURN_RIGHT: // 右直角
|
||||
MOTOR_SetDuty(HIGH_SPEED, -HIGH_SPEED);
|
||||
if (LineSeek_Equals("0000")) // 十字路口
|
||||
{
|
||||
Hanlde_Crossroad();
|
||||
}
|
||||
else if (LineSeek_Equals("1001") &&
|
||||
((preState != TUNE_RIGHT && turn_time >= MIN_TURN_TIME - 1) ||
|
||||
(preState == TUNE_RIGHT && turn_time >= MIN_TUNE_TIME))) // 直行
|
||||
{
|
||||
state = MOVE_STRAIGHT;
|
||||
preState = TURN_RIGHT;
|
||||
is_crossing = 0;
|
||||
}
|
||||
else if (LineSeek_Equals("0xxx") && !is_crossing) // 转过头了
|
||||
{
|
||||
turn_time = MIN_TURN_TIME;
|
||||
state = TURN_LEFT;
|
||||
preState = TURN_LEFT;
|
||||
}
|
||||
break;
|
||||
case TURN_LEFT_BIG: // 左锐角
|
||||
MOTOR_SetDuty(-MID_SPEED, MID_SPEED);
|
||||
if (LineSeek_Equals("0000")) // 十字路口
|
||||
{
|
||||
Hanlde_Crossroad();
|
||||
}
|
||||
else if (LineSeek_Equals("0001")) // 左直角
|
||||
{
|
||||
turn_time = 0;
|
||||
state = TURN_LEFT;
|
||||
preState = TURN_LEFT_BIG;
|
||||
}
|
||||
else if (LineSeek_Equals("xxx0")) // 转过头了
|
||||
{
|
||||
state = TUNE_RIGHT;
|
||||
// preState = TURN_LEFT_BIG;
|
||||
}
|
||||
break;
|
||||
case TURN_RIGHT_BIG: // 右锐角
|
||||
MOTOR_SetDuty(MID_SPEED, -MID_SPEED);
|
||||
if (LineSeek_Equals("0000")) // 十字路口
|
||||
{
|
||||
Hanlde_Crossroad();
|
||||
}
|
||||
else if (LineSeek_Equals("1000")) // 右直角
|
||||
{
|
||||
turn_time = 0;
|
||||
state = TURN_RIGHT;
|
||||
preState = TURN_RIGHT_BIG;
|
||||
}
|
||||
else if (LineSeek_Equals("0xxx")) // 转过头了
|
||||
{
|
||||
state = TUNE_LEFT;
|
||||
preState = TURN_RIGHT_BIG;
|
||||
}
|
||||
break;
|
||||
case TUNE_LEFT: // 左微调
|
||||
MOTOR_SetDuty(LOW_TUNE_SPEED, HIGH_SPEED);
|
||||
if (LineSeek_Equals("x00x"))
|
||||
{
|
||||
state = MOVE_STRAIGHT;
|
||||
}
|
||||
else if (LineSeek_Equals("0xx1") && GetCurrentDirection() != STRAIGHT) // 左转
|
||||
{
|
||||
turn_time = 0;
|
||||
state = TURN_LEFT;
|
||||
preState = TUNE_LEFT;
|
||||
}
|
||||
else if (LineSeek_Equals("1xx0") && GetCurrentDirection() != STRAIGHT) // 右转
|
||||
{
|
||||
turn_time = 0;
|
||||
state = TURN_RIGHT;
|
||||
preState = TUNE_LEFT;
|
||||
}
|
||||
break;
|
||||
case TUNE_RIGHT: // 右微调
|
||||
MOTOR_SetDuty(HIGH_SPEED, LOW_TUNE_SPEED);
|
||||
if (LineSeek_Equals("x00x"))
|
||||
{
|
||||
state = MOVE_STRAIGHT;
|
||||
}
|
||||
else if (LineSeek_Equals("0xx1") && GetCurrentDirection() != STRAIGHT) // 左转
|
||||
{
|
||||
turn_time = 0;
|
||||
state = TURN_LEFT;
|
||||
preState = TUNE_RIGHT;
|
||||
}
|
||||
else if (LineSeek_Equals("1xx0") && GetCurrentDirection() != STRAIGHT) // 右转
|
||||
{
|
||||
turn_time = 0;
|
||||
state = TURN_RIGHT;
|
||||
preState = TUNE_RIGHT;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
state = MOVE_STRAIGHT;
|
||||
MOTOR_SetDuty(MID_SPEED, MID_SPEED);
|
||||
break;
|
||||
}
|
||||
|
||||
// if (turn_time >= MAX_TURN_TIME)
|
||||
// {
|
||||
// // state = MOVE_STRAIGHT;
|
||||
// MOTOR_SetDuty(0, 0);
|
||||
// }
|
||||
|
||||
// printf("curr state: %d \n", state);
|
||||
}
|
||||
|
||||
void Hanlde_Crossroad(void)
|
||||
{
|
||||
if (is_crossing)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
beep_time = 0;
|
||||
buzzer(1);
|
||||
// state = MOVE_STRAIGHT;
|
||||
// buzzer(1);
|
||||
// HAL_Delay(50);
|
||||
// buzzer(0);
|
||||
|
||||
// 根据规划的路径进行决断
|
||||
is_crossing = 1;
|
||||
turn_time = 0;
|
||||
int direction = GetNextDirection();
|
||||
switch (direction)
|
||||
{
|
||||
case STRAIGHT:
|
||||
state = MOVE_STRAIGHT;
|
||||
break;
|
||||
case LEFT:
|
||||
state = TURN_LEFT;
|
||||
break;
|
||||
case RIGHT:
|
||||
state = TURN_RIGHT;
|
||||
break;
|
||||
default:
|
||||
state = TURN_RIGHT;
|
||||
break;
|
||||
}
|
||||
// printf("state: %d, dir: %d \n", state, direction);
|
||||
}
|
88
App/Src/app_line_seek_v1.c
Normal file
88
App/Src/app_line_seek_v1.c
Normal file
|
@ -0,0 +1,88 @@
|
|||
|
||||
#include "app_line_seek.h"
|
||||
|
||||
#include "line_seek.h"
|
||||
#include "main.h"
|
||||
#include "motor.h"
|
||||
#include "path_plan.h"
|
||||
#include "syscalls.h"
|
||||
|
||||
// #define LOW_SPEED 30
|
||||
// #define MID_SPEED 35
|
||||
// #define HIGH_SPEED 40
|
||||
|
||||
#define LOW_SPEED 25
|
||||
#define MID_SPEED 30
|
||||
#define HIGH_SPEED 35
|
||||
|
||||
|
||||
void App_LineSeek(void)
|
||||
{
|
||||
int LineL1 = 1, LineL2 = 1, LineR1 = 1, LineR2 = 1;
|
||||
|
||||
LineSeek_GetStatus(&LineL1, &LineL2, &LineR1, &LineR2); // 获取黑线检测状态
|
||||
|
||||
LineSeek_GetStatusStr();
|
||||
printf("%s\n", LineSeek_Status);
|
||||
|
||||
// return;
|
||||
|
||||
if (LineSeek_Equals("0000")) // 丁字(十字)路口
|
||||
{
|
||||
buzzer(1);
|
||||
|
||||
// 根据规划的路径进行决断
|
||||
switch (GetNextDirection())
|
||||
{
|
||||
case STRAIGHT:
|
||||
MOTOR_SetDuty(HIGH_SPEED, HIGH_SPEED); // 直走
|
||||
break;
|
||||
case LEFT:
|
||||
MOTOR_SetDuty(HIGH_SPEED, -HIGH_SPEED); // 左转
|
||||
break;
|
||||
case RIGHT:
|
||||
MOTOR_SetDuty(-HIGH_SPEED, HIGH_SPEED); // 右转
|
||||
break;
|
||||
default:
|
||||
MOTOR_Stop();
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
buzzer(0);
|
||||
}
|
||||
|
||||
if (LineSeek_Equals("1000")) // 右直角
|
||||
{
|
||||
MOTOR_SetDuty(HIGH_SPEED, -HIGH_SPEED); // 右旋
|
||||
// HAL_Delay(80);
|
||||
}
|
||||
else if (LineSeek_Equals("0001")) // 左直角
|
||||
{
|
||||
MOTOR_SetDuty(-HIGH_SPEED, HIGH_SPEED); // 左旋;
|
||||
// HAL_Delay(80);
|
||||
}
|
||||
else if (LineSeek_Equals("0xx1")) // 左最外侧检测(锐角)
|
||||
{
|
||||
MOTOR_SetDuty(-HIGH_SPEED, MID_SPEED); // 左旋
|
||||
// HAL_Delay(80);
|
||||
}
|
||||
else if (LineSeek_Equals("1xx0")) // 右最外侧检测(锐角)
|
||||
{
|
||||
MOTOR_SetDuty(MID_SPEED, -HIGH_SPEED); // 右旋
|
||||
// HAL_Delay(80);
|
||||
}
|
||||
else if (LineSeek_Equals("x01x")) // 中间黑线上的传感器微调车左转
|
||||
{
|
||||
MOTOR_SetDuty(0, LOW_SPEED); // 左转
|
||||
}
|
||||
else if (LineSeek_Equals("x10x")) // 中间黑线上的传感器微调车右转
|
||||
{
|
||||
MOTOR_SetDuty(LOW_SPEED, 0); // 右转
|
||||
}
|
||||
else if (LineSeek_Equals("x00x")) // 都是黑色, 加速前进
|
||||
{
|
||||
MOTOR_SetDuty(MID_SPEED, MID_SPEED); // 直走
|
||||
}
|
||||
}
|
8
CAR.ioc
8
CAR.ioc
|
@ -81,6 +81,8 @@ NVIC.PriorityGroup=NVIC_PRIORITYGROUP_2
|
|||
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
|
||||
NVIC.SysTick_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:false
|
||||
NVIC.TIM2_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
|
||||
NVIC.TIM3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
|
||||
NVIC.TIM4_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
|
||||
NVIC.USART2_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
|
||||
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
|
||||
PA0-WKUP.GPIOParameters=PinState,GPIO_Label
|
||||
|
@ -254,19 +256,21 @@ TIM2.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_DISABLE
|
|||
TIM2.IPParameters=Prescaler,Period,AutoReloadPreload
|
||||
TIM2.Period=72 - 1
|
||||
TIM2.Prescaler=10 - 1
|
||||
TIM3.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
|
||||
TIM3.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
|
||||
TIM3.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
|
||||
TIM3.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4
|
||||
TIM3.ClockDivision=TIM_CLOCKDIVISION_DIV1
|
||||
TIM3.IPParameters=Channel-PWM Generation4 CH4,Prescaler,ClockDivision,Period,Pulse-PWM Generation4 CH4,Channel-PWM Generation3 CH3,Channel-PWM Generation2 CH2,Pulse-PWM Generation2 CH2,Pulse-PWM Generation3 CH3
|
||||
TIM3.IPParameters=Channel-PWM Generation4 CH4,Prescaler,ClockDivision,Period,Pulse-PWM Generation4 CH4,Channel-PWM Generation3 CH3,Channel-PWM Generation2 CH2,Pulse-PWM Generation2 CH2,Pulse-PWM Generation3 CH3,AutoReloadPreload
|
||||
TIM3.Period=256 - 1
|
||||
TIM3.Prescaler=720 - 1
|
||||
TIM3.Pulse-PWM\ Generation2\ CH2=255
|
||||
TIM3.Pulse-PWM\ Generation3\ CH3=255
|
||||
TIM3.Pulse-PWM\ Generation4\ CH4=255
|
||||
TIM4.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
|
||||
TIM4.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
|
||||
TIM4.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
|
||||
TIM4.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Prescaler,Period,Pulse-PWM Generation1 CH1,Pulse-PWM Generation2 CH2
|
||||
TIM4.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Prescaler,Period,Pulse-PWM Generation1 CH1,Pulse-PWM Generation2 CH2,AutoReloadPreload
|
||||
TIM4.Period=100
|
||||
TIM4.Prescaler=720 - 1
|
||||
TIM4.Pulse-PWM\ Generation1\ CH1=100
|
||||
|
|
|
@ -57,6 +57,8 @@ void PendSV_Handler(void);
|
|||
void SysTick_Handler(void);
|
||||
void DMA1_Channel6_IRQHandler(void);
|
||||
void TIM2_IRQHandler(void);
|
||||
void TIM3_IRQHandler(void);
|
||||
void TIM4_IRQHandler(void);
|
||||
void USART2_IRQHandler(void);
|
||||
void EXTI15_10_IRQHandler(void);
|
||||
/* USER CODE BEGIN EFP */
|
||||
|
|
|
@ -27,10 +27,12 @@
|
|||
|
||||
/* Private includes ----------------------------------------------------------*/
|
||||
/* USER CODE BEGIN Includes */
|
||||
#include "app_line_seek.h"
|
||||
#include "bluetooth.h"
|
||||
#include "hcsr04.h"
|
||||
#include "led.h"
|
||||
#include "motor.h"
|
||||
#include "path_plan.h"
|
||||
#include "syscalls.h"
|
||||
/* USER CODE END Includes */
|
||||
|
||||
|
@ -77,6 +79,7 @@ int main(void)
|
|||
{
|
||||
/* USER CODE BEGIN 1 */
|
||||
|
||||
uint8_t pData[10] = {1, 2, 3};
|
||||
/* USER CODE END 1 */
|
||||
|
||||
/* MCU Configuration--------------------------------------------------------*/
|
||||
|
@ -102,24 +105,34 @@ int main(void)
|
|||
MX_TIM2_Init();
|
||||
MX_TIM3_Init();
|
||||
MX_TIM4_Init();
|
||||
MX_USART2_UART_Init();
|
||||
// MX_USART2_UART_Init();
|
||||
MX_USART1_UART_Init();
|
||||
|
||||
/* USER CODE BEGIN 2 */
|
||||
HC_SR04_Init();
|
||||
BLUETOOTH_Init();
|
||||
|
||||
/* USER CODE BEGIN 2 */
|
||||
|
||||
MOTOR_Init();
|
||||
PathPlanner_Init();
|
||||
LineSeek_Init();
|
||||
/* USER CODE END 2 */
|
||||
|
||||
/* Infinite loop */
|
||||
/* USER CODE BEGIN WHILE */
|
||||
while (1)
|
||||
{
|
||||
my_printf(HUART1, "test huart1 \r\n");
|
||||
my_printf(HUART2, "test huart2\r\n");
|
||||
// my_printf(HUART1, "%s\r\n", *pData);
|
||||
|
||||
// App_LineSeek();
|
||||
// if (HAL_OK == HAL_UART_Receive(&huart2, (uint8_t *)pData, 2, 1000))
|
||||
// {
|
||||
// HAL_UART_Transmit(&huart2, (uint8_t *)pData, sizeof(pData), 1000);
|
||||
// HAL_Delay(500);
|
||||
// }
|
||||
|
||||
// HAL_Delay(2000);
|
||||
|
||||
App_LineSeek();
|
||||
|
||||
// MOTOR_SetDuty(-20,20);
|
||||
|
||||
// HAL_Delay(300); // 延时300毫秒
|
||||
|
||||
|
|
|
@ -56,6 +56,8 @@
|
|||
|
||||
/* External variables --------------------------------------------------------*/
|
||||
extern TIM_HandleTypeDef htim2;
|
||||
extern TIM_HandleTypeDef htim3;
|
||||
extern TIM_HandleTypeDef htim4;
|
||||
extern DMA_HandleTypeDef hdma_usart2_rx;
|
||||
extern UART_HandleTypeDef huart2;
|
||||
/* USER CODE BEGIN EV */
|
||||
|
@ -228,6 +230,34 @@ void TIM2_IRQHandler(void)
|
|||
/* USER CODE END TIM2_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles TIM3 global interrupt.
|
||||
*/
|
||||
void TIM3_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN TIM3_IRQn 0 */
|
||||
|
||||
/* USER CODE END TIM3_IRQn 0 */
|
||||
HAL_TIM_IRQHandler(&htim3);
|
||||
/* USER CODE BEGIN TIM3_IRQn 1 */
|
||||
|
||||
/* USER CODE END TIM3_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles TIM4 global interrupt.
|
||||
*/
|
||||
void TIM4_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN TIM4_IRQn 0 */
|
||||
|
||||
/* USER CODE END TIM4_IRQn 0 */
|
||||
HAL_TIM_IRQHandler(&htim4);
|
||||
/* USER CODE BEGIN TIM4_IRQn 1 */
|
||||
|
||||
/* USER CODE END TIM4_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles USART2 global interrupt.
|
||||
*/
|
||||
|
|
|
@ -88,7 +88,7 @@ void MX_TIM3_Init(void)
|
|||
htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
htim3.Init.Period = 256 - 1;
|
||||
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
||||
htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
|
||||
htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
|
||||
if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
|
@ -150,7 +150,7 @@ void MX_TIM4_Init(void)
|
|||
htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
htim4.Init.Period = 100;
|
||||
htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
||||
htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
|
||||
htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
|
||||
if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
|
@ -214,6 +214,10 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
|
|||
/* USER CODE END TIM3_MspInit 0 */
|
||||
/* TIM3 clock enable */
|
||||
__HAL_RCC_TIM3_CLK_ENABLE();
|
||||
|
||||
/* TIM3 interrupt Init */
|
||||
HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0);
|
||||
HAL_NVIC_EnableIRQ(TIM3_IRQn);
|
||||
/* USER CODE BEGIN TIM3_MspInit 1 */
|
||||
|
||||
/* USER CODE END TIM3_MspInit 1 */
|
||||
|
@ -225,6 +229,10 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
|
|||
/* USER CODE END TIM4_MspInit 0 */
|
||||
/* TIM4 clock enable */
|
||||
__HAL_RCC_TIM4_CLK_ENABLE();
|
||||
|
||||
/* TIM4 interrupt Init */
|
||||
HAL_NVIC_SetPriority(TIM4_IRQn, 0, 0);
|
||||
HAL_NVIC_EnableIRQ(TIM4_IRQn);
|
||||
/* USER CODE BEGIN TIM4_MspInit 1 */
|
||||
|
||||
/* USER CODE END TIM4_MspInit 1 */
|
||||
|
@ -307,6 +315,9 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
|
|||
/* USER CODE END TIM3_MspDeInit 0 */
|
||||
/* Peripheral clock disable */
|
||||
__HAL_RCC_TIM3_CLK_DISABLE();
|
||||
|
||||
/* TIM3 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(TIM3_IRQn);
|
||||
/* USER CODE BEGIN TIM3_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END TIM3_MspDeInit 1 */
|
||||
|
@ -318,6 +329,9 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
|
|||
/* USER CODE END TIM4_MspDeInit 0 */
|
||||
/* Peripheral clock disable */
|
||||
__HAL_RCC_TIM4_CLK_DISABLE();
|
||||
|
||||
/* TIM4 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(TIM4_IRQn);
|
||||
/* USER CODE BEGIN TIM4_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END TIM4_MspDeInit 1 */
|
||||
|
|
|
@ -135,7 +135,7 @@
|
|||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>CMSIS_AGDI</Key>
|
||||
<Name>-X"" -O206 -S8 -C0 -P00 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F10x_128.FLM -FS08000000 -FL020000 -FP0($$Device:STM32F103C8$Flash\STM32F10x_128.FLM)</Name>
|
||||
<Name>-X"CMSIS-DAP_LU" -ULU_2022_8888 -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F10x_128.FLM -FS08000000 -FL020000 -FP0($$Device:STM32F103C8$Flash\STM32F10x_128.FLM)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
|
|
|
@ -81,7 +81,7 @@
|
|||
</BeforeMake>
|
||||
<AfterMake>
|
||||
<RunUserProg1>0</RunUserProg1>
|
||||
<RunUserProg2>1</RunUserProg2>
|
||||
<RunUserProg2>0</RunUserProg2>
|
||||
<UserProg1Name></UserProg1Name>
|
||||
<UserProg2Name></UserProg2Name>
|
||||
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
|
||||
|
|
3
Makefile
3
Makefile
|
@ -1,5 +1,5 @@
|
|||
##########################################################################################################################
|
||||
# File automatically-generated by tool: [projectgenerator] version: [4.4.0-B60] date: [Sat Jul 13 16:33:46 CST 2024]
|
||||
# File automatically-generated by tool: [projectgenerator] version: [4.4.0-B60] date: [Tue Jul 16 20:05:56 CST 2024]
|
||||
##########################################################################################################################
|
||||
|
||||
# ------------------------------------------------
|
||||
|
@ -68,6 +68,7 @@ User/Src/led.c \
|
|||
User/Src/bluetooth.c \
|
||||
User/Src/control.c \
|
||||
User/Src/line_seek.c \
|
||||
User/Src/path_plan.c \
|
||||
App/Src/app_line_seek.c
|
||||
|
||||
# ASM sources
|
||||
|
|
109
STM32-for-VSCode.config.yaml
Normal file
109
STM32-for-VSCode.config.yaml
Normal file
|
@ -0,0 +1,109 @@
|
|||
# Configuration file for the STM32 for VSCode extension
|
||||
# Arrays can be inputted in two ways. One is: [entry_1, entry_2, ..., entry_final]
|
||||
# or by adding an indented list below the variable name e.g.:
|
||||
# VARIABLE:
|
||||
# - entry_1
|
||||
# - entry_2
|
||||
|
||||
# The project name
|
||||
target: CAR
|
||||
# Can be C or C++
|
||||
language: C
|
||||
|
||||
optimization: Og
|
||||
|
||||
# MCU settings
|
||||
targetMCU: stm32f1x
|
||||
cpu: cortex-m3 # type of cpu e.g. cortex-m4
|
||||
fpu: # Defines how floating points are defined. Can be left empty.
|
||||
floatAbi:
|
||||
ldscript: STM32F103C8Tx_FLASH.ld # linker script
|
||||
|
||||
# Compiler definitions. The -D prefix for the compiler will be automatically added.
|
||||
cDefinitions: []
|
||||
cxxDefinitions: []
|
||||
asDefinitions: []
|
||||
|
||||
# Compiler definition files. you can add a single files or an array of files for different definitions.
|
||||
# The file is expected to have a definition each new line.
|
||||
# This allows to include for example a .definition file which can be ignored in git and can contain
|
||||
# This can be convenient for passing along secrets at compile time, or generating a file for per device setup.
|
||||
cDefinitionsFile:
|
||||
cxxDefinitionsFile:
|
||||
asDefinitionsFile:
|
||||
|
||||
# Compiler flags
|
||||
cFlags:
|
||||
- -Wall
|
||||
- -fdata-sections
|
||||
- -ffunction-sections
|
||||
|
||||
cxxFlags: []
|
||||
assemblyFlags:
|
||||
- -Wall
|
||||
- -fdata-sections
|
||||
- -ffunction-sections
|
||||
|
||||
linkerFlags:
|
||||
- -Wl,--print-memory-usage
|
||||
|
||||
|
||||
# libraries to be included. The -l prefix to the library will be automatically added.
|
||||
libraries:
|
||||
- c
|
||||
- m
|
||||
|
||||
# Library directories. Folders can be added here that contain custom libraries.
|
||||
libraryDirectories: []
|
||||
|
||||
# Files or folders that will be excluded from compilation.
|
||||
# Glob patterns (https://www.wikiwand.com/en/Glob_(programming)) can be used.
|
||||
# Do mind that double stars are reserved in yaml
|
||||
# these should be escaped with a: \ or the name should be in double quotes e.g. "**.test.**"
|
||||
excludes:
|
||||
- "**/Examples/**"
|
||||
- "**/examples/**"
|
||||
- "**/Example/**"
|
||||
- "**/example/**"
|
||||
- "**_template.*"
|
||||
|
||||
|
||||
# Include directories (directories containing .h or .hpp files)
|
||||
# If a CubeMX makefile is present it will automatically include the include directories from that makefile.
|
||||
includeDirectories:
|
||||
- Inc/**
|
||||
- Core/Inc/**
|
||||
- Core/Lib/**
|
||||
- Src/**
|
||||
- Core/Src/**
|
||||
- Core/Lib/**
|
||||
|
||||
|
||||
# Files that should be included in the compilation.
|
||||
# If a CubeMX makefile is present it will automatically include the c and cpp/cxx files from that makefile.
|
||||
# Glob patterns (https://www.wikiwand.com/en/Glob_(programming)) can be used.
|
||||
# Do mind that double stars are reserved in yaml
|
||||
# these should be escaped with a: \ or the name should be in double quotes e.g. "HARDWARE_DRIVER*.c"
|
||||
sourceFiles:
|
||||
- Src/**
|
||||
- Core/Src/**
|
||||
- Core/Lib/**
|
||||
|
||||
|
||||
# When no makefile is present it will show a warning pop-up.
|
||||
# However when compilation without the CubeMX Makefile is desired, this can be turned of.
|
||||
suppressMakefileWarning: false
|
||||
|
||||
# Custom makefile rules
|
||||
# Here custom makefile rules can be added to the STM32Make.make file
|
||||
# an example of how this can be used is commented out below.
|
||||
customMakefileRules:
|
||||
# - command: sayhello
|
||||
# rule: echo "hello"
|
||||
# dependsOn: $(BUILD_DIR)/$(TARGET).elf # can be left out
|
||||
|
||||
# Additional flags which will be used when invoking the make command
|
||||
makeFlags:
|
||||
# - -O # use this option when the output of make is mixed up only works for make version 4.0 and upwards
|
||||
# - --silent # use this option to silence the output of the build
|
||||
|
|
@ -36,12 +36,11 @@ BUILD_DIR = build
|
|||
######################################
|
||||
# C sources
|
||||
C_SOURCES = \
|
||||
App/Src/app_line_seek.c \
|
||||
Core/Src/dma.c \
|
||||
Core/Src/gpio.c \
|
||||
Core/Src/hcsr04.c \
|
||||
Core/Src/i2c.c \
|
||||
Core/Src/led.c \
|
||||
Core/Src/main.c \
|
||||
Core/Src/motor.c \
|
||||
Core/Src/stm32f1xx_hal_msp.c \
|
||||
Core/Src/stm32f1xx_it.c \
|
||||
Core/Src/syscalls.c \
|
||||
|
@ -63,7 +62,14 @@ Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_rcc.c \
|
|||
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_rcc_ex.c \
|
||||
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_tim.c \
|
||||
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_tim_ex.c \
|
||||
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_uart.c
|
||||
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_uart.c \
|
||||
User/Src/bluetooth.c \
|
||||
User/Src/control.c \
|
||||
User/Src/hcsr04.c \
|
||||
User/Src/led.c \
|
||||
User/Src/line_seek.c \
|
||||
User/Src/motor.c \
|
||||
User/Src/path_plan.c
|
||||
|
||||
|
||||
CPP_SOURCES = \
|
||||
|
@ -82,7 +88,7 @@ PREFIX = arm-none-eabi-
|
|||
POSTFIX = "
|
||||
# The gcc compiler bin path can be either defined in make command via GCC_PATH variable (> make GCC_PATH=xxx)
|
||||
# either it can be added to the PATH environment variable.
|
||||
GCC_PATH="E:/PROGRAM FILES/GNU ARM EMBEDDED TOOLCHAIN/10 2021.10/BIN
|
||||
GCC_PATH="E:/PROGRAM FILES (X86)/GNU ARM EMBEDDED TOOLCHAIN/10 2021.10/BIN
|
||||
ifdef GCC_PATH
|
||||
CXX = $(GCC_PATH)/$(PREFIX)g++$(POSTFIX)
|
||||
CC = $(GCC_PATH)/$(PREFIX)gcc$(POSTFIX)
|
||||
|
@ -135,11 +141,13 @@ AS_INCLUDES = \
|
|||
|
||||
# C includes
|
||||
C_INCLUDES = \
|
||||
-IApp/Inc \
|
||||
-ICore/Inc \
|
||||
-IDrivers/CMSIS/Device/ST/STM32F1xx/Include \
|
||||
-IDrivers/CMSIS/Include \
|
||||
-IDrivers/STM32F1xx_HAL_Driver/Inc \
|
||||
-IDrivers/STM32F1xx_HAL_Driver/Inc/Legacy
|
||||
-IDrivers/STM32F1xx_HAL_Driver/Inc/Legacy \
|
||||
-IUser/Inc
|
||||
|
||||
|
||||
|
||||
|
@ -235,13 +243,13 @@ $(BUILD_DIR):
|
|||
# flash
|
||||
#######################################
|
||||
flash: $(BUILD_DIR)/$(TARGET).elf
|
||||
"E:/PROGRAM FILES/OPENOCD-20231002-0.12.0/BIN/OPENOCD.EXE" -f ./openocd.cfg -c "program $(BUILD_DIR)/$(TARGET).elf verify reset exit"
|
||||
"D:/PROGRAM FILES/OPENOCD-20231002-0.12.0/BIN/OPENOCD.EXE" -f ./openocd.cfg -c "program $(BUILD_DIR)/$(TARGET).elf verify reset exit"
|
||||
|
||||
#######################################
|
||||
# erase
|
||||
#######################################
|
||||
erase: $(BUILD_DIR)/$(TARGET).elf
|
||||
"E:/PROGRAM FILES/OPENOCD-20231002-0.12.0/BIN/OPENOCD.EXE" -f ./openocd.cfg -c "init; reset halt; stm32f1x mass_erase 0; exit"
|
||||
"D:/PROGRAM FILES/OPENOCD-20231002-0.12.0/BIN/OPENOCD.EXE" -f ./openocd.cfg -c "init; reset halt; stm32f1x mass_erase 0; exit"
|
||||
|
||||
#######################################
|
||||
# clean up
|
||||
|
|
|
@ -1,7 +1,10 @@
|
|||
#ifndef __LINE_SEEK_H__
|
||||
#define __LINE_SEEK_H__
|
||||
|
||||
// 获取巡线状态
|
||||
void LineSeek_Get(int *p_iL1, int *p_iL2, int *p_iR1, int *p_iR2);
|
||||
extern char LineSeek_Status[5];
|
||||
|
||||
void LineSeek_GetStatus(int *p_iL1, int *p_iL2, int *p_iR1, int *p_iR2);
|
||||
char *LineSeek_GetStatusStr();
|
||||
int LineSeek_Equals(const char *expression);
|
||||
|
||||
#endif
|
|
@ -5,7 +5,7 @@
|
|||
|
||||
#include "main.h"
|
||||
|
||||
void MOTOR_Start(void);
|
||||
void MOTOR_Init(void);
|
||||
void MOTOR_Slip(void);
|
||||
void MOTOR_Stop(void);
|
||||
void MOTOR_SetDuty(int8_t leftDuty, int8_t rightDuty);
|
||||
|
|
29
User/Inc/path_plan.h
Normal file
29
User/Inc/path_plan.h
Normal file
|
@ -0,0 +1,29 @@
|
|||
#ifndef __PATH_PLAN_H
|
||||
#define __PATH_PLAN_H
|
||||
|
||||
// 定义方向枚举
|
||||
typedef enum
|
||||
{
|
||||
STRAIGHT,
|
||||
LEFT,
|
||||
RIGHT,
|
||||
STOP
|
||||
} Direction;
|
||||
|
||||
// 定义路径规划器结构
|
||||
typedef struct
|
||||
{
|
||||
int currentStep;
|
||||
Direction* path;
|
||||
int pathLength;
|
||||
} PathPlanner;
|
||||
|
||||
// 全局路径规划器
|
||||
extern PathPlanner pathPlanner;
|
||||
|
||||
void PathPlanner_Init();
|
||||
Direction GetCurrentDirection();
|
||||
Direction GetNextDirection();
|
||||
void AddPathStep(Direction step);
|
||||
|
||||
#endif
|
|
@ -3,15 +3,42 @@
|
|||
|
||||
#include "main.h"
|
||||
|
||||
char LineSeek_Status[5];
|
||||
|
||||
/**
|
||||
* @brief 获取巡线状态
|
||||
* @param[in] int *p_iL1, int *p_iL2, int *p_iR1, int *p_iR2 四路巡线位指针
|
||||
* @retval void
|
||||
*/
|
||||
void LineSeek_Get(int *p_iL1, int *p_iL2, int *p_iR1, int *p_iR2)
|
||||
void LineSeek_GetStatus(int *p_iL1, int *p_iL2, int *p_iR1, int *p_iR2)
|
||||
{
|
||||
*p_iL1 = HAL_GPIO_ReadPin(LineSeek_L1_GPIO_Port, LineSeek_L1_Pin);
|
||||
*p_iL2 = HAL_GPIO_ReadPin(LineSeek_L2_GPIO_Port, LineSeek_L2_Pin);
|
||||
*p_iR1 = HAL_GPIO_ReadPin(LineSeek_R1_GPIO_Port, LineSeek_R1_Pin);
|
||||
*p_iR2 = HAL_GPIO_ReadPin(LineSeek_R2_GPIO_Port, LineSeek_R2_Pin);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取巡线状态并返回表示状态的字符串
|
||||
* @retval char LineSeek_Status[5]
|
||||
*/
|
||||
char *LineSeek_GetStatusStr()
|
||||
{
|
||||
int iL1, iL2, iR1, iR2;
|
||||
LineSeek_GetStatus(&iL1, &iL2, &iR1, &iR2);
|
||||
sprintf(LineSeek_Status, "%d%d%d%d", iL1, iL2, iR1, iR2);
|
||||
return LineSeek_Status;
|
||||
}
|
||||
|
||||
int LineSeek_Equals(const char *expression)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 4; i++)
|
||||
{
|
||||
if (expression[i] != 'x' && expression[i] != LineSeek_Status[i])
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
|
|
@ -3,6 +3,7 @@
|
|||
#include "tim.h"
|
||||
|
||||
#define MAX_SPEED 100
|
||||
#define MIN_SPEED 5
|
||||
#define MOTOR_L 1
|
||||
#define MOTOR_R 2
|
||||
#define MOTOR_TIM (&htim4)
|
||||
|
@ -11,10 +12,10 @@
|
|||
|
||||
int8_t preProcess(int8_t duty, uint8_t motor);
|
||||
|
||||
void MOTOR_Start(void)
|
||||
void MOTOR_Init(void)
|
||||
{
|
||||
HAL_TIM_PWM_Start(MOTOR_TIM, MOTOR_L_CHAN);
|
||||
HAL_TIM_PWM_Start(MOTOR_TIM, MOTOR_R_CHAN);
|
||||
HAL_TIM_PWM_Start_IT(MOTOR_TIM, MOTOR_L_CHAN);
|
||||
HAL_TIM_PWM_Start_IT(MOTOR_TIM, MOTOR_R_CHAN);
|
||||
}
|
||||
|
||||
/// @brief 自由滑行
|
||||
|
@ -37,30 +38,21 @@ void MOTOR_Stop(void)
|
|||
|
||||
/// @brief 控制电机占空比
|
||||
|
||||
/// @param leftDuty 绝对值 20-100, 支持正负
|
||||
/// @param rightDuty 绝对值 20-100,支持正负
|
||||
/// @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);
|
||||
|
||||
uint16_t leftSpeed = (uint32_t)MAX_SPEED * leftDuty / 100;
|
||||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, leftSpeed);
|
||||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, leftSpeed);
|
||||
|
||||
uint16_t rightSpeed = (uint32_t)MAX_SPEED * rightDuty / 100;
|
||||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, rightSpeed);
|
||||
__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, rightSpeed);
|
||||
__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 (duty > 0)
|
||||
{
|
||||
if (duty > 100)
|
||||
duty = 100;
|
||||
else if (duty < 20)
|
||||
duty = 20;
|
||||
if (motor & MOTOR_L)
|
||||
{
|
||||
HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_SET);
|
||||
|
@ -71,13 +63,11 @@ int8_t preProcess(int8_t duty, uint8_t motor)
|
|||
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
|
||||
else if (duty < 0)
|
||||
{
|
||||
if (duty < -100)
|
||||
duty = -100;
|
||||
else if (duty > -20)
|
||||
duty = -20;
|
||||
duty = -duty;
|
||||
if (motor & MOTOR_L)
|
||||
{
|
||||
HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_RESET);
|
||||
|
@ -89,5 +79,24 @@ int8_t preProcess(int8_t duty, uint8_t motor)
|
|||
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;
|
||||
}
|
||||
|
|
51
User/Src/path_plan.c
Normal file
51
User/Src/path_plan.c
Normal file
|
@ -0,0 +1,51 @@
|
|||
|
||||
#include "path_plan.h"
|
||||
|
||||
#include "stdio.h"
|
||||
|
||||
// 全局路径规划器
|
||||
PathPlanner pathPlanner;
|
||||
|
||||
// 初始化路径规划器
|
||||
void PathPlanner_Init()
|
||||
{
|
||||
pathPlanner.currentStep = -1;
|
||||
pathPlanner.path = NULL;
|
||||
pathPlanner.pathLength = 0;
|
||||
SetPath();
|
||||
}
|
||||
|
||||
// 设置自定义路径
|
||||
void SetPath()
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 100; i++)
|
||||
{
|
||||
AddPathStep(LEFT);
|
||||
AddPathStep(RIGHT);
|
||||
AddPathStep(STRAIGHT);
|
||||
}
|
||||
}
|
||||
|
||||
// 获取当前方向
|
||||
Direction GetCurrentDirection()
|
||||
{
|
||||
return pathPlanner.path[pathPlanner.currentStep];
|
||||
}
|
||||
|
||||
// 获取下一步方向
|
||||
Direction GetNextDirection()
|
||||
{
|
||||
if (pathPlanner.currentStep >= pathPlanner.pathLength)
|
||||
{
|
||||
pathPlanner.currentStep = -1;
|
||||
}
|
||||
return pathPlanner.path[++pathPlanner.currentStep];
|
||||
}
|
||||
|
||||
// 添加路径
|
||||
void AddPathStep(Direction step)
|
||||
{
|
||||
pathPlanner.path = realloc(pathPlanner.path, ++pathPlanner.pathLength * sizeof(Direction));
|
||||
pathPlanner.path[pathPlanner.pathLength - 1] = step;
|
||||
}
|
Loading…
Reference in New Issue
Block a user