标题: 程序进入if 语句中就出不来了 江湖救急啊 [打印本页]

---

作者: 上善若水001    时间: 2017-11-30 20:59
标题: 程序进入if 语句中就出不来了 江湖救急啊
我用 mpu6050 模块读取当前倾角（用的是野火的例程），当达到某一角度时，电机启动 延时一段时间，这个控制程序是放在 main 函数 while(1) 的 if 语句中，可是不明白为什么程序进入 if 语句中就出不来了，然后电机就一直在那工作。唉，都快急死了。还请各位大哥大姐都来帮帮我，感激不尽！！！

---

作者: angmall    时间: 2017-11-30 21:29
你好！没有原理图，没有程序，这怎么分析问题的所在
发一下 程序和原理图

---

作者: 上善若水001    时间: 2017-11-30 22:03
---------------这是main 函数----------------
#include "stm32f10x.h"
#include "bsp_usart.h" // 包含 串口 头文件
#include "delay.h" // 包含 延时 头文件
//#include "bsp_TiMbase.h" // 包含 基本定时器 头文件
#include "stm32f10x_spi.h"
#include "ioi2c.h" // 包含 I2C 头文件
#include "MPU6050.h" // 包含 MPU6050 头文件
#include "control.h" // 包含 控制程序 头文件
#include "bsp_adc.h" // 包含 adc 头文件
#include "motor.h"   // 包含 电机 头文件

float Pitch,Roll,Yaw,fanguanjiao;

//*************************************************************
//* 函数名：DelayTime_ms
//* 描述  ：Time           延时的时间 MS
//* 输入  ：无
//* 输出  ：无
//*************************************************************
void delay_1ms(u32 time)  
{  
  u32 i=8000*time;  
  while(i--);  
}

//*************************************************************
//* 函数名：DelayTime_us
//* 描述  ：1us延时函数
//* 输入  ：Time           延时的时间 US
//* 输出  ：无       
//*************************************************************
void delay_1us(u32 time)  
{  
  u32 i=8*time;  
  while(i--);  
}


/**
  * @brief  主函数
  * @param  无
  * @retval 无
  */
int main(void)
{       
//                float Pitch,Roll,Yaw;
        USART_Config();       
        ADCx_Init();
        // 配置串口
//  BASIC_TIM_Init();
        delay_init();
        IIC_Init();
        NVIC_Configuration();          //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
        DMP_Init();
        Motor_12_Config(); //L298电机驱动初始化
        printf("\r\n ----这是一个ADC单通道DMA读取实验----\r\n");
       
        while (1)
        {               
                  ADCcaclulate();
                        printf("Read_DMP Return is %d\n",Read_DMP(&Pitch,&Roll,&Yaw));
                  fanguanjiao = Pitch;
//                printf("Pitch is:%f,Roll is:%f,Yaw is:%f\n",Pitch,Roll,Yaw);
                  printf("fanguanjiao is:%f",fanguanjiao);

                  if(fanguanjiao>30)
                        {
                         Motor_1_PRun();
                         delay_1ms(2000);
                         Motor_1_STOP();
                         delay_1ms(1000);
                   Motor_1_NRun();
                         delay_1ms(2000);       
                        }                                                               
//    printf("Read_DMP Return is %d\n",Read_DMP(&Pitch,&Roll,&Yaw));
//                printf("Pitch is:%f,Roll is:%f,Yaw is:%f\n",Pitch,Roll,Yaw);
        }
}

-----------------------这是mpu6050.c-------------
#include "MPU6050.h"
#include "IOI2C.h"
#include "bsp_usart.h"
#include <math.h>
#include "inv_mpu_dmp_motion_driver.h"
#include "inv_mpu.h"
#include "delay.h"
/**************************************************************************/
#define PRINT_ACCEL     (0x01)
#define PRINT_GYRO      (0x02)
#define PRINT_QUAT      (0x04)
#define ACCEL_ON        (0x01)
#define GYRO_ON         (0x02)
#define MOTION          (0)
#define NO_MOTION       (1)
#define DEFAULT_MPU_HZ  (200)
#define FLASH_SIZE      (512)
#define FLASH_MEM_START ((void*)0x1800)
#define q30  1073741824.0f

static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};

static  unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}


static  unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar;
    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}

static void run_self_test(void)
{
    int result;
    long gyro[3], accel[3];

    result = mpu_run_self_test(gyro, accel);
    if (result == 0x7) {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        float sens;
        unsigned short accel_sens;
        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0] * sens);
        gyro[1] = (long)(gyro[1] * sens);
        gyro[2] = (long)(gyro[2] * sens);
        dmp_set_gyro_bias(gyro);
        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
                printf("setting bias succesfully ......\r\n");
    }
}



