STM32倒立摆平衡源代码 - - Powered by Discuz!

#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);
}
}*/
}

复制代码