RoBoard魔人的機器人日誌

2011/12/22

[練習] graph走訪程式

當一台機器人

想要讓他走到所有想讓他去的地方

那就需要利用走訪的程式

(本魔承認有點硬要扯到機器人...)

總之

來介紹一下兩種走訪方式



深度優先搜尋(depth - first search) 簡稱DFS

簡單的說就是先找到最深的路  再退回來走其他的路

像我的範例

若是用深度優先
他會從1 -> 2 -> 4 -> 3 然後沒路了   退回4  再走5     再往後退   都沒路就結束

以下為DFS的code

#include "stdafx.h"
#define graph_Max 5
void DFS(int);
static bool graph[graph_Max][graph_Max] = {
           {0,1,1,0,0},
           {1,0,0,1,0},
           {1,0,0,1,0},
           {0,1,1,0,1},
           {0,0,0,1,0}};
static int point_name[graph_Max] = {1,2,3,4,5};
int main(){
 DFS(0);
 getchar();
 return 0;
}
bool visited[graph_Max] = {false};
void DFS(int v){
 visited[v] = true;
 printf("%d\n",point_name[v]);
 for(int i = 0; i < graph_Max; i++)
  if(graph[v][i] && !visited[i])
   DFS(i);
}


廣度優先搜尋(breath - first search) 簡稱BFS

簡單的說就是將所有可走的路先走過  再走第二次...走到全部沒路為止
像我的範例

若是用廣度優先
他會從1 -> 2 & 3  然後1沒其他路   
2 -> 4  然後2沒其他路   
3 也沒其他路
再來到4 -> 5
最後的5  也沒路了    全部都沒路   結束


以下為BFS的code

#include "stdafx.h"
#define graph_Max 5
static bool graph[graph_Max][graph_Max] = {
           {0,1,1,0,0},
           {1,0,0,1,0},
           {1,0,0,1,0},
           {0,1,1,0,1},
           {0,0,0,1,0}};
static int point_name[graph_Max] = {1,2,3,4,5};
void BFS(int);
void push(int);
int pop();
bool isEmpty();
int main()
{
 BFS(0);
 getchar();
 return 0;
}
bool visited[graph_Max] = {false};
void BFS(int v){
 visited[v] = true;
 printf("%d\n",point_name[v]);
 for(int i = 0; i < graph_Max; i++)
  if(graph[v][i] && !visited[i]){
   visited[i] = true;
   push(i);
  }
 while(!isEmpty())
  BFS(pop());
}
int queue[graph_Max];
int first = 0;
int last = 0;
void push(int x){
 if(last >= graph_Max)
  last = 0;
 queue[last++] = x;
}
int pop(){
 return queue[first++];
}
bool isEmpty(){
 return first == last;
}



以上就是本魔這次介紹的兩個走訪程式
Share:

2011/12/16

[練習] 數獨程式


用NXT做的數獨機器人

前面比較繁瑣的工作是判斷位置

以及掃描出數字

將9*9表格讀入機器後就可以開始進行運算





藉此機會

就來研究看看解數獨的程式該如何寫吧

首先要瞭解規則

在1~9行內絕對只有1~9的數字 且不重複
在1~9列內絕對只有1~9的數字 且不重複
以每9格(正方) 為單位  9格內也只有1~9的數字 且不重複

例如:

1 2 3 4 5 6 7 8 9
4 5 6 7 8 9 1 2 3
7 8 9 1 2 3 4 5 6
2 3 4 5 6 7 8 9 1
5 6 7 8 9 1 2 3 4
8 9 1 2 3 4 5 6 7
3 4 5 6 7 8 9 1 2

這就算是一個數獨的答案

然而數獨的每個題目  永遠都只會有一個答案(如果有很多答案就沒什麼好玩了)










第一種解法:
利用佇列解
我是參考書上寫的
不過這方法似乎只能解基本的數獨
因為他是將未填入數字的位置 存入佇列中
再一一拿出來做

因為有一些數獨是需要用判斷的才能解
所以這種解法可能無法適用於所有的題目




