stm32平衡車源碼

ID:402452 · 發表于 2018-11-25 20:06

單片機源程序如下:

#include "sys.h"
/**************************************************************************************************
程序作者：CUIT 3+1 Taocr
***************************************************************************************************/
extern u16 Gyro_y, Gyro_z;
u8 time_20ms, timer_5_flag, timer_10_flag;
u8 Turn_Off(float angle)
{
u8 temp;
if(angle<-40||angle>40)
{ //===傾角大于40度關閉電機
temp=1; //===Flag_Stop置1關閉電機
PWMA2 = 0;
PWMB2 = 0;
PWMA1 = 0;
PWMB1 = 0;
}
else
temp=0;
return temp;
}
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral;
float kp=300,ki=1.5;
//=============遙控前進后退部分=======================//
// if(1==Flag_Qian) Movement=90/Flag_sudu; //===如果前進標志位置1 位移為負
// else if(1==Flag_Hou) Movement=-90/Flag_sudu; //===如果后退標志位置1 位移為正
// else
Movement=0;
//=============速度PI控制器=======================//
Encoder_Least =(encoder_left+encoder_right)-0; //===獲取最新速度偏差==測量速度（左右編碼器之和）-目標速度（此處為零）
Encoder *= 0.8; //===一階低通濾波器
Encoder += Encoder_Least*0.2; //===一階低通濾波器
Encoder_Integral +=Encoder; //===積分出位移積分時間：10ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遙控器數據，控制前進后退
if(Encoder_Integral>10000) Encoder_Integral=10000; //===積分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===積分限幅
Velocity=Encoder*kp+Encoder_Integral*ki; //===速度控制
if(Turn_Off(Pitch)==1) Encoder_Integral=0; //===電機關閉后清除積分
return Velocity;
}
int turn(int encoder_left,int encoder_right,float gyro)//轉向控制
{
static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
float Turn_Amplitude=88/2,Kp=42,Kd=0;
//=============遙控左右旋轉部分=======================//
// if(1==Flag_Left||1==Flag_Right) //這一部分主要是根據旋轉前的速度調整速度的起始速度，增加小車的適應性
// {
// if(++Turn_Count==1)
// Encoder_temp=myabs(encoder_left+encoder_right);
// Turn_Convert=50/Encoder_temp;
// if(Turn_Convert<0.6)Turn_Convert=0.6;
// if(Turn_Convert>3)Turn_Convert=3;
// }
// else
// {
Turn_Convert=0.9;
Turn_Count=0;
Encoder_temp=0;
// }
// if(1==Flag_Left) Turn_Target-=Turn_Convert;
// else if(1==Flag_Right) Turn_Target+=Turn_Convert;
// else
Turn_Target=0;
if(Turn_Target>Turn_Amplitude) Turn_Target=Turn_Amplitude; //===轉向速度限幅
if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
// if(Flag_Qian==1||Flag_Hou==1) Kd=1;
// else
Kd=0; //轉向的時候取消陀螺儀的糾正有點模糊PID的思想
//=============轉向PD控制器=======================//
Turn=-Turn_Target*Kp -gyro*Kd; //===結合Z軸陀螺儀進行PD控制
return Turn;
}
int main(void)
{
int Speed_R, Speed_L;
int Motol_PWM, Moto2_PWM;
int flag = 0;
u8 Right_Flag = 0;
delay_init(); //延時初始化
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);// 設置中斷優先級分組2
LED_Init();
LCD_init();//OLED初始化
LCD_clear();//顯示屏清屏
PWM_Init(7199,0);
Encoder_Init_TIM3();
Encoder_Init_TIM2();
uart_init(115200); //初始化串口1
IIC_Init(); //模擬IIC初始化
MPU6050_initialize(); //=====MPU6050初始化
DMP_Init(); //初始化DMP
// NRF24L01_Init(); //初始化NRF24L01
//while(NRF24L01_Check()) //檢查NRF24L01是否在位.
// {
// delay_ms(200);
// delay_ms(200);
// }
printf("NRF24L01Ready!");
Timer1_Init(29,7199);
while(1)
{
if(time_20ms == 10)
{
time_20ms = 0;
printf("X軸傾角%f Y軸傾角%d 轉向角%d \r\n",Pitch,Speed_R,Speed_L);//向上位機發送數據
show_size8float4_2f(0,0,Roll);
show_size8float4_2f(0,4,Right_Flag);
time_20ms = 0;}
if(timer_5_flag == 1)
{
timer_5_flag = 0;
timer_10_flag = !timer_10_flag;
// if(timer_10_flag == 0)
// {
Read_DMP();
// }
Speed_R = Read_Encoder(2);
Speed_L = Read_Encoder(3);
Motol_PWM = 650*(Roll-5)+0.5*gyro[0]-velocity(Speed_L,Speed_R);//+turn(Speed_L,Speed_R,gyro[2]);
Moto2_PWM = 200*(Pitch)+1.5*gyro[1];//-velocity(Speed_L,Speed_R);//-turn(Speed_L,Speed_R,gyro[2]);
if(Roll>15&&flag<10000){Motol_PWM = 5000;flag++;}
if(Roll<-15&&flag<10000){Motol_PWM = -5000;flag++;}
if(Motol_PWM>=0)
{
if(Right_Flag == 0)
Right_Flag = 1;
PWMA1 = Motol_PWM;
PWMB1 = Motol_PWM;
PWMA2 = 1;
PWMB2 = 1;
}
else
{
if(Right_Flag == 0)
Right_Flag = 2;
PWMA2 = -Motol_PWM;
PWMB2 = -Motol_PWM;
PWMA1 = 1;
PWMB1 = 1;
}
}
if(Turn_Off(Pitch)==1){}
}
}
int TIM1_UP_IRQHandler(void)
{
if(TIM1->SR&0X0001)//5ms定時中斷
{
TIM1->SR&=~(1<<0); //===清除定時器1中斷標志位
time_20ms++;
timer_5_flag = 1;
}
return 0;
}

復制代碼

所有資料51hei提供下載:

平衡車源碼.rar (393.04 KB, 下載次數: 25)

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

stm32平衡車源碼