InstituteofEngineering

Industrieweg34A,9403ABAssen
Holland

TechnicalDesignReport

AnitaAjdinian
GabrielPagani
ThomasGeertsma
VolodymyrNestayko
HanzeUniversityofAppliedSciences
December12,2015


Abstract

InthisreportwedescribeatechnicaldesignofarobotforBoekenmuisproject.Our
principalisaGarageTDIcompany,whichcollaborateswithInstituteofEngineeringin
Assen.Thegoaloftheprojectistodeveloparobotwhichcanbebothautonomously
andremotelycontrolled.Duringdevelopmentandconstructionthereisanumberof
criteriathatmustbetakenintoaccount.Thisreportpresentconcept,design,
developmentandimplementationoftheapplicationsandtheirtestsoffunctionality
andperformance.

Thefollowingannexesareanintegralpartofthereport:

AppendixI
Remotecontroltestingcode
AppendixII
Contract
AppendixIII
MotivatedConceptReport

TableofContents
1.
2.
3.

4.

5.
6.
7.
8.

ListofFigures
Introduction
TechnicalDesign
3.1.
Objecttracking
3.2.
Obstacleavoidance
3.3.
Remotecontrol
3.4.
Videostreaming
3.5.
Robotarm
3.6.
Structure
TestPlanning
4.1. Objecttrackingtesting
4.2. Obstacleavoidancetesting
4.3. Remotecontroltesting
4.4. Videostreamingtesting
4.5. Robotarmtesting
4.6. Structuretesting
4.7. Batterytesting
References
AppendixI
AppendixII
AppendixIII

ListofFigures
Fig.1.Objecttrackingsystemsflowchart
Fig.2.Ultrasonicsensorsplacement
Fig.3.Remotecontrolchannelvalues
Fig.4.FPVsystemforvideostreaming
Fig.5.Robotarmwithoutgripper
Fig.6.Arobotarmgripper
Fig.7.Thestructureoftherobot
Fig.8Finalstructureoftherobot
Fig.9.Rover5platformusedfortesting
Fig.10.TestingtargetsforPixycamera


Introduction

Robotics is a growing field. Robots have found uses in a wide variety of industries due to
their robust resistance capabilities and precision functions [1]. This has caused many
universities to offer classes and programs in the field of robotics that combine elements of
electrical engineering, mechanical engineering and computer science. Additionally,
projectbasedlearningisanimportantpartoflearninganengineeringdiscipline.
The Garage TDI is a theater company located in Assen that creates and performsplaysfor
children and young adults. Next year (2016) they want to make a new play called
Boekenmuis, in which one of the actors will be a robot. The company approached Hanze
and asked if it would be possible to develop a project to build such arobot.Thegoalofthis
project is to build a robot that can be adapted to the environment of a theatreandmeetthe
criteria and attributes that the company requested from the project group, which have been
listedintheprojectcontract[2].
The Garage TDI company wants the robot to be equipped withanautonomousroboticarm,
capable of performing the task of picking up a book from a table and place it on a shelf
nearby. The specific location of the book, the dimensions of thetable,willbeadaptedbased
on the robots limitations, so there is no predefined requirement for those. Other main
functionalities are remote control and videostreaming system, so that the robot can be
controlled by someone outside the theater stage. Another main criteria is that therobothas
to be easily reprogrammable, so that it can be adapted to new roles in other plays in the
future. On top of that, the actorfollowing feature would be a bonus for the described
prototype, however not a main requirement. Together with the actorfollowing feature, the
robot also needs an obstacle avoidance system, in order to assure that it will not hit any
object in the stage while following the actor. Other nicetohave features are: a monitor on
the robots head that displays a face on the screen and the external structure. Those
extrafeatures will be built in case the group has enough time and budget after meeting the
mainrequirements.
For each of those features, the group discusseddifferentmethodsand approaches,andthe
results and conclusions can be found in the Motivated Concept Report[3].Thefocusofthis
report is to scrutinize and explain the relation between the robot parts, explain their
functionalities, and how theymeet theclientscriteria.Theobjecttrackingsystembuiltwitha
Pixy camera and Arduino microcontroller, the obstacle avoidance system using ultrasonic
sensors, video streaming and remote control, both using RF communication, the robot arm
design and specifications, and the structure design are discussed in the Technical Design
section below. Then in the Testing Planning section, experimental setup for the
aforementioned parts is presented. The testing platform that the project group used for the
testing of most of the system is presented, and the codes and some results are also
discussed.

