主要用到这些函数，会有一定的顺序来说大多数的函数，其中最后两个是抄的，不用过多解释。
下面是主函数，其中大多数被我注释掉了，因为主函数过多函数，电机会有延迟。
我这里蓝牙后期会改到，USART2中断中，以后会更新。
int main()
{
OLED_Init();//OLED
MySerial_Init();//蓝牙
MyMotor_Init();//电机
RMyEncoder_Init();//编码器
LMyEncoder_Init();//编码器
while(MPU_Init()!=0);//MPU6050
while(mpu_dmp_init()!=0)//DMP
{
}
OLED_ShowString(1,5,"Da:");
OLED_ShowString(2,5,"Sp:");
OLED_ShowString(3,5,"RE:");
OLED_ShowString(4,5,"LE:");
while(1)
{
mpu_dmp_get_data(&pitch,&roll,&yaw);
// OLED_ShowSignedNum(1,1,pitch,3);
// OLED_ShowSignedNum(2,1,roll,3);
// OLED_ShowSignedNum(3,1,yaw,3);
//
// if(USART_GetFlagStatus(USART1,USART_FLAG_RXNE) == SET)
// {
// MyDate=USART_ReceiveData(USART1);
// OLED_ShowHexNum(1,10,MyDate,5);
// }
// if(MyDate==1){Speed=720;}
// if(MyDate==2){Speed=0;}
// if(MyDate==3){Speed=2160;}
// if(MyDate==4){Speed=4320;}
// if(Speed==100){Speed=0;}
// if(MyDate==5){left_zhuan();}
// if(MyDate==6){right_zhuan();}
// MyMotor_SetSpeed(Speed);
// OLED_ShowSignedNum(2,10,Speed/72,5);
OLED_ShowSignedNum(3,10,REncoder_Get(),5);
OLED_ShowSignedNum(4,10,LEncoder_Get(),5);
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
measure = pitch; //roll测量值
calcu = zhongzhi; //roll理论值
velocity = ( LEncoder_Get() + REncoder_Get() )/2 ; //速度测量值 velocity = ( LEncoder_Get() + REncoder_Get() )/2
//PID计算：直立环+速度环
PWM = vertical_PID_value(measure, calcu) + velocity_PID_value(velocity);
PWM_Xianfu(7000,&PWM); //PWM限幅
SETPWM(PWM);
}
}
}1.OLED函数不多说，但是要注意他的引脚，
我这里用的PB14，和PB15。注意一下；
2.第二个函数说一下 蓝牙HC—05：
#include "stm32f10x.h" // Device header
//USART 蓝牙
uint8_t Date,MyserialDate,MyserialFalg;
void MySerial_Init()
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;//复用tuiwan输出,上拉输入
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;//复用tuiwan输出,上拉输入
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStructure);
USART_InitTypeDef USART_InitStructure;
USART_StructInit(&USART_InitStructure);
USART_InitStructure.USART_BaudRate = 9600;//波特率
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//硬件流kou
USART_InitStructure.USART_Mode = USART_Mode_Rx|USART_Mode_Tx;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_StopBits =USART_StopBits_1 ;
USART_InitStructure.USART_WordLength =USART_WordLength_8b ;//字长
USART_Init(USART1,&USART_InitStructure);
//
// USART_ITConfig(USART1,USART_IT_RXNE,ENABLE);
// NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//分组
//
// NVIC_InitTypeDef NVIC_InitStructure;
// NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
// NVIC_Init(&NVIC_InitStructure);
USART_Cmd(USART1,ENABLE);
}
uint8_t MySerial_GetRxFlag()//读后自动清除
{
if(MyserialFalg == 1)
{
MyserialFalg=0;
return 1;
}
return 0;
}
uint8_t MyReceiveData()
{
return MyserialDate;
}
（1）.开启RCC到GPIOA和USART1的时钟
（2）.配置TX，和RX这里可以不用配置TX，因为只有蓝牙到单片机的过程。
（3）.下来就是配置USART的相关配置，注意波特率
——其中硬件流控，我们不用他，我们的原理是，如果收到数据，标志位置1，不用自动。
一下是关于USART有关的函数：
1.USART_Init——初始化
typedef struct
{
uint32_t USART_BaudRate; /*!< This member configures the USART communication baud rate.
The baud rate is computed using the following formula:
- IntegerDivider = ((PCLKx) / (16 * (USART_InitStruct->USART_BaudRate)))
- FractionalDivider = ((IntegerDivider - ((u32) IntegerDivider)) * 16) + 0.5 */
uint16_t USART_WordLength; /*!< Specifies the number of data bits transmitted or received in a frame.
This parameter can be a value of @ref USART_Word_Length */
uint16_t USART_StopBits; /*!< Specifies the number of stop bits transmitted.
This parameter can be a value of @ref USART_Stop_Bits */
uint16_t USART_Parity; /*!< Specifies the parity mode.
This parameter can be a value of @ref USART_Parity
@note When parity is enabled, the computed parity is inserted
at the MSB position of the transmitted data (9th bit when
the word length is set to 9 data bits; 8th bit when the
word length is set to 8 data bits). */
uint16_t USART_Mode; /*!< Specifies wether the Receive or Transmit mode is enabled or disabled.
This parameter can be a value of @ref USART_Mode */
uint16_t USART_HardwareFlowControl; /*!< Specifies wether the hardware flow control mode is enabled
or disabled.
This parameter can be a value of @ref USART_Hardware_Flow_Control */
} USART_InitTypeDef;
USART_BaudRate——波特率
USART_WordLength——字节我们选择8位
USART_StopBits——停止位，我们选择一位，这样满足8位
USART_Parity——有无校验，我们无
USART_Mode——TX和RX我们可以选择，RX(我们只读)
USART_HardwareFlowControl——硬件流控（上面有有关解释）
USART总结一下：
先要理解一下总线:
（1）芯片总线：连接各个模块。用于各个模块的通信
（2）系统总线（内总线）：I2C，I2C需要外设，微控制器系统或智能仪器内部各模块，各器件之间传送信息的通道。
（3）通信总线：（外总线）,两个或多个系统之间的通信。
数据帧：信号传输时：有起始位，数据位，ji偶位，停止位。
波特率：异步通信要有相同的波特率，比特率越高，传输速度越快。
在32中，USART有五个串行通信接口，USART1在APB2总线中是72MHZ，其他在APB1中线中是36MHZ。
void USART_DeInit(USART_TypeDef* USARTx);
//USART初始化：根据USART初始化结构参数USART外设
void USART_Init(USART_TypeDef* USARTx, USART_InitTypeDef* USART_InitStruct);
//USART解除初始化//
void USART_StructInit(USART_InitTypeDef* USART_InitStruct);
//使用默认值填充USART初始化结构体成员
void USART_Cmd(USART_TypeDef* USARTx, FunctionalState NewState);
//允许或禁止USART外设
uint16_t USART_ReceiveData(USART_TypeDef* USARTx);
//返回USART外设最新接收的数据
void USART_SendData(USART_TypeDef* USARTx, uint16_t Data);
//通过USART外设发送当个数据
void USART_SendBreak(USART_TypeDef* USARTx);
//发送终止字符
void USART_ITConfig(USART_TypeDef* USARTx, uint16_t USART_IT, FunctionalState NewState);
//设置USART的某个中断请求是允许或禁止
ITStatus USART_GetITStatus(USART_TypeDef* USARTx, uint16_t USART_IT);
//获取USART中断状态
void USART_ClearITPendingBit(USART_TypeDef* USARTx, uint16_t USART_IT);
//清除USART某个挂起中断标志
FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint16_t USART_FLAG);
//获取标志状态：检测某个USART事件是否置位
void USART_ClearFlag(USART_TypeDef* USARTx, uint16_t USART_FLAG);
//挂起标志清除：清除某个USART事件挂起标志关于中断 NVIC：
USART_ITConfig(USART1,USART_IT_RXNE,ENABLE);//中断使能
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//分组（组别）
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//选择USART1中断
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//USART1使能
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;//抢占优先级
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;//响应优先级
NVIC_Init(&NVIC_InitStructure);组别优先顺序（第0组优先级最强，第4组优先级最弱）：NVIC_PriorityGroup_0>NVIC_PriorityGroup_1>NVIC_PriorityGroup_2>NVIC_PriorityGroup_3>NVIC_PriorityGroup_4 。
USART1_IRQHandler——中断函数，
void USART1_IRQHandler()//中断先判断标志位
{
if(USART_GetFlagStatus(USART1,USART_IT_RXNE)==SET)
{
MyserialDate=USART_ReceiveData(USART1);
MyserialFalg = 1;
USART_ClearITPendingBit(USART1,USART_IT_RXNE);
}
}
先判断USART1中断标志位，是否为“1”，如果有把收到了给 一个数据，然后返回。
下来说PWM控制电机，编码器的有关代码。
#include "stm32f10x.h" // Device header
void MyMotorPWM_Init()
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2|GPIO_Pin_3;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStructure);
TIM_InternalClockConfig(TIM2);//内部时钟
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision =TIM_CKD_DIV1 ;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 7200-1;//ARR
TIM_TimeBaseInitStructure.TIM_Prescaler = 36-1;//PSC
/*CCR=720=0.1
1440=0.2
*/
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OutputState =TIM_OutputState_Enable ;
TIM_OCInitStructure.TIM_Pulse = 0;//ccr
TIM_OC3Init(TIM2,&TIM_OCInitStructure);
TIM_OC4Init(TIM2,&TIM_OCInitStructure);
// TIM_ClearFlag(TIM2,TIM_FLAG_Update);//更新中断标志位清除，防止复位以后直接进入终端，计数从1开始计数
//
// TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);
//
// NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
//
// NVIC_InitTypeDef NVIC_InitStructure;
// NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;//抢占优先级
// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;//响应优先级
// NVIC_Init(&NVIC_InitStructure);
TIM_SetCompare3(TIM2,7200-0);
TIM_SetCompare4(TIM2,0);
TIM_Cmd(TIM2,ENABLE);
}
//改变CCR
void MyPWM_SetCompare3(uint16_t Compare)
{
TIM_SetCompare3(TIM2,Compare);
}
void MyPWM_SetCompare4(uint16_t Compare)
{
TIM_SetCompare4(TIM2,Compare);
}
void PWM_Xianfu(int max,int *PWM)
{
if(*PWM>max) *PWM = max;
if(*PWM<-max) *PWM =-max;
}
//===============================电机=================================//
//PA4,5,7,11
void MyMotor_Init()
{
MyMotorPWM_Init();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_1|GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStructure);
}
void MyMotor_SetSpeed(uint16_t Speed)
{
if(Speed>0)
{
GPIO_SetBits(GPIOA,GPIO_Pin_4);
GPIO_ResetBits(GPIOA,GPIO_Pin_5);//L
GPIO_SetBits(GPIOA,GPIO_Pin_1);
GPIO_ResetBits(GPIOA,GPIO_Pin_11);
MyPWM_SetCompare3(Speed);
MyPWM_SetCompare4(Speed);
}
else
{
GPIO_SetBits(GPIOA,GPIO_Pin_5);
GPIO_ResetBits(GPIOA,GPIO_Pin_4);
GPIO_SetBits(GPIOA,GPIO_Pin_11);
GPIO_ResetBits(GPIOA,GPIO_Pin_1);
MyPWM_SetCompare3(-Speed);
MyPWM_SetCompare4(-Speed);
}
}
//====================================编码器RTIM3CH1CH2（PA6，PA7） TIM4CH1 CH2（PB6，PB7）====================================//
void RMyEncoder_Init()
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
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);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStructure);
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 65535-1;//ARR
TIM_TimeBaseInitStructure.TIM_Prescaler = 1-1;//PSC
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
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_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInit(TIM3,&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
//TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInit(TIM3,&TIM_ICInitStructure);
TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//编码结构
// TIM_SetCounter(TIM3,0);//清零定时器计数值
// TIM_SetCounter(TIM4,0);//清零定时器计数值
TIM_Cmd(TIM3,ENABLE);
}
void LMyEncoder_Init()
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
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(GPIOB,&GPIO_InitStructure);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStructure);
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 65535-1;//ARR
TIM_TimeBaseInitStructure.TIM_Prescaler = 1-1;//PSC
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
//TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInit(TIM4,&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
//TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInit(TIM4,&TIM_ICInitStructure);
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//编码结构
TIM_Cmd(TIM4,ENABLE);
}
int16_t REncoder_Get()
{
int16_t MyCnt;
MyCnt=(short)TIM_GetCounter(TIM3);
TIM_SetCounter(TIM3,0);
return MyCnt;
}
int16_t LEncoder_Get()
{
int16_t MyCnt;
MyCnt=(short)TIM_GetCounter(TIM4);
TIM_SetCounter(TIM4,0);
return MyCnt;
}
//=======================================正反转==================//
void left_positive(void) //左轮正传
{
GPIO_SetBits(GPIOA,GPIO_Pin_1);
GPIO_ResetBits(GPIOA,GPIO_Pin_11);
}
void left_negative(void) //左轮反转
{
GPIO_SetBits(GPIOA,GPIO_Pin_11);
GPIO_ResetBits(GPIOA,GPIO_Pin_1);
}
void right_positive(void) //右轮正转
{
GPIO_SetBits(GPIOA,GPIO_Pin_4);
GPIO_ResetBits(GPIOA,GPIO_Pin_5);
}
void right_negative(void) //右轮反转
{
GPIO_SetBits(GPIOA,GPIO_Pin_5);
GPIO_ResetBits(GPIOA,GPIO_Pin_4);
}
//================================//
void left_zhuan()
{
left_negative();
right_positive();
MyPWM_SetCompare3(1440);
MyPWM_SetCompare4(1440);
}
void right_zhuan()
{
right_negative();
left_positive();
MyPWM_SetCompare3(20);
MyPWM_SetCompare4(20);
}
void SETPWM(int PWM)
{
if(PWM>0) //正转
{
left_positive();
right_positive();
MyPWM_SetCompare3(PWM);
MyPWM_SetCompare4(PWM);
}
else //反转
{
left_negative();
right_negative();
MyPWM_SetCompare3(-PWM);
MyPWM_SetCompare4(-PWM );
}
}
先说PWM的代码：
配置TIM2，和TIM2，OC
计算
TIM_InternalClockConfig(TIM2);//内部时钟
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OutputState =TIM_OutputState_Enable ;//用于设置定时器输出通道的使能状态
TIM_OCInitStructure.TIM_Pulse = 0;//ccr
TIM_OC3Init(TIM2,&TIM_OCInitStructure);
TIM_OC4Init(TIM2,&TIM_OCInitStructure);
TIM_SetCompare3(TIM2,7200-0);//写入ccr
TIM_SetCompare4(TIM2,0);//写入ccr
TIM_Cmd(TIM2,ENABLE); 
这里的限幅为了防止电机烧坏。有pwm限幅
反转GPIO口来，控制正反转，写入ccr的值，来控制PWM，电机转动。
下来是编码器：
利用输入捕获，只可以用到定时器CH1和CH2通道。
int16_t REncoder_Get()
{
int16_t MyCnt;
MyCnt=(short)TIM_GetCounter(TIM3);
TIM_SetCounter(TIM3,0);
return MyCnt;
}
int16_t LEncoder_Get()
{
int16_t MyCnt;
MyCnt=(short)TIM_GetCounter(TIM4);
TIM_SetCounter(TIM4,0);
return MyCnt;
}电机没啥的，也就是TIM2——PWM输出，TIM3,4——编码器输入捕获。
然后用TIM——GetCount（TIM3） 读取TIMx寄存器CNT中的计数值。
下来说一下PID
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
measure = pitch; //roll测量值
calcu = zhongzhi; //roll理论值
velocity = ( LEncoder_Get() + REncoder_Get() )/2 ; //速度测量值 velocity = ( LEncoder_Get() + REncoder_Get() )/2
//PID计算：直立环+速度环
PWM = vertical_PID_value(measure, calcu) + velocity_PID_value(velocity);
PWM_Xianfu(7000,&PWM); //PWM限幅
SETPWM(PWM);
}//ֱ//直立环：
int vertical_PID_value(float measure,float calcu)
{
err = measure - calcu; //误差
err_sum+=err; //误差的累加
err_difference = err - last_err; //误差的差值
last_err = err; //此次误差记录为“上次误差”
return Kp*err + Ki*err_sum + Kd*err_difference;
}
//速度环：
int velocity_PID_value(int velocity)
{
float a=0.3; //滤波系数（反映滤波程度）
filt_velocity = a*velocity + (1-a)*last_filt_velocity; //一阶速度滤波
velocity_sum += filt_velocity; //速度的累加
I_xianfu(10000); //累加限幅
last_filt_velocity = filt_velocity; //此次速度记录为“上次速度”
return VKp*filt_velocity + VKi*velocity_sum;
}
//I限幅：
void I_xianfu(int max)
{
if(velocity_sum>max) velocity_sum=max;
if(velocity_sum<-max) velocity_sum=-max;
}这里直立环P,是让小车，可以接近要求速度，但是因为会有摩擦力影响（Kp*误差），i是误差累计
上次误差加这一次的误差*Ki。D是让小车这次和上次的误差，所以说是一个负数。减小 小车大幅度震荡
下面是参数
扫一扫
8182
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