uint8_t buffer[14];

int16_t  MPU6050_FIFO[6][11];
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;


/**************************实现函数********************************************
*函数原型:                void  MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
*功　　能:            将新的ADC数据更新到 FIFO数组，进行滤波处理
*******************************************************************************/

void  MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
{
unsigned char i ;
int32_t sum=0;
for(i=1;i<10;i++){        //FIFO 操作
MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
}
MPU6050_FIFO[0][9]=ax;//将新的数据放置到 数据的最后面
MPU6050_FIFO[1][9]=ay;
MPU6050_FIFO[2][9]=az;
MPU6050_FIFO[3][9]=gx;
MPU6050_FIFO[4][9]=gy;
MPU6050_FIFO[5][9]=gz;

sum=0;
for(i=0;i<10;i++){        //求当前数组的合，再取平均值
   sum+=MPU6050_FIFO[0][i];
}
MPU6050_FIFO[0][10]=sum/10;

sum=0;
for(i=0;i<10;i++){
   sum+=MPU6050_FIFO[1][i];
}
MPU6050_FIFO[1][10]=sum/10;

sum=0;
for(i=0;i<10;i++){
   sum+=MPU6050_FIFO[2][i];
}
MPU6050_FIFO[2][10]=sum/10;

sum=0;
for(i=0;i<10;i++){
   sum+=MPU6050_FIFO[3][i];
}
MPU6050_FIFO[3][10]=sum/10;

sum=0;
for(i=0;i<10;i++){
   sum+=MPU6050_FIFO[4][i];
}
MPU6050_FIFO[4][10]=sum/10;

sum=0;
for(i=0;i<10;i++){
   sum+=MPU6050_FIFO[5][i];
}
MPU6050_FIFO[5][10]=sum/10;
}

/**************************实现函数********************************************
*函数原型:                void MPU6050_setClockSource(uint8_t source)
*功　　能:            设置  MPU6050 的时钟源
* CLK_SEL | Clock Source
* --------+--------------------------------------
* 0       | Internal oscillator
* 1       | PLL with X Gyro reference
* 2       | PLL with Y Gyro reference
* 3       | PLL with Z Gyro reference
* 4       | PLL with external 32.768kHz reference
* 5       | PLL with external 19.2MHz reference
* 6       | Reserved
* 7       | Stops the clock and keeps the timing generator in reset
*******************************************************************************/
void MPU6050_setClockSource(uint8_t source){
    IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);

}

/** Set full-scale gyroscope range.
* @param range New full-scale gyroscope range value
* @see getFullScaleRange()
* @see MPU6050_GYRO_FS_250
* @see MPU6050_RA_GYRO_CONFIG
* @see MPU6050_GCONFIG_FS_SEL_BIT
* @see MPU6050_GCONFIG_FS_SEL_LENGTH
*/
void MPU6050_setFullScaleGyroRange(uint8_t range) {
    IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}

/**************************实现函数********************************************
*函数原型:                void MPU6050_setFullScaleAccelRange(uint8_t range)
*功　　能:            设置  MPU6050 加速度计的最大量程
*******************************************************************************/
void MPU6050_setFullScaleAccelRange(uint8_t range) {
    IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}

/**************************实现函数********************************************
*函数原型:                void MPU6050_setSleepEnabled(uint8_t enabled)
*功　　能:            设置  MPU6050 是否进入睡眠模式
                                enabled =1   睡觉
                            enabled =0   工作
*******************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {
    IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}

/**************************实现函数********************************************
*函数原型:                uint8_t MPU6050_getDeviceID(void)
*功　　能:            读取  MPU6050 WHO_AM_I 标识         将返回 0x68
*******************************************************************************/
uint8_t MPU6050_getDeviceID(void) {

    IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
    return buffer[0];
}

/**************************实现函数********************************************
*函数原型:                uint8_t MPU6050_testConnection(void)
*功　　能:            检测MPU6050 是否已经连接
*******************************************************************************/
uint8_t MPU6050_testConnection(void) {
   if(MPU6050_getDeviceID() == 0x68)  //0b01101000;
   return 1;
           else return 0;
}

