[延伸範例] 用wii搖桿控制小機械手臂

 小機械手臂

手臂上共有4顆馬達為KONDO的KRS-786馬達 有feedback的功能

而前端的DMP小手之前介紹過 所以這邊不多說



介紹影片:

第一次上鏡頭有點緊張啦...

啟動的時候會震動是因為手臂下方過輕...

原本想要塞一些東西的  不過還沒找到適合的物品

以及剪刀石頭布的布  因為正常狀態下就是布了  所以我沒有多做

附註: DMP小手雖然是三個自由度   但還是有密技可做出奇怪的手勢(請自行想像)



以下是code:

#include "stdio.h"
#include "conio.h"
#define USE_COMMON
#include "roboard.h"
#include "roboard_dll.h"

const unsigned char wii_addr = 0x52;
int joy_x_axis;
int joy_y_axis;
int accel_x_axis;
int accel_y_axis;
int accel_z_axis;
int z_button = 0;
int c_button = 0;
void wii_data(unsigned char*);
void rock(unsigned long);
void paper(unsigned long);
void scissors(unsigned long);
int main(){
 
 unsigned long motion_frame[32] = {1500L};
 unsigned char buf[6];
 unsigned long PWM_period = 10000L;

 roboio_SetRBVer(RB_110);
 rcservo_SetServo(RCSERVO_PINS1, RCSERVO_DMP_RS0263);
 rcservo_SetServo(RCSERVO_PINS2, RCSERVO_DMP_RS0263);
 rcservo_SetServo(RCSERVO_PINS3, RCSERVO_DMP_RS0263);
 rcservo_SetServo(RCSERVO_PINS4, RCSERVO_KONDO_KRS78X);
 rcservo_SetServo(RCSERVO_PINS5, RCSERVO_KONDO_KRS78X);
 rcservo_SetServo(RCSERVO_PINS6, RCSERVO_KONDO_KRS78X);
 rcservo_SetServo(RCSERVO_PINS7, RCSERVO_KONDO_KRS78X);
 rcservo_Init(RCSERVO_USEPINS1 + RCSERVO_USEPINS2 + RCSERVO_USEPINS3 + RCSERVO_USEPINS4 +
    RCSERVO_USEPINS5 + RCSERVO_USEPINS6 + RCSERVO_USEPINS7);
    i2c_Initialize(I2CIRQ_DISABLE);
 i2c0_SetSpeed(I2CMODE_FAST, 400000L);
 i2c0master_StartN(wii_addr, I2C_WRITE, 2);
 i2c0master_WriteN(0x40);
 i2c0master_WriteN(0x00);

 rcservo_EnterPlayMode();
 rcservo_SetAction(motion_frame, 1000); 
 while(!kbhit()){

  i2c0master_StartN(wii_addr, I2C_WRITE, 1);
  i2c0master_WriteN(0x00);
  delay_ms(5);
  i2c_Receive(wii_addr,buf,6);
  for(int i = 0; i < 6; i++)
   buf[i] = (buf[i] ^ 0x17) + 0x17;
  wii_data(buf);


  if(z_button && !c_button)
   rock(PWM_period);
  else if(!z_button && c_button)
      scissors(PWM_period);
  else if(!z_button && !c_button)
   paper(PWM_period);
  else if(z_button && c_button){
   printf("Enter sleep mode.\n");
   rcservo_EnterCaptureMode();
   delay_ms(5000);
   printf("You can click C button and Z button to close sleep mode.\n");
   while(1){
    i2c0master_StartN(wii_addr, I2C_WRITE, 1);
    i2c0master_WriteN(0x00);
    delay_ms(5);
    i2c_Receive(wii_addr,buf,6);
    for(int i = 0; i < 6; i++)
     buf[i] = (buf[i] ^ 0x17) + 0x17;
    wii_data(buf);
    if(z_button && c_button){
     printf("Close sleep mode.\n");
     break;
    }
    delay_ms(100);
   }
  }
  motion_frame[3] = ((joy_x_axis - 34) * 8.5) + 600;
  motion_frame[5] = ((joy_y_axis - 25) * 8.8) + 600;
  motion_frame[4] = ((accel_y_axis - 285) * 4.3) + 600;
  motion_frame[6] = 2300 - ((accel_x_axis - 285) * 4.3);
  rcservo_EnterPlayMode();
  rcservo_SetAction(motion_frame, 1000); 
  rcservo_SetFPS(50);
  while(rcservo_PlayAction() != RCSERVO_PLAYEND);
  
 }
 i2c_Close();
 rcservo_Close();
 return 0;
}
void wii_data(unsigned char *buf){
joy_x_axis = buf[0];
joy_y_axis = buf[1];
accel_x_axis = buf[2] << 2;
accel_y_axis = buf[3] << 2;
accel_z_axis = buf[4] << 2;
z_button = 1;
c_button = 1;
if ((buf[5] >> 0) & 1) z_button = 0;
if ((buf[5] >> 1) & 1) c_button = 0;
if ((buf[5] >> 2) & 1) accel_x_axis += 2;
if ((buf[5] >> 3) & 1) accel_x_axis += 1;
if ((buf[5] >> 4) & 1) accel_y_axis += 2;
if ((buf[5] >> 5) & 1) accel_y_axis += 1;
if ((buf[5] >> 6) & 1) accel_z_axis += 2;
if ((buf[5] >> 7) & 1) accel_z_axis += 1;
}
void rock(unsigned long PWM_period){
 unsigned long PINS1_duty = 1800L;
 unsigned long PINS2_duty = 700L;
 unsigned long PINS3_duty = 1800L;
 rcservo_SendCPWM(RCSERVO_PINS1, PWM_period, PINS1_duty);
 rcservo_SendCPWM(RCSERVO_PINS2, PWM_period, PINS2_duty);
 rcservo_SendCPWM(RCSERVO_PINS3, PWM_period, PINS3_duty);
}
void paper(unsigned long PWM_period){
 unsigned long PINS1_duty = 700L;
 unsigned long PINS2_duty = 1800L;
 unsigned long PINS3_duty = 700L;
 rcservo_SendCPWM(RCSERVO_PINS1, PWM_period, PINS1_duty);
 rcservo_SendCPWM(RCSERVO_PINS2, PWM_period, PINS2_duty);
 rcservo_SendCPWM(RCSERVO_PINS3, PWM_period, PINS3_duty);
}
void scissors(unsigned long PWM_period){
 unsigned long PINS1_duty = 1800L;
 unsigned long PINS2_duty = 1800L;
 unsigned long PINS3_duty = 1800L;
 rcservo_SendCPWM(RCSERVO_PINS1, PWM_period, PINS1_duty);
 rcservo_SendCPWM(RCSERVO_PINS2, PWM_period, PINS2_duty);
 rcservo_SendCPWM(RCSERVO_PINS3, PWM_period, PINS3_duty);
}

