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)，當然這些常見的感測器在網路上應該都找得到別人已經寫好並且功能完善的資源庫，但在使用時還是建議要去了解內部的設定，才不容易在一知半解的狀況下遇到無法預期的問題。

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

Read More

Arduino〈倒單擺 Inverted Pendulum〉(1) 硬體架構

倒單擺，顧名思義就是倒立的單擺。要讓單擺維持穩定的倒立狀態，一直都是自動控制中經典的工程議題，其應用可說是相當廣泛且深遠。 現今可以找到各式各樣奧妙的控制手段，從數學的層面來對付這種系統，讓單擺幾乎沒有倒下的可能。然而一旦要透過硬體來實現整個控制方塊，情況似乎就會變得有些難以預料了，已經不會像在 MATLAB 與 SIMULINK 中來得那樣單純。

最簡單的倒單擺模型為一組台車，台車上有一支可自由擺動的單擺，在重力的作用下，單擺勢必下墜。實際的模型中，台車可對應到一對並列的車輪，而與車輪以軸承銜接的馬達和整組車體則可看作單擺的集合。無控制介入時，車體無法平衡，必會倒下。

簡單的倒單擺系統實作

馬達的作用是當單擺倒下的瞬間迅速的前/後轉動，使車體能夠向後/前擺動以維持自身的平衡。使用帶有減速齒輪箱的直流馬達應可滿足控制的需求，並搭配像是L298N這樣的H-bridge驅動模組來控制馬達的正反轉。稍微進階的馬達會帶有轉速表(tachometer)或是編碼器(encoder)，這些可以用來感測馬達當前的轉速或是角度，作進一步倒單擺的位置控制或是改善控制表現，不過這些並非絕對必要。

L298N H-bridge 模組

馬達搭配車輪這樣的零件組合，在光華商場或是模型店很容易找到，以5v驅動居多，雖然可以透過改變電壓(PWM)來控制馬達的轉速，然而無論是馬達本身、齒輪組或是輪胎，都會有摩擦力的存在，加上車體的重量負載，這些都是馬達轉動的阻力，因此在一定的電壓之下，馬達是沒有辦法轉動的，換言之，驅動電壓有最小值，可能會在額定電壓的1/3到1/2。另一方面，不同的馬達多少有些差異，在同一驅動電壓下，兩組馬達轉速可能不一致，台車原來的直線運動可能會變成左右旋轉，亦會影響控制表現，這些也要在軟體中盡量修正與補償。

車體結構的製作也許是最不重要卻也是最麻煩的部分吧！結構上需要足夠的空間，能將所有的零件固定並配置妥當，包含馬達、驅動器電路、感測器模組、單晶片、電池還有一些必要的螺絲孔。可以利用一些免費的CAD軟體作2D平面結構的設計，像是LibreCAD或是DraftSight軟體都很方便，製圖完成接著輸出給提供雷射切割服務的廠商幫忙加工，或是尋求各地自造者空間的加工服務，像是Fablab Taipei便設置了各種加工機台，如3D列印、雷切或是木工機具等，可以加入會員或是在免費的開放時段使用。再不然就是到美術材料行與五金行找尋自己需要的材料，手動繪圖、手動加工也行。

利用CAD軟體規劃車體平面結構

馬達需要多少控制電壓是由感測器量測倒單擺當前的狀態進一步計算而決定，簡單來說，這裡的狀態指的是倒單擺與鉛直線的夾角與其倒下的速度，也就是角速度，可以使用陀螺儀與加速規來達成感測這些狀態的工作，也可以購買如MPU-6050這種內部整合加速規與陀螺儀的6-DOF模組，接線上會比較方便，不過本專題使用的是兩組獨立的感測器，型號分別是ADXL345(GY-291)與ITG3200，這些在光華商場也都很好找，其運作方式稍後再介紹。

三軸加速規(左)與三軸陀螺儀(右)

最後，硬體架構需要一組控制核心作為系統的大腦，接受感測器的訊號，執行邏輯運算並輸出命令以控制馬達的轉動，Arduino UNO與其搭載的AVR單晶片-ATmega328，方便、迅速，由於目前非常流行，就不多作介紹了。

Arduino UNO

下一篇會接著探討感測器的工作。

Read More

利用Processing 顯示 Arduino 加速規模組姿態

加速規，用來量測加速度的裝置，但也因為重力恆向下，也可以拿來測量重力於其他方向的分量，進而推算得到物體目前的姿態。

本篇文章主要討論如何從三軸加速規擷取訊號到Arduino，並透過序列傳輸將三軸的分量傳至電腦由Processing接收，並將物體姿態繪出顯示於螢幕。

Arduino ADXL345 三軸重力加速模組 

本篇範例所使用的 Arduino ADXL345 三軸重力加速模組 (數位輸出) Datasheet 。

我們有兩種方法能讓Arduino與加速度模組溝通，分別是SPI 跟 I2C ，文章使用 I2C 通訊作為範例，主要因為Arduino 提供非常方便的 library 供我們使用。

