|
|
- #include "stm32f10x.h"
- #include "led.h"
- #include "delay.h"
- #include "usart1.h"
- #include "135.h"
- #include "time.h"
- #include "moto.h"
- #include "adc.h"
- #include "PID.h"
- #include "inv_mpu.h"
- #include "inv_mpu_dmp_motion_driver.h"
- #include "mpu6050.h"
- /*使用的资源 老王2017/7/13 --倒立摆
- 串口1：TX--PA9 RX--PA10
- key：PE0 ~ 9 上拉输入
- LED: PD8 9 10 推挽输出
- OLED: 3.3v
- D0(SCLK)--PC0 | D1(SDIN)--PC1 | RST--PG15 | DS-- PD3| CS--PD6 | RST--PG15
- 角度传感器：PB0
- */
- //#define zero_angle 3195
- #define ok_angle 230//倒立时候的传感器的值
- //#define task2_start_angle 1250
- vu16 len=0;//串口1接收数据的长度
- u8 task_flag=0;
- extern int pwm;
- extern int flag;
- extern int number;
- short aacx,aacy,aacz; //加速度传感器原始数据 --三轴加速度
- short gyrox,gyroy,gyroz; //陀螺仪原始数据 --三轴角速度
- float pitch,roll,yaw; //欧拉角 x z y
- int adc_result;
- u8 dis_flag=0;
- void Init(void) // 初始化函数
- {
-
-
- delay_init();
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
- LED_Init();
- adc_config(); //传感器
- DC_motor_init();
- DC_Set_Pwm (0);
- time5_Init(9999,71); //1ms控制
- time4_Init(9999,71); //1ms控制
- LocPIDInit(); //位移式PID初始化
- LocPIDSet(ok_angle);//设定初值
- uart_init(112500);
- time3_PWM_Init(5000,71); //不分频。PWM频率=72000000/900=80Khz 越大却快(370-899)
- }
- int main()
- {
- Init();
- while(1)
- {
- int pwm;
- int right,left;
- int dig_right,dig_left;
-
- right=ok_angle+50;
- left=ok_angle-50;
- dig_right=ok_angle+500;
- dig_left=ok_angle-2000;
- adc_result = Get_Adc();
-
- if(adc_result>=dig_left && adc_result<=dig_right )
- {
- LocP_I_D_Set(10,0,10); //PID参数的赋予
- pwm=LocPIDCalc(adc_result);
- DC_Set_Pwm(pwm);
- if(pwm>999) pwm=999;
- else if(pwm<-999) pwm=-999;
- }
- if(adc_result>=left&& adc_result<=right) DC_Set_Pwm(0); //降低灵敏性
- if(adc_result>dig_right&& adc_result<4100) DC_Set_Pwm(0);
- else if(adc_result<dig_left) DC_Set_Pwm(0);
- printf("pwm==%d\r",pwm);
- printf(" adc==%d\r\n",adc_result);
- }
-
-
- /*void renwu_2(void)
- {
-
- while(1)
- {
-
- int right,left;
- int dig_right,dig_left;
-
- right=ok_angle+5;
- left=ok_angle-5;
- dig_right=ok_angle+330;
- dig_left=ok_angle-330;
- adc_result = Get_Adc();
-
- if((adc_result>0&&adc_result<dig_left)||(adc_result>dig_right&&adc_result<4100))
- {
- TIM_Cmd(TIM4, ENABLE); //使能TIM4
- if(flag==1) DC_Set_Pwm(610);
- else if(flag==2) DC_Set_Pwm(-610);
- }
- else if(adc_result>=dig_left && adc_result<=dig_right )
- {
- TIM_Cmd(TIM4, DISABLE); //关闭TIM4
- number=0;
- flag=0;
- LocP_I_D_Set(20,2.1,80); //PID参数的赋予
- pwm=LocPIDCalc(adc_result);
- DC_Set_Pwm(pwm);
- if(pwm>899) pwm=899;
- else if(pwm<-899) pwm=-899;
- if(adc_result>=left&& adc_result<=right) DC_Set_Pwm(0); //降低灵敏性
- }
- printf("adc==%d\r\n",adc_result);
- }
- }*/
- }
|
-
-
倒立摆.7z
231.61 KB, 下载次数: 9, 下载积分: 黑币 -5
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
