armengine_basic.bsp

六自由度机械臂调试程序

'----[ArmEngine_Basic.bsp]-----------------------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... ArmEngine_Basic.bsp
'   Purpose.... Basic arm movement
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... [e-mail]
'   Created.... 5/26/2005
'   Updated.... 11/14/2005
'
' Hardware:
' (1) SG5/6-UT Robotic Arm
' (1) Parallax Basic Stamp P Module
' (1) Parallas BOE
' (1) Parallax Servo Controller (PSC)
'
'
'=========================================================================
' Updates 11/14/2005  Includes support for SG5&6 Version 2 Robotic arms
'=========================================================================
' Code updated for SG5 and SG6 version 2.0.  SG version 2 arms are blue
' If you have a SG version 2 change the line below to read
' #DEFINE sgVersion = 2 and save the file.  If you have a gray arm or
' version 1 do nothing.
'
#DEFINE sgVersion = 1
'
'=========================================================================
' Updates 9/24/2005  Includes support for SG6 (6 axis arm)
'=========================================================================
' ArmEngine_Basic.bsp will function for both the SG6 and SG5, 6 and 5
' axis robotic arms. If you purchase a 5 axis you should consider turning
' off the rotating wrist by editing one line of code.  Find the code
' section below.
'
' Main:
' ...
'  baseBit = 1
'  biceptBit = 1
'  elbowBit = 1
'  wristBit = 1
'  wristRotateBit = 1  'SG5 users change to 0
'  gripperBit = 1
'
' Change wristRotateBit = 1 assignment to
' wristRotateBit = 0
'
' SG5 Users:
' There is no harm in leaving the statement as is. Doing so causes
' unnecessary lines of code to run.  The result might look like a
' slight delay in program execution while the PSC sends data to a
' nonexistent joint.
'
'=========================================================================
' Getting Started
'=========================================================================
' 1. Verify your PSC channel connections.
' 2. Verify that your arm is adjusted physically and
'    programmatically.
' 3. Enter the "RightBicepOffset Constant Declaration" you found
'    running the Adjust_Bicept.bsp code.  This is part of the assembly
'    instructions as of 5/26/2005.  This code will not run until you
'    complete this step.  If you already assembled the arm, this value
'    is the difference between the bicep servos found in Chapter #5:
'    Adjusting the Biceps Servos.
'
'=========================================================================
' Arm servo joints to PSC channels connections
'=========================================================================
' PLEASE verify the PSC connection below.  They might be different from your
' assembly guide instructions.
' Base...........0
' Bicep..........1
' Elbow..........2
' Wrist..........3
' WristRotate....4 --> (SG6 only)
' Gripper........5
' RightBicep.....6 --> SG version 1 only (Gray Arm)
'
'=========================================================================
' Program Overview
'=========================================================================
' This is the base code used to move the SG arm joints.  It takes a
' 7 element array containing servo positions (in degrees) and one control
' byte (ctrlByte) as parameters.  The array provides instructions to
' position the arm joints.  The ctrlByte controls which joints move.
'
' All joints move within adjustable contraints.  Constraining the joints
' stops the arm from moving into an unrecoverable position, allows the
' user to control where the arm does its work, and/or stops a joint from
' moving to a position that could potentially harm a servo or servo
' assembly.
'
' This code is designed to give developers a starting place so that the
' developer does not need to rewrite or disassemble existing code.
' Sub routine details below
'
'=========================================================================
' Walk-Through:
'=========================================================================
' Move_Arm_Joints checks the baseBit, biceptBit, elbowBit, wristBit,
' and gripperBit variables respectively to determine which joint to move.
' A 1 means move the related joint to the position in array(x)
' 0 means skip this joint.
'
' For example, lets say you want to move the base to 30 degress
' You, the programmer, would assign baseBit = 1 and array(Base) = 30.
' The sub routine sees the baseBit equals 1 and knows you want to
' rotate the base.
'
' Next, joint constraints are checked by the Check_Joint_Constraint
' helper sub.  If the value in array(pscChannel), array(0) in this example,
' falls within the base servo contraints, nothing happens (more later).
'
' Since array(pscChannel) is in degrees and the PSC does not understand
' degrees the array(pscChannel) value is converted to PSC units by the
' Convert_Degrees_To_PSCUnits helper sub.
'
' All values are sent to Write_PSC which is responsible for
' sending the information to the PSC and moving the base servo.
'
' Move_Arm_Joints continues by checking each of the remaining "jointBits"
'
' Order:
'  1. baseBit
'  2. biceptBit
'  3. elbowBit
'  4. wristBit
'  5. wristRotateBit
'  6. gripperBit
'
'=========================================================================
' Sub Routines.
'=========================================================================
' Each sub routine can take input parameters and return or update
' variables.
'
' Example:
' sub(input parameter<data type>) return variable<data type>.
'
' The next several sections describe parameters, return values, and
' general functionality.
'
'=========================================================================
' Move_Arm_Joints
' sub(array(6)<byte>, ctrlByte <byte>)
'=========================================================================
' Move_Arm_Joints is the main sub routine for controlling arm movement.
' It takes the following required parameters:
'
' - ctrlByte This byte controls which joint to move.  It is made up of the
'            alieses below.
'   baseBit             VAR     ctrlByte.BIT0   ' 0 = no change
'   biceptBit           VAR     ctrlByte.BIT1   ' 1 = joint value changed
'   elbowBit            VAR     ctrlByte.BIT2
'   wristBit            VAR     ctrlByte.BIT3
'   wristRotateBit      VAR     ctrlByte.BIT4 --> SG6 only
'   gripperBit          VAR     ctrlByte.BIT5
'
' - array(6) contains degree positions for all (5 or 6) arm joints values.
'            These values range from 0 to 180 degrees or maximun servo
'            rotation.  It is up to the programmer to verify that thees
'            values do not exceed 180 else you will get unexpected
'            results.
'
'  array(Base)
'  array(Bicep)
'  array(Elbow)
'  array(Wrist)
'  array(WristRotate) --> SG6 only
'  array(Gripper)
'
'=========================================================================
' Check_Joint_Constraint
'=========================================================================
' sub(pscChannel<nib>) return array(pscChannel)<byte>
'
' - pscChannel This variable holds the PSC channel related to the current
'              joint to move and the array element that holds the joint
'              position.
'
' Check_Joint_Constraint is more like a property of the arm.  By suppling
' contraints to the arm you define where the arm does its work or lack of.
' Constraint information is stored in Scratch Pad RAM (PUT Command).  Each
' arm joint has a related lower and upper constraint.  These values can
' easily be changed programmatically during program execution so the work
' area can change dynamically during run time.
'
' Let's look at the base servo constraints.  First find the line of code
' below.  The first two numbers 30 and 150 are the base servo contraints.
'
'                  base    bicept  elbow  wrist  wristR gripper
' PUT Constraints, 30,150, 60,140, 0,150, 0,170, 0,180, 90,170
'
' When the Check_Joint_Constraint is called it graps the lower and
' upper base contraints (Constraints + (pscChannel*2)) and places them
' in lowerConstr and upperConstr variables.
' GET (Constraints + (pscChannel*2)), lowerConstr, upperConstr
'
' The value in array(pscChannel) <pscChannel = Base> is checked against
' the contraints.  If array(pscChannel) is less than the lower constraint
' then array(pscChannel) = lowerContr.  If array(pscChannel) is greater
' than upperConstr then array(pscChannel) = upperConstr.  If neither are
' true then array(pscChannel) is not modified at this time.
'
'=========================================================================
' Convert_Degrees_To_PSCUnits
'=========================================================================
' sub(pscChannel<nib>) return servoPos<word>
'
' - servoPos This variable holds the servo postion in PSC terms
'
' This function converts degrees to PSC units
' servoPos = ((angle * 56) / 10) + 250
'
'=========================================================================
' Convert_PSCUnits_To_Degrees
'=========================================================================
' sub(servoPos<byte>) return array(pscChannel)<byte>
'
' This function converts PSC units to degrees.  This function is not used.
' It is included for use in later applications.
' array(pscChannel) = ((servoPos - 250) * 10) / 56
'
'=========================================================================
' Write_PSC
'=========================================================================
' sub(PSCChannel <byte>, ramp <byte>, servoPos <word>)
'
' Write_PSC is responsible for sending joint information to the PSC.  The
' result is a joint moves.
'
'=========================================================================
' Display
'=========================================================================
' sub() returns <all variables>
'
' This sub displays all the variables and values used in this code when
'
' #DEFINE debugMode = 1