#include "stm32f10x.h" #include "Motor.h" #include "PWM.h" int16_t IRSensor_Count; int8_t Leftspeed; int8_t Rightspeed; void IRSensor_Init(void) { RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU ; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1| GPIO_Pin_2|GPIO_Pin_3; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource1); GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource2); GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource3); EXTI_InitTypeDef EXTI_InitStructure; //定义结构体变量 EXTI_InitStructure.EXTI_Line = EXTI_Line1 | EXTI_Line2|EXTI_Line3 ; //选择配置外部中断的0号线和1号线 EXTI_InitStructure.EXTI_LineCmd = ENABLE; //指定外部中断线使能 EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; //指定外部中断线为中断模式 EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling; //指定外部中断线为下降沿触发 EXTI_Init(&EXTI_InitStructure); //将结构体变量交给EXTI_Init，配置EXTI外设 /*NVIC中断分组*/ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_3); //配置NVIC为分组2 //即抢占优先级范围：0~3，响应优先级范围：0~3 /*NVIC配置*/ NVIC_InitTypeDef NVIC_InitStructure; //定义结构体变量 NVIC_InitStructure.NVIC_IRQChannel = EXTI1_IRQn; //选择配置NVIC的EXTI0线 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //指定NVIC线路使能 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; //指定NVIC线路的抢占优先级为1 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //指定NVIC线路的响应优先级为1 NVIC_Init(&NVIC_InitStructure); //将结构体变量交给NVIC_Init，配置NVIC外设 NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn; //选择配置NVIC的EXTI1线 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //指定NVIC线路使能 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; //指定NVIC线路的抢占优先级为1 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; //指定NVIC线路的响应优先级为2 NVIC_Init(&NVIC_InitStructure);问

#include “stm32f10x.h” #include “delay.h” #include “motor.h” #include “keysacn.h” #include “IRSEARCH.h” #include “IRAvoid.h” /* void ZYSTM32_run(int speed,int time); //前进函数 void ZYSTM32_brake(int time); //刹车函数 void ZYSTM32_Left(int speed,int time); //左转函数 void ZYSTM32_Spin_Left(int speed,int time); //左旋转函数 void ZYSTM32_Right(int speed,int time); //右转函数 void ZYSTM32_Spin_Right(int speed,int time);//右旋转函数 void ZYSTM32_back(int speed,int time); //后退函数 / int main(void) { delay_init(); KEY_Init(); IRSearchInit(); IRAvoidInit(); TIM4_PWM_Init(7199,0); //初始化PWM ZYSTM32_brake(500); keysacn(); while(1) { if(AVOID_IO == BARRIER_Y ) { BEEP_SET; LED_D3_SET; ZYSTM32_back(50,200); ZYSTM32_Spin_Left(50,350); delay_ms(300); } else { BEEP_RESET; LED_D3_RESET; SearchRun(); //循迹函数 } } } #include “IRSEARCH.h” #include “delay.h” #include “motor.h” #include “stm32f10x.h” // Device header char ctrl_comm = COMM_STOP;//控制指令 char ctrl_comm_last = COMM_STOP;//上一次的指令 void IRSearchInit(void) { GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB , ENABLE); GPIO_InitStructure.GPIO_Pin = SEARCH_R_PIN;//配置使能GPIO管脚 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;//配置GPIO模式,输入上拉 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//配置GPIO端口速度 GPIO_Init(SEARCH_R_GPIO , &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = SEARCH_L_PIN;//配置使能GPIO管脚 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;//配置GPIO模式,输入上拉 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//配置GPIO端口速度 GPIO_Init(SEARCH_L_GPIO , &GPIO_InitStructure); } void SearchRun(void) { if(SEARCH_L_IO == WHITE_AREA && SEARCH_R_IO == WHITE_AREA) ctrl_comm = COMM_UP; else if (SEARCH_L_IO == BLACK_AREA && SEARCH_R_IO == WHITE_AREA) ctrl_comm = COMM_RIGHT; else if (SEARCH_R_IO == BLACK_AREA & SEARCH_L_IO == WHITE_AREA) ctrl_comm = COMM_LEFT; else ctrl_comm = COMM_DOWN; if(ctrl_comm_last != ctrl_comm) { ctrl_comm_last = ctrl_comm; switch(ctrl_comm) { case COMM_UP: ZYSTM32_run(80,6);break; case COMM_DOWN: ZYSTM32_back(50,100);ZYSTM32_Spin_Left(70,400);break; case COMM_LEFT: ZYSTM32_back(50,100);ZYSTM32_Spin_Left(70,400);break; case COMM_RIGHT: ZYSTM32_back(50,100);ZYSTM32_Spin_Right(70,250);break; case COMM_STOP: ZYSTM32_brake(10);break; default : break; } } } #include “IRAvoid.h” #include “delay.h” #include “motor.h” #include “keysacn.h” #include “stm32f10x.h” // Device header void IRAvoidInit(void) { GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE); GPIO_InitStructure.GPIO_Pin = AVOID_PIN;//配置使能GPIO管脚 PB1 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;//配置GPIO模式,输入上拉 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//配置GPIO端口速度 GPIO_Init(AVOID_PIN_GPIO , &GPIO_InitStructure); } void AVoidRun(void) { if(AVOID_IO == BARRIER_N ) { ZYSTM32_run(50,10); BEEP_RESET; LED_D3_RESET; } else { BEEP_SET; LED_D3_SET; ZYSTM32_brake(300);//停止300MS ZYSTM32_back(50,400);//后退400MS ZYSTM32_Left(50,500);//左转500MS } } #ifndef _IRAVOID_H #define _IRAVOID_H #include “stm32f10x.h” // Device header void IRAvoidInit(void); void AVoidRun(void); //避障传感器 / 避障传感器 AVOID_PIN PB1 / #define AVOID_PIN GPIO_Pin_1 #define AVOID_PIN_GPIO GPIOB #define AVOID_IO GPIO_ReadInputDataBit(AVOID_PIN_GPIO, AVOID_PIN) #define BARRIER_Y 0 //有障碍物 #define BARRIER_N 1 //无障碍物 #endif #include “motor.h” #include “Math.h” #include “delay.h” #include “stm32f10x.h” // Device header signed short sPWMR,sPWML,dPWM; //GPIO配置函数 /void MotorGPIO_Configuration(void) { GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB, ENABLE); //使能PA,PB端口时钟 GPIO_InitStructure.GPIO_Pin = LEFT_MOTOR_GO; //左电机方向控制 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(LEFT_MOTOR_GO_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = LEFT_MOTOR_PWM; GPIO_Init(LEFT_MOTOR_PWM_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = RIGHT_MOTOR_PWM; GPIO_Init(RIGHT_MOTOR_PWM_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = RIGHT_MOTOR_GO; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(RIGHT_MOTOR_GPIO, &GPIO_InitStructure); } void run() //前进 { RIGHT_MOTOR_GO_SET; RIGHT_MOTOR_PWM_RESET;//PB9 LEFT_MOTOR_GO_SET; LEFT_MOTOR_PWM_RESET;//PB8 } / void TIM4_PWM_Init(unsigned short arr,unsigned short psc) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);// RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB , ENABLE); //使能GPIO外设时钟使能 GPIO_InitStructure.GPIO_Pin = LEFT_MOTOR_GO; //左电机方向控制 PB7 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(LEFT_MOTOR_GO_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = LEFT_MOTOR_PWM; //左电机PWM控制 PB8 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出 GPIO_Init(LEFT_MOTOR_PWM_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = RIGHT_MOTOR_GO; //右电机方向控制 PA4 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(RIGHT_MOTOR_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = RIGHT_MOTOR_PWM; //右电机PWM控制 PB9 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出 GPIO_Init(RIGHT_MOTOR_PWM_GPIO, &GPIO_InitStructure); TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值 80K TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值不分频 TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式 TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2 TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能 TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值 TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高 TIM_OC3Init(TIM4, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx TIM_OC4Init(TIM4, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx TIM_CtrlPWMOutputs(TIM4,ENABLE); //MOE 主输出使能 TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable); //CH1预装载使能 TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable); //CH1预装载使能 TIM_ARRPreloadConfig(TIM4, ENABLE); //使能TIMx在ARR上的预装载寄存器 TIM_Cmd(TIM4, ENABLE); //使能TIM1 } void SetMotorSpeed(unsigned char ucChannel,signed char cSpeed) { // static short sMotorSpeed = 0; short sPWM; // float fDir = 1; if (cSpeed>=100) cSpeed = 100; if (cSpeed<=-100) cSpeed = -100; sPWM = 7201 - fabs(cSpeed)72; switch(ucChannel) { case 0://右轮 TIM_SetCompare3(TIM4,sPWM); if (cSpeed>0) RIGHT_MOTOR_GO_RESET; else if(cSpeed<0) RIGHT_MOTOR_GO_SET; break; case 1://左轮 TIM_SetCompare4(TIM4,sPWM); if (cSpeed>0) LEFT_MOTOR_GO_SET; else if (cSpeed<0) LEFT_MOTOR_GO_RESET; break; } } //----------------------------------运动函数-------------------------------- void ZYSTM32_run(signed char speed,int time) //前进函数 { signed char f_speed = - speed; SetMotorSpeed(1,f_speed);//左轮 //为负数 SetMotorSpeed(0,speed);//右轮 //为正数 delay_ms(time); //时间为毫秒 } void ZYSTM32_brake(int time) //刹车函数 { SetMotorSpeed(1,0);//左轮 //为0 SetMotorSpeed(0,0);//右轮 //为0 RIGHT_MOTOR_GO_RESET; LEFT_MOTOR_GO_RESET; delay_ms(time); //时间为毫秒 } void ZYSTM32_Left(signed char speed,int time) //左转函数 { SetMotorSpeed(1,0);//左轮 //左轮不动 SetMotorSpeed(0,speed); //右轮为正 delay_ms(time); //时间为毫秒 } void ZYSTM32_Spin_Left(signed char speed,int time) //左旋转函数 { SetMotorSpeed(1,speed);//左轮 //左轮为正 SetMotorSpeed(0,speed); //右轮为正 delay_ms(time); //时间为毫秒 } void ZYSTM32_Right(signed char speed,int time) //右转函数 { signed char f_speed = - speed; SetMotorSpeed(1,f_speed);//左轮 //左轮为负 SetMotorSpeed(0,0); //右轮为0 delay_ms(time); //时间为毫秒 } void ZYSTM32_Spin_Right(signed char speed,int time) //右旋转函数 { signed char f_speed = - speed; SetMotorSpeed(1,f_speed);//左轮 //左轮为负 SetMotorSpeed(0,f_speed); //右轮为负 delay_ms(time); //时间为毫秒 } void ZYSTM32_back(signed char speed,int time) //后退函数 { signed char u_speed = 100- speed; signed char f_speed = - u_speed; SetMotorSpeed(1,u_speed);//左轮 //为正数 SetMotorSpeed(0,f_speed);//右轮 //为负数 delay_ms(time); //时间为毫秒 } #include “delay.h” ////////////////////////////////////////////////////////////////////////////////// //如果需要使用OS,则包括下面的头文件即可. #if SYSTEM_SUPPORT_OS #include “includes.h” //ucos 使用 #endif ////////////////////////////////////////////////////////////////////////////////// //本程序只供学习使用，未经作者许可，不得用于其它任何用途 //ZYSTM32-A0机器人主控板 //使用SysTick的普通计数模式对延迟进行管理（适合STM32F10x系列） //包括delay_us,delay_ms //智宇电子 //www.hnzhiyu.cn //创建日期:2017/4/14 //版本：V1.8 //All rights reserved //******************************************************************************** ////////////////////////////////////////////////////////////////////////////////// static u8 fac_us=0; //us延时倍乘数 static u16 fac_ms=0; //ms延时倍乘数,在ucos下,代表每个节拍的ms数 #if SYSTEM_SUPPORT_OS //如果SYSTEM_SUPPORT_OS定义了,说明要支持OS了(不限于UCOS). //当delay_us/delay_ms需要支持OS的时候需要三个与OS相关的宏定义和函数来支持 //首先是3个宏定义: // delay_osrunning:用于表示OS当前是否正在运行,以决定是否可以使用相关函数 //delay_ostickspersec:用于表示OS设定的时钟节拍,delay_init将根据这个参数来初始哈systick // delay_osintnesting:用于表示OS中断嵌套级别,因为中断里面不可以调度,delay_ms使用该参数来决定如何运行 //然后是3个函数: // delay_osschedlock:用于锁定OS任务调度,禁止调度 //delay_osschedunlock:用于解锁OS任务调度,重新开启调度 // delay_ostimedly:用于OS延时,可以引起任务调度. //本例程仅作UCOSII和UCOSIII的支持,其他OS,请自行参考着移植 //支持UCOSII #ifdef OS_CRITICAL_METHOD //OS_CRITICAL_METHOD定义了,说明要支持UCOSII #define delay_osrunning OSRunning //OS是否运行标记,0,不运行;1,在运行 #define delay_ostickspersec OS_TICKS_PER_SEC //OS时钟节拍,即每秒调度次数 #define delay_osintnesting OSIntNesting //中断嵌套级别,即中断嵌套次数 #endif //支持UCOSIII #ifdef CPU_CFG_CRITICAL_METHOD //CPU_CFG_CRITICAL_METHOD定义了,说明要支持UCOSIII #define delay_osrunning OSRunning //OS是否运行标记,0,不运行;1,在运行 #define delay_ostickspersec OSCfg_TickRate_Hz //OS时钟节拍,即每秒调度次数 #define delay_osintnesting OSIntNestingCtr //中断嵌套级别,即中断嵌套次数 #endif //us级延时时,关闭任务调度(防止打断us级延迟) void delay_osschedlock(void) { #ifdef CPU_CFG_CRITICAL_METHOD //使用UCOSIII OS_ERR err; OSSchedLock(&err); //UCOSIII的方式,禁止调度，防止打断us延时 #else //否则UCOSII OSSchedLock(); //UCOSII的方式,禁止调度，防止打断us延时 #endif } //us级延时时,恢复任务调度 void delay_osschedunlock(void) { #ifdef CPU_CFG_CRITICAL_METHOD //使用UCOSIII OS_ERR err; OSSchedUnlock(&err); //UCOSIII的方式,恢复调度 #else //否则UCOSII OSSchedUnlock(); //UCOSII的方式,恢复调度 #endif } //调用OS自带的延时函数延时 //ticks:延时的节拍数 void delay_ostimedly(u32 ticks) { #ifdef CPU_CFG_CRITICAL_METHOD OS_ERR err; OSTimeDly(ticks,OS_OPT_TIME_PERIODIC,&err); //UCOSIII延时采用周期模式 #else OSTimeDly(ticks); //UCOSII延时 #endif } //systick中断服务函数,使用ucos时用到 void SysTick_Handler(void) { if(delay_osrunning==1) //OS开始跑了,才执行正常的调度处理 { OSIntEnter(); //进入中断 OSTimeTick(); //调用ucos的时钟服务程序 OSIntExit(); //触发任务切换软中断 } } #endif //初始化延迟函数 //当使用OS的时候,此函数会初始化OS的时钟节拍 //SYSTICK的时钟固定为HCLK时钟的1/8 //SYSCLK:系统时钟 void delay_init() { #if SYSTEM_SUPPORT_OS //如果需要支持OS. u32 reload; #endif SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8); //选择外部时钟 HCLK/8 fac_us=SystemCoreClock/8000000; //为系统时钟的1/8 #if SYSTEM_SUPPORT_OS //如果需要支持OS. reload=SystemCoreClock/8000000; //每秒钟的计数次数单位为M reload=1000000/delay_ostickspersec; //根据delay_ostickspersec设定溢出时间 //reload为24位寄存器,最大值:16777216,在72M下,约合1.86s左右 fac_ms=1000/delay_ostickspersec; //代表OS可以延时的最少单位 SysTick->CTRL|=SysTick_CTRL_TICKINT_Msk; //开启SYSTICK中断 SysTick->LOAD=reload; //每1/delay_ostickspersec秒中断一次 SysTick->CTRL|=SysTick_CTRL_ENABLE_Msk; //开启SYSTICK #else fac_ms=(u16)fac_us1000; //非OS下,代表每个ms需要的systick时钟数 #endif } #if SYSTEM_SUPPORT_OS //如果需要支持OS. //延时nus //nus为要延时的us数. void delay_us(u32 nus) { u32 ticks; u32 told,tnow,tcnt=0; u32 reload=SysTick->LOAD; //LOAD的值 ticks=nusfac_us; //需要的节拍数 tcnt=0; delay_osschedlock(); //阻止OS调度，防止打断us延时 told=SysTick->VAL; //刚进入时的计数器值 while(1) { tnow=SysTick->VAL; if(tnow!=told) { if(tnow<told)tcnt+=told-tnow; //这里注意一下SYSTICK是一个递减的计数器就可以了. else tcnt+=reload-tnow+told; told=tnow; if(tcnt>=ticks)break; //时间超过/等于要延迟的时间,则退出. } }; delay_osschedunlock(); //恢复OS调度 } //延时nms //nms:要延时的ms数 void delay_ms(u16 nms) { if(delay_osrunning&&delay_osintnesting==0) //如果OS已经在跑了,并且不是在中断里面(中断里面不能任务调度) { if(nms>=fac_ms) //延时的时间大于OS的最少时间周期 { delay_ostimedly(nms/fac_ms); //OS延时 } nms%=fac_ms; //OS已经无法提供这么小的延时了,采用普通方式延时 } delay_us((u32)(nms1000)); //普通方式延时 } #else //不用OS时 //延时nus //nus为要延时的us数. void delay_us(u32 nus) { u32 temp; SysTick->LOAD=nusfac_us; //时间加载 SysTick->VAL=0x00; //清空计数器 SysTick->CTRL|=SysTick_CTRL_ENABLE_Msk ; //开始倒数 do { temp=SysTick->CTRL; }while((temp&0x01)&&!(temp&(1<<16))); //等待时间到达 SysTick->CTRL&=~SysTick_CTRL_ENABLE_Msk; //关闭计数器 SysTick->VAL =0X00; //清空计数器 } //延时nms //注意nms的范围 //SysTick->LOAD为24位寄存器,所以,最大延时为: //nms<=0xffffff81000/SYSCLK //SYSCLK单位为Hz,nms单位为ms //对72M条件下,nms<=1864 void delay_ms(u16 nms) { u32 temp; SysTick->LOAD=(u32)nmsfac_ms; //时间加载(SysTick->LOAD为24bit) SysTick->VAL =0x00; //清空计数器 SysTick->CTRL|=SysTick_CTRL_ENABLE_Msk ; //开始倒数 do { temp=SysTick->CTRL; }while((temp&0x01)&&!(temp&(1<<16))); //等待时间到达 SysTick->CTRL&=~SysTick_CTRL_ENABLE_Msk; //关闭计数器 SysTick->VAL =0X00; //清空计数器 } #endif 现在的代码不能使小车在跑道内（黑线内）躲避障碍物并且在不越过黑线的基础上通过跑道，会出现一直旋转的情况，而且会把地缝识别成黑线，而且受环境光影响，请说明应该如何修改，指出修改位置

