六自由度机械手用户手册

目录TOC\o"1-2"\h\u6402第一章准备安装 SG5/6机械手 21006所需工具 224179SG5/6基本配置机械手包括下列元器件： 218168第二章安装前准备 41887注意： 429107准备伺服电机 49164第三章组装底座 57988安装电机到底座盒 52324第四章组装上臂 1517307第五章组装前臂 2517986第六章组装手爪 366025第七章装手爪到前臂上 632089第八章弹簧安装和连接电机电缆 6530810第九章教学板和电机控制器的连接 7021319第十章使用和操作机械手时必须注意的事项： 7210999第十一章机械手例程 7427060例程1电机调试 747809例程2手臂的基本动作 779655例程3抓取和放置动作 8829999螺旋桨舵机控制器USB（＃28830） 985546特点 983329应用创意 9819542主要规格 9820288在连接你的螺旋桨伺服控制器USB之前 9812533连接和测试 981258USB连接 9813433TTL串行连接 998050通信协议 10015815命令集 1008313位置命令——设置位置伺服通道 10027907VER？-确定固件版本号 10124690SBR-设置波特率（2400或38.4K波特） 1025677PSS-设置软件端口(范围0-15或16-31) 10313138PSD-伺服禁用 1035523PSE-伺服启用 1049136EDD-启动伺服模式 10429927默认位置命令-设置默认位置的伺服频道 10510清除-清除EEPROM上数据 10619964硬件复位按钮 1064870编程跳线 107119模块尺寸 10814838技术支持 108第一章准备安装 SG5/6机械手所需工具下面是安装SG5/6时所需要的工具：螺丝刀钻孔器1/8英寸的钻头小型号的活动扳手或套筒扳手剪钳少量润滑油或类似物品完全配置机械手物品清单SG5/6基本配置机械手包括下列元器件：电子电器类\软件：SG5：3个HiTecHS-475HB伺服电机，1个HS-805BB伺服电机，1个HS-645MG伺服电机SG6：3个HiTecHS-475HB伺服电机，1个HS-805BB伺服电机，1个HS-225MG伺服电机，1个HS-645伺服电机电机延长线：12英寸长2条，6英寸长2条1个SPST开关1个6V-4A电源（只在完全配置中提供）铝质部件：1个电路板固定支架1个底座底座上转盘下托板底座上转盘上托板3个电机支4个手臂部件2个左手爪部件2个右手爪部件4个扁平连接件1齿轮支架1个电机齿轮传动支架1个前臂支架1个上臂支架螺母、螺钉、垫圈和取间隔装置4个#2螺母4个#2弹垫4颗#2-5/16英寸螺钉35个#4平垫13颗#4-1/2英寸螺钉12颗#4-3/8英寸螺钉26颗#4-1/4英寸螺钉4颗#4-1英寸螺钉1颗#4-5/8英寸螺钉3颗#4-5/16英寸螺钉19颗#4螺母23颗#4锁紧螺母19个#4弹垫14个#4-1/4英寸取间隔尼龙支柱4个#4-5/16英寸取间隔尼龙支柱8个#4-3/16英寸取间隔尼龙支柱14个#6-3/8英寸螺钉1个#6-1/2英寸螺钉8个#6锁紧螺母8个#6螺母8个#6弹垫1个#6平垫6个1/4英寸SAE平垫4个球形支撑件其他1本SG5/6机械手手册10条扎带1根长弹簧1根短弹簧2个齿轮4个橡胶垫2个夹子第二章安装前准备注意：在一个整齐、干净有较大空间的环境下开展组装工作整理和恰当地摆放你的各类不规格的螺钉、螺母等配件，这样有利于你在装配时正确的使用特定大小的螺钉、螺母、垫圈等，也可以很方便地取用它们。从容不迫！SG5/6机械手是一个精密的产品，它要求所有部件都按这个手册中讲的顺序安装。组装好机械手的平均时间在3到6小时。在安装的过程中请经常参考本安装手册中提供的参考图片。在开始组装任何部件之前，请仔细参考和研究本手册中提供的安装图片特别是那些特写的图片。必须一直注意螺钉和其他铝材的方位和方向以及装在机械手的那侧。因为不同的装法可导致完全不同的结果。准备伺服电机去掉从它们的保护包装中去掉铝件然后把它们放回在你工作空间里各自的类别中去。把六自由度机械手的六个伺服电机（五自由度为5个）上圆形的输出臂从电机输出周上取下并把它们放好。注：其中的一个HS-475HB电机上没有圆形的输出臂，去掉电机上所有的塑料突起（图1所示），这样做有利于把伺服电机更牢固地固定在电机固定支架上。这些电机在后面的步骤中将被陆续安装。型号为HS805BB的较大一些的电机在剪取塑料突起后，可能还需要锉刀修整一下，因为它的塑料突起比其他几个电机的要厚。图1第三章组装底座安装电机到底座盒图21．用2颗#6-3/8英寸螺钉和弹垫，把一个HS475HB电机固定到底座盒上如图2所示。注意让伺服电机的中线和安装孔的中线重合如图3所示。将电机的电缆从底座盒侧面中的狭引出。图32．用2颗#4-3/8英寸的螺钉，平垫，弹垫和螺母，如图4所示将电路板固定支架安装到底座盒上。在拧紧#4螺钉前，把4个橡胶装到电路板固定支架的底端，在必要时调整支架的高度于工作平面齐平（图6），然后拧紧#4-3/8英寸的螺钉。图4图53．用4颗5/16英寸#4取间尼龙支柱、平垫，把4个支撑小件安装到底座盒上如图6所示。在装支撑小件之前，#4平垫可以放在5/16英寸长取间尼龙支的顶端。用手拧紧4个支撑小件，不要用钳子或其他工具来拧它们。4．在每个支撑小件上涂适量的润滑油脂如图7所示，之后暂时把该组件放到一边。图6图7装配完成后的底座盒5．用一个1/8英寸大小的钻头，把图8中所示的孔扩大。图86．用4颗3/8英寸螺钉、锁紧螺母把圆形的电机输出臂安装到底座转盘下托板如图9所示。圆形的电机输出臂必须安装在托板上有四个圆柱的一侧（见图10）。7．把托板装到机械手底座上，顺时钟和逆时钟转动托板以便让在上面步骤中涂在支撑小件上的油脂均匀涂在下托板上，如图10所示。图108．卸下托板，用手指将油脂均匀涂到在上面步骤中转动托盘形成的油脂圆圈上，如图11所示。重新涂少量油脂到支撑小件上。再把托盘装回底座盒，这个时候还能用电机螺钉拧紧底座。图119.临时把下转动托盘装到底座盒上，旋转并调整转盘使它能够从中心狭槽向左和向右各转90度角，如图12所示。图1210．当转动托盘的位置调整正确后，用伺服电机上的螺钉把转盘固定好。注意不要用力过猛，以免拧坏电机的输出轴。拧紧螺钉到转盘能够自由地转动又不会因为太松而使转盘接触不到底座盒上4个支撑小件。11．用8颗#4-1/2英寸螺钉、螺母和3/16英寸的取间尼龙支柱，把Parallax控制电路板、微型控制器、Parallax电机控制器、1个SPST开关（可选）、电源插头（当您选择购买了6V、4安培电源时——图13b）安装到电路板固定支架上。控制电路板和电机控制器的朝向应如图13所示。电路板固定支架适合所有型号的Parallax控制电路板和电机控制器。如果你自己的电路板和支架不配套只需用工具扩一下孔就行。注意：为了您的方便我们提供了一个电源开关，电源可以直接接到电机控制器上也可以通过开关再到电机控制器。开关可用可不用，不影响机械手的正常操作。图13图13b第四章组装上臂12．用3颗#4螺钉，弹垫和螺母，把电机安装支架固定到上转动托盘上如图14所示。拧紧螺钉前必须注意让托盘上的孔和电机固定支架上的孔重合。图1413．用1/8英寸的钻头，把如下图15所示电机输出臂上的孔扩大，有两个圆形电机输出臂需要扩孔，型号为645和475的各一个，把输出臂装到电机输出轴上钻会更容易钻一些。图1514．用2颗1/4英寸的螺钉、弹垫把电机圆形的输出臂装到机械手上臂上如图16和图17所示。图16图1715．用1颗#6-9/16英的螺钉、3个1/4英寸的SAE平垫和1个锁紧螺母把右上臂安装到电机固定支架上的狭槽上如图18和图19所示。适当拧紧#6螺钉，让上臂和支架之间没有空隙但又不太紧而让臂的运动受阻。理想的状态是臂能竖起而不会倒下，圆形输出臂朝内侧。图18注意正确安排2个1/4英寸SAE平垫的次序，否则整个组件会变松动。图1916．用1/8英寸的钻头，把输出臂上任意两个相对的孔扩大如图20所示。孔钻好后把输出臂从电机上取下，重新把电机输出轴上的螺钉拧上以免丢失。图20注意：旋转电机的输出臂直到它上面的孔和左侧上臂上的孔完全吻合上，表面上看起来输出臂上所有孔都一样，事实上却不同。17．用3颗#6-3/8英寸螺钉，弹垫和螺母，把型号为805BB的伺服电机装到电机固定支架上如图20所示。图2018．用2颗#4-3/8英寸螺钉、锁紧螺母，把电机输出臂安装到左侧上臂弯曲部分的内侧如图22和图23所示。图22图2319．完成后的机械手上臂应如图24所示。在拧紧电机输出轴上螺钉前，需确认左侧上臂能从垂直位置向前向后转动90度。图2420．放置3颗#4-1/4英寸取间尼龙支柱到下转动托盘上如图25所示。图2521．用3颗1/2英寸#4螺钉，把图24中所示的组件装到下转动托盘上如图26所示。图26参考图27来确认组装是否正确。图27第五章组装前臂1．用2颗1/4英寸螺钉、锁紧螺母，把电机支架装到前臂上，注意孔位相对如图28所示，并通过前臂末端小孔固定电机支架，如图29所示。图28图292．用1颗5/16英寸#4螺钉、平垫、弹垫和螺母，把较短的弹簧安装到电机固定支架上。注意平垫和弹簧前端的小圈齐平紧贴，如图30和图31所示。图30图313．像图32一样套2个#4SAE平垫到臂上的铝柱上，并涂适量的油脂到最后一个平垫的外侧，注意让平垫平的一侧贴着臂上的构件。图324．用1颗9/16英寸#6螺钉、平垫、螺母，把图31中所示的组建装到图32中所示的组件上如图33所示。不要使劲拧紧9/16英寸的螺钉和螺母，拧到组件之间没有空隙手臂能够向上向下自由转动为好。图33完成组装后部件图示图34图35用1/8英寸的钻头，把图36中所示的孔扩大。图366．用2颗1/4英寸螺钉和锁紧螺母，把电机输出臂到上臂内侧如图37所示（输出臂平的一侧贴着前臂）。图3737．用2颗#6-3/8英寸螺钉，弹垫和螺母，把型号为HiTecHS-645MG电机装到固定支架和机械手前臂上，如图38和图39所示。图38图398．装完以后，把上臂上圆形电机输出臂和前臂上电机输出轴连接起来，要注意前臂能像图40和41所示一样向前向后自由地旋转90度，之后再拧紧螺钉。图40图41第六章组装手爪1．用2颗1/4英寸螺钉，把手爪的两片连接起来，如图42所示。图422．把1/16英寸的橡胶材料粘到手爪上。并把橡胶材料剪到与手爪边缘对齐，把遮住手爪上狭槽的橡胶材料挖去，后先暂时把手爪放到一边。3.用2颗3/8英寸螺钉、锁紧螺母把型号为HS475HB的电机安装到图43所示的构件上。先别完全拧紧#6螺钉，在后面的步骤中拧紧更合适。图43如果组装SG5五自由度机械手完成第4到第5步。如果组装SG6六自由度机械手完成第6到第11步。4．用2颗1/4英寸#4螺钉和锁紧螺母，把电机支架组装到上一步骤中组装好的组件中，如图44所示。注意垂直方向上电机支架上的狭槽和组件上的狭槽位置重合，见图44和图45。图44图455．用2颗#6螺钉弹垫和螺母，把型号为HS-475的伺服电机安装到支架上如图46所示。（注：图46中显示有四颗螺钉，安装时只要用2颗安装到对角线上就行了）。图466．用1/8英寸的钻头，把型号为HS225MG电机上输出臂上的两个孔扩大，如图47所示。图477．用2颗#4-5/16英寸螺钉，弹垫和螺母把电机输出臂装到支架上如图48和图49所示。图48图498．用2颗#6-3/8英寸螺钉，弹垫和螺母把型号为HS-225MG的电机安装到支架上如图50所示。在把电机装到支架上时，注意电机的的方向。图509．把型号为HS-225MG的电机装到支架上如图51和图52。在继续下一步之前得确认电机可以从中心向左向右转动相同的角度。图51图5210．用1颗#4平垫和弹垫，把电机输出轴上的螺钉拧到电机上如图53所示。平垫在最里面，然后是弹垫再是螺钉。图5311．用4颗#6-3/8英寸的螺钉，弹垫和螺母，把型号为HS475的电机装到支架上，两个电机中其中一个的安装方法参照本章第3步到第5步。完成安装后的组件如图54和图55所示。图54图5512．用2个#2弹垫、螺母把电机输出臂装到齿轮和其支架上如图56所示，之后再把另一个齿轮也装好。图5613．把电机齿轮传动组件装到电机支架上如图57所示。在用螺钉固定齿轮支架之前注意它要能够如图57、图58、图59中所示灵活地转动。图57图58图5914．用1颗1英寸#4螺钉，2个平垫，1颗锁紧螺母和1个1/4英寸的取间尼龙支柱，把齿轮组件和电机支架组件组装起来，如图60所示。注意开始位置两个齿轮组件的位置应和图61中指示的一样。拧紧1英寸#4螺钉，让组件中没有间隙但能用拨动。图60提示：在平垫和铝件之间涂适量的油脂，这样运转起来时会更灵活。图6115．用1颗3/8英寸#4螺钉，2平垫和1颗螺母，把手爪装到齿轮传动装置上如图62和图63所示。拧紧螺母让组件间没有间隙但能被手拨动。图62图6316．用1颗1/2英寸#4螺钉，4个平垫和1个锁紧螺母，把扁平的连杆装到齿轮机构上如图64和图65。还是拧紧螺钉到组件之间没有空隙但又能被手拨动。图64图65图6617．用1颗1英寸#4螺钉，2个平垫，2个1/4英寸取间尼龙支柱和1个锁紧螺母，安装扁平连接件到支架内侧，如图67所示。图6718．重复同样的安装循序把另一只手爪也组装好如图68和69所示。图68图6919．用2颗1/4英寸#4螺钉和锁紧螺母，把传感器支架安装到手爪机构上如图70所示。

