陀螺仪c程序.docx

下载文档

27
0
约6.4千字
约 6页
2017-12-17 发布于河南
举报
版权申诉
保障服务

陀螺仪c程序.docx

1、本文档共6页，可阅读全部内容。
2、有哪些信誉好的足球投注网站（book118）网站文档一经付费（服务费），不意味着购买了该文档的版权，仅供个人/单位学习、研究之用，不得用于商业用途，未经授权，严禁复制、发行、汇编、翻译或者网络传播等，侵权必究。
3、本站所有内容均由合作方或网友上传，本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺！文档内容仅供研究参考，付费前请自行鉴别。如您付费，意味着您自己接受本站规则且自行承担风险，本站不退款、不进行额外附加服务；查看《如何避免下载的几个坑》。如果您已付费下载过本站文档，您可以点击这里二次下载。
4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等，请点击“版权申诉”（推荐），也可以打举报电话：400-050-0827(电话支持时间：9:00-18:30)。

陀螺仪c程序

飞思卡尔XS128的程序/****************************************************************Code Warrior 5.9Target : MC9S12XS128Crystal: 16.000MhzCompetition Group：Electromagnetic GroupAuthor:Time:2012.03.26******************************************************************/#include hidef.h /* common defines and macros */#include derivative.h /* derivative-specific definitions */#include MC9S12XS128.h /* derivative information */#include SystemInit.hfloat g_fGravityAngle=0; //加速度计倾角float g_fGyroscopeAngleSpeed=0; //陀螺仪倾角速度float g_fCarAngle=0; //陀螺仪倾角float g_fGyroscopeAngleIntegral=0; //陀螺仪倾角积分#define g_fGyroscopeAngleIntegral_MAX 45#define g_fGyroscopeAngleIntegral_MIX -45float VOLTAGE_GYRO; //AD采集的陀螺仪AD值float VOLTAGE_GRAVITY; //AD采集的加速度计AD值float VOLTAGE_LEFT; //AD采集的左边电感AD值float VOLTAGE_RIGHT; //AD采集的右边电感AD值#define GRAVITY_ANGLE_RATIO 0.134 //加速度计Z轴:归一化比例系数 0.1345 #define GYROSCOPE_ANGLE_RATIO 0.24 //陀螺仪:归一化比例系数 0.1832 #define GRAVITY_ADJUST_TIME_CONSTANT 3 //加速度计时间补偿系数#define GRAVITY_OFFSET 1400 //加速度计Z轴偏移量#define GYROSCOPE_OFFSET 1110 //陀螺仪偏移量#define CONTROL_PERIOD 5 // Every 5 ms adjust all control#define GYROSCOPE_ANGLE_SIGMA_FREQUENCY (1000 / CONTROL_PERIOD) //陀螺仪积分时间 5msfloat g_fAngleControlOut; //角度控制输出量#define CAR_ANGLE_SET 0 //宏定义恒等于0#define CAR_ANGLE_SPEED_SET 0 //宏定义恒等于0#define ANGLE_CONTROL_P 0.3 //角度控制比例参数 0.3#define ANGLE_CONTROL_D 0.005 //角度控制微分参数0.005unsigned int AD_VAL_GYRO[20];unsigned int AD_VAL_ACCE[20];unsigned int AD_VAL_LEFT[20];unsigned int AD_VAL_RIGHT[20];unsigned int ADv0;unsigned int ADv1;unsigned int ADv2;unsigned int ADv3;float g_fLeftMotorOut;float g_fRightMotorOut;float AnglePWMControlOut;#define MOTOR_OUT_DEAD_VAL 1 int g_nAngleControlFlag;int g_nCarControlFlag;#define ANGLE_CONTROL_STOP g_nAngleControlFlag = 0#define ANGLE_CONTROL_START g_nAngleControlFlag = 1#define CAR_CONTROL_SET g_nCarControlFlag = 1#define CAR_CONTROL_CLEAR g_nCarControlFlag = 0#define MOTOR_OUT_MAX 1.0#define MOTOR_OUT_MIN -1.0#define ANGLE_CONTROL_OUT_MAX MOTOR_OUT_MAX * 10