- Thu Dec 14, 2017 4:24 pm
#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
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