2012年03月 文档列表

Bug无处不在……

上周做了个关于Servo库控制电机的小实验,经验证可以把PWM的频率提高到400Hz左右。于是一阵高兴,迅速改好代码,准备周末试飞。

本来计划先做分解实验,拴上绳子一步步测试之后再去试飞。结果星期六刮大风耽误一天,星期天头脑一热,没测试直接去飞了。结果飞机变成炮弹,眼皮还没眨完一下,就已经撞毁在墙壁上了。 :(

回家检查了下程序,果然发现巨大的bug一只。真是心急吃不了热豆腐啊,这个周末需要上班,然后是外地度假7天,再试飞只能是半个月后了。这几天干些乱七八糟的活,新买的4对儿电机电调都焊好了。下次试飞再不成功的话,就准备用wnq朋友的ACM飞控再做一个四轴啦。

另外,如果有电机轴摔弯的朋友,也可以参考wnq推荐的自己换轴的攻略。我试了下,确实比较方便,一根轴才几块钱,相当划算!

无刷电机换轴

无刷电机换轴

另外,还出手了一个JR XG8 2.4G的遥控器,准备下一步把手机遥控改成射频遥控。啧啧,这个东东真是不便宜啊,按照wnq同学的说法,恭喜我上贼船了,不过这个遥控器就我这种级别的玩家,恐怕一辈子都够用了 :)

JR_XG8

JR_XG8

接下来的计划是修改bug等待试飞,闲暇时间学习下ACM飞控和遥控器的使用。

Arduino自带的Servo库小实验

之前有很多朋友问过我,为什么不用Arduino自带的Servo库来控制电机。我在动手做四轴之前,其实也大概看了下Servo库的说明,当时看到

servo.write(angle) 这个函数,其中angle(int)是角度,取值范围是0 到 180

我想当然的认为,这个servo库最多也就是180级的梯度来设置PWM,这样说来精度完全不够啊;另外也没有看到有地方能修改PWM频率。

总结了下我当时不用Servo库的原因:
1. 控制的级数太少,不够精确;
2. 不可修改PWM频率;
3. 需要使用特殊的几个脚;(不知道在什么地方看错了,留下的错误印象)

这几天obK仔给了很多建议,我也看了看源代码。现在看来,上面这几点都是我瞎操心了。

首先,0~180是servo库对应的从min到max的控制,对应于0.9ms到2.1ms的电调来说,angle参数加1对应的时间变化为:
(2.1-0.9)*1000/180 = 6.7微秒,已经是比较精确了。
即使这样如果还觉得不够精确,还可以使用servo.writeMicroseconds,这个函数可以直接用微秒作为参数。

其次,PWM频率是可以修改的,在Servo.h文件中,修改下面这行可以把PWM频率升到400Hz:
#define REFRESH_INTERVAL 2500
不过这只能在编译之前改,不能在程序运行的过程中动态修改。当然修改源代码也可以解决这个问题,把这个参数做成一个变量,用接口进行修改控制即可。

最后,Servo库可以绑定所有digitalWrite的脚,这一点来说,比原生的PWM还方便!

做了一个简单的小程序,测试Servo库同时控制4个电机的情况。实验结果看上去还不错,不过没有测试4个电机转速各不相同的情况。
代码果然看上去简洁多了:

Servo powerServo[4];
void setup()  
{
  motorPins[0] = 38;   //x1
  motorPins[1] = 43;   //x2
  motorPins[2] = 42;   //y1
  motorPins[3] = 39;   //y2
  for(int i = 0; i < 4; i++) powerServo[i].attach(motorPins[i]);
}
void loop()
{
  long t0 = micros(); //记录进入loop的时间
  while(Serial.available()) {
    // 这里是获取控制指令
    pushData(Serial.read());
  }
  double T = 2500; // 一个周期的微秒值
  double len = 900 + ctrlPower * 5;  //(0~200对应总周期0.9ms~1.9ms)
  for(int i = 0; i < 4; i++) powerServo[i].writeMicroseconds(len);

  int leftMs = (int) (t0 + T - micros());
  delayMicroseconds(leftMs);
}

电调的特性实验

最近收到了好多朋友的帮助,在此表示感谢。wnq同学送了很多好东西,并且帮我挑选了一款好用的航模专用遥控器,终于让我下定决定再重新做一个新的四轴。很多快递都还在路上,这几天继续研究老四轴的问题。

今天有位年仅14岁的年轻朋友ENIAC留了言:“对于我开始的商品电调,由于里面自带的 PID 控制器。严重影响了转速的快速反应。这就造成了对于四轴稳定性之一的“自动悬停”基本无法做到了。由于自动悬停的首要要求就是在四轴倾斜时能够在最短时间内自动回到水平位置, 这就要求马达转速在四轴有倾斜时需要加快,而到快回到平衡位置时需要降下来。商品电调里的 PID 恰好阻止了这个的发生。转速上去可以,想要马上降下来?没门!我试验的结果就是一旦转速上去了,想要降下来,等 0.2秒吧。即使这段时间里通知电调降低转速也没效果。带来的后果就是四轴一旦倾斜后就会不可控地晃来晃去,好像抬轿子。

