ToBe Automation - Color Sorter Robot - Introduction
DESCRIPTION
This is a small project we did for a robotic course at UCN.
The project is to get a robot to sort objects depending on color. The camera looks for 2 colors, green and blue, all other colors will be discarded.
The objects are coming on the conveyor, which has two sensors. First is starting the conveyor and second stops. On the last sensor the robot gets a signal to pick up the object, according the color of the object the robot moves it to one of the three zones.
3 More Images
Conveyor belt (ready built with frequency converter, and proximity sensors)
TrossenRobotics PhantomX Pincher
Pixy cam
Led driver board (1 Green, 1 Red)
24VDC Relay
Some bananna wires
Rockwell Studio 5000, to make the program for the PLC
Arduino IDE, for uploading code to the Artibox board
Xcode IDE, to make the program for the Artibox board
The conveyor/frequency converter is connected via ethernet to the PLC.
A proximity sensor at each end of the conveyor is connected to a digital in on the PLC.
The 24VDC relay coil is connected to 0VDC on the PLC, and a digital output on the PLC, one side of the NO switch is connected to 5V on the Artibox board (Arduino type board on the PhantomX), the other side is connected to a digital I/O pin on the Artibox board
The relay board coil is connected to 0V and a digital I/O pin on the Artibox board, one side of the NO switch is connected to 24VDC from the PLC, the other side is connected to a digital in on the PLC
PLC programing is used for controlling conveyor belt.
There are two sensors, one on the beginning which starts the conveyor and on the end of conveyor which stops the conveyor. With first sensor, if there is an object starts also the counter up. With every object leaving the belt, there is a counter down. It means that PLC knows if there is product on the conveyor, or not. So it knows if it should start conveyor, or not. PLC is getting the signal from arduino, if the robot arm is in ready state. It means that it is in home position, doing nothing. When it is ready the PLC send the signal to arduino that there is object on the last sensor and robot can pick it. There is not possibility to send signal about another object, until first one is not done.
Included is the PLC program, made in Studio 5000, you will probably have to change the I/O numbers
Obviously, operating the robotic arm requires libraries from the manufacturer.
For using the poses, a special header file is included with the arduino project.
Next you will need Pixy libraries, for recognising colours, you will need Pixy.h and SPI.h.
The servo motors require ax12.h library and the movement are served with BioloidController.h. You could just use functions from ax12 to set the position manually, but then it won't take the shortest path and you have to set the speed every movement with delays, with Bioloid, you set it just once.
The led13.h library is also included in the zip file attached, but it is just a simple library, where you can see, how a function look like, etc... It include on/off/blink abilities.
Program for the Artibox board
Header file to hold the poses
//
// ToBeAut_Robotics.h // // // Created by Tomas Rus on 01/06/16. // //#ifndef ToBeAut_Robotics_h #define ToBeAut_Robotics_h
#include //hardcoded main poses PROGMEM prog_uint16_t homepose[] = {5, 492, 185, 893, 757, 512}; //home PROGMEM prog_uint16_t centered[] = {5, 213, 512, 512, 512, 256}; //centered PROGMEM prog_uint16_t abovecon[] = {5, 690, 530, 744, 715, 512};//above conveyor PROGMEM prog_uint16_t xabovecon[] = {5, 690, 530, 744, 715, 200}; //holded PROGMEM prog_uint16_t undercam[] = {5, 175, 310, 971, 711, 200};// in front of the cam PROGMEM prog_uint16_t pile1[] = {5, 343, 543, 866, 729, 200};//pile 1 PROGMEM prog_uint16_t xpile1[] = {5, 343, 543, 866, 729, 512};//pile 1 gripper open PROGMEM prog_uint16_t pile2[] = {5, 422, 724, 644, 599, 200}; //pile 2 PROGMEM prog_uint16_t xpile2[] = {5, 422, 724, 644, 599, 512}; //pile 2 gripper open PROGMEM prog_uint16_t dump[] = {5, 353, 378, 268, 328, 200}; //dump PROGMEM prog_uint16_t xdump[] = {5, 353, 378, 268, 328, 512}; //dump gripper open
#endif /* ToBeAut_Robotics_h */
Actual arduino program
//
// ToBeAut_Robotics.cpp // // // Created by Tomas Rus on 01/06/16. // // // Main program for ArbotiX PhantomX Pincher // Class - aut/0915 // Author: Tomas Rus, Benedikt Zaleha, Christian Hejlesen#include "ToBeEssentials.h" //hardcoded Poses #include //Pixy libraries #include // #include //library for AX12 servos #include //library for Move function and many more #include //small library for led operation, can be replaced
BioloidController bioloid = BioloidController(1000000);
//Global variables go here LED13 ledgreen; LED13 ledred; int PshBut = 0; //push button 1 pin number int val = 0; int fromPLC = 3; //pin for input from PLC, if there'ß a box int readySeq = 18; //pin for sending ready signal via relay to the PLC //recognizing the colours, colour 1 = blue, colour = 2 = green, else 0 int moveDelay = 1000; //speed of the robot in ms int Pressure; int colour = 0; int boxW = 0; //box waiting on the conveyor, internal bit
void setup ()
{ pinMode(3, INPUT); pinMode(16, OUTPUT); pinMode(17, OUTPUT); pinMode(18, OUTPUT); }
void loop () { delay(1000); Move(homepose); ledred.off(17); ledgreen.on(16); //signalizing ready status with green led boxW = 0; boxW = digitalRead(fromPLC); delay(1000); if (boxW) { ledgreen.off(16); //signalizing operation status with red led ledred.on(17); playSequence(); } } // void loop end void Move(prog_uint16_t* Inn) //inherited from bioloid library { delay(moveDelay); // recommended pause bioloid.loadPose(Inn); // load the pose from FLASH, into the nextPose buffer bioloid.readPose(); // read in current servo positions to the curPose buffer delay(moveDelay); bioloid.interpolateSetup(moveDelay); // setup for interpolation from current->next over 1/2 a second while(bioloid.interpolating > 0) { // do this while we have not reached our new pose bioloid.interpolateStep(); // move servos, if necessary. delay(3); } } void playSequence() { delay(1000); Move(abovecon); delay(1000); Move(xabovecon); /* while (Pressure<500) { //optional use of pressure sensor Pressure = analogRead(26); delay(200); gripper -= 30; if (gripper <0) { gripper = 0; } ax12SetRegister2(5, 30, gripper); } */ delay(1000); Move(undercam); delay(1000); colour = getColor(); switch (colour) { case '1': //moving boxes to the first pile Move(pile1); delay(1000); Move(xpile1); delay(1000); break; case '2': //moving boxes to he second pile Move(pile2); delay(1000); Move(xpile2); delay(1000); break; default: //moving unrecognized boxes to the dump pile Move(dump); delay(1000); Move(xdump); break; } Move(homepose); }
int getColor() { Pixy pixy; pixy.init(); delay(500); int blocks; int j; int i = 0; int color = 0; while (color != 1 || color != 2) { blocks = pixy.getBlocks(); if (blocks) { i++; // do this (print) every 20 frames because printing every // frame would bog down the Arduino if (i%20==0) { Serial.println(color); color = pixy.blocks[j].signature; return color; } } } }
So there you have it, an automated LEGO sorter, you can of course add more colors, and more poses to the system, so that you can sort all of your LEGO's if you don't like doing it by hand :-)
Regards
Tomas, Benedikt, and Christian