/************* 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 variables
int g_count = 0; // count the frequecy
int g_array[3]; // store the RGB value
int g_flag = 0; // filter of RGB queue
float g_SF[3]; // save the RGB Scale factor
boolean 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 function
void 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 loop
void 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
}
Comments