MAE576 GroupD Project Report

8/9/2019 MAE576 GroupD Project Report

http://slidepdf.com/reader/full/mae576-groupd-project-report 1/78

VISION BASED AUTONOMOUS BALL

SORTING ROBOTIC SYSTEM

Mechatronics 576Spring 2010

May 10th, 2010

“Group D”

Priyanshu AgarwalDipen Harishbhai Dave

Ravikiran Chollangi Jason Lieu

Department of Mechanical and Aerospace Engineering

State University of New York at BuffaloBuffalo, New York 14260



Abstract

Autonomous mobile robots capitalize on their ability to intelligently, efficiently and reliably interact

with their environment. This makes them suitable for various applications that are hazardous or

unpleasant for human beings. Their task may range from as simple as following a line, to as complex as

enduring the extreme environment of a distant planet, and thereby collect useful data for humans. Thisreport deals with an autonomous mobile robot guided by a vision system for sorting objects based on

their color. The system have autonomous mode, assitive mode and manual mode. In the autonomous

mode the robot and the vision system will work in unision to identify various objects (balls) and escort

them to their respective home positions based on their color. In the assistive mode, the vision system

assistis the user with a feedback and helps in capturing the ball and placing it at its home location. In the

manual mode the vision system provides a video of the robot workspace and the user can navigate the

robot to its home location. In all the above mentioned modes, the robot communicates with the computer

using bluetooth.



Contents

Abstract

Contents

1. Introduction1.1 Design Objective1.2 Features of DC Motor Control System1.3 System Architecture1.4 User Operation Instructions

2. Hardware2.1 List of Components2.2a Microcontroller (Master BASIC Stamp 2) Pin Assignment

2.2b Microcontroller (Slave BASIC Stamp 2) Pin Assignment2.3 Hardware Description2.4 Circuit Diagrams2.5 Mechanical Setup

3. Software3.1 PBASIC Commands3.2 Programming Challenges

4. Mathematical Methods4.1 On-Off Control4.2 Differential Gap Control4.3 Proportional Control4.4 PD Control4.5 PID Control4.6 PWM control

5. Implementation5.1 Software Algorithms

5.1.1 Controls Flowchart5.1.2 Auto Calibration Flowchart5.1.3 Master BS-2 Flowchart5.1.4 Slave BS-2 Flowchart

5.1.5 Open-Loop Control Flowchart



5.1.6 On-Off Control Flowchart5.1.7 Differential Gap Control Flowchart5.1.8 PID Control Flowchart

5.2 Subroutines

6. Calibration & Testing6.1 Digital Sensor Calibration6.2 Duty Cycle Vs Optical Encoder Count Calibration6.3 Testing6.3.1 Diagnostics6.3.2 Open-Loop Control6.3.3 On-Off Control6.3.4 Differential Gap Control6.3.5 Proportional Control6.3.6 PD Control6.3.7 PID Control

7. Discussion

8. Conclusion

Bibliography

Appendix

A. PBASIC Code for Speed Control System

A.1 Slave Micro-controller Code

A.2 Master Micro-controller Code

B. MATLAB GUI Code for Speed Control System



1. IntroductionAutonomous robots are often designed to handle repetitive and potentially dangerous tasks. In order tointeract with its environment effectively a mobile robot must sense and locate the various objects aroundit. Also, in order to locate itself in the environment it must be able to calculate its own position. Figure 1(a) and (b) show different ball handling autonomous mobile robots. Figure 1 (c) depicts a cameramounted robot for vision research.

(a) (b) (c)Figure 1 Autonomous robots for various ball handling and vision research applications a) The EBBmobile robot [1], b)Tekbot [2] and c) TJ Pro-Avr™ robot vision system [3]

However, there are various challenges involved when it comes to sensing the environment. A sensor might be as crude as a bump sensor, an ultrasonic sensor or a complex image capturing device. As thecomplexity of sensing graduates so the requirement of advanced hardware to process the sensed dataincreases. Typically with vision based navigation (onboard or environment based) the necessity of processing data on a separate processor becomes a necessity. This then require communication betweenthe autonomous agent and the remote processor for transmission of processed data (commands).

This report presents a problem that encompasses all the aforementioned challenges in the form of a

vision based autonomous robot that can sort objects on the basis of their color.

1.1 Design ObjectiveThe following are the design objectives for the developed Robotic System:(i) It should have an efficient and reliable algorithm to handle image processing in real-time.(ii) It should have a navigation algorithm such that the robot can track the balls successfully in real-

time.(iii)It should have the capability to calibration the system constants automatically.(iv)It should offer both the manual and autonomous mode to the user.(v) It should have the capability to support video in real-time for observation.(vi)It should be user friendly.

1.2 Features of Robotic SystemThis section describes the various features of the developed Speed Control System.

1. Auto Calibration – The system provides a feature of automatic calibration of the image processingconstants and the geometric constants of the robot. This feature becomes mandatory as the system performance vary significantly with environmental conditions especially the lightning condition of the robot workspace. The various features in this module are:



(i) Ball Area Calibration – The system has the feature of calibrating the ball area by simplyselecting the ball color from a drop-down menu and hitting the ball calibration button. Therobot workspace can contain any number balls of all the supported colors, but the system willidentify balls of chosen color and will update the area calibration constant for future.

(ii) Robot Marker Area Calibration – The robot has two circular areas which acts asidentification marks for the robot. This helps in calculating both the gripper position and therobot orientation. This calibration also helps in calculating characteristic length which is used

by the system for navigation control. At the same time the system also calculates the areacalibration constants for the two circular areas for future identification.

(iii) Error Message – The system also has the feature to successfully detect any mistake made bythe user in terms of missing ball or robot workspace having objects other than balls. Thisresults in an error message dialog box.

(iv) Constants Display – The system can also display all the ball constants and the robot constantscurrently being used for evaluation.

MATLAB GUI of the system asking the user to place ball(s) of red color in the robot workspace for areacalibration.



MATLAB GUI of the system asking the user to place the robot in the workspace for robot identificationmarks area calibration and geometric constant of the robot.

1. Modes – The system offers different modes for the user to effectively control the robot and sortingthe balls. The system currently supports the following modes:

(i) Manual Mode – In the manual mode the system receives the input from the user. Based theinput provided the robot simply performs the commanded move.

(ii) Assistive Mode – The Assistive mode of the system takes the move input from the GUI butcalculates the number of cycles based on the current robot and target location.

(iii) Autonomous Mode – The system has the feature of autonomously tracking the target basedon the image feedback without any user intervention.

1. Monitoring – The system also provides the feature of monitoring the actual system with the help of avideo from the webcam that is interfaced using MATLAB to the developed GUI. This

2. Message Window – The GUI also provides a small message window to let the end-user know

whether the system is performing an operation or has completed the assigned operation.

3. Dialog Box for Important Information and Error- The GUI provides important messages in the formof dialog boxes for user friendliness.

2.4 System Architecture



1.3 User Operation InstructionsThe following are the instructions for the user to operate the GUI of Smarty:

1. The GUI starts with the screen, with calibration controls in top left, mode selector below that and runand stop operations below that.

Welcome Screen of MATLAB GUI

2. The user can calibrate the bot with areas of balls and bot.3. Each color has to be selected sequentially and asked to be calibrated.

4. Bot can be calibrated in a similar manner 5. Constants button assigns predefined values to the areas6. Three modes of operation are offered, manual, assistive and autonomous.7. When using manual control, arrow keys are used to direct the bot. Enter key is used to bring the

gripper down and space key is used to bring the gripper up.8. Similar is the assistive mode9. In autonomous mode, no user input is required.10. Video button can be selected to get a live stream of video from the camera.

Asynchronous SerialCommunication

Colored

Image

Switch

PositionInput

PCW Modulation& Cycles

Duty Cycle&

No. of Cycles

Duty Cycle&

No. of Cycles

Asynchronous SerialCommunication

Switch Position





2. HardwareThe following sections deals with the details of the hardware used to implement the developed roboticsystem.

2.1 List of Components

S. No. Component Quantity1

1. BASIC Stamp Module II 12. Easy Bluetooth Module 13. Servo Motors 34. Gripper Mechanism 15. Wheels 26. Castor 17. IRF540N MOSFET 18. 10K Resistor 1

9. 1K Resistor 310. 220Ω Resistor 211. Plastic Balls NA12. Camera 113. AA Batteries

The total estimated cost of the system is around 500$ including both the kits.

2.2a Microcontroller BASIC Stamp 2 Pin Assignment (Onboard)

Pin No. I/O Description

0 Output Bluetooth Transmitter (Tx)2 Input Bluetooth Receiver (Rx)13 Output Gripper 14 Output Right Motor Control Pin15 Output Left Motor Control Pin

2.2b Microcontroller (BASIC Stamp 2) Pin Assignment (RF Communication)

Pin No. I/O Description0 Input Tracks forward switch