我們可以在Arduino 的 Wire Library 說明頁中看到 Uno 的腳位是  A4 (SDA),  A5 (SCL)，因此加速度模組與Arduino 的連接也非常的簡單。

ADXL345 的VCC 接到 Arduino 的 3.3V ，GND 接到 Arduino 的 GND，SDA 接到 Arduino 的 A4，SCL 接到 Arduino 的 A5，這樣接線就完成了。

Arduino 與加速度感測器的連接

硬體準備就緒後，就進入軟體部分。

首先下載 Arduino 的 Wire Library，解壓縮後放到 arduino 的Library資料夾中。


接著下載 ADXL345 的 Arduino Library 。若不想用別人的程式碼可以可以照著 ADXL345 的 Datasheet 自己試著寫，但在此我們先用別人寫好的來修改，畢竟我們的目的是在Processing 上顯示出姿態。

在 ADXL345 的 Datasheet 第10頁中解釋了如何透過I2C跟 ADXL345 溝通，也因此可以看到 Library 的前段是在配置記憶體位置，為了避免記憶體衝突。

為了將資料傳至Processing（注意：鮑率要對應），我們必須修改一小段程式碼，請將 readAccel() 的副程式改寫成如下：

void readAccel() {

  uint8_t howManyBytesToRead = 6;

  readFrom( DATAX0, howManyBytesToRead, _buff); //read the acceleration data from the ADXL345



  // each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!

  // thus we are converting both bytes in to one int

  int x = (((int)_buff[1]) << 8) | _buff[0];  

  int y = (((int)_buff[3]) << 8) | _buff[2];

  int z = (((int)_buff[5]) << 8) | _buff[4];




    Serial.write('X');

    Serial.write(_buff[1]);

    Serial.write(_buff[0]);

    Serial.write(_buff[3]);

    Serial.write(_buff[2]);

    Serial.write(_buff[5]);

    Serial.write(_buff[4]);



}

因為 Serial 是一個 byte 一個 byte 傳送，因此我們在Arduino 讀取到的高位元與低位元的值就直接傳至 Processing 再處理，不用再多做一個步驟，並加上一個 'X' 當做起始字元。

接下來就是Processing 的程式碼（再次提醒：鮑率要對應！）：

import processing.serial.*;



Serial serial;

int[] invalue=new int[6];

int[] Val=new int[3];



float G=250;

float xacc;

float yacc;

float zacc;



void setup(){

  size(600,300,P3D);

  fill(0, 102, 153);

  println(Serial.list());

  serial=new Serial(this,Serial.list()[3],9600);

  println("Start!");



}



void draw(){

  if(serial.available()>20){

      if(serial.read()=='X'){

        int sign = 1;

        invalue[1]=serial.read();

        invalue[0]=serial.read();

        invalue[3]=serial.read();

        invalue[2]=serial.read();

        invalue[5]=serial.read();

        invalue[4]=serial.read();

          if( invalue[1]>>7 == 1 ) {

             invalue[1]= 255 - invalue[1];

             invalue[0] = 256 - invalue[0];

             sign = -1;

          } else {

            sign = 1;

          }Val[0] = sign * ( invalue[1] << 8 | invalue[0] );

          if( invalue[3]>>7 == 1 ) {

             invalue[3]= 255 - invalue[3];

             invalue[2] = 256 - invalue[2];

             sign = -1;

          } else {

            sign = 1;

          }Val[1] = sign * ( invalue[3] << 8 | invalue[2] );

          if( invalue[5]>>7 == 1 ) {

             invalue[5]= 255 - invalue[5];

             invalue[4] = 256 - invalue[4];

             sign = -1;

          } else {

            sign = 1;

          }Val[2] = sign * ( invalue[5] << 8 | invalue[4] );

          println(Val);

          background(0);

          textSize(20);

          translate(width/2, height/2);

          text("z-acc:"+Val[2],-250,-100);

          text("x-acc:"+Val[0],-250,-80);

          text("y-acc="+Val[1],-250,-60);



          rotateX(asin(Val[1]/G));  //xacc

          rotateZ(asin(-Val[0]/G));   //Yacc

 

          box(100,20,100);

        }

     }

    }

注意到程式碼紅、藍、綠的地方，這裡我們要對接收到的高低位元資料做處理，ADXL345 傳過來的資料要合併這裡整理出一個規則：
如果值為正，則不改變，但如果值為負，則全部的0變為1；全部的1變為0。

也就是說若值為 256，則高低位元分別是 00000001 00000000;
而值為 -256，則高低位元是 11111110 11111111;
也因此有了上面的處理步驟。


得到資料後，便透過重力在個座標的分量來計算出現在加速規的姿態，這步驟需要一點空間概念，不過並不難，思考一下應該可以得到跟筆者一樣的答案。
以上就是如何利用Processing 顯示 Arduino 加速規模組姿態。

Read More