salamander closing report - sutd-mit international design ... · the uniqueness of this design,...

2016

SALAMANDERPORTABLEAMPHIBIOUSSPHERICALROBOTSTEVANUSSATRIA,LEEJIAWEN,CHANWEITINGSAMANTHA

2

EXECUTIVESUMMARYForestedterrainsandmudflatssurroundedbywaterbodiessuchasrivers,lakes,andswampsaredifficult tomaneuver around, especially forman-made automations and robots. The lack ofreliableroboticplatformsoftenhindersoperationssuchassearch-and-rescue,surveillance,andresearcheffortsmadeintheseterrains.Whereasmostdevelopmentsarefocusedonimprovingmaneuverabilityoneither landorwater, littlehasbeendone indevelopingroboticplatformswithamphibiouscapability.Toaddress thisdesigngap,anamphibious spherical rolling robotdesignisproposed.Asphericalrollingrobotisadvantageousintermsofitsself-rightingcapability,balancing capability, andminimum damage to the surroundings during its mobilization. Theproposeddesigncontainsabattery-poweredfixedtwo-axispendulumfortherobot’spowertrain,whichprovides the robotwith its self-righting capability. Twomodular live-feed cameras areinsertedtoprovide live-streamingcapability,bothofwhichcanbeeasily replacedwithotherrelevantsensorsifneeded.Acustom-made3D-printedsleeveisdesignedtoimprovetherobot’spropulsion on both land andwater.Most of the robot’s design features are highlymodular,facilitating customizationdependingon theuser’sneeds.With theseconfigurations, the finalprototypeexhibitedahighdegreeofmaneuverabilityonbothlandandwater.Thelive-streamingcapabilitywasprovenwithavideorecordingfromtherobot’scameraswhendeployed.AGPSmodulewasaddedlaterontoenablereal-timetrackingoftherobot’spositionviaGoogleMaps.Furthermore,implementationsofaclosed-loopcontrollerandwirelesschargingtechnologywereattempted,bothofwhichwouldimprovetherobot’sperformanceevenfurther.

3

TableofContents

EXECUTIVESUMMARY 2

MOTIVATION 4

USERNEEDSANDDESIGNSPECIFICATIONS 4

MECHANICALSUBSYSTEM 5

DRIVETRAINEXPLORATION 5WATERPROOFING 6

ELECTRONICSSUBSYSTEM 7

DRIVECONTROL 7SENSINGMECHANISM 8TRACKINGMECHANISM 8

RESULTSANDDISCUSSION 9

ASSEMBLEDPROTOTYPE 9TESTS 9

OUTREACH 10

MAKERFAIREEUROPEANEDITION2015ANDDISCOVERYCANADA’SDAILYPLANETSHOW 10OTHERPRESENTATIONS 10

PROPOSEDIMPROVEMENTS 11

CONTROLLERIMPLEMENTATION 11DYNAMICMODELING 11MODELVERIFICATION 12IMUIMPLEMENTATION 12WIRELESSCHARGINGEXPLORATION 13

CLOSING 15

ACKNOWLEDGMENT 15

REFERENCES 16

APPENDICES 17

APPENDIXA:GPSARDUINOSKETCH 17APPENDIXB:DYNAMICMODELINGFULLDERIVATION 19APPENDIXC:DYNAMICMODELINGMATLABCODE 23APPENDIXD:MPU6050EXAMPLECODE 26APPENDIXE:BLUETOOTHPCCONTROLTESTCODEFORMODELVERIFICATION 38

4

MOTIVATIONForestedterrainsandmudflatssurroundedbywaterbodiessuchasrivers,lakes,andswampsaredifficult tomaneuver around, especially forman-made automations and robots. The lack ofreliableroboticplatformsoftenhindersoperationssuchassearch-and-rescue,surveillance,andresearcheffortsmadeintheseterrains.Whereasmostdevelopmentsarefocusedonimprovingmaneuverabilityoneither landorwater, littlehasbeendone indevelopingroboticplatformswithamphibiouscapability.ThecurrentavailablesolutionistheGroundBot™byRotundus[1],whichforallitscapabilitiesandfeatures,wasbulkyandheavy.Thus,theTeamwishedtodesignandprototype aportable yet amphibious spherical rolling robot capableof both groundandaquaticdeploymentinvariousabovementionedapplications.

USERNEEDSANDDESIGNSPECIFICATIONSGiventheneedtosatisfyitsintendedapplicationswhilebeingportable,thefollowinguserneedswerededuced:1. Therobothastobesphericalandbeabletoroll.2. Therobotdoesn’tbreakdownunderabuse.3. Therobotissmallenoughtoenterconfinedspaces.4. Therobotcanberemotelyoperated.5. Therobotcandriveoveruneventerrains.6. Therobotisamphibious.7. Therobotcanprovideliveimagefeedtotheuser.8. Therobotcaneasilymaneuverpassobstacles.9. Therobotcanworkinharshtemperaturesandenvironments.Translatingtheseintodesignrequirements,thefollowinglistwasobtained:1. Theemptyshellshallbeabletorolldownaslopewith20˚inclination.2. Theunitshallbeabletowithstandadropfromaheightof1mabovethewatersurface.3. Theunitshallbeabletosurviveacrashatfullspeed.4. Theunitshallbeabletowithstandrollingdown20flightsofstairs.5. Theunitshallhaveadiameterofnotmorethan30cm.6. Theunitshallweightlessthan5kg.7. Theunitshallbefullycommunicablewithina10mradius.8. Theresponsetimeoftheunitshallnotexceed1s.9. Theunitshallstaywithin20%ofitsintendeddirectionwhentravellingthroughrough

terrain.10. Theunitshallbeabletoescapea5cm-deeppothole.11. Theunitshallbeabletofloat.12. Theunitshallbewaterproof.13. Theunitshallbeabletohousealive-feedcamera.14. Theunitshallnotdeviatefromtheintendeddirectionofmotionbymorethan10%onflat

ground.

5

15. Theunitshallbefullyassembledwithin6hours.16. Theunitshallnotrequireanyspecialtoolsforassembly.17. Theunitshallbeassembledwithzerocalibration.Withalltheseinmind,theTeambeganthedesignprocessbyexploringthevariousmechanicalpropertiesofasphericalrobot.

MECHANICALSUBSYSTEMDrivetrainExplorationEssentiallythereweretwotypesofsphericalrobots:(a)transformersand(b)rollingrobot.Theformer incorporated intricate reconfiguration which allowed for increased versatility in therobot’slocomotion.TheMorpHexwasonesuchrobot[2].Thelatter,ontheotherhand,movedpurelybasedonrolling.Variousactuatorshadbeendesignedinordertoachievethismotion,fromthesimplekart-in-wheeltothemoreelaborateomnidirectionalsystemofshiftingmasses[3].

Fig.1.1.VariousSphericalRollingRobotActuators(fromLefttoRight):Two-AxisPendulum,DifferentialDrivePendulum,andShiftingMassesSystem[3]

TheTeamthusadopteda3-stageapproachtodecideontheidealdrivetrainfortherobot:(a)primary brainstorming phase when individuals propose various existing or new designs, (b)preliminary selection phasewhere certain designswere filtered out based on feasibility andreliability considerations, and finally (c) selection phase where remaining designs wereconsideredusingaPughChart.

CRITERIA

GearedWheels+Steering

PendulumTwo-AxisPendulum

4OmniWheels

Maneuverability 0 0 1Complexity -1 0 -2Assembly -1 0 0

MaterialCost 0 0 -1Durability 0 0 -1TOTAL -2 0 -3