Read More

[範例] 讀取wii搖桿

讀取wii搖桿之值

螢幕上顯示的有toy X,Y 為搖桿兩軸
accel X,Y,Z 為加速度三軸
button Z,C 為兩顆按鈕


以下為code

#include "stdio.h"
#include "conio.h"
#define USE_COMMON
#include "roboard.h"
#include "roboard_dll.h"

const unsigned char wii_addr = 0x52;
int joy_x_axis;
int joy_y_axis;
int accel_x_axis;
int accel_y_axis;
int accel_z_axis;
int z_button;
int c_button;
void wii_data(unsigned char*);
int main(){
 roboio_SetRBVer(RB_110);
 unsigned char buf[6];

    i2c_Initialize(I2CIRQ_DISABLE);
 i2c0_SetSpeed(I2CMODE_FAST, 400000L);
 i2c0master_StartN(wii_addr, I2C_WRITE, 2);
 i2c0master_WriteN(0x40);
 i2c0master_WriteN(0x00);

 while(!kbhit()){

  i2c0master_StartN(wii_addr, I2C_WRITE, 1);
  i2c0master_WriteN(0x00);
  delay_ms(5);
  i2c_Receive(wii_addr,buf,6);
  for(int i = 0; i < 6; i++)
   buf[i] = (buf[i] ^ 0x17) + 0x17;
  wii_data(buf);
  printf("toy X:%d\ttoy Y:%d\n",joy_x_axis,joy_y_axis);
  printf("accel X:%d\taccel Y:%d\taccel Z:%d\n", accel_x_axis, accel_y_axis, accel_z_axis);
  printf("button Z:%d\tbutton C:%d\n",z_button,c_button);
  system("cls");
 }
 i2c_Close();
 return 0;
}
void wii_data(unsigned char *buf){
joy_x_axis = buf[0];
joy_y_axis = buf[1];
accel_x_axis = buf[2] << 2;
accel_y_axis = buf[3] << 2;
accel_z_axis = buf[4] << 2;
z_button = 1;
c_button = 1;
if ((buf[5] >> 0) & 1) z_button = 0;
if ((buf[5] >> 1) & 1) c_button = 0;
if ((buf[5] >> 2) & 1) accel_x_axis += 2;
if ((buf[5] >> 3) & 1) accel_x_axis += 1;
if ((buf[5] >> 4) & 1) accel_y_axis += 2;
if ((buf[5] >> 5) & 1) accel_y_axis += 1;
if ((buf[5] >> 6) & 1) accel_z_axis += 2;
if ((buf[5] >> 7) & 1) accel_z_axis += 1;
}

