小车直立环加上速度环的 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.26; //-30 -50 -60/
float pid_out;
void balance()
{
bias=Pitch-0;//xÖáµÄ½Ç¶È±ä»¯µÃ¼ÆËãÖ±Á¢Æ«²î
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) // Ò»½×µÍͨÂ˲¨
{
speed_sudu =(countl+ countr)*0.5; //ËÙ¶ÈÆ«²î
speed *= 0.7; //
speed += speed_sudu*0.3;
zuh += speed; //ËÙ¶ÈÆ«²î»ý·Ö
//zuh += qianhou;//µÄΪÒÆÖ²
if(zuh>80000)
zuh=80000;
if(zuh<-80000)
zuh=-80000;
} |
楼主
标题置顶
标题高亮