TechnicalDesign

ObjectTracking
Asdiscussedinthemotivatedconceptreport,thePixyCamera[4][5],togetherwith
anArduinoboard,wasusedtomaketheobjecttrackingbycolorpatternrecognition.In
ordertodoso,acolouredtargetwillbeattachedtotheactorsclothesandoncethecamera
detectsthattargetitwillgivethecoordinatesoftheimagescenter(xandy)andsizeofthe
objectsimage(lengthandheight).Therobotwillonlybeabletofollowtheactorifthetarget
isinsight.Sincetheonlyfactorofinterestistheactorshorizontaldisplacement,onlythex
coordinatevaluewillbeused.Thetargetwillbeplacedatthesameheightasthecamera,
andtheactorwillonlymovearoundthestage(hewontclimbaladderforexample),sothe
targetwillalwayshavethesameycoordinate.Inthecode,thethecenteroftheimageis
definedastheorigin,thereforehavingcoordinates(0,0).Thedistancefromthetargets
imagecentertotheorigincanbecalculatedas:

x_distance=origin_x+target_x

wheretheorigin_xisalways0,andthetarget_xisthexcoordinateofthecenterofthe
targetsimage.Soifthetargetistotheleftoftheorigin,thedistancewillbenegative(<0)
andifthedistanceispositive(>0),thesystemknowsthatthetargetistotheright.As
describedinfig[1],ifdistance>0,therobotshouldmovetotheleftuntilthedistancevalue
reaches0.

Otherthankeepingtrackoftheactorsposition,therobotalsoneedstofollowit,keepinga
determineddistance.Byknowingthesizeoftheimage,thedistancefromtheactortothe
cameraiscalculatedbyusingthefollowingformula:

actual_distance=initial_distance*sqrt(initial_area/measured_area)

wheretheinitial_areaistheareaoftheobjectwhenplacedatinitial_distancefromthe
camera,andthemeasured_areaiscalculatedfromthelengthandheightoftheimage
providedbyit.Athresholdvaluefordistancewillbeusedtodetermineiftherobotshould
moveforwardorbackwards.

Thesystemshouldalwaystrytoreachbalance.Thatoccurswhenx_distance=0and
actual_distance=threshold.

TheArduinoscodeforthePixycameraobjecttrackingcanbefoundinthisreports
AppendixIn.5.Thatcodeincludestheservomovementtospinthecameraaroundto
centralizethetargetsimage.


Fig.[1]Objecttrackingsystemsflowchart

Obstacleavoidance
Tomakesurethattherobotwillnothitanyobjectduringtheactorfollowingfunction,
anobstacleavoidancesystemwasbuilt.ThesystemusestwoHYSRF05/HCSR05
ultrasonicsensors[6][7]placedinfrontoftherobot(oneinthefurtherleftandoneinthe
furtherrightfront),asshowninFig.[2].

Fig.[2]Ultrasonicsensorsplacement


Forthatsystem,athresholdvaluefordistancewasset.Ifthedistancefromanyofthe
sensorsissmallerthanthatthresholdvalue,theobjectisconsideredadetectedobstacle.
ThesystemshouldworkasinFig.[3],andthedeviationproceduretotheleftisdescribedin
Fig.[4].

Fig.[3]Obstacleavoidancesystem


Fig.[4]Leftdeviationprocedure

OnFig.[4],thesystemshouldkeeptrackofhowmuchtimeittakestogofromstep1tostep
2.Thatsamevalueoftimeshouldbeusedonstep4tomakesuretherobotreturnstoits
originalorientation.Iftherobottakes3secondsspinningonstep1untilitnolongercansee
theobstacle,itshouldtake3,5secondstospinonstep4.The0,5extrasecondstherobot
shouldspinonstep2istomakesuretherobotkeepsacertaindistanceoftheobstacle
whengoingbyit.Onstep3,therobotshouldmovethesamedistanceasthethreshold
value,tomakesurethattheobstacleissurpassed.Thedeviationproceduretotheright
worksinasymmetricway.Thealgorithmisthesame,butinsteadofspinningtotheleft,the
robotshouldspintotheright.Thecodeforthatsystemcanbefoundonthisreports
AppendixIn.3.


