armengine_serial.bsp

六自由度机械臂调试程序

'----[ArmEngine_Serial.bsp]-----------------------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... ArmEngine_Serial.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
' arm.  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_Serial.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 functionality.
'
' Simply make sure that the ctrlByte variable always has a zero in the
' 4th position.
'
' ctrlByte = %XXX0 XXXX
' OR
' wristRotateBit = 0
'
' SG5 Users:
' There is no harm in accidentally accessing the rotating wrist.  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
'=========================================================================
' When this program runs, the arm moves to the side and picks up an
' object. I used one of my kids' wood block about 1.5" wide (40cm). The
' arm picks up the block and places it directly in front of the base.
' Then the arm picks up the same block and moves it back to the starting
' position.
' The process repeats forever.
'
'=========================================================================
' EEPROM Table DATA
'=========================================================================
' Below is a code sample from an EEPROM table that holds arm positions.
' The table is made up of columns and rows.  The columns define object
' types like a base servo or a delay value.  Rows represent an arm
' movement.  An arm movement can consist of 1 to 6 joint movements and
' a delay value. Joint positions are in degrees.
'
'             bse  bcp   elbw  wrst  wrstR grppr  dly  ctrlByte
' Stream DATA 90,  100,  100,   70,   90,   150,  20, %00111111,
'             30,  100,  100,   70,   90,   150,  20, %00000001,
'             ...
'             $FF 'END
'
' Table Design:
'----------------
' Always follow the example above when creating custom position tables.
' All 8 columns are necessary in the order shown below.
'
' Order         Column Type
' 1) bse........Base Servo
' 2) bcp........Bicep Servo
' 3) elbw.......Elbow Servo
' 4) wrst.......Wrist Servo
' 5) wrstR......Wrist Rotate Servo
' 6) grppr......Gripper
' 7) dly........Delay
' 8) ctrlByte...Control Byte
'
' Always end your table with an $FF.  Not doing so will cause unexpected
' results.
'
' Delay:
' Delay is the amount of time to wait before executing the next table
' row (arm position).  A delay of 20 = 20*100ms or 2 seconds.  Delays
' are important, they give the joints time to reach their positions.
'
' Control Byte:
' The control byte activates arm joints. The control byte consists of
' 8 bits where bits 0 to 6 equal an arm joint.  For example, %00000011
' activates the base and bicep servos.
'
' Control byte     Joint Activated
' ------------     ---------------
' %XX000001........Base Servo
' %XX000010........Bicep Servo
' %XX000100........Elbow Servo
' %XX001000........Wrist Servo
' %XX010000........Wrist Rotate Servo
' %XX100000........Gripper Servo
' %XX111111........All Servos
'
' A table row always contains 6 joint positions, one for each arm joint.
' For most applications you don't need to send commands to all 6 joints
' all the time.  You might need to move the base, bicep, and elbow first
' then move the wrist and gripper.  In this example, you would need two
' table rows; one to handle the base, bicep, and elbow and one for the
' wrist and gripper.
'
' Example:
'    bse  bcp   elbw  wrst  wrstR grppr  dly  ctrlByte
'    90,  100,  100,   70,   90,   115,  30, %00000111,
'    90,   90,   90,   60,   90,   115,  10, %00011000,
'
' Using the control byte in this fashion eliminates needless PSC I/O.
'
'=========================================================================
' Sub Routines.
'=========================================================================
' This program uses sub routines from ArmEngine_Basic.bsp.  For detailed
' information on ArmEngine_Basic.bsp please read the comments within the
' ArmEngine_Basic.bsp file.
'
'=========================================================================
' IGet_Stream
'=========================================================================
' IGet_Stream grabs 8 bytes from EEPROM, places the bytes into the
' array(Base), array(Bicep), array(Elbow), array(Wrist),
' array(WristRotate), array(Gripper), delay, and ctrlByte.  The idea is to
' simulate a data stream coming from an external device.  It's not a good
' idea to stream data into EEPROM tables.  A better method is using
' scratch pad ram.  Why, did I use a table?  I ran out of Scratch pad RAM
' for this demonstration.  Anyway, IGet_Stream starts at the top of the
' EEPROM table and continues to the end of the table.  When it sees
' the end ($FF) the process repeats.
'
' You might be thinking, what about the scratch pad RAM how does that
' work?  It works very much like the table example but the values are
' held in RAM not EEPROM.  Therefore, you could create a routine that
' looks to the serial port or a pin for an incoming stream of 8 bytes.
' Place the 8 bytes into scratch pad RAM, verify that you received the
' data, signal the sending device that the data was received, if not
' resend. Read the ram data into the 8 variables.  Request more data.
' Using scratch pad RAM can simulate cache.  Or you could simply
' grab 8 bytes of serial data and place it in the appropriate variables.
'
' The only requirement is that the incoming data is placed in the correct
' object.  A good way to accomplish this is to send the data in the order
' shown below.

' Order         Column Type
' 1) bse........Base Servo
' 2) bcpt.......Bicept Servo
' 3) elbw.......Elbow Servo
' 4) wrst.......Wrist Servo
' 5) wrstR......Wrist Rotate Servo
' 6) grppr......Gripper
' 7) dly........Delay
' 8) ctrlByte...Control Byte
'
'
' It's up to the developer to come up with a communication protocol.
'
'-------------------------------------------------------------------------

'-----[ I/O ]-------------------------------------------------------------
#IF ($stamp = BS2SX) OR ($stamp = BS2P) #THEN
  Baud              CON     1021            ' BS2p 2400 baud
#ELSE
  Baud              CON     33164           ' BS2 2400 baud
#ENDIF
PSC                 CON     15              ' PSC Module
Base                CON     0               ' Rotating base
Bicep               CON     1               ' Left bicept
Elbow               CON     2               ' Elbow
Wrist               CON     3               ' Wrist
WristRotate         CON     4               ' Rotating Wrist
Gripper             CON     5               ' Gripper
RightBicep          CON     6               ' Right bicept

'=================================================================
' Constant declaration statement found from
' running Adjust_Bicept.bsp during assembly.
' This code will not download until you declare
' the RightBicepOffset constant.
' Chapter #5: Adjusting the Biceps Servos.
#IF sgVersion = 1 #THEN