Table1.DrivetrainPughChart

6

Giventheaboveconsiderations,itwasdecidedthattheTwo-AxisPendulummechanismwasthemost appropriate for the Team’s design. Using the Computer AidedDrawing (CAD) softwareSolidWorks, the Team rendered an impression of the selected mechanism adapted to theintendedprototype,asshownbelow:

Fig.1.2.Two-AxisPendulumDrivetrainRender:IsometricView(Left)andTopView(Right)

ThemaindriverofthisdesignwasthePololu298:1MicrogearMotorwitha3:1GearRatio.Basedonasimplestatictorquecalculation[4],a100:1MicrogearMotorwiththesame3:1GearRatioshould be sufficient. Nevertheless, a safety factor of 3 was chosen to improve the robot’sreliability.Thewaterjetsteelcounterweights,ontheotherhand,weredrivenbyaservomotormountedbelowthemainhousing.Steelwasselectedtobethematerialofthecounterweighttoensurethatitwasheavyenoughtocreatethenecessarymomentforturning.WaterproofingAnotherintegralcomponentoftheTeam’sdesignwaswaterproofing,giventherobot’sintendedaquaticapplications.Inordertodoso,allopeningsmustbeproperlysealedwithrubber-basedmaterials.ThesimpleassemblyoftheTwo-AxisPendulumdrivetrainmeantthattherewereonlythreemajoropeningstobesealed:thetwomainshaftmountingholes(oneoneachhalfofthesphere)andthepartinglineofthesphericalshell.Theformertwowerequicklysolvedbypressingtworubbergasketswithwasherspriortotighteningthemountingnutsoftherobot.Thelatter,however,requiredtheimplementationofacustomsleevedesign.TheTeam’ssolutiontothiswasacustom3DprintedNinjaFlex®Sleeve[4],asshownbelow:

Fig.1.3.Custom3DPrintedNinjaFlex®Sleeve

7

NinjaFlex®itselfwasaflexiblefilamentprintableusingatypical3DprintersuchasMakerBot.TheTeamdesignedtheSleevearoundthispropertytoensureitsremovabilitywhilemaintainingatightfitwhenapplied.Theridgesontheexteriorweredesignedspecificallytoimprovetractionbothonlandandwater.Overall,theSleeveendedupdoublingasbothawaterproofingsolutionaswellasaperformanceenhancingfeatureoftherobot.

ELECTRONICSSUBSYSTEMDriveControlThefollowinghighlightstheflowofinputcommandfromtheusertotherobot:

Fig.2.1.LogicFlowchart

TheDFRobotRomeowas chosen as the robot’smicroprocessorbecauseof its in-builtmotordriversaswellasavailabilityofmultipleservopins,all inasizethatwas ideal fortheTeam’sdesign[5].Thein-builtmotordriverutilizedPulse-Width-Modulation(PWM)signalstocontrolthemotor’s speed.AnXBeemodulewas connected to theRomeo to enable communicationbetween themicroprocessor and the smartphone (through a custom application). Thus, theTeam needed only to code a sketchwhich listened to the signal from the smartphone, andconvert it to PWM and/or servo signals which would in turnmove themotor and/or servoaccordingly.

Fig.2.2.DFRobotRomeov2.2[5]

AndroidApp ArduinowithXBee Motor/Servo

8

SensingMechanismGiventheneedforlive-streamingcapabilities,twoAi-Ballcamerasweremountedoneachsideoftherobot’smainhousing.Thecurrentdesigninvolvedmountingoneasforward-facingandtheotherasdownward-facing.Therationalebehindthiswastoenableunderwatermonitoring(providedthewaterisclear)whilemaintainingtheavailabilityoftheforwardview.Toachievethis,twocustommountsweredesignedfortheAi-Ballcameras.Theuniquenessofthisdesign,however,wasitsmodularity,enablingeasyexchangewithotherdesirablesensorswhentheneedarose.Oneonlyneededtodesignandprintanothermountwhichmountingholesfit intotheslotsavailableonthemainhousing.Forthisiteration,thecustommountsdesignisshownbelow:

Fig.2.3.CustomAi-BallCameraMount[4]

TrackingMechanismThefinalfeatureontheTeam’sdesignwasGPStracking,madepossiblebymountingaGPSUnitfrom DJI and transmitting the collected data through a 900MHz radio telemetry unit from3DRobotics.AseparateArduinomicroprocessorwasutilizedheretominimizetheloadoneachmicroprocessorandimproveperformance.BysettingaspecificIPaddressonone’sbrowsertosetupthecommunicationprotocolbetweenthetransmitterandreceivertelemetry,onewouldbeabletotracktherobot’sexactpositioninreal-timeusingGoogleMaps.

Fig.2.4.GPSTrackingScreenshot(TriangularArrowRepresentsRobot)

9

RESULTSANDDISCUSSIONAssembledPrototype

Fig.3.1.AssembledDrivetrain Fig.3.2.AssembledPrototype

TestsRequirements TestResultsTheunitshallbeabletowithstandadropfromaheightof1mabovethewatersurface.

1m

Theunitshallhaveadiameterofnotmorethan30cm.

15.5cm

Theunitshallstaywithin20˚ofitsintendeddirectionwhenenteringaroughterrain(e.g.gravelgrounds).

15.6˚

Minimumpendulumtooverallmassratio. 1.5:10Theunitshallbeabletohousetwolive-feedcameras.

2

Theunitshallweighlessthan1kg. 950g.

Table2.KeyTestResultsInaddition,theunitwasabletonotonlyfloat,butpickupconsiderablespeedwhenmovedonwater; proof that the ridges do improve the water propulsion of the robot. After drivingcontinuously for 15 minutes, no leakage was spotted, verifying the functionality of thewaterproofingmechanism.Theunitwasalsoabletowithstandathrowfromaheightof1mintothewater,simulatingitsabilitytobethrownbyastandingpersonatarm’slengthintothewaterbody.Lastly,theGPStrackingfeatureworkedextremelywell,givingnearlytheexactpositionoftherobotatalltimes.

10

Thesimplemodulardesignofthemechanismsmeantthatassemblyprocesswasmucheasierthanexpected.Ittookonaveragelessthan3hourstofullyassembletherobotwiththetoolscommonly found in aworkshop, and the only adjustment to bemade is the position of thecamerasinsidetheshell.[4]

OUTREACHMakerFaireEuropeanEdition2015andDiscoveryCanada’sDailyPlanetShowTheTeammanagedtoactuallybringtheprototypeintothelimelightbygettingitacceptedintotheMakerFaireEuropeanEdition.ThisEditionoftheMakerFairewastheworld’s2nd largestMakerFaire,andin2015itwouldbeheldatLaSapienzaUniversityinRome,Italy.AttheMakerFaire,theteamshowcasedthecreationconsecutivelyfor2daysinfrontoftheEuropeancrowd.Theprototypeexhibitedreceivedlotsofpraisesfrommembersofthepublicforitsuniquenessaswellasmassivepotential,evenattractingseveralentitieswhowereinterestedincollaboratingwiththeTeamtobringthisproductforward.ArduinoalsoapproachedtheTeamtoinquireabouthepossibilityoffeaturingtherobotinitsblog.

Figure4.1.TheTeamandTheRobotatMakerFaireEuropeanEdition2015

