在gd32f103单片机环境下,无舵机的智能车通过pd算法进行电磁循迹,并使用HC-SR04超声波测距模块进行避障的代码
时间: 2024-04-30 14:19:19 浏览: 104
基于stm32的具有循迹避障的智能车的电路图设计
以下是一份基于GD32F103单片机环境下的无舵机电磁循迹与超声波避障代码,使用PD算法控制电机速度,具体实现细节需要根据具体硬件环境进行适当调整:
```c
#include "gd32f10x.h"
#include "systick.h"
/* 定义电机控制相关的宏 */
#define MOT1_PORT GPIOB
#define MOT1_PWM GPIO_PIN_0
#define MOT1_DIR1 GPIO_PIN_1
#define MOT1_DIR2 GPIO_PIN_2
#define MOT2_PORT GPIOB
#define MOT2_PWM GPIO_PIN_8
#define MOT2_DIR1 GPIO_PIN_9
#define MOT2_DIR2 GPIO_PIN_10
/* 定义红外循迹传感器引脚相关的宏 */
#define IR_PORT GPIOA
#define IR_L1 GPIO_PIN_0
#define IR_L2 GPIO_PIN_1
#define IR_M1 GPIO_PIN_2
#define IR_M2 GPIO_PIN_3
#define IR_R1 GPIO_PIN_4
#define IR_R2 GPIO_PIN_5
/* 定义超声波模块引脚相关的宏 */
#define US_PORT GPIOA
#define US_TRIG GPIO_PIN_6
#define US_ECHO GPIO_PIN_7
/* 定义全局变量 */
float error = 0;
float last_error = 0;
float kp = 0.3;
float kd = 0.2;
int speed = 0;
/* 定义函数原型 */
void motor_control(int left_speed, int right_speed);
void ir_track(void);
void us_measure(void);
int main(void)
{
/* 初始化SysTick */
delay_init(72);
/* 使能GPIOB和GPIOA时钟 */
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_GPIOA);
/* 配置MOT1_PWM、MOT1_DIR1和MOT1_DIR2引脚为推挽输出 */
gpio_init(MOT1_PORT, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, MOT1_PWM | MOT1_DIR1 | MOT1_DIR2);
/* 配置MOT2_PWM、MOT2_DIR1和MOT2_DIR2引脚为推挽输出 */
gpio_init(MOT2_PORT, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, MOT2_PWM | MOT2_DIR1 | MOT2_DIR2);
/* 配置IR_L1、IR_L2、IR_M1、IR_M2、IR_R1和IR_R2引脚为浮空输入 */
gpio_init(IR_PORT, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, IR_L1 | IR_L2 | IR_M1 | IR_M2 | IR_R1 | IR_R2);
/* 配置US_TRIG引脚为推挽输出 */
gpio_init(US_PORT, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, US_TRIG);
/* 配置US_ECHO引脚为浮空输入 */
gpio_init(US_PORT, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, US_ECHO);
/* 设置SysTick的中断周期为10ms */
SysTick_Config(SystemCoreClock / 100);
while(1)
{
/* 电磁循迹 */
ir_track();
/* 超声波避障 */
us_measure();
/* 根据PD算法计算电机速度 */
speed = kp * error + kd * (error - last_error);
last_error = error;
/* 控制电机速度 */
motor_control(speed, speed);
}
}
/* 电机控制函数 */
void motor_control(int left_speed, int right_speed)
{
/* 控制左电机 */
if(left_speed > 0)
{
/* 向前 */
gpio_bit_set(MOT1_PORT, MOT1_DIR1);
gpio_bit_reset(MOT1_PORT, MOT1_DIR2);
pwm_output_state_set(PWM1, ENABLE);
timer_channel_output_pulse_value_config(PWM1, TIMER_CH_0, left_speed);
}
else if(left_speed < 0)
{
/* 向后 */
gpio_bit_reset(MOT1_PORT, MOT1_DIR1);
gpio_bit_set(MOT1_PORT, MOT1_DIR2);
pwm_output_state_set(PWM1, ENABLE);
timer_channel_output_pulse_value_config(PWM1, TIMER_CH_0, -left_speed);
}
else
{
/* 停止 */
gpio_bit_reset(MOT1_PORT, MOT1_DIR1);
gpio_bit_reset(MOT1_PORT, MOT1_DIR2);
pwm_output_state_set(PWM1, DISABLE);
}
/* 控制右电机 */
if(right_speed > 0)
{
/* 向前 */
gpio_bit_set(MOT2_PORT, MOT2_DIR1);
gpio_bit_reset(MOT2_PORT, MOT2_DIR2);
pwm_output_state_set(PWM0, ENABLE);
timer_channel_output_pulse_value_config(PWM0, TIMER_CH_2, right_speed);
}
else if(right_speed < 0)
{
/* 向后 */
gpio_bit_reset(MOT2_PORT, MOT2_DIR1);
gpio_bit_set(MOT2_PORT, MOT2_DIR2);
pwm_output_state_set(PWM0, ENABLE);
timer_channel_output_pulse_value_config(PWM0, TIMER_CH_2, -right_speed);
}
else
{
/* 停止 */
gpio_bit_reset(MOT2_PORT, MOT2_DIR1);
gpio_bit_reset(MOT2_PORT, MOT2_DIR2);
pwm_output_state_set(PWM0, DISABLE);
}
}
/* 电磁循迹函数 */
void ir_track(void)
{
uint16_t ir_l1 = gpio_input_bit_get(IR_PORT, IR_L1);
uint16_t ir_l2 = gpio_input_bit_get(IR_PORT, IR_L2);
uint16_t ir_m1 = gpio_input_bit_get(IR_PORT, IR_M1);
uint16_t ir_m2 = gpio_input_bit_get(IR_PORT, IR_M2);
uint16_t ir_r1 = gpio_input_bit_get(IR_PORT, IR_R1);
uint16_t ir_r2 = gpio_input_bit_get(IR_PORT, IR_R2);
if(ir_l1 == 0 && ir_l2 == 0 && ir_m1 == 0 && ir_m2 == 0 && ir_r1 == 0 && ir_r2 == 0)
{
error = 0;
}
else if(ir_l1 == 1 && ir_l2 == 0 && ir_m1 == 0 && ir_m2 == 0 && ir_r1 == 0 && ir_r2 == 0)
{
error = -3;
}
else if(ir_l1 == 1 && ir_l2 == 1 && ir_m1 == 0 && ir_m2 == 0 && ir_r1 == 0 && ir_r2 == 0)
{
error = -2;
}
else if(ir_l1 == 0 && ir_l2 == 1 && ir_m1 == 0 && ir_m2 == 0 && ir_r1 == 0 && ir_r2 == 0)
{
error = -1;
}
else if(ir_l1 == 0 && ir_l2 == 1 && ir_m1 == 1 && ir_m2 == 0 && ir_r1 == 0 && ir_r2 == 0)
{
error = -0.5;
}
else if(ir_l1 == 0 && ir_l2 == 0 && ir_m1 == 1 && ir_m2 == 0 && ir_r1 == 0 && ir_r2 == 0)
{
error = 0;
}
else if(ir_l1 == 0 && ir_l2 == 0 && ir_m1 == 1 && ir_m2 == 1 && ir_r1 == 0 && ir_r2 == 0)
{
error = 0.5;
}
else if(ir_l1 == 0 && ir_l2 == 0 && ir_m1 == 0 && ir_m2 == 1 && ir_r1 == 0 && ir_r2 == 0)
{
error = 1;
}
else if(ir_l1 == 0 && ir_l2 == 0 && ir_m1 == 0 && ir_m2 == 1 && ir_r1 == 1 && ir_r2 == 0)
{
error = 2;
}
else if(ir_l1 == 0 && ir_l2 == 0 && ir_m1 == 0 && ir_m2 == 0 && ir_r1 == 1 && ir_r2 == 0)
{
error = 3;
}
else if(ir_l1 == 0 && ir_l2 == 0 && ir_m1 == 0 && ir_m2 == 0 && ir_r1 == 1 && ir_r2 == 1)
{
error = 4;
}
}
/* 超声波测距函数 */
void us_measure(void)
{
uint32_t start_time = 0;
uint32_t end_time = 0;
uint32_t distance = 0;
/* 发送超声波信号 */
gpio_bit_set(US_PORT, US_TRIG);
delay_us(10);
gpio_bit_reset(US_PORT, US_TRIG);
/* 等待收到回波信号 */
while(!gpio_input_bit_get(US_PORT, US_ECHO));
start_time = systick_get_value();
while(gpio_input_bit_get(US_PORT, US_ECHO));
end_time = systick_get_value();
/* 计算距离 */
distance = (end_time - start_time) / 58;
/* 超声波避障 */
if(distance < 10)
{
/* 靠近障碍物,需要向右转 */
error += 2;
}
}
```
需要注意的是,以上代码中只是一个简单的示例,实际的应用场景中需要根据具体情况进行调整和优化,例如增加PID算法等更高级的控制算法,以达到更好的效果。
阅读全文