I have made a colour sorting machine and when I put gems to know colour, It is giving blue as green and vice versa and also not detecting colour exactly please help me. I am providing my code below.
#include <Servo.h>
// ---- TCS3200 Pins ----
#define S0 2
#define S1 3
#define S2 4
#define S3 5
#define sensorOut 6
// ---- Servos ----
Servo topServo; // gate / feeder
Servo bottomServo; // sorter
int frequency = 0;
int color = 0;
// ---- Top servo angles ----
int TOP_CLOSED_ANGLE = 165;
int TOP_OPEN_ANGLE = 60;
int TOP_MID_ANGLE = 110; // mid pause angle, tune by trial
// ---- Bottom servo angles ----
int BOTTOM_CENTER_ANGLE = 90;
int BOTTOM_RED_ANGLE = 60;
int BOTTOM_GREEN_ANGLE = 100;
int BOTTOM_BLUE_ANGLE = 140;
// ----------------- Prototype centroids (from your samples) -----------------
const float BLUE_C[3] = {153.4, 117.6, 78.0};
const float GREEN_C[3] = {197.0, 115.0, 77.0};
const float RED_C[3] = {339.5, 197.0, 127.5};
const float WR = 1.4;
const float WG = 1.0;
const float WB = 1.0;
const float CONFIDENCE_FACTOR = 0.90;
void setup() {
// TCS3200 pins
pinMode(S0, OUTPUT);
pinMode(S1, OUTPUT);
pinMode(S2, OUTPUT);
pinMode(S3, OUTPUT);
pinMode(sensorOut, INPUT);
// Frequency scaling 20%
digitalWrite(S0, HIGH);
digitalWrite(S1, LOW);
topServo.attach(7); // top motor (dropper) on D7
bottomServo.attach(8); // bottom motor (sorter) on D8
topServo.write(TOP_CLOSED_ANGLE); // start closed
bottomServo.write(BOTTOM_CENTER_ANGLE); // sorter in center
Serial.begin(9600);
Serial.println("3-color sorter ready.");
}
void loop() {
// ---------- STEP 1: Drop exactly one gem with MID PAUSE ----------
Serial.println("Dropping one gem...");
// Ensure start at closed angle
topServo.write(TOP_CLOSED_ANGLE);
delay(200);
// Move from CLOSED -> MID slowly
for (int angle = TOP_CLOSED_ANGLE; angle >= TOP_MID_ANGLE; angle--) {
topServo.write(angle);
delay(20); // slower movement
}
// *** PAUSE at MID angle so gem stays over sensor ***
delay(4000); // increase/decrease for more/less pause
// Move from MID -> OPEN
for (int angle = TOP_MID_ANGLE; angle >= TOP_OPEN_ANGLE; angle--) {
topServo.write(angle);
delay(15); // slightly faster if you want
}
// Small pause while gem is just passing sensor
delay(200); // you can tune this too
// ---------- STEP 2: Read color ----------
Serial.println("Reading color...");
color = readColor();
delay(50);
// ---------- STEP 3: Close gate back (OPEN -> CLOSED via MID) ----------
// Move OPEN -> MID
for (int angle = TOP_OPEN_ANGLE; angle <= TOP_MID_ANGLE; angle++) {
topServo.write(angle);
delay(15);
}
// Optional small pause at MID while closing
delay(100);
// Move MID -> CLOSED
for (int angle = TOP_MID_ANGLE; angle <= TOP_CLOSED_ANGLE; angle++) {
topServo.write(angle);
delay(20);
}
delay(300); // let gem finish falling completely
// ---------- STEP 4: Move bottom servo based on color ----------
Serial.print("Detected color = ");
Serial.println(color);
switch (color) {
case 1: // RED
bottomServo.write(BOTTOM_RED_ANGLE);
break;
case 2: // GREEN
bottomServo.write(BOTTOM_GREEN_ANGLE);
break;
case 3: // BLUE
bottomServo.write(BOTTOM_BLUE_ANGLE);
break;
default: // Unknown / unclear -> center or "reject" bin
bottomServo.write(BOTTOM_CENTER_ANGLE);
break;
}
delay(800); // time for gem to drop into box
// Return bottom servo to center
bottomServo.write(BOTTOM_CENTER_ANGLE);
delay(500);
// ---------- STEP 5: Wait for next gem ----------
Serial.println("Cycle complete. Place next gem.");
delay(2000); // you place the next gem during this time
}
// ----------------- readColor() -----------------
int readColor() {
int R, G, B;
// ----- Read RED -----
digitalWrite(S2, LOW);
digitalWrite(S3, LOW);
frequency = pulseIn(sensorOut, LOW, 30000); // timeout 30ms
if (frequency == 0) frequency = 30000;
R = frequency;
delay(30);
// ----- Read GREEN -----
digitalWrite(S2, HIGH);
digitalWrite(S3, HIGH);
frequency = pulseIn(sensorOut, LOW, 30000);
if (frequency == 0) frequency = 30000;
G = frequency;
delay(30);
// ----- Read BLUE -----
digitalWrite(S2, LOW);
digitalWrite(S3, HIGH);
frequency = pulseIn(sensorOut, LOW, 30000);
if (frequency == 0) frequency = 30000;
B = frequency;
delay(30);
Serial.print("Raw R=");
Serial.print(R);
Serial.print(" G=");
Serial.print(G);
Serial.print(" B=");
Serial.println(B);
int cls = classifyColor(R, G, B);
return cls;
}
// ----------------- classifyColor() -----------------
// returns: 1=RED, 2=GREEN, 3=BLUE, 0=UNKNOWN
int classifyColor(int R, int G, int B) {
// Quick explicit RED rules for very strong red reflections:
if ( (R > 350 && R > 1.5 * (float)G && R > 2.0 * (float)B) ||
(R > 240 && R > 1.25 * (float)G && R > 1.5 * (float)B) ) {
Serial.println("Rule matched: STRONG RED");
return 1;
}
// compute weighted squared distances to each prototype centroid
float dr = (R - RED_C[0]);
float dg = (G - RED_C[1]);
float db = (B - RED_C[2]);
float distR = WR * dr * dr + WG * dg * dg + WB * db * db;
dr = (R - GREEN_C[0]);
dg = (G - GREEN_C[1]);
db = (B - GREEN_C[2]);
float distG = WR * dr * dr + WG * dg * dg + WB * db * db;
dr = (R - BLUE_C[0]);
dg = (G - BLUE_C[1]);
db = (B - BLUE_C[2]);
float distB = WR * dr * dr + WG * dg * dg + WB * db * db;
Serial.print("Distances -> RED:");
Serial.print(distR);
Serial.print(" GREEN:");
Serial.print(distG);
Serial.print(" BLUE:");
Serial.println(distB);
// find min and second min
float best = distR;
int bestIdx = 1;
float second;
int secondIdx;
if (distG < best) { best = distG; bestIdx = 2; second = distR; secondIdx = 1; }
else { second = distG; secondIdx = 2; }
if (distB < best) {
second = best; secondIdx = bestIdx;
best = distB; bestIdx = 3;
} else if (distB < second) {
second = distB; secondIdx = 3;
}
Serial.print("Best match = ");
Serial.print(bestIdx);
Serial.print(" (dist=");
Serial.print(best);
Serial.print(") second=");
Serial.print(secondIdx);
Serial.print(" (dist=");
Serial.print(second);
Serial.println(")");
// BLUE / GREEN handling + override
float sum = (float)R + (float)G + (float)B;
float rRatio = 0, gRatio = 0, bRatio = 0;
if (sum > 0) {
rRatio = (float)R / sum;
gRatio = (float)G / sum;
bRatio = (float)B / sum;
}
// If BLUE centroid wins and B is dominant -> keep it BLUE
if (bestIdx == 3 && bRatio > gRatio && bRatio > rRatio && bRatio > 0.36) {
Serial.println("Strong BLUE ratio -> keep BLUE (no override)");
if (best <= second * CONFIDENCE_FACTOR) {
Serial.println("Centroid decision = BLUE");
return 3;
}
}
// STRICT OVERRIDE: only very borderline BLUE→GREEN correction
int diffGB = abs(G - B);
float rel = (second > 0) ? best / second : 1.0;
if (bestIdx == 3 && // BLUE won
diffGB <= 5 && // G and B almost same
G > R && // G above R
rel > 0.85) { // distances close (not clear BLUE)
Serial.println("Override rule (strict): BLUE≈GREEN and G>R and close dists -> treat as GREEN");
return 2;
}
// Normal centroid decision with confidence
if (best <= second * CONFIDENCE_FACTOR) {
if (bestIdx == 1) {
Serial.println("Centroid decision = RED");
return 1;
} else if (bestIdx == 2) {
Serial.println("Centroid decision = GREEN");
return 2;
} else {
Serial.println("Centroid decision = BLUE");
return 3;
}
}
// Fallback: ratio-based tie-breaker
if (sum <= 0.0) {
Serial.println("Fallback: sum==0 -> UNKNOWN");
return 0;
}
Serial.print("Fallback ratios -> r=");
Serial.print(rRatio, 3);
Serial.print(" g=");
Serial.print(gRatio, 3);
Serial.print(" b=");
Serial.println(bRatio, 3);
if (rRatio > gRatio && rRatio > bRatio && rRatio > 0.38) {
Serial.println("Fallback ratio => RED");
return 1;
}
if (gRatio > rRatio && gRatio > bRatio && gRatio > 0.28) {
Serial.println("Fallback ratio => GREEN");
return 2;
}
if (bRatio > rRatio && bRatio > gRatio && bRatio > 0.22) {
Serial.println("Fallback ratio => BLUE");
return 3;
}
Serial.println("Final decision = UNKNOWN");
return 0;
}
