Automated Medication Color Sorter
Page last updated 30 Apr 2016, by 
David Ehrlich.
0
replies
David Ehrlich.
0
replies
Project Description
An embedded system to aide the elderly and blind by sorting pills by color.
Team Members
- David Ehrlich, ECE 4180 Section A
- Raj Patel, ECE 4180 Section A
- Miheer Bavare, ECE 4180 Section B
- Chandler Matz, ECE 4180 Section B
Design Methodology
A user fills the hopper with multi-colored medication pills for sorting. The control algorithm begins by rotating the agitator 90 degrees every second. When a pill settles into one of the four agitator holes, it will eventually pass over the hoppers only hole and drop into the selector box (similar to how a classic gumball machine works). The agitator spins exactly 90 degrees by counting ticks using one of the encoder wheels attached to the rotor of the DC motor controlling the agitator.
Once the pill falls into the selector box, it is pinned against the color sensor by the servo door, which is in the 'closed' position. The color sensor begins to analyze the pill, which sends a signal to the mbed to stop rotating the agitator. This prevents additional pills falling into the selector box. The color sensor algorithm works as follows: An LED shines light onto the pill and Red, Blue and Green color frequencies are reflected and received by the sensor and stored as 16-bit unsigned values. The program maintains a small library of average values of Red, Blue and Green compositions for popular colors. When a new pill is being analyzed, the new pill's RGB values are compared to the standardized values using the distance formula, and the 'closest' value is assigned as the color of the pill. Once the color is determined, that value is passed to the next part of the control algorithm to rotate the base plate and position the correct catch-bay directly under the selector box. Precise control of the base plate is achieved with the encoders by rotating the necessary amount of ticks to position the correct color bay directly under the selector box. Once the catch-bay is in place, the servo moves to the 'open' position, releasing the pill and allowing it to drop into the correct bay. The color sensor detects that the pill has dropped by reading an 'ambient light' value. At this state, the control algorithm returns the base plate back to 'home' position, followed by activating the DC motor controlling the agitator. The process repeats as long as there are pills in the hopper.
Components
- Two DC motors
- Two 8-pole magnetic encoder wheel kits with hall effect sensors for precise position control
- Standard RC servo
- Adafruit TCS34725 RGB Color Sensor
- 3D printed mechanical parts (gears, spindle connectors, hopper, agitator, catch-bay)
- Various laser cut parts
Gear
Spindle Connector
Agitator
Catch-Bay
Hopper
Link to Adafruit RGB Color Sensor Component Page and Code
https://developer.mbed.org/users/raj1995/notebook/adafruit-tcs34725-rgb-color-sensor/
Project Demo
Project Code
#include <math.h>
#include <stdio.h>
#include "mbed.h"
#define commonAnode true
#include <algorithm>
#include "Servo.h"
//Color Sensor
I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
int sensor_addr = 41 << 1;
DigitalOut led(p11);
//IR
Serial pc(USBTX, USBRX);
Serial device(p13, p14); // tx, rx
PwmOut IRLED(p21);
//Solenoid
DigitalOut Ctrl(p8); //Solenoid output control bit
//Motor A
PwmOut speedA(p23); // PWM Speed pin for Motor A
DigitalOut fwdA(p17); // Digital out signal for forward direction for Motor A
DigitalOut revA(p18); // Digital out signal for reverse direction for Motor A
//MotorB
PwmOut speedB(p24); // PWM Speed pin for Motor B
DigitalOut fwdB(p20); // Digital out signal for forward direction for Motor B
DigitalOut revB(p19); // Digital out signal for reverse direction for Motor B
//Bottom Encoder
DigitalIn Enc1(p16); //right motor
int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
int ticks = 0;
int ticksPrime = 0;
int dist2move = 0;
//Top Encoder
DigitalIn Enc2(p15); //right motor
int oldEnc2 = 0; //Was the encoder previously a 1 or zero?
int ticks2 = 0;
//Servo
Servo servo(p25);
//Prototypes (headers)
void irSetup();
void colorSensorSetup();
int readColor();
int readIR();
void solOn();
void solOff();
void motorA(int);
void motorB(int);
void encCount(int);
void encCount2(int);
void moveBottom(int);
void moveTop(int);
void pulseBack(int);
int redpos = 0;
int greenpos = 0;
int yellowpos = 0;
int purplepos = 0;
int randompos = 0;
int turnAmount = 75;
int greenList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
int redList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
int yellowList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
int purpleList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
int garbageList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
int currentColor = 4;
int counter = 0;
int main() {
//Color Sensor setup
colorSensorSetup();
//Bottom Encoder setup
Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
//Top Encoder setup
Enc2.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
servo = 0.05;
bool succ = true;
double servoClosed = -0.05;
while(1) {
//succ = false;
if (succ){
moveTop(94);
succ = false;
wait(0.5);
}
encCount2(1);
//pc.printf("%d\r\n",ticks2);
wait(0.8);
int nextColor = readColor();
//pc.printf("%d,%d\n\r",currentColor,nextColor);
switch (currentColor){
case 0:
switch (nextColor){
case 0:
moveBottom(greenList[0]);
servo = 0.4;
wait(1);
servo = servoClosed;
break;
case 1:
moveBottom(greenList[1]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 2:
moveBottom(greenList[2]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 3:
moveBottom(greenList[3]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 4:
moveBottom(greenList[4]);
servo = servoClosed;
counter++;
break;
}
break;
case 1:
switch (nextColor){
case 0:
moveBottom(redList[4]);
servo = 0.4;
wait(1);
servo = 0.05;
counter++;
break;
case 1:
moveBottom(redList[0]);
servo = 0.4;
wait(1);
servo = servoClosed;
break;
case 2:
moveBottom(redList[1]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 3:
moveBottom(redList[2]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 4:
moveBottom(redList[3]);
servo = servoClosed;
counter++;
break;
}
break;
case 2:
switch (nextColor){
case 0:
moveBottom(yellowList[3]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 1:
moveBottom(yellowList[4]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 2:
moveBottom(yellowList[0]);
servo = 0.4;
wait(1);
servo = servoClosed;
break;
case 3:
moveBottom(yellowList[1]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 4:
moveBottom(yellowList[2]);
servo = servoClosed;
counter++;
break;
}
break;
case 3:
switch (nextColor){
case 0:
moveBottom(purpleList[2]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 1:
moveBottom(purpleList[3]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 2:
moveBottom(purpleList[4]);
servo = 0.4;
wait(1);
servo = servoClosed;
counter++;
break;
case 3:
moveBottom(purpleList[0]);
servo = 0.4;
wait(1);
servo = servoClosed;
break;
case 4:
moveBottom(purpleList[1]);
servo = servoClosed;
counter++;
break;
}
break;
case 4:
switch (nextColor){
case 0:
ticksPrime = ticks;
dist2move = nextColor*75;
moveBottom(garbageList[1]);
servo = 0.4;
wait(1);
servo = servoClosed;
if(ticks <= ticksPrime + dist2move){
succ = true;
}
counter++;
break;
case 1:
ticksPrime = ticks;
dist2move = nextColor*75;
moveBottom(garbageList[2]);
servo = 0.4;
wait(1);
servo = servoClosed;
if(ticks <= ticksPrime + dist2move){
succ = true;
}
counter++;
break;
case 2:
ticksPrime = ticks;
dist2move = nextColor*75;
moveBottom(garbageList[3]);
servo = 0.4;
wait(1);
servo = servoClosed;
if(ticks <= ticksPrime + dist2move){
succ = true;
}
counter++;
break;
case 3:
ticksPrime = ticks;
dist2move = nextColor*75;
moveBottom(garbageList[4]);
servo = 0.4;
wait(1);
servo = servoClosed;
if(ticks <= ticksPrime + dist2move){
succ = true;
}
counter++;
break;
case 4:
ticksPrime = ticks;
dist2move = nextColor*75;
moveBottom(garbageList[0]);
servo = servoClosed;
if(ticks <= ticksPrime + dist2move){
succ = true;
}
break;
}
break;
}
encCount(1);
currentColor = nextColor;
if (counter==4){
pulseBack(16);
counter = 0;
pc.printf("%d\r\n",counter);
}
}
}
int readColor(){
// Initialize color sensor
char clear_reg[1] = {148};
char clear_data[2] = {0,0};
i2c.write(sensor_addr,clear_reg,1, true);
i2c.read(sensor_addr,clear_data,2, false);
int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
char red_reg[1] = {150};
char red_data[2] = {0,0};
i2c.write(sensor_addr,red_reg,1, true);
i2c.read(sensor_addr,red_data,2, false);
int red_value = ((int)red_data[1] << 8) | red_data[0];
char green_reg[1] = {152};
char green_data[2] = {0,0};
i2c.write(sensor_addr,green_reg,1, true);
i2c.read(sensor_addr,green_data,2, false);
int green_value = ((int)green_data[1] << 8) | green_data[0];
char blue_reg[1] = {154};
char blue_data[2] = {0,0};
i2c.write(sensor_addr,blue_reg,1, true);
i2c.read(sensor_addr,blue_data,2, false);
int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
// print sensor readings
//pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n\r", clear_value, red_value, green_value, blue_value);
//pc.printf("Red (%d), Green (%d), Blue (%d)\n\r", red_value, green_value, blue_value);
float red_in = (float)red_value;
float green_in = (float)green_value;
float blue_in = (float)blue_value;
float redSkittle[] = {4656.25,588.25,826};
float greenSkittle[] = {2968.25,3898.5,1346.5};
float yellowSkittle[] = {15387.75,9977.5,3173.75};
float purpleSkittle[] = {1215,1773,2541};
float closed[] = {1094, 1149, 987};
float distRed = sqrt(pow(red_in-redSkittle[0],2)+pow(green_in-redSkittle[1],2)+pow(blue_in-redSkittle[2],2));
float distGreen = sqrt(pow(red_in-greenSkittle[0],2)+pow(green_in-greenSkittle[1],2)+pow(blue_in-greenSkittle[2],2));
float distYellow = sqrt(pow(red_in-yellowSkittle[0],2)+pow(green_in-yellowSkittle[1],2)+pow(blue_in-yellowSkittle[2],2));
float distPurple = sqrt(pow(red_in-purpleSkittle[0],2)+pow(green_in-purpleSkittle[1],2)+pow(blue_in-purpleSkittle[2],2));
float distClosed = sqrt(pow(red_in-closed[0],2)+pow(green_in-closed[1],2)+pow(blue_in-closed[2],2));
float choices[] = {distRed, distGreen, distYellow, distPurple, distClosed};
sort(choices,choices+5);
float closest = choices[0];
if (closest==distRed){
//pc.printf("Red\n\r");
return 1;
}else if(closest==distGreen){
//pc.printf("Green\n\r");
return 0;
}else if(closest==distYellow){
//pc.printf("Yellow\n\r");
return 2;
}else if(closest==distPurple){
//pc.printf("Purple\n\r");
return 3;
}else //pc.printf("Dave messed up.\n\r");
return 4;
}
int readIR(){
if(pc.readable()) {
device.putc(pc.getc());
}
//IR Receive code
if(device.readable()) {
pc.putc(device.getc());
}
return 1;
}
void solOn(){
Ctrl = 1; //ON
}
void solOff(){
Ctrl = 0; //ON
}
void motorA(int state){
fwdA = 1;
revA = 0;
speedA = (float)state*0.15;
}
void motorB(int dir){
if (dir > 0){
fwdB = 1;
revB = 0;
}else{
fwdB = 0;
revB = 1;
dir = -dir;
}
speedB = (float)dir*.3;
}
void pulseBack(int dir) {
//int newticks = ticks - dir;
motorB(1);
wait(0.1);
motorB(0);
return;
}
void irSetup(){
//IR Transmit setup code
IRLED.period(1.0/38000.0);
IRLED = 0.5;
//Drive IR LED data pin with 38Khz PWM Carrier
//Drive IR LED gnd pin with serial TX
device.baud(2400);
}
void colorSensorSetup(){
pc.baud(9600);
led = 1; // off
// Connect to the Color sensor and verify whether we connected to the correct sensor.
i2c.frequency(200000);
char id_regval[1] = {146};
char data[1] = {0};
i2c.write(sensor_addr,id_regval,1, true);
i2c.read(sensor_addr,data,1,false);
if (data[0]==68) {
//green = 0;
wait (2);
//green = 1;
} else {
//green = 1;
}
char timing_register[2] = {129,0};
i2c.write(sensor_addr,timing_register,2,false);
char control_register[2] = {143,0};
i2c.write(sensor_addr,control_register,2,false);
char enable_register[2] = {128,3};
i2c.write(sensor_addr,enable_register,2,false);
}
void encCount(int dir){
if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched.
if (dir>0) ticks++;
else ticks--;
oldEnc1 = Enc1;
//pc.printf("%d\r\n",ticks);
}
}
void encCount2(int dir){
if(Enc2 != oldEnc2) { // Increment ticks every time the state has switched.
if (dir>0) ticks2++;
else ticks2--;
oldEnc2 = Enc2;
//pc.printf("%d\r\n",ticks2);
}
}
void moveBottom(int amt){
int newticks = ticks + amt;
motorB(-1);
while (ticks < newticks-5){
encCount(1);
//pc.printf("%d,%d\n\r",newticks,ticks);
}
motorB(0);
return;
}
void moveTop(int amt){
int newticks = ticks2 + amt;
motorA(1);
while (ticks2 < newticks-5){
encCount2(1);
//pc.printf("%d,%d\n\r",newticks,ticks2);
}
motorA(0);
return;
}
Please log in to post comments.
Automated Medication Color Sorter
Page owner: David Ehrlich
Created 29 Apr 2016.
Last updated 30 Apr 2016