Remotecontrol
Aftertestingdifferentmethodsforwirelessrobotcontrol,aRFtransmitterand
receiver,whichareusedforcontrollingRChelicopters,waschosen.Itworkson2.4GHz
frequency,has4channelsandmoreimportant,lowpowerconsumption.ThistypeofRC
providesextendedfunctionality,togetherwithrangeupto500m.Thegrouptookinto
considerationtheinterferenceproblem,andinordertopreventit,receiverandtransmitterfor
videostreamingthatworkondifferentfrequency(5.8GHz)wereordered.

MessagebitsfromRemotecontrolareencodedbytransmittingasinglepulseinoneof
possibletimeshifts.Inordertodecodesignal,asmallprogramwaswrittenforArduinomicro
controller.ThecodeforthatcanbefoundinthisreportsAppendixI.Thecodeisusedto
identifyallchannelsandmeasurevaluesaccordingtopositionofthejoystick,andtheresults
areshowninFig.[3].

Channels1and2wereusedtocontrolforwardandbackwarddirectionsofrobot,
respectively.Additionally,channels3and4wereusedforurgentturnoffincaseof
dangeroussituationandswitchbetweenmanualandautonomousmode,respectively.

Fig.[3]Remotecontrolchannelvalues


Videostreaming
Afteradditionalresearchontopicofvideostreaming,anunexpectedsolutionwas
foundFPVtransmitterandreceiverthatareusedinremotehelicopters,thereforeit
provideslongvideostreamingrangeandminimallatency.Anadditionaladvantageofthis
systemisthatitworkson9Vbatteries,whichcanbeeasilychangedinbetweenthetheatre
plays.ThehardwareforthatsystemisshowninFig.[4].

Fig.[4]FPVsystemforvideostreaming

Robotarm
Therobotarmconsistof:
2nidecmotorsforthefirst2axis[8]
2servos1forthethirdaxisandthesecondforthegripper[9]
1doublemotorcontrollerforbothnidecmotors[10]
circuitfortheencoders
aluminiumplates
aluminumgears
belt
endstops

Howdoesitwork
Thefirstaxispoweredbyanidecmotorviaasmallergearwithatransmissionof2,5.
Nidecmotorsweight1,5kiloandthatiswhythesecondaxcouldnothavethemotoronthe
axbutinsteadhadtobepoweredbyabeltfromthesamepositionasthefirstax.The
distancebetweentheaxisare40centimeterfromax1to2and35fromax2to3plusthe
gripperof10cm,thesedistancesarethesameasahumanarm[Fig5].Thethirdaxisa14
kg/cmservothatistheretorotatethegripperupanddown[Fig6].Weuseendstopsto
determinewhatpositiontherobotstartsfrom.Topowerthemotorsweuse212volt
batteriesanda7voltvoltageregulatorfortheservos.

Theclientwantedtousethearmtoliftabook.Aftersomeresearchweconcludedthatan
averagebookisnotheavierthan500gramsonowweneededtotestifourrobotarmcanlift
thisbook.


Thetotallengthofthearmis40+35+10=85cm
5N/m(nidecmotor)
Transmissionof2,5
Takenfromthefirstaxisthearmcantheoreticallylift=(5N/0,85m)*2,5=1,5~kilogram
Thethirdax(servo)canonlylift14kg/cmona10cmdistance=1,4kilogram
soitshouldbeabletoliftabook.

Fig.[5]Robotarm.

Fig.[6]Robotarmgripper.

Structure
ThebaseofourrobotisaMeyrapp2electricwheelchair.Theelectricwheelchair
uses2Fracmomotorstopowerthebackwheelsseparatelyandhas2castorsinfront.
Thereare3metalbarsweldedontheplatformoneoneachsideandoneinmiddlewhere
webuildapolethatisgoingtothearmandheadoftherobot[Fig7].Thebatteriesand
electronicswillbeplacedonthewheelbaseplatformforoptimalweightdistributionand
keepingtherobotfromfalling.

Thedimensionoftherobotarewidth55cmwithoutwheelsand62cmwithwheels,length
74cm,heightoftheplatformis20cmandtoheaditis162cm.

Fig.[7]Structureoftherobot.

Ourfinaldesignwillstillbeskeletsothetheatrecompanycanreusetherobotforotherplays
aswell.Thefinalrobotwillhaveasmalloldtvscreenasheadandweareworkingon
displayingsomethingonthescreenthatlooklikearobotface.Abovethissmalltvwewill
attachthePixyCamerawithawirefromthebackofthestructureto5/10cmonaboveofthe
screenanditwilllooklikeanantennashowninFig.[8].

Fig.[8]Finaldesignofrobot.

