编译程序时遇到这问题。。-Arduino中文社区 - Powered by Discuz!
设为首页收藏本站

Arduino中文社区

 找回密码
 立即注册

QQ登录

只需一步，快速开始

Arduino中文社区»首页 › Arduino讨论区 › 求助区 › 编译程序时遇到这问题。。
返回列表 发新帖
查看: 5083|回复: 4

[已解决] 编译程序时遇到这问题。。

[复制链接]
zzp
发表于 2014-5-9 13:11 | 显示全部楼层 |阅读模式
如图，貌似是串口接收中断有问题。。求救！
捕获.JPG
回复

举报

coloz
发表于 2014-5-9 13:40 | 显示全部楼层
贴上你的程序
回复 支持 反对

举报

zzp
 楼主| 发表于 2014-5-9 19:24 | 显示全部楼层
本帖最后由 zzp 于 2014-5-9 19:27 编辑
coloz 发表于 2014-5-9 13:40
贴上你的程序

程序如下：其实是一个小四轴源代码，参考匿名四轴代码，自己移植到Arduino。主要用了串口中断、定时器中断。串口中断用来接收指令，包括解锁、校准、起飞、降落等等。定时器中断用来读取MPU6050数据，计算姿态差，输入PID控制器计算得出电机PWM参数，再驱动电机等等。其实你会看到还有一个外部中断被我屏蔽了的。这个原本用来在串口中断中接收到降落指令时触发外部中断，调用外部中断降落函数，降落小四轴。然后锁上电机。现在编译时遇到上述问题。想问下是不是中断冲突什么的。。我还想设定中断优先级，外部中断最高，串口中断其次，定时器中断最低。。但是论坛里面有关这些知识比较少。。买了一本《Arduino技术内幕》，看了，发现他还是讲的很略。。
[mw_shl_code=c,true]#include <MPU6050.h>
#include <I2Cdev.h>
#include <Wire.h>
#include <MsTimer2.h> // 定时器2头文件
MPU6050 accelgyro;//测量范围加速度+-1g,角速度+-250度每秒
/**********************/
#define Gyro_G         0.0152672//角速度转换成度  此参数对应陀螺2000°/s
#define Gyro_Gr        0.0002663//角速度转换成弧度  此参数对应陀螺2000°/s
/**********************/
boolean Data_check ;//陀螺仪和加速度计数据校准标志
boolean Lock ;//电机解锁标志
boolean system_init;//初始化开始标志
/************************/
float GYRO_OFFSET_X = 0;//陀螺仪X轴零偏全局变量
float GYRO_OFFSET_Y = 0;//陀螺仪Y轴零偏全局变量
float GYRO_OFFSET_Z = 0;//陀螺仪Z轴零偏全局变量
float ACC_OFFSET_X = 0;//加速度计X轴零偏全局变量
float ACC_OFFSET_Y = 0;//加速度计Y轴零偏全局变量
float ACC_OFFSET_Z = 0;//加速度计Z轴零偏全局变量
float ACC_AVG_X, ACC_AVG_Y, ACC_AVG_Z;//加速度测量平均值.AVG表示AVERAGE,平均
float Angle_I_X,Angle_I_Y,Angle_I_Z;//定义陀螺仪积分角度 I表示积分
float Q_ANGLE_Roll,Q_ANGLE_Pitch,Q_ANGLE_Yaw;//定义四元数转换的欧拉角
/*****************************************/
int16_t ax,ay,az;//加速度计测量变量，范围+-1g
int16_t gx,gy,gz;//陀螺仪测量变量
//char Receive;//定义串口接收字符
//char oneChar;//定义单节串口接收字符
//unsigned char j ;
/*******************************/
void usart_init(unsigned long rate)//开启串口接收中断
{
  UCSR0A = 0<<TXC0|0<<U2X0|0<<MPCM0;
UCSR0B = 1<<RXCIE0|0<<TXCIE0|0<<UDRIE0|1<<RXEN0|0<<TXEN0|0<<UCSZ02|0<<TXB80;//置高的两位表示允许接受中断和开启接收
UCSR0C = 0<<UMSEL01|0<<UMSEL00|0<<UPM01|0<<UPM00|0<<USBS0|1<<UCSZ01|1<<UCSZ00|0<<UCPOL0;
UBRR0 = (F_CPU/16/rate-1);
}
/***********************************/
void setup()
{
system_init = false;//初始化开始标志
Wire.begin();
// Serial.begin(9600);
usart_init(9600);//配置USART波特率为9600bps,并且开启串口接收中断
accelgyro.initialize();//初始化MPU6050
//attachInterrupt(0,Land,HIGH);//设定外部中断int0,脚位D2,中断函数Land();高电平触发
MsTimer2::set(1, Timer);// 定时器2中断设置函数，每 1ms 进入一次中断
// MsTimer2::start();   //打开定时器
system_init = true;
}
/**************************************/
void loop()
{

}
/*************************************///串口接收中断函数
ISR(USART_RX_vect)
{
  unsigned char get = UDR0;//读取寄存器
    switch(get)
    {
      case 's'://开始执行 start
         MsTimer2::start();   //打开定时器
         break;
      case 'c'://校准数据 check
        Data_check=true;//陀螺仪和加速度计校准标志,准备校准传感器
        break;
      case 'f'://起飞fly
        Lock=1;//解锁
        break;
      default:
      digitalWrite(4,HIGH);//置高触发外部中断,D4与int0链接有一电阻
       break;
      // default:
      //Design_Highth = (int) Receive;//读取设定的高度
       //break;
   }
}
/***************************************/
/*void Land()//外中断降落函数
{
  j=200;
  while(j>0)
  {
analogWrite(6, j);
analogWrite(9, j);
analogWrite(10, j);
analogWrite(5, j);
j--;
  }
  digitalWrite(4,LOW);//关闭int0中断,D4与int0链接有一电阻
  Lock=0;//锁上电机
}*/