void ZYSTM32_Right(signed char speed,int time) //右转函数 { signed char f_speed = - speed; SetMotorSpeed(1,f_speed);//左轮 //左轮为负（后退？） SetMotorSpeed(0,0); //右轮为0（不动） delay_ms...

#include "stm32f10x.h" // Device header #include "OLED.h" #include "Delay.h" #include "math.h" // 舵机参数配置 #define PWM_PERIOD 20000 // 20ms周期(50Hz) #define PWM_MIN_PULSE 500 // 0.5ms脉宽(0°) #define PWM_MAX_PULSE 2500 // 2.5ms脉宽(180°) // 云台舵机定义 typedef enum { PITCH_AXIS, // 俯仰轴 YAW_AXIS // 偏航轴 } AxisType; // 云台舵机结构体 typedef struct { TIM_TypeDef* TIMx; uint16_t TIM_Channel; GPIO_TypeDef* GPIOx; uint16_t GPIO_Pin; float current_angle; float min_angle; float max_angle; uint16_t min_pulse; uint16_t max_pulse; } ServoMotor; // 云台控制器 typedef struct { ServoMotor pitch; // 俯仰舵机 ServoMotor yaw; // 偏航舵机 uint8_t is_smooth_enabled; } ServoTurret; // 全局云台实例 ServoTurret g_turret; // PWM初始化 void PWM_Init(ServoMotor* motor); // 设置舵机脉宽 void Servo_SetPulse(ServoMotor* motor, uint16_t pulse); // 设置舵机角度 (直接设置) void Servo_SetAngle(ServoMotor* motor, float angle); // 云台初始化 void Turret_Init(void); // 设置云台角度 (带平滑移动) void Turret_SetAngle(float pitch_angle, float yaw_angle, uint16_t duration_ms); // 舵机校准函数 void Servo_Calibrate(ServoMotor* motor, float min_deg, float max_deg, uint16_t min_us, uint16_t max_us); int main() { OLED_Init(); Turret_Init(); while (1) { // 平滑移动到(45°, 135°)，耗时1秒 Turret_SetAngle(45, 135, 1000); // 直接设置到(90°, 90°) Servo_SetAngle(&g_turret.pitch, 90); Servo_SetAngle(&g_turret.yaw, 90); Delay_ms(2000); // 校准俯仰轴范围 (实际0°对应600μs, 180°对应2400μs) Servo_Calibrate(&g_turret.pitch, 0, 180, 600, 2400); } } // PWM初始化 void PWM_Init(ServoMotor* motor) { // 1. 使能时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); if (motor->TIMx == TIM1) { RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); } else { RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 + (uint32_t)(motor->TIMx - TIM2), ENABLE); } RCC_APB2PeriphClockCmd((uint32_t)1 << (((uint32_t)motor->GPIOx - GPIOA_BASE) / 0x400), ENABLE); // 2. GPIO配置 GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = motor->GPIO_Pin; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(motor->GPIOx, &GPIO_InitStructure); // 3. 定时器配置 TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_TimeBaseStructure.TIM_Period = PWM_PERIOD - 1; TIM_TimeBaseStructure.TIM_Prescaler = SystemCoreClock / 1000000 - 1; // 1MHz时钟 TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(motor->TIMx, &TIM_TimeBaseStructure); // 4. PWM通道配置 TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = motor->min_pulse; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; switch(motor->TIM_Channel) { case TIM_Channel_1: TIM_OC1Init(motor->TIMx, &TIM_OCInitStructure); TIM_OC1PreloadConfig(motor->TIMx, TIM_OCPreload_Enable); break; case TIM_Channel_2: TIM_OC2Init(motor->TIMx, &TIM_OCInitStructure); TIM_OC2PreloadConfig(motor->TIMx, TIM_OCPreload_Enable); break; case TIM_Channel_3: TIM_OC3Init(motor->TIMx, &TIM_OCInitStructure); TIM_OC3PreloadConfig(motor->TIMx, TIM_OCPreload_Enable); break; case TIM_Channel_4: TIM_OC4Init(motor->TIMx, &TIM_OCInitStructure); TIM_OC4PreloadConfig(motor->TIMx, TIM_OCPreload_Enable); break; } // 5. 启动定时器 TIM_Cmd(motor->TIMx, ENABLE); if(motor->TIMx == TIM1) TIM_CtrlPWMOutputs(TIM1, ENABLE); } // 设置舵机脉宽 void Servo_SetPulse(ServoMotor* motor, uint16_t pulse) { pulse = (pulse < motor->min_pulse) ? motor->min_pulse : ((pulse > motor->max_pulse) ? motor->max_pulse : pulse); switch(motor->TIM_Channel) { case TIM_Channel_1: motor->TIMx->CCR1 = pulse; break; case TIM_Channel_2: motor->TIMx->CCR2 = pulse; break; case TIM_Channel_3: motor->TIMx->CCR3 = pulse; break; case TIM_Channel_4: motor->TIMx->CCR4 = pulse; break; } } void Servo_SetAngle(ServoMotor* motor, float angle) { // 角度限幅 angle = fmaxf(motor->min_angle, fminf(motor->max_angle, angle)); motor->current_angle = angle; // 优化后的脉宽计算（四舍五入+正确类型转换） float range_deg = motor->max_angle - motor->min_angle; float range_us = (float)(motor->max_pulse - motor->min_pulse); // 使用浮点计算并四舍五入 float pulse_float = (float)motor->min_pulse + (range_us * (angle - motor->min_angle)) / range_deg; // 四舍五入转换为整数 uint16_t pulse = (uint16_t)(pulse_float + 0.5f); // +0.5实现四舍五入 Servo_SetPulse(motor, pulse); } // 云台初始化 void Turret_Init(void) { // 俯仰轴配置 (TIM2 CH1 - PA0) g_turret.pitch.TIMx = TIM2; g_turret.pitch.TIM_Channel = TIM_Channel_1; g_turret.pitch.GPIOx = GPIOA; g_turret.pitch.GPIO_Pin = GPIO_Pin_0; g_turret.pitch.min_angle = 0; g_turret.pitch.max_angle = 180; g_turret.pitch.min_pulse = PWM_MIN_PULSE; g_turret.pitch.max_pulse = PWM_MAX_PULSE; // 偏航轴配置 (TIM3 CH2 - PA7) g_turret.yaw.TIMx = TIM3; g_turret.yaw.TIM_Channel = TIM_Channel_2; g_turret.yaw.GPIOx = GPIOA; g_turret.yaw.GPIO_Pin = GPIO_Pin_7; g_turret.yaw.min_angle = 0; g_turret.yaw.max_angle = 180; g_turret.yaw.min_pulse = PWM_MIN_PULSE; g_turret.yaw.max_pulse = PWM_MAX_PULSE; // 初始化PWM PWM_Init(&g_turret.pitch); PWM_Init(&g_turret.yaw); // 初始位置 Servo_SetAngle(&g_turret.pitch, 90); Servo_SetAngle(&g_turret.yaw, 90); g_turret.is_smooth_enabled = 1; } // 设置云台角度 (带平滑移动) void Turret_SetAngle(float pitch_angle, float yaw_angle, uint16_t duration_ms) { const uint16_t step_time = 20; // 每20ms更新一次 const uint16_t steps = duration_ms / step_time; float start_pitch = g_turret.pitch.current_angle; float start_yaw = g_turret.yaw.current_angle; for(uint16_t i = 0; i <= steps; i++) { float ratio = (float)i / steps; float current_pitch = start_pitch + (pitch_angle - start_pitch) * ratio; float current_yaw = start_yaw + (yaw_angle - start_yaw) * ratio; Servo_SetAngle(&g_turret.pitch, current_pitch); Servo_SetAngle(&g_turret.yaw, current_yaw); Delay_ms(step_time); } } // 舵机校准函数 void Servo_Calibrate(ServoMotor* motor, float min_deg, float max_deg, uint16_t min_us, uint16_t max_us) { motor->min_angle = min_deg; motor->max_angle = max_deg; motor->min_pulse = min_us; motor->max_pulse = max_us; Servo_SetAngle(motor, motor->current_angle); // 应用新参数 } 这是我最后完整用的你的代码，为什么舵机没反应