图70在组装手爪时可以参考下面的6副图。图71（SG5手爪）图72图73图74张开状态图75握紧状态注：如果手爪的末端不能够像图75一样啮合，只需要拧松齿轮上的电机，把电机往左移一些，转动一个或两个齿轮就可以调整好手爪。图76第七章装手爪到前臂上1．如图77所示八1个1/4英寸SAE平垫套到铝柱上，在平垫的外侧面涂少量润滑剂。垫圈平的一面必须靠前臂。图772．把手爪组件装到前臂，在固定组件之前需确保手爪组件能从中线开始各转45度，如图78。图78第八章弹簧安装和连接电机电缆1．调整前臂支架，使它离电机固定支架约1英寸远，如图79所示。测量前水平放置组件。较长的弹簧可装可不装，但当用机械手提取较重物体或经常要不断地长时间工作时，要求装上长弹簧。图792．用1颗1.5英寸#4螺钉,1个垫圈和4个1/4英寸的取间尼龙支柱,如图80所示,把长弹簧牢固的连接上.图803.用1颗#4-5/8英寸、平垫、弹垫、螺母和1/4英寸取间尼龙支柱，把长弹簧的另一端到前臂上如图81所示。拧紧螺钉时注意，弹簧末端的圈不能偏向左边或右边，应居中。图814．用1颗5/16英寸#4螺钉、垫圈和弹垫、螺母，把小弹簧装到手臂上如图82所示。拧紧螺钉时注意，弹簧末端的圈不能偏向左边或右边，应居中。小弹簧是必须的，它可以抵消手臂前向的作用力使上的电机能够达到提起的最大位置。图825．把电机电缆从上臂和前臂支架上孔中穿过，在用扎带扎紧电机电缆的之前比需确认电缆不能影响手臂的正常运动。在扎紧之前可以水平放置机械手来确定电缆的长度。6．电机电缆布置好后，用2根12英寸长和2根6英寸长的电缆延长线连接电机输出电缆，以便它能够足够长与PSC电机控制器相连，（图83）电机电缆与电机控制器的连接方法在后面有介绍。图83

