stm32两轮平衡小车速度环
时间:10-02
整理:3721RD
点击:
小车直立环很稳定 加上速度环的p都是可以让小车静止不到 但是 加上速度环 I后 车就不能静止 车会自己向单方向积分 单方向倒
#include "pid.h"
#include "pi.h"
#include "sys.h"
#include "stm32f10x_tim.h"
#include "led.h"
#include "usart.h"
#include "mpu6050.h"
#include "timer.h"
#include "inv_mpu.h"
#include "delay.h"
int bianma_zuo, bianma_you;
float Velocity,Encoder_Least,Encoder,Movement;
float Encoder_Integral;
extern float Pitch,Roll,Yaw;
int Moto1,Moto2;
int countl,countr;
float speed_sudu;
float speed;
float zuh;
float qianhou;//?????°??μ?á?
float bias, kp=60, kd=0.249; //96
float ks=-55,ki=0.01; //-30 -50 -60/
float pid_out;
void balance()
{
bias=Pitch-0;//x?áμ????è±??ˉμ??????±á¢??2?
pid_out=kp*bias+gyro[1]*kd+ks*speed+zuh*ki; //?????±á¢pwm
Moto1=pid_out;
Moto2=pid_out;
jis(Moto1,Moto2);
}
void jis(float zuo_pwm,float you_pwm)
{
if(Pitch<-60||Pitch>60)
{
zuo_pwm=0; you_pwm=0;
IN1=0; IN2=0; IN3=0; IN4=0;
}
if(zuo_pwm>0)
{
zuof();
zuo_pwm=zuo_pwm;
}
else
{
zuoz();
zuo_pwm=-zuo_pwm;
}
if(zuo_pwm>1000)
{
zuo_pwm=1000;
}
if(you_pwm>0)
{
youf();
you_pwm=you_pwm;
}
else
{
youz();
you_pwm=-you_pwm;
}
if(you_pwm>1000)
{ you_pwm=1000; }
/*if(zuo_pwm==600)
{
zuoz();
}
if(you_pwm==600)[
{
youz();
}*/
/* TIM_SetCompare1(TIM1,zuo_pwm+290);
TIM_SetCompare3(TIM1,you_pwm+270);*/
TIM_SetCompare1(TIM1,zuo_pwm+290); //300 290
TIM_SetCompare3(TIM1,you_pwm+235); //235
}
void Psn_Calcu(void) // ò??×μíí¨??2¨
{
speed_sudu =(countl+ countr)*0.5; //?ù?è??2?
speed *= 0.7; //
speed += speed_sudu*0.3;
zuh += speed; //?ù?è??2??y·?
//zuh += qianhou;//μ??aò??2
if(zuh>80000)
zuh=80000;
if(zuh<-80000)
zuh=-80000;
}
#include "pid.h"
#include "pi.h"
#include "sys.h"
#include "stm32f10x_tim.h"
#include "led.h"
#include "usart.h"
#include "mpu6050.h"
#include "timer.h"
#include "inv_mpu.h"
#include "delay.h"
int bianma_zuo, bianma_you;
float Velocity,Encoder_Least,Encoder,Movement;
float Encoder_Integral;
extern float Pitch,Roll,Yaw;
int Moto1,Moto2;
int countl,countr;
float speed_sudu;
float speed;
float zuh;
float qianhou;//?????°??μ?á?
float bias, kp=60, kd=0.249; //96
float ks=-55,ki=0.01; //-30 -50 -60/
float pid_out;
void balance()
{
bias=Pitch-0;//x?áμ????è±??ˉμ??????±á¢??2?
pid_out=kp*bias+gyro[1]*kd+ks*speed+zuh*ki; //?????±á¢pwm
Moto1=pid_out;
Moto2=pid_out;
jis(Moto1,Moto2);
}
void jis(float zuo_pwm,float you_pwm)
{
if(Pitch<-60||Pitch>60)
{
zuo_pwm=0; you_pwm=0;
IN1=0; IN2=0; IN3=0; IN4=0;
}
if(zuo_pwm>0)
{
zuof();
zuo_pwm=zuo_pwm;
}
else
{
zuoz();
zuo_pwm=-zuo_pwm;
}
if(zuo_pwm>1000)
{
zuo_pwm=1000;
}
if(you_pwm>0)
{
youf();
you_pwm=you_pwm;
}
else
{
youz();
you_pwm=-you_pwm;
}
if(you_pwm>1000)
{ you_pwm=1000; }
/*if(zuo_pwm==600)
{
zuoz();
}
if(you_pwm==600)[
{
youz();
}*/
/* TIM_SetCompare1(TIM1,zuo_pwm+290);
TIM_SetCompare3(TIM1,you_pwm+270);*/
TIM_SetCompare1(TIM1,zuo_pwm+290); //300 290
TIM_SetCompare3(TIM1,you_pwm+235); //235
}
void Psn_Calcu(void) // ò??×μíí¨??2¨
{
speed_sudu =(countl+ countr)*0.5; //?ù?è??2?
speed *= 0.7; //
speed += speed_sudu*0.3;
zuh += speed; //?ù?è??2??y·?
//zuh += qianhou;//μ??aò??2
if(zuh>80000)
zuh=80000;
if(zuh<-80000)
zuh=-80000;
}