armengine_serial.bsp

六自由度机械臂调试程序

  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
ptrStream           VAR     Word            ' Stream pointer
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
delay               VAR     Byte            ' Pause
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.
'                base    bicep   elbow  wrist  wristR gripper
PUT Constraints, 30,150, 60,140, 0,150, 0,170, 0,180, 90,170

'Note: the rotating wrist is not accessed
'           Bse  Bcpt  elbw  wrst  wrstR  grppr dly  ctrlByte
Stream DATA 90,  100,  100,   70,   90,   150,  20, %00111111,
            30,  100,  100,   70,   90,   150,  20, %00000001,
            30,   90,   90,   60,   90,   150,  15, %00101110,
            30,   85,   90,   60,   90,   115,  20, %00100010,
            90,  100,  100,   70,   90,   115,  30, %00001111,
            90,   90,   90,   60,   90,   115,  10, %00001110,
            90,   85,   90,   60,   90,   150,  10, %00100010,
            90,  100,  100,   70,   90,   150,  10, %00001110,
            90,   85,   90,   60,   90,   150,  10, %00001110,
            90,   85,   90,   60,   90,   115,  20, %00100000,
            90,  100,  100,   70,   90,   115,  10, %00001110,
            30,  100,  100,   70,   90,   115,  30, %00000001,
            30,   85,   90,   60,   90,   115,  10, %00001110,
            30,   85,   90,   60,   90,   150,  10, %00100000,
            90,  100,  100,   70,   90,   150,  20, %00001111,
            $FF 'END

#DEFINE debugMode = 0
ramp = $E

Main:
  GOSUB  IGet_Stream                            ' Get data
  PAUSE 2000                                    ' Pause before repeating
GOTO Main                                       ' Loop forever

IGet_Stream:
  ptrStream = Stream                            ' Assign table pointer
  READ ptrStream, array(Base), array(Bicep),    ' Read first EEPROM row
                 array(Elbow), array(Wrist),
                 array(WristRotate), array(Gripper),
                 delay, ctrlByte
  DO WHILE array(Base) <> $FF                   ' End of EEPROM table?
    GOSUB Move_Arm_Joints                       ' Move joint(s)
    PAUSE (delay*100)                           ' pause
    ptrStream = ptrStream + 8                   ' Point to next row
    READ ptrStream, array(Base), array(Bicep),  ' Read row
                   array(Elbow), array(Wrist),
                   array(4), array(Gripper),
                   delay, ctrlByte
  LOOP                                          ' Loop until end of EEPROM
RETURN

'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
RETURN

'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

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

'sub(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 ?ptrStream,
          'DEC ?pscChannel,
          '?servoPos,
          '?lowerConstr,
          '?upperConstr,
          '?RxChannels,
          '?channelValue,
          ?array(Base),
          ?array(Bicep),
          ?array(Elbow),
          ?array(Wrist),
          ?array(WristRotate), 'SG6 Users
          ?array(Gripper),
          BIN ?ctrlByte
  RETURN
#ENDIF
'           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, %00100000,
            '30,   90,   90,   60,   90,   150,  15, %00011101,
            '30,   85,   90,   60,   90,   115,  20, %00010001,
            '90,  100,  100,   70,   90,   115,  30, %00111100,
           ' 90,   90,   90,   60,   90,   115,  10, %00011100,
           ' 90,   85,   90,   60,   90,   150,  10, %00010000,
           ' 90,  100,  100,   70,   90,   150,  10, %00011100,
           ' 90,   85,   90,   60,   90,   150,  10, %00011100,
           ' 90,   85,   90,   60,   90,   115,  20, %00000001,
           ' 90,  100,  100,   70,   90,   115,  10, %00011100,
           ' 30,  100,  100,   70,   90,   115,  30, %00100000,
           ' 30,   85,   90,   60,   90,   115,  10, %00011100,
           ' 30,   85,   90,   60,   90,   150,  10, %00000001,
           ' 90,  100,  100,   70,   90,   150,  20, %00111100,
           ' $FF 'END