GRAPHICS & VISIONDECEMBER 2017

ISSUE 329CIRCUIT CELLAR | ISSU

E 329 | DECEMBER 2017

circuitcellar.com

EMBEDDED TECH EMPOWERS WEARABLE DEVICES

Building a Robotic Candy Sorter ARTICLE EXCERPT

Appeared in the December 2017 Issue of Circuit Cellar on page 10.

Inspiring the Evolution of Embedded Design

SPECIAL FOCUS

GRAPHICS

CIRCUIT CELLAR • DECEMBER 2017 #3291010

This pair of Cornell graduates designed and constructed a robotic candy sorter. The design exemplifies the value of partitioning an embedded system project into separate low-level and high-level subsystems.

F or picky eaters like us having candy sorted by color makes snacking much more enjoyable. To assist with this, we designed and constructed a candy sorting machine. It’s two main parts are a three-degrees-

of-freedom (DOF) robot arm and a vision system (Photo 1). A Microchip PIC32 microcontroller handles the controls and inverse kinematics for the arm. A Raspberry Pi module operates a camera that identifies candy and commands the robot arm to sort it. Our design worked well and provided delicious results.

Our robotic candy sorter system is an interdisciplinary project that integrates mechanical, electrical and software solutions to accomplish a task that requires a combination of tiered embedded systems. Figure 1 displays an overview of the project. At its heart, the machine combines a low-level system and a high-level system. The low-level system uses the PIC32 MCU to handle the real-time requirements associated with generating stable PWM signals to control the motors. Meanwhile, the high-level system has a Raspberry Pi using its additional horsepower to handle the computer vision work. These systems are fused through a serial link. This embedded teamwork aligned nicely with our academic purposes as well: It was our final project for Cornell University’s Microcontrollers and Embedded Operating Systems courses.

HARDWARE IMPLEMENTATIONWe put a lot of work into designing this project to be

affordable, which was a requirement of the class. Instead of implementing a custom robot arm design, we chose to go with the popular open source MeArm 1.0 design. The only

Building a Robotic Candy SorterPIC32 and Raspberry Pi Team Up

By Peter Slater and Mark (Moonyoung) Lee

PHOTO 1The candy sorting machine’s two main parts are a three-degrees-of-freedom (DOF) robot arm and a vision system.

circuitcellar.com 11

custom design modification of the MeArm we did was improving the stability of the base. The original design left a large gap between the fixed base and the rotating piece. This meant that whenever the arm would move, there would be additional deflection caused by the gap. To fix this instability, we designed a two-piece spacer system that is inserted in the base to prevent it from tipping when the arm extends. This greatly improved the rigidity of the entire arm. That made testing easier because the moves were much more repeatable. The DXFs used to laser cut the spacers are included on the project’s Github page.

The goal of the base design (Figure 2) was to protect and display the electronics while also enabling us to mount the camera. The base uses a jig-saw and t-nut pattern to allow the acrylic pieces to be assembled with minimal effort. This also produces a rigid column that was needed to support the cantilevered camera mount. All of the CAD work was done in Autodesk Inventor. The drawings were exported as PDFs so they could be laser cut at Cornell’s Rapid Prototyping Lab (RPL). More information about the RPL can be found on its website (www.cornellrpl.wixsite.com/cornellrpl).

The design of the camera mount was the hardest part of the mechanical design. We wanted to have a system that had flexible mounting options so that we could adjust the camera to get the perfect angle and maximize field of view. We ended up mounting the camera in parallel slots so it could be moved in and out to adjust the center of the image. Spacers were inserted under the camera to cancel out the tilt caused by the deflection of the cantilever. Finally, the tower was made slightly taller than it needed to be so we’d have room to adjust the range of the image.

The design of the PCB (Photo 2) was very simple as it only needs to breakout the connections and distribute power. The schematic is shown in Figure 3. Since the PIC32 runs from 3.3 V, a high power 5-V

source was needed for the servos. This was done by using an external DC power supply and running out a 2.1 mm barrel jack to hook up the power. Meanwhile, breakouts were made for the LCD, serial UART and PWM outputs. Finally, four potentiometers were included to give us the ability to control the arm without a computer. All the electronics and hardware were easily assembled. We then could quickly transition to writing the software. We would like to thank the Cornell Maker Club for letting us use its PCB mill to manufacture our boards.

