Bug无处不在……
由 动力老男孩 发表于 2012/03/28 23:04:48上周做了个关于Servo库控制电机的小实验，经验证可以把PWM的频率提高到400Hz左右。于是一阵高兴，迅速改好代码，准备周末试飞。
本来计划先做分解实验，拴上绳子一步步测试之后再去试飞。结果星期六刮大风耽误一天，星期天头脑一热，没测试直接去飞了。结果飞机变成炮弹，眼皮还没眨完一下，就已经撞毁在墙壁上了。 
回家检查了下程序，果然发现巨大的bug一只。真是心急吃不了热豆腐啊，这个周末需要上班，然后是外地度假7天，再试飞只能是半个月后了。这几天干些乱七八糟的活，新买的4对儿电机电调都焊好了。下次试飞再不成功的话，就准备用wnq朋友的ACM飞控再做一个四轴啦。
另外，如果有电机轴摔弯的朋友，也可以参考wnq推荐的自己换轴的攻略。我试了下，确实比较方便，一根轴才几块钱，相当划算！
无刷电机换轴
另外，还出手了一个JR XG8 2.4G的遥控器，准备下一步把手机遥控改成射频遥控。啧啧，这个东东真是不便宜啊，按照wnq同学的说法，恭喜我上贼船了，不过这个遥控器就我这种级别的玩家，恐怕一辈子都够用了 
JR_XG8
接下来的计划是修改bug等待试飞，闲暇时间学习下ACM飞控和遥控器的使用。
& 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
}
/*****************************************************************************/