我们正在处理舵机控制失效的问题，特别是STM32F103平台上的舵机无响应情况。结合用户提供的脉宽计算代码和舵机控制失效的上下文，我们需要系统地排查问题。以下是一个全面的故障排查流程： ### 一、硬件连接检查 1....

#include "stm32f10x.h" #include "Motor.h" #include "PWM.h" #include "Serial.h" #include "TRACK.h" #include "math.h" // 红外传感器引脚定义 #define IR1_PIN GPIO_Pin_12 // 最左侧传感器 #define IR2_PIN GPIO_Pin_13 // 左侧传感器 #define IR3_PIN GPIO_Pin_7 // 中间传感器 #define IR4_PIN GPIO_Pin_14 // 右侧传感器 #define IR5_PIN GPIO_Pin_15 // 最右侧传感器 #define IR_PORT GPIOB // 电机速度参数 #define BASE_SPEED 50 // 基础速度 (0-100) #define MAX_SPEED 80 // 最大速度 #define MIN_SPEED 20 // 最小速度 // 默认PID参数 #define DEFAULT_KP 8.0 // 比例系数 #define DEFAULT_KI 0.2 // 积分系数 #define DEFAULT_KD 5.0 // 微分系数 #define MAX_I_TERM 20.0 // 积分项限幅 // 红外传感器状态 uint8_t ir1, ir2, ir3, ir4, ir5; // PID控制变量 float error = 0; float last_error = 0; float integral = 0; float derivative = 0; float output = 0; // PID参数（可通过串口调整） float KP = DEFAULT_KP; float KI = DEFAULT_KI; float KD = DEFAULT_KD; // 电机速度（用于串口输出） int8_t motor_speed1, motor_speed2, motor_speed3, motor_speed4; // 调试计数器 uint32_t debug_counter = 0; /** * 函数：红外传感器初始化 */ void IR_Init(void) { RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; // 上拉输入 GPIO_InitStructure.GPIO_Pin = IR1_PIN | IR2_PIN | IR3_PIN | IR4_PIN | IR5_PIN; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(IR_PORT, &GPIO_InitStructure); } /** * 函数：读取所有红外传感器状态 */ void Read_IR_Sensors(void) { ir1 = (GPIO_ReadInputDataBit(IR_PORT, IR1_PIN) == Bit_RESET); // 检测到黑线为0 ir2 = (GPIO_ReadInputDataBit(IR_PORT, IR2_PIN) == Bit_RESET); ir3 = (GPIO_ReadInputDataBit(IR_PORT, IR3_PIN) == Bit_RESET); ir4 = (GPIO_ReadInputDataBit(IR_PORT, IR4_PIN) == Bit_RESET); ir5 = (GPIO_ReadInputDataBit(IR_PORT, IR5_PIN) == Bit_RESET); } /** * 函数：设置左右轮速度 */ void Set_Motor_Speeds(int8_t leftSpeed, int8_t rightSpeed) { // 限制速度范围 if (leftSpeed > MAX_SPEED) leftSpeed = MAX_SPEED; if (leftSpeed < -MAX_SPEED) leftSpeed = -MAX_SPEED; if (rightSpeed > MAX_SPEED) rightSpeed = MAX_SPEED; if (rightSpeed < -MAX_SPEED) rightSpeed = -MAX_SPEED; // 保存速度值用于串口输出 motor_speed1 = leftSpeed; // A路电机 motor_speed2 = leftSpeed; // B路电机 motor_speed3 = rightSpeed; // C路电机 motor_speed4 = rightSpeed; // D路电机 // 设置左侧两个电机同速 Motor_SetSpeed1(leftSpeed); Motor_SetSpeed2(leftSpeed); // 设置右侧两个电机同速 Motor_SetSpeed3(rightSpeed); Motor_SetSpeed4(rightSpeed); } /** * 函数：计算位置误差 * 返回值：位置误差（-2.0到2.0之间） */ float Calculate_Error(void) { // 传感器权重：[-2, -1, 0, 1, 2] float weighted_sum = 0; uint8_t sensor_count = 0; if (ir1) { weighted_sum += -2.0f; sensor_count++; } if (ir2) { weighted_sum += -1.0f; sensor_count++; } if (ir3) { weighted_sum += 0.0f; sensor_count++; } if (ir4) { weighted_sum += 1.0f; sensor_count++; } if (ir5) { weighted_sum += 2.0f; sensor_count++; } // 如果没有传感器检测到黑线，返回上一次的误差 if (sensor_count == 0) { return last_error; } return weighted_sum / sensor_count; } /** * 函数：设置PID参数 */ void Set_PID_Params(float kp, float ki, float kd) { KP = kp; KI = ki; KD = kd; integral = 0; // 重置积分项 } /** * 函数：打印电机速度到串口 */ void Print_Motor_Speeds(void) { Serial_Printf("M1:%d M2:%d M3:%d M4:%d E:%.2f I:%.2f D:%.2f O:%.2f\n", motor_speed1, motor_speed2, motor_speed3, motor_speed4, error, integral, derivative, output); } /** * 函数：根据传感器状态控制电机（PID控制） / void Line_Following_Control(void) { Read_IR_Sensors(); // 特殊情况1：所有传感器都检测到黑线（停止） if (ir1 && ir2 && ir3 && ir4 && ir5) { Set_Motor_Speeds(0, 0); integral = 0; // 重置积分项 return; } // 特殊情况2：所有传感器都未检测到黑线（保持最后动作） if (!(ir1 || ir2 || ir3 || ir4 || ir5)) { // 保持当前PID输出 } else { // 计算当前误差 error = Calculate_Error(); } // PID计算 integral += error; // 积分项累加 // 积分限幅防止积分饱和 if (integral > MAX_I_TERM) integral = MAX_I_TERM; if (integral < -MAX_I_TERM) integral = -MAX_I_TERM; derivative = error - last_error; // 微分项（误差变化率） // 动态PID调整 - 根据误差大小调整参数 float kp = KP, ki = KI, kd = KD; // 大偏差时增强P项，减弱I项 if (fabs(error) > 1.0f) { kp = KP 1.5f; ki = KI * 0.5f; } // 小偏差时增强I项，减弱D项 else if (fabs(error) < 0.3f) { ki = KI * 2.0f; kd = KD * 0.7f; } // PID输出 = Kpe + Ki∫e + Kdde/dt output = kp error + ki * integral + kd * derivative; // 保存当前误差用于下一次计算 last_error = error; // 设置电机速度 int8_t left_speed = BASE_SPEED - (int8_t)output; int8_t right_speed = BASE_SPEED + (int8_t)output; // 确保最小速度 if (left_speed > 0 && left_speed < MIN_SPEED) left_speed = MIN_SPEED; if (left_speed < 0 && left_speed > -MIN_SPEED) left_speed = -MIN_SPEED; if (right_speed > 0 && right_speed < MIN_SPEED) right_speed = MIN_SPEED; if (right_speed < 0 && right_speed > -MIN_SPEED) right_speed = -MIN_SPEED; Set_Motor_Speeds(left_speed, right_speed); // 每50次控制循环打印一次数据 if (debug_counter++ >= 50) { debug_counter = 0; Print_Motor_Speeds(); } } #include "stm32f10x.h" #include "Motor.h" #include "PWM.h" #include "Serial.h" #include "TRACK.h" #include <stdio.h> #include "Delay.h" #include "stm32f10x_it.h" extern volatile uint32_t SystemTick; // 引用主文件中的变量 int main(void) { // 外设初始化 SystemInit(); // 系统时钟初始化 Motor_Init(); // 电机初始化 PWM_Init(); // PWM初始化 Serial_Init(); // 串口初始化 IR_Init(); // 红外传感器初始化 // 初始PID参数设置 Set_PID_Params(DEFAULT_KP, DEFAULT_KI, DEFAULT_KD); // 欢迎消息 Serial_Printf("\n\n===== 五路循迹小车 PID调试系统 =====\n"); Serial_Printf("当前PID参数: KP=%.1f, KI=%.1f, KD=%.1f\n", KP, KI, KD); Serial_Printf("调试指令:\n"); Serial_Printf(" P/p: 增加/减少比例系数 (步长0.5)\n"); Serial_Printf(" I/i: 增加/减少积分系数 (步长0.1)\n"); Serial_Printf(" D/d: 增加/减少微分系数 (步长0.5)\n"); Serial_Printf(" R: 重置PID参数到默认值\n"); Serial_Printf(" S: 显示当前PID参数\n"); Serial_Printf(" T: 切换调试模式(开/关)\n"); uint8_t debug_enabled = 1; // 默认开启调试输出 Serial_Printf("SysTick测试: 开始计时...\n"); while(1) { // 1. 循迹控制 Line_Following_Control(); // 2. 串口命令处理 if(Serial_GetRxFlag()) { uint8_t cmd = Serial_GetRxData(); switch(cmd) { // 调整比例系数 case 'P': KP += 0.5f; break; case 'p': KP -= 0.5f; break; // 调整积分系数 case 'I': KI += 0.1f; break; case 'i': KI -= 0.1f; break; // 调整微分系数 case 'D': KD += 0.5f; break; case 'd': KD -= 0.5f; break; // 重置PID参数 case 'R': KP = DEFAULT_KP; KI = DEFAULT_KI; KD = DEFAULT_KD; integral = 0; // 重置积分项 break; // 显示当前参数 case 'S': Serial_Printf("当前PID: KP=%.1f, KI=%.1f, KD=%.1f\n", KP, KI, KD); break; // 切换调试模式 case 'T': debug_enabled = !debug_enabled; Serial_Printf("调试模式: %s\n", debug_enabled ? "开启" : "关闭"); break; } // 限制PID参数范围 if(KP < 0) KP = 0; if(KI < 0) KI = 0; if(KD < 0) KD = 0; // 显示调整后的参数 if(cmd != 'S' && cmd != 'T') { Serial_Printf("PID调整: KP=%.1f, KI=%.1f, KD=%.1f\n", KP, KI, KD); } } // 3. 手动控制调试输出频率 if(debug_enabled) { static uint32_t last_print = 0; if(SystemTick - last_print > 500) { // 每500ms输出一次 Print_Motor_Speeds(); last_print = SystemTick; } } // 4. 延时确保稳定性 Delay_ms(10); // 10ms延时，控制频率约100Hz } } // 简单的延时函数 void Delay_ms(uint32_t ms) { for(uint32_t i = 0; i < ms * 5000; i++); } // 系统滴答计时器（简化实现） __IO uint32_t SystemTick = 0; void SysTick_Handler(void) // 需要在中断向量表中配置 { SystemTick++; } 为什么这个代码我运行的时候，主函数中显示KP,DEFAULT_KP,KI = DEFAULT_KI,KD, DEFAULT_KD,integral均未声明，可是我已经在TRACK.c文件中，声明过了，那么应该如何修改

