MaxArm: Arduino Robot Arm conduct Color Sorting in Two Ways

Arduino Code Comparison of Color Sensor and AI Vision Module in Color Sorting

MaxArm is an ESP32 open source robot arm which supports Arduino and Python programming developed by Hiwonder.

Color sorting is an AI game where robot arm is programmed to recognize color first, then controlled by inverse kinematics to pick and place blocks of different colors to specific position.

Before sorting, what robot arm need to do is use color sensor or WonderCam vision module to recognize the color of block.

This post compares and analyzes the codes of color sensor and AI vision module to implement color recognition and sorting. Codes uploaded for reference.

1. Open Program

1) Open the program ‘Tracking_Sorting.ino’.

2) Select the corresponding board model. Click “Tool -- Board -- ESP 32 Dev Module” in sequence.

3) Select the corresponding port in “Tools->Port”. For example, COM7. (The port on each computer may be different, and you need to select the port based on your own computer. Please avoid COM1 which is a system communication port in general.)

4) Click on tick icon to verify the program. If no error is reported, status “Compiling->Compile complete” will be displayed in turn on the status bar. After compilation is completed, information such as number of bytes used, and occupied program storage will be given.

5) Click on right arrow icon to upload the program once the program is compiled successfully. Uploading status “Compiling->Uploading->Complete” will be displayed in sequence.

2. Code Analysis

2.1 Import Function Library

Import buzzer, PWM servo, serial bus servo and air pump libraries before the program is executed.

Color Sorting by Color Sensor

Besides the libraries mentioned above, color sensor library “Arduino_APDS9960.h” is also required, as color recognition is achieved by color sensor.

#include "ESPMax.h"
#include "Buzzer.h"
#include "Ultrasound.h"
#include "ESP32PWMServo.h"
#include "SuctionNozzle.h"
#include "Arduino_APDS9960.h"

Color Sorting by AI vision module

“WonderCam.h” in vision module library needs to be imported, since colors are identified by camera vision.

#include "PID.h"
#include "ESPMax.h"
#include "Buzzer.h"
#include "WonderCam.h"
#include "SuctionNozzle.h"
#include "ESP32PWMServo.h"

2.2 Initialization

The next step is to initialize modules.

Color Sorting by Color Sensor

void setup() {
Buzzer_init();
ESPMax_init();
Nozzle_init();
PWMServo_init();
Valve_on();
go_home(2000);
Valve_off();
delay(100);
SetPWMServo(1, 1500, 1000);
Serial.begin(115200);
Serial.println("start...");
if (!APDS.begin()) {
Serial.println("Error initializing APDS-9960 sensor.");
}
ultrasound.Color(255, 255, 255, 255, 255, 255);
}

Color Sorting by AI vision module

void setup() {
cam.begin();
Buzzer_init();
ESPMax_init();
Nozzle_init();
PWMServo_init();
Valve_on();
SetPWMServo(1, 1500, 1000);
Valve_off();
setBuzzer(100);
Serial.begin(115200);
Serial.println("start...");
cam.changeFunc(APPLICATION_COLORDETECT);}

2.3 Color Detection

Following, detect the color of block. These two games adopt different modules to recognize color. As a result, the way to acquire and process color is different. Analysis of color detection code is given below.

Color Sorting by Color Sensor

The program calls “APDS.colorAvailable()” function of color sensor library to detect RGB value of the object. Then compute and process the value.

while (!APDS.colorAvailable()) delay(5);

Next, define four variables r, g, b and c to store the obtained color channel value. After that, read color value through “APDS.readColor()”.

int r, g, b, c;
APDS.readColor(r, g, b);

Narrow down the range of the obtained color channel value through mapping operation based on “map()” function. The final range is “0-255”.

r = map(r, r_f, R_F, 0, 255);
g = map(g, g_f, G_F, 0, 255);
b = map(b, b_f, B_F, 0, 255);

Compare the obtained color values, after which copy the value to C variable to output. Different color channel values correspond to different color.

if (r > g) c = RED;
else c = GREEN;
if (c == GREEN && g < b) c = BLUE;
if (c == RED && r < b) c = BLUE;

Color Sorting by AI vision module

