Arduino〈倒單擺 Inverted Pendulum〉(2) 陀螺儀與加速規模組設定

為了感測倒單擺傾倒的狀態，基本上陀螺儀與加速規缺一不可。其他像是姿態或是方向感測器幾乎也都是以這兩種感測器為基礎搭配磁力計整合成的n-DOF模組。 本篇要先來認識一下使用的感測器模組，以及如何對其進行設定。




1. 陀螺儀模組

使用的是 Grove-3-Axis Digital Gyro Module, 這在光華商場找得到，其內部感測核心為ITG-3200MEMS陀螺儀晶片，可量測物體三個方向的旋轉角速度與環境溫度，不過在倒單擺中只需要擷取一個方向即可，至於是哪個方向則要看感測器與倒單擺的組裝方式才好了解，而溫度感測器通常是用來校正感測器隨溫度變化所產生的漂移誤差(drift)。除了晶片的物理量測規格，datasheet也會載明感測器支援的通訊格式與晶片上暫存器(Register)的使用功能，從暫存器表格中可以知道感測器將三軸陀螺儀的量測值儲存在0x1D~0x22的位址上，每一軸資料分高低位元組共佔兩個位址，Arduino要讀取的值便是從這裡。

ITG-3200 datasheet中的暫存器功能表

要讓陀螺儀順利運作還需要設定幾組暫存器，簡單整裡如下:

(1). Power Manager(0x3E)

a. H_RESET 與 SLEEP 設為0。

b. 啟動特定軸向的陀螺儀，將其對應的bit設為0(normal)。

    例如假設僅使用Y軸，則STBY_YG = 0, STBY_XG = STBY_ZG = 1。

c. 時脈建議配合使用的陀螺儀為振盪參考來源，
    如使用Y軸，就選PLL with Y Gyro reference，
    因此Bit[2-0]設為010。


(2). DLPF, Full Scale(0x16)

a. 選擇量測範圍+/- 2000 degree/sec，Bit[4-3]設為11。
b. 設定晶片取樣頻率 8kHz， Bit[2-0] 為 000。


(3) Sample Rate Divider(0x15)

a.透過除法器進一步的設定(降低)晶片的取樣率，保留defult即可。

其他還有中斷功能的設定，不過這裡並不會用到，一樣保留defult。

ITG-3200支援I2C(400kHz)通訊協定的資料傳輸，其Slave Adress為0x68或0x69，Slave Adress可視為在I2C bus上用來呼叫特定裝置的ID，不同的感測器模組，只要支援I2C通訊，便會有一組固定的裝置ID，在bus上被呼叫ID的感測器才會有回應，如此便可以並聯多組不同的I2C感測模組仍不致衝突，Arduino可以利用Wire.h資源庫來跟感測器作I2C通訊，以下是一段簡單寫入與讀出I2C裝置的程式碼範例。

void writeGyro(uint8_t _register, uint8_t _data) {
  Wire.beginTransmission(ITG3200_DEVICE_ID);   // ITG3200_DEVICE_ID = 0x68
  Wire.write(_register);   
  Wire.write(_data);
  Wire.endTransmission();
}

writeGyro(0x3E, 0x2A);  //ex:  Configuring Power Manager of Gyro 

//****************************************************************************
int16_t readGyro(uint8_t addressh) {
  int data, t_data;

  Wire.beginTransmission(ITG3200_DEVICE_ID);  // ITG3200_DEVICE_ID = 0x68
  Wire.write(addressh);
  Wire.endTransmission();
  Wire.requestFrom(ITG3200_DEVICE, 2);
  
  if(Wire.available() > 0) {
    t_data = Wire.read();
    data = t_data << 8;
  }

  if(Wire.available() > 0) {
    data |= Wire.read();
  }
  
  Wire.endTransmission();

  return data;
}

int16_t gx = readGyro(0x1D);  //ex: Get Data From X-Axis Gyroscope


每個陀螺儀由於內部機構設計或是製造的因素，使得量測值都會有offset的存在，offset是指在沒有旋轉時，陀螺儀仍然有輸出值，雖可透過校正的方式降低offset的影響，但是單純的校正無法完全消除offset，原因是offset可能是個無理數且會隨著時間變化，而晶片上的暫存器有解析度的限制，即使大量取樣offset並取其平均，得到的仍是近似值，在white noise的作用下，感測器的輸出仍會稍微的偏向一側，所以陀螺儀除了校正，還需要其他的手段來抑制量測上的offset與drift。以下是以取樣取平均校正陀螺儀的程式碼範例。