/*********************************///定时器2中断函数
void Timer()
{
  static unsigned char t1,t2;
if(!system_init) return;//判断初始化是否完成
  t1++;
  t2++;
  if(t1==1)
  {
    t1=0;
  void preparedata();//准备MPU6050数据
  }
   if(t2==2)
    {
     t2=0;
     Get_Attitude();//姿态解算
     Control(Q_ANGLE_Roll,Q_ANGLE_Pitch,Q_ANGLE_Yaw);//电机控制
    }
}
/*****************************/
void preparedata()//准备MPU6050数据function
{
          static uint8_t filter_cnt=0;
        int32_t temp1=0,temp2=0,temp3=0;
        uint8_t i;
       int FILTER_NUM = 20;
       int16_t        ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM];//定义滑动数组,对所测得加速度计数值求平均
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取6050数据        
        Solveaccelgyro();//处理6050数据
         ACC_X_BUF[filter_cnt] = ax - ACC_OFFSET_X;//更新滑动窗口数组
        ACC_Y_BUF[filter_cnt] = ay - ACC_OFFSET_Y;//所测得的数据减去偏移量
        ACC_Z_BUF[filter_cnt] = az - ACC_OFFSET_Z;
        for(i=0;i<FILTER_NUM;i++)
        {
                temp1 += ACC_X_BUF;
                temp2 += ACC_Y_BUF;
                temp3 += ACC_Z_BUF;
        }
        ACC_AVG_X = temp1 / FILTER_NUM;
        ACC_AVG_Y = temp2 / FILTER_NUM;
        ACC_AVG_Z = temp3 / FILTER_NUM;
        filter_cnt++;
        if(filter_cnt==FILTER_NUM)        
          filter_cnt=0;
        
        Angle_I_X += gx*Gyro_G*0.001;//0.002是时间间隔,prepare()的执行周期 2ms
        Angle_I_Y+= gy*Gyro_G*0.001;//陀螺仪积分求角度 I表示积分
        Angle_I_Z += gz*Gyro_G*0.001;//陀螺仪积分求角度 I表示积分
}
/************************************************************/
void Solveaccelgyro()//处理MPU6050读到的数据
{
  if(Data_check)//判断是否要校准传感器
  {
   static int32_t tempgx=0,tempgy=0,tempgz=0;
   static int32_t tempax=0,tempay=0,tempaz=0;
   static uint8_t cnt_g=0;
   // LED1_ON;
        if(cnt_g==0)
        {
                GYRO_OFFSET_X=0;//将之前保存的X轴陀螺仪零偏清零
                GYRO_OFFSET_Y=0;//将之前保存的Y轴陀螺仪零偏清零
                GYRO_OFFSET_Z=0;//将之前保存的Z轴陀螺仪零偏清零
                ACC_OFFSET_X = 0;//将之前保存的X轴加速度计零偏清零
                ACC_OFFSET_Y = 0;//将之前保存的Y轴加速度计零偏清零
                ACC_OFFSET_Z = 0;//将之前保存的Z轴加速度计零偏清零
                tempgx = 0;
                tempgy = 0;
                tempgz = 0;

                tempax = 0;
                tempay = 0;
                tempaz = 0;
                cnt_g = 1;
                return;
        }
                tempgx+= gx;
                tempgy+= gy;
                tempgz+= gz;
                tempax+= ax;
                tempay+= ay;
                tempaz+= az;
                if(cnt_g==20)
                {
                        GYRO_OFFSET_X=tempgx/cnt_g/131.0;
                        GYRO_OFFSET_Y=tempgy/cnt_g/131.0;
                        GYRO_OFFSET_Z=tempgz/cnt_g/131.0;
                        
                        ACC_OFFSET_X=tempax/cnt_g/16384.0;
                        ACC_OFFSET_Y=tempay/cnt_g/16384.0;
                        ACC_OFFSET_Z=tempaz/cnt_g/16384.0;
                        cnt_g = 0;
                        Data_check= false;//传感器校准完成标志
                        return;
                }
        cnt_g++;
   }
}

