SparkFun Forums 

Where electronics enthusiasts find answers.

Have questions about a SparkFun product or board? This is the place to be.
By sbrow102
#197207
I'm using a Skittles sorting machine that operates using an Arduino Uno microcontroller. When I plug it in it gives it power but the machine does not distinguish colors or move the slot to the intended areas. When I run the Arduino program it gets stuck on COM1. I'm using these codes:
Color Sensor: //
// COLOR LIGHT SENSOR DECLARE aka. colorsensor.h
//
// Pin layout:
// Sensor -> Arduino controller
// - LED -> LEDPIN
// - 3V3 -> 3V3
// - GND -> GND
// - SCL -> SCL (hardwired)
// - SDA -> SDA (hardwired)
//
#ifndef COLORSENSOR_H
#define COLORSENSOR_H

// I2C address for the device. Note: only 7-bit address, excl. LSB for read/write
#define I2C_ADDR 0x74

// I2C PINS
// Classic Arduinos
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__)|| defined(__AVR_ATmega168__)
#define SDA_PIN 27 // Arduino silkscreen pin A4
#define SCL_PIN 28 // Arduino silkscreen pin A5
// Megas
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define SDA_PIN 44 // Arduino silkscreen pin 20
#define SCL_PIN 43 // Arduino silkscreen pin 21
// Leonardo
#elif defined(__AVR_ATmega32U4__)
#define SDA_PIN 19 // Arduino silkscreen pin 2 (PWM)
#define SCL_PIN 18 // Arduino silkscreen pin 3 (PWM)
#endif

// LED pin
#ifndef LEDLIGHT_PIN
#define LEDLIGHT_PIN 11
#endif

// Register addresses
#define REG_CAP_RED 0x06
#define REG_CAP_GREEN 0x07
#define REG_CAP_BLUE 0x08
#define REG_CAP_CLEAR 0x09

#define REG_INT_RED_LO 0x0A
#define REG_INT_RED_HI 0x0B
#define REG_INT_GREEN_LO 0x0C
#define REG_INT_GREEN_HI 0x0D
#define REG_INT_BLUE_LO 0x0E
#define REG_INT_BLUE_HI 0x0F
#define REG_INT_CLEAR_LO 0x10
#define REG_INT_CLEAR_HI 0x11

#define REG_DATA_RED_LO 0x40
#define REG_DATA_RED_HI 0x41
#define REG_DATA_GREEN_LO 0x42
#define REG_DATA_GREEN_HI 0x43
#define REG_DATA_BLUE_LO 0x44
#define REG_DATA_BLUE_HI 0x45
#define REG_DATA_CLEAR_LO 0x46
#define REG_DATA_CLEAR_HI 0x47

typedef enum {red, green, blue, orange, yellow, purple, unknown, white} ItemColor;

void colorCalibrate (void);
void getColors (int[]);
ItemColor determineColor(double detectedhsv[]);
void rgbToHSV (double, double, double, double[]);

#endif

Functions:
//
// Sorting machine - functions
//

#ifndef SORTINGFUNC_H
#define SORTINGFUNC_H

#include "timer.h"
#include "colorsensor.h"

// data types
typedef enum {none, startwheel, stopwheel, slowwheel, reversewheel} FeedWheelState;
typedef enum {waiting, steady, blinking} StatusState;
char* colortext[] = {"red", "green", "blue", "orange", "yellow", "purple"};

// global system variables
// feed wheel process -> carousel process
ItemColor nextcolor = unknown;
ItemColor prevcolor = unknown;
int colorduplicate = 0;
// carousel process -> feed wheel process
int carouselmoving = 0;
int carouseldelivering = 0;
// feed wheel monitor -> feed wheel process
int outofposition = 0;
int inposition = 0;
// feed wheel -> status process
StatusState statusindicator = waiting;
ItemColor statuslight = unknown; // same as off

// servo pins
#if !defined(SERVO0_PIN)
#define SERVO0_PIN FEEDHWHEEL_PIN
#endif

#if !defined(SERVO1_PIN)
#define SERVO1_PIN CAROUSEL_PIN
#endif

int readDistanceSensor();
int getTravelTime(ItemColor prevcolor, ItemColor);
void moveFeedWheel(FeedWheelState);
void moveCarousel(ItemColor);
void setCarouselPos (int);
void setFeedWheelPos (int);
void displayItemColor(ItemColor);
#endif