SOFTWARE IMPLEMENTATIONThe Python code we wrote for the

Raspberry Pi is summarized in the finite state machine depicted in Figure 4. In

HD Camera

RGBInput PIC32

InverseKinematics

PWM

Robotic armRaspberryPi

FSMVision

Pixel X, Y

Claw coordinatesObject coordinates

FIGURE 1This project's block Diagram shows the major systems, their tasks and what information is passed between them. The camera is connected to the Raspberry Pi to leverage its computing power for the computer vision tasks. The robot arm is connected to the PIC32 since it has the better timers for the PWM signals. The two embedded systems communicate over a serial connection.

FIGURE 2Shown in this CAD rendering of the base are the tower for the electronics and the cantilever for the camera. It also shows the bins designed for the sorted candies. The base shape was decided by the reach of the arm with the goal to fully enclose its motion within the base. This was done for safety reasons.

CIRCUIT CELLAR • DECEMBER 2017 #3291212

the IDLE state the code prompts the user for which candy should be sorted. After the input is accepted and verified, the code then commands the robot arm to be initialized. The camera is used to capture and identify all the candy initially on the board and put their locations into a list. The code then uses user input to select a candy from this list and move on to the DETECT stage. Here the camera is used to find the position of the arm’s claw and then send an appropriate command to move the claw closer to the target candy. Once the claw gets within an acceptable radius— 1 mm—above the center of the candy, the arm drops down and to pick it up. Then the candy is sorted into the correct color bin. Afterward, the code returns to the IDLE state and awaits a new command.

To detect the claw and candy, our

algorithm uses the Raspberry PiCam and the OpenCV library for Python. Our algorithm is summarized in the Figure 5 diagram. Our first step was to convert the color scheme and apply a binary threshold to only see the color pixels that are close to the desired object color. This makes the next step faster—to apply a morphological filter to idealize the shape of the color blob to prevent multiple edges from being detected. The details of the morphological filter are as follows.

First, the image is dilated to fill in the unwanted color voids. Then it is contracted to maintain the scale of the object. That was a critical step because when the candy had a wrapper, the printing on the wrapper caused problems. The logo was being detected as a separate candy. Our morphological filter did a lot to help with this, but it was still too unreliable. We choose to go ahead and unwrap the candy. This created a consistent color blob that was easier to detect. The next part was to take the color blob and find the rectangular approximation of its bounds. From this, we could find the object’s center and verify that its base-to-width ratio was appropriately close to that of the candy’s. These centers were then used to form the candy list and update the measured position of the claw.

The PySerial library was used to send and receive data from the Raspberry Pi to the PIC32. We developed our own command language to encode and standardize how data was sent over the serial interface. Each command is in the format of a character command letter and a float value. There are options to set the absolute Cartesian and cylindrical coordinates or to take steps in any direction. A Python module was written to abstract this language to a set of function calls that could be used to initialize and control the arm.

CONTROLLING ARM MOVEMENTSTo begin discussing the PIC32 code,

we must first define each joint angle and then determine how this is transformed into cylindrical coordinates with inverse kinematics (Figure 6). Since the arm has three DOF and a claw actuator, there are four joints to consider: the base, the shoulder, the elbow, and the claw. The goal of inverse kinematics is then to take an input cylindrical coordinate (theta, r, z, claw) and determine what joint angles would get the arm there. Cartesian input coordinates are first transformed into cylindrical in the standard way:

PHOTO 2Shown here is the PCB we designed and milled for the project. It is a carrier board for the PIC32—breaking out the power inputs and signal outputs to convenient connectors. The board was constructed on the PCB mill operated by the Cornell Maker Club.

circuitcellar.com 13

r x y= +2 2

θ = −tan 1 yx

Looking from above, the base angle is just the internal angle formed by the arm and the line extending from the center to the right.

If we let this line become the x-axis, then the base angle is simply the desired theta coordinate. The claw angle is defined in a similar way. It is the internal angle between the center of the claw and gripper position.