TestPlanning

Toensureoperabilityofoursystems,asmallerplatformwasusedfortesting:theRover5
Platform[11],showninFig.[10].Thisallowedustotestmostofthesystemswhilethefinal
structurewasbeingbuilt,andsavedusalotoftimeduringthedevelopmentphase.Two
ultrasonicsensorsHCSR04[12],similartotheoneswewilluseonthefinalrobot(the
HCSR05[6][7]),wereusedfortheobstacleavoidancesystem.Theremotecontrolwas
implementedonthatplatform,andalsothevideostreaming.Oncethosesystemsare
workingonthetestingplatform,thegrouponlyneedstoadaptittothefinalone.Thegroup
wrotefunctionsformotorcontrol,thatcanbefoundinthisreportsAppendixIn.4.

Obstacleavoidance
Totesttheobstacleavoidancesystemthe
Rover5platformwasusedandthetestcodecanbe
foundinAppendixI,n.3.Sincenorequirementswere
providedfortheobstaclesthattherobotshoulddetect,
severalshapeswillbetested,andaftercollectingthe
results,thegroupwillprovidealistofwhatshapeswork
betterforthesystem.Thatway,theTDIcompanycan
adapttheenvironmentoftheplaytomeetthose
requirements.Someoftheshapesthatwillbetested
are:rectangles,roundobjects,edgedobjects,and
irregularobjects.Duringthetesting,thegroupwilltake
notesoftheultrasonicsensorsrangeforeachdifferent
object,andcalculatethebestvalueforthethreshold.
Fig.[9]Rover5platformusedfor
testing

Objecttracking
TheobjecttrackingsystemwastestedusingthecodepresentedinAppendixI,n.5,
providedbyCMUcam5company[12],andadaptedbyourgroup.ThealgorithmusesPD
techniquetocompensateforthedistanceerror.Thetargetchosenforthetestingwasa
regulartennisballandacolorfulpartyballoonlikeshowninFig.[11].Thesystemwastested
withthetargetat50cmfromthecamera,anditwasabletokeeptrackofitsmovement,
spinningthecamerasothattheimagewasalwayscentralized.Thegroupfiguredoutthat
roundshapedtargetsworkbetterwiththealgorithm,becausethelengthandheightofthe
imageremainsthesameiflookingfromdifferentangles.Thatfeatureisimportanttothe
system,duetothefactthatthosevaluesareneededtocalculatethedistance.Further
testingwillincludedifferentcoloursandmaterialsforthetarget,andalistofrequirementsfor
choosingapropertargetwillbeprovided.


Fig.[10]TestingtargetsforPixycamera

Remotecontrol
Remotecontrolmustworkinarangeof510meterswiththeminimalpossibledelay.
Tomakesurethatsystemwillworkwithoutproblemsoveralongperiodoftime,remote
controlrangewouldbetestedandlatencyindifferentlocationsfromuniversitylabtoGarage
TDIcompany.Theremotecontroltestingwillbeconsideredsuccessfulinthecaseofgetting
therangeupto20meterswithoutanylatency.Theremotecontrolwasalsotestedwiththe
Rover5platform,andthearduinotestingcodecanbefoundinthisreportsAppendixI,n.1.
Afterseveraltestsofremotecontrolconnectedtoourtestingplatform,noproblemswere
foundwhencontrollingtherobotfromadistanceofuptoapproximately50meters.

Videostreaming
Thevideostreamingsystemwasalsotestedusingthetestingplatform.Thegroup
focusedonthreetypesofproblemsthatcouldoccur:range,latencyandinterference.
Duringthetests,theworkingrangewasregisteredasmorethan50meters,whichisway
morethantherequiredfortheproject.Therewasalsonoproblemswithlatency.

Tomakesurethatthesignalsfromtheremotecontrolwillnotinterferewithvideostreaming
receiverthegrouptestedittogetherwithturnedonRC.Thevideofromthecamerawas
becomingnoisyfromtimetotimewhenwewereincreasingspeedofthetestingplatform
fromtheremotecontrol.Firstwetriedtochangereceiverchannelsfrequencytogetless
noisyvideo,butitdidn'twork.Laterthegroupfoundoutthattherewasalmostnonoisewhen
theplatformwasnotmoving,buttheremotecontrolwasturnedon,soitwasconcludedthat
thevideosignalwaslostduetothetrepidationofthetransmitter.Itisbelievedthat,by