OntopofshowcasingtheproductattheMakerFaireRome,theteamwasalsoapproachedbyDiscoveryChannel Canada’s “Daily Planet”,whowere interested in featuring the robot in itsteaserepisodepriortotheMakerFaireitself.ThevideowasbroadcastedontheCanadianTVchannelonFriday,16thOctober2015.AttachedistheYouTubevideoofthebroadcastcanbefoundhere:https://www.youtube.com/watch?v=2tRMoOzDQLk.OtherPresentationsWithinSUTD,theTeamalsopresentedtheprototypetoDeputyPrimeMinisterTeoCheeHeanduringhisvisittoSUTD.TheprototypewasalsofeaturedinvariousSUTDeventssuchasOpenHouses,aswellasothervisitsbyentitiessuchtheMinistryofDefence(MINDEF)andMinistryofEducation(MOE).

11

PROPOSEDIMPROVEMENTSControllerImplementationDynamicModelingTokick-startthecontrollerdesignprocess,theTeamutilizedaLagrangianEnergyApproachtoderive(andlinearize)theequationsofmotionofthependulumdrivemechanism.Prioritywasgiven first to the forward motion of the robot, because upon consultation with a ResearchAssistantatRobotics InnovationLab,AkashAjayVibhute, theTeamdiscovered that itwillbeimpossibletofullycontrola3degrees-of-freedomsystem(forward,turning,andtilting)withatwo degrees-of-freedom drivemechanism (two-axis pendulum). Limiting the problem into asingledegree-of-freedomwillthusenabletheTeamtoattheveryleastminimizetheoscillationoftherobotwhenstartingandstoppingitsforwardmotion.Thederivationofthemodelisasfollows:

Fig.5.1.ModelingDiagram

Theobtained(linearized)modelinstate-spaceformatisasfollows:

12

ModelVerificationTosimulatetheaccuracyofthemodel,aMATLABcodewascraftedtoobtaintheresponsegraphofthemodelwithagivensteptorqueinput.Theresponsegraphobtainedisasshownbelow:

Fig.5.2.Thetavs.TimeResponse

Fig.5.3.ThetaDotvs.TimeResponse

Fromtheabove,itcanbeseenthattheresponseresembledtheactualsystemprettywell.Thus,theTeamhadreasonableconfidenceinthevalidityofthemodel.However,abetterverificationwouldbeusingtheViconCameratoobtainagraphfortheactualsystemaswell.Furthermore,the Vicon Cameras was also integral to accurately fill the constant values (e.g. dampingcoefficient)neededbeforeproceedingwiththecontrolalgorithmdesign.

IMUImplementationInordertoobtainaccuratemeasurementsofthedatatoverifythedynamicscontrolmodel,thecurrentCADdesignoftherobotwasmodifiedtoincludeadditionalspacesandfittingsforthemountingofsensorattachments.

13

Thesensormountisdesignedtobefittedin3areas,thesides,center-topandcenter-frontthusallowingdatatobecollectedatdifferentlocationsoftherobot.Themountsatthesideofrobotismadevariableaswell,assuchoneisfixedandtheotherhasaswivelthusallowingthedesignertorigitatanangleshouldtherebeaneed.

Fig.5.4.OverviewofRobotwithSensorMounts

Fig.5.5.Close-UpViewofMountsDesign(FromLefttoRight:Fixed,Center,andSwivel)

WirelessChargingExplorationTheinitialdesignoftherobotrequiresdismantlingoftheoutershelltoaccessandtochargeits2cellLithiumPolymerbattery.Sincetheeaseofuseisanimportantaspectofthedesignoftherobot,theteamlookedintoimplementingawirelesschargingsystem.Two9Vwirelesschargingcouples (EF03092), DC input 2-3 cell balance charger (PAS-CHG-2S3S) and step-up voltageregulator(U3V12F12)wereusedtoprototypethesystem.

14

Awirelesschargingcoupleconsistsofatransmitterunit(includingthecoil),whichproducesachangingmagneticfield,andareceiverunit,inwhichvoltageisinducedduetothefluctuatingfield. Due to limited information and datasheets for thewireless charger couple, a testwasconducted tomeasure the amount of voltage attainedwhen transmitter and receiver are atdifferent distances apart. A 12V DC, 1A input was connected to the transmitter unit and amultimeterwasconnectedtothereceiverunittomeasurethevoltageinduced,themultimeterthenwasconnectedinserieswithanexternalload(balancecharger)tomeasurethecurrent.

DistanceApart(mm) Voltage(V) Current(mA)

0 9.4 601

5 9.39 462

10 9.4 220

15 9.38 79

20 7.03 17

25 4.54 5

Table3.WirelessChargingCoupleTestData

Thetestwasconductedwithairasthemedium(noobjectsbetweenthetransmitterandreceiver).Aseparatetestwasheldthathadtheouterplasticshelloftherobotbetweentheunits,therewasfoundtobenegligibledifferencebetweentheresultsofthesetwotests,thusthepresenceoftheshellwouldnotgreatlyaffecttheefficiencyofthewirelesspowertransmission.

Thebalancechargerhasaninputratingof10-15VDCanddrawsacurrentof800mA.Tomatchthese ratings, the twowireless charger receiver unitswere connected in parallel to attain avoltageof9.4Vandameasuredcurrentof1.12A.Thisoutputcouldthenbefedthroughastep-upvoltageregulatortogetatotaloutputof12Vand850mA,whichissufficientenoughtopowerthebalancechargerandchargea2cellLithiumPolymerbattery.

Theproposeddesignthatwouldbeintegratedwiththerobotshouldbesuchthatthereceiverunits, voltage regulator and balance charger are attached to a redesigned pendulum piece,shiftingthecenterofmasslowertoprovidebetterstability.Thereceiverunitsshouldbeatthefurthest end of the pendulum, as close to the shell as possible but without touching it, tomaximize the voltage and current received. The balance chargerwould be connected to thebalance charging port of the robot’s on-board Lithium Polymer battery. There would be a

15

separatechargingstationwiththetransmitterunitswheretheusercanplacetherobotonandchargeitwithouttoneedtodismantleit.

Theprototype’sdesignisnotidealastworeceiverunitsandaregulatorarerequiredtopowerthebalancecharger,addingextraweightof233gtotherobot(balancecharger:200g,receivers:25g,regulator:8g).Theweightcouldbeminimizedbyusingwirelesschargingset(s)whichcantransmitaminimumof10V,800mA.However,suchunitsarenotreadilyavailableoff-the-shelfandtheTeamwasunabletosourceforthem,therefore9Vsets(highestvoltageoutputthatcouldbesourced)wereusedinstead.Alternatively,awirelesschargingpaircouldbefabricatedbuttheprocessiscomplexandwouldnotbeasefficientorcompactasoff-the-shelfones.Thesizeoftherobotmayalsoneedtobeincreasedfrom16to20cmtoaccommodatetheadditionalwirelesschargingparts.

Withtheseprojectfindingsandconsiderations,thesuggestedwirelesschargersystemmeetsthepower requirements and could be fittedwith Salamander. Further steps like redesigning thependulummountanddesignofthechargingstationwouldbeneededtofully implementthesystem.

CLOSINGAll inall, theTeammanagedtocomeupwithan innovativeandgroundbreakingdesignforaportableamphibioussphericalrollingrobot,possessingmassivepotentialtobecomingagame-changing product to aid various surveillance, research, and search-and-rescuemissions. Theportability,ease-of-assembly,andmodulardesignhaveallhelpedinmakingtherobotaversatileanduser-friendlypieceoftechnology.Theravereviewsitgarneredduringthevariousexhibitionsitundertook–mostimportantlytheMakerFaireEuropeanEdition2015–weretestamentstothepotentialthisdesignhas.Nonetheless,alotneedstobedonetotakethisdesigntothenextlevel,anditwasunfortunatethattheTeamcouldnotbringtheseimprovementsintoareality.Nonetheless, the Team believed that the foundational works of modeling, designing, andresearchingthevariousproposedimprovementshavebeendone,andhopedthatanyonetakingoverthisprojectwouldonedaytrulybringthebestoutofthisfantasticprototypeintotheworld.

