[Arduino感測器實作] 用 ITG-3200 三軸高精度陀螺儀控制 3 顆伺服馬達

 

<來源 : seeedstudio>

一、簡介

ITG-3200三軸陀螺儀使用I2C介面(400kHz)。可用3.3V或5V來驅動。官網中更多規格資料，摘錄如下 :

• Supply Voltage: 3.3V, 5V
• Operation Current: 6.5mA
• Standby current: 5μA
• Sensitivity: 14 LSBs per °/sec
• Full scale range: ±2000°/sec
• Acceleration: 10,000g for 0.3ms
• I2C Interface
• ±2000°/s full scale range and 14.375 LSBs per °/s sensitivity
• Three integrated 16-bit ADCs
• On-chip temperature sensor
• Integrated amplifiers and low-pass filters
• Hermetically sealed for temp and humidity resistance
• RoHS and Green compliant

接線方式，很簡單，將Vcc、GND、SDA、SCL接到相對應位置即可。官網的接線範例如下 :

<來源 : seeedstudio>

二、程式碼實作 

參考Youtuber David Khudaverdian的作品進行程式修改 :

為了要讓程式正常運作，請先到此Github下載相關套件，再將名為 "itg3200filv05" 的套件，放到電腦中arduino的libaries資料夾中。

再依序將伺服馬達接到定義的腳位中，燒入程式後，即可順利運作。

完整程式碼如下 :

#include <ITG3200.h>

#include <Wire.h>

#include <ITG3200.h>

#include <Servo.h> 

ITG3200 gyro = ITG3200();

float  x,y,z;

float angle_x=0;

float angle_y=0;

float angle_z=0;

float ServoPosition_x;

float ServoPosition_y;

float ServoPosition_z;

int ix, iy, iz;

unsigned long time_1=0;

unsigned long time_2=0;

Servo myservo_x, myservo_y, myservo_z; 

void setup(void) 

{

  Serial.begin(9600);

  myservo_x.attach(4);

  myservo_y.attach(8);

  myservo_z.attach(9);

  Wire.begin();      

  delay(10);

  gyro.init(ITG3200_ADDR_AD0_LOW); 

  Serial.print("zeroCalibrating...");

  gyro.zeroCalibrate(2500, 2);//(number of mesaurments, milliseconds between them)

  Serial.println("done.");

}

void loop(void) 

{

  while (gyro.isRawDataReady()) 

  {

    //Reads calibrated values in deg/sec    

    gyro.readGyro(&x,&y,&z); 

    time_1=time_2;

    time_2 = micros();

    if (time_1!=0)

  {

      angle_x=angle_x+1*x*((float)time_2-(float)time_1)/200000;  

      angle_y=angle_y+1*y*((float)time_2-(float)time_1)/200000;

      angle_z=angle_z+1*z*((float)time_2-(float)time_1)/200000;

    }

    //Serial.println(angle);  

    ServoPosition_x=90-angle_x;

    ServoPosition_y=90-angle_y;

    ServoPosition_z=90-angle_z;

    myservo_x.write(ServoPosition_x);

    myservo_y.write(ServoPosition_y);

    myservo_z.write(ServoPosition_z);

    Serial.print("values of X , Y , Z: ");

    Serial.print(x);

    Serial.print(" , ");

    Serial.print(y);

    Serial.print(" , ");

    Serial.println(z);

  } 

}

---

參考資料 : 

• wiki.seeedstudio
• Gihub : FreeIMU