int16_t   offset_GX = 0;
void calibrateGX(int samples, unsigned int sampleDelayMS) {
  int16_t gx_offset_temp = 0;
  int16_t gx = 0;
  offset_GX = 0;

  for (int i = 0; i < samples; i++) {
    delay(sampleDelayMS);
    gx =  readGyro(0x1D);  // Get Data From X-Axis Gyroscope
    gx_offset_temp += gx;
  }
  offset_GX = - gx_offset_temp / samples;  // Calculate offset

}

calibrateGX(100, 10);
int16_t gx_calibrated =  readGyro(0x1D) + offset_GX ;  // Calibrated Output

以上便完成陀螺儀的基本設定。


2. 加速規模組

使用的是Digital 3-Axis Acceleration of Gravity Tilt Module，一樣是常見的感測器，其內部的核心晶片為ADXL-345，可量測物體三個方向的加速度，其值暫存於0x32~0x37的位址中。通訊協定支援SPI與I2C兩種格式，在這裡使用SPI作資料傳輸，根據datasheet中的傳輸要求，在Arduino中作SPI初始化設定時，需將SPI的傳輸模式設為MODE3，並需要一個CS pin腳位作為傳輸的開關。

 // SPI initialization

  SPI.begin();   

  SPI.setDataMode(SPI_MODE3);
  pinMode(PIN_CS, OUTPUT);  // ex: PIN_CS can be pin 10 on Arduino 

  digitalWrite(PIN_CS, HIGH);

ADXL345 datasheet中的暫存器功能表

暫存器設定簡單整裡如下:

(1). Power Control(0x2D)

a. 由於不會用到感測器的中斷與休眠功能，

這裡只需要啟動Measure Mode 即可。 D3 設為1，其他為0。

(2). Data Format Control(0x31)

a. 量測範圍+/- 2g，D[1-0]設為00。

b. 使用4-wire SPI mode，D6設為0。

c. 其他保持defalt，皆設為0。

(3). Data Rate and Power Mode Control(0x2C)

a. 不會用到省電模式，D4設為0。

b. 資料輸出頻率設為最高值，D[3-0]為1111。

(4). XYZ-Axis Offset(0x1E-0x20)

加速規也有offset問題，也就是沒有加速度時，輸出仍然有值，將此offset值寫入這些暫存器，感測器在之後的量測便可以自動扣除，得到較準確的輸出值，這部分可以透過手動校正，依序將各軸加速規擺到水平位置，將不為零的值紀錄並寫入暫存器中。不過offset暫存器的解析度(15.6mg/LSB)與量測值的解析度不同(4mg/LSB)，意思是兩個暫存器裡一個位元所代表的加速度值不相同，校正時建議多試幾次，觀察輸出的變化應該會比較清楚。

ADXL-345尚還有許多其他的功能，像是拍擊(Tap)偵測與掉落(Free Fall)偵測，不過這些在倒單擺的應用中都非必要，其暫存器的設定接保持defalt不使用。

以下是一段簡單寫入與讀出SPI裝置的程式碼範例。

void writeAcc(uint8_t _register, uint8_t _data) {
digitalWrite(PIN_CS, LOW);
SPI.transfer(_register);
SPI.transfer(_data);
digitalWrite(PIN_CS, HIGH);

}

writeAcc(0x1E, 0x01); // ex: set x-axis offset value(0x01) into OFSX register(0x1E)
//****************************************************************************

int16_t readAcc(uint8_t addressl) {
int data, t_data;

char address = 0x80 | addressl;
address = address | 0x40;

digitalWrite(PIN_CS, LOW);
SPI.transfer(address);

data = SPI.transfer(0x00);
  t_data = SPI.transfer(0x00);
  data |= (t_data << 8);

  digitalWrite(PIN_CS, HIGH);

  return data;

}

int16_t ax = readAcc(0x32);  //ex: Get Data From X-Axis accelerometer

以上便完成加速規的基本設置。

了解這些模組內部的暫存器設置，是為了方便我們撰寫感測器的資源庫(library)，當然這些常見的感測器在網路上應該都找得到別人已經寫好並且功能完善的資源庫，但在使用時還是建議要去了解內部的設定，才不容易在一知半解的狀況下遇到無法預期的問題。

完成感測器的設定表示可以從這些模組讀取量測值了，但是這些讀值常會伴隨很嚴重的干擾，無法作為控制的依據，尤其是角度的量測。因此下一篇會介紹所使用的濾波方法來抑制干擾以得到較準確的量測輸出。