Color-Sorting Robot

William Zhang

IMG_2079.MOV

Functional Demonstration

Robot Concept

The very first idea I had for this robot was a hexapedal robot equipped with a forward gripper that would be capable of navigating to small objects and sorting them based on their color. It would be controlled by an Arduino and primarily use ultrasonic sensors to navigate. The task would also require a color sensor of some sort. I eventually decided upon a ColorPAL color sensor instead of trying to use a camera and run image analysis, but this method proved to have some limitations after some preliminary tests, namely short range. Thus, the robot would not be capable of identifying colors from a distance and would have to place the ColorPAL in close proximity or direct contact with the object being analyzed.

This was quickly revised to be a simpler design that would use two wheels for locomotion instead, since the original design would be too complicated to handle solo.

The final version of the robot as presented would have to handle obstacle avoidance as well, but it was decided that it would be fine if I physically had to hand it an object instead of trying to get it to find one on its own.

Final Robot - Top

Final Robot - Front

Robot Finalized Design Details

The finished robot has two continuous servos driving its wheels and two more servos controlling the movements of its arm, with one lifting and lowering the arm and the other responsible for opening and closing the claw. It uses three ultrasonic sensors as rangefinders - the two on top are responsible for obstacle avoidance, while the third sensor is attached to the claw and positioned to detect objects on the ground. Also attached to the claw is a ColorPAL color sensor, which points downwards into the claw's grip. The robot is programmed to move forwards so long as it does not result in a collision with obstacles, and it correctly picks up and identifies the colors of small objects using the ColorPAL. When it picks up something that is colored red, it will turn right and drive forwards for a random period of time before setting it down again. If it picks up any other color, it will set it back down immediately. As pictured above the robot is controlled by an Arduino Pro Mini, but this has since been replaced by a Arduino Uno because the Pro Mini was damaged in some fashion and unable to accept power from the intended power source for the controller, a nine-volt battery. Everything else on the robot is powered by a set of four AA batteries.

Arduino Code

#include <Servo.h>

#include <ColorPAL.h>

const int rightTrig = 2;

const int rightEcho = 3;

const int leftTrig = 4;

const int leftEcho = 5;

const int handTrig = 6;

const int handEcho = 7;

const int rightPin = 8;

const int leftPin = 9;

const int armPin = 10;

const int handPin = 11;

const int colorPin = 12;

Servo rightWheel;

Servo leftWheel;

Servo armBase;

Servo armHand;

ColorPAL cSensor;

int redVal, greenVal, blueVal;

int prevLeft, prevRight, prevHand;

int rightDist, leftDist, handDist;

void setup() {

// put your setup code here, to run once:

pinMode(rightTrig, OUTPUT);

pinMode(rightEcho, INPUT);

pinMode(leftTrig, OUTPUT);

pinMode(leftEcho, INPUT);

pinMode(handTrig, OUTPUT);

pinMode(handEcho, INPUT);

rightWheel.attach(rightPin); //Attaching servoes

leftWheel.attach(leftPin);

armBase.attach(armPin);

armHand.attach(handPin);

cSensor.attachPAL(colorPin); //Attach color sensor

Serial.begin(9600);

//Robot's wheels do not move to start.

rightDist = pingSensor(rightTrig, rightEcho);

prevRight = rightDist;

delay(100);

leftDist = pingSensor(leftTrig, leftEcho);

prevLeft = leftDist;

delay(100);

handDist = pingSensor(handTrig, handEcho);

prevHand = handDist;

delay(100);

stopMove();

raiseArm();

openHand();

delay(1000);

}

