RobotC 4.X實作：樂高Technic Beam積木顏色分類機(LEGO BRICKS SORTER)

RobotC 4.X版原來對於EV3的I2C功能一直不是很穩定，常出現Bus error的錯誤，不過從4.27版之後就已經能夠正常支援了，所以這次以mindsensors的PFMate與NXTServo(V1.0)兩個I2C數位控制器，再搭配EV3中型馬達以及觸控' 顏色兩種感應器實作一個『樂高Technic Beam的積木顏色分類機(LEGO BRICKS SORTER)』，用來測試RobotC相關的EV3功能，包含：馬達、感應器、I2C通訊以及EV3的多工作業等。

示範影片：

機構總覽：

Bricks Sorter整體機構

整體機構共分成五個部分：

1. 托盤 加 輸送帶：
   透過PFMate控制LEGO PF馬達驅動輸送帶，並使用EV3 Touch sensor控制PFMate一次只運送一個積木給顏色感應器。

   

   EV3 Touch Sensor

2. 顏色辨識：使用EV3 Color sensor

   

   EV3 Color Sensor

3. 積木傳送機構(Brick feeder)：
   以一顆EV3中型馬達驅動包含：抓取以及傳送兩個動作的Under-actuation機構(即：機構的自由度大於驅動的馬達數目)。

   

   Brick Feeder機構(Under-actuation)

4. 積木載入機構(Brick loader)：
   透過PFMate控制LEGO PF伺服馬達將Technic beam垂直抬舉，以方便機械手臂抓取。

   

   Brick Loader機構

5. 機械手臂機構 加 積木分色置放槽：
   透過NXTServo驅動一個四個自由度的機械手臂將積木放入各個顏色的置放槽中。

   

   Robot Arm

功能總攬：

1. 輸送待分類積木：
   以輸送帶由托盤一次運送一個Technic beam給Color Sensor進行辨識，。
2. 顏色辨識：
   當Color Sensor測量到color value非等於None時，就可以判斷已經有一個積木正等待分類，接著就驅動Brick Feeder將積木傳送給Brick Loader。
3. 轉送積木給機械手臂：透過判讀Brick Feeder Under-actuation機構  的EV3中型馬達RPM值，可以判斷積木已經轉送到Brick Loader，接著驅動Brick Loader將積木垂直抬起。
4. 積木分類：以機械手臂從Brick Loader抓取積木放置在不同顏色的分類槽中(紅' 白' 黃)，而如果不是上述三種顏色的積木，機械手臂會移到後方丟棄。

RobotC I2C通訊程式摘要：

• PFMate與NXTServo等I2C數位感應器的Sensor Type需要設定成  sensorEV3_GenericI2C

#pragma config(Sensor, S1,     EV3_COLOR,  sensorEV3_Color, modeEV3Color_Color)
#pragma config(Sensor, S2,     EV3_TOUCH,  sensorEV3_Touch)
#pragma config(Sensor, S3,     PFMATE,     sensorEV3_GenericI2C)
#pragma config(Sensor, S4,     NXTSERVO,   sensorEV3_GenericI2C)
#pragma config(Motor,  motorA, EV3_Mmotor, tmotorEV3_Medium, PIDControl, encoder)               

• 讀取I2C Sensor Value的範例程式碼：
  首先需要先寫入I2C bus 以及所要讀取的register位址，等EV3完成建立通訊之後，再傳送資料buffer以及讀取的資料byte數。 

//////////////////////////////////////////////////////////////////////////////////////////
// Read battery voltage value of NXTServo.
//////////////////////////////////////////////////////////////////////////////////////////
float getNXTServoBatteryVoltage()
{
 TI2CStatus nStatus;
 ubyte I2C_Request[3], I2C_Reply[1];
 short errCnt=0;
 while(nI2CStatus[NXTSERVO] == i2cStatusPending) {
  errCnt++;
  if(errCnt>500) {
   displayBigTextLine(7, "NXTServo_TimeOut(M)");
   delay(5000);
   return false;
  }
  delay(10);
 }

 memset(I2C_Request, 3, 0);
 I2C_Request[0] = 2;
 I2C_Request[1] = NXTSERVO_ADDR;
 I2C_Request[2] = 0x41;
 sendI2CMsg(NXTSERVO, &I2C_Request[0], 1);
 delay(100); // Do not change this value, or servos will unstable.

 while(nI2CStatus[NXTSERVO] == i2cStatusPending) wait1Msec(1);
 memset(I2C_Reply, 1, 0);
 nStatus = readI2CReply(NXTSERVO, &I2C_Reply[0], 1);
 return ((4700.0/127.0)*I2C_Reply[0]);
}           

寫入I2C 裝置的範例程式碼：

//////////////////////////////////////////////////////////////////////////////////////////
// Control multiple servors simultaneously.
//////////////////////////////////////////////////////////////////////////////////////////
bool setMultiServo(const int delayMSec)
{
 TI2CStatus nStatus;
 ubyte I2C_Request[19];
 short errCnt=0;
 while(nI2CStatus[NXTSERVO] == i2cStatusPending) {
  errCnt++;
  if(errCnt>500) {
   displayBigTextLine(7, "NXTServo_TimeOut(M)");
   delay(5000);
   return false;
  }
  delay(10);
 } 
 // Set servo speed
 memset(I2C_Request, 19, 0);
 I2C_Request[0] = 2+NO_OF_SERVOS;
 I2C_Request[1] = NXTSERVO_ADDR;
 I2C_Request[2] = servoSpeedReg[0];
 for(int xi=0; xi＜NO_OF_SERVOS; xi++) {
  I2C_Request[xi+3] = servoArray[xi].servoSpeed;
 }
 sendI2CMsg(NXTSERVO, &I2C_Request[0], 0);
 delay(200); // Do not change this value, or servos will unstable.

 // Set servo position
 memset(I2C_Request, 0, 19);
 I2C_Request[0] = 2+NO_OF_SERVOS*2;
 I2C_Request[1] = NXTSERVO_ADDR;
 I2C_Request[2] = servoPosReg[0];
 for(int xi=0; xi＜NO_OF_SERVOS; xi++) {
  I2C_Request[2*xi+3] =  (servoArray[xi].servoPos & 0x00FF);
  I2C_Request[2*xi+4] = ((servoArray[xi].servoPos ＞＞ 8) & 0x00FF);
 }
 nStatus = sendI2CMsg(NXTSERVO, &I2C_Request[0], 0);

 delay(delayMSec);
 return (nStatus == i2cStatusNoError);
}