这段文字是ENIAC在无线电上看到的,不得不佩服现在的孩子们,14岁已经开始看这种专业的书籍了。回想我这么大的时候,干的事情基本都是从楼上尿尿,往厕所里扔鞭炮之类的。闲话不多说,我在网上也很容易搜到了相关的讨论。刚看到它的时候,我确实是心头一惊,如果有0.2秒的下降延时,那么我的350Hz的采样,50Hz的控制全白瞎了。但是转念一想,不对啊,现在很多飞的很好(包括悬停动作)的飞控,配套的也是普通的电调啊。

再后来看到一名叫做gale的网友评论:“误区啊。。。 1500元以下的商品电调,根本没有PID。。。连P都没有,何谈ID? 我的经验是商品电调使用挺好的,PPM的信号控制速度也挺快的,目前最低档的KK飞控,也达到了300HZ的控制频率,其所谓的时延问题最主要还是在电机和桨的惯性上。I2C没有看出有明显的必要。” 原文看这里

从这篇讨论贴中的视频看,确实有人用KK飞控+普通电调实现了较为稳定的控制,这样说来我就放心点儿了。另外一个有用的信息是其中提到的300Hz的频率,前几天我刚做了一个实验(企图修改电调PWM频率的实验),当时的结论是电调的频率不可修改。今天突然意识到一个错误的理解:我假定电调是按高电平的占空比来控制电机的,事实上,电调也可能不看占空比,而是只计算高电平的脉宽来控制转速。

加大电调控制频率的方式

加大电调控制频率的方式

上面这幅图表示了从50Hz的控制变成100Hz控制的方式,其中中间那部分是我之前的错误理解,以为高电平的脉宽应该减半,事实上应该是下面那种方式。

为了验证这个想法,特地又做了一个小实验,代码如下:

void loop()
{
  long t0 = micros(); //记录进入loop的时间
  while(Serial.available()) {
    // 这里是获取控制指令
    pushData(Serial.read());
  }
  double frequency = P_Control * 1.5 + 50;  //(0~200对应50到350)
  double T = 1000000 / frequency; // 一个周期的微秒值
  double len = 900 + ctrlPower * 5;  //(0~200对应总周期0.9ms~1.9ms)
  for(int i = 0; i < 4; i++) digitalWrite(motorPins[i], HIGH);
  delayMicroseconds(len);
  for(int i = 0; i < 4; i++) digitalWrite(motorPins[i], LOW);
  int leftMs = (int) (t0 + T - micros());
  if(leftMs < 1 ) leftMs = 1;
  if(leftMs > 2000) {
    int ms = leftMs >> 10 - 1;
    delay(ms);
    delayMicroseconds(leftMs - ms<<10);
  } else {
    delayMicroseconds(leftMs);
  }

其中P_Control和ctrlPower是我从手机遥控器传入的参数,偷了个懒保留了P_Control的变量名,其实它在这里是用来调节PWM周期的。

手机端截图

手机端截图

其中ctrlPower就是图中那个油门值了。这个实验表明,普通商品电调的控制频率确实可以修改的!
我之前的控制频率是50Hz,确实有点少了,反应比较慢,如果推理正确的话,也许可以加到到300Hz左右。

今天有点生病了,做完这个实验,有点头痛欲裂的感觉。把接下来打算做的事情整理一下,然后去睡觉啦:
1. 修改电调的控制频率,看看是否能让飞行器更平稳
2. 四个电机轴都已经有点歪了,买了几个新电机,也买了几根新的电机轴,看看能不能自己修好
3. 给老四轴加上螺旋桨保护罩,前几天听说有人玩航模砸死了过路人,罪过啊,大家也一定要多加小心!
4. 用wnq朋友的飞控和机架组装一套新四轴,改为用遥控器控制
5. SSS_SXS朋友提到了Arduino自带的PID库,这个可以考虑替换掉自己的PID

截止到3月1号的四轴飞行器代码

应SSS_SXS建议,把目前的代码贴出来供大家分析。其中注释有点乱,有英文有中文,凑合看吧。

【特别感谢】有一位非常热心的朋友wnq送了我几套木质机架,还有一套CACM的飞控+GPS模块。所以最近正在计划重新用成品飞控搭一套四轴找找感觉。原来的四轴暂时不打算拆,正在淘宝各种散件和遥控器。另外电机轴也摔歪了,所以准备按wnq的攻略把几根轴都换一换。

下面这段代码是3月1号最后一次修改,很遗憾最近这段时间一直没啥更新。
大致思路是先计算出飞行姿态角AngleX和AngleY,然后根据角度和角速度,计算出blanceX和blanceY。
这两个参数的含义是,blanceX表示x轴两个电机的参数分配比例,例如balanceX = 1,表示x1和x2两个电机转速相同,
balanceX = 0.5,表示x1的转速是0.5,x2转速是1.5。
Y轴的概念是类似的,之所以这样处理,是期望x轴两个电机的转速和大致和y轴两个电机相同,这样飞行器不至于打转。
另外,还有个balanceXY,用于在电机转速不均匀的时候,微调x轴电机和y轴电机的转速比,这个本来是打算在电子罗盘的应用中加上的,现在暂时保持等于1。

#include 
#include 

/**
 * 记录:目前较为稳定的实验参数为33, 66,挂绳貌似能垂直起降,但是真正试飞时依然侧飞
 * 试飞时,在3秒内飞到10米高,测距周期是100ms,对应的距离是33cm
 * 起飞时:
 * 1. 遥控在0~directCtrlRange时,按n*5计算转速,方便调试
 * 2. 遥控>directCtrlRange时,按n-directCtrlRange+10计算高度,油门按一定速度缓慢上升,直到离地指定的高度
 * 3. 超声波测距量程是2~180cm,不知道为什么超出量程时会读出7cm的数值,必须注意纠错
 */
boolean connectToPc = false;
long stableTime = 1500000;
int directCtrlRange = 40;
double fastStep = 1.8;
double maxThrottle = 650;

/* Gyro sensor */
int GYRO_ADDR = 0x68; // 7-bit address: 1101000, change to 0x69 if SDO is High
byte GYRO_READ_START = 0xA8; //Start register address, total 6 bytes: (XL,XH,YL,YH,ZL,ZH)
int gyroXYZ[3];
int gyroOffset[3];
int gyroCS_Pin = 4;
int gyroSDO_Pin = 5;

/*  Accessory sensor  */
int ACC_ADDR = 0x53; // 7-bit address, change to 0x53 if SDO is High
byte ACC_READ_START = 0x32; //Start register address, total 6 bytes: (XL,XH,YL,YH,ZL,ZH)
int accXYZ[3];
int accOffset[3];

/* Compass sensor */
int COMP_ADDR = 0x3C >> 1;
byte COMP_READ_START = 0x03;
int compXYZ[3];   //Notice that it is XZY

/* Motor settings */
int motorPins[4];
int minPWM = 900; //电机控制的PWM最低幅值为0.9ms,最高幅值为2.1ms
int maxPWM_Delay = 1100; //电机控制的PWM范围,这里记录它们的差并留一点裕度
int ctrlPower = 0; //从遥控器发来的控制指令
double throttle = 0;  //油门范围,从0到1200,对应从minPWM到2100
int motorPowers[4];
int motorCtrl[8]; //电机控制的顺序和时间,值表示从0.9ms开始,(delayMs-PinIdx)*4

/* Ultrasonic sensor */
int trigPin = 3;
int echoPin = 2;  //对应的中断号是0
volatile double ultraT1 = 0;
volatile double ultraT2 = 0;
double distance = 0; //测量的离地距离,单位是厘米
double preDistance = 0;
double stablePower = 0;  //正好可以离地而起的马力(假设保持这个马力,可以匀速上升或下降)
int maxDistance  = 250; //离地可测量的最大高度
int targetDistance = 10; //目标离地高度
int ultraCount = 0;

/* For testing */
int reportType = 0;
int reportArr[3];
int reportT[3];

/* Read from control */
int buffer[6];   //0,1: 0xFF, 2: Power, 3: Log Type, 4: P_Value, 5: I_Value
int bufferIndex = 0;

/* PID console */
double P_Control, I_Control;
double balanceGyro = 0.99; //用于融合陀螺仪和加速度计的角度
double balanceAcc = 0.9; // 加速度传感器读数平均,用于消除抖动
double balanceSpeed = 0.05; // 用于陀螺仪读数平均(陀螺仪比较精确)
double balanceXY = 1, balanceX = 1, balanceY = 1;
double angleX, angleY, angleZ, speedX, speedY, accAngleX, accAngleY;
double testAngleX;
double compAngle;
boolean hasReadCompAngle = false;

/* Main control start */
void setup()
{
  motorPins[0] = 38;   //x1
  motorPins[1] = 43;   //x2
  motorPins[2] = 42;   //y1
  motorPins[3] = 39;   //y2
  for(int i = 0; i < 4; i++) pinMode(motorPins[i], OUTPUT);
  Serial.begin(9600);
  Wire.begin();
  pinMode(gyroCS_Pin, OUTPUT);
  pinMode(gyroSDO_Pin, OUTPUT);
  digitalWrite(gyroCS_Pin, 1); //enable I2C for GYRO
  digitalWrite(gyroSDO_Pin, 0); //set I2C address last bit
  setupGyroSensor(250);
  setupAccSensor();
  setupCompSensor();
  pinMode(trigPin, OUTPUT);
  attachInterrupt(0, blink, CHANGE);
  digitalWrite(trigPin, 0);
  // 从EEPROM中读取零点的校准值
  for(int i = 0; i < 3; i++) {
    gyroOffset[i] = EEPROM.read(2 * i + 1) << 8;
    gyroOffset[i] |= EEPROM.read(2 * i);

    accOffset[i] = EEPROM.read(7 + 2 * i) << 8;
    accOffset[i] |= EEPROM.read(6 + 2 * i);
  }
}

// 用于处理超声波传感器产生的中断,尽量减少在中断函数中处理逻辑
void blink()
{
  if(ultraT1 == 0) ultraT1 = micros();
  else ultraT2 = micros();
}

int loopIndex = 0;
void loop()
{
  if(reportType == 103) {
    /* 校准模式,在静止的平地上测量N次读数,记录零点误差
       校准隔一段时间做一次即可,例如季节变化,地点变化等 */
    gyroOffset[0] = gyroOffset[1] = gyroOffset[2] = 0;
    accOffset[0] = accOffset[1] = accOffset[2] = 0;
    int repeatTime = 100;
    for(int i = 0; i < repeatTime; i++) {
      // 单次读数大约6ms,校准读数总耗时在0.3秒左右
      readGyroSensor();
      readAccSensor();
      for(int j = 0; j < 3; j++) {
        gyroOffset[j] += gyroXYZ[j];
        accOffset[j] += accXYZ[j];
      }
      delay(10);
    }
    // 读数完毕写入EEPROM,每个字节需要3.3ms写入
    for(int i = 0; i < 3; i++) {
      gyroOffset[i] /= repeatTime;
      EEPROM.write(2 * i, (byte) (gyroOffset[i] & 0xFF));
      EEPROM.write(2 * i + 1, (byte) ((gyroOffset[i] >> 8) & 0xFF));
      accOffset[i] /= repeatTime;
      EEPROM.write(6 + 2 * i, (byte) (accOffset[i] & 0xFF));
      EEPROM.write(7 + 2 * i, (byte) (accOffset[i] >> 8 & 0xFF));
    }
    // 校准完成,恢复成普通模式
    reportType = 0;
  }
  /*
   * 采样率为350Hz,所以每个周期2.857ms。每个周期中都需要读一次陀螺仪和加速度计,
   * 用时大约0.7ms;然后剩余的时间分别用来做不同的事情,例如控制电机,计算角度等。
   * 然后进行角度姿态的融合,大约需要0.7ms。这部分放在控制电机的后面,因为0.9ms开始
   * 就需要控制电机,时间不够用,放在后面则刚好够用。
   * 电机的控制周期是20ms,也就是每7次循环修改一次电机功率
   */
  long t0 = micros(); //记录进入loop的时间
  if(loopIndex == 1) {
    for(int i = 0; i < 4; i++) digitalWrite(motorPins[i], HIGH);
  } else {
    // digitalWrite需要20微秒,为了读数时间均匀,其他情况统一做相应延时
    delayMicroseconds(20);
  }
  // 读取陀螺仪和加速度计的读数,前1.5秒等待传感器稳定
  if(t0 > stableTime) {
    readGyroSensor();
    readAccSensor();
  }
  if(connectToPc && loopIndex == ctrlPower % 7) reportT[0] = (int) (micros() - t0);
  double newSpeedX, newSpeedY, newAngleX, newAngleY, power, powerX, powerY, adjust;
  int loopCountLimit;
  switch(loopIndex) {
    case 0:
      // 这时候角度数值已就绪:angleX, angleY, angleZ, speedX, speedY
      // 但是需要修正45度安装的换算
      newSpeedX = (speedX + speedY) * 0.707;
      newSpeedY = (speedY - speedX) * 0.707;
      // 假设四轴翻转不会超过90度
      newAngleX = asin((sin(angleX) + sin(angleY)) * 0.707);
      newAngleY = asin((sin(angleY) - sin(angleX)) * 0.707);
      // 计算平衡参数
      adjust = 1;
      if(throttle > 350) adjust = 500.0 / throttle;
      balanceX = 1 - (newSpeedX * P_Control + newAngleX * I_Control) * adjust;
      balanceX = min(max(balanceX, 0), 2);
      balanceY = 1 - (newSpeedY * P_Control + newAngleY * I_Control) * adjust;
      balanceY = min(max(balanceY, 0), 2);
      // 计算油门值
      if(ctrlPower <= directCtrlRange) {
        if(throttle > ctrlPower * 5) {
          // 缓冲下降20ms下降0.8,1秒钟下降40
          throttle -= fastStep;
        } else {
          // 上升之前可以不考虑缓冲问题
          throttle = ctrlPower * 5;
        }
      } else {
        // 目标离地高度为ctrlPower - directCtrlRange + 10;
        targetDistance = ctrlPower - directCtrlRange + 10;
        if(stablePower == 0 && distance > preDistance + 2) {
          // 100ms内爬升了2cm以上,设置2主要是为了避免读数跳动误差
          stablePower = throttle;
        }
        if(stablePower == 0) {
          // 处于起飞状态
          throttle += fastStep;
        } else {
          // 可以认为已经离地
          double newThrottle = stablePower + (targetDistance - distance) * 0.11- (distance - preDistance) * 0.13;
          if(newThrottle > throttle + fastStep) throttle += fastStep;
          else if(newThrottle < throttle - fastStep) throttle -= fastStep;
          else throttle = newThrottle;
        }
        // 油门保护,不超过某个最大值(避免飞太快超出控制范围)
        if(throttle > maxThrottle) throttle = maxThrottle;
        // directCtrlRange优先控制权
        if(throttle < directCtrlRange * 5) throttle = directCtrlRange * 5;
      }
      // 计算电机功率
      power = throttle;
      powerX = power * balanceXY;
      powerY = power * 2 - powerX;
      motorPowers[0] = (int) (powerX * balanceX);
      motorPowers[1] = (int) (powerX * 2) - motorPowers[0];
      motorPowers[2] = (int) (powerY * balanceY);
      motorPowers[3] = (int) (powerY * 2) - motorPowers[2];
      motorCtrl[0] = 9999;
      for(int i = 0; i < 4; i++) {
        motorPowers[i] += 5 * i;
        if(motorPowers[i] > maxPWM_Delay) motorPowers[i] = maxPWM_Delay;
        if(motorPowers[i] < motorCtrl[0]) {
          motorCtrl[0] = motorPowers[i];
          motorCtrl[1] = i;
        }
      }
      // 排序, 设置motorCtrl
      for(int i = 1; i < 4; i++) {
        int pre = motorCtrl[i * 2 - 2];
        int preIdx = motorCtrl[i * 2 - 1];
        motorCtrl[i * 2] = 9999;
        for(int j = 0; j < 4; j++) {
          if(motorPowers[j] > pre || (motorPowers[j] == pre && j > preIdx)) {
            if(motorPowers[j] < motorCtrl[i * 2]) {
              motorCtrl[i * 2] = motorPowers[j];
              motorCtrl[i * 2 + 1] = j;
            }
          }
        }
      }
      // 计算delayMs
      for(int i = 0; i < 3; i++) {
        motorCtrl[6 - i * 2] -= (motorCtrl[4 - i * 2] + 5);
      }
      break;
    case 1:
      // 控制电机转速,首先运行到0.9ms
      delayMicroseconds(minPWM + t0 - micros());
      for(int i = 0; i < 4; i++) {
        // 根据间隔时间设置低电平
        if(motorCtrl[i * 2] > 0) delayMicroseconds(motorCtrl[i * 2]);
        digitalWrite(motorPins[motorCtrl[i * 2 + 1]], LOW);
      }
      break;
    case 2:
      // 电子指南针读数,这个不需要太精确,所以20ms读一次
      if(t0 > stableTime) {
        readCompSensor();
      }
      // 从串口获取遥控信息
      loopCountLimit = 18;
      while(Serial.available() && loopCountLimit--) {
        pushData(Serial.read());
      }
      break;
    case 3:
      // 发送各种报告
      if(connectToPc) {
        if(reportType == 1) writeBuffer(gyroOffset, 3);
        if(reportType == 2) writeBuffer(accOffset, 3);
        if(reportType == 3) writeBuffer(compXYZ, 3);
        //if(reportType == 1) writeNums(motorCtrl[0],motorCtrl[1],motorCtrl[2]);
        //if(reportType == 2) writeNums(motorCtrl[3],motorCtrl[4],motorCtrl[5]);
        //if(reportType == 3) writeNums(motorCtrl[6],motorPowers[2],motorPowers[3]);
        if(reportType == 4) writeBuffer(reportArr, 3);
        if(reportType == 5) writeNums((int)distance, (int)(speedX * 1000), motorPowers[0]);
        //if(reportType == 5) writeNums(reportArr[1], motorPowers[2], motorPowers[3]);
        if(reportType == 6) writeBuffer(reportT, 3);
      }
      break;
    case 4:
      // 测量离地距离,音速340m/s = 34cm/ms = 0.034cm/μs,测量的是来回距离,所以需要除以2
      if(ultraT1 > 0 && ultraT2 > ultraT1) {
        double newDistance = (ultraT2 - ultraT1) * 0.017;
        if(newDistance < 250) {
          preDistance = distance;
          if(distance > 100 && newDistance < 10) {
            // 高度突然变小的情况(从100以上变到小于10,可以认为是超出量程)
            distance = maxDistance;
          } else {
            distance = newDistance;
          }
        }
      }
      // 考虑到回音等问题,把测量的时间间隔增加到100ms,相当于17米的反射
      ultraCount++;
      if(ultraCount >= 5) {
        ultraT1 = 0;
        ultraT2 = 0;
        // 因为每次执行到这个循环是20ms,所以计数器增长到5时进行一次测量
        ultraCount = 0;
        // 用10μs的高电平发出触发信号
        digitalWrite(trigPin, 1);
        delayMicroseconds(10);
        digitalWrite(trigPin, 0);
      }
      break;
    default:
      // 其他扩展功能
      break;
  }
  if(connectToPc && loopIndex == ctrlPower % 7) reportT[1] = (int) (micros() - t0);
  // 计算角度angleX, angleY, speedX, speedY, accAngleX, accAngleY
  if(t0 > stableTime) {
    // 量程为250时,测量值单位是8.75 mdps/digi;
    // 正确:180/3.1415926*1000/8.75 = 6548.09
    // 错误:3754.94 = (65536/1000)*(180/3.1415926)
    double readSpeedX = (gyroXYZ[0] - gyroOffset[0]) / 6548.09; //单位:弧度/秒
    speedX = balanceSpeed * speedX + (1 - balanceSpeed) * readSpeedX;
    accAngleX = accAngleX * balanceAcc + atan2(accXYZ[0] - accOffset[0], accXYZ[2]) * (1 - balanceAcc);
    angleX = (angleX + readSpeedX * 0.002857) * balanceGyro + accAngleX * (1 - balanceGyro);
    testAngleX = testAngleX + readSpeedX * 0.002857;
    reportArr[0] = (int) (angleX * 1000);
    reportArr[1] = gyroXYZ[0] ;
    reportArr[2] = (int) (testAngleX * 1000);
    double readSpeedY = (gyroXYZ[1] - gyroOffset[1]) / 6548.09; //单位:弧度/秒
    speedY =  balanceSpeed * speedY + (1 - balanceSpeed) * readSpeedY;
    accAngleY = accAngleY * balanceAcc + atan2(accXYZ[1] - accOffset[1], accXYZ[2]) * (1 - balanceAcc);
    angleY = (angleY + readSpeedY * 0.002857) * balanceGyro + accAngleY * (1 - balanceGyro);
  }
  if(loopIndex == ctrlPower % 7) reportT[2] = (int) (micros() - t0);
  // 更新loopIndex
  loopIndex++;
  if(loopIndex >= 7) loopIndex = 0;
  // 截止到2.857ms
  t0 = micros() % 2857;
  delayMicroseconds(2817 - t0);
}

/* get data from bluetooth devices */
void pushData(byte data)
{
  //验证是否数据的开始
  if (bufferIndex < 1 && data != (byte)0xFF) {
    bufferIndex = 0;
  } else {
    buffer[bufferIndex] = data;
    if (bufferIndex == 5) {
      setParams();
    }
    bufferIndex = (bufferIndex + 1) % 6;
  }
}

void setParams()
{
  // 因为超声波中断可能导致数据丢失,所以需要额外的一个字节用于校验
  if(buffer[1] ^ buffer[2] ^ buffer[3] ^ buffer[4] == buffer[5]) {
    ctrlPower = buffer[1];
    reportType = buffer[2];
    //P_Control = (20.0 + buffer[3] / 4.0) / 200.0;
    //I_Control = (20.0 + buffer[4] / 4.0) / 200.0;
    P_Control = buffer[3] / 100.0;
    I_Control = buffer[4] / 100.0;
    if(reportType == 100 && !connectToPc) {
      Serial.end();
      connectToPc = true;
      Serial.begin(115200);
    }
    if(reportType == 101) {
      // 设置为上拉绳调试模式
      directCtrlRange = maxThrottle / 5;
    }
    if(reportType == 102) {
      // 设置为下拉绳调试模式
      directCtrlRange = 40;
    }
    //if(reportType == 103) {} //校准模式
  }
}

/*****************************************************************************/

void writeBuffer(int arr[], int len) {
  byte buff[len * 2 + 2];
  buff[0] = 0xFF;
  buff[1] = 0xFF;
  for(int i = 0; i < len; i++) {
    buff[i * 2 + 2] = (byte) (arr[i] >> 8 & 0xFF);
    buff[i * 2 + 3] = (byte) (arr[i] & 0xFF);
  }
  Serial.write(buff, len * 2 + 2);
}

void writeNums(int n1, int n2, int n3) {
   byte buff[8];
   buff[0] = 0xFF;
   buff[1] = 0xFF;
   buff[2] = (byte) (n1 >> 8 & 0xFF);
   buff[3] = (byte) (n1 & 0xFF);
   buff[4] = (byte) (n2 >> 8 & 0xFF);
   buff[5] = (byte) (n2 & 0xFF);
   buff[6] = (byte) (n3 >> 8 & 0xFF);
   buff[7] = (byte) (n3 & 0xFF);
   Serial.write(buff, 8);
}

/*****************************************************************************/

void writeRegister(byte deviceAddress, byte address, byte val) {
    Wire.beginTransmission(deviceAddress); // start transmission to device
    Wire.send(address);       // send register address
    Wire.send(val);         // send value to write
    Wire.endTransmission();     // end transmission
}

byte readRegister(int deviceAddress, byte address){
    Wire.beginTransmission(deviceAddress);
    Wire.send(address);
    Wire.endTransmission();
    Wire.requestFrom(deviceAddress, 1);
    while(!Wire.available()) {}
    return Wire.receive();
}

/*****************************************************************************/

int setupGyroSensor(int scale){
  byte GYRO_CTRL_REG1 = 0x20;
  byte GYRO_CTRL_REG2 = 0x21;
  byte GYRO_CTRL_REG3 = 0x22;
  byte GYRO_CTRL_REG4 = 0x23;
  byte GYRO_CTRL_REG5 = 0x24;
  writeRegister(GYRO_ADDR, GYRO_CTRL_REG1, 0x0f);
  writeRegister(GYRO_ADDR, GYRO_CTRL_REG2, 0x00);
  writeRegister(GYRO_ADDR, GYRO_CTRL_REG3, 0x08);
  if(scale == 250){
    writeRegister(GYRO_ADDR, GYRO_CTRL_REG4, 0x00);
  }else if(scale == 500){
    writeRegister(GYRO_ADDR, GYRO_CTRL_REG4, 0x10);
  }else{
    writeRegister(GYRO_ADDR, GYRO_CTRL_REG4, 0x30);
  }
  writeRegister(GYRO_ADDR, GYRO_CTRL_REG5, 0x00);
}

void readGyroSensor(){
    Wire.beginTransmission(GYRO_ADDR);
    Wire.send(GYRO_READ_START);
    Wire.endTransmission();
    Wire.requestFrom(GYRO_ADDR, 6);
    for(int i = 0; i < 3; i++) {
      while(!Wire.available()) {}
      gyroXYZ[i] = Wire.receive();  //Low byte
      while(!Wire.available()) {}
      gyroXYZ[i] |= (Wire.receive() << 8); //High byte
    }
}

/*****************************************************************************/
void readAccSensor(){
    Wire.beginTransmission(ACC_ADDR);
    Wire.send(ACC_READ_START);
    Wire.endTransmission();
    Wire.requestFrom(ACC_ADDR, 6);
    for(int i = 0; i < 3; i++) {
      while(!Wire.available()) {}
      accXYZ[i] = Wire.receive();  //Low byte
      while(!Wire.available()) {}
      accXYZ[i] |= (Wire.receive() << 8); //High byte
    }
}

int setupAccSensor(){
  writeRegister(ACC_ADDR, 0x2D, 0x28);  //Power control
}

/*****************************************************************************/

void readCompSensor(){
    Wire.beginTransmission(COMP_ADDR);
    Wire.send(COMP_READ_START);
    Wire.endTransmission();
    Wire.requestFrom(COMP_ADDR, 6);
    for(int i = 0; i < 3; i++) {
      while(!Wire.available()) {}
      compXYZ[i] = Wire.receive();  //High byte
      while(!Wire.available()) {}
      compXYZ[i] = Wire.receive() | (compXYZ[i] << 8); //Low byte
    }
    // Switch xzy to xyz
    int temp = compXYZ[1];
    compXYZ[1] = compXYZ[2];
    compXYZ[2] = temp;
}

int setupCompSensor() {
  writeRegister(COMP_ADDR, 0x00, 0x70); //Mod setting
  writeRegister(COMP_ADDR, 0x02, 0x00); //Mod setting
}

/*****************************************************************************/

带好盈程序的电子调速器控制

最近制作四轴飞行器陷入困境,用手抓住它左右摇晃,能够感受到螺旋桨明显的阻力。但是PID参数调节的结果,要么就是飞行器剧烈摇摆失控,要么就是起飞后向一侧斜飞,很难达到一个较为稳定的状态。

应朋友们的建议,先陆陆续续写一些攻略和步骤。一来可以相当于做好笔记,二来也可以请大家帮忙查找原因,出谋划策。

前面几篇提到了目前控制电机的一些问题,正好在这里介绍下我控制电机的方式。

电机

首先,航模的电机一般都是无刷电机,这种高速的无刷电机不像直流电机那样,接上一定电压就可以旋转,一般需要在多个电源线中输入周期性的交流电。例如我买的电机就是下面这样的:

无刷电机

无刷电机

它有三根输入电源线,如果用单片机直接输出交变的电信号给它,因为单片机输出功率太低,是转不动的。
如果把单片机的输出经过电流放大电路,例如L298N,事实上很多步进电机就是这样控制的。它的问题在于一个单片机只能控制几个电机,并且很难再做其他的计算了。

电调

由于上面的原因,我们必须采用专门控制电机转速的设备,就叫做电调(全称电子调速器,英文electronic speed controller ,简称ESC)。它的作用就是接受单片机的PWM指令,并且生成成无刷电机需要的高频交流电。

电调

电调

大家可以看到电调有很多线,其中两根粗线连接的是电源的正负极;三根输出线分别连电机的三根输入线(如果需要电机反转,任意对换两根接线即可);另外还有三根细线,分别是单片机接地,5V输出电源和PWM信号线。

 根据航模标准,PWM信号线的频率应该是50Hz,对应的每个周期总时长是20ms。经过我的实测,基本上超过55Hz以后就不转了。
在这个20ms的周期中,高电平的持续时间应该在0.92ms到2.120ms之间,分别对应电机的最低转速和最高转速。

我曾经在想为什么只给转速控制留了这么小的PWM范围?如果是按Arduino系统PWM输出的话,用0~255来输出20ms之间的PWM值,有效控制转速的值范围大约只有pwm=(20~40)之间。后来大概明白了,这是为了给单片机节约宝贵的CPU资源,这样才可以在剩余的18ms中处理其他事务。

好盈程序

看电调的说明书,它有很多功能。比如设置电池的特性,设置刹车特性,设置最大转速等等。这些功能都可以通过遥控器进行设置,其中大部分功能我们使用默认值就可以,只有初始化一项是必须做的。
具体的设置方法是,把油门推到最大(2.12ms),这时候电调就会滴滴几声提醒你进入设置模式,然后就可以把油门迅速拉到最低(0.92ms)保持一会儿之后,电调就可以初始化成功了。

新手可能会有和我一样的疑问:把油门推到最大的时候,会不会电机突然暴走起来?
所以这里需要强调的是,电调的使用一定要按照说明书操作。单片机通电的时候,需要立刻给电调输出最低油门的PWM值(0.92ms),经过我的实验,0.85或0.95也没有太大关系。再低的话就会滴滴滴的报警,告诉你没有初始化成功;再高的话(0.95到2.1之间)只会有一声警报,但是无法正常转动;再再高到最高油门值(2.12ms)的时候,就会提醒你进入设置模式。

这样做是安全的,如果你不慎把遥控器留到了高位,这时候给电机通电,不至于突然上升到非常高的转速。前几天我企图修改PWM的时候,发生了一次这样的失误,手机控制的参数一下子升到最大,螺旋桨的风力大到手几乎抓不住,真是吓的屁滚尿流……

初始化和调速

之前有提到过用电脑通过串口通信控制电机,这里也是类似,在PC上写一段小程序,可以输出0~200之间的一个整数到Arduino。
在Arduino上,分别把0对应到0.9ms的高电平;200对应到2.1ms的高电平。

void loop()
{
  long t0 = micros(); //记录进入loop的时间
  while(Serial.available()) {
    // 这里是获取控制指令
    pushData(Serial.read());
  }
  //ctrlPower从串口读取,0~200分别对应高电平脉宽0.9ms~2.1ms
  double len = 1000 * (0.9 + ctrlPower * (2.1 - 0.9) / 200.0));
  // 输出高电平
  for(int i = 0; i < 4; i++) digitalWrite(motorPins[i], HIGH);
  // 延时len毫秒
  delayMicroseconds(len);
  // 输出低电平
  for(int i = 0; i < 4; i++) digitalWrite(motorPins[i], LOW);
  // 等待一段时间,形成20ms的周期
  delayMicroseconds(t0 + 20000 - micros());
}

