国产精品一区二区小说,久久精品欧美一区,成人软件在线观看

wwwxxx国产_337p日本欧洲亚洲大胆张筱雨_免费在线看成人av_日本黄色不卡视频_国产精品成熟老女人_99视频一区_亚洲精品97久久中文字幕_免费精品视频在线_亚洲色图欧美视频_欧美一区二三区

/***************************************************************
自平衡程序
***************************************************************/
#include "main.h"
#define position_max 6000 //位置最大值
int j=0;
int position=0;
int k=0;
int i=0;
int speed1=0;
int speed=0;
int acc_y=0;
int acc_z=0;
int gyr_x=0;
int receive=0;
int pwm=0;
float G_X=0,A_X=0;
float gyro_angle=0;
float balan_angle=0;
float balan_angle1=0;
float G_X1=0,A_X1=0,G_X2=0,G_X3=0,GXERRO=0;
float erro[3];
int pwmneed=0;
int pwmturn=0;
void adjust(void);
void judge()
{
pwm=PID_control(erro[0],G_X)+PID_control_2(speed,position);
//pwm+=pwmneed;
if(pwm>100) //pwm限幅
{
pwm=100;
}
if(pwm<-100)
{
pwm=-100;
}
moto_right(pwm+pwmturn);
moto_left(pwm-pwmturn);
if((balan_angle>45)||(balan_angle<(-45)))//角度大于45度電機(jī)停止
{
moto_stop();
}
}
void bluetooth()
{
while (!(IFG1 & URXIFG0));
receive=U0RXBUF;
if(receive==0x01) //直走
{
pwmneed=12;
pwmturn=0;
}
if(receive==0x02)//后退
{
pwmneed=-13;
pwmturn=0;
}
if(receive==0x04)//右轉(zhuǎn)
{
pwmturn=10;
pwmneed=0;
}
if(receive==0x03)//左轉(zhuǎn)
{
pwmturn=-10;
pwmneed=0;
}
if(receive==0x00)//停
{
pwmturn=0;
pwmneed=0;
}
}
void main(void)
{
WDTCTL=WDTPW+WDTHOLD;
init_all( );
InitMPU6050(); //初始化MPU6050
_EINT(); //開總中斷
while(1)
{
bluetooth();
}
}
#pragma vector=PORT1_VECTOR //外部中斷計(jì)脈沖
__interrupt void port1(void)
{
if(P1IFG&BIT0==BIT0)
{
P1IFG=0X00;
if((P1IN&BIT1)==BIT1)
i++;
else
i--;
}
}
//定時(shí)器定時(shí)中斷 10ms
#pragma vector=TIMERA0_VECTOR
__interrupt void Timer_A(void)
{
adjust();
}
void adjust(void)
{
acc_y=GetData(ACCEL_YOUT_H); //讀取三軸Y的值
acc_z=GetData(ACCEL_ZOUT_H); //讀取三軸Z的值
gyr_x=GetData(GYRO_XOUT_H); //讀取陀螺儀X的值
A_X=count_acc_angle(acc_y,acc_z); //計(jì)算X角度a
G_X=read_gyro(gyr_x); //計(jì)算X軸轉(zhuǎn)動(dòng)角速度
erro[2]=erro[1];
erro[1]=erro[0];
balan_angle=blance_filter(balan_angle,A_X,G_X); //互補(bǔ)濾波
erro[0]=balan_angle;
speed=i; //計(jì)算速度
speed=(int)(0.3*speed+0.7*speed1); //速度濾波
speed1=speed;
position+=speed; //計(jì)算出位置
position+=pwmneed; //前進(jìn)后退積分
……………………
…………限于本文篇幅余下代碼請(qǐng)從51黑下載附件…………

復(fù)制代碼