void Set_Motor_Speeds(int8_t leftSpeed, int8_t rightSpeed); float Calculate_Error(void); void Set_PID_Params(float kp, float ki, float kd); void Print_Motor_Speeds(void); void Line_Following_Control...

#include "stm32f10x.h" // Device header #include "Delay.h" #include "OLED.h" #include "Encoder.h" #include "Motor.h" #include "Timer.h" #include "Serial.h" int8_t L=0; int8_t l_stop=0; int16_t Larr[]={1000,2000,3000,4000,5000}; int16_t countL=0; int16_t Llocation=0; float Lkp=0.5,Lki=0.14,Lkd=0; int16_t Lspeed_goal=50,Lspeed_current; int16_t Lspeederror_current,Lspeederror_add=0; int16_t Lspeederror_before=0; float Loutput=0; int8_t R=0; int8_t r_stop=0; int16_t Rarr[]={5000,5000,5000,5000,5000}; int16_t countR; int16_t Rlocation=0; float Rkp=0.4,Rki=0.15,Rkd=0; int16_t Rspeed_goal=50,Rspeed_current; int16_t Rspeederror_current,Rspeederror_add=0; int16_t Rspeederror_before=0; float Routput=0; int main(void) { OLED_Init(); Motor_Init(); Encoder_Init(); Timer_Init(); Serial_Init(); while (1) { Serial_Printf("%d,%d\r\n",Rlocation,Llocation); OLED_ShowChar(0,0,'L',OLED_8X16); OLED_ShowSignedNum(8,0,Lspeed_current,3,OLED_8X16); OLED_ShowSignedNum(0,16,Llocation,5,OLED_8X16); OLED_ShowChar(0,32,'R',OLED_8X16); OLED_ShowSignedNum(8,32,Rspeed_current,3,OLED_8X16); OLED_ShowSignedNum(0,48,Rlocation,5,OLED_8X16); OLED_Update(); } } void TIM3_IRQHandler(void)//1毫秒进一次中断 { if(TIM_GetITStatus(TIM3, TIM_IT_Update) == SET) { countL++; if(countL==40)//每四十毫秒进入一次 { //*************************************************************************************************************** //这是左轮子 Lspeed_current=Encoder_GetA(); Lspeederror_current=Lspeed_goal-Lspeed_current; Lspeederror_add+=Lspeederror_current; //积分限幅 if(Lspeederror_add>350) { Lspeederror_add=350; } else if(Lspeederror_add<-350) { Lspeederror_add=-350; } //计算输出 Loutput=LkpLspeederror_current+LkiLspeederror_add+Lkd*(Lspeederror_current-Lspeederror_before); //输出限幅 if(Loutput>100) { Loutput=100; } else if(Loutput<-100) { Loutput=-100; } Llocation+=Lspeed_current; if(Llocation>=Larr[L]-50&&Larr[L]-50<=Llocation) { L++; Llocation=0; l_stop=1; } else { l_stop=0; } if(l_stop==0|| (l_stop&&r_stop) ) { Motor_SetSpeedA(Loutput);//输出 l_stop=0; } else { Motor_SetSpeedA(0);//等一下另一个轮子 } Lspeederror_before=Lspeederror_current; countL=0; } countR++; if(countR==40) { //******************************************************************************************************************** //这是右轮 Rspeed_current=Encoder_GetB(); Rspeederror_current=Rspeed_goal-Rspeed_current; Rspeederror_add+=Rspeederror_current; //积分限幅 if(Rspeederror_add>300) { Rspeederror_add=300; } else if(Rspeederror_add<-300) { Rspeederror_add=-300; } //计算输出 Routput=RkpRspeederror_current+RkiRspeederror_add+Rkd*(Rspeederror_current-Rspeederror_before); //输出限幅 if(Routput>100) { Routput=100; } else if(Routput<-100) { Routput=-100; } Rlocation+=Rspeed_current; if(Rlocation>=Rarr[R]-50&&Rarr[R]-50<=Rlocation) { R++; Rlocation=0; r_stop=1; } else { r_stop=0; } if(r_stop==0|| (l_stop&&r_stop) ) { Motor_SetSpeedB(Routput);//输出 r_stop=0; } else { Motor_SetSpeedB(0);//等一下另一个轮子 } Rspeederror_before=Rspeederror_current; countR=0; } TIM_ClearITPendingBit(TIM3, TIM_IT_Update); } } 我希望小车轮子能停下来等另一个小车轮子，为什么停不下来，是哪里写错了吗

通过分析提供的代码片段以及 STM32F10x 的特性，可以发现一些可能导致轮子不同步的原因。 #### 1. **停止条件中的绝对值判断** 代码中使用了 if (abs(encountA) <= 5 && pidMotor1Speed.target_val == 0) 来决定...