Read More

[教學] 如何讀取wii搖桿

上一次有提到wii的搖桿要如何接上I2C的線

這次就說明一下如何讀取

首先可以看到
http://www.windmeadow.com/node/42
這個網站

他有完整的說明

不過我這邊再仔細的說一次

• WII搖桿的Speed

根據網站上寫的
Since the nunchuck uses "Fast" I2C, we will need to change the default speed:

#define TWI_FREQ 400000L.

可以得知
speed_mode為fast

bps為400000L

 

• WII搖桿傳輸的基本設定

文件中提到
For the Arduino to communicate with the nunchuck, it must send a handshake. So first send 2 bytes "0x40,0x00". Then send one byte "0x00" each time you request data from the nunchuck. The data from the nunchuck will come back in 6 byte chunks.
在一開始必須先傳兩個byte給搖桿
分別為0x40與0x00

然後再每次讀取前必須先傳一個0x00給搖桿

傳輸完後會回傳6個byte回來

建議在傳輸完0x00後delay一小段時間

• wii解碼

這邊是比較多人容易忽略的

再文件中似乎沒提到
不過在下面的範例程式有提到
char

nunchuk_decode_byte (char x)

{

  x = (x ^ 0x17) + 0x17;

  return x;

}

 

這表示傳回的6個byte都必須先經過解碼才可以使用
否則會得到錯誤的值
解碼的方式為 先將值XOR 0x17再加上0x17

• 如何解讀6個BYTE

首先要先了解到搖桿有哪些值會讀入
有搖桿,加速度,兩顆按鍵

搖桿有XY兩軸
加速度則有XYZ三軸
所以對照到文中的表格

1. 第一個byte為搖桿的X軸
2. 第二個byte為搖桿的Y軸
3. 第三個byte為加速度的X軸的前8個bit
4. 第四個byte為加速度的Y軸的前8個bit
5. 第五個byte為加速度的Z軸的前8個bit
6. 第六個byte   先分為8個bit看

           第一個bit為Z鍵 0為按下 1為沒按下
           第二個bit為C鍵 0為按下 1為沒按下
           接著
           第三及第四為加速度X的後兩個bit
           第五及第六為加速度Y的後兩個bit
           第七及第八為加速度Z的後兩個bit

所以這裡可以得知加速度一共有10個bit

這樣就可以讀出所有wii搖桿的資訊了









影片及code將會補貼...

Read More

[教學] 如何wii搖桿接上I2C

wii搖桿接上I2C



由於WII的搖桿傳輸是用I2C   所以Roboard沒道理不能控制他

先前已經介紹過G146 現在介紹的為wii的搖桿

要有wii搖桿, I2C線材, 最重要的RB一台

首先先告訴各位如何將wii接上I2C的線材

各位可以用wiki看到wii腳位的對應圖

http://wiibrew.org/wiki/Wiimote/Extension_Controllers

這裡很清楚的可以看到

wii上有6個腳位

分別為

SCL    保留   GND

VCC   保留   SDA

這六個腳位

其中中間的兩個保留腳位  我想大概是留來用於未來新增功能用的

接著看到RB的I2C腳位

RB上有六個腳位

只要將相對應的4個腳位接上去即可

注意的一點為

wii搖桿只吃3.3V  給5V的話   燒壞不負責...

基本上沒有接錯的話就不會有太大的問題

還有一點必須注意

每個腳位之間 盡量想辦法別讓腳位有機會互相碰觸到

可以用膠帶 或熱融膠等東西固定並且隔開

Read More