|
8266开源项目 第二讲esp8266陀螺仪自稳
//ail副翼,ele升降舵,thr油门,rud方向舵
//ch1 ail 副翼(左右侧飞/横滚) 对应 roll 左右侧飞
//ch2 ele 升降舵前后(机头向下/上 升降舵) 对应 pitch 前后
//ch3 thr 油门 直升机叫 螺距 四轴 thr油门
//ch4 rud 方向舵(方向左右) 对应于yaw 方向
#include <Servo.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#define RECEIVE_PIN 2 //IO2引脚
#define CHANNEL_AMOUNT 8 //8个PWM通道到
#define DETECTION_SPACE 2500
#define METHOD RISING
int ch[CHANNEL_AMOUNT + 1];
MPU6050 mpu;
int16_t ax, ay, az, gx, gy, gz; //加速度计陀螺仪原始数据
//舵机云台组件
Servo aservo; //副翼
Servo eservo; //升降
Servo rservo; //方向
Servo tservo; //油门
int servoA = 12; //副翼io
int servoE = 13; //升降io
int servoR = 14; //方向io
int servoT = 16; //油门io
int anew=90;//副翼数据
int enew=90;//升降数据
int rnew=90;//方向数据
int tnew=90;//油门数据
void setup(){
Serial.begin(115200);
pinMode(RECEIVE_PIN, INPUT_PULLUP);
Wire.begin();
Serial.println("Initializing I2C devices...");
mpu.initialize();
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
pinMode(servoA, OUTPUT);//定义端口为OUTPUT输出模式
pinMode(servoE, OUTPUT);//定义端口为OUTPUT输出模式
aservo.write(90);
aservo.attach(servoA);
eservo.write(90);
eservo.attach(servoE);
attachInterrupt(digitalPinToInterrupt(RECEIVE_PIN), ppm_interrupt, METHOD);
}
void loop(){
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
ax = map (ax, -17000, 17000, 5, 180) ;
ay = map (ay, -17000, 17000, 0, 120) ;
az = map (az, -17000, 17000, 5, 180) ;
//循环打印通道值,取1-8的值
for (byte i = 0; i < CHANNEL_AMOUNT + 1; i++){
// Serial.print(ch[i]);
// Serial.print("\t");
//;
}
Serial.println("");
anew=(90+(1500-ch[1])/5);
enew=(90+(1500-ch[2])/5);//500:90的系数
// eservo.write(enew);
// aservo.write(anew);
Serial.print(ax);
eservo.write(ay);
aservo.write(ax);
}
ICACHE_RAM_ATTR void ppm_interrupt(){
static byte i;
static unsigned long int t_old;
unsigned long int t = micros(); //储存时间值t当引脚值下降/上升时
unsigned long int dt = t - t_old; //计算两个峰之间的时间
t_old = t;
if ((dt > DETECTION_SPACE) || (i > CHANNEL_AMOUNT)){
i = 0;
}
ch[i++] = dt;
}
|
|