void loop() {

stopMove();

rightDist = pingSensor(rightTrig, rightEcho);

if (abs(rightDist - prevRight) >=5) {

rightDist = pingSensor(rightTrig, rightEcho);

}

prevRight = rightDist;

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

delay(100);

leftDist = pingSensor(leftTrig, leftEcho);

if (abs(leftDist - prevLeft) >=5) {

rightDist = pingSensor(leftTrig, leftEcho);

}

prevLeft = leftDist;

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

delay(100);

handDist = pingSensor(handTrig, handEcho);

if (abs(handDist - prevHand) >=5) {

rightDist = pingSensor(rightTrig, rightEcho);

}

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

prevHand = handDist;

delay(100);

if (handDist <= 4) {

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

Serial.println("Executing grab");

stopMove();

openHand();

lowerArm();

closeHand();

raiseArm();

delay(500);

for(int i=0; i<4; i++) {

redVal = readRed(); greenVal = readGreen(); blueVal = readBlue();

}

if (isRed(redVal, greenVal, blueVal) == true) {

Serial.print("RGB "); Serial.print(redVal); Serial.print(greenVal); Serial.println(blueVal);

Serial.println("It's red!");

turnRight();

delay(random(500, 1000));

forwardMove();

delay(random(1000,1500));

stopMove();

lowerArm();

openHand();

raiseArm();

backMove();

delay(random(1000,1500));

turnRight();

delay(random(300, 600));

stopMove();

}

else {

Serial.print("RGB ");

Serial.print(redVal); Serial.print(" ");

Serial.print(greenVal); Serial.print(" ");

Serial.println(blueVal); Serial.print(" ");

Serial.println("It's not red!");

lowerArm();

delay(1500);

openHand();

delay(1500);

raiseArm();

delay(1500);

backMove();

delay(random(1000,1500));

turnLeft();

delay(random(200, 500));

stopMove();

}

else if ((rightDist > 15) && (leftDist > 15) && (handDist>4) && (handDist< leftDist-5 && rightDist-5)) {

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

Serial.println("Rush");

forwardMove();

}

else if ((rightDist <= 15) && (leftDist >= rightDist + 5)) {

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

Serial.println("Turn left");

turnLeft();

}

else if ((leftDist <= 15) && (rightDist >= leftDist + 5)) {

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

Serial.println("Turn right");

turnRight();

}

else if ((leftDist <= 15) && (rightDist <= 15)) {

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

Serial.println("retreat");

backMove();

}

else {

Serial.print("LRH ");

Serial.print(leftDist); Serial.print(" ");

Serial.print(rightDist); Serial.print(" ");

Serial.println(handDist); Serial.print(" ");

Serial.println("just advance");

forwardMove();

}

delay(500);

}

void forwardMove() {

rightWheel.write(110);

leftWheel.write(80);

}

void backMove() {

rightWheel.write(80);

leftWheel.write(110);

}

void turnRight() {

rightWheel.write(80);

leftWheel.write(80);

}

void turnLeft() {

rightWheel.write(110);

leftWheel.write(110);

}

void stopMove() {

rightWheel.write(95);

leftWheel.write(95);

}

void lowerArm() {

armBase.write(120);

delay(1500);

}

void raiseArm() {

armBase.write(100);

delay(1500);

}

void openHand() {

armHand.write(0);

delay(1000);

}

void closeHand() {

armHand.write(90);

delay(1000);

}

int readRed() {

int red = cSensor.redPAL();

return red;

}

int readGreen() {

int green = cSensor.greenPAL();

return green;

}

int readBlue() {

int blue = cSensor.bluePAL();

return blue;

}

bool isRed(int red, int green, int blue) {

if (red >= 6 && green <= 5 && blue <= 5) {

return true;

}

else {

return false;

}

void showColors() {

Serial.print("RGB ");

Serial.print(readRed()); Serial.print(" ");

Serial.print(readGreen()); Serial.print(" ");

Serial.print(readBlue());

Serial.println();

delay(100);

}

int pingSensor(int trigPin, int echoPin) {

int duration, distance;

digitalWrite(trigPin, LOW);

delayMicroseconds(2);

digitalWrite(trigPin, HIGH);

delayMicroseconds(10);

digitalWrite(trigPin, LOW);

duration = pulseIn(echoPin, HIGH);

distance = (duration*.0343)/2;

return distance;

}

IN-DEV PHOTOS

Various images of the robot in the process of being constructed. Some small challenges that had to be overcome:

The breadboard was originally intended to hang off the robot's back, but this shifted the robot's center of gravity too much and it would topple over if the castor wheel was positioned wrong. The breadboard was moved to the front instead, but this unfortunately cut down on usable space substantially
The arm's servo could not be securely attached to the robot with screws because the wood brace it was attached to was too layered and would split apart if drilled at all. It was attached with hot glue instead
The initial castor wheel turned out to be too rusted to turn easily, and had to be replaced