Introduction: LEGO MINDSTORMS EV3 Color Sorter Programming Using MATLAB & Stateflow
This project was inspired by the LEGO® MINDSTORMS® Education EV3 Color Sorter presented on the LEGO Education UK Youtube chanel. Small modification to the building instruction was made to automatically detect colored bricks in the Color Sorter’s brick container. In this project, we will show how to build modified Color Sorter, test sensors, motors, design several control strategies in the Stateflow to show multiple options in Color Sorter operation. We assume that you know how to communicate with LEGO® MINDSTORMS® EV3 hardware, build model and use External mode in Simulink. If not please read this tutorial. We also assume you have basic knowledge of Stateflow.
You can download all Simulink models from MathWorks File Exchange. You can request 30 days MATLAB trial via MathWorks website as well.
Step 1: Building Instructions
The building instructions for the LEGO MINDSTORMS EV3 (45544) Color Sorter model can be found and downloaded online. For example, they can be downloaded from the Robot Square web page. We made small adjustment to color sensor position, because we wanted to automate color detection for the bricks in the container. Construction for our sensor holder is in the pictures above.
Step 2: Preliminary Test
It is a good practice to test communication and functionality of the sensors and motors before designing control. The external mode in Simulink provides functionality to write/read values to/from the LEGO EV3 in real-time. Build the Simulink schema with 2 motors, 2 encoders, color sensor and touch sensor as you can see in the image and run simulation in the external mode. Motor speed for portal and pushing mechanism can be set to chosen speed and test with manual switch. To change directions just change speed sign. Encoder values will be provided after motor movement. You can test touch and color sensor as well. In the figure you can see that we had red brick in sorter and touch sensor was pressed.
Step 3: Moving to Initial Position - Homing
Homing is the process of moving brick container to the initial position. The best position for the brick container is on the left side at the start. Portal should press touch sensor in this position. The control logic for this type of application is based on the events, because we don’t know exact time when the touch sensor will be pressed. This type of event driven control logic is typically build in Stateflow chart. Input to he Stateflow chart will be touch sensor and synchronization clock of 0.1s (Pulse generator, Period 0.2, Pulse width 50%). Output will be motor speed and we can set signal to the user, which can be 1 if the brick container is successfully in the initial position and 0 otherwise.
Core of the homing logic is in the Stateflow chart. Clock signal is added as event (in the Model Explorer), which is triggered by rising and falling trigger with pulse generator signal. Input to the Stateflow is touch sensor (variable touch) and outputs are data for motor speed and homing signalization (variables motor_belt and home). Logic starts with brick container moving to the left side until touch sensor is pressed. After that, movement of the brick container is stopped.
Homing can be preformed for the pushing mechanism as well. For the lack of reasonable sensor we decided to do this manually. We assume that rotating brick in pushing mechanism is parallel to conveyor belt at the start. You can use time-based logic for the pushing mechanism, but you can damage motors or bricks, so be carefull when try to do this.
Step 4: Simple Control Logic
Homing will move brick container to the initial position, but brick container will stop there. The next step is to design simple control. This control will move brick container to position based on the angle provided by motor encoder and push brick out from the brick container. Simulink model now contains Stateflow chart, motors, encoder, touch and color sensor.
Stateflow chart has now two parallel states (dashed borders, decomposition can be changed to parallel by right clicking on parent state). Control state waits for the homing to finish and if color sensor detects color other than black brick container should start to move. First we must remember position from motor encoder and then we will move portal until motor encoder gains another 180 degrees. After that, brick container will push brick out. Next, the signal (pusch_done) is send to homing state to move brick container back to homing position to repeat the control cycle if another brick is in the brick container.
Step 5: Color Sorting
The simple control logic can push brick only at one position. To sort colors we must build more complex model. Additional inputs to he Stateflow logic provide more options to users. First two additional inputs are for speed of the brick container and pushing mechanism. Next four are positions of the colored cups. Benefit of the External mode in Simulink is interaction with model and possibility to change these parameters during execution.
Control logic for color sorting extends simple control logic. Logic starts with homing because we must set starting angle in the initial position. If sensor detects colored brick in the brick container, control logic determine offset from initial homing position based on the color - our next setpoint. If setpoint is on the right side to the brick containers actual position we move to the right otherwise we move to the left. Movement stops and brick is pushed out of the brick container once the actual setpoint is reached. If there is another colored brick available in the portal stack we repeat control logic without homing. If the brick container is empty homing will be executed and movement stops.
Step 6: Graphical Function
Control logic for the Color Sorter contains many states and transitions. More flexibility to the model can be added via Stateflow’s graphical function. For example, if we like to add another color to our control logic we would have to expand control states. This would be probably harder to read. To be more transparent, we can create a graphical function for offsets setting based on the brick colors.
Step 7: Summary
We built the Color Sorter machine, Simulink model and control logic in Stateflow. The Simulink Support Package for LEGO MINDSTORMS EV3 Hardware was used to read sensor data and set motor values in the External Mode. Starting with simple model, we built control logic for Color Sorter homing to determine starting position. Based on homing logic model was extended to simple model, where we pushed all brick at same position. Color sorting model was built to sort colored brick into several cups. With Simulink and External mode we can change speed of the portal and position of the cups where we want to push bricks out. To simplify model we build graphical function in Stateflow.