#include <stdio.h>
struct data_item{
 int x,y,val;
};
struct sudoku_item{
 int x,y;
 int num;
 int val[10];
 int resVal;
};
sudoku_item sudoku[9][9];
int rear,front;
data_item *dataQ[9*9];
void createQueue(){
   rear = front = 0;
}
void enque(data_item *dataItem){
 dataQ[rear] = dataItem;
 rear = (rear +1) % (9*9);
}
data_item *deque(){
 data_item *dataItem = dataQ[front];
 front = (front + 1)% (9*9);
 return dataItem;
}
int isEmpty(){
 return rear == front;
}
data_item *clrSudokuItem(int x, int y, int n){
 data_item *dataItem;
 int i;
 if(sudoku[x][y].resVal == 0){
  sudoku[x][y].num-= sudoku[x][y].val[n];
  sudoku[x][y].val[n] = 0;
  if(sudoku[x][y].num == 1)
   for(i = 1; i < 10; i++)
    if(sudoku[x][y].val[i] == 1){
     sudoku[x][y].resVal = i;
     dataItem = (data_item *) new data_item;
     dataItem ->x = x;
     dataItem ->y = y;
     dataItem ->val = i;
     return dataItem;
    }
 }
 return NULL;
}
void preprocess(int dataArr[][9]){
 data_item *dataItem;
 int i,j,k;
 createQueue();
 for(i = 0; i < 9;i++){
  for(j = 0; j< 9; j++){
   printf("%d ",dataArr[i][j]);
   if(dataArr[i][j] != 0){
    sudoku[i][j].num = 1;
    sudoku[i][j].resVal = dataArr[i][j];
    dataItem = (data_item *) new data_item;

    dataItem->x = i;
    dataItem->y = j;
    dataItem->val = dataArr[i][j];
    enque(dataItem);
   }else{
    sudoku[i][j].x = i;
    sudoku[i][j].y = i;
    for(k = 1; k<10; k++)
     sudoku[i][j].val[k] = 1;
    sudoku[i][j].num = 9;
    sudoku[i][j].resVal = 0;
   }
  }
  printf("\n");
 }
}
void processSudoku(){
 data_item *dataItem,*retData;
 int i,j;
 while(!isEmpty()){
  int x,y,val;
  dataItem = deque();
  x =dataItem->x;
  y =dataItem->y;
  val = dataItem->val;
  for(i = 0; i<9; i++){
   if(i != y){
    retData = clrSudokuItem(x,i,val);
    if(retData != NULL)
     enque(retData);
   }
   if(i != x){
    retData = clrSudokuItem(i,y,val);
    if(retData != NULL)
     enque(retData);
   }
  }
  for(i = 0; i<3; i++)
   for(j = 0; j<3; j++){
    int m = x/3 *3 +i;
    int n = y/3 *3 +j;
    if(m!=x && n != y){
     retData = clrSudokuItem(m,n,val);
     if(retData != NULL)
      enque(retData);
    }
   }
 }
}
void output(){
 int i,j;
 printf("Answer:\n");
 for(i = 0; i<9; i++){
  for(j = 0; j<9; j++)
   printf("%d ",sudoku[i][j].resVal);
  printf("\n");
 }
}
int main(){
 int data[9][9] = {
  {3,0,1,0,0,9,0,5,0},
  {0,9,0,0,0,4,1,8,0},
  {0,0,0,6,2,0,9,0,4},
  {1,0,0,0,4,0,0,6,0},
  {0,5,0,0,6,0,0,4,0},
  {0,8,0,9,7,0,5,0,2},
  {0,0,5,0,3,2,0,0,0},
  {0,2,9,0,0,0,0,0,5},
  {0,6,0,5,9,0,4,0,0}};
 preprocess(data);
 processSudoku();
 output();
 getchar();
 return 0;
}







第二種解法:
第二次我是利用遞迴讓所有可能性直接跑完(直到有答案)
相信大家都知道遞迴的缺點 浪費時間,空間
不過我還沒想到可以縮短執行時間,空間的方法
若有 歡迎指教