The final two joints—the shoulder and the elbow—were the most difficult as simultaneous movements are required to produce the last two coordinates: r and z. The inverse kinematics is performed by solving the triangle formed by the forearm, bicep

FIGURE 3As this final project schematic shows, there are separate sub-circuits for the power, the servos, the serial connection and a LCD. The schematic and PCB layout were drawn in the open source KiCad EDA software.

Dist < grabDistance

Dist > grabDistance

IDLE INIT DETECT Moveto

Moveobject

!sorted

Median filter position

validInput

!validInput

Initialize Arm,starburst object

FIGURE 4This Sorting Finite State Machine (FSM) is the main logic of the system. It is closed loop in nature to ensure that the arm and candy are properly aligned. This FSM is implemented in Python and runs on the Raspberry Pi.

CIRCUIT CELLAR • DECEMBER 2017 #32914

and the line formed by the desired r and z coordinates. Then—by the law of cosines twice applied on this triangle—each joint angle is solved. For that solution, the shoulder angle is the angle of elevation of the bicep and the horizontal plane, and the elbow angle is the angle of declination between the forearm and the horizontal plane. This math produced a set of equations that were converted into efficient C code to run on the arm. Listing 1 shows the section of the code that solves the inverse kinematics.

Code execution begins in the main function which makes calls to initialize and start the thread scheduler. To keep the code short, it was decided that the code needed

to be separated into modules. The first of which is types.h that houses all the structs to package the joint and servo values into one variable. Additionally, scaling.h and scaling.c were written to hold helper functions that convert and map coordinate systems. The user input is handled in the parse.h and parse.c files. Their job is to have a function handler for each command character that first verifies the validity of the command value in the range and then sets the appropriate input value. To know what the limits of the arm are, limits.h and limits.c were written to house the macro constants and functions for testing whether the move was in bounds. If the move was in bounds, the calculation thread calls the solve function in ik.h and ik.c to get the output servo angles. The output thread then uses arm.h and arm.c to first convert the angles to PWM values using the calibration data in calibration.h and then write the PWM data to the motors.

The main advantage of the modular design was that each module could be fully written and tested before moving onto the

ABOUT THE AUTHORSPeter Slater is an undergraduate student at Cornell University studying Electrical and Computer Engineering. His primary interests are controls, robotics and radio frequency systems. He can be reached at pas324@cornell.edu.

Mark (Moonyoung) Lee graduated from Cornell University and is developing humanoid platforms at KAIST, South Korea. His primary interest is in motor control and vision processing with embedded systems.

FIGURE 5This is the Object Detection Sequence in terms of computer vision tasks. The most important steps are the contour detection and moments used to find the shape and center of the targets. The threshold and morphological filters were added to simplify the raw input and focus on only the colors that could be the target.

Z Z

X

R

R

Y ForearmBICEP

θELBOW

θSHOULDERθBASE

FIGURE 6These are the angle definitions on the arm. Induced on the base is an inertial, Cartesian coordinate system (X, Y, Z). To simplify the calculations an additional cylindrical coordinate system is added (R, θBASE, Z). There are two more joint angles for the shoulder and elbow.

circuitcellar.com 15

void ik_solve(pos_t* pos, ang_t* ang){ /* test if pos is inbounds */ if(checkAll(pos) == INBOUNDS){ /* set the easy angles */ ang->bs = pos->o; ang->cw = pos->c; /* calculate the difficult angles */ float b = sqrt((pos->r * pos->r) + (pos->z * pos->z)); float P = atan2(pos->z, pos->r); float A = acos((-FRM_LEN_SQ + BCP_LEN_SQ + (b * b))/(2.0 * BCP_LEN * b)); float B = acos((FRM_LEN_SQ + BCP_LEN_SQ - (b * b)) / (2.0 * BCP_LEN * FRM_LEN));

float sh_rad = A + P; float el_rad = M_PI - B - A - P; /* set the difficult angles */ ang->sh = (180.0 / M_PI) * sh_rad; ang->el = (180.0 / M_PI) * el_rad; } else ang = &IK_OOB;}

LISTING 1The section of the code shown here solves the inverse kinematics. The desired position is given to the code that uses the law of cosines to solve for the angle outputs for each motor.

