倒立摆资料

倒立摆淘宝商家给的资料，源码，基于STM32F103VET6单片机，寄存器开发

#include “control.h“		
  /**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
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  2018-11-07 23:16  倒立摆资料\
     目录           0  2018-11-07 23:16  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆原理图\
     文件     5791232  2016-03-31 11:28  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆原理图\Forest S1 PCB封装.PcbLib
     文件      521664  2016-03-25 10:23  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆原理图\Forest S1 STM32最小系统原理图（倒立摆控制板）.pdf
     文件        5632  2016-03-31 11:27  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆原理图\Forest S1最小系统原理图封装.SCHLIB
     文件      413037  2017-11-15 19:12  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆原理图\底板原理图.pdf
     目录           0  2018-11-07 23:17  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆源码\
     目录           0  2018-11-07 23:16  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\
     目录           0  2018-11-07 23:16  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\
     文件        5621  2017-12-17 15:51  倒立摆资料\【平衡小车之家】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  2016-02-22 11:47  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\MFC10DB.tmp
     文件           0  2016-02-23 08:58  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\CONTROL\MFC3A5E.tmp
     目录           0  2018-11-07 23:16  倒立摆资料\【平衡小车之家】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  2018-11-07 23:16  倒立摆资料\【平衡小车之家】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  2018-11-07 23:16  倒立摆资料\【平衡小车之家】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-11-07 23:16  倒立摆资料\【平衡小车之家】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  2018-11-07 23:16  倒立摆资料\【平衡小车之家】Forest D1 旋转倒立摆源码\BALANCE\show\
............此处省略162个文件信息