楼主: 荷叶轻舟

[其他] 求助 陀螺仪积分出来的角度不对

[复制链接]

该用户从未签到

4

主题

24

帖子

0

新手上路

Rank: 1

积分
31
最后登录
1970-1-1
发表于 2012-4-20 16:42:14 | 显示全部楼层 |阅读模式
飞思卡尔XS128的程序
/****************************************************************
Code Warrior 5.9
Target : MC9S12XS128
Crystal: 16.000Mhz
Competition Group:Electromagnetic Group
Author:
Time:2012.03.26
******************************************************************/
#include            /* common defines and macros */
#include "derivative.h"      /* derivative-specific definitions */
#include      /* derivative information */
#include "SystemInit.h"
float g_fGravityAngle=0;               //加速度计倾角
float g_fGyroscopeAngleSpeed=0;        //陀螺仪倾角速度
float g_fCarAngle=0;                   //陀螺仪倾角
float g_fGyroscopeAngleIntegral=0;     //陀螺仪倾角积分
#define g_fGyroscopeAngleIntegral_MAX 45
#define g_fGyroscopeAngleIntegral_MIX -45
float 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)     //陀螺仪积分时间        5ms
float 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.005
unsigned 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
#define ANGLE_CONTROL_OUT_MIN   MOTOR_OUT_MIN * 10
#define CAR_FAILURE_ANGLE_MAX   45.0
#define CAR_FAILURE_ANGLE_MIN   -45.0
void SetMotorVoltage(float fLeftVoltage, float fRightVoltage) {
                                                // Voltage : > 0 : Move forward;
                                                //           < 0 : Move backward
/* short nPeriod;
 int nOut;
 
 nPeriod = (short)getReg(PWM_PWMCM);         */
 
 //--------------------------------------------------------------------------
 if(fLeftVoltage > 1.0)    fLeftVoltage = 1.0;
 else if(fLeftVoltage < -1.0)  fLeftVoltage = -1.0;
 
 if(fRightVoltage > 1.0)   fRightVoltage = 1.0;
 else if(fRightVoltage < -1.0) fRightVoltage = -1.0;
                                             
 //--------------------------------------------------------------------------                                                                                           
 if(fLeftVoltage > 0) {
  //setReg(PWM_PWMVAL1, 0);
  PWMDTY01=0;
  //nOut = (int)(fLeftVoltage * nPeriod);
  //setReg(PWM_PWMVAL0, nOut);
  PWMDTY23=(int)(fLeftVoltage * 100);
 } else {
  //setReg(PWM_PWMVAL0, 0);
  PWMDTY23=0;
  fLeftVoltage = -fLeftVoltage;
  //nOut = (int)(fLeftVoltage * nPeriod);
  //setReg(PWM_PWMVAL1, nOut);
  PWMDTY01=(int)(fLeftVoltage * 100);
 }                                    
 //--------------------------------------------------------------------------
 if(fRightVoltage > 0) {
  //setReg(PWM_PWMVAL2, 0);
  PWMDTY45=1;
  //nOut = (int)(fRightVoltage * nPeriod);
  //setReg(PWM_PWMVAL3, nOut);
  PWMDTY67=(int)(fRightVoltage * 100);
 } else {
  //setReg(PWM_PWMVAL3, 0);
  PWMDTY67=0;
  fRightVoltage = -fRightVoltage;
  //nOut = (int)(fRightVoltage * nPeriod);
  //setReg(PWM_PWMVAL2, nOut);    
  PWMDTY45=(int)(fRightVoltage * 100);                  
 }
}
void MotorSpeedOut(void) 
{
 float fLeftVal, fRightVal;
 
 fLeftVal = g_fLeftMotorOut;
 fRightVal = g_fRightMotorOut;
 
 if(fLeftVal > 0)    fLeftVal += MOTOR_OUT_DEAD_VAL;
 else if(fLeftVal < 0)   fLeftVal -= MOTOR_OUT_DEAD_VAL;
 
 if(fRightVal > 0)   fRightVal += MOTOR_OUT_DEAD_VAL;
 else if(fRightVal < 0)  fRightVal -= MOTOR_OUT_DEAD_VAL;
  
 if(fLeftVal > MOTOR_OUT_MAX) fLeftVal = MOTOR_OUT_MAX;
 if(fLeftVal < MOTOR_OUT_MIN) fLeftVal = MOTOR_OUT_MIN;
 if(fRightVal > MOTOR_OUT_MAX) fRightVal = MOTOR_OUT_MAX;
 if(fRightVal < MOTOR_OUT_MIN) fRightVal = MOTOR_OUT_MIN;
   
 SetMotorVoltage(fLeftVal, fRightVal);
}
void MotorOutput(void)
{
 float fLeft, fRight;
 //fLeft = g_fAngleControlOut - g_fSpeedControlOut - g_fDirectionControlOut;
 fLeft = g_fAngleControlOut;
 //fRight = g_fAngleControlOut - g_fSpeedControlOut + g_fDirectionControlOut;
 fRight = g_fAngleControlOut;
 
 if(fLeft > MOTOR_OUT_MAX)  fLeft = MOTOR_OUT_MAX;
 if(fLeft < MOTOR_OUT_MIN)  fLeft = MOTOR_OUT_MIN;
 if(fRight > MOTOR_OUT_MAX)  fRight = MOTOR_OUT_MAX;
 if(fRight < MOTOR_OUT_MIN)  fRight = MOTOR_OUT_MIN;
  
 g_fLeftMotorOut = fLeft;
 g_fRightMotorOut = fRight;
 MotorSpeedOut();
}
void AngleCalculate(void)
{
 float fDeltaValue;
 // Define Angle
 g_fGravityAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO;
 g_fGyroscopeAngleSpeed = (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO;
 
 g_fCarAngle = g_fGyroscopeAngleIntegral;
 fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
 
//#if SPEED_CONTROL_METHOD == 0
 g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
//#else if SPEED_CONTROL_METHOD == 1
// g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + g_fAngleControlSpeedFeedback) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
//#endif // SPEED_CONTROL_METHOD
}
void AngleControl(void)
{
 float fValue;
 if(g_nAngleControlFlag == 0) {
  g_fAngleControlOut = 0;
  return;
 }
 
 //--------------------------------------------------------------------------
 fValue = (CAR_ANGLE_SET - g_fCarAngle) * ANGLE_CONTROL_P + (CAR_ANGLE_SPEED_SET - g_fGyroscopeAngleSpeed) * ANGLE_CONTROL_D;
         
 if(fValue > ANGLE_CONTROL_OUT_MAX)   fValue = ANGLE_CONTROL_OUT_MAX;
 else if(fValue < ANGLE_CONTROL_OUT_MIN)  fValue = ANGLE_CONTROL_OUT_MIN;
 g_fAngleControlOut = fValue;
 
}
void CarControlStart(void) {
 CAR_CONTROL_SET;
 ANGLE_CONTROL_START;
 //SPEED_CONTROL_START;
 //DIRECTION_CONTROL_START;
 //TIME_TEST_ENA;
}
void CarControlStop(void)
{
 SetMotorVoltage(0, 0);
 
 CAR_CONTROL_CLEAR;
 ANGLE_CONTROL_STOP;
 //SPEED_CONTROL_STOP;
 //DIRECTION_CONTROL_STOP;
 //TIME_TEST_DIS;
 g_fLeftMotorOut = g_fRightMotorOut = 0;
 g_fAngleControlOut = 0;
 //g_fSpeedControlOut = 0;
 //g_fDirectionControlOut = 0;

 //g_nWaitCarStandCount = 0;
}
/*void WaitCarStand(void) {
 if(g_nCarControlFlag == 1) return;
 if(g_nWaitCarStandCount < WAIT_CAR_STAND_COUNT) {
  g_nWaitCarStandCount ++;
  return;
 }
 
 
 if(g_fCarAngle > CAR_STAND_ANGLE_MIN && g_fCarAngle < CAR_STAND_ANGLE_MAX && g_fGravityAngle > CAR_STAND_ANGLE_MIN && g_fGravityAngle < CAR_STAND_ANGLE_MAX)
 {
     CarControlStart();
 }  
}    */
void CheckCarStand(void) {
 if(g_nCarControlFlag == 0) return;
  
 if(g_fCarAngle >= CAR_FAILURE_ANGLE_MAX || g_fCarAngle
我知道答案 目前已有9人回答
回复

使用道具 举报

  • TA的每日心情
    难过
    2021-12-15 16:01
  • 签到天数: 1 天

    连续签到: 1 天

    [LV.1]初来乍到

    305

    主题

    4701

    帖子

    0

    中级会员

    Rank: 3Rank: 3

    积分
    377
    最后登录
    2023-8-16
    发表于 2012-4-20 16:45:39 | 显示全部楼层

    RE:求助 陀螺仪积分出来的角度不对

    现在角度差多少?和实际的。
    该会员没有填写今日想说内容.
    回复 支持 反对

    使用道具 举报

    该用户从未签到

    4

    主题

    24

    帖子

    0

    新手上路

    Rank: 1

    积分
    31
    最后登录
    1970-1-1
     楼主| 发表于 2012-4-20 16:45:52 | 显示全部楼层

    RE:求助 陀螺仪积分出来的角度不对

    哪位大侠帮我看看  多谢
    回复 支持 反对

    使用道具 举报

    该用户从未签到

    4

    主题

    24

    帖子

    0

    新手上路

    Rank: 1

    积分
    31
    最后登录
    1970-1-1
     楼主| 发表于 2012-4-20 16:49:25 | 显示全部楼层

    回复:求助 陀螺仪积分出来的角度不对

    回复第 2 楼 于2012-04-20 16:45:39发表:
    现在角度差多少?和实际的。 

    加速度计的获得角度很准确

    然后陀螺仪信号经过算法处理之后的角度要等好久
    才积分到加速度计的角度
    回复 支持 反对

    使用道具 举报

    该用户从未签到

    4

    主题

    24

    帖子

    0

    新手上路

    Rank: 1

    积分
    31
    最后登录
    1970-1-1
     楼主| 发表于 2012-4-20 16:50:56 | 显示全部楼层

    回复:求助 陀螺仪积分出来的角度不对

    回复第 2 楼 于2012-04-20 16:45:39发表:
    现在角度差多少?和实际的。 

    我把车子倾斜30读,加速度计马上算出是30度,但是陀螺仪还在一直慢慢的从0加到30度
    过了好几分钟才积分达到30度
    回复 支持 反对

    使用道具 举报

  • TA的每日心情
    难过
    2021-12-15 16:01
  • 签到天数: 1 天

    连续签到: 1 天

    [LV.1]初来乍到

    305

    主题

    4701

    帖子

    0

    中级会员

    Rank: 3Rank: 3

    积分
    377
    最后登录
    2023-8-16
    发表于 2012-4-21 15:44:16 | 显示全部楼层

    RE:求助 陀螺仪积分出来的角度不对

    陀螺仪采集的数据慢?你用的是哪个陀螺仪?
    该会员没有填写今日想说内容.
    回复 支持 反对

    使用道具 举报

    该用户从未签到

    4

    主题

    24

    帖子

    0

    新手上路

    Rank: 1

    积分
    31
    最后登录
    1970-1-1
     楼主| 发表于 2012-4-23 12:33:45 | 显示全部楼层

    回复:求助 陀螺仪积分出来的角度不对

    回复第 6 楼 于2012-04-21 15:44:16发表:
    陀螺仪采集的数据慢?你用的是哪个陀螺仪? 

    日本村田的ENC-03MB
    回复 支持 反对

    使用道具 举报

    该用户从未签到

    4

    主题

    24

    帖子

    0

    新手上路

    Rank: 1

    积分
    31
    最后登录
    1970-1-1
     楼主| 发表于 2012-4-23 13:20:31 | 显示全部楼层

    回复:求助 陀螺仪积分出来的角度不对

    回复第 6 楼 于2012-04-21 15:44:16发表:
    陀螺仪采集的数据慢?你用的是哪个陀螺仪? 

    日本村田的ENC-03MB
    回复 支持 反对

    使用道具 举报

    该用户从未签到

    4

    主题

    24

    帖子

    0

    新手上路

    Rank: 1

    积分
    31
    最后登录
    1970-1-1
     楼主| 发表于 2012-4-24 09:52:22 | 显示全部楼层

    回复:求助 陀螺仪积分出来的角度不对

    回复第 6 楼 于2012-04-21 15:44:16发表:
    陀螺仪采集的数据慢?你用的是哪个陀螺仪? 

    日本村田的ENC-03MB
     
    回复 支持 反对

    使用道具 举报

  • TA的每日心情
    难过
    2021-12-15 16:01
  • 签到天数: 1 天

    连续签到: 1 天

    [LV.1]初来乍到

    305

    主题

    4701

    帖子

    0

    中级会员

    Rank: 3Rank: 3

    积分
    377
    最后登录
    2023-8-16
    发表于 2012-4-24 14:00:11 | 显示全部楼层

    RE:求助 陀螺仪积分出来的角度不对

    如果是采集到陀螺仪的数据与比实际的慢,说明是陀螺仪的问题了。
    该会员没有填写今日想说内容.
    回复 支持 反对

    使用道具 举报

    您需要登录后才可以回帖 注册/登录

    本版积分规则

    关闭

    站长推荐上一条 /3 下一条

    Archiver|手机版|小黑屋|恩智浦技术社区

    GMT+8, 2025-7-28 07:28 , Processed in 0.105435 second(s), 29 queries , MemCache On.

    Powered by Discuz! X3.4

    Copyright © 2001-2024, Tencent Cloud.

    快速回复 返回顶部 返回列表