I2C:
//
// I2C/TWI implementation for AVR microcontrollers.
//

#ifndef I2C_H
#define I2C_H

#define MAX_TRIES 5
#define I2C_START 0
#define I2C_DATA 1
#define I2C_STOP 2

void i2c_init (void);
unsigned char i2c_transmit(unsigned char);
int i2c_writebyte(unsigned int, unsigned int, int);
int i2c_readbyte(unsigned int, unsigned int, unsigned int*);

#endif

process_caousel:

//
// Sorting machine - sorting carousel process
//
#include "softservo.h"

void sortCarousel() {
static int init = 0;
static enum {waitingfornewpos, movetopos, waitingforpos, stayinginpos} state;
static ItemColor currentcolor = unknown;
static int waitfornewpos = 0;
static Timer t = newTimer();

// initialize
if(!init) {
//Serial.println("Initializing Carousel Process");
nextcolor = unknown;
state = movetopos;

init = 1;
}

// switch the program into the proper state
switch (state) {
case waitingfornewpos:
// same color as previous item, duplicate color, stay put and wait for item to roll into bin
if (colorduplicate) {
// clear status
colorduplicate = 0;

startTimer(waitinpos, t);
carouseldelivering = 1;
state = stayinginpos;
}

// new color detected by feed wheel, move carousel into position
if (currentcolor != nextcolor) {
// update status to let feedwheel know it has received the notification of a new item and started moving
Serial.print("Carousel: Different color detected, carousel is moving to new position, bin: ");
Serial.println(colortext[nextcolor]);
carouselmoving = 1;
currentcolor = nextcolor;

state = movetopos;
}
break;
case movetopos:
// rotate the carousel to the new position determined by the new detected color
waitfornewpos = getTravelTime(prevcolor, nextcolor);
if (!waitfornewpos) waitfornewpos = 1;

Serial.print("Carousel: Waiting for carousel to move to position, takes (ms): ");
Serial.println(waitfornewpos);
startTimer(waitfornewpos, t);
moveCarousel(currentcolor);

state = waitingforpos;
case waitingforpos:
// wait for the carousel to move into position (no external feedback, only timer)
if (timeout(t)) {
Serial.print("Carousel: Carousel is now in position, bin: ");
Serial.println(colortext[currentcolor]);

startTimer(waitinpos, t);
carouseldelivering = 1;
state = stayinginpos;
}
break;
case stayinginpos:
// when in position, stay put to finish the delivery (rolling) of the item to the bin
if (timeout(t)) {
// clear status
carouseldelivering = 0;
state = waitingfornewpos;
}
break;
}
}

processs_feedwheel:

//
// Sorting machine - feedwheel process
//
#include "colorsensor.h"
#include "softservo.h"

