armengine_basic.bsp

六自由度机械臂调试程序

'
' This is a handy troubleshooting technique if you decide to change the
' base code and need to know what the variables are doing.
'
' For more information look up "compiler directives" in the PBASIC help.
'
'=========================================================================
' Main
'=========================================================================
' This sub is designed to give a programmer hands on experience with
' the variables and constants used in this program.  Like the move base
' example above, if you want to move the base and only the base servo
' to 30 degrees do the following.
'
' Main:
'  array(Base) = 30
'  array(Bicep) = 90
'  array(Elbow) = 90
'  array(Wrist) = 90
'  array(WristRotate) = 90  --> SG6 only
'  array(Gripper) = 120
'  ctrlByte = %00000000        ' Reset control byte

'  baseBit = 1
'  biceptBit = 0
'  elbowBit = 0
'  wristBit = 0
'  wristRotateBit = 0
'  gripperBit = 0
'
' Note: Only the baseBit = 1.  Therefore only the base servo rotates.
'
' Run the program.
'-------------------------------------------------------------------------

'-----[ 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
degrees             CON     2               ' Amount to move a joint
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
  RightBicepOffset    CON    '-10            ' Right Bicep offset
#ENDIF
'=================================================================

Constraints         CON     0               ' Start of constraints
RC_On               CON     1               ' Receiver ON
RC_Off              CON     0               ' Receiver OFF

'---- [ Variables ] ------------------------------------------------------
channelValue        VAR     Word            ' Rx channel value
servoPos            VAR     Word            ' PSC servo position
array               VAR     Byte(6)         ' Servo positions (degrees)
lowerConstr         VAR     Byte            ' lower angle constraint
upperConstr         VAR     Byte            ' upper angle constraint
ramp                VAR     Byte            ' servo rotation speed
pscChannel          VAR     Nib             ' PSC channel
RxChannels          VAR     Nib             ' Receiver channel
RC                  VAR     Bit             ' Rx On/Off

ctrlByte            VAR     Byte            ' Servo joint(s) updated
baseBit             VAR     ctrlByte.BIT0   ' 0 = no change
bicepBit            VAR     ctrlByte.BIT1   ' 1 = joint value changed
elbowBit            VAR     ctrlByte.BIT2
wristBit            VAR     ctrlByte.BIT3
wristRotateBit      VAR     ctrlByte.BIT4
gripperBit          VAR     ctrlByte.BIT5

'-------------------------------------------------------------------------
' Joint constraints in angles
' The values below limit where the arm joints can be positioned.
' The constraints stop the arm from getting itself into
' a unrecoverable position or a position that could damage
' a servo.  For example the base servo can move from 30
' to 150 degrees no more and no less.
'
'                base    bicep   elbow  wrist  wristR gripper
PUT Constraints, 30,150, 60,140, 0,150, 0,170, 0,180, 90,170

#DEFINE debugMode = 0

ramp = $B

Main:
  array(Base) = 90
  array(Bicep) = 100
  array(Elbow) = 90
  array(Wrist) = 90
  array(WristRotate) = 90
  array(Gripper) = 120
  ctrlByte = %00000000        ' Reset control byte

  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1  'SG5 users change to 0
  gripperBit = 1

  GOTO  Move_Arm_Joints
END

'sub(array(6) <byte>, ctrlByte <byte>)
Move_Arm_Joints:
'Move base if baseBit = 1 (update position)
'Skip if base bit = 0 (no change)
  IF baseBit THEN
    pscChannel = Base
    GOSUB Check_Joint_Constraint
    GOSUB Convert_Degrees_To_PSCUnits
    GOSUB Write_PSC
    #IF debugMode #THEN
      GOSUB Display
    #ENDIF
  ENDIF
'Move bicept if biceptBit = 1
  IF bicepBit THEN
    pscChannel = Bicep
    GOSUB Check_Joint_Constraint
    GOSUB Convert_Degrees_To_PSCUnits
    GOSUB Write_PSC
    #IF sgVersion = 1 #THEN
      pscChannel = RightBicep
      servoPos = servoPos + RightBicepOffset
      GOSUB Write_PSC
    #ENDIF
    #IF debugMode #THEN
      GOSUB Display
    #ENDIF
  ENDIF
'Move elbow if elbowBit = 1
  IF elbowBit THEN
    pscChannel = Elbow
    GOSUB Check_Joint_Constraint
    GOSUB Convert_Degrees_To_PSCUnits
    GOSUB Write_PSC
    #IF debugMode #THEN
      GOSUB Display
    #ENDIF
  ENDIF
'Move wrist if wristBit = 1
  IF wristBit THEN
    pscChannel = Wrist
    GOSUB Check_Joint_Constraint
    GOSUB Convert_Degrees_To_PSCUnits
    GOSUB Write_PSC
    #IF debugMode #THEN
      GOSUB Display
    #ENDIF
  ENDIF
'Move wristRotate if wristRotateBit = 1
  IF wristRotateBit THEN
    pscChannel = WristRotate
    GOSUB Check_Joint_Constraint
    GOSUB Convert_Degrees_To_PSCUnits
    GOSUB Write_PSC
    #IF debugMode #THEN
      GOSUB Display
    #ENDIF
  ENDIF
'Move gripper if gripperBit = 1
  IF gripperBit THEN
    pscChannel = Gripper
    GOSUB Check_Joint_Constraint
    GOSUB Convert_Degrees_To_PSCUnits
    GOSUB Write_PSC
    #IF debugMode #THEN
      GOSUB Display
    #ENDIF
  ENDIF
END

'function(pscChannel<nib>) return array(pscChannel)<byte>
Check_Joint_Constraint:
  GET (Constraints + (pscChannel*2)), lowerConstr, upperConstr
  IF (array(pscChannel) <= lowerConstr)  THEN
    array(pscChannel) = lowerConstr
  ENDIF
  IF (array(pscChannel) >= upperConstr) THEN
    array(pscChannel) = upperConstr
  ENDIF
RETURN

'function(pscChannel<nib>) return servoPos<word>
Convert_Degrees_To_PSCUnits:
  servoPos = ((array(pscChannel) * 56) / 10) + 250
RETURN

'function(servoPos<byte>) return array(pscChannel)<byte>
Convert_PSCUnits_To_Degrees:
  array(pscChannel) = ((servoPos - 250) * 10) / 56
RETURN

'sub(PSCChannel <byte>, ramp <byte>, servoPos <word>)
Write_PSC:
  SEROUT PSC,Baud,["!SC",pscChannel, ramp,
         servoPos.LOWBYTE, servoPos.HIGHBYTE, CR]
RETURN

' Display all variables
#IF debugMode #THEN
  Display:
    DEBUG "-----------------------",CR
    IF RC THEN
      DEBUG "Receiver is ON!",CR
    ELSE
      DEBUG "Receiver is OFF!",CR
    ENDIF
    DEBUG DEC ?pscChannel,
          ?servoPos,
          ?lowerConstr,
          ?upperConstr,
          '?RxChannels,
          ?channelValue,
          ?array(Base),
          ?array(Bicep),
          ?array(Elbow),
          ?array(Wrist),
          ?array(WristRotate), '--> SG6
          ?array(Gripper),
          BIN ?ctrlByte
  RETURN
#ENDIF