#include "stdafx.h"
struct local_mark{
 int x[9];
 int y[9];
 int z[9];
};
struct point{
 int x;
 int y;
};
local_mark *Number_Place_set(int [][9]);
bool Number_Place_game(int [][9], local_mark *);
int local_val(int, int);
int bitcount(int );
void print_data(int [][9]);
int main(){ 
 int data[9][9] = {
     {3,0,1,0,0,9,0,5,0},
     {0,9,0,0,0,4,1,8,0},
     {0,0,0,6,2,0,9,0,4},
     {1,0,0,0,4,0,0,6,0},
     {0,5,0,0,6,0,0,4,0},
     {0,8,0,9,7,0,5,0,2},
     {0,0,5,0,3,2,0,0,0},
     {0,2,9,0,0,0,0,0,5},
     {0,6,0,5,9,0,4,0,0}};
 printf("Subject:\n");
 print_data(data);
 local_mark *mark = Number_Place_set(data);
 printf("Answer:\n");
 if(Number_Place_game(data,mark)){
  print_data(data);
 }else{
  printf("No solution!\n");
 } 

 getchar();
 return 0;
}

local_mark *Number_Place_set(int data[][9]){
 local_mark *mark = (local_mark*) new local_mark;
 for(int i = 0; i < 9; i++ )
  mark -> x[i] = mark -> y[i] = mark -> z[i] = 0;
 for(int i = 0; i < 9; i++ )
  for(int j = 0; j < 9; j++)
   if(data[i][j]){
    mark -> x[j] |= (1 << data[i][j]-1);
    mark -> y[i] |= (1 << data[i][j]-1);
    mark -> z[local_val(i,j)] |= (1 << data[i][j]-1);
   }
 return mark;
}
bool Number_Place_game(int data[][9], local_mark *mark){
 point *p = (point*) new point;
 int m , max = 0 , count = 0;
 for(int i = 0; i < 9; i++ )
  for(int j = 0; j < 9; j++)
   if(!data[i][j]){
    count++;
    m = mark->x[j] | mark->y[i] | mark->z[local_val(i,j)];
    if(bitcount(m) > bitcount(max)){
     max = m;
     p -> x = j;
     p -> y = i;
    }
   }
 if(count == 0)
  return true;
 for(int i = 0; i < 9; i++)
  if(max != (max | (1 << i))){
   data[ p->y ][ p->x ] = i+1;
   mark -> x[ p->x ] |= (1 << i);
   mark -> y[ p->y ] |= (1 << i);
   mark -> z[ local_val( p->y , p->x ) ] |= (1 << i);
   if(Number_Place_game(data,mark)){
    return true;
   }else{
    data[ p->y ][ p->x ] = 0;
    mark -> x[ p->x ] &= ~(1 << i);
    mark -> y[ p->y ] &= ~(1 << i);
    mark -> z[ local_val( p->y , p->x ) ] &= ~(1 << i);
   }
  }
 return false;
}
int local_val(int x,int y){
 return y/3+(x/3)*3;
}
int bitcount(int x){
 int b;
 for(b = 0; x; x &= x-1)
  ++b;
 return b;
}
void print_data(int data[][9]){
 for(int i = 0; i < 9; i++){
  for(int j = 0; j < 9; j++)
   printf("%d ",data[i][j]);
  printf("\n");
 }
}

Share:

2011/12/8

[推薦] 在blogger裡使用google-code-prettify

我想會來到魔人這邊的訪客大部分應該都對機器人有興趣

甚至有已經接觸過機器人的吧?

成品作出來之後當然會想拿出來和大家分享囉!!

機器人免不了要經過coding的過程

所以這邊推薦一個蠻好用的程式碼高亮度顯示程式



安裝步驟按照下面作計可以囉~


首先到Blogger的[資訊主頁]   選擇[設計]

然後找到[修改HTML] 將以下所有內容貼至<head>至</head>之間

<link href='http://google-code-prettify.googlecode.com/svn/trunk/src/prettify.css' type='text/css' rel='stylesheet' />
<script type='text/javascript' src='http://google-code-prettify.googlecode.com/svn/trunk/src/prettify.js'></script>





















