Arduino M&M and Skittles Color Sorter (2024)

/************* M&M Sorter *************************** Christopher Bartolome MAKE Course Fall 2015 ************* ***************************/  //Include necessary libraries#include <TimerOne.h>#include <StepperAK.h>#define S0 7#define S1 5#define S2 4#define S3 3#define OUT 2#define SERVO_PIN 6#define gearratio 64 //Define global variablesint g_count = 0; // count the frequecyint g_array[3]; // store the RGB valueint g_flag = 0; // filter of RGB queuefloat g_SF[3]; // save the RGB Scale factorboolean volatile servoHigh = false;const int stepsPerRevolution = 2048; uint16_t volatile servoTime = 0;uint16_t volatile servoHighTime = 3000; // instantiate a 4-wire stepper on pins 19 through 16:Stepper myStepper(stepsPerRevolution, 19,18,17,16); //Setup for functionvoid setup(){ //Setup for servo servoInit(); servoSetPosition(1600); delay(1000);  //Setup for Color Sensor TSC_Init(); Serial.begin(115200); Timer1.initialize(); // defaulte is 1s Timer1.attachInterrupt(TSC_Callback);  attachInterrupt(0, TSC_Count, RISING);  delay(4000); for(int i=0; i<3; i++)  { Serial.println(g_array[i]); g_SF[0] = 255.0/ g_array[0]; //R Scale factor g_SF[1] = 255.0/ g_array[1] ; //G Scale factor g_SF[2] = 255.0/ g_array[2] ; //B Scale factor } Serial.println(g_SF[0]); Serial.println(g_SF[1]); Serial.println(g_SF[2]);  //Setup for stepper motor myStepper.setSpeed(0.15*gearratio);  // initialize the serial port: Serial.begin(115200); }//Main loopvoid loop(){ //Prints RGB value for scaling factor g_flag = 0; int R = int(g_array[0] * g_SF[0]); int G = int(g_array[1] * g_SF[1]); int B = int(g_array[2] * g_SF[2]);   //Evaluates Conditionals, and based on these conditionals  if((R>225) && (R<400) && (G>210) && (G<275) && (B>210) && (B<275)) // White { Serial.println("Color is white"); servoSetPosition(1500); delay(500); myStepper.step(stepsPerRevolution/6); } else if((R>=190) && (R<=210) && (G>B) && (G>R) && (B>=185) && (B<=205)) // Green { Serial.println("Color is green"); servoSetPosition(700); delay(500); myStepper.step(stepsPerRevolution/6);   } else if((R>=185) && (R<=210) && (G>=185) && (G<=208) && (B>=185) && (B<=208)) // Brown { Serial.println("Color is brown");  servoSetPosition(1450); delay(500); myStepper.step(stepsPerRevolution/6);  } else if((R>=195) && (R<=212) && (G>=190) && (G<=210) && (B>G) && (B>=194) && (B<=210)) // Blue { Serial.println("Color is blue"); servoSetPosition(1700); delay(500); myStepper.step(stepsPerRevolution/6); } else if((R>=205) && (R<=225) && (G>=190) && (G<=203) && (B>=185) && (B<=205)) // Orange { Serial.println("Color is Orange");  servoSetPosition(2050); delay(500); myStepper.step(stepsPerRevolution/6); } else if((R>=210) && (R<=230) && (G>=205) && (G<=220) && (B<R) && (B<G)) // Yellow {  Serial.println("Color is yellow");  servoSetPosition(1250); delay(500); myStepper.step(stepsPerRevolution/6); } else if((R>B) && (R>G) && (G>=185) && (G<=206) && (B>=185) && (B<=200)) // Red { Serial.println("Color is red");  servoSetPosition(1000);  delay(500); myStepper.step(stepsPerRevolution/6); } else { Serial.print("Red = "); Serial.println(R); Serial.print("Green = "); Serial.println(G); Serial.print("Blue = "); Serial.println(B); }; delay(4000); }/************Functions for color sensor*******/ // Init TSC230 and setting Frequency. void TSC_Init() { pinMode(S0, OUTPUT); pinMode(S1, OUTPUT); pinMode(S2, OUTPUT); pinMode(S3, OUTPUT); pinMode(OUT, INPUT); digitalWrite(S0, LOW); // OUTPUT FREQUENCY SCALING 2% digitalWrite(S1, HIGH);  } // Select the filter color  void TSC_FilterColor(int Level01, int Level02) { if(Level01 != 0) Level01 = HIGH; if(Level02 != 0) Level02 = HIGH; digitalWrite(S2, Level01);  digitalWrite(S3, Level02);  } void TSC_Count() { g_count ++ ; } void TSC_Callback() { switch(g_flag) { case 0:  TSC_WB(LOW, LOW); //Filter without Red break; case 1: g_array[0] = g_count; TSC_WB(HIGH, HIGH); //Filter without Green break; case 2: g_array[1] = g_count; TSC_WB(LOW, HIGH); //Filter without Blue break; case 3: g_array[2] = g_count; TSC_WB(HIGH, LOW); //Clear(no filter)  break; default: g_count = 0; break; } } void TSC_WB(int Level0, int Level1) //White Balance { g_count = 0; g_flag ++; TSC_FilterColor(Level0, Level1); Timer1.setPeriod(1000000); // set 1s period } /********Functions for Servo************/ ISR(TIMER2_COMPA_vect) { // The time that passed since the last interrupt is OCR2A + 1 // because the timer value will equal OCR2A before going to 0. servoTime += OCR2A + 1;  static uint16_t highTimeCopy = 3000; static uint8_t interruptCount = 0;  if(servoHigh) { if(++interruptCount == 2) { OCR2A = 255; }  if(servoTime >= highTimeCopy) { // The pin has been high enough, so do a falling edge. digitalWrite(SERVO_PIN, LOW); servoHigh = false; interruptCount = 0; } }  else { // The servo pin is currently low. if(servoTime >= 40000) { // We've hit the end of the period (20 ms), // so do a rising edge. highTimeCopy = servoHighTime; digitalWrite(SERVO_PIN, HIGH); servoHigh = true; servoTime = 0; interruptCount = 0; OCR2A = ((highTimeCopy % 256) + 256)/2 - 1; } } }  void servoInit() { digitalWrite(SERVO_PIN, LOW); pinMode(SERVO_PIN, OUTPUT); // Turn on CTC mode. Timer 2 will count up to OCR2A, then // reset to 0 and cause an interrupt. TCCR2A = (1 << WGM21); // Set a 1:8 prescaler. This gives us 0.5us resolution. TCCR2B = (1 << CS21); // Put the timer in a good default state. TCNT2 = 0; OCR2A = 255;  TIMSK2 |= (1 << OCIE2A); // Enable timer compare interrupt. sei(); // Enable interrupts. }  void servoSetPosition(uint16_t highTimeMicroseconds) { TIMSK2 &= ~(1 << OCIE2A); // disable timer compare interrupt servoHighTime = highTimeMicroseconds * 2; TIMSK2 |= (1 << OCIE2A); // enable timer compare interrupt }