平衡车程序.doc

平衡车程序#include "" #include "" void AngleCalculate(); void AngleControl(); void SpeedControl(); void SpeedControlOutput(); void DirectionControl(); void DirectionControlOutput(); void MotorOutputr(); void MotorOutputl(); void pit_hander(); void delay(); void D(); d_get(); uint8 Normalized_U8(uint16); void init_all(); float speed_count=0; void Center_Calculate(); /*************** 角度控制参数值****************/ #define E_OFFSET 1750//1775//1788//1845//1870// 1830 //1850 // 加速度计零偏值#define GRAVITY_ANGLE_RATIO 180/1960 // 加速度计角度转换系数 1945 #define CAR_GYRO_RATIO_INT // 可调在 左右,陀螺仪 AD 值与角速度的比例系数#define GRAVITY_ADJUST_TIME_CONSTANT 1 // 修正系数#define ANGLE_CONTROL_P 12//20 // 可调, P值#define CAR_ANGLE_SET 0 // 平衡// 小车角度设置#define ANGLE_CONTROL_D // // 可调, D值 #define CAR_ANGLE_SPEED_SET 0 // 平衡// 小车角速度设置#define ANGLE_CONTROL_OUT_MAX 90 // 电机最大占空比#define ANGLE_CONTROL_OUT_MIN 0 // 电机最小占空比,死区电压#define GYROSCOPE_ANGLE_SIGMA_FREQUENCY //5ms /*************** 速度控制参数值****************/ #define CAR_SPEED_SET // // 速度设置#define SPEED_CONTROL_P 150 //20 //50 #define SPEED_CONTROL_I 1 // // #define SPEED_INTEGRAL_MAX 100 #define SPEED_INTEGRAL_MIN 1 /**************** 方向控制参数值********************/ #define DIRECTION_CONTROL_P 2//27 //25// 小了 3 大#define DIRECTION_CONTROL_D ////// //////// uint16 cy_gyro_zero,cx_gyro_zero; // 陀螺仪零偏值 int k,l=0,m=0; int accvalue, gyrovalue; uint16 Gyro_ad_y,Acc_ad,Gyro_ad_x; // 陀螺仪加速度计//AD 采集 int g_n1MsEventCount=0,g_fGravityAngle=0; float g_fCarAngle=0,g_fCarSpeed=0; float g_fGyroscopeAngleSpeed=0,g_fGyroscopeAngleIntegral=0,g_fAng leControlOut=0,g_fSpeedControlIntegral=0; float g_fSpeedControlOutOld=0,g_fSpeedControlOutNew=0; float g_direction_outold=0,g_direction_outnew=0; float g_fSpeedControlOut=0,g_fSpeedControl,g_fGyroscopeAngleSpeed; int g_nSpeedControlPeriod=0; int g_nSpeedControlCount=0,g_nDirectionControlCount=0; int g_nDirectionControlOut=0,g_nDirectionControlPeriod=0; int