Search

2011年6月1日 星期三

Arduino - 使用 Wii Nunchuck

前言:


Wii Nunchuck是使用Inter-Integrated Circuit也就是俗稱的I2C作為protocol,關於I2C的初始化等等工作可以參考Example -> Wire裡面的範例,Nunchuck有四個重要的腳位:VCC、GND、SDA、SCL,這四個腳可以分別接上Arduino Analog 2~5,因為Analog 4、5是Arduino的I2C腳位SDA、SCL,而Analog 2、3則就會設成GND與VCC。







Nunchuck會傳回6個字元的資料,按照順序是搖桿X,Y與加速度X,Y,Z以及比較特殊的第6個字元,每個加速度值是用10bits表示,因此加速度前面一個字元還要在分別加上第6字元的0~5位元才表示真正的加速度值,而第6字元的6、7位元就是按鈕 c、z 的狀態了,當按按鈕下時會將表示該按鈕狀態的位元設定為0反之設為1,而將資料轉換為實際數值的公式則是:val = (val ^ 0x17) + 0x17。



關於Nunchuck與Arduino連接可以在網路上買到這種連接介面,省去拆解的麻煩





實驗:


先測試Nunchuck是否能夠正確讀取資料,要注意的是I2C初始化裝置位址是0x52。


程式:


#include <Wire.h>


const int NGnd = 16;
const int NVcc = 17;


static uint8_t NBuff[6];


void setup()
{
    Serial.begin(19200);
    PowerSetup();
    WiiNunchuckInit();
}


void loop()
{
    delay(Run());
}


void PowerSetup()
{
    pinMode(NGnd,OUTPUT);
    pinMode(NVcc,OUTPUT);
    digitalWrite(NGnd,LOW);
    digitalWrite(NVcc,HIGH);
    delay(200);
}


void WiiNunchuckInit()
{
    Wire.begin();
    Wire.beginTransmission(0x52);
    Wire.send(0x40);
    Wire.send(0x00);
    Wire.endTransmission();
}


void SendRequest()
{
    Wire.beginTransmission(0x52);
    Wire.send(0x00);
    Wire.endTransmission();
}


char Decode(char c)
{
    c = (c ^ 0x17) +0x17;
    return c;
}


int GetData()
{
    int count = 0;
    Wire.requestFrom(0x52,6);


    while(Wire.available()){
        NBuff[count] = Decode(Wire.receive());
        count++;
    }


    SendRequest();
    if(count >= 5){
        return 1;
    }
    return 0;
}


int Run()
{
    GetData();


    int joy_x = NBuff[0];
    int joy_y = NBuff[1];
    int accel_x = NBuff[2];
    int accel_y = NBuff[3];
    int accel_z = NBuff[4];


    int button_c = 0;
    int button_z = 0;


    if((NBuff[5] >> 0) & 1){
        button_z = 1;
    }
    if((NBuff[5] >> 1) & 1){
        button_c = 1;
    }


    if((NBuff[5] >> 2) & 1){
        accel_x += 2;
    }
    if((NBuff[5] >> 3) & 1){
        accel_x += 1;
    }


    if((NBuff[5] >> 4) & 1){
        accel_y += 2;
    }
    if((NBuff[5] >> 5) & 1){
        accel_y += 1;
    }


    if((NBuff[5] >> 6) & 1){
        accel_z += 2;
    }
    if((NBuff[5] >> 7) & 1){
        accel_z += 1;
    }


    Serial.print("joy:");
    Serial.print(joy_x,DEC);
    Serial.print(",");
    Serial.print(joy_y,DEC);
    Serial.print(" \t");
    Serial.print("accle:");
    Serial.print(accel_x,DEC);
    Serial.print(",");
    Serial.print(accel_y,DEC);
    Serial.print(",");
    Serial.print(accel_z,DEC);
    Serial.print(" \t");
    Serial.print("button:");
    Serial.print(button_z,DEC);
    Serial.print(",");
    Serial.print(button_c,DEC);
    Serial.println("");
    return 100;
}









也可以結合致動裝置或是無線模組來做控制,這裡直接用L293D來控制兩個DC MOTOR。


程式:


#include <Wire.h>


const int NGnd = 16;
const int NVcc = 17;


const int LeftMotorEnable = 7;
const int LeftMotorPin1 = 6;
const int LeftMotorPin2 = 5;


const int RightMotorEnable = 4;
const int RightMotorPin1 = 3;
const int RightMotorPin2 = 2;


static uint8_t NBuff[6];


void setup()
{
  pinMode(LeftMotorEnable,OUTPUT);    
  pinMode(LeftMotorPin1,OUTPUT);    
  pinMode(LeftMotorPin2,OUTPUT);    
  pinMode(RightMotorEnable,OUTPUT);    
  pinMode(RightMotorPin1,OUTPUT);    
  pinMode(RightMotorPin2,OUTPUT);
  digitalWrite(LeftMotorEnable,HIGH);   
  digitalWrite(RightMotorEnable,HIGH);
  PowerSetup();    
  WiiNunchuckInit();
}


void loop()
{    
  delay(Run());
}


void PowerSetup()
{    
  pinMode(NGnd,OUTPUT);    
  pinMode(NVcc,OUTPUT);  
  digitalWrite(NGnd,LOW);
  digitalWrite(NVcc,HIGH); 
  delay(200);
}