/**************************************************/
void Get_Attitude(void)
{
        IMUupdate(gx*Gyro_Gr, gy*Gyro_Gr, gz*Gyro_Gr, ACC_AVG_X, ACC_AVG_Y, ACC_AVG_Z);        //*0.0174转成弧度
}
/*****************************************/
/***************************************************///一阶龙格-库塔法更新四元数
#define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f                          // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f                   // half the sample period采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // 四元数初始值(1 0 0 0)表示飞控平稳
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
void IMUupdate(float GX, float GY, float GZ, float AX, float AY, float AZ)
{
  float norm;
  float vx, vy, vz;// wx, wy, wz;
  float ex, ey, ez;

  // 先把这些用得到的值算好,提高运算效率
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
//  float q0q3 = q0*q3;
  float q1q1 = q1*q1;
//  float q1q2 = q1*q2;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;
        
  if(AX*AY*AZ==0)//此时飞控姿态平稳，不需要计算姿态
         return;
               
  norm = sqrt(AX*AX+AY*AY+AZ*AZ);//acc数据归一化,防止四元数发散
  AX = AX /norm;
  AY = AY / norm;
  AZ = AZ / norm;

  // estimated direction of gravity and flux (v and w)    估计重力方向和流量/变迁
  vx = 2*(q1q3 - q0q2);        //四元素中xyz的表示
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

  // error is sum of cross product between reference direction of fields and direction measured by sensors
  ex = (AY*vz - AZ*vy) ; //向量外积再相减得到差分就是误差
  ey = (AZ*vx - AX*vz) ;
  ez = (AX*vy - AY*vx) ;

  exInt = exInt + ex * Ki;//对误差进行积分
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  // adjusted gyroscope measurements
  GX = GX + Kp*ex + exInt;        //将误差PI后补偿到陀螺仪，即补偿零点漂移
  GY = GY + Kp*ey + eyInt;
  GZ = GZ + Kp*ez + ezInt;        //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

  // integrate quaternion rate and normalise         //四元数的微分方程
  q0 = q0 + (-q1*GX - q2*GY - q3*GZ)*halfT;
  q1 = q1 + (q0*GX + q2*GZ - q3*GY)*halfT;
  q2 = q2 + (q0*GY - q1*GZ + q3*GX)*halfT;
  q3 = q3 + (q0*GZ + q1*GY - q2*GX)*halfT;

  // normalise quaternion
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  Q_ANGLE_Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw 转换成欧拉角输出
  Q_ANGLE_Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  Q_ANGLE_Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
/********************************************/
void Control(float roll, float pitch, float yaw)
{
        int  MotorPwm1=0,MotorPwm2=0,MotorPwm3=0,MotorPwm4=0;//Pwm占空比参数
         
/************************************///设定PID参数
        char PID_ROL_P = 12;
        char PID_ROL_I = 0;
        char PID_ROL_D = 0.2;
        
        char PID_PIT_P = 12;
        char PID_PIT_I = 0;
        char PID_PIT_D = 0.2;
        
        char PID_YAW_P = 0;
        char PID_YAW_I = 0;
        char PID_YAW_D = 0.5;
        
        char PID_ROL_pout = 0;
        char PID_ROL_iout = 0;
        char PID_ROL_dout = 0;
        
        char PID_PIT_pout = 0;
        char PID_PIT_iout = 0;
        char PID_PIT_dout = 0;
        
        char PID_YAW_pout = 0;
        char PID_YAW_iout = 0;
        char PID_YAW_dout = 0;
/*************************************/
        int PID_ROL_OUT;//PID Output
        int PID_PIT_OUT;//PID Output
        int PID_YAW_OUT;//PID Output
        
        PID_ROL_pout = PID_ROL_P * roll;
        PID_ROL_dout = PID_ROL_D * gx;
        
        PID_PIT_pout = PID_PIT_P * pitch;
        PID_PIT_dout = PID_PIT_D * gy;
        
        PID_YAW_dout = PID_YAW_D * gz;
        
        PID_ROL_OUT = PID_ROL_pout + PID_ROL_iout + PID_ROL_dout;
        PID_PIT_OUT = PID_PIT_pout + PID_PIT_iout + PID_PIT_dout;
        PID_YAW_OUT = PID_YAW_pout + PID_YAW_iout + PID_YAW_dout;
        
        //if(Rc_Get_THROTTLE>1200)
        
                 MotorPwm1 = 100 - PID_ROL_OUT - PID_PIT_OUT + PID_YAW_OUT;
                 MotorPwm2 = 100 + PID_ROL_OUT - PID_PIT_OUT - PID_YAW_OUT;
                 MotorPwm3 = 100 + PID_ROL_OUT + PID_PIT_OUT + PID_YAW_OUT;
                 MotorPwm4 = 100 - PID_ROL_OUT + PID_PIT_OUT - PID_YAW_OUT;
        
        if(Lock)//判断锁上标志        
             PwmOutput( MotorPwm1, MotorPwm2, MotorPwm3, MotorPwm4);//调用产生PWM方波函数驱动电机
        else                        
             PwmOutput(0,0,0,0);
}
/**************************************/
void PwmOutput(unsigned char Pwm1,unsigned char Pwm2,unsigned char Pwm3,unsigned char Pwm4)//uchar 0-255
{
if(Pwm1>255) Pwm1 = 255;//限幅处理
if(Pwm2>255) Pwm2 = 255;
if(Pwm3>255) Pwm3 = 255;
if(Pwm4>255) Pwm4 = 255;

if(Pwm1<0)   Pwm1 = 0;//限幅处理
if(Pwm2<0)   Pwm2 = 0;
if(Pwm3<0)   Pwm3 = 0;
if(Pwm4<0)   Pwm4 = 0;

analogWrite(6, Pwm1);
analogWrite(9, Pwm2);
analogWrite(10, Pwm3);
analogWrite(5, Pwm4);

}
[/mw_shl_code]
回复 支持 反对

举报

zzp
 楼主| 发表于 2014-5-9 19:36 | 显示全部楼层
一些库文件好像发不了。。给个链接https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
这个是MPU6050的库，很强大。我只是用它的accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 函数来读取数据而已，厉害的人还用它的DMP直接产生姿态数据，送到电机PWM产生函数控制电机。根本不用自己写函数解算姿态。。
那个#include <MsTimer2.h>库不发了，相信大家都有。。
回复 支持 反对

举报

zzp
 楼主| 发表于 2014-5-11 14:19 | 显示全部楼层
难道就没人给个说法吗。。
回复 支持 反对

举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

小黑屋|Archiver|手机版|Arduino中文社区

GMT+8, 2024-12-5 03:48 , Processed in 0.079712 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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