WoderCam vision module has color recognition function. Before detection, let WonderCam learn red, green and blue blocks separately to ID1, ID2 and ID3 under color recognition mode.

Then, use While statement to constantly update the color data and coordinate detected by vision module.

while (true) {
int color_x = 160;
int color_y = 120;
cam.updateResult();
if (cam.anyColorDetected()) {
WonderCamColorDetectResult p;
if (cam.colorIdDetected(1)) {
cam.colorId(1, &p);
color_x = p.x; color_y = p.y;
angle_pwm = 2100;
place[0] = -120; place[1] = -140; place[2] = 85;

} else if (cam.colorIdDetected(2)) {
cam.colorId(2, &p);
color_x = p.x; color_y = p.y;
angle_pwm = 2300;
place[0] = -120; place[1] = -80; place[2] = 85;
} else if (cam.colorIdDetected(3)) {
cam.colorId(3, &p);
color_x = p.x; color_y = p.y;
angle_pwm = 2500;
place[0] = -120; place[1] = -20; place[2] = 85;
}

2.4 Set Feedback

After color detection and recognition, set recognition feedback before sorting.

During color sorting game, robot detects colored blocks ahead through ultrasonic sensor to get the distance from it. Once the distance is within the set distance, robot arm is controlled by inverse kinematics to sort blocks according to color.

But, the sorting part of color tracking and sorting game is activated only when the condition that the coordinate of the block stops changing during tracking, is satisfied.

The following content analyzes the recognition feedback codes of these two games.

Color Sorting by Color Sensor

Use “GetDistance()” function of ultrasonic sensor library to get the distance between robot and colored block. To avoid wrong recognition, the program will detect the distance five times in each round, and take the average.

for (int i = 0; i < 5; i++) {
distance += ultrasound.GetDistance();
delay(100);
}
int dis = int(distance / 5);

Then set a distance range. When the detected distance is within this range, buzzer on robot beeps, then robot arm starts sorting.

if (60 < dis & dis < 80) {
if (detect_color) {
setBuzzer(100);

Color Sorting by AI vision module

As tracking is achieved by pid algorithm, you need to set pid parameter(proportional, integral, and derivative) as pictured.

arc::PID<double> x_pid(0.045, 0.0001, 0.0001);
arc::PID<double> y_pid(0.045, 0.0001, 0.0001);

Besides, X-axis and Y-axis coordinates need to be set, because its motion in the direction of X, Y and Z axes is limited.(It only tracks the target along X and Y axes)

if (pos[0] > 100) pos[0] = 100;
if (pos[0] < -100) pos[0] = -100;
if (pos[1] > -60) pos[1] = -60;
if (pos[1] < -240) pos[1] = -240;

Next, set X-axis and Y-axis coordinates of the center of vision module screen, which is used to subtract the coordinate of block center so as to get the distance between vision module and the block.

int color_x = 160;
int color_y = 120;

Later, AI vision module saves the obtained coordinate of block center to these two variables, “color_x” and “color_y”.

color_x = p.x; color_y = p.y;

To obtain the distance between the block and screen center, subtract X-axis coordinate of the vision module screen center from X-axis coordinate of the color block.

if (abs(color_x - 160) < 15) {
color_x = 160;
}
x_pid.setTarget(160);
x_pid.setInput(color_x);
float dx = x_pid.getOutput();
pos[0] -= dx;

Y-axis coordinate is solved in the same way as pictured.

if (abs(color_y - 120) < 10) {
color_y = 120;
}
y_pid.setTarget(120);
y_pid.setInput(color_y);
float dy = y_pid.getOutput();
pos[1] -= dy;

After solving the coordinate of the colored block on the screen, substitute this value into the inverse kinematics function to achieve tracking.

set_position(pos, 50);

The first parameter “pos” represents the array “pos[0]” “pos[1]” that is the difference between the coordinate of vision module screen center and colored block center.

The second parameter “50” refers to motion time in ms.

As long as the coordinate is substituted to the above function, robot arm moves to the corresponding position.

If the colored block keeps moving, its coordinate changes so that the difference between its coordinate and the coordinate of vision module screen center also changes. Following, parameters of the above function will also change, which makes robot arm keep moving with the colored block to realize tracking.

2.5 Color Sorting

Sorting part of these two games are implemented by inverse kinematics, and analysis of the corresponding codes is as below.

Color Sorting by Color Sensor

In case the distance from the colored block detected by the ultrasonic sensor is from 60 to 80mm, robot arm starts sorting.

As before, robot arm is controlled by inverse kinematics to execute sorting. Robot arm moves above the block first.

pos[0] = 0; pos[1] = -160; pos[2] = 100;
set_position(pos, 1500);

These values, “pos[0] = 0; pos[1] = -160; pos[2] = 100” refers to the coordinate that robot arm needs to reach above the block. The program keeps these values in an array, then assign them to pos sp as to set the coordinate.

Then, remain X-axis and Y-axis coordinate the same, and change Z-axis from 100 to 85 to lower down robot arm.

pos[0] = 0; pos[1] = -160; pos[2] = 85;
set_position(pos, 800);

Next, start air pump through “Pump_on()” to enable the suction nozzle to suck the block.

Pump_on();

After that, change Z-axis value i.e. “pos[2]” from 85 to 180 to lift the robot arm.

pos[0] = 0; pos[1] = -160; pos[2] = 180;
set_position(pos, 1000);

Next step is to sort and place the blocks to corresponding area based on color. Robot arm moves above the placing area first.

pos[0] = x; pos[1] = y; pos[2] = 180;
set_position(pos, 1500);

“pos[0] = x; pos[1] = y” is the coordinate of the placing area. Block in each color has its own destination, and the coordinates of the area where red block, green block and blue block are placed are as follow.

Red:

x = 120;
y = -140;
z = 85;

Green:

x = 120;
y = -80;
z = 85;

Blue:

x = 120;
y = -20;
z = 82;

When the block is transferred above sorting area, set the angle of suction nozzle i.e. compensation angle through “SetPWMServo()” function to enable robot arm to place the block in a right direction.

SetPWMServo(1, angle_pul, 800);

The first parameter “1” represents servo ID. ID of servo on suction nozzle is 1.

The second parameter “angle_pul” refers to the pulse width of the servo, which is defined during initialization. Compensation angle of red block is 2200, green block is 2000 and blue block is 1800.

The third parameter “800” represents the time in ms taken for servo to rotate.

After the block is put down, turn off the air pump with Valve_on() function

Valve_on();

Next, increase Z-axis value i.e. “pos[2]” to raise robot arm.

pos[0] = x; pos[1] = y; pos[2] = 200;
set_position(pos, 1000);

Lastly, close electromagnetic valve to reset robot arm. Following, next round of detection starts.

Valve_off();
go_home(1500);
SetPWMServo(1, 1500, 800);

Color Sorting by AI vision module

The sorting process of color tracking and sorting game is similar to that of color sorting. The main differences are condition that triggers sorting and some parameters.

When vision module detects the coordinate of colored block stops changing, sorting function is activated.

if ((abs(dx) < 0.1) & (abs(dy) < 0.1)) {

To avoid wrong recognition, the program continuously detects ten times, and then start a new round of detection.

i ++;
if (i > 10) {
i = 0;

The name of compensation angle of suction nozzle is different. It is called “angle_pwm”.

SetPWMServo(1, angle_pwm, 800);

Furthermore, the coordinate of robot arm when it stops above block is not fixed. Only when the coordinate of block stops changing, the final coordinate of robot arm can be determined, which can be calculated by pid algorithm.

float d_x = pos[0] / 2.3;
float d_y = 68 - abs(d_x / 3);

Then substitute the result of “d_x” and “d_y” to inverse kinematics function to control robot arm to move above the colored block.

pos[0] += d_x; pos[1] -= d_y;
set_position(pos, 1000);

The steps to pick and place blocks as well as call functions are consistent with those of color sorting. However the coordinates of the area where blocks are placed are different.

Red:

place[0] = -120; place[1] = -140; place[2] = 85;

Green:

place[0] = -120; place[1] = -80; place[2] = 85;

Blu:

place[0] = -120; place[1] = -20; place[2] = 85;