CH LEGO BLOG: Arduino筆記：移植『EV3樂高積木顏色分類機』至Arduino平台(NXShield or EVShield)

※ 本文接續自：
『EV3樂高積木顏色分類機』移植Arduino平台計畫 Part I

◎ 本文 Brick Feeder、Brick Loader及 Robot Arm等三個功能單元測試影片

Brick Feeder：

【控制功能】
Brick Feeder的功能是將已經完成顏色辨識的積木透過以下動作傳送給Brick Loader機構：
(1) 驅動 EV3 M-Motor(Reverse)抓取積木，當抓緊積木之後，經由Under-Actuation 機制會轉動爪取手臂將積木開始往Brick Loader傳送，同時透過另一組齒輪系抬起托盤。

(2) 當積木已經傳送到托盤所抬起的目標位置時，會因齒輪的作用造成EV3 馬達停滯無法繼續運轉，而這個狀態需要透過程式判斷，在RobotC可以使用偵測馬達的 RPM值達成，至於 NXShield的 API則需以讀取EV3馬達 Encoder值的變化來判斷。

(3) 再以反向驅動 EV3 M-Motor(Forward)釋放積木至托盤，同樣經由Under-Actuation 機制，除了會將爪取手臂轉回EV3顏色感應器所在的位置之外，同時也會放下托盤將積木滑至Brick Loader。

【Arduino程式】

對於Brick Feeder，程式最重要的功能就是判斷 EV3 馬達的停滯狀態(stalled)，原廠的說明文件中曾提到，每一個Motor port會有一個Status Byte，其中 bit7可以檢測 "Motor is stalled"狀態，實際上測試似乎沒有作用，因此，改成以讀取馬達的 Encoder value作為判斷，以下為Arduino程式碼：
```
////////////////////////////////////////////////////////////////////////////////
// Control the Brick Feeder(EV3 M-Motor)
////////////////////////////////////////////////////////////////////////////////
void controlBrickFeeder(bool isFeeding) {
  int32_t curr_encoder, prev_encoder;
  
  nxshield.bank_b.motorStop(SH_Motor_2, SH_Next_Action_Brake);
  delay(300);
  nxshield.bank_b.motorResetEncoder(SH_Motor_2);
  curr_encoder = nxshield.bank_b.motorGetEncoderPosition(SH_Motor_2);
  prev_encoder = curr_encoder;
  if (isFeeding)
    nxshield.bank_b.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, 30);
  else
    nxshield.bank_b.motorRunUnlimited(SH_Motor_2, SH_Direction_Forward, 50);
  delay(1000);
  while(true) {
    curr_encoder = nxshield.bank_b.motorGetEncoderPosition(SH_Motor_2);
    if(abs(curr_encoder - prev_encoder) < 10) break;
    prev_encoder = curr_encoder;
    delay(500);
  }
  nxshield.bank_b.motorStop(SH_Motor_2, SH_Next_Action_Float);
  delay(500);  
}
    
```
Brick Loader：

【控制功能】
Brick Loader的功能在於控制如上圖中裝卸桶的位置，先接收來自 Brick Feeder 所傳送的積木，然後再舉起讓積木呈垂直讓Robot Arm的夾手能夠抓取。
裝卸桶位置是使用 PFMate + PF IR Receiver 驅動 PF Servo Motor 控制，PF Servo Motor 的特性如同 RC Servo，轉動範圍為 180 度，共可設定 15個位置，非常適合需要精確位置控制的機構。

【Arduino程式】
在NXShield 程式庫中，同樣是使用 PFMate 的API，以下的程式碼是將裝卸桶由水平位置分三階段成為垂直狀態的控制：
```
{    
  NXSH_PFMate.controlMotor(PF_Channel_1, PF_Contol_B, PF_Operation_Forward, 6);
  NXSH_PFMate.sendSignal();
  delay(3000);
  
  NXSH_PFMate.controlMotor(PF_Channel_1, PF_Contol_B, PF_Operation_Forward, 4);
  NXSH_PFMate.sendSignal();
  delay(1000);
  
  NXSH_PFMate.controlMotor(PF_Channel_1, PF_Contol_B, PF_Operation_Forward, 2);
  NXSH_PFMate.sendSignal();
  delay(1000);
  
  NXSH_PFMate.controlMotor(PF_Channel_1, PF_Contol_B, PF_Operation_Brake, 0);
  NXSH_PFMate.sendSignal();
  delay(3000); 
}
    
```
Robot Arm：

【控制功能】
Robot Arm的功能即是由 Brick Loader的裝卸桶中抓取積木，並依據EV3 Color sensor所辨識的顏色分別放置至紅、白、黃三個槽中，完成積木的顏色分類工作，至於非上述的三種顏色積木，則會往後丟棄。
上圖中的機械手臂共由四顆 RC Servo所組成，NXShield 已經將Arduino UNO 中對應
＜ Servo.h＞的 6 個 PWM 腳位做好 6 組 3-pin的接腳，直接接上 RC Servo的信號線就可以使用。
【Arduino程式】
在NXShield連接 RC Servo直接使用 Arduino 標準的＜ Servo.h＞程式庫，不需要 include NXShield程式庫。