本帖最后由 比贫 于 2013-5-7 20:14 编辑
读了Ansifa的《Arduino串口接收字符串》黑马的《我的自平衡小车D4——PID控制器——第一次的直立行走》和桐桐 的《arduino和flash的as3连接》三篇文章后我也试着做了一个自平衡小车并用flash通过串口来控制小车的行走。
我用到的硬件有：arduino Uno控制板，L298N电机驱动板，ADXL345、L3G4200D、和HMC5883L组成的I2C传感器。带减速电机的三轮小车（拆掉万向轮），自制6V电源。
Flash端的代码（可以复制到flash CS3中运行，舞台大小为800*380）
var mc2:Sprite = new Sprite();//绘制坐标
this.addChild(mc2);
mc2.graphics.lineStyle(2, 0xff0000, 1);
mc2.graphics.moveTo(-150,-150);
mc2.graphics.lineTo(150,-150);
mc2.graphics.lineTo(150,150);
mc2.graphics.lineTo(-150,150);
mc2.graphics.lineTo(-150,-150);
mc2.graphics.lineStyle(1, 0x000000, 1);
mc2.graphics.moveTo(0,-150);
mc2.graphics.lineTo(0,150);
mc2.graphics.moveTo(-150,0);
mc2.graphics.lineTo(150,0);
mc2.x=500;
mc2.y=200;
/*创建文本框显示从arduino 发送来的数据*/
for(var i=0;i<18;i++){//创建文本框
this["txte" + i] = new TextField();
this["txte" + i].autoSize = TextFieldAutoSize.LEFT;
this["txte" + i].x=680;
this["txte" + i].y=0+i*30;
this["format" + i] = new TextFormat();
this["format" + i].font = "微软雅黑";
this["format" + i].size = 16;
this["txte" + i].defaultTextFormat = this["format" + i];
addChild(this["txte" + i]);
}
this["txte12"].text = "北";this["txte12"].x=490;this["txte12"].y=25;
this["txte13"].text = "南";this["txte13"].x=490;this["txte13"].y=347;
this["txte14"].text = "西";this["txte14"].x=328;this["txte14"].y=185;
this["txte15"].text = "东";this["txte15"].x=652;this["txte15"].y=185;
this["txte16"].x=350;this["txte16"].y=20;
this["txte17"].x=10; this["txte17"].y=0;
/*绘制一个小球*/
var mc0:Sprite = new Sprite();//绘制一个小球
this.addChild(mc0);
mc0.x=500;
mc0.y=200;
var r=20;
var type:String = GradientType.RADIAL;
var colors:Array = [0xff0000,0x000000];
var alphas:Array = [1,1];
var ratios:Array = [0, 0xff];
var spreadMethod:String = SpreadMethod.PAD;
var interp:String = InterpolationMethod.LINEAR_RGB;
var focalPtRatio:Number = 0;
var matrix:Matrix = new Matrix();
var boxWidth:Number = 2 * r;
var boxHeight:Number = 2 * r;
var boxRotation:Number = 0//Math.PI/2; // 90?
var tx:Number = -1 * boxWidth/2;
var ty:Number = -1 * boxHeight/2;
matrix.createGradientBox(boxWidth, boxHeight, 1, tx, ty);
mc0.graphics.beginGradientFill(type,colors,alphas,ratios, matrix,spreadMethod, interp, focalPtRatio);
mc0.graphics.lineStyle(0,0x000000,0)
mc0.graphics.moveTo(r, 0);
for(var n=0;n<2*Math.PI;n+=0.01){
mc0.graphics.lineTo(r*Math.cos(n),r*Math.sin(n));
}
/*与Arduino连接 flash与Arduino 连接时要用serproxy.exe */
var tmpString="";var j=""; var ardu:Array=[0,0,0,0,0,0,0,0,0,0,0,0];
var hostName:String = "127.0.0.1";//与Arduino连接
var port:Number = 5331;
var socket:XMLSocket= new XMLSocket();
configureListeners(socket);
socket.connect(hostName, port);
function configureListeners(dispatcher:IEventDispatcher):void {
dispatcher.addEventListener(Event.CLOSE, closeHandler);
dispatcher.addEventListener(Event.CONNECT, connectHandler);
dispatcher.addEventListener(DataEvent.DATA, dataHandler);
dispatcher.addEventListener(IOErrorEvent.IO_ERROR, ioErrorHandler);
dispatcher.addEventListener(ProgressEvent.PROGRESS, progressHandler);
dispatcher.addEventListener(SecurityErrorEvent.SECURITY_ERROR, securityErrorHandler);
}
function closeHandler(event:Event):void {this["txte17"].text= event;}//
function connectHandler(event:Event):void { this["txte17"].text="连接成功";}//trace("connectHandler: " + event);
function ioErrorHandler(event:IOErrorEvent):void {this["txte17"].text= event.text; }
function progressHandler(event:ProgressEvent):void { trace("progressHandler loaded:" + event.bytesLoaded );}
function securityErrorHandler(event:SecurityErrorEvent):void {trace("securityErrorHandler: " + event); }
function dataHandler(event:DataEvent):void {//接收来自Arduino的数据
tmpString = event.data;
if(tmpString.charAt (0)=="$"){//从Arduino发送来的字符串第一个字符是"$"
j=tmpString.slice (1,tmpString.length);
ardu=j.split (","); //把接收到的字符串以","为分隔拆分为数组
fx0a(1);
fx0(1);
fxh2(1)
}
}
/*把前面绘制的小球模拟成控制杆*/
var lr:Rectangle = new Rectangle(350, 50, 300, 300);
var k=0.05;
mc0.x=500; mc0.y=200;
mc0.addEventListener(MouseEvent.MOUSE_DOWN,startDragging);
mc0.addEventListener(MouseEvent.MOUSE_UP, stopDragging);
mc0.addEventListener(MouseEvent.MOUSE_OUT, stopDragging);
function startDragging(event:MouseEvent):void{
this.removeEventListener (Event.ENTER_FRAME,fx1a);
mc0.startDrag(true,lr);
}
function stopDragging(event:MouseEvent):void{
mc0.stopDrag();
this.addEventListener(Event.ENTER_FRAME,fx1a);
}
function fx1a(e){
mc0.x-=k*(mc0.x-500);
mc0.y-=k*(mc0.y-200);
if(Math.abs(mc0.x-500)<1 && Math.abs(mc0.y-200)<1 ){//&& Math.abs(mc0.y-200)<0.5
mc0.x=500;
mc0.y=200;
this.removeEventListener (Event.ENTER_FRAME,fx1a);
}
};
var temps1 = Math.round( mc0.x * 10/3-500/3);
var temps2 = Math.round(-mc0.y * 10/3+6500/3);
/*定义一个函数用于向arduino发送控制数据*/
function fx0a(e){
var tempd;
temps1 = Math.round( mc0.x * 10/3-500/3);
temps2 = Math.round(-mc0.y * 10/3+6500/3);
/* "t""z""w"用于把两个变量分开，在arduino端就可以接收两个变量了*/
tempd = "t"+temps1+"z"+temps2+"w";
socket.send(tempd); //发送数据
}
var xangleM = 0; var yangleM = 0; var zangleM = 0; // 根据磁场分量得到的角度
var xangleA = 0; var yangleA = 0; var zangleA = 0; // 根据加速度分量得到的角度
var xta = 0.0; var yta = 0.0; var zta = 0.0; //角速度分量
var x_angle = 0.0; var y_angle = 0.0; var z_angle = 0.0; // 滤波处理后的角度值
var yh=0;
var q0:Object = new Object(); var q1:Object = new Object();
var q2:Object = new Object(); var q3:Object = new Object();
var q4:Object = new Object(); var q5:Object = new Object();
var q6:Object = new Object(); var q7:Object = new Object();
var temxa = 0; var temya = 0; var temza = 0;
var K = 3;// 取值权重
var Ax;var Ay;var Az;// 加权系数
/*对收到的加速度分量和角速度分量进行滤波并创建矩形*/
function fx0(e){//对收到的加速度分量和角速度分量进行滤波并创建矩形
//temps1 = Math.round( mc0.x * 10/3-500/3);
//temps2 = Math.round(-mc0.y * 10/3+6500/3);
this["txte16"].text="x="+temps1+"; "+"y="+temps2;
xangleA = ardu[1];
yangleA = 0;
zangleA = 0;
ay = 1 * zangleA * Math.PI/180;
az = 1 * yangleA * Math.PI/180;
ax = 1 * xangleA * Math.PI/180;
fx1(q0, 60, 10+yh, 100);
fx1(q1, 60, 10+yh,-100);
fx1(q2, 60,-10+yh,-100);
fx1(q3, 60,-10+yh, 100);
fx1(q4,-60, 10+yh, 100);
fx1(q5,-60, 10+yh,-100);
fx1(q6,-60,-10+yh,-100);
fx1(q7,-60,-10+yh, 100);
fx2();
this["txte0"].text = "角度" + ardu[0];
this["txte1"].text = "滤波1" + ardu[1];
this["txte2"].text = "滤波2" + ardu[2];
this["txte3"].text = "时间" + ardu[3];
}
/*让3D矩形按小车的倾角转动*/
var ax=0,ay=0,az=0;//Math.PI
var x1,y1,z1;
function fx1(q,x0,y0,z0){//旋转
x1=x0
y1=y0*Math.cos(ax)-z0*Math.sin(ax);
z1=y0*Math.sin(ax)+z0*Math.cos(ax);
x0=x1;
y0=y1;
z0=z1;
x1= x0*Math.cos(ay)-z0*Math.sin(ay);
y1= y0;
z1=x0*Math.sin(ay)+z0*Math.cos(ay);
x0=x1;
y0=y1;
z0=z1;
x1=x0*Math.cos(az)-y0*Math.sin(az);
y1=x0*Math.sin(az)+y0*Math.cos(az);
z1=z0;
x0=x1;y0=y1;z0=z1;
var k = 500/(500+z0);
var xd = x0 * k;
var yd = y0 * k;
q.x = xd ;
q.y = yd ;
q.z = z0;
}
var child:Shape = new Shape();
addChild(child);
child.x=170;
child.y=180;
var color = 0xff8811;
var ap0,ap1,ap2,ap3,ap4,ap5,lw,lc,Lc;
lw = 0;lc = 0x555555;Lc = 0x000000;
function fx2(){//填充矩形
child.graphics.clear();
if((q3.x-q0.x)*(q2.y-q3.y)-(q3.y-q0.y)*(q2.x-q3.x)>0){ap0=0}else{ap0=1}
pointA.x =q0.x ;pointA.y =q0.y ;pointA.z =q0.z ;
pointB.x =q1.x ;pointB.y =q1.y ;pointB.z =q1.z ;
pointC.x =q2.x ;pointC.y =q2.y ;pointC.z =q2.z ;
lc=getAdjustedColor();
child.graphics.lineStyle(lw, Lc, 0);
child.graphics.beginFill(lc,ap0);
child.graphics.moveTo(q0.x,q0.y);
child.graphics.lineTo(q1.x,q1.y);
child.graphics.lineTo(q2.x,q2.y);
child.graphics.lineTo(q3.x,q3.y);
child.graphics.lineTo(q0.x,q0.y);
if((q4.x-q0.x)*(q5.y-q4.y)-(q4.y-q0.y)*(q5.x-q4.x)>0){ap1=1}else{ap1=0}
pointA.x =q0.x ;pointA.y =q0.y ;pointA.z =q0.z ;
pointB.x =q4.x ;pointB.y =q4.y ;pointB.z =q4.z ;
pointC.x =q5.x ;pointC.y =q5.y ;pointC.z =q5.z ;
lc=getAdjustedColor();
child.graphics.lineStyle(lw, Lc, 0);
child.graphics.beginFill(lc,ap1);
child.graphics.moveTo(q0.x,q0.y);
child.graphics.lineTo(q4.x,q4.y);
child.graphics.lineTo(q5.x,q5.y);
child.graphics.lineTo(q1.x,q1.y);
child.graphics.lineTo(q0.x,q0.y);
if((q4.x-q0.x)*(q7.y-q4.y)-(q4.y-q0.y)*(q7.x-q4.x)>0){ap2=0}else{ap2=1}
pointA.x =q3.x ;pointA.y =q3.y ;pointA.z =q3.z ;
pointB.x =q7.x ;pointB.y =q7.y ;pointB.z =q7.z ;
pointC.x =q4.x ;pointC.y =q4.y ;pointC.z =q4.z ;
lc=getAdjustedColor();
child.graphics.lineStyle(lw, Lc, 0);
child.graphics.beginFill(lc,ap2);
child.graphics.moveTo(q0.x,q0.y);
child.graphics.lineTo(q4.x,q4.y);
child.graphics.lineTo(q7.x,q7.y);
child.graphics.lineTo(q3.x,q3.y);
child.graphics.lineTo(q0.x,q0.y);
if((q6.x-q7.x)*(q2.y-q6.y)-(q6.y-q7.y)*(q2.x-q6.x)>0){ap3=0}else{ap3=1}
pointA.x =q2.x ;pointA.y =q2.y ;pointA.z =q2.z ;
pointB.x =q6.x ;pointB.y =q6.y ;pointB.z =q6.z ;
pointC.x =q7.x ;pointC.y =q7.y ;pointC.z =q7.z ;
lc=getAdjustedColor();
child.graphics.lineStyle(lw, Lc, 0);
child.graphics.beginFill(lc,ap3);
child.graphics.moveTo(q2.x,q2.y);
child.graphics.lineTo(q3.x,q3.y);
child.graphics.lineTo(q7.x,q7.y);
child.graphics.lineTo(q6.x,q6.y);
child.graphics.lineTo(q2.x,q2.y);
if((q6.x-q5.x)*(q2.y-q6.y)-(q6.y-q5.y)*(q2.x-q6.x)>0){ap4=1}else{ap4=0}
pointA.x =q5.x ;pointA.y =q5.y ;pointA.z =q5.z ;
pointB.x =q6.x ;pointB.y =q6.y ;pointB.z =q6.z ;
pointC.x =q2.x ;pointC.y =q2.y ;pointC.z =q2.z ;
lc=getAdjustedColor();
child.graphics.lineStyle(lw, Lc, 0);
child.graphics.beginFill(lc,ap4);
child.graphics.moveTo(q2.x,q2.y);
child.graphics.lineTo(q6.x,q6.y);
child.graphics.lineTo(q5.x,q5.y);
child.graphics.lineTo(q1.x,q1.y);
child.graphics.lineTo(q2.x,q2.y);
if((q7.x-q4.x)*(q6.y-q7.y)-(q7.y-q4.y)*(q6.x-q7.x)>0){ap5=1}else{ap5=0}
pointA.x =q4.x ;pointA.y =q4.y ;pointA.z =q4.z ;
pointB.x =q7.x ;pointB.y =q7.y ;pointB.z =q7.z ;
pointC.x =q6.x ;pointC.y =q6.y ;pointC.z =q6.z ;
lc=getAdjustedColor();
child.graphics.lineStyle(lw, Lc, 0);
child.graphics.beginFill(lc,ap5);
child.graphics.moveTo(q4.x,q4.y);
child.graphics.lineTo(q5.x,q5.y);
child.graphics.lineTo(q6.x,q6.y);
child.graphics.lineTo(q7.x,q7.y);
child.graphics.lineTo(q4.x,q4.y);
child.graphics.endFill();
}
function getAdjustedColor():uint{
var red:Number = color >> 16;
var green:Number = color >> 8 & 0xff;
var blue:Number =color & 0xff;
var lightFactor:Number = getLightFactor();
red *= lightFactor;
green *= lightFactor;
blue *= lightFactor;
return red << 16 | green << 8 | blue;
}
var pointA:Object = new Object();
var pointB:Object = new Object();
var pointC:Object = new Object();
function getLightFactor():Number{
var ab:Object = new Object();
ab.x = pointA.x - pointB.x;
ab.y = pointA.y - pointB.y;
ab.z = pointA.z - pointB.z;
var bc:Object = new Object();
bc.x = pointB.x - pointC.x;
bc.y = pointB.y - pointC.y;
bc.z = pointB.z - pointC.z;
var norm:Object = new Object();
norm.x = (ab.y * bc.z) - (ab.z * bc.y);
norm.y =-((ab.x * bc.z) - (ab.z * bc.x));
norm.z = (ab.x * bc.y) - (ab.y * bc.x);
var dotProd:Number = norm.x * (-100) + norm.y * (-100) + norm.z * (-100);
var normMag:Number = Math.sqrt(norm.x * norm.x + norm.y * norm.y + norm.z * norm.z);
var lightMag:Number = Math.sqrt((-100) * (-100) +(-100) * (-100) +(-100) * (-100));
return (Math.acos(dotProd / (normMag * lightMag)) / Math.PI)* 1;
}
/*把arduino发送过来的数据绘制成曲线*/
var mch:Sprite = new Sprite();
this.addChild(mch);
var x0=0;var xo=50;
var y0=0;var yo=200;
mch.graphics.clear();
mch.graphics.lineStyle(2,0x00ff00);
mch.graphics.moveTo(xo,yo);
function fxh2(e){
y0 = ardu[1]*15;
x0 = x0 + 1;
if(x0>700){
x0=0;
mch.graphics.clear();
mch.graphics.lineStyle(2,0x00ff00);
mch.graphics.moveTo(xo,yo-y0);
}
mch.graphics.lineTo(xo+x0,yo-y0);
}
arduino端的代码（可以复制到arduino1.0.1中运行）
#include <Wire.h> // IIC运行库
int BMPAddress = 0x77;
int ADXAddress = 0x53; // ADXL345的I2C地址
int L3GAddress = 0x69; // L3G4200D的I2C地址
int HMCAddress = 0x1E; // HMC5883L的I2C地址
byte vL, vH,vOF; // 存放低位、高位值
int xAcc, yAcc, zAcc; // 存放加速度值(整型）
int xGyro, yGyro, zGyro; // 存放角速度值(整型）
int xMag, yMag, zMag; // 存放地磁场值(整型）
/************ 传感器参数 ***********/
#define Gry_offset -3 // 陀螺仪偏移量
#define Gyr_Gain 0.07 // 满量程2000dps时灵敏度(dps/digital)
#define pi 3.14159
/********** 互补滤波器参数 *********/
float f_angle = 0.0; // 滤波处理后的角度值
float y1=0, Com2_angle=0;
/*********** PID控制器参数 *********/
unsigned long lastTime; // 前次时间
float ITerm=0;//lastInput; // 积分项、前次输入
float Output = 0.0; // PID输出值
char comdata;
String tep0="";String tep1="";
//int tem13=1500; int tem03=1500;
int mark=0,m=0;int pwm13=1500; int pwm03=1500;
void setup(){
Serial.begin(38400); // 定义串口
Wire.begin();
delay(100);
Serial.println(" Device Initializating......");
/*定义数据口控制电机转动方向*/
pinMode(2,OUTPUT);
pinMode(4,OUTPUT);
pinMode(7,OUTPUT);
pinMode(8,OUTPUT);
deviceInt(); //设备初始化
delay(200);
}
void loop(){
/*接收flash从串口传过来的数据*/
m=0;
while (Serial.available() > 0) {
comdata = char(Serial.read());
/*把字符串按"t""z""w"拆分成两段，在arduino端就可以接收两个变量了*/
if(comdata == 't'){ m=1; }
if(comdata == 'z'){ m=2; }
if(comdata == 'w'){ m=0; mark=1; Serial.flush(); break;} // break;Serial.flush();break;
if(m == 1){tep0 += comdata;}
if(m == 2){tep1 += comdata;}
}
if(mark==1){
pwm13 = 0;
pwm03 = 0;
for(int i = 1; i < tep0.length(); i++){ pwm13=pwm13*10+(tep0-'0');}
for(int i = 1; i < tep1.length(); i++){ pwm03=pwm03*10+(tep1-'0');}
tep0 = String("");
tep1 = String("");
}else{
pwm13 = 1500;
pwm03 = 1500;
}
/*接收到的两个变量用于控制小车的前进后退的速度和左右转向*/
pwm13 = map(pwm13, 1000, 2000, -7, 7);
pwm03 = map(pwm03, 1000, 2000, -7, 7);
mark=0;
unsigned long now = millis(); // 当前时间(ms)
float dt = (now - lastTime) / 1000.0; // 微分时间(s)
getAccValues(); //输出加速度
getGyroValues(); //输出角速度
/********** 读取姿态传感器 *********/
float Y_Acc = yAcc; // 获取向前的加速度(digite)
float Z_Acc = zAcc; // 获取向下的加速度(digite)
float angleA = atan(Y_Acc / Z_Acc) * 180 / pi; // 根据加速度分量得到的角度(degree)
float omega = Gyr_Gain * (xGyro + Gry_offset); // 当前角速度(degree/s)
/*********** 一阶互补滤波 **********/
if(abs(omega)<0.5){omega=0;}
float K = 5; // 取值权重
float A = K / (K + dt); // 加权系数
f_angle = A * (f_angle + omega * dt) + (1-A) * angleA; // 互补滤波算法
/*********** 二阶互补滤波 **********/
K = 0.5;
float x1 = (angleA - Com2_angle) * K * K;
y1 = y1 + x1 * dt;
float x2 = y1 + 2 * K *(angleA - Com2_angle) + omega;
Com2_angle = Com2_angle + x2 * dt;
/************ PID控制器 ***********/
float Input = 0;
Input = f_angle; // 输入赋值
float Kp = 11, Ki = 3, Kd = 0.5; // 比例系数、积分系数、微分系数
/*通过变量pwm03控制小车的前进后退的速度*/
float Setpoint = -pwm03;
float error = Setpoint - Input; // 偏差值
ITerm+= (Ki * error * dt); // 计算积分项
ITerm = constrain(ITerm, -90, 90); // 限定值域
float DTerm = omega * Kd; //计算微分项 这里被我改过了
Output = Kp * error + ITerm - DTerm; // 计算输出值
Output = constrain(Output, -150, 150); // 限定值域
lastTime = now; // 记录本次时间
/*控制电机转动方向*/
if(Output<0){
digitalWrite(2,HIGH); digitalWrite(4,LOW);
digitalWrite(7,HIGH); digitalWrite(8,LOW);
}else if(Output>0){
digitalWrite(2,LOW); digitalWrite(4,HIGH);
digitalWrite(7,LOW); digitalWrite(8,HIGH);
}else{
digitalWrite(2,LOW); digitalWrite(4,LOW);
digitalWrite(7,LOW); digitalWrite(8,LOW);
}
/* 把PID控制器的输出转化为PWM*/
float temp=abs(Output)*2;
temp = constrain(temp, 0, 255);
float Y_Acct=abs(Y_Acc);
if(Y_Acct>60){temp=0; } //判断小车是否倒下
/*通过变量pwm13控制小车的左右转向*/
float LR = 0;
LR=pwm13;
float tempL = temp + LR;
float tempR = temp - LR;
tempL = constrain(tempL, 0, 255);
tempR = constrain(tempR, 0, 255);
/*电机死区控制*/
if(tempL<80){tempL=0;}
if(tempR<80){tempR=0;}
/*输出PWM到电机控制板*/
analogWrite(3, tempL);
analogWrite(9, tempR);
/*通过串口发送字符串到flash*/
Serial.print("$");
Serial.print(angleA);
Serial.print(",");
Serial.print(f_angle);
Serial.print(",");
Serial.print(Com2_angle);
Serial.print(",");
Serial.println(dt);
delay(2);// 控制微分时间
}
void deviceInt(){
writeRegister(ADXAddress, 0x2D, 0b00001000); // 测量模式
// 配置ADXL345
writeRegister(L3GAddress, 0x20, 0b00001111); // 设置睡眠模式、x, y, z轴使能
writeRegister(L3GAddress, 0x21, 0b00000000); // 选择高通滤波模式和高通截止频率
writeRegister(L3GAddress, 0x22, 0b00000000); // 设置中断模式
writeRegister(L3GAddress, 0x23, 0b00110000); // 设置量程(2000dps)、自检状态、SPI模式
writeRegister(L3GAddress, 0x24, 0b00000000); // FIFO & 高通滤波
// 配置L3G4200D(2000 deg/sec)
writeRegister(HMCAddress, 0x02, 0x00); // 连续测量
// 配置HMC5883L
}
void setAccOffset(){ //加速度偏移设置
writeRegister(ADXAddress, 0x1E, 0x00);
writeRegister(ADXAddress, 0x1F, 0x00);
writeRegister(ADXAddress, 0x20, 0x00);
}
void getAccValues(){ // 加速度值读取
vL = readRegister(ADXAddress, 0x32);
vH = readRegister(ADXAddress, 0x33);
vOF = readRegister(ADXAddress, 0x1E);
xAcc = (vH << 8) | vL;
xAcc = xAcc +vOF;
//xAf = xAcc/2560.0000;
vL = readRegister(ADXAddress, 0x34);
vH = readRegister(ADXAddress, 0x35);
vOF = readRegister(ADXAddress, 0x1F);
yAcc = (vH << 8) | vL;
yAcc = yAcc + vOF;
//yAf = yAcc/2560.0000;
vL = readRegister(ADXAddress, 0x36);
vH = readRegister(ADXAddress, 0x37);
vOF = readRegister(ADXAddress, 0x20);
zAcc = (vH << 8) | vL;
zAcc = zAcc + vOF;
//zAf = zAcc/2560.0000;
}
void getGyroValues(){ // 角速度值读取
vL = readRegister(L3GAddress, 0x28);
vH = readRegister(L3GAddress, 0x29);
xGyro = (vH << 8) | vL;
vL = readRegister(L3GAddress, 0x2A);
vH = readRegister(L3GAddress, 0x2B);
yGyro = (vH << 8) | vL;
vL = readRegister(L3GAddress, 0x2C);
vH = readRegister(L3GAddress, 0x2D);
zGyro = (vH << 8) | vL;
}
void getMagValues(){ // 磁场值读取
vH = readRegister(HMCAddress, 0x03);
vL = readRegister(HMCAddress, 0x04);
xMag = (vH << 8) | vL;
vH = readRegister(HMCAddress, 0x05);
vL = readRegister(HMCAddress, 0x06);
yMag = (vH << 8) | vL;
vH = readRegister(HMCAddress, 0x07);
vL = readRegister(HMCAddress, 0x08);
zMag = (vH << 8) | vL;
}
int readRegister(int deviceAddress, byte address){ // 读寄存器
int v;
Wire.beginTransmission(deviceAddress);
Wire.write(address);
Wire.endTransmission();
Wire.requestFrom(deviceAddress, 1);
while(!Wire.available()) {}
v = Wire.read();
return v;
}
void writeRegister(int deviceAddress, byte address, byte val){ // 写寄存器
Wire.beginTransmission(deviceAddress);
Wire.write(address);
Wire.write(val);
Wire.endTransmission();
}
本站仅提供存储服务，所有内容均由用户发布，如发现有害或侵权内容，请
点击举报。
