陀螺仪c程序

《陀螺仪c程序》由会员上传分享，免费在线阅读，更多相关内容在教育资源-天天文库。

1、飞思卡尔XS128的程序/****************************************************************CodeWarrior5.9Target:MC9S12XS128Crystal:16.000MhzCompetitionGroup：ElectromagneticGroupAuthor:Time:2012.03.26******************************************************************/#include

2、def.h>/*commondefinesandmacros*/#include"derivative.h"/*derivative-specificdefinitions*/#include/*derivativeinformation*/#include"SystemInit.h"floatg_fGravityAngle=0;//加速度计倾角floatg_fGyroscopeAngleSpeed=0;//陀螺仪倾角速度floatg_fCarAngle=0;//陀螺仪倾角floatg_fG

3、yroscopeAngleIntegral=0;//陀螺仪倾角积分#defineg_fGyroscopeAngleIntegral_MAX45#defineg_fGyroscopeAngleIntegral_MIX-45floatVOLTAGE_GYRO;//AD采集的陀螺仪AD值floatVOLTAGE_GRAVITY;//AD采集的加速度计AD值floatVOLTAGE_LEFT;//AD采集的左边电感AD值floatVOLTAGE_RIGHT;//AD采集的右边电感AD值#defineGRAVITY_ANGLE_R

4、ATIO0.134//加速度计Z轴:"归一化比例系数"0.1345#defineGYROSCOPE_ANGLE_RATIO0.24//陀螺仪:"归一化比例系数"0.1832#defineGRAVITY_ADJUST_TIME_CONSTANT3//加速度计时间补偿系数#defineGRAVITY_OFFSET1400//加速度计Z轴偏移量#defineGYROSCOPE_OFFSET1110//陀螺仪偏移量#defineCONTROL_PERIOD5//Every5msadjustallcontrol#defineGYR

5、OSCOPE_ANGLE_SIGMA_FREQUENCY(1000/CONTROL_PERIOD)//陀螺仪积分时间5msfloatg_fAngleControlOut;//角度控制输出量#defineCAR_ANGLE_SET0//宏定义恒等于0#defineCAR_ANGLE_SPEED_SET0//宏定义恒等于0#defineANGLE_CONTROL_P0.3//角度控制比例参数0.3#defineANGLE_CONTROL_D0.005//角度控制微分参数0.005unsignedintAD_VAL_GYRO[

6、20];unsignedintAD_VAL_ACCE[20];unsignedintAD_VAL_LEFT[20];unsignedintAD_VAL_RIGHT[20];unsignedintADv0;unsignedintADv1;unsignedintADv2;unsignedintADv3;floatg_fLeftMotorOut;floatg_fRightMotorOut;floatAnglePWMControlOut;#defineMOTOR_OUT_DEAD_VAL1intg_nAngleControlFl

7、ag;intg_nCarControlFlag;#defineANGLE_CONTROL_STOPg_nAngleControlFlag=0#defineANGLE_CONTROL_STARTg_nAngleControlFlag=1#defineCAR_CONTROL_SETg_nCarControlFlag=1#defineCAR_CONTROL_CLEARg_nCarControlFlag=0#defineMOTOR_OUT_MAX1.0#defineMOTOR_OUT_MIN-1.0#defineANGLE_CO

8、NTROL_OUT_MAXMOTOR_OUT_MAX*10#defineANGLE_CONTROL_OUT_MINMOTOR_OUT_MIN*10#defineCAR_FAILURE_ANGLE_MAX45.0#defineCAR_FAILURE_ANGLE_MIN-45.0voidSetMotorVoltage(f