MADAO/ManGoWalk_STM32
1
0
Fork 0
generated from Template/H563ZI-HAL-CMake-Template
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add Encoder and PID control and Abstacle control

Browse Source
This commit is contained in:
MADAO
2025-07-03 00:29:57 +08:00
parent 0361cd17af
commit a635b6d5eb
49 changed files with 9072 additions and 7610 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
fun/Ultrasound.c
View File

@@ -9,7 +9,7 @@ volatile uint32_t ic_val1 = 0; //
volatile uint32_t ic_val2 = 0; // <20><><EFBFBD><EFBFBD>ֵ2
volatile uint8_t is_first_capture = 0; // <20>Ƿ<EFBFBD>Ϊ<EFBFBD><CEAA>һ<EFBFBD>β<EFBFBD><CEB2><EFBFBD>
volatile uint32_t distance_cm = 0; // <20><><EFBFBD>밴<EFBFBD><EBB0B4> cm<63><6D><EFBFBD><EFBFBD>
volatile uint8_t obstacle_level = 0; // 0 - <20><><EFBFBD>ϰ<EFBFBD> , 1 = Զ ,2 = <20><> , 3 = <20><>
/*******
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Psc: 250 - 1 <20>Դﵽ 1tick = 1us<75><73>Ч<EFBFBD><D0A7>
@@ -64,7 +64,7 @@ void HCSR04_Trigger(void)
#ifdef TEST
void ultrasonic_task_entry(ULONG thread_input) {
HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_2);
HAL_TIM_IC_Start_IT(&htim5, TIM_CHANNEL_1);
DWT_Init();
while (1) {
@@ -74,9 +74,22 @@ void ultrasonic_task_entry(ULONG thread_input) {
if (tx_event_flags_get(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR_CLEAR,
&events, TX_WAIT_FOREVER) == TX_SUCCESS) {
if (distance_cm < 30) {
tx_event_flags_set(&system_events,EVENT_OBSTACLE_DETECTED,TX_OR);// ִ<EFBFBD>б<EFBFBD><EFBFBD>ϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>񶯡<EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>
}
obstacle_level = 3; // <20>ܽ<EFBFBD>
}else if(distance_cm > 30 && distance_cm < 50)
{
obstacle_level = 2; // <20><>
}else if(distance_cm > 50 && distance_cm < 80)
{
obstacle_level = 1;
}else
{
obstacle_level = 0; // <20><><EFBFBD>ϰ<EFBFBD>
}
if(obstacle_level > 0)
{
tx_event_flags_set(&response_events,EVENT_OBSTACLE_DETECTED,TX_OR); // <20><><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD>
}
}
tx_thread_sleep(50); // ÿ<>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 50 ticks 20Hz <20><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>
@@ -94,14 +107,14 @@ Echo
******/
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
if (is_first_capture == 0) {
ic_val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
__HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_FALLING);
ic_val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
__HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
is_first_capture = 1;
} else {
ic_val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
__HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_RISING);
ic_val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
__HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
is_first_capture = 0;
uint32_t delta = (ic_val2 > ic_val1) ? (ic_val2 - ic_val1) : (0xFFFF - ic_val1 + ic_val2);