void WiiNunchuckInit()
{   
  Wire.begin();  
  Wire.beginTransmission(0x52); 
  Wire.send(0x40); 
  Wire.send(0x00); 
  Wire.endTransmission();
}


void SendRequest()
{  
  Wire.beginTransmission(0x52);  
  Wire.send(0x00);
  Wire.endTransmission();
}


char Decode(char c)
{   
  c = (c ^ 0x17) +0x17;   
  return c;
}


int GetData()
{   
  int count = 0;  
  Wire.requestFrom(0x52,6);
  while(Wire.available()){      
    NBuff[count] = Decode(Wire.receive());
    count++;
  }
  SendRequest(); 
  if(count >= 5){     
    return 1;
  }    
  return 0;
}


int Run()
{    
  GetData();
  int joy_x = NBuff[0];   
  int joy_y = NBuff[1];   
  int accel_x = NBuff[2];  
  int accel_y = NBuff[3]; 
  int accel_z = NBuff[4];
  int button_c = 0;   
  int button_z = 0;
  
  if((NBuff[5] >> 0) & 1){     
    button_z = 1;
  }   
  if((NBuff[5] >> 1) & 1){   
    button_c = 1;
  }
  if((NBuff[5] >> 2) & 1){    
    accel_x += 2;
  }    
  if((NBuff[5] >> 3) & 1){  
    accel_x += 1;
  }
  if((NBuff[5] >> 4) & 1){   
    accel_y += 2;
  }   
  if((NBuff[5] >> 5) & 1){
    accel_y += 1;
  }
  if((NBuff[5] >> 6) & 1){   
    accel_z += 2;
  }   
  if((NBuff[5] >> 7) & 1){  
    accel_z += 1;
  }
  if(joy_y > 200){   
    digitalWrite(LeftMotorPin1,HIGH);
    digitalWrite(LeftMotorPin2,LOW);
    digitalWrite(RightMotorPin1,LOW);
    digitalWrite(RightMotorPin2,HIGH);
  }   
  else if(joy_y < 50){
    digitalWrite(LeftMotorPin1,LOW); 
    digitalWrite(LeftMotorPin2,HIGH);
    digitalWrite(RightMotorPin1,HIGH);
    digitalWrite(RightMotorPin2,LOW); 
  }
  else if(joy_x > 200){
    digitalWrite(LeftMotorPin1,HIGH);
    digitalWrite(LeftMotorPin2,LOW);
    digitalWrite(RightMotorPin1,HIGH);
    digitalWrite(RightMotorPin2,LOW);
  }   
  else if(joy_x < 40){ 
    digitalWrite(LeftMotorPin1,LOW); 
    digitalWrite(LeftMotorPin2,HIGH); 
    digitalWrite(RightMotorPin1,LOW); 
    digitalWrite(RightMotorPin2,HIGH);
  }
  else{
    digitalWrite(LeftMotorPin1,LOW);
    digitalWrite(LeftMotorPin2,LOW);
    digitalWrite(RightMotorPin1,LOW);
    digitalWrite(RightMotorPin2,LOW);
  }
  return 15;
}



DEMO:



=============2012/08/20===============


  如果想將加速度計的值換成角度可以用映射的方式,下面提供一個方法類似Arduino自己的map method,依照加速度計的值決定輸入範圍,利如我的x、y實際取值都是80~180,而z是90~180,則輸入值的範圍就是上面兩種,然後三軸的值都將其映射到-90~+90度,要注意這邊是做正規化(Normalize)而已。

Map:


double Map(double value,double Input_Min,double Input_Max,double Output_Min,double Output_Max)
{
  double rValue = (value - Input_Min) * (Output_Max - Output_Min) / (Input_Max - Input_Min) + Output_Min;

  double rMin,rMax;
  if(Output_Min < Output_Max){
    rMin = Output_Min;
    rMax = Output_Max;
  }
  else{
    rMin = Output_Max;
    rMax = Output_Min;
  }
  if(rValue < rMin){
    return rMin;
  }
  if(rValue > rMax){
    return rMax;
  }

  return rValue;
}

Use:


    int mapX = Map(accel_x,80.0,180.0,-90.0,90.0);
    Serial.print(mapX,DEC);
    Serial.print(",");
    int mapY = Map(accel_y,80.0,180.0,-90.0,90.0);
    Serial.print(mapY,DEC);
    Serial.print(",");
    int mapZ = Map(accel_z,90.0,180.0,-90.0,90.0);
    Serial.print(mapZ,DEC);
    Serial.print(" \t");

接著以Wii Nunchuck拿正時(搖桿朝上按鈕朝前),將X、Y正規化之後的值與Z正規化之後的值用atan2()即可求得X、Y與Z與地球重力的關係,即繞X軸旋轉(Roll)以及繞Y軸旋轉(Pitch),而繞Z軸旋轉(Yaw)要有陀螺儀才有辦法測定。


    int Roll = atan2(mapX,mapZ) / 3.14159 * 180.0;
    Serial.print(Roll,DEC);
    Serial.print(",");
    int Pitch = atan2(mapY,mapZ) / 3.14159 * 180.0;
    Serial.print(Pitch,DEC);
    Serial.print(" \t");







3 則留言:

  1. 請問如何將KINECT 與 ARDUINO 做連接?

    回覆刪除
  2. OpenNI,不過MCU一般都只做實體Device的控制並不做vision處理。

    回覆刪除