changingstiffnessofsuspensionorbyplacingreceiverontopoftherobot,thatproblem
couldbesolved.Uptothispoint,thegroupconcludedthatthetestingofvideostreaming
wentwell,butadditionaltestswithupgradedstructureneedtobedonelateronfinal
prototype.

Robotarmtesting

Thearmhastopickupabook(averagebookisaround0,5kilogram).Firstweneed
totestifthegripperisstrongenoughtoalwaysgrab.Ifthisisthecaseweneedtotestifall
axiscanstillrotatewiththebookgrippedinthearm.Allaxisshouldbeabletorotate180
degreein5sec.Thetotaltimeforbringingthearminplacecannottakemorethan15
seconds.Furthermorewewilltesttheprecisionofthearmbymovingthearmfromstart
positiontoacertainpointandcheckifthispointiswithinanerrormargin.Thiserrorcannot
bemorethan1cminalldirections.Oncealltheseparametersarereachedwewilltestthe
arm20timeswiththesameexerciseandcheckiftheseareallthesameresults.

Structuretesting
Thestructuremustbedurableandrobustforthetheatreplay.Testingthestructure
willbefocusedonthese2requirements.Testingfordurabilityinsuchalimitedtimebutthis
istestedbyperformingafewplaysandcheckaftertheconditionoftherobot.Testingfor
robustnessistestedbyshakingtherobotheavyandcheckaftertheconditionoftherobot.

Batterytesting
Therobothastowork2playsinarow,thereforetestingtherobotistocheckifthe
batterycanpowertherobotfor2plays.

References

1. Wikipediacontributors.
"R.U.R.."Wikipedia,TheFreeEncyclopedia.Wikipedia,The
FreeEncyclopedia
,9Dec.2015.Web.14Dec.2015.
2. A.Ajdinian,T.Geertsma,V.Nestayko,G.Pagani.
ProjectContract.
2015.Available
onblackboard

3. A.Ajdinian,T.Geertsma,V.Nestayko,G.Pagani.
MotivatedConceptReport.
2015.
Availableonblackboard

4. OmnivisionWebsite.
OV9715,CMOSWXGA(1megapixel)HDSensorwith
OmniPixel3HSTechnology
.[Web].(Accessed:15.12.15).Available:
http://www.ovt.com/products/sensor.php?id=43

5. CmucamWebsite.
CMUcam5Pixy
.[Web].(Accessed12.15.2015).Avaiable:
http://cmucam.org/projects/cmucam5/documents

6. RobotelectronicsWebsite.
SRF05UltraSonicRanger.TechnicalSpecification.

[Web].(Accessed:15.12.2015).Available:
http://www.robotelectronics.co.uk/htm/srf05tech.htm

7. UpgradeIndustriesWebsite.
PreciseUltrasonicDistanceSensor(HYSRF05/
HCSR05)
.[Web].(Accessed:15.12.2015).Avaiable:
https://www.upgradeindustries.com/product/70/PreciseUltrasonicDistanceSensor(
HYSRF05/HCSR05)

8. smolkaberlin.
nidecdcmotor(406.005)
.[Web].(Accessed:15.12.2015).Avaiable:

http://www.smolkaberlin.de/includes/download_pdf.php?ID=365

9. electronicoscaldas.
TowerProservos(mg995)
.[Web].(Accessed:15.12.2015).

Avaiable:
http://www.electronicoscaldas.com/datasheet/MG995_TowerPro.pdf

10. aliexpress.
DualHBridgedcmotor(
DCM001)
.[Web].(Accessed:15.12.2015).
Avaiable:
http://nl.aliexpress.com/item/336VDual15AHBridgeDCMotorDriverPeak30AI
RF3205ForRobotSmartCar/938370821.html

11. Rover5PlatformDatasheet:
https://www.pololu.com/file/0J467/Rover%205.pdf

12. HCSR04ultrasonicsesnsordatasheet:
http://www.electroschematics.com/wpcontent/uploads/2013/07/HCSR04datasheetv
ersion1.pdf

13. PixyCameraPan/Tiltdemocode
http://cmucam.org/projects/cmucam5/wiki/Run_the_Pantilt_Demo

AppendixI

1. Remotecontroltestingcode

