Hello everyone - My first post on the forum so please be gentle if I ask some noob or stupid questions.
I'm a programming neophyte, and I mostly get by with leveraging what others have already done and adapting it for my purposes.
I have a built a candy sorting machine that essentially sorts 4 colors + a Reject 5th color by using a color sensor that reads the RGB values. I'm using a Nano board, but open to using another board if it's better suited. The original program was written by someone else and I have augmented it to display some Startup messages and capture the serial outputted that displays values of each candy that has been sorted. I'd like keep a count of each candy that has been sorted and display a running counter of each of the colors.
Can anyone please help me with injecting that piece of code that I can display? Thanks is advance!
My code so far which is working perfectly...
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <Wire.h>
// initialize the library with the numbers of the interface pins
LiquidCrystal_I2C lcd(0x27,16,2);
Servo servo1;
Servo servo2;
/*
- Input pins for the color sensor & servos
*/
const int s0 = 8;
const int s1 = 9;
const int s2 = 12;
const int s3 = 11;
const int out = 10;
const int servo1_pin = 6;
const int servo2_pin = 5;
/*
- Vars
*/
const int tot_colors = 5; // 4 colors + 1 empty "color"
const int rep = 8; // repeat xx color reads
const int d = 5; // add range of XX to the read
volatile unsigned de = 1; // delay servo movement
/*
- Servo positions
- Set the positions in degree where the servo
- has to stop to drop the ball
*/
const int s1pos[] = { 125, 87, 55 ,0 ,180};
const int s2pos[] = { 180, 34, 58, 79, 102}; // first item = unknown color was 180,34,58,80,104
int colors_found = 0;
unsigned long red, green, blue = 0;
typedef struct{
int red;
int green;
int blue;
} colors_type;
colors_type colors[tot_colors]; // length of array: sizeof(colors)/sizeof(colors_type)
void setup() {
Serial.begin(9600);
servo1.attach(servo1_pin);
servo2.attach(servo2_pin);
pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(out, INPUT);
digitalWrite(s0, HIGH);
digitalWrite(s1, HIGH);
// initialize the lcd
lcd.init();
lcd.backlight();
lcd.begin(16, 2);
Serial.begin(9600);
//Boot Message
lcd.print("Saturn Candy Inc");
lcd.setCursor(0, 1);
lcd.print("Plant 2, Mach 12");
delay(3000);
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Smart Connected");
lcd.setCursor(0, 1);
lcd.print("Candy Machine");
delay(2000);
//Initialize Machine
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Initializing");
lcd.setCursor(0,1);
lcd.print("Sorting Machine");
delay(1000);
lcd.clear();
//Begin Calibration
lcd.setCursor(0,0);
lcd.print("Begin Calibration");
lcd.setCursor(0,1);
lcd.print("Now");
delay(2000);
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Green Blue Purple");
lcd.setCursor(0,1);
lcd.print("Pink ORANGE");
delay(5000);
lcd.clear();
initCalibration();
}
void loop(){
load();
pos();
readColor();
checkColor();
}
void initCalibration(){
gopos(0,4);
delay(1000);
gopos(0,5);
delay(1000);
gopos(0,2); // empty the loader for calibration
delay(200); // ADDED DELAY 10/9
eject(); //delay(500);
pos();
delay(500); // Original delay was 700
readColor();
colors[0].red = red;
colors[0].green = green;
colors[0].blue = blue;
//Serial.println("Empty: ");
//Serial.println(String(red) + " "+ String(green) + " " + String(blue));
}
void checkColor(){
for (int i = 0 ; i < 5 ; i++) {
// Serial.println(String(i) + ": "+ String(colors.red) + " " + String(colors_.green) + " "+ String(colors*.blue));*_
* if ( *
inRange(red, colors_.red - d, colors*.red + d) &&
inRange(green, colors.green - d, colors.green + d) &&
inRange(blue, colors.blue - d, colors.blue + d)
)
{
// Color match!
if (i==0) {
delay(100); // Original Delay was 100*
* // eject();
return;
}
//Serial.println(String(red) + " "+ String(green) + " " + String(blue));
Serial.println("Color match " + String(i));
lcd.print("Color match " + String(i));
gopos(i,2);
eject();
return;
}
}*_
if (colors_found < tot_colors){ // Color not found, adding color to the array
* colors_found++;
_ Serial.print("New color found! Total: ");_
Serial.println(colors_found);
_ lcd.setCursor(0,0);
lcd.print("Total Colors: ");_
lcd.print(colors_found);
if (colors_found >= 5){
_ gopos(0,2);
delay(100); //ADDED DELAY 10/9 WAS 200*
* eject();
return;*_
* }*
* gopos(colors_found,2 );
_ eject();_
colors[colors_found].red = red;
colors[colors_found].green = green;
colors[colors_found].blue = blue;
_ return;
}
gopos(0,2);
delay(100); //ADDED DELAY 10/9 Was 200*
* eject();*_
* //initCalibration();*
//delay(100);
}
void readColor(){
red = green = blue = 0;
for (int i = 1; i <= rep ; i++) {
* digitalWrite(s2, LOW);*
* digitalWrite(s3, LOW);*
* red += pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);*
* digitalWrite(s3, HIGH);*
* blue += pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);*
* digitalWrite(s2, HIGH);*
* green += pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);*
* delay(3);*
}
//Serial.println(String(red) + " " + String(green) + " " + String(blue) + " ");
red = red / rep;
blue = blue / rep;
green = green / rep;
int sum = red + green + blue;
red = red * 100 / sum;
blue = blue * 100 / sum;
green = green * 100/ sum;
Serial.println(String(red) + " " + String(green) + " " + String(blue) + " ");
lcd.setCursor(0,1);
lcd.println("Color: " + String(red) + " " + String(green) + " " + String(blue) + " ");
}
void load() {
gopos(0,1);
}
void pos() {
gopos(1,1);
}
void eject() {
gopos(2,1);
delay(200); //ADDED DELAY 10/19 WAS 300
}
bool inRange(int val, int minimum, int maximum) {
return ((minimum <= val) && (val <= maximum));
}
void gopos(int p, int z){
*int r = (z == 1) ? servo1.read() : servo2.read(); *
int go = (z == 1) ? s1pos[p] : s2pos[p];
if (r == go ) return;
if (go > r){
* for (int s = r; s <= go ;s++){*
* if (z == 1) servo1.write(s);*
* if (z == 2) servo2.write(s);*
* delay(de);*
* }*
}
else{
* for (int s = r; s >= go ; s--){*
* if (z == 1) servo1.write(s);*
* if (z == 2) servo2.write(s);*
* delay(de);*
* }*
}
delay(90);
}
color-sensor-2.1_REV4.ino (5.8 KB)
