Arduino筆記(93)：Wii Nunchcuk使用PCA9685控制四軸機器手臂

byCeiling Tsai — 0
常看到影片中的製造工廠自動化，都是透過機器手臂或無人搬運車來完成。今天的實作要接續上一篇：Arduino筆記(92)：Wii Nunchcuk控制伺服馬達，將多個伺服馬達，使用固定架組合成機器手臂，控制其轉向及擺動動作，當伺服馬達數量越多時，手臂的動作會越細緻。我利用 PCA9685 16路電機控制板，連接 4 個伺服馬達，控制其轉向、上臂彎曲、手腕彎曲及爪子抓物品。過去曾做過使用 PCA9685 16路電機控制板來控制伺服馬達的實作：Arduino筆記(74)：PCA9685控制Servo伺服馬達，這次要將這個實作，再結合 Wii nunchuck 控制轉向及手臂擺動。


實作過程中，控制伺服馬達轉動的角度，程式中的 pwm.setPWM()函式是將伺服馬達轉動到控制的角度，以為控制範圍的參數是 0 ~ 180，上傳程式到 Arduino 執行時，伺服馬達轉動的角度很小。一查 Adafruit_PWM Servo Driver 的函式庫說明，才知道 pwm.setPWM(channel,on,off) 的的 on, off 值可以從 0 到 4095，經過幾次嘗試找到適合的轉動角度，這個角度會因為伺服馬達的廠牌不同而有差異，只能多試幾次，得知大約的轉動位置是否適合你的環境。

得知轉動位置後，可設定為伺服馬達的最大跟最小值，並利用 map()函式進行轉換，將 nunchuck 取得的搖桿及加速度值，map成介於上述伺服馬達最小與最大值之間的值，再透過 pwm.setPWM()控制伺服馬達轉動的角度。

由於 4 個伺服馬達的控制角度不同，對於校準伺服馬達，我一次只接一個伺服馬達進行調校，以伺服馬達A來說，他在機器手臂最下方，控制轉動方向。我設定最小值為 0，最大值為 550，為什麼會是這個值呢？只因為這個值會讓 Arduino 啟動時，手臂的角度剛好與底座成 90度垂直，稍微改一下最大與最小值，都會讓起動的角度偏移。例如第 4 個馬達，設定介於 150 與 600 之間，若將 600 減少的 350左右，爪子剛好可以閉合，可是沒有抓緊的力道，將最大值稍微設定高一點，可以增加爪子緊縮的力道。

第 2 個伺服馬達控制大臂的彎曲角度，我使用 z 軸的傾斜角度來控制，第 3 個伺服馬達控制爪子手腕的彎曲角度，我用搖桿的 y 軸來控制，往下按時，爪子手腕也會往下，至於設定的值是根據實際測試得到，讓手臂不要因彎曲角度過大敲到桌面，或是因最大與最小值的角度範圍太大變得不好控制等，這需要多幾次嘗試，才能知道該設定角度的大小範圍。

我沒有將組裝機器手臂的過程放上來，如有需要瞭解如何安裝的朋友，可以參考 這篇 文章。

[安裝Adafruit-PWM-Servo-Driver-Library]

本實作需要安裝以下程式庫：
程式庫(Library)安裝方法請參考另一篇文章:  Arduino筆記：安裝 Arduino IDE 程式庫(Library)。

[材料]

  • Arduino Uno x1
  • WiiChuck 轉接板 x1
  • Wii 擴充搖桿 x1
  • 麵包板 x1
  • 伺服馬達 MG996R x4
  • 18650電池 x2
  • 18650電池座 x1
  • 可調變壓轉換器 x1
  • U型固定架 x2
  • MG996R固定架 x3
  • 長軸U型固定架 x2
  • MG996R控制爪子組件 x1
  • 排線 x n 條

[接線圖]

Aruino UnoWii NunchuckPCA9685
3V3+N/A
5VN/AVCC
GND-GND
A4(SDA)D(Data)N/A
A5(SCL)C(Clock)N/A
SCLN/ASCL
SDAN/ASDA



[程式]

#include <Wire.h>
#include <Servo.h>
#include <Adafruit_PWMServoDriver.h>
#define TWI_FREQ 400000L

#define SERVO_A_MIN  0   // 伺服馬達A最小脈寬值
#define SERVO_A_MAX  550 // 伺服馬達A最大脈寬值
#define SERVO_B_MIN  150 
#define SERVO_B_MAX  600
#define SERVO_C_MIN  300 
#define SERVO_C_MAX  900
#define SERVO_D_MIN  150 
#define SERVO_D_MAX  600

#define MIN_PULSE_WIDTH       544
#define MAX_PULSE_WIDTH       2400
#define FREQUENCY           50
 
// 呼叫伺服驅動程式函數，預設I2C位址為 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// 定義伺服機使用的輸出Port
int ServoA = 0;
int ServoB = 1;
int ServoC = 2;
int ServoD = 3;

// 從nunchuck回傳的資料是一個 6位元的值
#define BUFFERSIZE 6
uint8_t outbuf[BUFFERSIZE];
int bufPos = 0;

int joy_x_axis = outbuf[0];
int joy_y_axis = outbuf[1];
int accel_x_axis = outbuf[2] * 2 * 2;
int accel_y_axis = outbuf[3] * 2 * 2;
int accel_z_axis = outbuf[4] * 2 * 2;
int z_button = 0;
int c_button = 0;
  