#include "stm32f10x.h" // Device header #include "Delay.h" #include "Car.h" #include "Get.h" int L1,L2,R1,R2; int count = 0; // 计数器，用于记录传感器状态变化的次数 int last_state = 0; // 记录上一次传感器状态 int current_state; void read_sensors() { L1 = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_4); L2 = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5); R1 = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_6); R2 = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_7); } int main(void) { Get_Init(); Car_Init(); while(1) { read_sensors(); current_state = (L1 << 3) | (L2 << 2) | (R1 << 1) | R2; // if (last_state == 0xF && current_state != 0xF) // { // count++; // 传感器状态变化，计数器加1 // Delay_ms(300); // if (count >= 11) // { // Car_Stop(); // 计数器达到11，车辆停止 // while(1); // } // } // else // { switch (current_state) { case 0x0: Go_Ahead(); break; case 0x8: Self_Left(); Delay_ms(50); break; case 0xC: Self_Left(); Delay_ms(50); break; case 0x4: Turn_Left(); Delay_ms(10); break; case 0x2: Turn_Right(); Delay_ms(10); break; case 0x1: Self_Right(); Delay_ms(50); break; case 0x3: Self_Right(); Delay_ms(50); break; case 0xF: Car_SlowDown(); break; case 0x6: Car_SlowDown(); Delay_ms(50); break; default: break; } } // // } } 详细解释