/**************************实现函数********************************************
*函数原型:                void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
*功　　能:            设置 MPU6050 是否为AUX I2C线的主机
*******************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
    IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}

/**************************实现函数********************************************
*函数原型:                void MPU6050_setI2CBypassEnabled(uint8_t enabled)
*功　　能:            设置 MPU6050 是否为AUX I2C线的主机
*******************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
    IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}

/**************************实现函数********************************************
*函数原型:                void MPU6050_initialize(void)
*功　　能:            初始化         MPU6050 以进入可用状态。
*******************************************************************************/
void MPU6050_initialize(void) {
    MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO); //设置时钟
    MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺仪最大量程 +-1000度每秒
    MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);        //加速度度最大量程 +-2G
    MPU6050_setSleepEnabled(0); //进入工作状态
         MPU6050_setI2CMasterModeEnabled(0);         //不让MPU6050 控制AUXI2C
         MPU6050_setI2CBypassEnabled(0);         //主控制器的I2C与        MPU6050的AUXI2C        直通。控制器可以直接访问HMC5883L
}




/**************************************************************************
函数功能：MPU6050内置DMP的初始化
入口参数：无
返回  值：无
作    者：平衡小车之家
**************************************************************************/
void DMP_Init(void)
{
   u8 temp[1]={0};
   i2cRead(0x68,0x75,1,temp);
         printf("mpu_set_sensor complete ......\r\n");
        if(temp[0]!=0x68)NVIC_SystemReset();
        if(!mpu_init())
  {
          if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
                   printf("mpu_set_sensor complete ......\r\n");
          if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
                   printf("mpu_configure_fifo complete ......\r\n");
          if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
                   printf("mpu_set_sample_rate complete ......\r\n");
          if(!dmp_load_motion_driver_firmware())
                  printf("dmp_load_motion_driver_firmware complete ......\r\n");
          if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
                   printf("dmp_set_orientation complete ......\r\n");
          if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
                DMP_FEATURE_GYRO_CAL))
                   printf("dmp_enable_feature complete ......\r\n");
          if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
                   printf("dmp_set_fifo_rate complete ......\r\n");
          run_self_test();
          if(!mpu_set_dmp_state(1))
                   printf("mpu_set_dmp_state complete ......\r\n");
  }
}
/**************************************************************************
函数功能：读取MPU6050内置DMP的姿态信息
入口参数：无
返回  值：无
作    者：平衡小车之家
**************************************************************************/
uint8_t Read_DMP(float* Pitch,float* Roll,float* Yaw)
{       
                short gyro[3], accel[3], sensors;
                float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
          unsigned long sensor_timestamp;
                unsigned char more;
                long quat[4];
                                if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) return 1;               
                                if (sensors & INV_WXYZ_QUAT)
                                {   
                                         q0=quat[0] / q30;
                                         q1=quat[1] / q30;
                                         q2=quat[2] / q30;
                                         q3=quat[3] / q30;
                                         *Pitch = (float)asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;        
                                         *Roll = (float)atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
                                         *Yaw = (float)atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
//                                         delay_ms(10);
                                         return 0;
                                }       
                                else return 2;                               
}
/**************************************************************************
函数功能：读取MPU6050内置温度传感器数据
入口参数：无
返回  值：摄氏温度
作    者：平衡小车之家
**************************************************************************/
int Read_Temperature(void)
{          
          float Temp;
          Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L);
                if(Temp>32768) Temp-=65536;
                Temp=(36.53+Temp/340)*10;
          return (int)Temp;
}
//------------------End of File----------------------------

---

作者: 上善若水001    时间: 2017-12-2 10:16
没有人吗？

---

作者: 无线电菜虫    时间: 2017-12-2 11:10
你先加输出定位问题的原因在哪  这么多代码很难帮你分析具体问题的

---

作者: szb0321    时间: 2017-12-2 11:15
简单啊！！！分析下：如果 在没有满足 fanguanjiao>30的条件下  电机是不会转动，程序运行之后当满足  fanguanjiao>30条件之后  电机开始转动。程序再次循环呢？？？fanguanjiao>30条件是没有满足，可你没有编写fanguanjiao>30不满足关闭电机的程序啊！！！所以电机一直在转动。解决方案...  
                  if(fanguanjiao>30)
                        {
                         Motor_1_PRun();
                         delay_1ms(2000);
                         Motor_1_STOP();
                         delay_1ms(1000);
                   Motor_1_NRun();
                         delay_1ms(2000);        
                        }
                else
                  ｛
                    //关闭电机的程序
                   ｝



你测试下

---

作者: 上善若水001    时间: 2017-12-3 13:07

szb0321 发表于 2017-12-2 11:15
简单啊！！！分析下：如果 在没有满足 fanguanjiao>30的条件下  电机是不会转动，程序运行之后当满足  fang ...

多谢了，我已经找到原因了，是6050的刷新速度太快了

---

作者: 上善若水001    时间: 2017-12-3 13:08

无线电菜虫 发表于 2017-12-2 11:10
你先加输出定位问题的原因在哪  这么多代码很难帮你分析具体问题的

还是谢谢你来评论，我把问题解决了

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1