intpwm_left=3//PWMcontrolformotoroutputs1and2isondigitalpin3
intpwm_right=11//PWMcontrolformotoroutputs3and4isondigitalpin11
intdir_left=12//directioncontrolformotoroutputs1and2isondigitalpin12
intdir_right=13//directioncontrolformotoroutputs3and4isondigitalpin13
intval=0//valueforfade
intincByte=0
intmax_speed=255
intmin_speed=40//Shouldbe40foriftherobotwheelsaretouchingtheground
//Ifthewheelsarenottouchingtheground,therobotwillnotstopatthatpwmvalue
intchannel1=2
intchannel2=8
intchannel1_val=1620
intchannel2_val=1430
intpwmVal=0

voidsetup(){
pinMode(pwm_left,OUTPUT)//Setcontrolpinstobeoutputs
pinMode(pwm_right,OUTPUT)
pinMode(dir_left,OUTPUT)
pinMode(dir_right,OUTPUT)
pinMode(channel1,INPUT)//initialisesthechannels
pinMode(channel2,INPUT)//asinputs
Serial.begin(9600)
}

voidloop(){

channel1_val=(pulseIn(channel1,HIGH))//Checksthevalueofchannel1

channel2_val=(pulseIn(channel2,HIGH))//Checksthevalueofchannel2

if(channel1_val<1420)
{
pwmVal=map(channel1_val,1420,1195,min_speed,max_speed)
left(pwmVal)
}
if(channel1_val>1820)
{
pwmVal=map(channel1_val,1820,2047,min_speed,max_speed)
right(pwmVal)
}
if(channel2_val<1230)
{
pwmVal=map(channel2_val,1230,1030,min_speed,max_speed)
forward(pwmVal)
}
if(channel2_val>1630)
{
pwmVal=map(channel2_val,1630,1802,min_speed,max_speed)
backward(pwmVal)
}
if(channel2_val>1230&&channel2_val<1630&&channel1_val>1420&&
channel1_val<1820||channel1_val==0||channel2_val==0)
{
stopped()
}

voidforward(intpwmVal)//GoforwardwithspecifiedpwmValspeed
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,LOW)
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
}

voidbackward(intpwmVal)//GobackwardwithspecifiedpwmValspeed
{
digitalWrite(dir_left,HIGH)
digitalWrite(dir_right,HIGH)
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
}

voidleft(intpwmVal)//Turnleftaroundrightwheelaxis
{
digitalWrite(dir_right,LOW)
digitalWrite(dir_left,HIGH)
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
}

voidright(intpwmVal)//Turnrightaroundrightwheelaxis
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,HIGH)
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
}

voidbreaks()//breakssmoothly
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,LOW)
fadeout()
}

voidstopped()//stopscompletelyandinstantly
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,LOW)
analogWrite(pwm_left,0)
analogWrite(pwm_right,0)
}

voidfadeout()
{
//fadeoutfrommaxtomininincrementsof1points:
for(pwmVal=max_speedpwmVal>=0pwmVal=1)
{
//setsthevalue(rangefrom0to255):
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
//waitfor30millisecondstoseethedimmingeffect
delay(30)
}
}

2.Channeltestingcode

intchannel1=2
intchannel2=8
intchannel3=4
intchannel4=5
intchannel5=6
intchannel6=7
voidsetup()
{

pinMode(channel1,INPUT)
pinMode(channel2,INPUT)
pinMode(channel3,INPUT)
pinMode(channel4,INPUT)
pinMode(channel5,INPUT)
pinMode(channel6,INPUT)
Serial.begin(9600)//Setsthebaudrateto9600bps
}

voidloop()
{
Channel1=(pulseIn(channel1,HIGH))//Checksthevalueofchannel1
Serial.print("channel1:")
Serial.println(Channel1)//Printsthechannelsvalueontheserialmonitor

Channel2=(pulseIn(channel2,HIGH))//Checksthevalueofchannel2
Serial.print("channel2:")
Serial.println(Channel2)//Printsthechannelsvaluevalueontheserialmonitor

Channel3=(pulseIn(channel3,HIGH))//Checksthevalueofchannel3
Serial.print("channel3:")
Serial.println(Channel3)//Printsthechannelsvaluevalueontheserialmonitor

Channel4=(pulseIn(channel4,HIGH))//Checksthevalueofchannel4
Serial.print("channel4:")
Serial.println(Channel4)//Printsthechannelsvaluevalueontheserialmonitor

Channel5=(pulseIn(channel5,HIGH))//Checksthevalueofchannel5
Serial.print("channel5:")
Serial.println(Channel5)//Printsthechannelsvaluevalueontheserialmonitor

Channel6=(pulseIn(channel6,HIGH))//Checksthevalueofchannel6
Serial.print("channel6:")

Serial.println(Channel6)//Printsthechannelsvaluevalueontheserialmonitor

delay(2000)}