1 Input Tracks backward switch2 Input Tracks right switch3 Input Tracks left switch



2.2 Hardware Description

1. Basic Stamp Kit INEX-1000

http://www.mouser.com/catalog/supplier/library/ParallaxMiniCatalog.pdf

1. INEX-1000 is a high-quality prototype and experiment area for all 24-pin BASIC Stamp modules2. Audio amplifiers with screw terminals for 8 ohm speakers. an audio amplifier for external speakers

and current limit resistors sized for driving more LEDs. 510 ohm resistors allow more LED driving.3. Board provides socket ports for each BASIC Stamp I/O pin.4. A parallel LCD with cable.5. 16 LEDs to monitor I/O pin status.6. DB-9 connector for program download and debugging.7. 4-digit LED 7-segment display with common cathode.8. 8 pushbutton switches (active low without pull-up resistors) 8 DIP switches (with built-in pull-up

resistors)9. ULN2003 high-current driver for relay and stepper motors10. Pulse generator for 1Hz, 10 Hz, 100 Hz, and 1 kHz11. RS-232 interface port for communication with COM program12. Socket for 24-pin BASIC Stamp modules13. Parallel LCD module with connector and brightness control14. Piezospeaker 15. 10K potentiometer 16. 2.5" x 7" breadboard with 800 contact points17. 7.5V DC 1 amp power supply with polarity protectionSource: http://www.apexvalue.com/basic_stamp/product_id_28135.htm

2. Board of Education Development Board - Serial Version





http://www.parallax.com

Mechanically interlocked power supply to prevent dual connection of wall-pack and 9-volt battery

DB9 connector for BS2-IC programming and serial communication during run-time

P0 - P15 I/O pins, Vdd and Vss connections brought adjacent to 1.375 x 2 in ( 35x 51 mm) breadboard area

Female 10-pin dual row connector for optional AppMods

On-board regulator delivers up to 1 amp of power for larger projects

Jumper selection of servo power: regulated (Vdd) or unregulated (Vin)

Three-position power switch allows various powering options for programmingwithout providing power to servo connectors

Key Specifications:

Power Requirements: 6 to 9 VDC

Communication: Serial for Programming

Dimensions: 3.75 x 2.75 in (95 x 70 mm)

Operating Temperature: +32 to +185 °F (0 to +70 °C)

Source: http://www.parallax.com

3. Basic Stamp II Module

Microcontroller PIC 16C57 C signifies EPROM program memoryProcessor Speed Max Operation Frequency 20 MHz

Program Execution Speed ~4,000 PBASIC instructions/secCurrent Draw @ 5 VDC 3mA Run, 50 μA Sleep

Package 28-pin DIP

http://www.parallax.com/






Max Source / Sink Current by any I/O pin 20mA/25mAEEPROM (Program) Size: 2 Kbytes ~500 PBASIC instructions

RAM Size: 32 Bytes (6 for I/0, 26 for Variable)

4. Easy Bluetooth Module:

The module has two parts, the RBT-001 module and the SIP with voltage regulator PCB.

With the on-board regulator, the module can be connected to voltages higher than 3.3VDC, such as the Board of Education regulated supply (+5 VDC) without worry of damaging the unit; while the RX and TX can utilize serial communication at CMOS andTTL levels.

Features 1.x & 2.0 Bluetooth Compliant Class 2 Operation (nominal range up to 30 meters) 10-pin SIP package for breadboard, perfboard, or Board of Education AppMod

Header On-board regulator for safe operations across various voltages

CMOS & TTL Compatible

Key Specifications Power requirements: 3.3 to 5.5 VDC Communication: UART Command/Data Port supports for up to 921.6k baud Operating temperature: +32 to +113 °F (0 to +45 °C) Dimensions: 1.40 x 1.79 x .49 in (34.41 x 45.65 x 12.51 mm)

5. Servo Motor:

a. Standard Servo (180o)



http:www.parallax.com

Features:

Holds any position between 0 and 180 degrees

43.1 oz-in torque at 6 V

Accepts four mounting screws

High precision gear made of the POM (polyacetal) resin makes the operation

smooth causing no backlash.Key Specifications:

Power Requirements: 4 to 6 VDC*

Communication: Pulse-width modulation

Dimensions: 2.2 x 0.8 x 1.6 in (55.8x 19 x 406 mm) excluding servo horn