下一個步驟必須要找到<body> 然後再onload的事件加入prettyPrint();

找不到<body>的話可以直接用搜尋(ctrl+F) 找"<body"字串



onload='prettyPrint()'



















做完以上的步驟之後就可以在你的文章中貼漂亮的程式碼囉

不過貼了之後  還要到原始碼模式

在程式碼前後加上
<pre class="prettyprint">
//這邊貼程式碼
</pre>


還有必須注意的一點為

程式碼不可在原始碼模式下直接貼上

否則原始碼模式會把& < >字元吃掉

在原始碼模式下
“&” 需轉換為 “&amp;”
”<” 需轉換為 “&lt;”
”>” 需轉換為 “&gt;”



貼個小範例程式吧!
#include "stdio.h"
int main(){
   printf("Hello, World!");
   getchar();
   return 0;
}
Share:

2011/11/30

[延伸範例] 用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);
}
Share:

2011/11/29

[範例] 讀取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;
}
Share:

2011/11/12

[教學] 如何讀取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將會補貼...
Share:

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

wii搖桿接上I2C


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

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

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



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

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


這裡很清楚的可以看到

wii上有6個腳位

分別為

SCL    保留   GND

VCC   保留   SDA


這六個腳位

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


接著看到RB的I2C腳位










RB上有六個腳位

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

注意的一點為

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

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

還有一點必須注意

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

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




Share:

2011/10/20

[教學] 如何控制RS0263-DMP小手

DMP小手 (RS0263*3)


RS0263為RoBoard的馬達
必須用RC servo控制馬達轉動

  • RC servo的控制原理
  1. 利用 PWM pulse 的寬度指定 servo 轉動的位置
  2. controller circuit 控制 servo 轉到指定的位置
  3. feedback potentiometer 會將 servo 目前的位置迴授給控制器
由於RS0263沒有feedback的功能
所以並不會將servo的位置回傳


  • 腳位相對位置
RB100有24個PWM
而RB110只有16個PWM
但其實還是十分好分辨的
靠外側的腳位為地(GND) 通常接的為黑線
靠中間的腳位為電(Vxx) 通常接的為紅線
靠內側的腳位為訊號(GPxx) 沒有特定顏色

切記勿接錯 否則會發生慘案

然而腳位編號請看官網的介紹投影片(Hardware Introduction Slide)


  • RC Servo lib
 rcservo_Init(...);
這邊記得將要用的所有Pin腳都加入
例如要加入1號與5號的腳位即:
rcservo_Init(RCSERVO_USEPINS1 + RCSERVO_USEPINS5);



 rcservo_SetServo(pin, servo_model);
SetServo要為妳要用的腳位設定傳輸的型態
這lib很佛心的是 幾乎所有會用到的RCservo 都已經先幫我們寫好型態了
所以只要依照馬達給相對應的模式 就可以使用了


rcservo_EnterPWMMode()
這是一個告訴PWM要開始的函式

然而傳輸方式有以下兩種
1. rcservo_SendPWM(pin, PWM period, PWM duty, count)

2. reservo_SendCPWM(pin, PWM period, PWM duty)

相信有人會問兩者的差別
第1種方法要搭配另一個函式rcservo_IsPWMCompleted(pin)
如函式名稱所寫   它是用來判斷此腳位的通訊是否完成
然而第一種方法的count就是要連續發出count筆通訊資料給馬達
當然在發資料的同時 程式會繼續往下跑    為了讓程式能稍稍暫停
就會用到判斷通訊完成的函式了

第二種方法雖然比較好用
不過使用時必須要小心
因為它是一直傳送通訊資料
直到使用函式reservo_StopPWM(pin)為止

兩者各有優缺

















