Robotic Candy Sorter

Software Implementation   top

The main execution flow of the robotic arm is handled on the Raspberry Pi. The RPi allows for general purpose input and output peripheral ports as well as the ease of development on Linux operating system. As a result, RPi handles the high-level sequence of execution code as well as the integration with the PiCamera for image processing. Once the image processing section of the FSM is completed, the RPi initiates sequence of sort function that detects position of the object and requests sequence of motor control tasks to the PIC32 microcontroller via serial communication protocol. This structure demonstrates high level of abstraction on the RPi control scheme because the motor control commands is abstracted to PIC32, and RPi can proceeds to the next stage of the FSM.

FSM on the Raspberry Pi

The Finite State Machine is what is executed in the main while loop of the python script for the Robotic Candy Sorter. The FSM has 5 different states: IDLE, INIT, DETECT, MOVE TO, MOVE OBJECT. What is executed and transition between the states are summarized in the illustration below.

Figure 7: The Finite State Machine.

In the IDLE state, the script throws a blocking function call to receive a valid input from the user keyboard. If the character input matches to the list of provided sortable colors, FSM advances to the INIT state. In the INIT state, instance of robot arm is created as well as initializing the serial interface through the PySerial library. In addition, one frame of input image is processed to create list of starburst objects to determine how many objects are in the visible sorting field. Each starburst object has variables of color, position, and isSorted boolean to facilitate the sorting and tracking procedure. The DETECT state is where bulk of image processing is done to determine the updating position of the robot claw as well as the position of the targeted starburst. To make the control robust to outlier or dropped instance of a frame, we utilize a median filter of size 5 for the detected position. It leverages the history of positions from the past 5 frames to make the control scheme more error-proof. If the euclidian distance from the claw and targeted object is less than specified radius of 10 pixels, the FSM transitions to the MOVE OBJECT state. If the distance is greater than the radius, the FSM transitions to MOVE TO where the arm will take an incremental step in the calculated delta X, delta Y direction. Lastly, once the arm has taken sufficient incremental steps to the targeted object, it will transition to the MOVE OBJECT state, in which the robot will execute sequence of predefined movements to close the claw, pick up the starburst, go to the sort basket, drop the starburst, and return back to default position. Once completed, the FSM returns back to the IDLE state where the user can input another request of sort.

Object Detection on Raspberry Pi

The object recognition is handled by utilizing OpenCV library in Python on the RPi. The PiCamera library interface is enabled through importing the 5 MB picamera raw RGB input to numpy 2D arrays to provide the Mat structure commonly utilized for OpenCV. Once the RGB frame from the PiCamera is captured into a matrix format, image operations from OpenCV can be easily utilized to ultimately recognize colored starburst objects. The image processing pipeline is as shown below.

Figure 8: Object Detection sequence.

We process the raw captured RGB input into the HSV color scheme. Operating in HSV color scheme provides significant benefit over the RGB color scheme such as isolating the desired color into light and value intensity. Bounding the lower and upper bound range of a particular color value is narrowed to one variable rather than three variables of RGB in raw input color scheme. This isolation also provides robustness to fluctuation in lighting condition. The next matrix operation is to threshold by flooring or ceiling pixel values into a binary matrix is within the desired HSV color bounds for the desired color inputs of red, yellow, and green.

To determine the position of the claw, we tagged the tips of the claw with green markers. The position of the claw is determined by averaging the two centers of mass of the green objects. The claw position is only updated when two contours of the green object is clearly captured.

The threshold outputs corresponding to the claw and the targeted starburst are then de-noised with a morphological filter that erode and dilate the binary matrix. The morphological filter resulted in a better contour detection over alternative filtering schemes such as traditional median or Gaussian filter with a 5x5 window. This is because the resulting binary matrix from eroding and dilating function maintains the edge while filling in the non-connected values in the surrounding body.

We observe the greatest benefit of utilizing OpenCV library with the provided contour detection function. With the given input of binary matrix, the contour detection function can result list of bounded objects. This list may contain spurious detected objects from remaining noise or even sometimes detect region of one starburst as duplicate contours. To prevent these cases, we filter out contours by size and shape. If the detected contour is less than specified pixel area, than it is not determined as a starburst. In addition, because starbursts have a regular ratio between width and height, we can further filter our contours to isolate detected instances that are starburst size and shaped. The sequence of the described image matrix operations can be illustrated in the figure below.

Figure 9: Raspberry pi screen capture with code running.

To facilitate the debugging and development purpose, we overlay the detected contour and its center of mass onto the raw RGB capture frame. In addition, we print out all relevant variables onto the terminal. As shown in the figure, the terminal output captures current state, detected delta X,Y positions between starburst and claw, and the euclidian distance. The state transition is clearly indicated to incremental hover towards the targeted starburst until the calculated distance is within grabbing distance, and then initiates the sequence of sorting movements.

Serial Communication

As mentioned above, the PySerial library was used to send and receive data from the RPi to the PIC32. A command language was developed to standardize how the data was sent. 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 can be used to initialize and control the arm.

PIC32 Code

Figure 10: The Base angle definition.

Figure 11: The Shoulder and Elbow angle definition.

Figure 12: The Claw angle definition.

To begin discussing the PIC32 Code we must first define how each joint angle is defined and then go over how this is transformed into cylindrical coordinates with inverse kinematics. Since our arm has 3 DOF and a claw their 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 coordinate (theta, r, z, claw) and determine what joint angles would get the arm there. 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 in that it is the internal angle between the center of the claw and the gripper position and is thus the claw coordinate.

Figure 13: The Inverse Kinimatic System.

The final two joints, the shoulder and the elbow were the most difficult as the spatial coordinates that they need to produce, z and r, require simultaneous movement of the joints. The inverse kinematics is performed by solving the triangle formed by the forearm, bicep and the line formed by the desired r and z coordinates. Then through the law of cosines and more trigonometry each joint angle can be solved. With the shoulder angle being the angle of elevation of the bicep and the horizontal, and the elbow angle being the angle of declination between the forearm and the horizontal. The math produced a few equations which were converted into efficient c code to be ran on the arm.

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 which houses all of the structs to package all of the joint and servo values. Additionally, scaling.h and scaling.c were written to hold helper functions that were used to convert and map coordinate systems. The user input is handled in the parse.h and parse.c files. It is their job to have a function handler for each command that first verifies the validity of the command range and then sets the appropriate value. To know what the limits of the arm were, 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 inbounds, 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 next part and that the code was easily written into threads.. This became very useful when working with the inverse kinematics as it allowed us to verify that the math worked and that the arm would not try to destroy itself. To help protect the arm, conservative limits were placed on the ranges of each coordinates which limited what the arm could do but was not a problem in our setup. 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.