ArduinoとRGBセンサーで作る高度な色選別マシン

ソースコード

LFK_sorting_machine (C/C++)

Arduino IDE用の完全なスケッチです。TCS34725ライブラリとServoライブラリが必要です。

#include <Wire.h>
#include "Adafruit_TCS34725.h"
#include <Servo.h>

// RGBセンサー設定
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_1X);
int colour = 0; // 0=黒/なし, 1=黄, 2=緑, 3=橙, 4=赤, 5=紫
int previous_colour = colour;
int black_count = 0;
int start_measurment_angle = 88;
int measurment_sweep = 2;
int measurment_angle_increment = 4;

// 色判定閾値 [Rmin, Rmax, Gmin, Gmax, Bmin, Bmax]
uint16_t black_limit_values[]   = {0, 200, 0, 200, 0, 200};
uint16_t yellow_limit_values[]  = {850, 2200, 800, 2200, 400, 1100};
uint16_t green_limit_values[]   = {400, 800, 700, 1400, 300, 700};
uint16_t orange_limit_values[]  = {650, 1800, 300, 850, 210, 600};
uint16_t red_limit_values[]     = {400, 950, 150, 450, 150, 400};
uint16_t purple_limit_values[]  = {150, 400, 150, 450, 150, 500};

// RGB LEDピン (共通アノードなのでLOWで点灯)
byte R_pin = 5, G_pin = 6, B_pin = 11;
int current_red_value = 255, current_green_value = 255, current_blue_value = 255;
const int orange[] = {0, 230, 255};
const int red[]    = {0, 255, 255};
const int green[]  = {255, 0, 255};
const int yellow[] = {0, 170, 255};
const int purple[] = {0, 255, 40};

// サーボ設定
int release_delay = 220;
const int colour_angles[] = {10, 46, 82, 118, 154};
const int backward_anti_backlash = 2;
const int forward_anti_backlash = 2;
Servo feeder_servo;
Servo holder_servo;

void setup() {
  Serial.begin(9600);
  pinMode(R_pin, OUTPUT); pinMode(G_pin, OUTPUT); pinMode(B_pin, OUTPUT);
  analogWrite(R_pin, 255); analogWrite(G_pin, 255); analogWrite(B_pin, 255);
  feeder_servo.attach(3);
  holder_servo.attach(2);
  feeder_servo.write(0);
  holder_servo.write(colour_angles[0]);
  
  if (tcs.begin()) {
    Serial.println("Found sensor");
    starting_sequence();
  } else {
    Serial.println("No TCS34725 found ... check your connections");
    while (1) { set_led(300, red); set_led(300, black); } // 赤高速点滅
  }
}

void loop() {
  if (black_count < 10) {
    feeder_servo.write(0);
    delay(450);
    previous_colour = colour;
    get_colour();
    if (colour == 0) { black_count++; shake(); }
    else if (colour == 6) { black_count++; }
    else { move_holder(); release_skittle(); black_count = 0; }
  } else { end_loop(1000); }
}

void get_colour() {
  uint16_t r, g, b, c;
  uint16_t total_r = 0, total_g = 0, total_b = 0;
  feeder_servo.write(start_measurment_angle);
  delay(200);
  for (int i = 0; i <= measurment_sweep; i++) {
    tcs.getRawData(&r, &g, &b, &c);
    feeder_servo.write(start_measurment_angle + i * measurment_angle_increment);
    total_r += r; total_g += g; total_b += b;
    delay(15);
  }
  total_r /= measurment_sweep;
  total_g /= measurment_sweep;
  total_b /= measurment_sweep;
  Serial.print(total_r); Serial.print(" "); Serial.print(total_g); Serial.print(" "); Serial.println(total_b);

  // 色判定ロジック（閾値との比較）
  if (total_r < black_limit_values[1] && total_g < black_limit_values[3] && total_b < black_limit_values[5]) { colour = 0; Serial.println("black"); }
  else if (total_r >= yellow_limit_values[0] && total_r < yellow_limit_values[1] && total_g >= yellow_limit_values[2] && total_g < yellow_limit_values[3] && total_b >= yellow_limit_values[4] && total_b < yellow_limit_values[5]) { colour = 1; Serial.println("yellow"); }
  else if (total_r >= green_limit_values[0] && total_r < green_limit_values[1] && total_g >= green_limit_values[2] && total_g < green_limit_values[3] && total_b >= green_limit_values[4] && total_b < green_limit_values[5]) { colour = 2; Serial.println("green"); }
  else if (total_r >= orange_limit_values[0] && total_r < orange_limit_values[1] && total_g >= orange_limit_values[2] && total_g < orange_limit_values[3] && total_b >= orange_limit_values[4] && total_b < orange_limit_values[5]) { colour = 3; Serial.println("orange"); }
  else if (total_r >= red_limit_values[0] && total_r < red_limit_values[1] && total_g >= red_limit_values[2] && total_g < red_limit_values[3] && total_b >= red_limit_values[4] && total_b < red_limit_values[5]) { colour = 4; Serial.println("red"); }
  else if (total_r >= purple_limit_values[0] && total_r < purple_limit_values[1] && total_g >= purple_limit_values[2] && total_g < purple_limit_values[3] && total_b >= purple_limit_values[4] && total_b < purple_limit_values[5]) { colour = 5; Serial.println("purple"); }
  else { colour = 6; Serial.println("unknown"); }
}

void move_holder() {
  int new_holder_position = colour_angles[colour - 1];
  int holder_delay = abs(previous_colour - colour) * 70;
  if (holder_delay < 0) holder_delay = 0;
  if (previous_colour > colour) {
    holder_servo.write(new_holder_position - backward_anti_backlash);
  } else {
    holder_servo.write(new_holder_position + forward_anti_backlash);
    holder_servo.write(new_holder_position);
  }
  // LED色変更と同期
  if (colour == 1) set_led(holder_delay, yellow);
  else if (colour == 2) set_led(holder_delay, green);
  else if (colour == 3) set_led(holder_delay, orange);
  else if (colour == 4) set_led(holder_delay, red);
  else if (colour == 5) set_led(holder_delay, purple);
}

void release_skittle() { feeder_servo.write(180); delay(release_delay); }

void shake() {
  feeder_servo.write(180); delay(release_delay);
  feeder_servo.write(120); delay(80);
  for (int i = 0; i <= 5; i++) {
    feeder_servo.write(90); delay(80);
    feeder_servo.write(180); delay(80);
  }
}

void starting_sequence() {
  for (int c = 1; c <= 5; c++) { colour = c; move_holder(); set_led(300, (c==1?yellow:c==2?green:c==3?orange:c==4?red:purple)); delay(400); }
  previous_colour = 5; colour = 3; move_holder(); end_loop(200);
}

void end_loop(int duration) {
  set_led(duration, orange); set_led(duration, red); set_led(duration, green); set_led(duration, yellow); set_led(duration, purple);
}

// delay()の代わりにLEDを滑らかに変化させながら待機
void set_led(int duration, int color[3]) {
  int start = millis();
  while (millis() - start < duration) {
    int elapsed = millis() - start;
    analogWrite(R_pin, map(elapsed, 0, duration, current_red_value, color[0]));
    analogWrite(G_pin, map(elapsed, 0, duration, current_green_value, color[1]));
    analogWrite(B_pin, map(elapsed, 0, duration, current_blue_value, color[2]));
  }
  current_red_value = color[0]; current_green_value = color[1]; current_blue_value = color[2];
}

回路図

配線はシンプルです。バックコンバーターの出力が5Vになっていることを確認してから接続してください！