其中ctrlPower就是从PC端读取的那个控制值。新电机和电调买回来,把4个装到一起,然后从这个程序统一进行初始化和控制转速。强烈建议第一次玩电机的朋友,先不要装上螺旋桨,应该等调试胸有成竹之后再安装。 :)

企图修改电调PWM频率的实验

之前控制电调的PWM频率是50Hz,怀疑可能是这个频率较低,所以导致了四轴飞行器无法平衡。今天特地做了一个小实验,用两个参数:
一个调节油门,用0~200控制油门的高低;
一个调节频率,计划从50Hz实验到450Hz。

我使用的电调是天行者SKYWALKER 20A无刷电调。在实验中差点发生危险,当频率不在50Hz时,调节油门电机完全不动;这时候把频率调回50Hz,因为之前的油门值设置的很大,所以瞬间就升到了很大的转速,一片有裂纹的叶片直接甩断飞出来。可能是震动太大把蓝牙弄松了,所以用手机关闭电源未遂,最后我从后面拔掉了电源线才算完事。
当然目前就说PWM不可改有点武断,可能代码中还有逻辑错误。今晚不打算试了,虎口震的发麻,心有余悸的……

小结:
1. 任何时候,开通电机前都要戴好护目镜;
2. 测试电调和电机时,应该把叶片拆掉;
3. 调节参数时,应该先把一些危险参数还原;