ACKNOWLEDGMENTThe Team gratefully acknowledge the support of the TL@SUTD: Systems Technology forAutonomousReconnaissance&Surveillancegrant.SupportisalsoacknowledgedfromtheSUTD-MITInternationalDesignCentreandSUTDRoboticsInnovationLab(RIL).SpecialthankstoProf.KevinOttoandProf.SubburajKarupppasamyfortheirsupport,suggestions,andfeedbackduringthedesignandprototypingphase,Prof.FoongShaohuiforthementorshipreceivedduringtheresearchphaseofthisproject,SongBinyangforhersupportinallowingustoworkintheARMSLab after office hours, Andy Lim Kok Pin and Jackson Chng KokWee for their assistance inmachining custom-made parts, Richard Goh Chin Twee for the 3D-printing facilities in theFabricationLab,ChuWenjingandNyanLinHtatformanagingtheARMSLabwheremostofthe

16

assembly and testing are done, and Akash Ajay Vibhute for his technical advice as well asmanagementoftheRIL.

REFERENCES[1]"Rotundus",Rotundus.se,2016.[Online].Available:http://www.rotundus.se/.[Accessed:02-Sep-2016].[2]"MorpHexpart1-ZentaRoboticCreations",ZentaRoboticCreations,2016.[Online].Available:http://zentasrobots.com/robot-projects/morphex-part-1/.[Accessed:02-Sep-2016].[3]R.ChaseandA.Pandya,"AReviewofActiveMechanicalDrivingPrinciplesofSphericalRobots",Robotics,vol.1,no.1,pp.3-23,2012.[4]S.Satria,J.LeeandW.Chan,"PortableAmphibiousSphericalRollingRobotwithLive-StreamingCapabilityforGroundandAquaticDeployment",inIRCConferenceonScience,Engineering,andTechnology,NationalUniversityofSingapore,2015.[5]"DFRobot-QualityArduinoandRoboticsProducts",Dfrobot.com,2016.[Online].Available:http://dfrobot.com.[Accessed:02-Sep-2016].

17

APPENDICESAppendixA:GPSArduinoSketch#include<SoftwareSerial.h>#include"NazaDecoderLib.h"SoftwareSerialgps(2,3);//rx,txconstbyteLED=13;unsignedlongpreviousTime;unsignedlongcurrentTime;unsignedlongpreviousTime2;unsignedlongcurrentTime2;voidsetup(){Serial.begin(115200);gps.begin(115200);pinMode(LED,OUTPUT);}voidloop(){//readNAZA'sSerialportif(gps.available()){uint8_tdecodedMessage=NazaDecoder.decode(gps.read());switch(decodedMessage){caseNAZA_MESSAGE_GPS:currentTime=millis();unsignedlongdiff=currentTime-previousTime;if(diff>10){printPort();previousTime=currentTime;}//printPort1();break;}}}

18