Operating temp range: +14 to +144 °F (-10 to +50 °C

b. Continuous Rotation Servo:

http:www.parallax.com

Features:

Bidirectional continuous rotation

0 to 50 RPM, with linear response to PWM for easy ramping

Accepts four mounting screws

Easy to interface with any Parallax microcontroller or PWM-capable device

Very easy to control with PBASIC's or SX/B's PULSOUT commands

Key Specifications:

Power requirements: 4 to 6 VDC*

Communication: Pulse-width modulation



Dimensions: 2.2 x 0.8 x 1.6 in (55.8x 19 x 406 mm) excluding servo horn

Operating temp range: +14 to +122 °F (-10 to +50 °C)

USB Webcam:

Features:1. Plug and play, no driver necessary.2. High Resolution Video Quality.3. High Speed USB 2.0 Interface.4. Frame Rate upto 30fps.5. Optics: Manual Focus

Wheels:

Specifications:1. Diameter – 50 mm apporx

Castor:

Specifications:1. Diameter – 20 mm approx



2.4 Circuit DiagramsRobot Interface Circuit (Bluetooth Module & Motors)

RF Communication Circuit



Remote Layout for RF Communication

Complete Circuit Diagram for Developed System



B l u e t o o t h

D o n g l e

( L a p t o p )

B l u e t o o t h

M o d u l e

T x

R x

0 2

1 3

1 4

1 5

C o n t r o l

C o n t r o l

C o n t r o l

G r i p p e r

M o t o r

+

-

+

-

+

-

R i g h t

M o t o r

L e f t

M o t o r

R F

M o d u l e

B a s i c S t a m p 2

( R F

C o m m u n i c a t i o n )

1 0 K

1 0 K

1 0 K

1 0 K

3 2 1 0

5 V

3 –

T r a c k i f L e f t i s c o m m a n d e d f r o m R F R e m o t e

2 –

T r a c k i f R i g h t i s c o m m a n d e d f r o m R F R e m o t e

1 –

T r a c k i f B a c k w a r d

i s c o m m a n d e d f r o m R F R e m o t e

0 –

T r a c k i f F o r w a r d i s c o m m a n d e d f r o m

R F R e m o t e

1 2

4

3

1 –

F o r w a r d

2 –

B a c k w a r d

3 –

L e f t

4 –

R i g h t



2.5 Mechanical Setup

a. Robot

Gripper Motor

Gripper Motor

AA BatteriesCase

Aluminum Link

Aluminum Sheet



1. The Gripper motor is mounted on the flexi-glass sheet which is fastened to the Bot’s Base.2. A link is connected to the servo motor’s horn using nut and bolt arrangement. On the other side of thelink a rectangular aluminum sheet is attached as shown in the above figure.3. The bot is covered with a black box to match the background.4. Two white circles pasted on the bot are used to get its orientation.

b. Camera Stand:1. The arrangement is made using aluminums links.2. The camera is mounted on the horizontal link facing towards the arena.

c. Arena Setup:

Board Of Education

Bluetooth

Gripper Motor



1. The color of arena is black with elevated borders to prevent balls or bot from going out.2. Arena Size: 75cmx40cm

WebcamHolder Stand

Arena

Bot

Web Cam

Balls



3. Software

The following section presents a basic overview of the important commands used and major programming challenges faced while implementing the system.

3.1 PBASIC Commands

The PBASIC version 2.5 is used to program the BASIC Stamp II module [2]. The following specialcommands are used to program the module:

1. SERIN - Receive asynchronous serial data (e.g., RS-232 data).Syntax: SERIN Rpin {\Fpin}, Baudmode, {Plabel,} {Timeout, Tlabel,} [InputData]

2. SEROUT - Transmit asynchronous serial data (e.g., RS-232 data).Syntax: SEROUT Tpin {\Fpin}, Baudmode, {Pace,} {Timeout, Tlabel,} [OutputData]

3.2 Programming Challenges

The following are the challenges faced while implementing the system:

(i) Image Processing: The task of reliably detecting the balls of different colors under differing lightconditions was one of the major challenges. The balls are identified by using processing on channelsof different colors present in the captured image

(ii) Bluetooth Communication Error: The error or miscommunication between the robot Bluetoothmodule and the MATLAB remains a major challenge. There are instances when robot waits for thecommand from the MATLAB.

(iii)Navigation Algorithm: The task of successfully tracking the target, capturing it and placing it at thedesired location remains a challenge for certain scenarios.

(iv)RF Commnication: The task of picking the RF signal without error is a challenge.



OFFON

4. Mathematical Methods

4.1 Manual Mode (On-Off Control)

The manual control for the robot is based on on-off control.

For On-Off control, instantaneous value of the variable is measured and compared to thesetpoint. If the value is more than the setpoint, driving factor is decreased and drivingfactor is increased if the value is below the setpoint.

4.2 Autonomous/Assistive Mode (Proportional Control)In the proportional control algorithm, the controller output is proportional to the error

signal, which is the difference between the set point and the process variable. In other

words, the output of a proportional controller is the multiplication product of the error signal and the proportional gain.

This can be mathematically expressed as

POut= Kp .e(t)

Where

Pout: Output of the proportional controller

Kp: Proportional gain

e(t): Instantaneous process error at time 't'. e(t) = SP − PV

SP: Set point

PV: Process variable

We have used proportional control for controlling both the position and orientation of the bot. In case of position, SP is the position of the ball and PV is position of the bot. Pout isthe number of cycles of the motor required at each instant.In case of orientation control, SP is the angle made by the line connecting big circle on bot and target and PV is the angle made by the big and small circle on the bot

Set-Point



(orientation of the bot).

dT= θrobot- θballθrobot=atan2 ybig circle- ysmall circle, xbig circle- xsmall circle

θball=atan2 ybig circle- yball, xbig circle- xball



5. Implementation

The following section details the algorithms used along with the various MATLAB and Basic Stampsubroutines used to implement them.

The following figure shows the manual mode RF communication implemented using NX-1000Experiment Board.

5.1 Algorithms

The following section presents the various algorithms used to implement different features of thesystem. The different algorithms used are as follows:

1. System Overview Flowchart2. Navigation Algorithm3. Image Processing4. Ball Area Calibration5. Robot Constants Calibration6. Manual Mode7. Assistive Mode

RF Joystick

NX-1000Board

RF Module



8. Basic Stamp-II (RF-Communication)9. Basic Stamp-II (Onboard)

5.1.1 System Overview Flowchart





1.1.2 Navigation Algorithm





1.1.3 Image Processing (Algorithm 1)

Algorithm in Matlab.1. Read the input image and display it.2. Convert the input RGB model to HSV model.3. Eliminate the reflection of light from ball’s surface i.e shiny spots.



4. Extract the Hue and Saturation Component’s from the HSV Model and store themseparately.5. Perform a gray-level slicing of the Saturation image.6. Set the minimum and maximum Threshold values as well the Sigma used for theLaplacian of Gaussian.7. Apply Canny Filter to the Hue Image and find the red and yellow balls boundaries.8. Apply Canny Filter to the Saturation Image.9. Inverting the Output of applying the saturation image.

10. Performing Morphological Operation ‘Opening’.11. Performing Morphological Operation ‘Closing’.12. Find the Blue and Green Balls boundaries.

Step 1 Step7

Step8 Step9

Step 10 Step11



1.1.4 Ball Area Calibration

1.1.5 Robot Constants Calibration





1.1.6 Manual Mode

1.1.7 Assistive Mode



1.1.8 Basic Stamp-II (RF-Communication)



1.1.9 Basic Stamp-II (Onboard)





5.2 MATLAB Subroutines

The following functions are used in MATLAB GUI for implementing various features:

(i) Ball Calibration Function – This function performs the calibration for a ball of given color andsets the area limit above which the ball will be detected.

(ii) Bot Calibration Function – This function performs the calibration for the area limit of whiteareas on the robot and gripper location. The distance between the two circles is treated as acharacteristic length based on which the other lengths in the workspace are decided. Based on thischaracteristic length and the distance of the bigger white area from the marked gripper center the robotratio is decided which is used to calculated the gripper location when the robot is tracking the target.

(iii) Mode Function – This function incorporates the algorithms for the different modes in the systemi.e. manual, assistive and autonomous.

(iv) Display Constants Function – This function displays all the important current calibration

constants of the system.

(v) Display Video Function – This function displays the video of the setup.



6. Calibration & Testing

6.1 Different Balls CalibrationThe calibration for balls is carried out to determine the constant like ball area. This was needed toaccommodate different sizes of same color balls. Also, the system now adapts to the varying lighting

conditions.

6.2 Bot Calibration:This function first bot ratio which is defined as the ratio of distance between the gripper center to thelarger circle on bot to the distance between the two white circles on the bot.The following steps are followed:1. Initially the system asks to place the bot in the arena. It checks any other objects or balls are placedalso and an error message is generated.

2. If it detects only the bot then the center for the white circles on the bot is calculated and thecorresponding distance is calculated. [Refer fig:]3. Now it asks to place the marker so that the user can select the gripper center and the distance requiredfor bot ratio is collected. [Refer Fig:]4. After successful calculations it displays the constants calculated.





6.3 TestingThe following section presents the testing of the various modes and calibration routines developed for the system:

6.3.1 Ball Calibration



Since, we have selected the calibration for blue while the system cannot find blue balls in the image itgenerates error “Calibration Failed for BLUE Ball, No BLUE Ball Present.”

6.3.2 Robot CalibrationThis is needed to check if the bot is lying in the arena. If it is out of arena or partially present it generatesan error [Refer Fig:]. Also, simultaneously it looks for other objects (other than blue, red white balls)and if they are present; if other objects are present it again generates an error [ Refer Fig:]. This makessure that only the bot is calibrated.



Fig

Fig:



6.3.3 Manual ModeHere, the Bot is controlled by the user. The user gives input from keyboard. Also, the bot can becontrolled from a RF remote.

6.3.4 Assistive ModeIn manual mode even though the bot can be controlled totally, it is very

difficult to precisely align the center of gripper with the center of the target ball. Because of this we have incorporated the assistive mode where eventhough the main control inputs are from the user i.e forward, backward, left or right, the assistive controller will determine the amount(duty cycles) requiredto move in that direction.



6.3.5 Autonomous Mode1. This is the totally autonomous mode, wherein the targets are selected andthe bot automatically sorts based on the algorithm.



7. Discussion

The system still has a scope for improvement. The following is a list of the features that can either improve the performance of the system or can be an added feature:

Image Processing: The algorithm developed for image processing is sensitive to differentintensities and uneven background. However suitable modifications in the algorithm cancounter these changes. The following is a list of the features that can either improve the performance or can be an added feature to the existing system:

• The threshold for red, green and blue channels of the input image changes with respect todifferent intensities of light/shading. A way to eliminate would be using the HSV color model instead of RGB. Since, the hue more or less remains the same in changingintensity levels. Although we made an algorithm considering the HSV model, it wasn’timplemented in the final system.

• Noise in background: To eliminate such noise we used a plain black background which

gave least noise changes. But in order to make it independent of the background color,

we can incorporate another process as follows. Take a picture of the current background

and store it in a variable. Now place the balls on the background, take the picture again

and store it. Subtract the previous image with the current image which eliminates the

background and only the colors of balls are left. This was also implemented one of the

algorithm, but wasn’t incorporated into the current system.

• A better resolution camera can be used for accurate image capture and analysis of balls.

• Histogram equalization and other morphological operations can be performed for accurate detection of ball’s location.

Bluetooth Communication: There seems to be error in communication between the Bluetooth dongleand the Bluetooth module on the robot. This can be avoided by using synchronous communication.

Navigation Algorithm: There is a lot of scope for improving the navigation algorithm in order toincorporate various what if scenarios. Another major possibility is to develop a navigation algorithmthat takes of all the ball locations in the area and makes the robot move in such a fashion that itdoesn’t collide with any of the balls present.

Autonomous Mode: The autonomous mode needs reinforcing for taking into consideration caseswhere the ball is not captured properly. Though this has been taken into consideration is the current program but still the implementation needs improvement to cater all possible cases.

Assistive Mode: The assistive mode can be designed where the user will just click the target to betracked and the robot will take the ball to its respective home position.

Manual Mode: Though efforts have been made to have a system where the robot can be controlledusing RF remote control. However, noise in RF communication makes it mandatory to have somekind of filtering algorithm to filter out noise.



8. Conclusion

The following design objectives are met:

(i) Real time image processing was achieved which is efficient and works for most of the indoor surroundings

(ii) Robot can navigate successfully in real time and also check for dynamic changes in ball

locations i.e. new balls can be added without stopping the program.(iii) System can be calibrated automatically i.e system constants can be calculated.

(iv) The GUI is user-friendly and also offers both the manual and autonomous mode to the user.Also, an assistive mode was successfully implemented to help the user to precisely align thegripper position with respect to the target ball.

(v) Matlab GUI also provides the feature to view the arena in real-time for human observationand intervention which paves the way for remote site control.



Contribution by each Team Member

Priyanshu Agarwal: Algorithm Coding, overall system integration, coordination among team membersand lab report.

Dipen Harishbhai Dave: Developed image processing algorithm, mechanical set up for the robot, andmathematical methods section in lab report.

Ravikiran Chollangi: Developed circuits for the system and alternate algorithm for image processing,mechanical setup for the robot, RF communication, and hardware section in lab report.

Jason Lieu: Drawn flow charts of the system and circuit diagrams for lab report.



Bibliography[1] Process Control, Student Guide, PARALLAX INC., Version 1.0

[2] PBASIC Syntax Guide, PARALLAX INC., BASIC Stamp Editor/Development System, Version2.4.2.

[3] StampWorks Manual Version 1.1a, Parallax Inc.



Appendix

A. PBASIC Code for Speed Control System

A.1 Onboard Micro-controller Code

' {$STAMP BS2}' {$PBASIC 2.5}

' -----[ Title ]-----------------------------------------------------------

'========================================================================='FINAL PROJECT: VISION BASED AUTONOMOUS BALL SORTING ROBOTIC SYSTEM'' GROUP D

'' Priyanshu Agarwal' Dipen Harishbhai Dave' Ravikiran Chollangi' Jason Lieu''=========================================================================

' -------------------------------------------------------------------------

' -----------------CONSTANTS & VARIABLES DEFINITION------------------------' -------------------------------------------------------------------------

Gripper PIN 13 ' Pin for Gripper motorServoRight PIN 14 ' Pin for right servo motorServoLeft PIN 15 ' Pin for left servo motor

RX CON 2 'Receive Pin TX CON 0 'Transmit PinBaud CON 84 '9600 Baud

counter VAR Word ' Counter for movecycles VAR Byte ' No of cycles for a movemove VAR Word 'Communication Bytetemp VAR Byte ' temporary variable to swap values

' -------------------------------------------------------------------------' --------------------------------MAIN PROGRAM-----------------------------' -------------------------------------------------------------------------

DO



ReRead:SERIN RX, Baud, [DEC move, WAIT(10), DEC cycles, WAIT(10)] 'Accept data seriallyvia bluetooth

'DEBUG "1move=", DEC move,"cycles=", DEC cycles, CR ' Display data fordebugging

IF (move>7) THEN ' Check for commnication error and swap valuescycles = tempcycles = movemove = tempELSEIF(move=0 & cycles<8) THENGOTO ReReadENDIF

'DEBUG "2move=", DEC move,"cycles=", DEC cycles, CR ' Display data fordebugging

IF(move = 1) THEN 'Move ForwardFOR counter = 0 TO cyclesPULSOUT ServoRight, 500PULSOUT ServoLeft, 1000PAUSE 10NEXT

ELSEIF(move = 2) THEN ' Move BackwardFOR counter = 0 TO cyclesPULSOUT ServoRight, 1000PULSOUT ServoLeft, 500

PAUSE 10NEXT

ELSEIF(move = 3) THEN 'Turn RightFOR counter = 0 TO cyclesPULSOUT ServoRight, 1000PULSOUT ServoLeft, 1000PAUSE 10NEXT

ELSEIF(move = 4) THEN 'Turn LeftFOR counter = 0 TO cyclesPULSOUT ServoRight, 500PULSOUT ServoLeft, 500PAUSE 10NEXT

ELSEIF(move = 5)THEN 'Gripper DownGOSUB GripOn

ELSEIF(move = 6)THEN ' Gripper Up



GOSUB GripOff

ELSEIF(move = 7)THEN ' Gripper Up FastGOSUB GripOffFastENDIF

LOOP

' -------------------------------------------------------------------------' --------------------------------SUBROUTINES------------------------------' -------------------------------------------------------------------------

' --------------------------------GRIPPER ON-------------------------------GripOn:FOR counter = 1 TO 20PULSOUT Gripper, 900PAUSE 20

NEXTFOR counter = 1 TO 230PULSOUT Gripper, counter+900PAUSE 20NEXT

FOR counter = 1 TO 20PULSOUT Gripper, 1130PAUSE 20NEXTRETURN

' -------------------------------------------------------------------------

' --------------------------------GRIPPER OFF-------------------------------GripOff:FOR counter = 1 TO 230PULSOUT Gripper, 1130-counterPAUSE 20NEXTFOR counter = 1 TO 20PULSOUT Gripper, 600PAUSE 20

NEXTRETURN

GripOffFast:FOR counter = 1 TO 20PULSOUT Gripper, 600PAUSE 20NEXTRETURN' -------------------------------------------------------------------------



A.2 RF-Communication Micro-controller Code

' {$STAMP BS2}' {$PBASIC 2.5}

' -------------------------------------------------------------------------' -----------------CONSTANTS & VARIABLES DEFINITION------------------------' -------------------------------------------------------------------------

Forward PIN 0 '0 HIGH ' Val = 13Backward PIN 1 '1 HIGH ' Val = 14Right PIN 2 '0 HIGH ' Val = 9Left PIN 3 '0 HIGH ' Val = 6

send VAR Byteval VAR Byte ' digit to be sent via bluetooth

DIRA = 0 ' Define PIN directions as input

' -------------------------------------------------------------------------

' --------------------------------MAIN PROGRAM-----------------------------' -------------------------------------------------------------------------

Program_Start:DO

val = INA ' Read RF Switch ValuesDEBUGIN DEC send, WAIT(10) ' Wait for serial input data from MATLABPAUSE(100)IF send = 10 THENDEBUG DEC val,10, CR ' Send RF Remote Switch positions to MATLAB serially

ENDIF

LOOP

' -------------------------------------------------------------------------

B. MATLAB Code

B.1 MATLAB Code for Initialization

%Commands to check for any open devices and close them% devices = instrfind;% if(~isempty(devices))

% fclose(devices);% end %Commands to establish Commucation port between MATLAB and Robot via%Serial port (RS-232)% sPort =serial('COM4','BaudRate',9600,'Timeout',10,'Terminator',10,'OutputBufferSize',8)% fopen(sPort)



%Commands to establish Commucation port between MATLAB and Robot via%Bluetooth% devices = instrfind;% if(~isempty(devices))% fclose(devices);% end%% sPort =

serial('COM15','BaudRate',9600,'Timeout',10,'Terminator',10,'OutputBufferSize',8)% fopen(sPort)

% Commands to establish video connection through webcam% vid = videoinput('winvideo');% preview(vid); XCOR_LIMIT = 720; % Workspace X LIMITYCOR_LIMIT = 480; % Workspace Y LIMIT

BOUNDARY = 50; % Boundary at the end RLIMIT = 700; % Ball Area limit for Red colorYLIMIT = 1000; % Ball Area limit for Yellow colorBLIMIT = 50; % Ball Area limit for Blue colorGLIMIT = 500; % Ball Area limit for Green colorWLIMIT = 1000; % Ball Area limit for White color on Robot COLOR_THRESH = 100; % Threshholding for Image Color Calibrate_Ball = 'Red';

BALL_AREA_RATIO = 0.5; %Ratio for ball area calibration C1_AREA = 1000; % Area limit for bigger white area on botC2_AREA = 3000; % Area limit for smaller white area on botC1_RATIO = 0.6; % Calibration area ratio for bigger circleC2_RATIO = 0.6; % Calibration area ratio for smaller circleBOT_RATIO = 8/3.4; % Calibration Ratio for gripper to characteristiclength RED_LOCATION = [BOUNDARY,BOUNDARY]; % Home position for Red ballGREEN_LOCATION = [XCOR_LIMIT-BOUNDARY,YCOR_LIMIT-BOUNDARY]; % Home

position for Green BallBLUE_LOCATION = [XCOR_LIMIT-BOUNDARY,BOUNDARY]; % Home position forBlue BallYELLOW_LOCATION = [BOUNDARY,YCOR_LIMIT-BOUNDARY]; % Home position forYellow Ball cycles_ang = 10; % No. of cycles for which the motors are to bepowered for rotationcycles_lin = 10; % No. of cycles for which the motors are to bepowered for linear motionval = 0; % The value to be sent to bluetooth module



Kp_ang = 0.5; % Proportional gain for the angular position controllerKp_lin = 0.25; % Proportional gain for the linear position controller moveflag = 0; % Flag to solve move ambiguity Target = [300 300]; % Default location for targetTarget_Location = [50, 50]; % Default position for home locationTarget_Ball = [100, 100]; % Default position for ball

grip_state=0; % Gripper state (Down - 1, Up - 0) loop = 0; % Variable to check mission status

B.2 MATLAB Code for Manual mode

val = num2str(getkey()) % Look for any key pressed by the user% val = num2str(int8(get(fh,'CurrentCharacter')))switch(val) case{'28'}

disp('Turn Left');fprintf(sPort,'%d\n',4); % Send turn left command to robotfprintf(sPort,'%d\n',cycles_ang); % Send number of cycles to

robot case{'29'}

disp('Turn Right');fprintf(sPort,'%d\n',3); % Send turn right command to robotfprintf(sPort,'%d\n',cycles_ang); % Send number of cycles to

robot case{'30'}

disp('Move Forward');fprintf(sPort,'%d\n',1); % Send move forward command to robotfprintf(sPort,'%d\n',cycles_lin);% Send number of cycles to

robot case{'31'}

disp('Move Backward');fprintf(sPort,'%d\n',2); % Send move backward command to robotfprintf(sPort,'%d\n',cycles_lin);% Send number of cycles to

robot case{'13'}

disp('Gripper Down');fprintf(sPort,'%d\n',5); % Send gripper down command to robotfprintf(sPort,'%d\n',cycles_lin);% Send number of cycles to

robotgrip_state=1;

case{'32'}disp('Gripper Up');fprintf(sPort,'%d\n',6); % Send gripper up command to robotfprintf(sPort,'%d\n',cycles_lin);% Send number of cycles to

robotgrip_state=0;

case{'27'} %Escape key % Terminate the loop return; case{'101'} %e



return; case{'69'} %E return; endpause(0.12);

B.3 MATLAB Code for Assistive modeBot_Pos_semiautomatic_GUI % Code to calculate the number of

cycles for the assitive modeval = num2str(getkey())

% val = num2str(int8(get(fh,'CurrentCharacter'))); switch(val) case{'28'}

disp('Turn Left'); % fscanf(sPort,'%d\n') ' alternative method to % reduce commnication error

fprintf(sPort,'%d\n',4);

% pause(0.001)fprintf(sPort,'%d\n',cycles_ang);

case{'29'}disp('Turn Right');

% fscanf(sPort,'%d\n')fprintf(sPort,'%d\n',3);pause(0.001)fprintf(sPort,'%d\n',cycles_ang);

case{'30'}disp('Move Forward');

% fscanf(sPort,'%d\n')

fprintf(sPort,'%d\n',1);pause(0.001)fprintf(sPort,'%d\n',cycles_lin);

case{'31'}disp('Move Backward');

% fscanf(sPort,'%d\n')fprintf(sPort,'%d\n',2);pause(0.001)fprintf(sPort,'%d\n',cycles_lin);

case{'13'} % if(grip_state==0)

disp('Gripper Down'); % fscanf(sPort,'%d\n')fprintf(sPort,'%d\n',5);pause(0.001)fprintf(sPort,'%d\n',cycles_lin);grip_state=1;

% end case{'32'} % if(grip_state==1)

disp('Gripper Up'); % fscanf(sPort,'%d\n')



fprintf(sPort,'%d\n',6);pause(0.001)fprintf(sPort,'%d\n',cycles_lin);grip_state=0;

% end case{'27'} %Escape key return; case{'101'} %e

return; case{'69'} %E return;

end% set(fh,'CurrentCharacter',num2str(0));

B.4 MATLAB Subroutine Code for Assistive mode

input = getsnapshot(vid);[r c d]=size(input);

output=zeros(r,c,d);original=input;imshow(original);%-----Noise removal by thresholding-----output=(input>180); label1=output(:,:,1);label2=output(:,:,2);label3=output(:,:,3); botout=(label1>0)&(label2>0)&(label3>0);

label1=label1-botout;label2=label2-botout;label3=label3-botout; redlabel=(label1-(label2+label3));greenlabel=(label2-(label1+label3));bluelabel=(label3-(label2+label1)); redlabel=redlabel.*(redlabel>0);greenlabel=greenlabel.*(greenlabel>0);

bluelabel=bluelabel.*(bluelabel>0); redprops=regionprops(bwlabel(redlabel),'Area','Centroid');greenprops=regionprops(bwlabel(greenlabel),'Area','Centroid');blueprops=regionprops(bwlabel(bluelabel),'Area','Centroid');whiteprops=regionprops(bwlabel(botout),'Area','Centroid'); rcount=0;for i=1:length(redprops) if redprops(i).Area>RLIMIT

rcount=rcount+1;



RedCentroid(rcount,1)=round(redprops(i).Centroid(1));RedCentroid(rcount,2)=round(redprops(i).Centroid(2));

endend gcount=0;for i=1:length(greenprops) if greenprops(i).Area>GLIMIT

gcount=gcount+1;GreenCentroid(gcount,1)=round(greenprops(i).Centroid(1));GreenCentroid(gcount,2)=round(greenprops(i).Centroid(2));

endend bcount=0;for i=1:length(blueprops) if blueprops(i).Area>BLIMIT

bcount=bcount+1;BlueCentroid(bcount,1)=round(blueprops(i).Centroid(1));

BlueCentroid(bcount,2)=round(blueprops(i).Centroid(2)); endend Bot = find([whiteprops.Area] > WLIMIT); %30000, 10000 hold on if(rcount>0)

plot(RedCentroid(1:rcount,1),RedCentroid(1:rcount,2),'b*')end

if(gcount>0)plot(GreenCentroid(1:gcount,1),GreenCentroid(1:gcount,2),'y*')end if(bcount>0)plot(BlueCentroid(1:bcount,1),BlueCentroid(1:bcount,2),'r*')end plot(Target(1),Target(2),'go');for i=1:1:length(Bot)

plot(whiteprops(Bot(i)).Centroid(1),whiteprops(Bot(i)).Centroid(2),'k*')end if(length(Bot)==2) if(whiteprops(Bot(1)).Area < whiteprops(Bot(2)).Area)

x1 = whiteprops(Bot(2)).Centroid(1);x2 = whiteprops(Bot(1)).Centroid(1);y1 = whiteprops(Bot(2)).Centroid(2);y2 = whiteprops(Bot(1)).Centroid(2);

else




endLength = sqrt((x1-x2)^2+(y1-y2)^2)Bot_Ang = rad2deg(atan2(y1-y2,x1-x2))Grip = [x1+Length*(BOT_RATIO)*cosd(Bot_Ang)

y1+Length*(BOT_RATIO)*sind(Bot_Ang)]Set_Target_Grip_GUITarget_Ang1 = rad2deg(atan2(Target(2)-y1,Target(1)-x1))plot(Grip(1),Grip(2),'yo')Lin_Error = sqrt((Grip(1)-Target(1))^2+(Grip(2)-Target(2))^2)Angle_Error = Kp_ang*(Bot_Ang-Target_Ang1)cycles_ang = round(abs(Angle_Error))cycles_lin = round(Kp_lin*Lin_Error)

endif(get(handles.checkbox_axis,'Value')==1)

axis on

grid onendpause(0.01);

B.4 MATLAB Subroutine Code for Assistive mode

rcount=0;gcount=0;bcount=0;ycount=0;tcount = 0;Target_Error = 1000; redlabel=(label1-(label2+label3));greenlabel=(label2-(label1+label3));bluelabel=(label3-(label2+label1)); redlabel=redlabel.*(redlabel>0);greenlabel=greenlabel.*(greenlabel>0);bluelabel=bluelabel.*(bluelabel>0); redprops=regionprops(bwlabel(redlabel),'Area','Centroid');

greenprops=regionprops(bwlabel(greenlabel),'Area','Centroid');blueprops=regionprops(bwlabel(bluelabel),'Area','Centroid'); if(grip_state==0) for i=1:length(redprops) if redprops(i).Area>RLIMIT if(norm(redprops(i).Centroid-RED_LOCATION)>BOUNDARY);

rcount=rcount+1;RedCentroid(rcount,1)=round(redprops(i).Centroid(1));RedCentroid(rcount,2)=round(redprops(i).Centroid(2));



Current_Target_Error = norm(Grip-RedCentroid(rcount,:)); if(Current_Target_Error<Target_Error)

Target_Error = Current_Target_Error;Target_Ball = RedCentroid(rcount,:);Target_Location = RED_LOCATION;

end end

end end

for i=1:length(greenprops) if greenprops(i).Area>GLIMIT if(norm(greenprops(i).Centroid-GREEN_LOCATION)>BOUNDARY);

gcount=gcount+1;GreenCentroid(gcount,1)=round(greenprops(i).Centroid(

1));GreenCentroid(gcount,2)=round(greenprops(i).Centroid(

2));Current_Target_Error = norm(Grip-

GreenCentroid(gcount,:)); if(Current_Target_Error<Target_Error)

Target_Error = Current_Target_Error;Target_Ball = GreenCentroid(gcount,:);Target_Location = GREEN_LOCATION;

end end end end

for i=1:length(blueprops) if blueprops(i).Area>BLIMIT if(norm(blueprops(i).Centroid-BLUE_LOCATION)>BOUNDARY);

bcount=bcount+1;BlueCentroid(bcount,1)=round(blueprops(i).Centroid(1)

);BlueCentroid(bcount,2)=round(blueprops(i).Centroid(2)

);Current_Target_Error = norm(Grip-

BlueCentroid(bcount,:));

if(Current_Target_Error<Target_Error)Target_Error = Current_Target_Error;Target_Ball = BlueCentroid(bcount,:);Target_Location = BLUE_LOCATION;

end end end end

if(rcount>0)plot(RedCentroid(1:rcount,1),RedCentroid(1:rcount,2),'b*')



end

if(gcount>0)plot(GreenCentroid(1:gcount,1),GreenCentroid(1:gcount,2),'y*')

end

if(bcount>0)plot(BlueCentroid(1:bcount,1),BlueCentroid(1:bcount,2),'r*')

end

tcount = rcount+gcount+bcount; if(tcount>0)

Target = Target_Ballplot(Target(1),Target(2),'gx');

elsedisp('Mission Accomplished');loop = 27;val = num2str(27);

end

elseTarget = Target_Locationplot(Target(1),Target(2),'gx');

end

B.5 MATLAB GUI Code for System

function varargout = Sorter(varargin)% SORTER M-file for Sorter.fig gui_Singleton = 1;gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @Sorter_OpeningFcn, ... 'gui_OutputFcn', @Sorter_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []);if nargin && ischar(varargin{1})