下面是使用的代码,明天拆掉页面继续实验。

void loop()
{
  long t0 = micros(); //记录进入loop的时间
  while(Serial.available()) {
    // 这里是获取控制指令
    pushData(Serial.read());
  }
  double frequency = ctrlFreq * 2 + 50;  //(ctrlFreq 0~200对应50到450)
  double T = 1000000 / frequency; // 一个周期的微秒值
  double len = (0.9 + ctrlPower / 200.0) * T / 20;  //(ctrlPower 0~200对应总周期0.9ms~1.9ms)
  delayMicroseconds(t0 + T - len - micros());
  for(int i = 0; i < 4; i++) digitalWrite(motorPins[i], HIGH);
  delayMicroseconds(len);
  for(int i = 0; i < 4; i++) digitalWrite(motorPins[i], LOW);
}

半个月没啥进展

上次说到减小重力传感器的读数权重,得到了比较好的角度波形图。充满希望的去试飞了一次,结果还是非常不稳定。
正好这两周很忙,这件事情就被搁置了,正好有时间仔细的回想思考问题。

这次又发现了一个潜在的可能原因,就是控制电调(电子调速器)的PWM频率。当时看说明书,提到电调需要工作在50Hz的PWM条件下,这样一来,实际上我费尽心思把角度读数采样提高到350Hz是没有意义的,因为读7次角度后才会修改一次电机转速。