long refreshTime = 1000 / 30;    // 更新時間，以為秒計 millisecs
long lastTime = 0;
  
void setup()  {
  Serial.begin (57600);
  Wire.begin ();

  pwm.begin();
  pwm.setPWMFreq(FREQUENCY);  //最大的 PWM 頻率 
  nunchuck_init ();

  Serial.println("\r\n");
  Serial.print ("Start Wii Nunchuk demo\n");
  Serial.print ("joy_x\tjoy_y\tacc_x\tacc_y\tacc_z\tz_but\tc_but\r\n");
}
  
void loop()  {
  long now = millis(); // get the current time
  
  // 最後一次取得資料到這次取得的時間大於更新時間
  if ( now - lastTime >= refreshTime) {
    lastTime = now;  //儲存現在時間
  
    Wire.requestFrom (0x52, BUFFERSIZE);  // 從 nunchuck 取得資料
  
    // 將收到的資料放到 buffer
    while (Wire.available () && bufPos < BUFFERSIZE) {
      outbuf[bufPos] = nunchuk_decode_byte(Wire.read());
      bufPos++;
    }
  
    if (bufPos == BUFFERSIZE) { // 收到 buffer 滿了
      printNunchuckData(); 
      ServoStart();
    }
      
    // 更新 buffer 位址
    bufPos = 0;
    send_zero();
  }
}
  
void nunchuck_init()  {
  Wire.beginTransmission (0x52);
  Wire.write (0x40);
  Wire.write (0x00);
  Wire.endTransmission ();
}
  
void send_zero()  {
  Wire.beginTransmission (0x52);
  Wire.write (0x00);
  Wire.endTransmission ();
}
  
void printNunchuckData()  {
  
  joy_x_axis = outbuf[0];
  joy_y_axis = outbuf[1];
  accel_x_axis = outbuf[2] * 2 * 2;
  accel_y_axis = outbuf[3] * 2 * 2;
  accel_z_axis = outbuf[4] * 2 * 2;
  z_button = 0;
  c_button = 0;

  // byte outbuf[5] 包含 z 和 c 按鍵的 bits，也包含加速度的值
  if ((outbuf[5] >> 0) & 1) {
    z_button = 1;
  }
  if ((outbuf[5] >> 1) & 1) {
    c_button = 1;
  }
  if ((outbuf[5] >> 2) & 1) {
    accel_x_axis += 2;
  }
  if ((outbuf[5] >> 3) & 1) {
    accel_x_axis += 1;
  }
  if ((outbuf[5] >> 4) & 1) {
    accel_y_axis += 2;
  }
  if ((outbuf[5] >> 5) & 1) {
    accel_y_axis += 1;
  }
  if ((outbuf[5] >> 6) & 1) {
    accel_z_axis += 2;
  }
  if ((outbuf[5] >> 7) & 1) {
    accel_z_axis += 1;
  }
  
  // 顯示搖桿資料，顯示搖桿上方方向鍵的 X, Y座標
  Serial.print(joy_x_axis, DEC);
  Serial.print("\t");
  Serial.print(joy_y_axis, DEC);
  Serial.print("\t");
  
  // 顯示X,Y,Z三軸的加速度值
  Serial.print(accel_x_axis, DEC);
  Serial.print("\t");
  
  Serial.print(accel_y_axis, DEC);
  Serial.print("\t");
  
  Serial.print(accel_z_axis, DEC);
  Serial.print("\t");
  
  // 顯示前方的兩個按鍵狀態
  Serial.print(z_button, DEC);
  Serial.print("\t");
  
  Serial.print(c_button, DEC);
  Serial.print("\t");  
//  Serial.print("\r\n");        
}

void ServoStart()  {
  // 使用搖桿控制伺服馬達，依照實測的搖桿及z水平加速值範圍設定map值，
  // 轉成介於伺服馬達最小與最大值之間
  int valueA = map(joy_x_axis, 25, 220, SERVO_A_MAX, SERVO_A_MIN);
  int valueB = map(accel_z_axis, 25, 990, SERVO_B_MIN, SERVO_B_MAX);
  int valueC = map(joy_y_axis, 25, 220, SERVO_C_MIN, SERVO_C_MAX);
  int valueD = 0;

  //第四個伺服馬達控制爪子，按下閉合
  if (z_button == 1) {
    valueD = SERVO_D_MIN;
  }
  if (z_button == 0) {
    valueD = SERVO_D_MAX;
  }        
  
  // 讓伺服馬達移動到該位置 
  pwm.setPWM(ServoA, 0, valueA); 
  pwm.setPWM(ServoB, 0, valueB); 
  pwm.setPWM(ServoC, 0, valueC); 
  pwm.setPWM(ServoD, 0, valueD); 

  // 在串列埠顯示值
  Serial.print(valueA);  
  Serial.print("\t");
  Serial.print(valueB);  
  Serial.print("\t");
  Serial.print(valueC);    
  Serial.print("\t");
  Serial.print(valueD);      
  Serial.print("\r\n");    
}
  
char nunchuk_decode_byte (char x)   {
  x = (x ^ 0x17) + 0x17;
  return x;
}

[結果]


[參考資料]

Tags:

Post a Comment

張貼留言

較新的 較舊