gui_State.gui_Callback = str2func(varargin{1});end if nargout

[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});else

gui_mainfcn(gui_State, varargin{:});end

% --- Executes just before Sorter is made visible.function Sorter_OpeningFcn(hObject, eventdata, handles, varargin) % fh = figure;handles.output = hObject;



global sPort global XCOR_LIMIT YCOR_LIMIT BOUNDARYglobal RLIMIT YLIMIT BLIMIT GLIMIT WLIMIT COLOR_THRESH Calibrate_BallBALL_AREA_RATIOglobal C1_AREA C2_AREA C1_RATIO C2_RATIO BOT_RATIOglobal RED_LOCATION GREEN_LOCATION BLUE_LOCATION YELLOW_LOCATION

global cycles_ang cycles_lin Kp_ang Kp_lin global val vidglobal Target Target_Location Target_Ball grip_state loop %fh % Update handles structure guidata(hObject, handles); clcimaqreset

%RS-232% devices = instrfind;% if(~isempty(devices))% fclose(devices);% end%% sPort =serial('COM4','BaudRate',9600,'Timeout',10,'Terminator',10,'OutputBufferSize',8)

% fopen(sPort) %Bluetoothdevices = instrfind;if(~isempty(devices))

fclose(devices);end sPort =serial('COM15','BaudRate',9600,'Timeout',10,'Terminator',10,'OutputBufferSize',8)

