在Arduino上使用陀螺仪控制舵机

• • Leave a reply

今天拿到了四轴飞行器的一个重要部件，就是陀螺仪。^_^ 下面这张图上的就是了，这是一个三轴陀螺仪，seeedstudio GROVE套装的一个部件。

gyro

这个陀螺仪使用ITG3200芯片，可以直接使用了I2C接口连接到Arduino上面。默认的地址是0x68，但是可以修改成0x69，只要将板子上的JP1连起来就可以修改。接口四个针脚的定义就像下面这张图中所画的。

photo

是的。。你没看错，他们用红色作为地线 -_-b

舵机就是最简单的使用pwm的口就可以了。最后链接成下面这张图中的样子。

photo

我写了一个程序，代码在下方，能够做到让陀螺仪的转动直接反应在舵机的摇臂上面。但是，这样也会遇到问题，就是当动的次数比较多的时候，舵机的角度就会越来越大，直到超过最大的范围，所以这也是陀螺仪不是很精确的地方。另外就是，在陀螺仪静止的情况下，z轴的输出不为0，始终为-1。也就是说，这个陀螺仪的零点可能是不准确的，同时，正负转动的也有可能有差别，不知道这回对将来飞控算法带来多大的影响。

最后用到的程序就是下面这段。

#include <Wire.h>
#include <Servo.h>

#define GYRO_ADDR  0x68

Servo servo;

int currentPos;

void setup()
{
  Wire.begin();
  initGyro();
  servo.attach(2);
  currentPos = 90;
  //Serial.begin(9600);
  pinMode(3, INPUT);
}

void loop()
{
  int x;
  int v;
  x = readx();
  v = ready();
  v = readz();
  currentPos = currentPos + x;

  int buttonState;
  buttonState = digitalRead(3);
  if (buttonState == HIGH)
  {

    currentPos = 90;
  }
  else
  {
    //Serial.println("LOW");
  }

  servo.write(currentPos);

}

int readx()
{
  int x;
  x = readGyro(0x1d, 0x1e);
  return x;
}

int ready()
{
  int y;
  y = readGyro(0x1f, 0x20);
  return y;
}

int readz()
{
  int z;
  z = readGyro(0x21, 0x22);
  return z;
}

char readGyro(unsigned char addrH, unsigned char addrL)
{
  char ret;
  Wire.beginTransmission(GYRO_ADDR);
  Wire.send(addrH);
  Wire.endTransmission();
  Wire.requestFrom(GYRO_ADDR, 1);
  if (Wire.available() > 0)
  {
    ret = Wire.receive();
  }
  Wire.beginTransmission(GYRO_ADDR);
  Wire.send(addrL);
  Wire.endTransmission();
  Wire.requestFrom(GYRO_ADDR, 1);
  if (Wire.available() > 0)
  {
    ret != Wire.receive()<<8;
  }

  return ret;
}

void initGyro()
{
  Wire.beginTransmission(GYRO_ADDR);
  Wire.send(0x3E);
  Wire.send(0x80);  //send a reset to the device
  Wire.endTransmission(); //end transmission

  Wire.beginTransmission(GYRO_ADDR);
  Wire.send(0x15);
  Wire.send(0x00);   //sample rate divider
  Wire.endTransmission(); //end transmission

  Wire.beginTransmission(GYRO_ADDR);
  Wire.send(0x16);
  Wire.send(0x18); // ±2000 degrees/s (default value)
  Wire.endTransmission(); //end transmission
}

程序当中Wire库就是使用I2C，具体文档可以看下面的链接。

http://arduino.cc/en/Reference/Wire

http://v.youku.com/v_show/id_XMjYzMTg2ODQ0.html

1,612 total views, 1 views today

Leave a Reply