3.Obstacleavoidancesystemtestingv1.1

#include<NewPing.h>//REMEMBERTOCHANGENO_ECHOVALUEONLIBRARY
CODETO5700
//Thatmakesthecodereturns"100"incaseitcannotfindanyobjectinrange

#defineTRIGGER_PIN_LEFT9//YELLOWArduinopintiedtotriggerpinontheultrasonic
sensor.
#defineECHO_PIN_LEFT10//WHITEArduinopintiedtoechopinontheultrasonic
sensor.
#defineMAX_DISTANCE100//Maximumdistancewewanttopingfor(incentimeters).
Maximumsensordistanceisratedat400500cm.
#defineTRIGGER_PIN_RIGHT5
#defineECHO_PIN_RIGHT6
#definedistance_threshold15//Setto50!!!

NewPingleft_sonar(TRIGGER_PIN_LEFT,ECHO_PIN_LEFT,MAX_DISTANCE)//
NewPingsetupofpinsandmaximumdistance.
NewPingright_sonar(TRIGGER_PIN_RIGHT,ECHO_PIN_RIGHT,MAX_DISTANCE)

intpwm_left=3//PWMcontrolformotoroutputs1and2isondigitalpin3
intpwm_right=11//PWMcontrolformotoroutputs3and4isondigitalpin11
intdir_left=12//directioncontrolformotoroutputs1and2isondigitalpin12
intdir_right=13//directioncontrolformotoroutputs3and4isondigitalpin13
intval=0//valueforfade
intmax_speed=80
intmin_speed=40//Shouldbe40foriftherobotwheelsaretouchingtheground
//Ifthewheelsarenottouchingtheground,therobotwillnotstopatthatpwmvalue(40)
intdistance_L=0
intdistance_R=0
intt=0

voidsetup()
{
pinMode(pwm_left,OUTPUT)//Setcontrolpinstobeoutputs
pinMode(pwm_right,OUTPUT)
pinMode(dir_left,OUTPUT)
pinMode(dir_right,OUTPUT)
Serial.begin(9600)
}


voidloop()
{
//readsonar()//debug/printingdistances
//Serial.print("Left:")
//Serial.println(distance_L)
//Serial.print("Right:")
//Serial.println(distance_R)

if(distance_L>=distance_threshold&&distance_R>=distance_threshold)
forward()
else
deviationProcedure()

voidforward()//fullspeedforward
{
digitalWrite(dir_left,LOW)//Reversemotordirection,1high,2low
digitalWrite(dir_right,LOW)//Reversemotordirection,3low,4high
analogWrite(pwm_left,max_speed)//setbothmotorstorunatmax_speed
analogWrite(pwm_right,max_speed)
}

voidleft()//Spinleftarounditscenter
{
digitalWrite(dir_right,LOW)
digitalWrite(dir_left,HIGH)
analogWrite(pwm_left,max_speed)
analogWrite(pwm_right,max_speed)
}

voidright()//Spinrightarounditscenter
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,HIGH)
analogWrite(pwm_left,max_speed)
analogWrite(pwm_right,max_speed)
}

voidstopped()//stopscompletelyandinstantly
{
analogWrite(pwm_left,0)
analogWrite(pwm_right,0)
}


voidreadsonar()
{
delay(50)
distance_L=left_sonar.ping_median(5)/US_ROUNDTRIP_CM
delay(50)
distance_R=right_sonar.ping_median(5)/US_ROUNDTRIP_CM
}

voiddeviationProcedure()
{
if(distance_L<distance_R)//DeviationtotheRIGHT
{
intdev_dir=1
while(distance_L<distance_threshold)
{
t=millis()
left()
delay(100)
readsonar()
}

}else{//DeviationtotheLEFT
intdev_dir=2
while(distance_R<distance_threshold)
{
t=millis()
right()
delay(100)
readsonar()
}
}
delay(100)
t=millis()t
stopped()
forward()
delay(1000)//Youshouldchangethisdelaydependingonthespeedoftherobotandthe
thresholdvalue.
//Thisvalueshouldbethetimetherobottakestotravelthethresholdvalue.Forexample,if
thethresholdis30,
//thatdelayshouldbeenoughtomaketherobottravel30cm.
stopped()
if(dev_dir==1)
{
right()
delay(t)

}else{
left()
delay(t)
}
}

