stm32f103+减速电机
单片机源程序如下:
- /***************************************************************
- 自平衡程序
- ***************************************************************/
- #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度 电机停止
- {
- 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)//右转
- {
- pwmturn=10;
- pwmneed=0;
- }
- if(receive==0x03)//左转
- {
- 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 //外部中断计脉冲
- __interrupt void port1(void)
- {
-
- if(P1IFG&BIT0==BIT0)
- {
- P1IFG=0X00;
- if((P1IN&BIT1)==BIT1)
- i++;
- else
- i--;
- }
- }
- //定时器定时中断 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); //计算X角度a
-
- G_X=read_gyro(gyr_x); //计算X轴转动角速度
- erro[2]=erro[1];
- erro[1]=erro[0];
- balan_angle=blance_filter(balan_angle,A_X,G_X); //互补滤波
- erro[0]=balan_angle;
-
- speed=i; //计算速度
- speed=(int)(0.3*speed+0.7*speed1); //速度滤波
- speed1=speed;
- position+=speed; //计算出位置
- position+=pwmneed; //前进后退 积分
- ……………………
- …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码
所有资料51hei提供下载:
自平衡小车 - 直走 蓝牙.zip
(61.94 KB, 下载次数: 70)
|