第九章教学板和电机控制器的连接下面的图片是SG5五自由度、SG6六自由度机械手教学板BOE和电机控制器PSC之间典型的连接方法。结合图87来具体了解伺服电机和电机控制器PSC之间的连接。图86图87第十章使用和操作机械手时必须注意的事项：在操作机械手前必须把机械手固定在一个平台上（底座盒上有3个用来固定机械手的部件）。在编程或操作机械手时，最好让离机械手3英尺的空间范围内没有任何物体。当停止运行机械手时注意用手扶着，或在关闭电源时让机械手水平放置。使用5V-6V电源，用过高的电压会让伺服电机过热而损坏电机。一般来说机器人上使用5V-6V电压的电源是正常的。5V-6V电源电压或5V、4A的电源的比较理想。当机械手第一次运行时，必须注意检查电机是否过热。检查手爪上电机是否过热，如果电机过热检查手爪上的螺钉是否拧得过紧，这种情况经常发生。在编写程序时请不要设置手爪张开位置或握紧位置到手爪的极限位置，以免损坏电机和手爪。不要让SG5/6机械手长时间处在图84所示前向位置，否则因手臂重量产生力矩使肩部电机过热而损坏电机。当机械手在未上电或在操作过程中静止时，手臂的肩膀应尽可能的垂直，长的弹簧形变尽可能的小。如图85所示。参照随机械手配置的说明书正确连接这两部分，也可以以本手册最后的图作一个参照。请勿使用教学板的电源来给电机供电。请使用一个与电路板电源独立的电源给电机供电，电压在5-6伏特，至少能提供4安培电流的电源，否则机械部不能正常工作。图84不正确的静止或保持位置图85正确的静止或保持位置第十一章机械手例程例程1电机调试'[Adjust_Bicept.bsp]'{$STAMPBS2p}'{$PBASIC2.5}''FileAdjust_Bicept.bsp'PurposeAlignupperpivotdeckservos'AuthorCrustCrawlerInc.(MikeGebhard)'E-mailsupport@'Started6/1/2005'Updated''Hardware:'(1)SG5/6-UTRoboticArm'(1)ParallaxBasicStampPModule'(1)ParallasBOE'(1)ParallaxServoController(PSC)''========================================================================='GettingStarted'========================================================================='PLEASEverifythePSCtojointconnectionsbelow.Theymightbe'differentfromyourassemblyguideinstructions.''BasePSCChannel0'BicepPSCChannel1'ElbowPSCChannel2'WristPSCChannel3'WristRotatePSCChannel4(SG6Connection)'GripperPSCChannel5'RightBicepPSCChannel6''========================================================================='ProgramOverview'========================================================================='Thisprogramisusedtoprogrammaticallyadjustthetwobicep'servosincasetheycannotbealignedphysically.Thisstep'iscrucialtoensurelongservolifeandlowcurrentconsumption.'Iftheservosarenotalignedproperly,theywillrunhotand'usemorecurrentthanwhat'srequiredfornormaloperation.''========================================================================='Aligningthebicepservos'========================================================================='1.Downloadthisprogram"asis"toyourbasicstamp.Bydefaultthe'bicepservosareprogrammedtocenter.Noticethedebugscreen'displays:''RightBicepOffsetConstantDeclaration:''RightBicepOffsetCON0''2.Lookforanydifferencesbetweenthebicepsservospositions.If'theyalignperfectlyyou'redone.Ifnotcontinuetostep3'physicaladjustments.''3.Physicallyadjustjustonebicepstomatchtheotherbicepsposition'asmuchaspossible.Removeonebicepfromtheservospindle,'rotateit,andsecureitbackontotheservospindle.''4.Lookforanydifferencesbetweenthebicepsservospositions.If'theyalignperfectlyyou'redone.Ifnotcontinuetostep5'Programmaticadjustments.''5.Findthissectionofcodebelow.'rightBicepPos=750'Changethe750valuetosomethingelse,like745,downloadthiscode'andlookforanydifferencesinthebicepservopositions.Continue'toprogrammaticallyadjusttherightBicepPosvariableuntilthe'bicepsareasclosetoparallelaspossible.''6.Writedowntheconstantdeclarationstatementonthedebugscreen.'YouwillusethisvalueinallSTAMPcodewrittenforthearm.'Thevaluecouldbeanegativenumber.'''Notes:'Servoscanvary15to20stepsfromeachother.TherightBicepPos'couldbe+/-20unitsfromtheotherbicepservo.Ifyourgetting'valuesabove20youshouldgobacktostep3andphysically'adjustthebicep.''#IF($stamp=BS2SX)OR($stamp=BS2P)#THENBaudCON1021'BS2p2400baud#ELSEBaudCON33164'BS22400baud#ENDIFPSCCON15'PSCModuleBicepCON1'LeftBicepPSCchannelRightBicepCON6'RightBicepPSCchannelrampCON$F'ServorotationspeedbicepPosVARWord'BiceptPositionrightBicepPosVARWord'RightBiceptPositiondiffVARWord'DifferenceinPosition'RecordyourRightBicepOffsetconstantdeclaration.RightBicepOffsetCON-10bicepPos=750'==================================================='AdjustthevaluebelowtoaligntheservosbracketrightBicepPos=750'===================================================Write_PSC:SEROUTPSC,Baud,["!SC",Bicep,ramp,bicepPos.LOWBYTE,bicepPos.HIGHBYTE,CR]SEROUTPSC,Baud,["!SC",RightBicep,ramp,rightBicepPos.LOWBYTE,rightBicepPos.HIGHBYTE,CR]diff=rightBicepPos-bicepPosDEBUG"RightBicepOffsetConstantDeclaration:",CR,"",CR,CR,"RightBicepOffsetCON",SDECdiffEND例程2手臂的基本动作'[ArmEngine_Basic.bsp]'{$STAMPBS2p}'{$PBASIC2.5}''FileArmEngine_Basic.bsp'PurposeBasicarmmovement'AuthorCrustCrawlerInc.(MikeGebhard)'E-mailsupport@'Created5/26/2005'Updated11/14/2005''Hardware:'(1)SG5/6-UTRoboticArm'(1)ParallaxBasicStampPModule'(1)ParallasBOE'(1)ParallaxServoController(PSC)'''========================================================================='Updates11/14/2005IncludessupportforSG5&6Version2Roboticarms'========================================================================='CodeupdatedforSG5andSG6version2.0.SGversion2armshavea'singlebicepservo.IfyouhaveaSGversion2changethelinebelow'toread#DEFINEsgVersion=2andsavethefile.Ifyouhaveagray'armoranarmwith2servobicepservosleave#DEFINEsgVersion=1.'#DEFINEsgVersion=1''========================================================================='Updates9/24/2005IncludessupportforSG6(6axisarm)'========================================================================='ArmEngine_Basic.bspwillfunctionforboththeSG6andSG5,6and5'axisroboticarms.Ifyoupurchasea5axisyoushouldconsiderturning'offtherotatingwristbyeditingonelineofcode.Findthecode'sectionbelow.''Main:'...'baseBit=1'biceptBit=1'elbowBit=1'wristBit=1'wristRotateBit=1'SG5userschangeto0'gripperBit=1'ChangewristRotateBit=1assignmentto'wristRotateBit=0''SG5Users:'Thereisnoharminleavingthestatementasis.Doingsocauses'unnecessarylinesofcodetorun.Theresultmightlooklikea'slightdelayinprogramexecutionwhilethePSCsendsdatatoa'nonexistentjoint.''========================================================================='GettingStarted'========================================================================='1.VerifyyourPSCchannelconnections.'2.Verifythatyourarmisadjustedphysicallyand'programmatically.'3.Enterthe"RightBicepOffsetConstantDeclaration"youfound'runningtheAdjust_Bicept.bspcode.Thisispartoftheassembly'instructionsasof5/26/2005.Thiscodewillnotrununtilyou'completethisstep.Ifyoualreadyassembledthearm,thisvalue'isthedifferencebetweenthebicepservosfoundinChapter#5:'AdjustingtheBicepsServos.''========================================================================='ArmservojointstoPSCchannelsconnections'========================================================================='PLEASEverifythePSCconnectionbelow.Theymightbedifferentfromyour'assemblyguideinstructions.'Base0'Bicep1'Elbow2'Wrist3'WristRotate4-->(SG6only)'Gripper5'RightBicep6-->SGversion1only(GrayArm)''========================================================================='ProgramOverview'========================================================================='ThisisthebasecodeusedtomovetheSGarmjoints.Ittakesa'7elementarraycontainingservopositions(indegrees)andonecontrol'byte(ctrlByte)asparameters.Thearrayprovidesinstructionsto'positionthearmjoints.ThectrlBytecontrolswhichjointsmove.''Alljointsmovewithinadjustablecontraints.Constrainingthejoints'stopsthearmfrommovingintoanunrecoverableposition,allowsthe'usertocontrolwherethearmdoesitswork,and/orstopsajointfrom'movingtoapositionthatcouldpotentiallyharmaservoorservo'assembly.''Thiscodeisdesignedtogivedevelopersastartingplacesothatthe'developerdoesnotneedtorewriteordisassembleexistingcode.'Subroutinedetailsbelow''========================================================================='Walk-Through:'========================================================================='Move_Arm_JointschecksthebaseBit,biceptBit,elbowBit,wristBit,'andgripperBitvariablesrespectivelytodeterminewhichjointtomove.'A1meansmovetherelatedjointtothepositioninarray(x)'0meansskipthisjoint.''Forexample,letssayyouwanttomovethebaseto30degress'You,theprogrammer,wouldassignbaseBit=1andarray(Base)=30.'ThesubroutineseesthebaseBitequals1andknowsyouwantto'rotatethebase.''Next,jointconstraintsarecheckedbytheCheck_Joint_Constraint'helpersub.Ifthevalueinarray(pscChannel),array(0)inthisexample,'fallswithinthebaseservocontraints,nothinghappens(morelater).''Sincearray(pscChannel)isindegreesandthePSCdoesnotunderstand'degreesthearray(pscChannel)valueisconvertedtoPSCunitsbythe'Convert_Degrees_To_PSCUnitshelpersub.''AllvaluesaresenttoWrite_PSCwhichisresponsiblefor'sendingtheinformationtothePSCandmovingthebaseservo.''Move_Arm_Jointscontinuesbycheckingeachoftheremaining"jointBits"''Order:'1.baseBit'2.biceptBit'3.elbowBit'4.wristBit'5.wristRotateBit'6.gripperBit''========================================================================='SubRoutines.'========================================================================='Eachsubroutinecantakeinputparametersandreturnorupdate'variables.''Example:'sub(inputparameter<datatype>)returnvariable<datatype>.''Thenextseveralsectionsdescribeparameters,returnvalues,and'generalfunctionality.''========================================================================='Move_Arm_Joints'sub(array(6)<byte>,ctrlByte<byte>)'========================================================================='Move_Arm_Jointsisthemainsubroutineforcontrollingarmmovement.'Ittakesthefollowingrequiredparameters:''-ctrlByteThisbytecontrolswhichjointtomove.Itismadeupofthe'aliesesbelow.'baseBitVARctrlByte.BIT0'0=nochange'biceptBitVARctrlByte.BIT1'1=jointvaluechanged'elbowBitVARctrlByte.BIT2'wristBitVARctrlByte.BIT3'wristRotateBitVARctrlByte.BIT4-->SG6only'gripperBitVARctrlByte.BIT5''-array(6)containsdegreepositionsforall(5or6)armjointsvalues.'Thesevaluesrangefrom0to180degreesormaximunservo'rotation.Itisuptotheprogrammertoverifythatthees'valuesdonotexceed180elseyouwillgetunexpected'results.''array(Base)'array(Bicep)'array(Elbow)'array(Wrist)'array(WristRotate)-->SG6only'array(Gripper)''========================================================================='Check_Joint_Constraint'========================================================================='sub(pscChannel<nib>)returnarray(pscChannel)<byte>''-pscChannelThisvariableholdsthePSCchannelrelatedtothecurrent'jointtomoveandthearrayelementthatholdsthejoint'position.''Check_Joint_Constraintismorelikeapropertyofthearm.Bysuppling'contraintstothearmyoudefinewherethearmdoesitsworkorlackof.'ConstraintinformationisstoredinScratchPadRAM(PUTCommand).Each'armjointhasarelatedlowerandupperconstraint.Thesevaluescan'easilybechangedprogrammaticallyduringprogramexecutionsothework'areacanchangedynamicallyduringruntime.''Let'slookatthebaseservoconstraints.Firstfindthelineofcode'below.Thefirsttwonumbers30and150arethebaseservocontraints.''basebiceptelbowwristwristRgripper'PUTConstraints,30,150,60,140,0,150,0,170,0,180,90,170''WhentheCheck_Joint_Constraintiscalleditgrapsthelowerand'upperbasecontraints(Constraints+(pscChannel*2))andplacesthem'inlowerConstrandupperConstrvariables.'GET(Constraints+(pscChannel*2)),lowerConstr,upperConstr''Thevalueinarray(pscChannel)<pscChannel=Base>ischeckedagainst'thecontraints.Ifarray(pscChannel)islessthanthelowerconstraint'thenarray(pscChannel)=lowerContr.Ifarray(pscChannel)isgreater'thanupperConstrthenarray(pscChannel)=upperConstr.Ifneitherare'truethenarray(pscChannel)isnotmodifiedatthistime.''========================================================================='Convert_Degrees_To_PSCUnits'========================================================================='sub(pscChannel<nib>)returnservoPos<word>''-servoPosThisvariableholdstheservopostioninPSCterms''ThisfunctionconvertsdegreestoPSCunits'servoPos=((angle*56)/10)+250''========================================================================='Convert_PSCUnits_To_Degrees'========================================================================='sub(servoPos<byte>)returnarray(pscChannel)<byte>''ThisfunctionconvertsPSCunitstodegrees.Thisfunctionisnotused.'Itisincludedforuseinlaterapplications.'array(pscChannel)=((servoPos-250)*10)/56''========================================================================='Write_PSC'========================================================================='sub(PSCChannel<byte>,ramp<byte>,servoPos<word>)''Write_PSCisresponsibleforsendingjointinformationtothePSC.The'resultisajointmoves.''========================================================================='Display'========================================================================='sub()returns<allvariables>''Thissubdisplaysallthevariablesandvaluesusedinthiscodewhen''#DEFINEdebugMode=1''Thisisahandytroubleshootingtechniqueifyoudecidetochangethe'basecodeandneedtoknowwhatthevariablesaredoing.''Formoreinformationlookup"compilerdirectives"inthePBASIChelp.''========================================================================='Main'========================================================================='Thissubisdesignedtogiveaprogrammerhandsonexperiencewith'thevariablesandconstantsusedinthisprogram.Likethemovebase'exampleabove,ifyouwanttomovethebaseandonlythebaseservo'to30degreesdothefollowing.''Main:'array(Base)=30'array(Bicep)=90'array(Elbow)=90'array(Wrist)=90'array(WristRotate)=90-->SG6only'array(Gripper)=120'ctrlByte=%00000000'Resetcontrolbyte'baseBit=1'biceptBit=0'elbowBit=0'wristBit=0'wristRotateBit=0'gripperBit=0''Note:OnlythebaseBit=1.Thereforeonlythebaseservorotates.''Runtheprogram.''[I/O]#IF($stamp=BS2SX)OR($stamp=BS2P)#THENBaudCON1021'BS2p2400baud#ELSEBaudCON33164'BS22400baud#ENDIFPSCCON15'PSCModuledegreesCON2'AmounttomoveajointBaseCON0'RotatingbaseBicepCON1'LeftbiceptElbowCON2'ElbowWristCON3'WristWristRotateCON4'RotatingWristGripperCON5'GripperRightBicepCON6'Rightbicept'================================================================='Constantdeclarationstatementfoundfrom'runningAdjust_Bicept.bspduringassembly.'Thiscodewillnotdownloaduntilyoudeclare'theRightBicepOffsetconstant.'Chapter#5:AdjustingtheBicepsServos.#IFsgVersion=1#THENRightBicepOffsetCON'-10'RightBicepoffset#ENDIF'=================================================================ConstraintsCON0'StartofconstraintsRC_OnCON1'ReceiverONRC_OffCON0'ReceiverOFF'[Variables]channelValueVARWord'RxchannelvalueservoPosVARWord'PSCservopositionarrayVARByte(6)'Servopositions(degrees)lowerConstrVARByte'lowerangleconstraintupperConstrVARByte'upperangleconstraintrampVARByte'servorotationspeedpscChannelVARNib'PSCchannelRxChannelsVARNib'ReceiverchannelRCVARBit'RxOn/OffctrlByteVARByte'Servojoint(s)updatedbaseBitVARctrlByte.BIT0'0=nochangebicepBitVARctrlByte.BIT1'1=jointvaluechangedelbowBitVARctrlByte.BIT2wristBitVARctrlByte.BIT3wristRotateBitVARctrlByte.BIT4gripperBitVARctrlByte.BIT5''Jointconstraintsinangles'Thevaluesbelowlimitwherethearmjointscanbepositioned.'Theconstraintsstopthearmfromgettingitselfinto'aunrecoverablepositionorapositionthatcoulddamage'aservo.Forexamplethebaseservocanmovefrom30'to150degreesnomoreandnoless.''basebicepelbowwristwristRgripperPUTConstraints,30,150,60,140,0,150,0,170,0,180,90,170#DEFINEdebugMode=0ramp=$BMain:array(Base)=90array(Bicep)=100array(Elbow)=90array(Wrist)=90array(WristRotate)=90array(Gripper)=120ctrlByte=%00000000'ResetcontrolbytebaseBit=1bicepBit=1elbowBit=1wristBit=1wristRotateBit=1'SG5userschangeto0gripperBit=1GOTOMove_Arm_JointsEND'sub(array(6)<byte>,ctrlByte<byte>)Move_Arm_Joints:'MovebaseifbaseBit=