fopen(sPort) vid = videoinput('winvideo');input = getsnapshot(vid);imshow(input);hold on XCOR_LIMIT = 720;YCOR_LIMIT = 480;BOUNDARY = 50;



RLIMIT = 700;YLIMIT = 1000;BLIMIT = 50;GLIMIT = 500;WLIMIT = 500; COLOR_THRESH = 100;

Calibrate_Ball = 'Red';BALL_AREA_RATIO = 0.5; C1_AREA = 3000;C2_AREA = 1000;C1_RATIO = 0.6;C2_RATIO = 0.6;BOT_RATIO = 8/3.4; RED_LOCATION = [BOUNDARY,BOUNDARY];GREEN_LOCATION = [XCOR_LIMIT-BOUNDARY,YCOR_LIMIT-BOUNDARY];

BLUE_LOCATION = [XCOR_LIMIT-BOUNDARY,BOUNDARY];YELLOW_LOCATION = [BOUNDARY,YCOR_LIMIT-BOUNDARY]; cycles_ang = 10;cycles_lin = 10;val = 0;Kp_ang = 0.5;Kp_lin = 0.25; % Kp_ang = 0.2;% Kp_lin = 0.05;

moveflag = 0; Target = [300 300];Target_Location = [50, 50];Target_Ball = [100, 100];grip_state=0;loop = 0;% UIWAIT makes Sorter wait for user response (see UIRESUME)% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.function varargout = Sorter_OutputFcn(hObject, eventdata, handles)

