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

6
fun/HCBle.c
View File

@@ -128,8 +128,14 @@ void HCBle_ParseAndHandleFrame(const char *frame)
if (sscanf(frame, "#{\"leftSpeed\":%d,\"rightSpeed\":%d}$", &left, &right) == 2) {
cmd.LeftSpeed = left;
cmd.RightSpeed = right;
// <20><><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD>
target_rpm_L = map_speed_to_rpm(cmd.LeftSpeed);
target_rpm_R = map_speed_to_rpm(cmd.RightSpeed);
HCBle_SendData("left=%d, right=%d\r\n", cmd.LeftSpeed, cmd.RightSpeed);
// HCBle_SendData("left=%d, right=%d\r\n", left, right);
DriveBOTH(cmd.LeftSpeed,cmd.RightSpeed); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
return;
}

2
fun/Motor.c
View File

@@ -82,5 +82,5 @@ void DriveBOTH(int16_t speedA,int16_t speedB)
MotorB_Dir(1);
speedB = -speedB;
}
MotorA_Speed(speedB);
MotorB_Speed(speedB);
}

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);

2
fun/Ultrasound.h
View File

@@ -6,7 +6,7 @@
#define HCSR_TEST 1
#define EVENT_ECHO_DONE 0x01
extern volatile uint8_t obstacle_level;
void DWT_Init(void);
/********
delay_us() <20><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5> (ʹ<><CAB9>DWT) <20><>ȷ<EFBFBD><C8B7>

119
fun/encoder.c Normal file
View File

@@ -0,0 +1,119 @@
#include "encoder.h"
volatile int32_t tick_L = 0,tick_R = 0;
volatile int16_t rpm_L = 0,rpm_R = 0;
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim8;
static TX_THREAD enc_thread;
static UCHAR enc_stack[512];
static void enc_thread_entry(ULONG);
UINT Encoder_ThreadCreate(void)
{
UINT status;
HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);
__HAL_TIM_SET_COUNTER(&htim1,0);
__HAL_TIM_SET_COUNTER(&htim8,0);
status = tx_thread_create(&enc_thread, "Encoder", enc_thread_entry, 0,
enc_stack, sizeof(enc_stack),
6, 6, TX_NO_TIME_SLICE, TX_AUTO_START);
return status;
}
static void enc_thread_entry(ULONG arg)
{
const float msPerMin = 6000.0f;
const float ticksPerRev = 1024.0f; // <20><><EFBFBD><EFBFBD> * 4
uint16_t lastL = 0, lastR = 0;
while (1)
{
uint16_t cntL = __HAL_TIM_GET_COUNTER(&htim1);
uint16_t cntR = __HAL_TIM_GET_COUNTER(&htim8);
int16_t dL = (int16_t)(cntL - lastL);
int16_t dR = (int16_t)(cntR - lastR);
lastL = cntL;
lastR = cntR;
tick_L += dL;
tick_R += dR;
rpm_L = (int16_t)(dL * msPerMin / (ticksPerRev * 10));
rpm_R = (int16_t)(dR * msPerMin / (ticksPerRev * 10));
tx_thread_sleep(10);
}
}
//
//ControlThread
//
static TX_THREAD control_thread;
static UCHAR control_stack[512];
volatile int16_t target_rpm_L = 0;
volatile int16_t target_rpm_R = 0;
static void control_thread_entry(ULONG arg);
UINT ControlThreadCreate(void)
{
UINT status;
status = tx_thread_create(&control_thread, "Control", control_thread_entry, 0,
control_stack, sizeof(control_stack),
7, 7, TX_NO_TIME_SLICE, TX_AUTO_START);
return status;
}
static void control_thread_entry(ULONG arg)
{
int16_t pwmL = 0, pwmR = 0;
float integralL = 0, integralR = 0;
float Kp = 1.0f, Ki = 0.1f;
while (1)
{
int16_t errorL = target_rpm_L - rpm_L;
int16_t errorR = target_rpm_R - rpm_R;
integralL += errorL;
integralR += errorR;
pwmL += (int16_t)(Kp * errorL + Ki * integralL);
pwmR += (int16_t)(Kp * errorR + Ki * integralR);
if (pwmL < 0) pwmL = 0;
if (pwmL > 255) pwmL = 255;
if (pwmR < 0) pwmR = 0;
if (pwmR > 255) pwmR = 255;
DriveBOTH(pwmL, pwmR);
tx_thread_sleep(10);
}
}
int16_t map_speed_to_rpm(int speed)
{
if (speed < 60) return 0;
return (speed - 60) * 1.5; // <20><><EFBFBD><EFBFBD>
}

12
fun/encoder.h Normal file
View File

@@ -0,0 +1,12 @@
#ifndef __ENCODER_H__
#define __ENCODER_H__
#include "headfile.h"
extern volatile int16_t target_rpm_L;
extern volatile int16_t target_rpm_R;
int16_t map_speed_to_rpm(int speed);
UINT ControlThreadCreate(void);
UINT Encoder_ThreadCreate(void);
#endif

2
fun/headfile.h
View File

@@ -23,6 +23,8 @@
#include "Motor.h"
#include "IMU.h"
#include "imu948.h"
#include "encoder.h"
#include "value.h" // ȫ<>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>