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)
2018-6-30 21:18 上傳
點擊文件名下載附件
|
