salamander closing report - sutd-mit international design ... · the uniqueness of this design,...
TRANSCRIPT
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𝑚&𝑣 +
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<[email protected]>
//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,[email protected]
//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);
}