code: 用G146地磁控制DMP小手做剪刀石頭布
#include "stdio.h"
#include "conio.h"
#include "roboard.h"
#include "roboard_dll.h"
void rock(unsigned long);
void paper(unsigned long);
void scissors(unsigned long);
const unsigned char acceler_addr = 0x30>>1;
int main(void)
{
 roboio_SetRBVer(RB_110);
 rcservo_Init(RCSERVO_USEPINS1 + RCSERVO_USEPINS2 + RCSERVO_USEPINS3);
 rcservo_SetServo(RCSERVO_PINS1, RCSERVO_DMP_RS0263);
 rcservo_SetServo(RCSERVO_PINS2, RCSERVO_DMP_RS0263);
 rcservo_SetServo(RCSERVO_PINS3, RCSERVO_DMP_RS0263);
 rcservo_EnterPWMMode();

    i2c_Initialize(I2CIRQ_DISABLE);
 i2c0_SetSpeed(I2CMODE_AUTO, 400000L);
 i2c0master_StartN(acceler_addr, I2C_WRITE,2);
 i2c0master_WriteN(0x20);
 i2c0master_WriteN(0x37);

 unsigned int acceler_X, acceler_Y,mode,lastmode;
 unsigned long PWM_period = 10000L;
 while(!kbhit()){
  i2c0master_StartN(acceler_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x29);
  i2c0master_StartN(acceler_addr, I2C_READ,1);
  acceler_X = i2c0master_ReadN();

  i2c0master_StartN(acceler_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x2b);
  i2c0master_StartN(acceler_addr, I2C_READ,1);
  acceler_Y = i2c0master_ReadN();
  
  mode = 0;
  mode = acceler_X >= 0x80 ? mode + 1 : mode;
  mode = acceler_Y >= 0x80 ? mode + 2 : mode;
  switch(mode){
     case 0:
      if(lastmode != mode)
       printf("paper!\n");
      paper(PWM_period);
      break;
     case 1:
      if(lastmode != mode)
       printf("scissors!\n");
      scissors(PWM_period);
      break;
     case 3:
      if(lastmode != mode)
       printf("rock!\n");
      rock(PWM_period);
      break;
  }
  lastmode = mode;
 }
 i2c_Close();
 rcservo_Close();
 return 0;
}
void rock(unsigned long PWM_period){
 unsigned long PINS1_duty = 1900L;
 unsigned long PINS2_duty = 700L;
 unsigned long PINS3_duty = 1900L;
 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 = 1900L;
 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 = 1900L;
 unsigned long PINS2_duty = 1900L;
 unsigned long PINS3_duty = 1900L;
 rcservo_SendCPWM(RCSERVO_PINS1, PWM_period, PINS1_duty);
 rcservo_SendCPWM(RCSERVO_PINS2, PWM_period, PINS2_duty);
 rcservo_SendCPWM(RCSERVO_PINS3, PWM_period, PINS3_duty);
}
Share:

2011/10/17

[教學] 如何控制RM-G146陀螺儀

RM-G146

 

 
控制G146必須要用I2C去控制
因為我這邊使用的範例為RB110

 
所以沒有特別再去弄線材
首先必須知道的資訊
陀螺儀的位址 在0xD0
以及陀螺儀的XYZ軸的位址
AddrHex   AddrDecimal     Register Name        
1D             29                       GYRO_XOUT_H
1E             30                       GYRO_XOUT_L

 
1F             31                       GYRO_YOUT_H
20             32                       GYRO_YOUT_L

 
21             33                       GYRO_ZOUT_H
22             34                       GYRO_ZOUT_L
這樣就可以用一些基本的指令去讀取陀螺儀得值了

 
而I2C的讀取方法為
| start | slave address | R/W | A | data | A/A |stop |

 
所以基本上會用到的指令有
i2c0master_StartN(slave address,R/W,byte)           告訴某位置要讀或寫N的byte
i2c0master_WriteN(byte)           寫入的值
i2c0master_ReadN()          會回傳一個值

 
在W/R之後會值接回傳STOP

 
所以無須再傳stop

 

 

 


 

 

 

 

 

 

 

 
範例code    用G146控制AX-12馬達:

#include "stdio.h"
#include "conio.h"
#include "windows.h"
#include "roboard.h"
#include "roboard_dll.h"