voidprintPort(){intsat=NazaDecoder.getNumSat();doubledroneSpeed=NazaDecoder.getSpeed();doublelat=NazaDecoder.getLat();doublelon=NazaDecoder.getLon();doublespd=NazaDecoder.getSpeed();doublebearing=NazaDecoder.getHeading();Serial.print(lat,6);Serial.print("x");Serial.print(lon,6);Serial.print("y");Serial.print(sat);Serial.print("s");Serial.print(spd);Serial.print("v");Serial.print(bearing);Serial.print("b");Serial.print("\n");//while(Serial.available()>0)Serial.read();}

19

AppendixB:DynamicModelingFullDerivation

DegreesofFreedomà2Reference-MovingFrameRelationship:

𝜃"# = 𝜃%# = 𝜃%& + 𝜃&# Assumingno-slipconditions,

𝑣&# = −𝑟+𝜃&# 𝑣%# = 𝑣&# + 𝑟%𝜃%# = −𝑟+𝜃&# + 𝑟%𝜃%#

TotalTranslationalKineticEnergy,

𝐸-,/ =12𝑚&𝑣&#3 +

12𝑚%𝑣%#3

𝐸-,/ =12𝑚&𝑟&3𝜃&#

3 +12𝑚% −𝑟&𝜃&# + 𝑟%𝜃%#

3

𝐸-,/ =12𝑚&𝑟&3𝜃&#

3 +12𝑚%𝑟+3𝜃&#

3 +12𝑚%𝑟%3𝜃%#

3 − 𝑚%𝑟+𝑟%𝜃&#𝜃%#

TotalRotationalKineticEnergy,

𝐸#,/ =12 𝐼&𝜃&#

3 +12 𝐼%𝜃%#

3

𝐸#,/ =12 𝐼&𝜃&#

3 +12 𝐼%𝜃%#

3

TotalPotentialEnergy,𝑈 = −𝑚%𝑔𝑟% cos 𝜃%#

Non-conservativeWorkfromInputTorque,𝛿𝑊 = 𝜏𝛿𝜃%& = 𝜏𝛿𝜃%# − 𝜏𝛿𝜃&#

Non-conservativeWorkduetoDampingfromtheGearingMechanisms,

20

𝛿𝑊 = −𝑏𝑟𝜃%& 𝛿𝑟𝜃%& = −𝑏𝑟3𝜃%&𝛿𝜃%& = −𝑏𝑟3𝜃%#𝛿𝜃%# + 𝑏𝑟3𝜃&#𝛿𝜃&# UsingLagrangeMethod,

𝑚& +𝑚% 𝑟&3 + 𝐼& 𝜃&# − 𝑚%𝑟+𝑟%𝜃%# = −𝜏 + 𝑏𝑟3𝜃&# 𝑚%𝑟%3 + 𝐼% 𝜃%# − 𝑚%𝑟+𝑟%𝜃&# + 𝑚%𝑔𝑟% sin 𝜃%# = 𝜏 − 𝑏𝑟3𝜃%#

Simplifying,weget,(1),

𝜃%# =𝑚& +𝑚% 𝑟&3 + 𝐼&

𝑚%𝑟+𝑟%𝜃&# −

𝑏𝑟3

𝑚%𝑟+𝑟%𝜃&# +

𝜏𝑚%𝑟+𝑟%

𝜃%# −𝑚%𝑟+𝑟%

𝑚%𝑟%3 + 𝐼%𝜃&# +

𝑚%𝑔𝑟%𝑚%𝑟%3 + 𝐼%

sin 𝜃%# =𝜏 − 𝑏𝑟3𝜃%#𝑚%𝑟%3 + 𝐼%

or(2),

𝜃&# =𝑚%𝑟+𝑟%

𝑚& + 𝑚% 𝑟&3 + 𝐼&𝜃%# +

𝑏𝑟3

𝑚& + 𝑚% 𝑟&3 + 𝐼&𝜃&# −

𝜏𝑚& + 𝑚% 𝑟&3 + 𝐼&

𝑚%𝑟%3 + 𝐼%𝑚%𝑟+𝑟%

𝜃%# − 𝜃&# +𝑚%𝑔𝑟%𝑚%𝑟+𝑟%

sin 𝜃%# =𝜏 − 𝑏𝑟3𝜃%#𝑚%𝑟+𝑟%

(1)and(2)giveriseto,

ABCAD EBFCGBADEHED

− ADEHEDADEDFCGD

𝜃&# −IEF

ADEHED𝜃&# +

ADJEDADEDFCGD

sin 𝜃%# +IEF

ADEDFCGD𝜃%# =

KADEDFCGD

− KADEHED

ADEDFCGDADEHED

− ADEHEDABCAD EBFCGB

𝜃%# +ADJEDADEHED

sin 𝜃%# +IEF

ADEHED𝜃%# −

IEF

ABCAD EBFCGB𝜃&# =

KADEHED

− KABCAD EBFCGB

ConvertingtoState-Space,𝑧M = 𝜃&# 𝑧3 = 𝜃%#

𝑧N = 𝑧M = 𝜃&# 𝑧O = 𝑧3 = 𝜃%#

21

𝑧N =

𝑏𝑟3𝑚%𝑟+𝑟%

𝑚& + 𝑚% 𝑟&3 + 𝐼&𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚%𝑟%3 + 𝐼%

𝑧N

−

𝑚%𝑔𝑟%𝑚%𝑟%3 + 𝐼%

𝑚& + 𝑚% 𝑟&3 + 𝐼&𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚%𝑟%3 + 𝐼%

sin 𝑧3

−

𝑏𝑟3𝑚%𝑟%3 + 𝐼%

𝑚& + 𝑚% 𝑟&3 + 𝐼&𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚%𝑟%3 + 𝐼%

𝑧O

+

𝜏𝑚%𝑟%3 + 𝐼%

𝑚& + 𝑚% 𝑟&3 + 𝐼&𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚%𝑟%3 + 𝐼%

−

𝜏𝑚%𝑟+𝑟%

𝑚& + 𝑚% 𝑟&3 + 𝐼&𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚%𝑟%3 + 𝐼%

𝑧O =

𝑏𝑟3𝑚& + 𝑚% 𝑟&3 + 𝐼&

𝑚%𝑟%3 + 𝐼%𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚& + 𝑚% 𝑟&3 + 𝐼&

𝑧N

−

𝑚%𝑔𝑟%𝑚%𝑟+𝑟%

𝑚%𝑟%3 + 𝐼%𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚& + 𝑚% 𝑟&3 + 𝐼&

sin 𝑧3

−

𝑏𝑟3𝑚%𝑟+𝑟%

𝑚%𝑟%3 + 𝐼%𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚& + 𝑚% 𝑟&3 + 𝐼&

𝑧O

+

𝜏𝑚%𝑟+𝑟%

𝑚%𝑟%3 + 𝐼%𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚& + 𝑚% 𝑟&3 + 𝐼&

−

𝜏𝑚& + 𝑚% 𝑟&3 + 𝐼&

𝑚%𝑟%3 + 𝐼%𝑚%𝑟+𝑟%

− 𝑚%𝑟+𝑟%𝑚& + 𝑚% 𝑟&3 + 𝐼&

22

Linearizingthestate-spaceexpressionandconvertingitintomatrixform,

𝒛 =

0 0 1 00 0 0 1

0 −𝑚𝑃𝑔𝑟𝑃

𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

𝑚𝑃𝑟𝑠𝑟𝑃− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃

𝑏𝑟2𝑚𝑃𝑟𝑠𝑟𝑃

𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃

−𝑏𝑟2

𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

𝑚𝑃𝑟𝑠𝑟𝑃− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃

0 −𝑚𝑃𝑔𝑟𝑃𝑚𝑃𝑟𝑠𝑟𝑃

𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

UVF

WBXWD VBFXYB𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

−𝑏𝑟2

𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

𝒛 +

00

1𝑚𝑃𝑟𝑃2+𝐼𝑃

− 1𝑚𝑃𝑟𝑠𝑟𝑃

𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃

1𝑚𝑃𝑟𝑠𝑟𝑃

− ZWBXWD VBFXYB

𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

𝝉

Hence,wehave:𝐴 =0 0 1 00 0 0 1

0 −𝑚𝑃𝑔𝑟𝑃

𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

𝑚𝑃𝑟𝑠𝑟𝑃− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃

𝑏𝑟2𝑚𝑃𝑟𝑠𝑟𝑃

𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃

−𝑏𝑟2

𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

𝑚𝑃𝑟𝑠𝑟𝑃− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃

0 −𝑚𝑃𝑔𝑟𝑃𝑚𝑃𝑟𝑠𝑟𝑃

𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

UVF

WBXWD VBFXYB𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

−UVF

𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

𝐵 =

00

1𝑚𝑃𝑟𝑃2+𝐼𝑃

− 1𝑚𝑃𝑟𝑠𝑟𝑃

𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑃𝑟𝑃2+𝐼𝑃

1𝑚𝑃𝑟𝑠𝑟𝑃

− ZWBXWD VBFXYB

𝑚𝑃𝑟𝑃2+𝐼𝑃𝑚𝑃𝑟𝑠𝑟𝑃

− 𝑚𝑃𝑟𝑠𝑟𝑃𝑚𝑆+𝑚𝑃 𝑟𝑆2+𝐼𝑆

23

AppendixC:DynamicModelingMATLABCode

functionSALAMANDER_Dynamical_Model

%clearall

closeall

%clc

%parameters

ms=.5;%massofsphericalshell+MCU

mp=.4;%massofpendulum

rs=.0775;%radiusofsphericalshell

rp=.05;%radiusofpendulum

g=9.81;%gravitationalconstantinSIUnit

Is=0;%momentofinertiaofshell

Ip=0;%momentofinertiaofpendulum

b=0.5;%dampingcoefficient

dz3=((((ms+mp)*rs*rs+Is)/(mp*rs*rp))-mp*rs*rp/(mp*rp*rp+Ip));%denominatorforz3dot

dz4=(((mp*rp*rp+Ip)/(mp*rs*rp))-(mp*rs*rp)/((ms+mp)*rs*rs+Is));%denominatorforz4dot

r=0.2;%gearradius

tao=1;%inputtorqueinSIUnit

param=[ms,mp,rs,rp,g,Is,Ip,b,dz3,dz4,r,tao];

%state-spacematrix

A = [0,0,1,0;0,0,0,1;0,-(mp*g*rp/(mp*rp*rp+Ip))/dz3,(b*r*r/(mp*rs*rp))/dz3,-(b*r*r/(mp*rp*rp+Ip))/dz3;0,-((mp*g*rp)/(mp*rs*rp))/dz4,(b*r*r/((ms+mp)*rs*rs+Is))/dz4,-((b*r*r)/(mp*rs*rp))/dz4]

B=[0;0;(1/(mp*rp*rp+Ip)-1/(mp*rs*rp))/dz3;(1/(mp*rs*rp)-1/((ms+mp)*rs*rs+Is))/dz4]

Co=[B,A*B,A*A*B,A*A*A*B]

Poles=eig(A)

Determinant_of_Co=det(Co)

%initialconditions

24

initial_thetas=0;%initialthetasphere

initial_thetap=0.5;%initialthetapendulum

initial_thetasd=0;%initialthetaspheredot

initial_thetapd=0;%initialthetapendulumdot

%timescale

t=linspace(0,10,1000);

%odesolve

[t,z]=ode45(@(t,z)rhs(t,z,param),t,[initial_thetasinitial_thetapinitial_thetasdinitial_thetapd],param);

[t,x]=ode45(@(t,x)rhsl(t,x,param),t,[initial_thetasinitial_thetapinitial_thetasdinitial_thetapd],param);

figure(1);

subplot(2,1,1);plot(t,z(:,1),'b',t,z(:,2),'r');xlabel('Time');ylabel('Theta');title('Thetavs.Time');

subplot(2,1,2);plot(t,z(:,3),'b',t,z(:,4),'r');xlabel('Time');ylabel('Theta_d_o_t');title('Theta_d_o_tvs.Time');

figure(2);

subplot(2,1,1);plot(t,x(:,1),'b',t,x(:,2),'r');xlabel('Time');ylabel('Theta');title('LinearizedThetavs.Time');

subplot(2,1,2);plot(t,x(:,3),'b',t,x(:,4),'r');xlabel('Time');ylabel('Theta_d_o_t');title('LinearizedTheta_d_o_tvs.Time');

functiondzdt=rhs(t,z,param)%herewewritethedifferentialequationthatwewanttosolve

ms1=param(1);%massofsphericalshell+MCU

mp1=param(2);%massofpendulum

rs1=param(3);%radiusofsphericalshell

rp1=param(4);%radiusofpendulum

g1=param(5);%gravitationalconstantinSIUnit

Is1=param(6);%momentofinertiaofshell

Ip1=param(7);%momentofinertiaofpendulum

b1=param(8);%dampingcoefficient

dz31=param(9);%denominatorforz3dot

dz41=param(10);%denominatorforz4dot

25

r1=param(11);%gearradius

tao1=param(12);%inputtorqueinSIUnit

dzdt_1=z(3);

dzdt_2=z(4);

dzdt_3 = ((b1*r1*r1/(mp1*rs1*rp1))/dz31)*z(3)-(mp1*g1*rp1/(mp1*rp1*rp1+Ip1))/dz31*sin(z(2))-(b1*r1*r1/(mp1*rp1*rp1+Ip1))/dz31*z(4)+(tao1/(mp1*rp1*rp1+Ip1))/dz31-(tao1/(mp1*rs1*rp1))/dz31;

dzdt_4=((b1*r1*r1/(mp1*rs1*rp1))/dz41)*z(3)-(g1/rs1)/dz41*sin(z(2))-(b1*r1*r1/(mp1*rs1*rp1))/dz41*z(4)+(tao1/(mp1*rs1*rp1))/dz41-(tao1/(mp1*rp1*rp1+Ip1))/dz41;

dzdt=[dzdt_1;dzdt_2;dzdt_3;dzdt_4];

end

functiondxdt=rhsl(t,x,param)%herewewritethedifferentialequationthatwewanttosolve

ms1=param(1);%massofsphericalshell+MCU

mp1=param(2);%massofpendulum

rs1=param(3);%radiusofsphericalshell

rp1=param(4);%radiusofpendulum

g1=param(5);%gravitationalconstantinSIUnit

Is1=param(6);%momentofinertiaofshell

Ip1=param(7);%momentofinertiaofpendulum

b1=param(8);%dampingcoefficient

dz31=param(9);%denominatorforz3dot

dz41=param(10);%denominatorforz4dot

r1=param(11);%gearradius

tao1=param(12);%inputtorqueinSIUnit

dxdt_1=x(3);

dxdt_2=x(4);

dxdt_3 = ((b1*r1*r1/(mp1*rs1*rp1))/dz31)*x(3)-(mp1*g1*rp1/(mp1*rp1*rp1+Ip1))/dz31*sin(x(2))-(b1*r1*r1/(mp1*rp1*rp1+Ip1))/dz31*x(4)+(tao1/(mp1*rp1*rp1+Ip1))/dz31-(tao1/(mp1*rs1*rp1))/dz31;

dxdt_4=((b1*r1*r1/(mp1*rs1*rp1))/dz41)*x(3)-(g1/rs1)/dz41*sin(x(2))-(b1*r1*r1/(mp1*rs1*rp1))/dz41*x(4)+(tao1/(mp1*rs1*rp1))/dz41-(tao1/(mp1*rp1*rp1+Ip1))/dz41;

dxdt=[dxdt_1;dxdt_2;dxdt_3;dxdt_4];

end

end

26

AppendixD:MPU6050ExampleCode//I2Cdeviceclass(I2Cdev)demonstrationArduinosketchforMPU6050classusingDMP(MotionAppsv2.0)

//6/21/2012byJeffRowberg<jeff@rowberg.net>

//Updatesshould(hopefully)alwaysbeavailableathttps://github.com/jrowberg/i2cdevlib

//

//Changelog:

//2013-05-08-addedseamlessFastwiresupport

//-addednoteaboutgyrocalibration

//2012-06-21-addednoteaboutArduino1.0.1+Leonardocompatibilityerror

//2012-06-20-improvedFIFOoverflowhandlingandsimplifiedreadprocess

//2012-06-19-completelyrearrangedDMPinitializationcodeandsimplification

//2012-06-13-pullgyroandacceldatafromFIFOpacketinsteadofreadingdirectly

//2012-06-09-fixbrokenFIFOreadsequenceandchangeinterruptdetectiontoRISING

//2012-06-05-addgravity-compensatedinitialreferenceframeaccelerationoutput

//-add3DmathhelperfiletoDMP6examplesketch

//-addEuleroutputandYaw/Pitch/Rolloutputformats

//2012-06-04-removeacceloffsetclearingforbetterresults(thanksSungonLee)

//2012-06-01-fixedgyrosensitivitytobe2000deg/secinsteadof250

//2012-05-30-basicDMPinitializationworking

/*============================================

I2CdevdevicelibrarycodeisplacedundertheMITlicense

Copyright(c)2012JeffRowberg

Permissionisherebygranted,freeofcharge,toanypersonobtainingacopy

ofthissoftwareandassociateddocumentationfiles(the"Software"),todeal

intheSoftwarewithoutrestriction,includingwithoutlimitationtherights

touse,copy,modify,merge,publish,distribute,sublicense,and/orsell

copiesoftheSoftware,andtopermitpersonstowhomtheSoftwareis

furnishedtodoso,subjecttothefollowingconditions:

Theabovecopyrightnoticeandthispermissionnoticeshallbeincludedin

allcopiesorsubstantialportionsoftheSoftware.

27

THESOFTWAREISPROVIDED"ASIS",WITHOUTWARRANTYOFANYKIND,EXPRESSOR

IMPLIED,INCLUDINGBUTNOTLIMITEDTOTHEWARRANTIESOFMERCHANTABILITY,

FITNESSFORAPARTICULARPURPOSEANDNONINFRINGEMENT.INNOEVENTSHALLTHE

AUTHORSORCOPYRIGHTHOLDERSBELIABLEFORANYCLAIM,DAMAGESOROTHER

LIABILITY,WHETHERINANACTIONOFCONTRACT,TORTOROTHERWISE,ARISINGFROM,

OUTOFORINCONNECTIONWITHTHESOFTWAREORTHEUSEOROTHERDEALINGSIN

THESOFTWARE.

===============================================

*/

//I2CdevandMPU6050mustbeinstalledaslibraries,orelsethe.cpp/.hfiles

//forbothclassesmustbeintheincludepathofyourproject

#include"I2Cdev.h"

#include"MPU6050_6Axis_MotionApps20.h"

//#include"MPU6050.h"//notnecessaryifusingMotionAppsincludefile

//ArduinoWirelibraryisrequiredifI2CdevI2CDEV_ARDUINO_WIREimplementation

//isusedinI2Cdev.h

#ifI2CDEV_IMPLEMENTATION==I2CDEV_ARDUINO_WIRE

#include"Wire.h"

#endif

//classdefaultI2Caddressis0x68

//specificI2Caddressesmaybepassedasaparameterhere

//AD0low=0x68(defaultforSparkFunbreakoutandInvenSenseevaluationboard)

//AD0high=0x69

MPU6050mpu;

//MPU6050mpu(0x69);//<--useforAD0high

/*=========================================================================

28

NOTE:Inadditiontoconnection3.3v,GND,SDA,andSCL,thissketch

dependsontheMPU-6050'sINTpinbeingconnectedtotheArduino's

externalinterrupt#0pin.OntheArduinoUnoandMega2560,thisis

digitalI/Opin2.

*=========================================================================*/

/*=========================================================================

NOTE:Arduinov1.0.1withtheLeonardoboardgeneratesacompileerror

whenusingSerial.write(buf,len).TheTeapotoutputusesthismethod.

ThesolutionrequiresamodificationtotheArduinoUSBAPI.hfile,which

isfortunatelysimple,butannoying.ThiswillbefixedinthenextIDE

release.Formoreinfo,seetheselinks:

http://arduino.cc/forum/index.php/topic,109987.0.html

http://code.google.com/p/arduino/issues/detail?id=958

*=========================================================================*/

//uncomment"OUTPUT_READABLE_QUATERNION"ifyouwanttoseetheactual

//quaternioncomponentsina[w,x,y,z]format(notbestforparsing

//onaremotehostsuchasProcessingorsomethingthough)

//#defineOUTPUT_READABLE_QUATERNION

//uncomment"OUTPUT_READABLE_EULER"ifyouwanttoseeEulerangles

//(indegrees)calculatedfromthequaternionscomingfromtheFIFO.

//NotethatEuleranglessufferfromgimballock(formoreinfo,see

//http://en.wikipedia.org/wiki/Gimbal_lock)

//#defineOUTPUT_READABLE_EULER

//uncomment"OUTPUT_READABLE_YAWPITCHROLL"ifyouwanttoseetheyaw/

//pitch/rollangles(indegrees)calculatedfromthequaternionscoming

//fromtheFIFO.Notethisalsorequiresgravityvectorcalculations.

//Alsonotethatyaw/pitch/rollanglessufferfromgimballock(for

//moreinfo,see:http://en.wikipedia.org/wiki/Gimbal_lock)

29

#defineOUTPUT_READABLE_YAWPITCHROLL

//uncomment"OUTPUT_READABLE_REALACCEL"ifyouwanttoseeacceleration

//componentswithgravityremoved.Thisaccelerationreferenceframeis

//notcompensatedfororientation,so+Xisalways+Xaccordingtothe

//sensor,justwithouttheeffectsofgravity.Ifyouwantacceleration

//compensatedfororientation,usOUTPUT_READABLE_WORLDACCELinstead.

//#defineOUTPUT_READABLE_REALACCEL

//uncomment"OUTPUT_READABLE_WORLDACCEL"ifyouwanttoseeacceleration

//componentswithgravityremovedandadjustedfortheworldframeof

//reference(yawisrelativetoinitialorientation,sincenomagnetometer

//ispresentinthiscase).Couldbequitehandyinsomecases.

//#defineOUTPUT_READABLE_WORLDACCEL

//uncomment"OUTPUT_TEAPOT"ifyouwantoutputthatmatchesthe

//formatusedfortheInvenSenseteapotdemo

//#defineOUTPUT_TEAPOT

#defineLED_PIN13//(Arduinois13,Teensyis11,Teensy++is6)

boolblinkState=false;

//MPUcontrol/statusvars

booldmpReady=false;//settrueifDMPinitwassuccessful

uint8_tmpuIntStatus;//holdsactualinterruptstatusbytefromMPU

uint8_tdevStatus;//returnstatusaftereachdeviceoperation(0=success,!0=error)

uint16_tpacketSize;//expectedDMPpacketsize(defaultis42bytes)

uint16_tfifoCount;//countofallbytescurrentlyinFIFO

uint8_tfifoBuffer[64];//FIFOstoragebuffer

//orientation/motionvars

Quaternionq;//[w,x,y,z]quaternioncontainer

30

VectorInt16aa;//[x,y,z]accelsensormeasurements

VectorInt16aaReal;//[x,y,z]gravity-freeaccelsensormeasurements

VectorInt16aaWorld;//[x,y,z]world-frameaccelsensormeasurements

VectorFloatgravity;//[x,y,z]gravityvector

floateuler[3];//[psi,theta,phi]Euleranglecontainer

floatypr[3];//[yaw,pitch,roll]yaw/pitch/rollcontainerandgravityvector

//packetstructureforInvenSenseteapotdemo

uint8_tteapotPacket[14]={'$',0x02,0,0,0,0,0,0,0,0,0x00,0x00,'\r','\n'};

//================================================================

//===INTERRUPTDETECTIONROUTINE===

//================================================================

volatileboolmpuInterrupt=false;//indicateswhetherMPUinterruptpinhasgonehigh

voiddmpDataReady(){

mpuInterrupt=true;

}

//================================================================

//===INITIALSETUP===

//================================================================

voidsetup(){

//joinI2Cbus(I2Cdevlibrarydoesn'tdothisautomatically)

#ifI2CDEV_IMPLEMENTATION==I2CDEV_ARDUINO_WIRE

Wire.begin();

TWBR=24;//400kHzI2Cclock(200kHzifCPUis8MHz)

#elifI2CDEV_IMPLEMENTATION==I2CDEV_BUILTIN_FASTWIRE

Fastwire::setup(400,true);

#endif

//initializeserialcommunication

//(115200chosenbecauseitisrequiredforTeapotDemooutput,butit's

//reallyuptoyoudependingonyourproject)

31

Serial.begin(115200);

while(!Serial);//waitforLeonardoenumeration,otherscontinueimmediately

//NOTE:8MHzorslowerhostprocessors,liketheTeensy@3.3vorArdunio

//ProMinirunningat3.3v,cannothandlethisbaudratereliablydueto

//thebaudtimingbeingtoomisalignedwithprocessorticks.Youmustuse

//38400orslowerinthesecases,orusesomekindofexternalseparate

//crystalsolutionfortheUARTtimer.

//initializedevice

Serial.println(F("InitializingI2Cdevices..."));

mpu.initialize();

//verifyconnection

Serial.println(F("Testingdeviceconnections..."));

Serial.println(mpu.testConnection()?F("MPU6050connectionsuccessful"):F("MPU6050connectionfailed"));

//waitforready

Serial.println(F("\nSendanycharactertobeginDMPprogramminganddemo:"));

while(Serial.available()&&Serial.read());//emptybuffer

while(!Serial.available());//waitfordata

while(Serial.available()&&Serial.read());//emptybufferagain

//loadandconfiguretheDMP

Serial.println(F("InitializingDMP..."));

devStatus=mpu.dmpInitialize();

//supplyyourowngyrooffsetshere,scaledforminsensitivity

mpu.setXGyroOffset(220);

mpu.setYGyroOffset(76);

mpu.setZGyroOffset(-85);

mpu.setZAccelOffset(1788);//1688factorydefaultformytestchip

32

//makesureitworked(returns0ifso)

if(devStatus==0){

//turnontheDMP,nowthatit'sready

Serial.println(F("EnablingDMP..."));

mpu.setDMPEnabled(true);

//enableArduinointerruptdetection

Serial.println(F("Enablinginterruptdetection(Arduinoexternalinterrupt0)..."));

attachInterrupt(0,dmpDataReady,RISING);

mpuIntStatus=mpu.getIntStatus();

//setourDMPReadyflagsothemainloop()functionknowsit'sokaytouseit

Serial.println(F("DMPready!Waitingforfirstinterrupt..."));

dmpReady=true;

//getexpectedDMPpacketsizeforlatercomparison

packetSize=mpu.dmpGetFIFOPacketSize();

}else{

//ERROR!

//1=initialmemoryloadfailed

//2=DMPconfigurationupdatesfailed

//(ifit'sgoingtobreak,usuallythecodewillbe1)

Serial.print(F("DMPInitializationfailed(code"));

Serial.print(devStatus);

Serial.println(F(")"));

}

//configureLEDforoutput

pinMode(LED_PIN,OUTPUT);

}

//================================================================

//===MAINPROGRAMLOOP===

33

//================================================================

voidloop(){

//ifprogrammingfailed,don'ttrytodoanything

if(!dmpReady)return;

//waitforMPUinterruptorextrapacket(s)available

while(!mpuInterrupt&&fifoCount<packetSize){

//otherprogrambehaviorstuffhere

//.

//.

//.

//ifyouarereallyparanoidyoucanfrequentlytestinbetweenother

//stufftoseeifmpuInterruptistrue,andifso,"break;"fromthe

//while()looptoimmediatelyprocesstheMPUdata

//.

//.

//.

}

//resetinterruptflagandgetINT_STATUSbyte

mpuInterrupt=false;

mpuIntStatus=mpu.getIntStatus();

//getcurrentFIFOcount

fifoCount=mpu.getFIFOCount();

//checkforoverflow(thisshouldneverhappenunlessourcodeistooinefficient)

if((mpuIntStatus&0x10)||fifoCount==1024){

//resetsowecancontinuecleanly

mpu.resetFIFO();

Serial.println(F("FIFOoverflow!"));

34

//otherwise,checkforDMPdatareadyinterrupt(thisshouldhappenfrequently)

}elseif(mpuIntStatus&0x02){

//waitforcorrectavailabledatalength,shouldbeaVERYshortwait

while(fifoCount<packetSize)fifoCount=mpu.getFIFOCount();

//readapacketfromFIFO

mpu.getFIFOBytes(fifoBuffer,packetSize);

//trackFIFOcounthereincasethereis>1packetavailable

//(thisletsusimmediatelyreadmorewithoutwaitingforaninterrupt)

fifoCount-=packetSize;

#ifdefOUTPUT_READABLE_QUATERNION

//displayquaternionvaluesineasymatrixform:wxyz

mpu.dmpGetQuaternion(&q,fifoBuffer);

Serial.print("quat\t");

Serial.print(q.w);

Serial.print("\t");

Serial.print(q.x);

Serial.print("\t");

Serial.print(q.y);

Serial.print("\t");

Serial.println(q.z);

#endif

#ifdefOUTPUT_READABLE_EULER

//displayEuleranglesindegrees

mpu.dmpGetQuaternion(&q,fifoBuffer);

mpu.dmpGetEuler(euler,&q);

Serial.print("euler\t");

35

Serial.print(euler[0]*180/M_PI);

Serial.print("\t");

Serial.print(euler[1]*180/M_PI);

Serial.print("\t");

Serial.println(euler[2]*180/M_PI);

#endif

#ifdefOUTPUT_READABLE_YAWPITCHROLL

//displayEuleranglesindegrees

mpu.dmpGetQuaternion(&q,fifoBuffer);

mpu.dmpGetGravity(&gravity,&q);

mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);

Serial.print("ypr\t");

Serial.print(ypr[0]*180/M_PI);

Serial.print("\t");

Serial.print(ypr[1]*180/M_PI);

Serial.print("\t");

Serial.println(ypr[2]*180/M_PI);

#endif

#ifdefOUTPUT_READABLE_REALACCEL

//displayrealacceleration,adjustedtoremovegravity

mpu.dmpGetQuaternion(&q,fifoBuffer);

mpu.dmpGetAccel(&aa,fifoBuffer);

mpu.dmpGetGravity(&gravity,&q);

mpu.dmpGetLinearAccel(&aaReal,&aa,&gravity);

Serial.print("areal\t");

Serial.print(aaReal.x);

Serial.print("\t");

Serial.print(aaReal.y);

Serial.print("\t");

Serial.println(aaReal.z);

36

#endif

#ifdefOUTPUT_READABLE_WORLDACCEL

//displayinitialworld-frameacceleration,adjustedtoremovegravity

//androtatedbasedonknownorientationfromquaternion

mpu.dmpGetQuaternion(&q,fifoBuffer);

mpu.dmpGetAccel(&aa,fifoBuffer);

mpu.dmpGetGravity(&gravity,&q);

mpu.dmpGetLinearAccel(&aaReal,&aa,&gravity);

mpu.dmpGetLinearAccelInWorld(&aaWorld,&aaReal,&q);

Serial.print("aworld\t");

Serial.print(aaWorld.x);

Serial.print("\t");

Serial.print(aaWorld.y);

Serial.print("\t");

Serial.println(aaWorld.z);

#endif

#ifdefOUTPUT_TEAPOT

//displayquaternionvaluesinInvenSenseTeapotdemoformat:

teapotPacket[2]=fifoBuffer[0];

teapotPacket[3]=fifoBuffer[1];

teapotPacket[4]=fifoBuffer[4];

teapotPacket[5]=fifoBuffer[5];

teapotPacket[6]=fifoBuffer[8];

teapotPacket[7]=fifoBuffer[9];

teapotPacket[8]=fifoBuffer[12];

teapotPacket[9]=fifoBuffer[13];

Serial.write(teapotPacket,14);

teapotPacket[11]++;//packetCount,loopsat0xFFonpurpose

#endif

//blinkLEDtoindicateactivity

37

blinkState=!blinkState;

digitalWrite(LED_PIN,blinkState);

}

}