% --- Executes on button press in button_right.function button_right_Callback(hObject, eventdata, handles) % --- Executes on button press in button_left.function button_left_Callback(hObject, eventdata, handles) % --- Executes on button press in button_backward.



function button_backward_Callback(hObject, eventdata, handles) % --- Executes on button press in button_forward.function button_forward_Callback(hObject, eventdata, handles) % --- Executes when selected object is changed in uipanel_control.function uipanel_control_SelectionChangeFcn(hObject, eventdata,handles)

global sPort global XCOR_LIMIT YCOR_LIMIT BOUNDARYglobal RLIMIT YLIMIT BLIMIT GLIMIT WLIMIT COLOR_THRESH Calibrate_BallBALL_AREA_RATIOglobal C1_AREA C2_AREA C1_RATIO C2_RATIO BOT_RATIOglobal RED_LOCATION GREEN_LOCATION BLUE_LOCATION YELLOW_LOCATIONglobal cycles_ang cycles_lin Kp_ang Kp_lin global val vidglobal Target Target_Location Target_Ball grip_state loop %fh

if(get(handles.checkbox_axis,'Value')==1)

axis ongrid on

end if(get(handles.radio_run,'Value') ==1)

Initialize_GUIh=msgbox(['Set Targets in the Workspace!'],'Info','help');uiwait(h);

%%%%%%%%%%%%%%%%%%%%%%%%%%Autonomouns Robot Motion%%%%%%%%%%%%%%%

%%%%%%%%%% while(get(handles.radio_run,'Value') ==1) if(get(handles.radio_manual,'Value')==1)

Robot_Motion_Manual_GUI elseif(get(handles.radio_assistive,'Value')==1)

Robot_Motion_Semiautomatic_GUI elseif(get(handles.radio_autonomous,'Value')==1) %%%%%%%%%%%%%%%%%%%%%%%%%%Bot Position Autonomous%%%%%%%%%%%%%%%%%%%%%%%%%%

input = getsnapshot(vid);imshow(input);

hold on if(get(handles.checkbox_border,'Value')==1)plot([0 XCOR_LIMIT],[YCOR_LIMIT-BOUNDARY YCOR_LIMIT-

BOUNDARY],'w');plot([0 XCOR_LIMIT],[BOUNDARY BOUNDARY],'w');plot([XCOR_LIMIT-BOUNDARY XCOR_LIMIT-BOUNDARY],[0

YCOR_LIMIT],'w');plot([BOUNDARY BOUNDARY],[0 YCOR_LIMIT],'w');

end %-----Noise removal by thresholding-----

output=(input>COLOR_THRESH);



label1=output(:,:,1);label2=output(:,:,2);label3=output(:,:,3);

botout=(label1>0)&(label2>0)&(label3>0);whiteprops=regionprops(bwlabel(botout),'Area','Centroid');Bot = find([whiteprops.Area] > WLIMIT);

for i=1:1:length(Bot)plot(whiteprops(Bot(i)).Centroid(1),whiteprops(Bot(i)

).Centroid(2),'k*') end

grip_state if(length(Bot)==2) if(whiteprops(Bot(1)).Area < whiteprops(Bot(2)).Area)

x1 = whiteprops(Bot(2)).Centroid(1);x2 = whiteprops(Bot(1)).Centroid(1);y1 = whiteprops(Bot(2)).Centroid(2);

y2 = whiteprops(Bot(1)).Centroid(2); else


end

Length = sqrt((x1-x2)^2+(y1-y2)^2)Bot_Ang = rad2deg(atan2(y1-y2,x1-x2))

Grip = [x1+Length*(BOT_RATIO)*cosd(Bot_Ang)y1+Length*(BOT_RATIO)*sind(Bot_Ang)] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Set Target%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rcount=0;gcount=0;bcount=0;ycount=0;tcount = 0;Target_Error = 1000;

redlabel=(label1-(label2+label3));greenlabel=(label2-(label1+label3));bluelabel=(label3-(label2+label1));

redlabel=redlabel.*(redlabel>0);greenlabel=greenlabel.*(greenlabel>0);bluelabel=bluelabel.*(bluelabel>0);