const unsigned char gyro_addr = 0xD0>>1;

int main()
{
 unsigned int gyro_X, gyro_Y, d;
 unsigned char cmd1[11] = {0xff, 0xff, 0x01, 0x07, 0x03, 0x1e, 0x00, 0x02, 0x00, 0x02, 0x00};
 unsigned char cmd2[11] = {0xff, 0xff, 0x02, 0x07, 0x03, 0x1e, 0x00, 0x02, 0x00, 0x02, 0x00};
 unsigned char buf[6] = {0};
 unsigned int i = 0;

 roboio_SetRBVer(RB_110);

 com3_Init(COM_HDUPLEX);
 com3_SetFormat(COM_BYTESIZE8, COM_STOPBIT1, COM_NOPARITY);
 com3_SetBaud(COMBAUD_115200BPS);

    i2c_Initialize(I2CIRQ_DISABLE);
 i2c0_SetSpeed(I2CMODE_AUTO, 400000L);
 i2c0master_StartN(gyro_addr, I2C_WRITE,2);
 i2c0master_WriteN(0x16);
 i2c0master_WriteN(0x18);
 while(i++ < 60){
  i2c0master_StartN(gyro_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x1d);
  i2c0master_StartN(gyro_addr, I2C_READ,1);
  gyro_X = i2c0master_ReadN();
  gyro_X = gyro_X << 8;
  
  i2c0master_StartN(gyro_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x1e);
  i2c0master_StartN(gyro_addr, I2C_READ,1);
  d = i2c0master_ReadN();
  gyro_X = gyro_X + d;

  i2c0master_StartN(gyro_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x1f);
  i2c0master_StartN(gyro_addr, I2C_READ,1);
  gyro_Y = i2c0master_ReadN();

  gyro_Y = (gyro_Y + 1) &(~256);
  gyro_Y = gyro_Y << 8;

  i2c0master_StartN(gyro_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x20);
  i2c0master_StartN(gyro_addr, I2C_READ,1);
  d = i2c0master_ReadN();
  gyro_Y = gyro_Y + d;
  
  gyro_X /= 0x40;
  if(gyro_X > 0x1ff){
   gyro_X -= 0x200;
   if(cmd1[6] + (gyro_X % 0x100) >= 0x100){
    cmd1[6] = cmd1[6] + (gyro_X % 0x100) - 0x100;
    cmd1[7] = cmd1[7] + 0x01;
   }else{
    cmd1[6] = cmd1[6] + (gyro_X % 0x100);
    cmd1[7] = cmd1[7] + (gyro_X >> 8);
   }
  }else{
   cmd1[6] = cmd1[6] + (gyro_X % 0x100) - 0x1ff; 
   cmd1[7] = cmd1[7] + (gyro_X >> 8) - 0x1ff;
  }

  gyro_Y /= 0x40;
  if(gyro_Y > 0x1ff){
   gyro_Y -= 0x200;
   if(cmd2[6] + (gyro_Y % 0x100) >= 0x100){
    cmd2[6] = cmd2[6] + (gyro_Y % 0x100) - 0x100;
    cmd2[7] = cmd2[7] + 0x01;
   }else{
    cmd2[6] = cmd2[6] + (gyro_Y % 0x100);
    cmd2[7] = cmd2[7] + (gyro_Y >> 8);
   }
  }else{
   cmd2[6] = cmd2[6] + (gyro_Y % 0x100) - 0x1ff; 
   cmd2[7] = cmd2[7] + (gyro_Y >> 8) - 0x1ff;
  }

  cmd1[7] = cmd1[7] >= 0x03 ? 0x03 : cmd1[2];
  cmd1[7] = cmd1[7] < 0x00 ? 0x00 : cmd1[2];
  cmd2[7] = cmd2[7] >= 0x03 ? 0x03 : cmd2[2];
  cmd2[7] = cmd2[7] < 0x00 ? 0x00 : cmd2[2];

  cmd1[10] = ~(cmd1[2] + cmd1[3] + cmd1[4] + cmd1[5] + cmd1[6] + cmd1[7] + cmd1[8] + cmd1[9]);
  cmd2[10] = ~(cmd2[2] + cmd2[3] + cmd2[4] + cmd2[5] + cmd2[6] + cmd2[7] + cmd2[8] + cmd2[9]);
  com3_ServoTRX(cmd1, 11, buf, 6);
  com3_ServoTRX(cmd2, 11, buf, 6);
  _sleep(250);
 }
 i2c_Close();
 com3_Close();
 return 0;
}
Share:

2011/10/7

[教學] 如何控制Bioloid AX-12馬達

上一個星期已經告訴過各位AX-12是如何傳輸的

那現在要控制其實已經差不遠了

首先必須要有一條com3轉接AX-12的轉接線
然後就可以開始撰寫程式部分了

首先要先將專案屬性先設定好

組態先選擇為Release (因為Debug會出現問題)

要把組態結構 -> C/C++ -> 一般 其中的 "其他Include目錄" 將他指定到RoBoIO-bin-v1.8-winxp\Include

然後組態結構 -> 連結器 -> 一般 中的 "其他程式庫目錄"  將他指定RoBoIO-bin-v1.8-winxp\Lib\VC2008

最後將組態結構 -> 連結器 -> 輸入 中的 "其他相依性" 內容輸入RoBoIO.lib

這樣設定好  就可以使用RoBoard的Lib了













首先程式碼

必須要有的是設定RB的型號
 roboio_SetRBVer(RB_100); 
Or
roboio_SetRBVer(RB_110);

之後就是做com3的設定了

首先至少要有2個array

一個是傳出 一個是傳入

將初值都設定好了之後

先做com3 Init的動作

然後設定Data Formate
com3_SetFormat(bytesize, stopbit, parity);
bytesize是每筆資料共有幾個bits
stopbit為停止位元
parity則是設定檢查碼的方式

接著設定Baud

Baud是用來讓RoBoard和馬達之間有一個共同的速度傳輸

否則收發速度不同則會導致資料錯亂

接著就可以開始設計一整個程式碼了

當中會用到的有
com3_ServoTRX(cmd, csize, buf, size);
cmd為傳出給馬達的資料陣列
csize為cmd的size
buf是用來儲存馬達回傳的值  通常若有錯誤訊息可以在這找到
size為buf的size

然後在程式的末端

記得打上com3_Close();

將com3關閉






#include "stdio.h"
#include "conio.h"
#include "roboard.h"

int main(void)
{
 unsigned char c;
 unsigned char cmd1[11] = {0xff, 0xff, 0x01, 0x07, 0x03, 0x1e, 0x00, 0x02, 0x00, 0x02, 0x00};
 unsigned char cmd2[11] = {0xff, 0xff, 0x03, 0x07, 0x03, 0x1e, 0x00, 0x02, 0x00, 0x02, 0x00};
 unsigned char buf[6] = {0};
 int num1, num2;

 num1 = num2 = 0x200;
 roboio_SetRBVer(RB_110);

 if(com3_Init(COM_HDUPLEX) == false)
 {
  printf("Init COM3 fail\n");
  return 1;
 }

 com3_SetFormat(COM_BYTESIZE8, COM_STOPBIT1, COM_NOPARITY);
 com3_SetBaud(COMBAUD_115200BPS);

 while((c = _getch()) != 27)
 {
  switch(c)
  {
  case 'a': case 'A':
   num1 += 0x05;
   break;
  case 'z': case 'Z':
   num1 -= 0x05;
   break;
  case 's': case 'S':
   num2 += 0x05;
   break;
  case 'x': case 'X':
   num2 -= 0x05;
   break;
  default:
   break;
  }
  
  switch(c)
  {
  case 'a': case 'A': case 'z': case 'Z':
   if(num1 < 0) num1 = 0x0000;
   if(num1 > 0x3ff) num1 = 0x3ff;
   cmd1[7] = (unsigned char) ((num1 >> 8) & 0xff);
   cmd1[6] = num1 & 0xff;
   cmd1[10] = ~(cmd1[2] + cmd1[3] + cmd1[4] + cmd1[5] + cmd1[6] + cmd1[7] + cmd1[8] + cmd1[9]);
   com3_ServoTRX(cmd1, 11, buf, 6);
   break;
  case 's': case 'S': case 'x': case 'X':
   if(num2 < 0) num2 = 0x0000;
   if(num2 > 0x3ff) num2 = 0x3ff;
   cmd2[7] = (unsigned char) ((num2 >> 8) & 0xff);
   cmd2[6] = num2 & 0xff;
   cmd2[10] = ~(cmd2[2] + cmd2[3] + cmd2[4] + cmd2[5] + cmd2[6] + cmd2[7] + cmd2[8] + cmd2[9]);
   com3_ServoTRX(cmd2, 11, buf, 6);
   break;
  default:
   cmd1[10] = ~(cmd1[2] + cmd1[3] + cmd1[4] + cmd1[5] + cmd1[6] + cmd1[7] + cmd1[8] + cmd1[9]);
   cmd2[10] = ~(cmd2[2] + cmd2[3] + cmd2[4] + cmd2[5] + cmd2[6] + cmd2[7] + cmd2[8] + cmd2[9]);
   com3_ServoTRX(cmd1, 11, buf, 6);
   com3_ServoTRX(cmd2, 11, buf, 6);
   break;
  }
 }

 com3_Close();
 return 0;
}
Share:

2011/10/1

[教學] Bioloid AX-12馬達如何傳輸

如果想要控制Bioloid AX-12馬達

首先必須先了解Bioloid AX-12馬達是如何傳輸的

可以看AX-12的datasheet比較容易理解




正如上圖

首先會先傳兩個0XFF告訴馬達要開始了

然後第三個指令封包ID 是要確認傳送給此ID的馬達

接著為LENGTH 是告訴馬達開始接收連續幾個封包

INSTRUCTION是決定要讀出,寫入等

接下來的幾個是下指令的部分

首先是指令的Address 可以把他想成程式中的指標

然後接下來的是將Address指到的後面N個改為給的值

最後的CHECK SUM正如datasheet寫的 將前面所有的值相加做not











舉個例子應該會比較好懂些
假如我傳入這串指令
0xff, 0xff, 0x01, 0x07, 0x03, 0x1e, 0x00, 0x02, 0x00, 0x02, 0x00

他的意思是對ID:1的馬達座寫入
寫入的Address為0x1e 後面有4個值0x00, 0x02, 0x00, 0x02
這邊看對照表





前面兩個為馬達的角度

後面兩個為馬達的速度

再來看到詳細的說明


這邊提到0x1e與0x1f為馬達角度 最高0~300度
極限設定為 0x000~0x3ff

這邊提醒下 前面提到0x1e為L 低電位 0x1f為H 高電位
所以連續兩個封包0x00, 0x02 在機器中是讀為0x200的角度
所以轉到會150度的地方



然後這邊是說 馬達的轉速為0x000~0x3ff

和前面一樣0x20為L 低電位 0x21為H 高電位
所以連續兩個封包0x00, 0x02 在機器中是讀為0x200的速度

所以馬達會用最中等的速度轉動




最後一個CHECK SUM我沒有計算出來
不過算式是很簡單的
~(0x01 + 0x07 + 0x03 + 0x1e + 0x00 + 0x02 + 0x00 + 0x02)
Share:

2011/9/20

[教學]如何用fritzing做一個RoBoard的樣板

首先進入fritzing的官網

將fritzing下載下來

http://fritzing.org/download/




開啟程式之後首先會看到麵包板檢視

然後需要新增一個RoBoard的元件

在右下角有一個元件的圖片














選取新增

會出現新元件的內容





















此時將名字.圖檔.說明...等等輸入


























新增完後必須新增連接器

才可以在frizing裡面使用

不然會只有一張圖在那喔


















新增連接器

並且移動到相對應的位置(這邊我簡略的拉幾個做代表就好)
















之後就可以在frizing上面做跟麵包板的連結了!

Share:
技術提供:Blogger.

追蹤者