38

AppendixE:BluetoothPCControlTestCodeforModelVerification/*AngularVelocityandPositionTest

*ConnectsArduinowithPCusingBluetooth

*Press'x'onPCtostart

*Robotwillwaitfor2second,moveforwardfor10secondsthenstop

*Duringthistime,itwillsendfloatnumberstoPC

*/

#include<Servo.h>

//DeclareServo

Servopendulum;

//StandardPWMDCcontrol

constintE1=5;//E1SpeedControl

constintM1=4;//M1DirectionControl

charpcVal;

longrunTime=10000;//howlongthemotorswillrun

longprevTime=0;

floattheta;

floatthetadot;

voidsetup(){

//putyoursetupcodehere,torunonce:

Serial1.begin(9600);

pinMode(4,OUTPUT);

pinMode(5,OUTPUT);

pendulum.attach(11);//pin11asservoobject

Serial1.println(F("Pressxtobegin"));

}

voidloop(){

39

//putyourmaincodehere,torunrepeatedly:

if(Serial1.available()){

pcVal=Serial1.read();

}

elseif(pcVal=='x'){

Serial1.println(F("TestStarting"));

delay(2000);//wait2seconds

unsignedlongcurrentTime=millis();

prevTime=currentTime;

while(currentTime-prevTime<runTime){

//Serial1.println(F("Running"));

digitalWrite(M1,HIGH);//moveforward

analogWrite(E1,150);//speed

pendulum.write(90);//pendulumposition

currentTime=millis();

theta=25.4;//testvaluefortheta

thetadot=34.5;//testvalueforthetadot

Serial1.print(theta);

Serial1.print("\t");

Serial1.print(thetadot);

Serial1.println("\t");

}

Serial1.println(F("EndofTest"));

pcVal='o';

}

else{

digitalWrite(M1,LOW);

analogWrite(E1,LOW);

}

delay(1);

}