redprops=regionprops(bwlabel(redlabel),'Area','Centro

id');



greenprops=regionprops(bwlabel(greenlabel),'Area','Centroid');

blueprops=regionprops(bwlabel(bluelabel),'Area','Centroid'); if(grip_state==0) for i=1:length(redprops) if redprops(i).Area>RLIMIT if(norm(redprops(i).Centroid-

RED_LOCATION)>BOUNDARY);rcount=rcount+1;RedCentroid(rcount,1)=round(redprops(

i).Centroid(1));RedCentroid(rcount,2)=round(redprops(

i).Centroid(2));Current_Target_Error = norm(Grip-

RedCentroid(rcount,:)); if(Current_Target_Error<Target_Error)

Target_Error =Current_Target_Error;

Target_Ball =RedCentroid(rcount,:);

Target_Location = RED_LOCATION; end end end end

for i=1:length(greenprops) if greenprops(i).Area>GLIMIT

if(norm(greenprops(i).Centroid-GREEN_LOCATION)>BOUNDARY);gcount=gcount+1;GreenCentroid(gcount,1)=round(greenpr

ops(i).Centroid(1));GreenCentroid(gcount,2)=round(greenpr

ops(i).Centroid(2));Current_Target_Error = norm(Grip-

GreenCentroid(gcount,:)); if(Current_Target_Error<Target_Error)

Target_Error =

Current_Target_Error; Target_Ball =GreenCentroid(gcount,:);

Target_Location = GREEN_LOCATION; end end end end

for i=1:length(blueprops) if blueprops(i).Area>BLIMIT



if(norm(blueprops(i).Centroid-BLUE_LOCATION)>BOUNDARY);

bcount=bcount+1;BlueCentroid(bcount,1)=round(blueprop

s(i).Centroid(1));BlueCentroid(bcount,2)=round(blueprop

s(i).Centroid(2));Current_Target_Error = norm(Grip-

BlueCentroid(bcount,:)); if(Current_Target_Error<Target_Error)

Target_Error =Current_Target_Error;

Target_Ball =BlueCentroid(bcount,:);

Target_Location = BLUE_LOCATION; end end end end

if(rcount>0)

plot(RedCentroid(1:rcount,1),RedCentroid(1:rcount,2),'b*') end

if(gcount>0)plot(GreenCentroid(1:gcount,1),GreenCentroid(

1:gcount,2),'y*') end

if(bcount>0)plot(BlueCentroid(1:bcount,1),BlueCentroid(1:bcount,2),'r*') end

tcount = rcount+gcount+bcount; if(tcount>0)

Target = Target_Ballplot(Target(1),Target(2),'gx');set(handles.text_targetxy,'String',

[num2str(Target(1),'%3d') ' : ' num2str(Target(2),' %3d')]);

elseh=msgbox('MissionAccomplished','Info','help');

uiwait(h);loop = 27;val = num2str(27);

end else

Target = Target_Locationplot(Target(1),Target(2),'gx');



set(handles.text_targetxy,'String',[num2str(Target(1),'%3d') ' : ' num2str(Target(2),' %3d')]); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%Control Calculation%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%Target_Ang1 = rad2deg(atan2(Target(2)-y1,Target(1)-

x1))

plot(Grip(1),Grip(2),'yo')set(handles.text_gripxy,'String',

[num2str(Grip(1),'%3.0f') ' : ' num2str(Grip(2),' %3.0f')]); if(loop~=27)

Lin_Error = sqrt((Grip(1)-Target(1))^2+(Grip(2)-Target(2))^2)

if((sqrt((x1-Target(1))^2+(y1-Target(2))^2))<Length*(BOT_RATIO))

Lin_Error = -Lin_Error end

Angle_Error = Kp_ang*(Bot_Ang-Target_Ang1)cycles_ang = round(abs(Angle_Error))cycles_lin = round(Kp_lin*abs(Lin_Error))

switch(num2str(grip_state)) case{'0'}

if(Lin_Error<-Length/12 & moveflag~=1)

if(Lin_Error>-Length/8)Kp_lin = 0.1; end

val = num2str(31) %Move Backward elseif(Angle_Error>1)

val = num2str(28) %Turn Left elseif(Angle_Error<-1)

val = num2str(29) %Turn Right elseif(Lin_Error>Length/12 & moveflag~=2) if(Lin_Error<Length/8)

Kp_lin = 0.1;

endval = num2str(30) %Move Forward else

val = num2str(13) %Gripper Down end case{'1'}

% Lin_Error_Ball =sqrt((Grip(1)-Target_Ball(1))^2+(Grip(2)-Target_Ball(2))^2) %if(Lin_Error_Ball>Length/1.1)



%if(Target_Error>Length/1.1) % disp('Target Lost'); % val = num2str(33)%Gripper Up % pause % else

set(handles.text_message,'String','Ball

Captured'); if(Angle_Error>1)

val = num2str(28) %Turn Left elseif(Angle_Error<-1)

val = num2str(29) %Turn Right elseif(Lin_Error>Length/10 & moveflag~=2) if(Lin_Error<Length/3)

Kp_lin = 0.05; end

val = num2str(30) %Move Forward elseif(Lin_Error<-Length/10 & moveflag~=1)

if(Lin_Error>-Length/3)Kp_lin = 0.05;

endval = num2str(31) %Move Backward

elseval = num2str(32) %Gripper Up

end % end otherwise

val = -1; end

moveflag = 0; if(((Grip(1)>XCOR_LIMIT-BOUNDARY/2) &(Grip(2)<YCOR_LIMIT-3*BOUNDARY) & (Grip(2)>3*BOUNDARY))|...

((Grip(1)<BOUNDARY/2) &(Grip(2)<YCOR_LIMIT-3*BOUNDARY) & (Grip(2)>3*BOUNDARY))|...

((Grip(2)>YCOR_LIMIT-BOUNDARY/2) &(Grip(1)<XCOR_LIMIT-3*BOUNDARY) & (Grip(1)>3*BOUNDARY))|...

((Grip(2)<BOUNDARY/2) &(Grip(1)<XCOR_LIMIT-3*BOUNDARY) & (Grip(1)>3*BOUNDARY)))

val = num2str(31) %Move Backwardcycles_lin = 50;

% moveflag = 3; end

if(length(Bot)==1)whiteprops(Bot(1)).AreaC1_AREA

if(whiteprops(Bot(1)).Area>C1_AREA)val = num2str(30) %Move Forwardcycles_lin = 100;

% moveflag = 1; else



val = num2str(31) %Move Backwardcycles_lin = 100;

% moveflag = 2; end end end end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%% switch(val) case{'28'}

set(handles.text_execution,'String','Turn Left');fprintf(sPort,'%d\n',4);pause(0.001)fprintf(sPort,'%d\n',cycles_ang);

case{'29'}set(handles.text_execution,'String','Turn Right');fprintf(sPort,'%d\n',3);pause(0.001)

fprintf(sPort,'%d\n',cycles_ang); case{'30'}

set(handles.text_execution,'String','MoveForward');

fprintf(sPort,'%d\n',1);pause(0.001)fprintf(sPort,'%d\n',cycles_lin);

case{'31'}set(handles.text_execution,'String','Move

Backward');fprintf(sPort,'%d\n',2);

pause(0.001)fprintf(sPort,'%d\n',cycles_lin); case{'13'}

set(handles.text_execution,'String','GripperDown');


case{'32'}set(handles.text_execution,'String','Gripper Up');


case{'33'}pause(1)set(handles.text_execution,'String','Gripper Up

Fast');fprintf(sPort,'%d\n',7);pause(0.001)fprintf(sPort,'%d\n',cycles_lin);



grip_state=0; case{'27'} %Escape key

pause(1)set(handles.text_execution,'String','Gripper Up

Fast');fprintf(sPort,'%d\n',7);pause(0.001)fprintf(sPort,'%d\n',cycles_lin);

grip_state=0;set(handles.radio_run,'Value',0);set(handles.radio_stop,'Value',1);

return; case{'101'} %e return; case{'69'} %E return; end

val = 0; end

set(handles.text_execution,'String','Gripper Up');fprintf(sPort,'%d\n',7);pause(0.001)fprintf(sPort,'%d\n',7);grip_state=0;

endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % --- Executes on selection change in popup_ball.

function popup_ball_Callback(hObject, eventdata, handles) % --- Executes during object creation, after setting all properties.function popup_ball_CreateFcn(hObject, eventdata, handles)if ispc && isequal(get(hObject,'BackgroundColor'),get(0,'defaultUicontrolBackgroundColor'))

set(hObject,'BackgroundColor','white');end % --- Executes on button press in pushbutton_ball.function pushbutton_ball_Callback(hObject, eventdata, handles)




if(get(handles.popup_ball,'Value')==1)

Calibrate_Ball = 'Red';elseif(get(handles.popup_ball,'Value')==2)

Calibrate_Ball = 'Green';else(get(handles.popup_ball,'Value')==3)

Calibrate_Ball = 'Blue';end

h=msgbox(['Please place ' Calibrate_Ball ' ball(s) under thecamera'],'Info','help');uiwait(h);input = getsnapshot(vid);imshow(input);pause(0.001);if(get(handles.checkbox_axis,'Value')==1)

axis ongrid on

end

hold onif(get(handles.checkbox_border,'Value')==1)

plot([0 XCOR_LIMIT],[YCOR_LIMIT-BOUNDARY YCOR_LIMIT-BOUNDARY],'w');

plot([0 XCOR_LIMIT],[BOUNDARY BOUNDARY],'w');plot([XCOR_LIMIT-BOUNDARY XCOR_LIMIT-BOUNDARY],[0


end%-----Noise removal by thresholding-----output=(input>COLOR_THRESH);

label1=output(:,:,1);label2=output(:,:,2);label3=output(:,:,3); botout=(label1>0)&(label2>0)&(label3>0); label1=label1-botout;label2=label2-botout;label3=label3-botout;switch(Calibrate_Ball)

case{'Red'}RLIMIT = 300;AREA = 0;redlabel=(label1-(label2+label3));redlabel=redlabel.*(redlabel>0);redprops=regionprops(bwlabel(redlabel),'Area','Centroid');rcount=0;

for i=1:length(redprops) if redprops(i).Area>RLIMIT

rcount=rcount+1;RedCentroid(rcount,1)=round(redprops(i).Centroid(1));



RedCentroid(rcount,2)=round(redprops(i).Centroid(2));AREA = AREA + redprops(i).Area

end end if(rcount>0)

plot(RedCentroid(1:rcount,1),RedCentroid(1:rcount,2),'b*')RLIMIT = BALL_AREA_RATIO*AREA/rcounth=msgbox(['RED Ball Area Limit = '

num2str(RLIMIT)],'Info','help');uiwait(h);

elseh=msgbox('Calibration Failed for RED Ball! No RED Ball

Present! ','Info','error');uiwait(h);

end

case{'Blue'}BLIMIT = 100;AREA = 0;

bluelabel=(label3-(label2+label1));bluelabel=bluelabel.*(bluelabel>0);blueprops=regionprops(bwlabel(bluelabel),'Area','Centroid');bcount=0;

for i=1:length(blueprops) if blueprops(i).Area>BLIMIT

bcount=bcount+1;BlueCentroid(bcount,1)=round(blueprops(i).Centroid(1)

);BlueCentroid(bcount,2)=round(blueprops(i).Centroid(2)

);

AREA = AREA + blueprops(i).Area end end

if(bcount>0)plot(BlueCentroid(1:bcount,1),BlueCentroid(1:bcount,2),'r

*')BLIMIT = BALL_AREA_RATIO*AREA/bcounth=msgbox(['BLUE Ball Area Limit = '

num2str(BLIMIT)],'Info','help');uiwait(h);

elseh=msgbox('Calibration Failed for BLUE Ball! No BLUE BallPresent! ','Info','error');

uiwait(h); end

case{'Yellow'}YLIMIT = 500;AREA = 0;yellowlabel=label1+label2-redlabel-greenlabel;



yellowprops=regionprops(bwlabel(yellowlabel),'Area','Centroid');

ycount=0; for i=1:length(yellowprops) if yellowprops(i).Area>YLIMIT

ycount=ycount+1;YellowCentroid(ycount,1)=round(yellowprops(i).Centroi

d(1));

YellowCentroid(ycount,2)=round(yellowprops(i).Centroid(2));

AREA = AREA + yellowprops(i).Area end end if(ycount>0)

plot(YellowCentroid(1:ycount,1),YellowCentroid(1:ycount,2),'g*')

YLIMIT = BALL_AREA_RATIO*AREA/ycounth=msgbox(['YELLOW Ball Area Limit = '

num2str(YLIMIT)],'Info','help');

uiwait(h); else

h=msgbox('Calibration Failed for YELLOW Ball! No YELLOWBall Present! ','Info','error');

uiwait(h); end

case{'Green'}GLIMIT = 100;AREA = 0;greenlabel=(label2-(label1+label3));

greenlabel=greenlabel.*(greenlabel>0);greenprops=regionprops(bwlabel(greenlabel),'Area','Centroid');gcount=0;

for i=1:length(greenprops) if greenprops(i).Area>GLIMIT

gcount=gcount+1;GreenCentroid(gcount,1)=round(greenprops(i).Centroid(

1));GreenCentroid(gcount,2)=round(greenprops(i).Centroid(

2));AREA = AREA + greenprops(i).Area

end end if(gcount>0)

plot(GreenCentroid(1:gcount,1),GreenCentroid(1:gcount,2),'y*')

GLIMIT = BALL_AREA_RATIO*AREA/gcounth=msgbox(['GREEN Ball Area Limit = '

num2str(GLIMIT)],'Info','help');uiwait(h);

else



h=msgbox('Calibration Failed for GREEN Ball! No GREENBall Present! ','Info','error');

uiwait(h); endend % --- Executes on button press in pushbutton_bot.function pushbutton_bot_Callback(hObject, eventdata, handles)


if(get(handles.checkbox_axis,'Value')==1)

axis ongrid on

end h=msgbox(['Please place the bot under the camera'],'Info','help');uiwait(h); set(handles.text_execution,'String','Gripper Down');fprintf(sPort,'%d\n',5);

pause(0.001)fprintf(sPort,'%d\n',5);grip_state=1;pause(5); h=msgbox(['Place the gripper Marker'],'Info','help');uiwait(h); input = getsnapshot(vid);imshow(input);pause(0.001)

hold onif(get(handles.checkbox_border,'Value')==1)plot([0 XCOR_LIMIT],[YCOR_LIMIT-BOUNDARY YCOR_LIMIT-

BOUNDARY],'w');plot([0 XCOR_LIMIT],[BOUNDARY BOUNDARY],'w');plot([XCOR_LIMIT-BOUNDARY XCOR_LIMIT-BOUNDARY],[0


end %-----Noise removal by thresholding-----



output=(input>COLOR_THRESH); label1=output(:,:,1);label2=output(:,:,2);label3=output(:,:,3); botout=(label1>0)&(label2>0)&(label3>0);whiteprops=regionprops(bwlabel(botout),'Area','Centroid');

Bot = find([whiteprops.Area] > WLIMIT); %30000, 10000for i=1:1:length(Bot)

plot(whiteprops(Bot(i)).Centroid(1),whiteprops(Bot(i)).Centroid(2),'k*')end if(length(Bot)==2) if(whiteprops(Bot(1)).Area < whiteprops(Bot(2)).Area)

C1_AREA = C1_RATIO*whiteprops(Bot(2)).AreaC2_AREA = C2_RATIO*whiteprops(Bot(1)).Areax1 = whiteprops(Bot(2)).Centroid(1);

x2 = whiteprops(Bot(1)).Centroid(1);y1 = whiteprops(Bot(2)).Centroid(2);y2 = whiteprops(Bot(1)).Centroid(2);

elseC1_AREA = C1_RATIO*whiteprops(Bot(1)).AreaC2_AREA = C2_RATIO*whiteprops(Bot(2)).Areax1 = whiteprops(Bot(1)).Centroid(1);x2 = whiteprops(Bot(2)).Centroid(1);y1 = whiteprops(Bot(1)).Centroid(2);y2 = whiteprops(Bot(2)).Centroid(2);

end

pause(1);h=msgbox('Please Select the Grip Location using the Left Mousebutton','Info','help');

uiwait(h,5); if ishandle(h) == 1

delete(h); end

set(handles.text_message,'String','Please Select the GripLocation using the Left Mouse button');

but=0; while (but ~= 1) %Repeat until the Left button is not clicked

[xval,yval,but]=ginput(1); endxval=round(xval);yval=round(yval);BOT_RATIO = norm([x1-xval, y1-yval])/norm([x1-x2, y1-y2])

% pause elseif(length(Bot)==1)

h=msgbox('Bot is partially outside theWorkspace!','Info','error');

uiwait(h);



elseh=msgbox('Calibration Unsuccessful! Clear

Workspace!','Info','error');uiwait(h);

% pause(0.01);end set(handles.text_execution,'String','Gripper Up');

fprintf(sPort,'%d\n',6);pause(0.001)fprintf(sPort,'%d\n',6);grip_state=0;h=msgbox(['Bot Constants C1 AREA = ' num2str(C1_AREA) ', C2 AREA = 'num2str(C2_AREA) ', BOT RATIO = ' num2str(BOT_RATIO)],'Info','help');uiwait(h); % --- Executes on button press in pushbutton_video.function pushbutton_video_Callback(hObject, eventdata, handles)global vid

preview(vid); % --- Executes on button press in checkbox_axis.function checkbox_axis_Callback(hObject, eventdata, handles)if(get(handles.checkbox_axis,'Value')==1)

axis ongrid on

end % --- Executes on button press in checkbox_border.function checkbox_border_Callback(hObject, eventdata, handles)

% --- Executes on button press in pushbutton_constants.function pushbutton_constants_Callback(hObject, eventdata, handles)global C1_AREA C2_AREA BOT_RATIO RLIMIT GLIMIT BLIMITh=msgbox(['Bot Constants C1 AREA = ' num2str(C1_AREA) ', C2 AREA = 'num2str(C2_AREA) ', BOT RATIO = ' num2str(BOT_RATIO)],'Info','help');uiwait(h);h=msgbox(['RED Ball Area Limit = ' num2str(RLIMIT)],'Info','help');uiwait(h);h=msgbox(['GREEN Ball Area Limit = ' num2str(GLIMIT)],'Info','help');uiwait(h);

h=msgbox(['BLUE Ball Area Limit = ' num2str(BLIMIT)],'Info','help');uiwait(h);

% --- Executes on button press in pushbutton_exit.function pushbutton_exit_Callback(hObject, eventdata, handles)global viddevices = instrfind;if(~isempty(devices))

fclose(devices);end



stop(vid);clc;close all;

Date post:	30-May-2018
Category:	Documents
Upload:	ravidbgt
View:	215 times
Download:	0 times

MAE576 GroupD Project Report

Documents