void feedWheel() {
static int init = 0;
static enum {standby, feeditem, waitformovement, waitreverse, waitforfeed, performanalysis, analyzecolor, releaseitem, dropitem} state;
static FeedWheelState wheelaction = none;
static int waitpreemptmove = 0;
static int colorsamples = 0;
static int detectedrgbcolors[4] = {0, 0, 0, 0};
static ItemColor founditemcolors[3] = {unknown, unknown, unknown};
static ItemColor finalcolor = unknown;
static Timer t = newTimer();
static int testcolor = 0;
double detectedhsv[3];
static int conseqcount = 0;

// initialize
if(!init) {
//Serial.println("Initializing Feed Wheel Process");
state = feeditem;

init = 1;
}

// switch the program into the proper state
switch (state) {
case standby:
Serial.println("");
Serial.println("Feed Wheel: Entering standby mode, type S to start processing");
while ((char)Serial.read() != 'S');
//Serial.println("Feed Wheel: Received command to start feed wheel");
Serial.println("");
state = feeditem;
break;
case feeditem:
// move feed wheel to release an item and load a new item
wheelaction = startwheel;
moveFeedWheel(wheelaction);

// update status indicator
statusindicator = blinking;
//statuslight = white;

// use timer to detect blockage if no movment signal has been received
startTimer(900, t);

Serial.println("Feed Wheel: Moving, awaiting confirmation");
state = waitformovement;
break;
case waitformovement:
// wait for feedwheelmonitor to indicate the wheel is moving
if (outofposition) {
// clear status
outofposition = 0;

// sensor detected change from empty hole to wheel wall
Serial.println("Feed Wheel: Moving, waiting to be in position");
state = waitforfeed;
} else if (timeout(t)) {
// try to detect blockage, reverse wheel
wheelaction = reversewheel;
moveFeedWheel(wheelaction);
startTimer(300, t);
Serial.println("Feed Wheel: Detected blockage, reversing wheel");
state = waitreverse;
}
break;
case waitreverse:
if (timeout(t)) {
// clear any movement signals, begin from stach
inposition = 0;
outofposition = 0;

state = feeditem;
}
break;
case waitforfeed:
// wait for feedwheelmonitor to indicate a new empty hole and stop the wheel
if (inposition) {
// clear status
inposition = 0;

// sensor detected change from wheel wall to hole, move slow to allow sensor to perform measurements
Serial.println("Feed Wheel: Stopping, in position to load and analyze new item");
wheelaction = slowwheel;
moveFeedWheel(wheelaction);
startTimer(1, t);
statusindicator = steady;

state = performanalysis;
}
break;
case performanalysis:
// analyze the item, detect color three times while wheel is rotating slightly

if (timeout(t)) {
if (colorsamples < 3) {
getColors(detectedrgbcolors);
rgbToHSV((double)detectedrgbcolors[0], (double)detectedrgbcolors[1], (double)detectedrgbcolors[2], detectedhsv);
if (detectedhsv[0] > 352) detectedhsv[0] = 0; // red discontinuity
founditemcolors[colorsamples] = determineColor(detectedhsv);
Serial.print("Feed Wheel: Performed analysis number #");
Serial.print(colorsamples+1);
Serial.print(": ");
Serial.print(colortext[founditemcolors[colorsamples]]);
/* Serial.print(" - (rgb) ");
Serial.print(detectedrgbcolors[0]);
Serial.print(" ");
Serial.print(detectedrgbcolors[1]);
Serial.print(" ");
Serial.print(detectedrgbcolors[2]);
Serial.print(" (");
Serial.print(detectedrgbcolors[3]);
Serial.print(") hsv: ");
*/
Serial.print(" hsv: ");
Serial.print(detectedhsv[0]);
Serial.print(" ");
Serial.print(detectedhsv[1]);
Serial.print(" ");
Serial.println(detectedhsv[2]);

//statuslight = founditemcolors[colorsamples];
startTimer(waitcolorreadings, t);
colorsamples++;
} else {
// reset sample count and stop wheel
colorsamples = 0;
wheelaction = stopwheel;
Serial.println("Feed Wheel: All samples collected");
moveFeedWheel(wheelaction);
state = analyzecolor;
}
}
break;
case analyzecolor:
// analyze the final item color
prevcolor = nextcolor;

// if any two measurements are equal, then conclude that the item color is detected
if (founditemcolors[0] == founditemcolors[1]) finalcolor = founditemcolors[0];
else if (founditemcolors[1] == founditemcolors[2]) finalcolor = founditemcolors[1];
else if (founditemcolors[0] == founditemcolors[2]) finalcolor = founditemcolors[2];
else finalcolor = unknown;

// for testing
/*
testcolor += random(0,6);
if (testcolor > 6) testcolor = 0;
finalcolor = (ItemColor)testcolor;
*/

Serial.print("Feed Wheel: Analysis found item color: ");
Serial.println(colortext[finalcolor]);

statuslight = finalcolor;

// update global variable to let carousel know a new item has been analyzed and ready to be released
nextcolor = finalcolor;

// update status indicator
statuslight = nextcolor;

// if three unknown is detected consequently, feed is probably empty, standby
if (conseqcount < conseqempty) {
// emtpy buckets register as yellow or purple/unknown, use this to an advantage, given there won't be <conseqcount> sequence of those colors
if (finalcolor == unknown || finalcolor == yellow) {
conseqcount++;
} else {
conseqcount = 0;
}
state = releaseitem;
} else {
conseqcount = 0;
state = standby;
//wheelaction = reversewheel;
//moveFeedWheel(wheelaction);
//startTimer(1000, t);
//Serial.println("Feed Wheel: No items in feed tube, reversing wheel");
//state = waitreverse;
}
break;
case releaseitem:
// wait for carousel to register the new color, should be instant
if (!carouseldelivering) {
if (carouselmoving) {
// clear status
carouselmoving = 0;

// time to wait before carousel is just on the way to be in pos, purpose for preemptive release of item
waitpreemptmove = getTravelTime(prevcolor, nextcolor);
if ((waitpreemptmove -= preempttime) <= 0) waitpreemptmove = 1; // set low wait time, release immediately

Serial.print("Feed Wheel: Waiting for carousel, holding back item (ms): ");
Serial.println(waitpreemptmove);
startTimer(waitpreemptmove, t);
state = dropitem;
} else if (nextcolor == prevcolor) {
// color is the same, no need to wait for the carousel to move, release immediately
colorduplicate = 1;
Serial.println("Feed Wheel: Already positioned, releasing item immediately and loading new");
Serial.println("");
state = feeditem;
}
}
break;
case dropitem:
// to optimize operation, start to release item from feed wheel just before carousel is arriving to pos
if (timeout(t)) {
// move feed wheel to release and load a new item, simultaneously
Serial.println("Feed Wheel: Releasing item and loading new, repeating loop");
Serial.println("");
state = feeditem;
}
break;
}
}

