Forest D1 STM32 旋转倒立摆源码 电子竞赛

【平衡小车之家】Forest D1 STM32 旋转倒立摆STM32源码 电子竞赛

#include “control.h“		
  /**************************************************************************
作者：Sam
我的淘宝小店：客户满意使我们的服务宗旨
**************************************************************************/
int Balance_PwmPosition_Pwm;
u8 Flag_TargetPosition_Target;
/**************************************************************************
函数功能：所有的控制代码都在这里面
          TIM1控制的5ms定时中断 
**************************************************************************/
int TIM1_UP_IRQHandler（void）  
{    
	if（TIM1->SR&0X0001）//5ms定时中断
	{   
		  TIM1->SR&=~（1<<0）;                                       //===清除定时器1中断标志位	                     
	     if（delay_flag==1）
			 {
				 if（++delay_50==10）	 delay_50=0delay_flag=0;          //===给主函数提供50ms的精准延时
			 }		
    	Encoder=Read_Encoder（4）;             	                   //===更新编码器位置信息
      Angle_Balance=Get_Adc_Average（315）;                     //===更新姿态	
     	Balance_Pwm =balance（Angle_Balance）;                                          //===角度PD控制	
	    if（++Position_Target>4）	Position_Pwm=Position（Encoder）Position_Target=0;     //===位置PD控制 25ms进行一次位置控制
      Moto=Balance_Pwm-Position_Pwm;        //===计算电机最终PWM
		  Xianfu_Pwm（）;                         //===PWM限幅 反正占空比100%带来的系统不稳定因素
		  if（Turn_Off（Voltage）==0）              //===低压和倾角过大保护
			Set_Pwm（Moto）;                        //===赋值给PWM寄存器
	  	Led_Flash（100）;                       //===LED闪烁指示系统正常运行 
	    Voltage=Get_battery_volt（）;           //===获取电池电压	      
			Key（）;                                //===扫描按键变化
	}       	
	 return 0;	  
} 

/**************************************************************************
函数功能：倾角PD控制
入口参数：角度
返回  值：倾角控制PWM
作    者：平衡小车之家
**************************************************************************/
int balance（float Angle）
{  
   float Bias;                       //倾角偏差
	 static float Last_BiasD_Bias;    //PID相关变量
	 int balance;                      //PWM返回值 
	 Bias=Angle-ZHONGZHI;              //求出平衡的角度中值 和机械相关
	 D_Bias=Bias-Last_Bias;            //求出偏差的微分 进行微分控制
	 balance=-Balance_KP*Bias-D_Bias*Balance_KD;   //===计算倾角控制的电机PWM  PD控制
   Last_Bias=Bias;                   //保持上一次的偏差
	 return balance;
}

/**************************************************************************
函数功能：位置PD控制 
入口参数：编码器
返回  值：位置控制PWM
作    者：平衡小车之家
**************************************************************************/
int Position（int Encoder）
{  
   static float Position_PWMLast_PositionPosition_BiasPosition_Differential;
	 static float Position_Least;
  	Position_Least =Encoder-Position_Zero;             //===
    Position_Bias *=0.8;		   
    Position_Bias += Position_Least*0.2;	             //===一阶低通滤波器  
	  Position_Differential=Position_Bias-Last_Position;
	  Last_Position=Position_Bias;
		Position_PWM=Position_Bias*Position_KP+Position_Differential*Position_KD; //===速度控制	
	  return Position_PWM;
}

/***************************************************

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-03-22 10:34  【平衡小车之家】Forest D1 旋转倒立摆源码\
     目录           0  2017-03-22 10:34  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\
     目录           0  2018-04-20 09:27  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\
     文件           0  2016-02-22 11:47  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\MFC10DB.tmp
     文件           0  2016-02-23 08:58  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\MFC3A5E.tmp
     文件        5604  2018-04-20 09:27  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\control.c
     文件       11154  2016-02-22 15:13  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\control.c~RF18b9998.TMP
     文件         574  2016-04-23 13:57  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\control.h
     目录           0  2017-03-22 10:34  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DMP\
     文件       19342  2012-12-14 11:16  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DMP\dmpKey.h
     文件        6765  2012-12-14 11:16  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DMP\dmpmap.h
     文件       84104  2015-07-09 14:05  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DMP\inv_mpu.c
     文件        4335  2015-07-09 14:05  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DMP\inv_mpu.h
     文件       57839  2015-07-09 14:05  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DMP\inv_mpu_dmp_motion_driver.c
     文件        3386  2015-07-09 14:05  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DMP\inv_mpu_dmp_motion_driver.h
     目录           0  2017-03-22 10:34  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DataScope_DP\
     文件        2891  2015-07-09 14:05  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DataScope_DP\DataScope_DP.C
     文件         586  2015-07-09 14:05  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\DataScope_DP\DataScope_DP.h
     目录           0  2017-03-22 10:34  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\MPU6050\
     文件       11099  2015-12-04 10:34  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\MPU6050\MPU6050.c
     文件       14787  2015-10-17 15:04  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\MPU6050\mpu6050.h
     目录           0  2017-03-22 10:34  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\filter\
     文件        2105  2015-12-07 15:52  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\filter\filter.c
     文件         399  2015-12-07 15:52  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\filter\filter.h
     目录           0  2018-04-20 09:27  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\show\
     文件        4978  2018-04-20 09:27  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\show\show.c
     文件         332  2016-04-19 16:22  【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\show\show.h
     目录           0  2017-03-22 10:34  【平衡小车之家】Forest D1 旋转倒立摆源码\HARDWARE\
     目录           0  2018-04-20 09:27  【平衡小车之家】Forest D1 旋转倒立摆源码\HARDWARE\ADC\
     文件        4587  2018-04-20 09:27  【平衡小车之家】Forest D1 旋转倒立摆源码\HARDWARE\ADC\adc.c
     文件         487  2017-12-17 15:51  【平衡小车之家】Forest D1 旋转倒立摆源码\HARDWARE\ADC\adc.h
............此处省略161个文件信息