LCR-Reader Pro• 0.5% Basic Accuracy• LCR- and ESR-meter• NIST Traceable

Calibration

LCR-Reader-MP• L-C-R, ESR, Diode/LED• 0.1% Basic Accuracy• AC/DC Voltage, Frequency, Pulse Period, Duty Cycle• 100 kHz Oscilloscope

Shielded KelvinProbe Connector• Smart Tweezers/LCR-Reader• 5 Replaceable Probes

MADE IN CANADA

Smart Tweezers ST5S-BT• 0.2% Basic Accuracy• LCR, ESR, Q/D, Diode Test• NIST Traceable,

Blue Tooth

www.lcr-reader.com

LCR-Reader-MP• L-C-R, ESR, Diode/LED• 0.1% Basic Accuracy• AC/DC Voltage, Frequency, Pulse Period, Duty Cycle• 100 kHz Oscilloscope

CIRCUIT CELLAR • DECEMBER 2017 #32916

next part. It was also easier to write the code as threads. The threading library was an implementation of the protothreads library (www.dunkels.com/adam/pt/) with additions by Bruce Land. There were three threads used: the command thread to take user input, the calculation thread to process it and the output thread to send the results to the servos. The modules became useful when working with the inverse kinematics because it enabled us to verify that the math worked and that the arm would not attempt to destroy itself. To protect the arm, conservative limits were placed on the ranges of each coordinate. This limited what the arm could do, but this was not a problem in our setup.

TESTING THE SYSTEMThroughout the development of the

project, we leveraged a modular functionality, incremental development and test-driven approach. Given the extensive amount of mechanical, electrical and software work required to integrate the robotic arm, we initially defined the design components, the high-level requirements within each component and the dependencies between the subsystems. This approach allowed us to highly parallelize the development process. For instance, the vision and FSM code on the Raspberry Pi was developed in parallel with the motor control on the PIC32.

The precision of the robotic arm movement was a challenging aspect of this project. To overcome this hurdle, we introduced a vision-based corrective control so the system could use visual feedback to correct its path to the target. Finally, the color thresholding ranges for the vision as well as the material of object to sort had to be tested across various options. We initially started with Skittles as our candies to sort because they are tasty. However, they have a mechanical complexity due to their small, slippery, round surface. We instead resorted to an easier sorting candy: Starburst. Starburst candies have more

rigidity and a bigger size, but less favorable colors to detect.

We put a lot of thought into safety to protect our project and ourselves. The base is designed so that with the software limits the arm’s movement such that it can’t reach outside of the imposed limits or collide with itself. Because the acrylic that we used was not FDA approved for food handling or storage we did not eat the candy after the arm touched it.

RESULTS AND CONCLUSIONSWe were successfully able to implement

a vision-based Robotic Candy Sorter (see Circuit Cellar article materials page for a link to video). The user can provide the desired input color between red and yellow, and the robotic arm can locate the correct candy and sort it. We observed high consistency of correct color and position detection with our vision system. Under a fixed lighting environment, the camera would reliably detect the target candy. If a single frame couldn’t detect the target, the median filter of the previous five target positions would act as a buffer to ensure the robot arm has a constant trajectory to follow.

All that said, certain locations of the robot workspace still have different lighting conditions, resulting in the detected target position to be slightly off. In these situations, the robot arm is prone to undershooting the travel distance and dropping the Starburst upon grab. Quantitatively, we observed the robot arm’s movement to be repeatable within a millimeter. The claw’s success rate at grabbing was approximately 80% for a red Starburst and 90% for a yellow Starburst with no detection false positives. The yellow candy yielded higher success rate because the target is easily distinguished from the red acrylic of the robot arm. Our robot could sort a single candy very reliably in an average of 20 seconds, and tested to sort reliably with as many as five candy variations on the field at once.

While our project does not tackle a big issue, or fix a major life problem, we believe that it still has merit. It is an interesting and fun introduction into robotics and computer vision. Moreover, it can be repurposed as an attractive display to inspire the next generation of engineers.

For detailed article references and additional resources go to:www.circuitcellar.com/article-materials

RESOURCESMicrochip | www.microchip.com

Raspberry Pi Foundation | www.raspberrypi.org

Thingiverse | www.thingiverse.com