2015-06-18

Arduino筆記：移轉『EV3樂高積木顏色分類機』至Arduino平台(NXShield or EVShield) - ( I )

在五月底曾答應 Mindsensors 的 Mr. Deepak要將『樂高Technic Beam積木顏色分類機』由 EV3 + RobotC 移轉至 Arduino + EVShield 平台，而在等候 EVShield寄達之前，先行以 NXShield測試各個組成機構轉換成Arduino的功能，接下來在移轉至EVShield時，就可以直接整合，省掉單元測試的工作。

EV3 to Arduino 轉換計畫


EV3版的『樂高Technic Beam積木顏色分類機』共由五個主要機構所組成，包含：
  • 托盤 及 輸送帶：使用 PFMate+PF IR接收器 控制 PF L-Motor 以及 EV3觸控感應器。
  • 顏色辨識：使用 EV3顏色感應器。
  • Brick feeder：使用 EV3 中型馬達。
  • Brick loader：使用 PFMate + PF IR Receiver 控制 PF Servo Motor。 
  • Robot Arm：使用 NXTServo 控制四顆 RC Servo。
EV3 版 的五個組成機構
在計畫先以 Arduino + NXShield 平台進行單元測試前，需因應 NXShield 與 EV3 感應器的相容問題調整以下的配備：

以下圖示為單元測試之所有機構：

NXShield版測試機構

以下圖示為單元測試之功能架構圖：




NXShield版各部機構功能之單元測試

  1. 托盤 與 輸送帶：
    【控制功能】 這個機構由 PFMate 控制器 + Lego PF IR接收器 控制 PF L-Motor驅動輸送帶，並使用NXT觸控感應器偵測輸送帶上的擋板以控制一次只會輸送一個Technic Beam積木給 EV3的顏色感應器進行顏色辨識。
       

    【Arduino程式】在NXShield 的Arduino程式庫中已經內建 PFMate 與 NXT Touch sensor 的API，以下是控制這個機構功能的 Arduino 程式碼：
    
#include <Wire.h>
#include <NXShield.h>
#include <PFMate.h>
#include <NXTTouch.h>

NXShield   nxshield;
PFMate     NXSH_PFMate(0x48);
NXTTouch   NXSH_Touch;

void setup()
{ 
  char str[256]:
  Serial.begin(9600);
      
  nxshield.init( SH_HardwareI2C );  
  delay(100);  
  NXSH_PFMate.init( &nxshield, SH_BBS1 );
  delay(100);
  NXSH_Touch.init( &nxshield, SH_BAS2 );
  delay(100);
  
  sprintf(str, "DeviceID: %s", NXSH_PFMate.getDeviceID());
  NXSH_PFMate.controlMotor(PF_Channel_1, PF_Contol_Both, PF_Operation_Float, 0);
  NXSH_PFMate.sendSignal();
  delay(100);
  Serial.println(str);
}

void loop()
{ 
  Serial.println("Running the conveyor ..");
  while(!NXSH_Touch.isPressed()) {
    NXSH_PFMate.controlMotor(PF_Channel_1, PF_Contol_A, PF_Operation_Forward, 7);
    NXSH_PFMate.sendSignal();
    delay(500);
  }
  Serial.println("Bumping is detected!!");
  NXSH_PFMate.controlMotor(PF_Channel_1, PF_Contol_A, PF_Operation_Brake, 0);
  NXSH_PFMate.sendSignal();
  delay(2000); 
}
    
  2. 顏色辨識：
    【控制功能】這個機構會使用EV3 Color sensor辨識由輸送帶傳送過來的積木顏色，同時也會檢查當在一個指定的時間內若沒有偵測到任何的積木時，就會判斷已經完成所有積木的顏色分類工作，因此即可以終止整體系統的執行。
    而NXShield 須透過 mindsensors的 『EV3 Sensor Adapter for NXT, NXShield  or Arduino』才能連接使用EV3 Color sensor，實體機構圖如下：
      

    【Arduino程式】在NXShield的程式庫中，已提供有 EV3Color 的 API，而 EV3Color 則繼承自   EV3SensorAdapter  類別，以下是控制這個機構功能的 Arduino 程式碼：
    
#include <Wire.h>
#include <NXShield.h>
#include <EV3Color.h>
NXShield  nxshield;
EV3Color  NXSH_EV3Color;
#include "NXShield_ColorDetection.h"

void setup()
{ 
  char str[256];
  Serial.begin(9600);
      
  nxshield.init( SH_HardwareI2C );  
  delay(100);  
  NXSH_EV3Color.init( &nxshield, SH_BBS2 );
  delay(100);
  NXSH_EV3Color.setMode(MODE_Color_MeasureColor);
  delay(100); 
  
  sprintf(str, "DeviceID: %s, Mode: %x", NXSH_EV3Color.getDeviceID(), NXSH_EV3Color.getMode());
  delay(100);
  Serial.println(str); 
}

void loop()
{
   int dColor = detectColor(3000);   
}
    

     NXShield_ColorDetection.h
    
////////////////////////////////////////////////////////////////////////////////
// EV3 Color Sensor return value
////////////////////////////////////////////////////////////////////////////////
#define colorNone    0
#define colorBlack   1
#define colorBlue    2
#define colorGreen   3
#define colorYellow  4
#define colorRed     5
#define colorWhite   6
#define colorBrown   7

////////////////////////////////////////////////////////////////////////////////
// Detect the brick color
////////////////////////////////////////////////////////////////////////////////
int detectColor(unsigned long idleMillis) {
  int dColor, errCnt = 0;
  unsigned long checkMillis;
  while(!(errCnt > 3)) {
    dColor = colorNone;
    checkMillis = millis();
    while(dColor == colorNone) {
      dColor = NXSH_EV3Color.readValue();
      delay(100);
      if((millis() - checkMillis) > idleMillis) break;
    }
    Serial.println(dColor);
    if(!(dColor == colorNone)) return dColor;
    errCnt++;
  }
}  

其餘三個機構: Brick Feeder, Brick Loader及 Robot Arm的功能單元測試，另整理於： Part II 文章 〉〉連結


張貼者： 於

沒有留言:

張貼留言

訂閱： 張貼留言 (Atom)