刚才搜到了一篇文章,提到了以下内容(来自5imx
控制频率低,普通电调在设计的时候大多考虑的是标准PWM(PPM)脉宽,而标准PWM(PPM)的控制频率只有50Hz即20ms一个周期。虽然标准的PWM只有50Hz的控制频率,但多数电调都能工作在比这个高的多的范围内而不会有任何异常。在我们测试过的电调中,它们基本上都能很好的工作在200Hz到300Hz的控制频率上,但这个频率只能满足一般的需求。

这样说来,适当提高PWM的频率还是有点希望的,但是这个是不是问题的罪魁祸首还不能确定,只能改改看了。
之前做磁悬浮的时候,曾经找过资料,可以把Arduino的系统PWM提高到1000Hz以上,所以看上去比较稳定。但是这个系统的PWM用在四轴上却有点问题,因为电调的转速区间很小。对于50Hz(20毫秒)的PWM来说,控制转速的区间大约在0.9ms到2.1ms之间。对应的Arduino的PWM值范围是12到26之间,也就是说从最低转速调到最高转速,只有14级的范围。这个跳跃的范围有点过大了,而且从我实测的经验看,19还飞不起来,20可能就已经飞的很高了 :(

所以这次还需要对程序进行比较大的修改,同样需要在代码中直接用DigitalWrite操作PWM输出,希望这次能有好运!