4.Motorcontrolfunctions:

voidforward(intpwmVal)//GoforwardwithspecifiedpwmValspeed
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,LOW)
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
}

voidbackward(intpwmVal)//GobackwardwithspecifiedpwmValspeed
{
digitalWrite(dir_left,HIGH)
digitalWrite(dir_right,HIGH)
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
}

voidleft(intpwmVal)//Turnleftaroundrightwheelaxis
{
digitalWrite(dir_right,LOW)
digitalWrite(dir_left,HIGH)
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
}

voidright(intpwmVal)//Turnrightaroundrightwheelaxis
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,HIGH)
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
}

voidbreaks()//breakssmoothly
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,LOW)
fadeout()
}


voidstopped()//stopscompletelyandinstantly
{
digitalWrite(dir_left,LOW)
digitalWrite(dir_right,LOW)
analogWrite(pwm_left,0)
analogWrite(pwm_right,0)
}

voidfadeout()
{
//fadeoutfrommaxtomininincrementsof1points:
for(pwmVal=max_speedpwmVal>=0pwmVal=1)
{
//setsthevalue(rangefrom0to255):
analogWrite(pwm_left,pwmVal)
analogWrite(pwm_right,pwmVal)
//waitfor30millisecondstoseethedimmingeffect
delay(30)
}
}

5.PixyCameraObjecttrackingarduinotestcode

//
//
//
//ThisfileispartofPixyCMUcam5or"Pixy"forshort
//
//AllPixysourcecodeisprovidedunderthetermsofthe
//GNUGeneralPublicLicensev2(http://www.gnu.org/licenses/gpl2.0.html).
//ThosewishingtousePixysourcecode,softwareand/or
//technologiesunderdifferentlicensingtermsshouldcontactusat
//cmucam@cs.cmu.edu.Suchlicensingtermsareavailablefor
//allportionsofthePixycodebasepresentedhere.
//
//endlicenseheader
//
//Thissketchisasimpletrackingdemothatusesthepan/tiltunit.For
//moreinformation,gohere:
//
//http://cmucam.org/projects/cmucam5/wiki/Run_the_Pantilt_Demo
//

#include<SPI.h>
#include<Pixy.h>


Pixypixy

#defineX_CENTER((PIXY_MAX_XPIXY_MIN_X)/2)

#defineY_CENTER((PIXY_MAX_YPIXY_MIN_Y)/2)

classServoLoop
{
public:
ServoLoop(int32_tpgain,int32_tdgain)

voidupdate(int32_terror)

int32_tm_pos
int32_tm_prevError
int32_tm_pgain
int32_tm_dgain
}

ServoLooppanLoop(300,500)
ServoLooptiltLoop(500,700)

ServoLoop::ServoLoop(int32_tpgain,int32_tdgain)
{
m_pos=PIXY_RCS_CENTER_POS
m_pgain=pgain
m_dgain=dgain
m_prevError=0x80000000L
}

voidServoLoop::update(int32_terror)
{
longintvel
charbuf[32]
if(m_prevError!=0x80000000)
{

vel=(error*m_pgain+(errorm_prevError)*m_dgain)>>10
//sprintf(buf,"%ld\n",vel)
//Serial.print(buf)
m_pos+=vel
if(m_pos>PIXY_RCS_MAX_POS)
m_pos=PIXY_RCS_MAX_POS
elseif(m_pos<PIXY_RCS_MIN_POS)
m_pos=PIXY_RCS_MIN_POS
}

m_prevError=error
}

voidsetup()
{
Serial.begin(9600)
Serial.print("Starting...\n")

pixy.init()
}

voidloop()
{
staticinti=0
intj
uint16_tblocks
charbuf[32]
int32_tpanError,tiltError

blocks=pixy.getBlocks()

if(blocks)
{
panError=X_CENTERpixy.blocks[0].x
tiltError=pixy.blocks[0].yY_CENTER

panLoop.update(panError)
tiltLoop.update(tiltError)

pixy.setServos(panLoop.m_pos,tiltLoop.m_pos)

i++

//dothis(print)every50framesbecauseprintingevery
//framewouldbogdowntheArduino
if(i%50==0)
{
sprintf(buf,"Detected%d:\n",blocks)
Serial.print(buf)
for(j=0j<blocksj++)
{
sprintf(buf,"block%d:",j)
Serial.print(buf)
pixy.blocks[j].print()

}
}
}
}