void Set_Motor_Speed(Motor_Side motor, uint8_t speed) { uint16_t pwm_val = (speed * TIM1->ARR) / 100; // 将百分比转换为PWM值 if(motor == LEFT_MOTOR) { TIM_SetCompare1(TIM1, pwm_val); } else { TIM...

#include "stm32f10x.h" // 初始化GPIO和定时器 void Motor_Init(void) { // 1. 使能时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); // 2. 配置DIR和ENABLE引脚（普通GPIO输出） GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_2; // DIR(PA1), ENABLE(PA2) GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStruct); // 3. 配置STEP引脚（复用推挽输出） GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6; // STEP(PA6, TIM3_CH1) GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP; // 复用推挽 GPIO_Init(GPIOA, &GPIO_InitStruct); // 4. 配置定时器TIM3 TIM_TimeBaseInitTypeDef TIM_InitStruct; TIM_InitStruct.TIM_Period = 1000; // ARR值（初始频率） TIM_InitStruct.TIM_Prescaler = 72 - 1; // 72MHz/72 = 1MHz计数器频率 TIM_InitStruct.TIM_ClockDivision = 0; TIM_InitStruct.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM3, &TIM_InitStruct); // 5. 配置输出比较模式（自动翻转） TIM_OCInitTypeDef TIM_OCInitStruct; TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_Toggle; // 翻转模式 TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStruct.TIM_Pulse = 500; // 初始CCR值（占空比50%） TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(TIM3, &TIM_OCInitStruct); // 通道1 // 6. 启动定时器 TIM_Cmd(TIM3, ENABLE); TIM_CtrlPWMOutputs(TIM3, ENABLE); } // 设置电机速度（脉冲频率） void SetMotorSpeed(uint16_t freq_hz) { // 计算ARR值：freq = 1MHz / (2 * ARR) → ARR = 500000 / freq uint32_t arr_val = 500000 / freq_hz; // 1MHz计数器，翻转两次=1周期 TIM3->ARR = arr_val; // 直接设置自动重装载值 TIM3->CCR1 = arr_val / 2; // 比较值设为50%占空比 } // 设置电机方向 void SetMotorDirection(uint8_t dir) { // dir=0: 反向, dir=1: 正向 GPIO_WriteBit(GPIOA, GPIO_Pin_1, dir ? Bit_SET : Bit_RESET); } // 使能/禁用电机 void SetMotorEnable(uint8_t en) { // en=0: 禁用, en=1: 使能 GPIO_WriteBit(GPIOA, GPIO_Pin_2, en ? Bit_RESET : Bit_SET); // 低电平使能 } int main(void) { Motor_Init(); SetMotorEnable(1); // 使能电机 SetMotorDirection(1); // 正向旋转 SetMotorSpeed(500); // 500Hz脉冲频率 while(1) { // 主循环（可添加调速逻辑） } } 这是我的代码，使用stm32f103单片机标准库控制步进电机旋转，我想让你帮我修改一下代码，能实现设置一个目标角度，当电机的旋转角度达到目标角度之后就能自己停下来，在保证代码能实现目标功能的情况下，尽可能的简单

void Stepper_Rotate(int32_t steps, uint8_t direction) { // 设置方向 if (direction) GPIO_SetBits(GPIOA, GPIO_Pin_1); // 正转 else GPIO_ResetBits(GPIOA, GPIO_Pin_1); // 反转 // 设置目标步数（取...

#include "stm32f10x.h" // Device header /** * 函数：编码器初始化 * 参数：无 * 返回值：无 / void Encoder_Init(void) { / 开启时钟 */ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitTypeDef GPIO_InitStructure; /** 左侧编码器 (TIM3_CH1 & CH2) -> PA6 PA7 / GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); / 右侧编码器 (TIM4_CH1 & CH2) -> PB6 PB7 / GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; GPIO_Init(GPIOB, &GPIO_InitStructure); / TIM3 初始化 - 左侧编码器 / TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; TIM_TimeBaseInitStructure.TIM_Prescaler = 0; // 不分频 TIM_TimeBaseInitStructure.TIM_Period = 65535; TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStructure); TIM_ICInitTypeDef TIM_ICInitStructure; TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; TIM_ICInitStructure.TIM_ICFilter = 0xF; TIM_ICInit(TIM3, &TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; TIM_ICInitStructure.TIM_ICFilter = 0xF; TIM_ICInit(TIM3, &TIM_ICInitStructure); TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling); TIM_Cmd(TIM3, ENABLE); / TIM4 初始化 - 右侧编码器 / TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStructure); TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; TIM_ICInit(TIM4, &TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; TIM_ICInit(TIM4, &TIM_ICInitStructure); TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling); TIM_Cmd(TIM4, ENABLE); } / * 函数：获取左侧编码器的增量值 * 参数：无 * 返回值：自上次调用此函数后，左侧编码器的增量值 */ int16_t Encoder_Get_Left(void) { int16_t temp = TIM_GetCounter(TIM3); TIM_SetCounter(TIM3, 0); // 清零计数器 return temp; } /** * 函数：获取右侧编码器的增量值 * 参数：无 * 返回值：自上次调用此函数后，右侧编码器的增量值 */ int16_t Encoder_Get_Right(void) { int16_t temp = TIM_GetCounter(TIM4); TIM_SetCounter(TIM4, 0); // 清零计数器 return temp; } 这是编码器代码是否需要根据编码器写一个电机的pid

int16_t current_count = get_encoder(); int speed = (current_count - last_count) * 1000 / MOTOR_PID_INTERVAL_MS; // 脉冲数/秒 last_count = current_count; last_time = current_time; return speed; }...

#include "stm32f10x.h" // Device header /** * 函数：定时中断初始化 * 参数：无 * 返回值：无 / void Timer_Init(void) { /开启时钟/ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //开启TIM2的时钟 /配置时钟源/ TIM_InternalClockConfig(TIM2); //选择TIM2为内部时钟，若不调用此函数，TIM默认也为内部时钟 /时基单元初始化/ TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; //定义结构体变量 TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; //时钟分频，选择不分频，此参数用于配置滤波器时钟，不影响时基单元功能 TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; //计数器模式，选择向上计数 TIM_TimeBaseInitStructure.TIM_Period = 10 - 1; //计数周期，即ARR的值 TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1; //预分频器，即PSC的值 TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0; //重复计数器，高级定时器才会用到 TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure); //将结构体变量交给TIM_TimeBaseInit，配置TIM2的时基单元 /中断输出配置/ TIM_ClearFlag(TIM2, TIM_FLAG_Update); //清除定时器更新标志位 //TIM_TimeBaseInit函数末尾，手动产生了更新事件 //若不清除此标志位，则开启中断后，会立刻进入一次中断 //如果不介意此问题，则不清除此标志位也可 TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //开启TIM2的更新中断 /NVIC中断分组/ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //配置NVIC为分组2 //即抢占优先级范围：0~3，响应优先级范围：0~3 //此分组配置在整个工程中仅需调用一次 //若有多个中断，可以把此代码放在main函数内，while循环之前 //若调用多次配置分组的代码，则后执行的配置会覆盖先执行的配置 /NVIC配置/ NVIC_InitTypeDef NVIC_InitStructure; //定义结构体变量 NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; //选择配置NVIC的TIM2线 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //指定NVIC线路使能 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; //指定NVIC线路的抢占优先级为2 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //指定NVIC线路的响应优先级为1 NVIC_Init(&NVIC_InitStructure); //将结构体变量交给NVIC_Init，配置NVIC外设 /TIM使能/ TIM_Cmd(TIM2, ENABLE); //使能TIM2，定时器开始运行 } / 定时器中断函数，可以复制到使用它的地方 void TIM2_IRQHandler(void) { if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) { TIM_ClearITPendingBit(TIM2, TIM_IT_Update); } } */ 使用我的函数，不进行更改

#include "stm32f10x.h" // 系统时间计数器，每1ms增加1（通过中断中计数100次实现） static volatile uint32_t tick_counter = 0; // 中断计数，每10us中断一次，100次为1ms static volatile uint16_t interrupt_...

/* USER CODE BEGIN Header */ / **************************************************************************** * @file : main.c * @brief : Main program body ****************************************************************************** * @attention * * Copyright (c) 2022 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ / **************************************************************************** * 作者:好家伙VCC * 联系方式:[email protected] * 软件版本:V3.3.0 * 使用硬件版本:V3.3.0 * ****************************************************************************** * 更新日志: * * * ****************************************************************************** / / USER CODE END Header / / Includes ------------------------------------------------------------------/ #include "main.h" #include "adc.h" #include "tim.h" #include "usart.h" #include "gpio.h" / Private includes ----------------------------------------------------------/ / USER CODE BEGIN Includes / #include "oled.h" #include "stdio.h" #include "motor.h" #include "niming.h" #include "pid.h" #include "cJSON.h" #include <string.h> #include "HC_SR04.h" #include "mpu6050.h" #include "inv_mpu.h" #include "inv_mpu_dmp_motion_driver.h" / USER CODE END Includes / / Private typedef -----------------------------------------------------------/ / USER CODE BEGIN PTD / / USER CODE END PTD / / Private define ------------------------------------------------------------/ / USER CODE BEGIN PD / / USER CODE END PD / / Private macro -------------------------------------------------------------/ / USER CODE BEGIN PM / extern float Motor1Speed ;//声明电机1速度 extern float Motor2Speed ;//声明电机2速度 extern tPid pidMotor1Speed;//声明电机1PID速度控制结构体类型变量 extern tPid pidMotor2Speed; extern tPid pidFollow; //定距离跟随PID extern tPid pidMPU6050YawMovement; //利用6050偏航角进行姿态控制的PID extern uint8_t Usart1_ReadBuf[255]; //串口1 缓冲数组 float p,i,d,a,b;//使用JSON时候使用的变量 uint8_t OledString[50];//OLED显示使用的字符串数组 extern float Mileage;//里程数 extern tPid pidHW_Tracking;//红外循迹的PID uint8_t g_ucaHW_Read[4] = {0};//保存红外对管电平的数组 int8_t g_cThisState = 0;//这次状态 int8_t g_cLastState = 0; //上次状态 float g_fHW_PID_Out;//红外对管PID计算输出速度 float g_fHW_PID_Out1;//电机1的最后循迹PID控制速度 float g_fHW_PID_Out2;//电机2的最后循迹PID控制速度 uint8_t g_ucUsart3ReceiveData; //保存串口三接收的数据 uint8_t Usart3String[50];//串口三输出字符串使用的字符串数组 float g_fHC_SR04_Read;//超声波传感器读取障碍物数据 float g_fFollow_PID_Out;//定距离跟随PID计算输出速度 float pitch,roll,yaw; //俯仰角横滚角航向角 float g_fMPU6050YawMovePidOut = 0.00f; //姿态PID运算输出 float g_fMPU6050YawMovePidOut1 = 0.00f; //第一个电机控制输出 float g_fMPU6050YawMovePidOut2 = 0.00f; //第一个电机控制输出 uint8_t g_ucMode = 0; //小车运动模式标志位 0:显示功能、1:PID循迹模式、2:手机遥控普通运动模式、3.超声波避障模式、4:PID跟随模式、5:遥控角度闭环 / USER CODE END PM / / Private variables ---------------------------------------------------------/ / USER CODE BEGIN PV / / USER CODE END PV / / Private function prototypes -----------------------------------------------/ void SystemClock_Config(void); / USER CODE BEGIN PFP / / USER CODE END PFP / / Private user code ---------------------------------------------------------/ / USER CODE BEGIN 0 / / USER CODE END 0 */ /** * @brief The application entry point. * @retval int / int main(void) { / USER CODE BEGIN 1 / / USER CODE END 1 / / MCU Configuration--------------------------------------------------------/ / Reset of all peripherals, Initializes the Flash interface and the Systick. / HAL_Init(); / USER CODE BEGIN Init / / USER CODE END Init / / Configure the system clock / SystemClock_Config(); / USER CODE BEGIN SysInit / / USER CODE END SysInit / / Initialize all configured peripherals / MX_GPIO_Init(); MX_USART1_UART_Init(); MX_TIM1_Init(); MX_TIM2_Init(); MX_TIM4_Init(); MX_ADC2_Init(); MX_USART3_UART_Init(); / USER CODE BEGIN 2 / OLED_Init(); //初始化OLED OLED_Clear() ; HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);//开启定时器1 通道1 PWM输出 HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);//开启定时器1 通道4 PWM输出 HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);//开启定时器2 HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);//开启定时器4 HAL_TIM_Base_Start_IT(&htim2); //开启定时器2 中断 HAL_TIM_Base_Start_IT(&htim4); //开启定时器4 中断 HAL_TIM_Base_Start_IT(&htim1); //开启定时器1 中断 __HAL_UART_ENABLE_IT(&huart1,UART_IT_RXNE); //开启串口1接收中断 PID_init();//PID参数初始化 HAL_UART_Receive_IT(&huart3,&g_ucUsart3ReceiveData,1); //串口三接收数据 HAL_Delay(500);//延时0.5秒 6050上电稳定后初始化 MPU_Init(); //初始化MPU6050 while(MPU_Init()!=0);//初始化MPU6050模块的MPU 注意初始化阶段不要移动小车 while(mpu_dmp_init()!=0);//mpu6050,dmp初始化 // cJSON cJsonData ,cJsonVlaue; / USER CODE END 2 / / Infinite loop / / USER CODE BEGIN WHILE / while (1) { / USER CODE END WHILE / / USER CODE BEGIN 3 / sprintf((char )OledString," g_ucMode:%d",g_ucMode);//显示g_ucMode 当前模式 OLED_ShowString(0,6,OledString,12); //显示在OLED上 sprintf((char )Usart3String," g_ucMode:%d",g_ucMode);//蓝牙APP显示 HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 if(g_ucMode == 0) { //0LED显示功能 sprintf((char)OledString, "V1:%.2fV2:%.2f", Motor1Speed,Motor2Speed);//显示速度 OLED_ShowString(0,0,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数， sprintf((char)OledString, "Mileage:%.2f", Mileage);//显示里程 OLED_ShowString(0,1,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数， sprintf((char)OledString, "U:%.2fV", adcGetBatteryVoltage());//显示电池电压 OLED_ShowString(0,2,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数， sprintf((char )OledString,"HC_SR04:%.2fcm\r\n",HC_SR04_Read());//显示超声波数据 OLED_ShowString(0,3,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数， sprintf((char )OledString,"p:%.2f r:%.2f \r\n",pitch,roll);//显示6050数据俯仰角横滚角 OLED_ShowString(0,4,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数， sprintf((char )OledString,"y:%.2f \r\n",yaw);//显示6050数据航向角 OLED_ShowString(0,5,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数， //蓝牙APP显示 sprintf((char)Usart3String, "V1:%.2fV2:%.2f", Motor1Speed,Motor2Speed);//显示速度 HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 //阻塞方式发送可以保证数据发送完毕，中断发送不一定可以保证数据已经发送完毕才启动下一次发送 sprintf((char)Usart3String, "Mileage:%.2f", Mileage);//显示里程 HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 sprintf((char)Usart3String, "U:%.2fV", adcGetBatteryVoltage());//显示电池电压 HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 sprintf((char )Usart3String,"HC_SR04:%.2fcm\r\n",HC_SR04_Read());//显示超声波数据 HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 sprintf((char )Usart3String,"p:%.2f r:%.2f \r\n",pitch,roll);//显示6050数据俯仰角横滚角 HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 sprintf((char )Usart3String,"y:%.2f \r\n",yaw);//显示6050数据航向角 HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char *)Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 //获得6050数据 while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0){} //这个可以解决经常读不出数据的问题 //在显示模式电机停转设置小车速度为0 motorPidSetSpeed(0,0); } if(g_ucMode == 1) { /// 红外PID循迹功能******/ g_ucaHW_Read[0] = READ_HW_OUT_1;//读取红外对管状态、这样相比于写在if里面更高效 g_ucaHW_Read[1] = READ_HW_OUT_2; g_ucaHW_Read[2] = READ_HW_OUT_3; g_ucaHW_Read[3] = READ_HW_OUT_4; if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 ) { // printf("应该前进\r\n");//注释掉更加高效，减少无必要程序执行 g_cThisState = 0;//前进 } else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )//使用else if更加合理高效 { // printf("应该右转\r\n"); g_cThisState = -1;//应该右转 } else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 ) { // printf("快速右转\r\n"); g_cThisState = -2;//快速右转 } else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0) { // printf("快速右转\r\n"); g_cThisState = -3;//快速右转 } else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 0 ) { // printf("应该左转\r\n"); g_cThisState = 1;//应该左转 } else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 1 ) { // printf("快速左转\r\n"); g_cThisState = 2;//快速左转 } else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 1) { // printf("快速左转\r\n"); g_cThisState = 3;//快速左转 } g_fHW_PID_Out = PID_realize(&pidHW_Tracking,g_cThisState);//PID计算输出目标速度这个速度，会和基础速度加减 g_fHW_PID_Out1 = 3 + g_fHW_PID_Out;//电机1速度=基础速度+循迹PID输出速度 g_fHW_PID_Out2 = 3 - g_fHW_PID_Out;//电机1速度=基础速度-循迹PID输出速度 if(g_fHW_PID_Out1 >5) g_fHW_PID_Out1 =5;//进行限幅限幅速度在0-5之间 if(g_fHW_PID_Out1 <0) g_fHW_PID_Out1 =0; if(g_fHW_PID_Out2 >5) g_fHW_PID_Out2 =5;//进行限幅限幅速度在0-5之间 if(g_fHW_PID_Out2 <0) g_fHW_PID_Out2 =0; if(g_cThisState != g_cLastState)//如何这次状态不等于上次状态、就进行改变目标速度和控制电机、在定时器中依旧定时控制电机 { motorPidSetSpeed(g_fHW_PID_Out1,g_fHW_PID_Out2);//通过计算的速度控制电机 } g_cLastState = g_cThisState;//保存上次红外对管状态 } if(g_ucMode == 2) { //遥控模式// //遥控模式的控制在串口三的中断里面 } if(g_ucMode == 3) { //超声波避障模式// ////避障逻辑 if(HC_SR04_Read() > 25)//前方无障碍物 { motorPidSetSpeed(1,1);//前运动 HAL_Delay(100); } else{ //前方有障碍物 motorPidSetSpeed(-1,1);//右边运动原地 HAL_Delay(500); if(HC_SR04_Read() > 25)//右边无障碍物 { motorPidSetSpeed(1,1);//前运动 HAL_Delay(100); } else{//右边有障碍物 motorPidSetSpeed(1,-1);//左边运动原地 HAL_Delay(1000); if(HC_SR04_Read() >25)//左边无障碍物 { motorPidSetSpeed(1,1);//前运动 HAL_Delay(100); } else{ motorPidSetSpeed(-1,-1);//后运动 HAL_Delay(1000); motorPidSetSpeed(-1,1);//右边运动 HAL_Delay(50); } } } } if(g_ucMode == 4) { //PID跟随功能// g_fHC_SR04_Read=HC_SR04_Read();//读取前方障碍物距离 if(g_fHC_SR04_Read < 60){ //如果前60cm 有东西就启动跟随 g_fFollow_PID_Out = PID_realize(&pidFollow,g_fHC_SR04_Read);//PID计算输出目标速度这个速度，会和基础速度加减 if(g_fFollow_PID_Out > 6) g_fFollow_PID_Out = 6;//对输出速度限幅 if(g_fFollow_PID_Out < -6) g_fFollow_PID_Out = -6; motorPidSetSpeed(g_fFollow_PID_Out,g_fFollow_PID_Out);//速度作用与电机上 } else motorPidSetSpeed(0,0);//如果前面60cm 没有东西就停止 HAL_Delay(10);//读取超声波传感器不能过快 } if(g_ucMode == 5) { //*MPU6050航向角 PID转向控制***********// sprintf((char )Usart3String,"pitch:%.2f roll:%.2f yaw:%.2f\r\n",pitch,roll,yaw);//显示6050数据俯仰角横滚角航向角 HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char *)Usart3String),0xFFFF);//通过串口三输出字符 strlen:计算字符串大小 //mpu_dmp_get_data(&pitch,&roll,&yaw);//返回值:0,DMP成功解出欧拉角 while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0){} //这个可以解决经常读不出数据的问题 g_fMPU6050YawMovePidOut = PID_realize(&pidMPU6050YawMovement,yaw);//PID计算输出目标速度这个速度，会和基础速度加减 g_fMPU6050YawMovePidOut1 = 1.5 + g_fMPU6050YawMovePidOut;//基础速度加减PID输出速度 g_fMPU6050YawMovePidOut2 = 1.5 - g_fMPU6050YawMovePidOut; if(g_fMPU6050YawMovePidOut1 >3.5) g_fMPU6050YawMovePidOut1 =3.5;//进行限幅 if(g_fMPU6050YawMovePidOut1 <0) g_fMPU6050YawMovePidOut1 =0; if(g_fMPU6050YawMovePidOut2 >3.5) g_fMPU6050YawMovePidOut2 =3.5;//进行限幅 if(g_fMPU6050YawMovePidOut2 <0) g_fMPU6050YawMovePidOut2 =0; motorPidSetSpeed(g_fMPU6050YawMovePidOut1,g_fMPU6050YawMovePidOut2);//将最后计算的目标速度通过motorPidSetSpeed控制电机 } /// 红外PID循迹功能******/ // g_ucaHW_Read[0] = READ_HW_OUT_1;//读取红外对管状态、这样相比于写在if里面更高效 // g_ucaHW_Read[1] = READ_HW_OUT_2; // g_ucaHW_Read[2] = READ_HW_OUT_3; // g_ucaHW_Read[3] = READ_HW_OUT_4; // if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 ) // { //// printf("应该前进\r\n");//注释掉更加高效，减少无必要程序执行 // g_cThisState = 0;//前进 // } // else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )//使用else if更加合理高效 // { //// printf("应该右转\r\n"); // g_cThisState = -1;//应该右转 // } // else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 ) // { //// printf("快速右转\r\n"); // g_cThisState = -2;//快速右转 // } // else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0) // { //// printf("快速右转\r\n"); // g_cThisState = -3;//快速右转 // } // else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 0 ) // { //// printf("应该左转\r\n"); // g_cThisState = 1;//应该左转 // } // else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 1 ) // { //// printf("快速左转\r\n"); // g_cThisState = 2;//快速左转 // } // else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 1) // { //// printf("快速左转\r\n"); // g_cThisState = 3;//快速左转 // } // g_fHW_PID_Out = PID_realize(&pidHW_Tracking,g_cThisState);//PID计算输出目标速度这个速度，会和基础速度加减 // g_fHW_PID_Out1 = 3 + g_fHW_PID_Out;//电机1速度=基础速度+循迹PID输出速度 // g_fHW_PID_Out2 = 3 - g_fHW_PID_Out;//电机1速度=基础速度-循迹PID输出速度 // if(g_fHW_PID_Out1 >5) g_fHW_PID_Out1 =5;//进行限幅限幅速度在0-5之间 // if(g_fHW_PID_Out1 <0) g_fHW_PID_Out1 =0; // if(g_fHW_PID_Out2 >5) g_fHW_PID_Out2 =5; // if(g_fHW_PID_Out2 <0) g_fHW_PID_Out2 =0; // if(g_cThisState != g_cLastState)//如何这次状态不等于上次状态、就进行改变目标速度和控制电机、在定时器中依旧定时控制电机 // { // motorPidSetSpeed(g_fHW_PID_Out1,g_fHW_PID_Out2);//通过计算的速度控制电机 // } // // g_cLastState = g_cThisState;//保存上次红外对管状态 ////通过串口三(蓝牙)输出信息 ////*串口三输出**********************// // sprintf((char )Usart3String,"V1:%.2fV2:%.2f\r\n",Motor1Speed,Motor2Speed);//显示两个电机转速单位：转/秒 // HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 // // sprintf((char )Usart3String,"Mileage%.2f\r\n",Mileage);//计算小车里程单位cm // HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 // // sprintf((char )Usart3String,"U:%.2fV\r\n",adcGetBatteryVoltage());//显示电池电压 // HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小 // // sprintf((char )Usart3String,"HC_SR04:%.2fcm\r\n",HC_SR04_Read());//显示超声波数据 // HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),0xFFFF);//通过串口三输出字符 strlen:计算字符串大小 // // sprintf((char )Usart3String,"pitch:%.2f roll:%.2f yaw:%.2f\r\n",pitch,roll,yaw);//显示6050数据俯仰角横滚角航向角 // HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),0xFFFF);//通过串口三输出字符 strlen:计算字符串大小 // // //mpu_dmp_get_data(&pitch,&roll,&yaw);//返回值:0,DMP成功解出欧拉角 // while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0){} //这个可以解决经常读不出数据的问题 // // // HAL_Delay(5);//注意调用不可以过于频繁HC_SR04_Read() ////MPU6050航向角 PID转向控制*****// // sprintf((char )Usart3String,"pitch:%.2f roll:%.2f yaw:%.2f\r\n",pitch,roll,yaw);//显示6050数据俯仰角横滚角航向角 // HAL_UART_Transmit(&huart3,( uint8_t )Usart3String,strlen(( const char )Usart3String),0xFFFF);//通过串口三输出字符 strlen:计算字符串大小 // // //mpu_dmp_get_data(&pitch,&roll,&yaw);//返回值:0,DMP成功解出欧拉角 // while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0){} //这个可以解决经常读不出数据的问题 // // // g_fMPU6050YawMovePidOut = PID_realize(&pidMPU6050YawMovement,yaw);//PID计算输出目标速度这个速度，会和基础速度加减 // g_fMPU6050YawMovePidOut1 = 1.5 + g_fMPU6050YawMovePidOut;//基础速度加减PID输出速度 // g_fMPU6050YawMovePidOut2 = 1.5 - g_fMPU6050YawMovePidOut; // if(g_fMPU6050YawMovePidOut1 >3.5) g_fMPU6050YawMovePidOut1 =3.5;//进行限幅 // if(g_fMPU6050YawMovePidOut1 <0) g_fMPU6050YawMovePidOut1 =0; // if(g_fMPU6050YawMovePidOut2 >3.5) g_fMPU6050YawMovePidOut2 =3.5; // if(g_fMPU6050YawMovePidOut2 <0) g_fMPU6050YawMovePidOut2 =0; // motorPidSetSpeed(g_fMPU6050YawMovePidOut1,g_fMPU6050YawMovePidOut2); ////避障功能// ////避障逻辑 // if(HC_SR04_Read() > 25)//前方无障碍物 // { // motorPidSetSpeed(1,1);//前运动 // HAL_Delay(100); // } // else{ //前方有障碍物 // motorPidSetSpeed(-1,1);//右边运动原地 // HAL_Delay(500); // if(HC_SR04_Read() > 25)//右边无障碍物 // { // motorPidSetSpeed(1,1);//前运动 // HAL_Delay(100); // } // else{//右边有障碍物 // motorPidSetSpeed(1,-1);//左边运动原地 // HAL_Delay(1000); // if(HC_SR04_Read() >25)//左边无障碍物 // { // motorPidSetSpeed(1,1);//前运动 // HAL_Delay(100); // } // else{ // motorPidSetSpeed(-1,-1);//后运动 // HAL_Delay(1000); // motorPidSetSpeed(-1,1);//右边运动 // HAL_Delay(50); // } // } // } ////无PID跟随功能// // if(HC_SR04_Read() > 25) // { // motorPidSetSpeed(1,1);//前运动 // HAL_Delay(100); // } // if(HC_SR04_Read() < 20) // { // motorPidSetSpeed(-1,-1);//后运动 // HAL_Delay(100); // } ////PID跟随功能*******// // g_fHC_SR04_Read=HC_SR04_Read();//读取前方障碍物距离 // if(g_fHC_SR04_Read < 60){ //如果前60cm 有东西就启动跟随 // g_fFollow_PID_Out = PID_realize(&pidFollow,g_fHC_SR04_Read);//PID计算输出目标速度这个速度，会和基础速度加减 // if(g_fFollow_PID_Out > 6) g_fFollow_PID_Out = 6;//对输出速度限幅 // if(g_fFollow_PID_Out < -6) g_fFollow_PID_Out = -6; // motorPidSetSpeed(g_fFollow_PID_Out,g_fFollow_PID_Out);//速度作用与电机上 // } // else motorPidSetSpeed(0,0);//如果前面60cm 没有东西就停止 // HAL_Delay(10);//读取超声波传感器不能过快 // ANO_DT_Send_F2(Motor1Speed100, 3.0100,Motor2Speed100,3.0100);//向匿名上位机通过F2帧发送4个int16类型的数据曲线显示 // if(Usart_WaitReasFinish() == 0)//是否接收完毕 // { // cJsonData = cJSON_Parse((const char )Usart1_ReadBuf); // if(cJSON_GetObjectItem(cJsonData,"p") !=NULL) // { // cJsonVlaue = cJSON_GetObjectItem(cJsonData,"p"); // p = cJsonVlaue->valuedouble; // pidMotor1Speed.Kp = p; // } // if(cJSON_GetObjectItem(cJsonData,"i") !=NULL) // { // cJsonVlaue = cJSON_GetObjectItem(cJsonData,"i"); // i = cJsonVlaue->valuedouble; // pidMotor1Speed.Ki = i; // } // if(cJSON_GetObjectItem(cJsonData,"d") !=NULL) // { // cJsonVlaue = cJSON_GetObjectItem(cJsonData,"d"); // d = cJsonVlaue->valuedouble; // pidMotor1Speed.Kd = d; // } // if(cJSON_GetObjectItem(cJsonData,"a") !=NULL) // { // // cJsonVlaue = cJSON_GetObjectItem(cJsonData,"a"); // a = cJsonVlaue->valuedouble; // pidMotor1Speed.target_val =a; // } // if(cJSON_GetObjectItem(cJsonData,"b") !=NULL) // { // // cJsonVlaue = cJSON_GetObjectItem(cJsonData,"b"); // b = cJsonVlaue->valuedouble; // pidMotor2Speed.target_val =b; // } // if(cJsonData != NULL){ // cJSON_Delete(cJsonData);//释放空间、但是不能删除cJsonVlaue不然会出现异常错误 // } // memset(Usart1_ReadBuf,0,255);//清空接收buf，注意这里不能使用strlen // } // printf("P:%.3f I:%.3f D:%.3f A:%.3f\r\n",p,i,d,a); // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 10); // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 10); // HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET); // HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET); // uint8_t c_Data[] = "串口输出测试:好家伙VCC\r\n"; // HAL_UART_Transmit(&huart1,c_Data,sizeof(c_Data),0xFFFF); // HAL_Delay(1000); // printf("printf:好家伙VCC测试\r\n"); // HAL_GPIO_TogglePin(LED_GPIO_Port,LED_Pin); // HAL_Delay(500); } / USER CODE END 3 */ } /** * @brief System Clock Configuration * @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; RCC_PeriphCLKInitTypeDef PeriphClkInit = {0}; /** Initializes the RCC Oscillators according to the specified parameters * in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks / RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { Error_Handler(); } PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC; PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV6; if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) { Error_Handler(); } } / USER CODE BEGIN 4 / / USER CODE END 4 */ /** * @brief This function is executed in case of error occurrence. * @retval None / void Error_Handler(void) { / USER CODE BEGIN Error_Handler_Debug / / User can add his own implementation to report the HAL error return state / __disable_irq(); while (1) { } / USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** * @brief Reports the name of the source file and the source line number * where the assert_param error has occurred. * @param file: pointer to the source file name * @param line: assert_param error line source number * @retval None / void assert_failed(uint8_t file, uint32_t line) { /* USER CODE BEGIN 6 / / User can add his own implementation to report the file name and line number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) / / USER CODE END 6 / } #endif / USE_FULL_ASSERT */ #include "motor.h" #include "tim.h" #include "pid.h" #define MAX_SPEED_UP 3 extern float Motor1Speed ; extern float Motor2Speed ; extern tPid pidMotor1Speed; extern tPid pidMotor2Speed; float motorSpeedUpCut = 0.5;//加减速速度变量 void Motor_Set(int Motor1,int Motor2) { //取反更适合控制习惯，大于0小车向前运动 Motor1 =-Motor1; Motor2 =-Motor2; //1.先根据正负设置方向GPIO 高低电平 if(Motor1 <0) BIN1_SET; else BIN1_RESET; if(Motor2 <0) AIN1_SET; else AIN1_RESET; //2.然后设置占空比 if(Motor1 <0) { if(Motor1 <-99) Motor1 =-99; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (100+Motor1)); } else { if(Motor1 >99) Motor1 = 99; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,Motor1); } if(Motor2<0) { if(Motor2 <-99) Motor2=-99; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, (100+Motor2)); } else { if(Motor2 >99) Motor2 =99; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, Motor2); } } /******************* * @brief 通过赋值PID目标速度控制小车运动 * @param Motor1SetSpeed:电机1目标速度 Motor2SetSpeed:电机2目标速度 * @return 无 * motorPidSetSpeed(1,2);//右边运动 motorPidSetSpeed(2,1);//左边运动 motorPidSetSpeed(1,1);//前运动 motorPidSetSpeed(-1,-1);//后运动 motorPidSetSpeed(-1,1); //向右原地旋转 motorPidSetSpeed(1,-1); //向左原地旋转 *******************/ void motorPidSetSpeed(float Motor1SetSpeed,float Motor2SetSpeed) { //设置Pid目标转速 pidMotor1Speed.target_val=Motor1SetSpeed; pidMotor2Speed.target_val=Motor2SetSpeed; //PID计算控制电机 Motor_Set(PID_realize(&pidMotor1Speed,Motor1Speed),PID_realize(&pidMotor2Speed,Motor2Speed)); } //向前加速函数 void motorPidSpeedUp(void) { if(motorSpeedUpCut <= MAX_SPEED_UP) motorSpeedUpCut +=0.5;//如果没有超过最大值就增加0.5 motorPidSetSpeed(motorSpeedUpCut,motorSpeedUpCut);//设置到电机 } //向前减速函数 void motorPidSpeedCut(void) { if(motorSpeedUpCut >=0.5)motorSpeedUpCut-=0.5;//判断是否速度太小 motorPidSetSpeed(motorSpeedUpCut,motorSpeedUpCut);//设置到电机 } #include "pid.h" //定义一个结构体类型变量 tPid pidMotor1Speed;//电机1速度PID闭环参数 tPid pidMotor2Speed;//电机2速度PID闭环参数 tPid pidHW_Tracking;//红外循迹的PID结构体类型变量 tPid pidFollow; //定距离跟随PID结构体类型变量 tPid pidMPU6050YawMovement; //利用6050偏航角进行姿态控制的PID //给结构体类型变量赋初值 void PID_init() { pidMotor1Speed.actual_val=0.0; pidMotor1Speed.target_val=0.00; pidMotor1Speed.err=0.0; pidMotor1Speed.err_last=0.0; pidMotor1Speed.err_sum=0.0; pidMotor1Speed.Kp=15; pidMotor1Speed.Ki=5; pidMotor1Speed.Kd=0; pidMotor2Speed.actual_val=0.0; pidMotor2Speed.target_val=0.00; pidMotor2Speed.err=0.0; pidMotor2Speed.err_last=0.0; pidMotor2Speed.err_sum=0.0; pidMotor2Speed.Kp=15; pidMotor2Speed.Ki=5; pidMotor2Speed.Kd=0; pidHW_Tracking.actual_val=0.0; pidHW_Tracking.target_val=0.00;//红外循迹PID 的目标值为0 pidHW_Tracking.err=0.0; pidHW_Tracking.err_last=0.0; pidHW_Tracking.err_sum=0.0; pidHW_Tracking.Kp=-1.50; pidHW_Tracking.Ki=0; pidHW_Tracking.Kd=0.80; pidFollow.actual_val=0.0; pidFollow.target_val=22.50;//定距离跟随目标距离22.5cm pidFollow.err=0.0; pidFollow.err_last=0.0; pidFollow.err_sum=0.0; pidFollow.Kp=-0.5;//定距离跟随的Kp大小通过估算PID输入输出数据，确定大概大小，然后在调试 pidFollow.Ki=-0.001;//Ki小一些 pidFollow.Kd=0; pidMPU6050YawMovement.actual_val=0.0; pidMPU6050YawMovement.target_val=0.00;//设定姿态目标值 pidMPU6050YawMovement.err=0.0; pidMPU6050YawMovement.err_last=0.0; pidMPU6050YawMovement.err_sum=0.0; pidMPU6050YawMovement.Kp=0.02;//6050航向角PID运动控制 pidMPU6050YawMovement.Ki=0; pidMPU6050YawMovement.Kd=0.1; } //比例p调节控制函数 float P_realize(tPid * pid,float actual_val) { pid->actual_val = actual_val;//传递真实值 pid->err = pid->target_val - pid->actual_val;//当前误差=目标值-真实值 //比例控制调节输出=Kp当前误差 pid->actual_val = pid->Kppid->err; return pid->actual_val; } //比例P 积分I 控制函数 float PI_realize(tPid * pid,float actual_val) { pid->actual_val = actual_val;//传递真实值 pid->err = pid->target_val - pid->actual_val;//当前误差=目标值-真实值 pid->err_sum += pid->err;//误差累计值 = 当前误差累计和 //使用PI控制输出=Kp当前误差+Ki误差累计值 pid->actual_val = pid->Kppid->err + pid->Kipid->err_sum; return pid->actual_val; } // PID控制函数 float PID_realize(tPid * pid,float actual_val) { pid->actual_val = actual_val;//传递真实值 pid->err = pid->target_val - pid->actual_val;////当前误差=目标值-真实值 pid->err_sum += pid->err;//误差累计值 = 当前误差累计和 //使用PID控制输出 = Kp当前误差 + Ki误差累计值 + Kd(当前误差-上次误差) pid->actual_val = pid->Kppid->err + pid->Kipid->err_sum + pid->Kd(pid->err - pid->err_last); //保存上次误差: 这次误差赋值给上次误差 pid->err_last = pid->err; return pid->actual_val; } 提取所有有关于红外循迹的代码部分，并将代码分别分装在pid，sensor，motor（c文件和h文件）

#include "stm32f10x.h" uint8_t read_sensors(void); // 读取传感器值 #endif sensor.c: c #include "sensor.h" uint8_t read_sensors(void) { // 实现读取红外传感器的代码 // 例如：读取GPIO或ADC...

写一个使用STM32F103RCT6的pwm控制步进电机速度驱动程序

#include "stm32f10x.h" #define PWM_PERIOD 2000 // PWM周期为2000个时钟周期 #define MOTOR_STEPS 200 // 步进电机一圈有200步 #define MOTOR_SPEED 50 // 步进电机速度为50rpm uint16_t pwm_duty = 1000; // ...

stm32f103c8t6电机PID调速代码

#include "stm32f10x.h" #define PWM_FREQ 10000 // PWM频率 #define MOTOR_PPR 1000 // 电机每转脉冲数 // PID控制参数 #define KP 1.0f // 比例系数 #define KI 0.5f // 积分系数 #define KD 0.2f // 微分系数 ...

写出stm32f103c8t6使用tb6612的代码

#include "stm32f10x.h" #define LED_PIN GPIO_Pin_13 #define LED_GPIO_PORT GPIOC #define LED_GPIO_CLK RCC_APB2Periph_GPIOC #define PWM_FREQ 20000 // PWM输出频率 #define PWM_PERIOD ((SystemCoreClock / ...

Stm 32c8t6，驱动模块是TB 6612 fng，七路灰度寻迹小车

#include "stm32f10x.h" #include "stm32f10x_gpio.h" #include "stm32f10x_tim.h" #include "stm32f10x_adc.h" #include "stm32f10x_rcc.h" // 定义TB6612控制引脚 #define AIN1_PIN GPIO_Pin_0 #define ...

stm32f103控制直流电动机

#include "stm32f10x.h" void TIM2_PWM_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; RCC_APB1...

stm32驱动520编码器电机

#include "stm32f10x.h" void Encoder_Init(void) { // 1. 配置GPIO（PA0/PA1） GPIO_InitTypeDef GPIO_InitStruct; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); GPIO_InitStruct.GPIO_Pin = GPIO_...

stm32f103ze循迹六路

示例代码（仅供参考）：*/#include"stm32f10x.h"#include"motor.h"//假设有一个motor.h，里面定义了电机控制函数//定义传感器引脚#defineS0_PINGPIO_Pin_0#defineS1_PINGPIO_Pin_1#defineS2_PINGPIO_Pin_2#defineS3_...

stm32f103rct6驱动步进电机

#include "stm32f10x.h" void GPIO_Configuration(void){ GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); // 启用GPIOA时钟 // 配置PA7作为PUL+ GPIO_...

相关推荐

STM32F10x.h

stm32串口通信学习0.rar_XBB_stm32f10x_conf.h_stm32f10x_lib.h_串口

I2C.zip_STM3210E_STM32F10X.h_stm32_eval.h_stm32_eval_i2c__stm32f

写一个使用STM32F103RCT6的pwm控制步进电机速度驱动程序

stm32f103c8t6电机PID调速代码

写出stm32f103c8t6使用tb6612的代码

Stm 32c8t6，驱动模块是TB 6612 fng，七路灰度寻迹小车

stm32f103控制直流电动机

stm32驱动520编码器电机

stm32f103ze循迹六路

stm32f103rct6驱动步进电机

大家在看

SAP实施顾问宝典中文版PDF

Frequency-comb-DPLL:数字锁相环软件，用于使用Red Pitaya锁定频率梳

MT2D 正演程序完整版

华为OLT MA5680T工具.zip

组装全局刚度矩阵：在 FEM 中组装是一项乏味的任务，这个 matlab 程序可以完成这项任务。-matlab开发

最新推荐

基于西门子S7-300 PLC的全自动生产线包装机设计与实现

Visual C++.NET编程技术实战指南

HarmonyOS内核深度探秘：优化自由行旅游系统的策略

tkinter模块所有控件

局域网五子棋游戏：娱乐与聊天的完美结合

自由行旅游新篇章：HarmonyOS技术融合与系统架构深度解析

足底支撑相到达73%是什么问题

宾馆预约系统开发与优化建议

HarmonyOS在旅游领域的创新：揭秘最前沿应用实践

数据架构师需要具备什么能力