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

Final

Browse Source
This commit is contained in:
MADAO
2025-07-08 23:30:16 +08:00
parent a635b6d5eb
commit a9884051f8
21 changed files with 7673 additions and 7958 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
fun/HCBle.c
View File

@@ -136,6 +136,7 @@ void HCBle_ParseAndHandleFrame(const char *frame)
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>
// DriveBOTH(cmd.LeftSpeed,cmd.RightSpeed);
return;
}

81
fun/Motor.c
View File

@@ -64,23 +64,68 @@ void MotorB_Speed(uint8_t speed)
// ʾ<><CABE> ͬʱ<CDAC><CAB1><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD>A/B
// <20><>ʾ <20><><EFBFBD><EFBFBD> Dir 0 ---- ǰ<><C7B0> go forward
// <20><><EFBFBD><EFBFBD> Dir 1 ---- <20><><EFBFBD><EFBFBD> go forward
void DriveBOTH(int16_t speedA,int16_t speedB)
//void DriveBOTH(int16_t speedA,int16_t speedB)
//{
// // <20><><EFBFBD><EFBFBD> A
// if (speedA >= 0) MotorA_Dir(0);
// else
// {
// MotorA_Dir(1);
// speedA = -speedA;
// }
// MotorA_Speed(speedA);
//
// // <20><><EFBFBD><EFBFBD> B
// if (speedB >= 0) MotorB_Dir(0);
// else
// {
// MotorB_Dir(1);
// speedB = -speedB;
// }
// MotorB_Speed(speedB);
//}
// Motor.c <20><><EFBFBD>Ͻ<EFBFBD><CFBD>д<EFBFBD><D0B4><EFBFBD>
void DriveBOTH(int16_t speedA, int16_t speedB)
{
// <20><><EFBFBD><EFBFBD> A
if (speedA >= 0) MotorA_Dir(0);
else
{
MotorA_Dir(1);
speedA = -speedA;
}
MotorA_Speed(speedA);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A
if(speedA == 0) {
// <20>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>͵<EFBFBD>ƽ
HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
}
else if(speedA > 0) {
HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
speedA = abs(speedA); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
}
else {
HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
speedA = abs(speedA); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
}
// <20><><EFBFBD><EFBFBD> B
if (speedB >= 0) MotorA_Dir(0);
else
{
MotorB_Dir(1);
speedB = -speedB;
}
MotorB_Speed(speedB);
}
if(speedB == 0) {
// <20>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>͵<EFBFBD>ƽ
HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
}
else if(speedB > 0) {
HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
speedB = abs(speedB); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
}
else {
HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
speedB = abs(speedB); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
}
// ͬ<><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>B...
// <20><><EFBFBD><EFBFBD>PWM
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, speedA);
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, speedB);
}

1
fun/Ultrasound.c
View File

@@ -123,6 +123,7 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
// ֪ͨ<CDA8><D6AA><EFBFBD><EFBFBD> ȡ<><C8A1>ע<EFBFBD><D7A2><EFBFBD>Ի<EFBFBD><D4BB>Ѳ<EFBFBD><D1B2><EFBFBD><EFBFBD>߳<EFBFBD>
tx_event_flags_set(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR);
}
HAL_TIM_IC_Start_IT(&htim5,TIM_CHANNEL_1);
}
}

1
fun/Ultrasound.h
View File

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

22
fun/encoder.c
View File

@@ -85,8 +85,8 @@ UINT ControlThreadCreate(void)
static void control_thread_entry(ULONG arg)
{
int16_t pwmL = 0, pwmR = 0;
float integralL = 0, integralR = 0;
static int16_t pwmL = 0, pwmR = 0;
static float integralL = 0, integralR = 0;
float Kp = 1.0f, Ki = 0.1f;
while (1)
@@ -96,7 +96,19 @@ static void control_thread_entry(ULONG arg)
integralL += errorL;
integralR += errorR;
// <20>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD>ۼӺ<DBBC><D3BA><EFBFBD><EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD>
if (errorL == 0) integralL = 0;
if (errorR == 0) integralR = 0;
const float max_integral = 1000.0f;
if(integralL > max_integral) integralL = max_integral;
if(integralL < -max_integral) integralL = -max_integral;
if(integralR > max_integral) integralR = max_integral;
if(integralR < -max_integral) integralR = -max_integral;
// ͬ<><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>integralR...
pwmL += (int16_t)(Kp * errorL + Ki * integralL);
pwmR += (int16_t)(Kp * errorR + Ki * integralR);
@@ -111,9 +123,13 @@ static void control_thread_entry(ULONG arg)
}
}
int16_t map_speed_to_rpm(int speed)
{
if (speed < 60) return 0;
if (abs(speed) < 30) return 0;
return (speed - 60) * 1.5; // <20><><EFBFBD><EFBFBD>
}