process_feedwheelmonitor:
//
// Sorting machine - feedwheel monitor process
//

void feedWheelMonitor() {
static int init = 0;
static enum {waiting, readsensor} state;
static int distancevalue = 0;
static int distancereading = 0;
static int distancebefore = 0;
static Timer t = newTimer();

// initialize
if(!init) {
//Serial.println("Initializing Feed Wheel Monitor Process");
state = readsensor;
init = 1;
}

// switch the program into the proper state
switch (state) {
case waiting:
if (timeout(t)) {
state = readsensor;
}
break;
case readsensor:
distancevalue = readDistanceSensor();
// use map to create regions of valid zones, filter noise
// in position reads: 300 (~8mm distance between sensor and item hole end)
// out of position reads: 900 (~2mm)
distancereading = map(distancevalue, fardistance, fardistance, 0, 1);

if (distancereading < distancebefore) {
Serial.print("Monitor: In position/item located, value: ");
Serial.print(distancereading);
Serial.print(", ");
Serial.println(distancevalue);
inposition = 1;
outofposition = 0;
distancebefore = distancereading;

} else if (distancereading > distancebefore) {
Serial.print("Monitor: Out of position, value: ");
Serial.print(distancereading);
Serial.print(", ");
Serial.println(distancevalue);
inposition = 0;
outofposition = 1;
distancebefore = distancereading;
} else {
// inbetween waiting for something to happen, wheel spinning
}

startTimer(waitdistancesensor, t);
state = waiting;
break;
}
}

process_status:

//
// Sorting machine - status led process
//

void statusIndicator() {
static int init = 0;
//static enum {waiting, steady, blinking} state;
static StatusState state;
static Timer t = newTimer();
static int blinkingstate = 0;
static int waitblinkoff = 50; // 150
static int waitblinkon = 25; // 75
static ItemColor statuslightbefore = unknown;

// initialize
if(!init) {
//Serial.println("Initializing Status Indicator Process");
state = steady;

init = 1;
}

// switch the program into the proper state
switch (state) {
case waiting:
// wait for blinking timer to expire
if (timeout(t)) {
state = blinking;
}
break;
case steady:
// keep the status light illumiated
if (statusindicator != steady) {
state = statusindicator;
} else if (statuslight != statuslightbefore) {
displayItemColor(statuslight);
statuslightbefore = statuslight;
}
break;
case blinking:
// switch the status light on and off in a defined period
if (statusindicator != blinking) {
state = statusindicator;
} else {
if (!blinkingstate) { // off
displayItemColor(unknown);
startTimer(waitblinkoff, t);
blinkingstate = 1;
} else { // on
displayItemColor(statuslight);
startTimer(waitblinkon, t);
blinkingstate = 0;
}
state = waiting;
}
break;
}
}

softservo:

//
// Software 16-bit PWM using Output Compare in Normal Mode on timer0 or timer2 (8-bit)
//

#ifndef SOFTSERVO_H
#define SOFTSERVO_H

#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif

#if !defined(SERVO0_PIN)
#define SERVO0_PIN 12
#endif

#if !defined(SERVO1_PIN)
#define SERVO1_PIN 13
#endif

void initServo();
void setServoPos(int, int);
static void servoUpdate(int);

// globals for servoUpdate() and interrupt vector
static uint8_t ocrCount = 0;
static int servoPos[2] = {1500, 1500};
static const uint8_t servoPins[2] = {SERVO0_PIN, SERVO1_PIN};
#endif

sorting_machine_rev3:

//
// Sorting machine v1.0
// A modulized system for sorting M&M or Skittles into six different bins based its color
//
// by ivc <ivc@x-pec.com> Nov 2012
//
#include "functions.h"

// settings
// pins for the servos, any digital will work
#define FEEDHWHEEL_PIN 12
#define CAROUSEL_PIN 13
// color light led
#define LEDLIGHT_PIN A1
// analog distance sensor (using ADC readout)
#define DISTANCESENSOR_PIN A0
// RGB LED pins
#define RED_PIN 6
#define GREEN_PIN 5
#define BLUE_PIN 3

// time values
// distance check: how long to wait between distance sensor checks
const int waitdistancesensor = 10;
// color sampling: time to wait between color sensor readings (3x readings performed)
const int waitcolorreadings = 30;
// color determining: radius of the confidence interval, 2 stddev 95%, 3 stddev 99%
const int colordeterminestddev = 2;
// release: travel time needed to travel past one cup, e.g. 1560ms/6 = 260ms travel time per cup
const int travelpercup = 230; // s125 1t 1560/6=260ms - hs322 75ms
// release: try to preempt an item release before carousel is in position to gain performance
const int preempttime = 1*travelpercup;
// delivery: how long to stay put waiting for the item to roll into the bin
const int waitinpos = 400; // 750 quiet

// standby: how many consequent/unkown analysis should run by before stopping the wheel, 4 is a full rotation
const int conseqempty = 15;
// distance sensor trigger values for feed wheel positioning, betweeen 50-750
const int fardistance = 75;

void setup() {
// initialize serial communication:
Serial.begin(115200);
Serial.println("Sorting Machine v1.0 - Welcome!");
Serial.println("");

// setup counter registers for servos
initServo();

//temp
//analogWrite(A3, 255);
}

void loop() {
// main loop of processes
feedWheel();
sortCarousel();
feedWheelMonitor();
statusIndicator();
}

timer:
//
// TIMER DECLARE
//
// The controller for the Arduino Mega series is the Atmel AVR ATmega1280 or the ATmega2560.
// Also identical only differs in memory size. These controllers have 6 timers. Timer 0,
// timer1 and timer2 are identical to the ATmega168/328. The timer3, timer4 and timer5 are
// all 16bit timers, similar to timer1.

#ifndef TIMER_H
#define TIMER_H

#include <avr/io.h>
#include <stdlib.h>
#include <avr/interrupt.h>

typedef struct timer {
struct timer * next;
volatile unsigned long counter;
volatile int expired;
}
timer_type, * Timer;
Timer newTimer(void);
void deleteTimer(Timer);
void startTimer(unsigned long, Timer);
int timeout(Timer);
void startTiming();
unsigned long long timeSinceStart(); // returns 64 bit integer!
void delayMilli(unsigned long);
void delayMicro(unsigned int);

static Timer first = NULL;
static int initialized = 0;
static volatile unsigned long millisecs_since_start = 0;
#endif


I really appreciate the help!

-Spencer
By paulvha
#197212
Like to help BUT there is a lot of code and due to the copy/paste, very hard to read (nothing you can do about that).
Can you be a bit more precise on what happens and when. E.g. what messages do you get on the serial line, when does it stop doing what you expect, and WHAT is not happening etc.etc Where do you think your problem is ?
By paulvha
#198408
In void sortCarousel(), case movetopos: is NOT having a break; clause end the end. As such it will directly also execute the code in case waitingforpos:. Is that correct ??
By sbrow102
#198468
Looking into it a little more I verified the codes on the Arduino app and the only error message was, "Low memory available. Stability problems may occur." I also came to the conclusion that it's not the color recognition that's having problems it's the servo at the base that's the problem. It budges a little bit so I don't think it's a wiring issue, just with the servo.