'----[BlockSort.bsp]-----------------------------------------

' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... BlockSort.bsp
'   Purpose.... Sort blocks based on color
'   Author..... Steve Norris
'               Arm movement subroutines by Mike Gebhard
'   E-mail..... norris56@comcast.net
'   Created.... 9/25/2006
'   Updated.... 9/28/2006
'

'=========================================================================
' Arm servo joints to PSC channels connections
'=========================================================================
' Base...........0
' Bicep..........1
' Elbow..........2
' Wrist..........3
' WristRotate....4
' Gripper........5

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

' Color sensor
EN                  CON     1
A0                  CON     2
S0                  CON     3
S1                  CON     4
S2                  CON     5
S3                  CON     6
nLED                CON     7
OUT                 CON     8

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

pRED                CON     32
pGREEN              CON     21
pBLUE               CON     16

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

key                 VAR     Byte
temp                VAR     Word
counter             VAR     Byte

RED                 VAR     Word
GREEN               VAR     Word
BLUE                VAR     Word
Match               VAR     Bit

'-------------------------------------------------------------------------
' 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,170, 60,140, 0,150, 0,170, 5,180, 70,140

ramp = 16

Main:
  ' Define initial state of all pins
  OUTS=%0000000000000000
  '     FEDCBA9876543210
  DIRS=%1000000011111110    '1=Output

  DEBUG CLS,"Block Sort Program",CR
  DEBUG "Initializing...",CR

  ' Init color
  LOW  A0                               'For Unit 0; use high for Unit 1.
  HIGH  S0                              'Maximum output rate.
  HIGH  S1
  HIGH  nLED                            'Turn off LED.
  HIGH  EN                              'Enable sensor output.

  HIGH PSC
  PAUSE 1000
  GOSUB SetupArm
  PAUSE 2000
  DEBUG "Running...",CR

  DO
    GOSUB GetBlock
    GOSUB IsDone
    IF Match = 1 THEN
      DEBUG "Done.",CR
      EXIT
    ENDIF

    ' Red block?
    GOSUB IsRed
    IF Match = 1 THEN
        GOSUB PutBlockBin1
    ENDIF

    ' Green block?
    IF Match = 0 THEN
      GOSUB IsGreen
      IF Match = 1 THEN
          GOSUB PutBlockBin2
      ENDIF
    ENDIF

    ' Blue block?
    IF Match = 0 THEN
      GOSUB IsBlue
      IF Match = 1 THEN
          GOSUB PutBlockBin3
      ENDIF
    ENDIF

    ' Yellow block?
    IF Match = 0 THEN
      GOSUB IsYellow
      IF Match = 1 THEN
          GOSUB PutBlockBin4
      ENDIF
    ENDIF

    ' Unknown
    IF Match = 0 THEN
      GOSUB PutBlockBin4
    ENDIF
  LOOP

  GOSUB RestArm
  END


' Test for a red block
IsRed:
  Match = 0
  IF RED > 85 AND Green < 30 AND BLUE < 30 THEN
    Match = 1
  ENDIF
  RETURN


' Test for a green block
IsGreen:
  Match = 0
  IF RED < 25 AND Green < 28 AND BLUE < 28 THEN
    Match = 1
  ENDIF
  RETURN


' Test for a blue block
IsBlue:
  Match = 0
  IF RED < 30 AND Green > 25 AND BLUE > 60 THEN
    Match = 1
  ENDIF
  RETURN


' Test for a yellow block
IsYellow:
  Match = 0
  IF RED > 200 AND Green > 140 AND BLUE > 35 THEN
    Match = 1
  ENDIF
  RETURN


' Test for no block
IsDone:
  Match = 0
  IF RED < 10 AND Green < 10 AND BLUE < 10 THEN
    Match = 1
  ENDIF
  RETURN


' Gets the color from the Taos Color Sensor
GetColor:
  LOW nLED
  LOW  S2                      'Address the red output.
  LOW  S3
  COUNT  OUT, pRED, RED        'Read the red component.
  HIGH  S3                     'Address the blue output.
  COUNT  OUT, pBLUE, BLUE      'Read the blue component.
  HIGH  S2                     'Address the green output.
  COUNT  OUT, pGREEN, GREEN    'Read the green component.
  HIGH nLED

'  DEBUG "RGB: "
'  DEBUG DEC3 RED," "
'  DEBUG DEC3 GREEN," "
'  DEBUG DEC3 BLUE
'  DEBUG CR
  RETURN


' Get a block from the dispenser
GetBlock:
  GOSUB GetColor
  GOSUB IsDone
  IF Match = 1 THEN
    RETURN
  ENDIF

  ' Get in grab position
  ramp = 12
  array(Base) = 152
  array(Bicep) = 101
  array(Elbow) = 78
  array(Wrist) = 17
  array(WristRotate) = 95
  array(Gripper) = 100
  ctrlByte = %00000000        ' Reset control byte
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 1100

  ' Grab
  ramp = 10
  array(Gripper) = 80
  ctrlByte = %00000000        ' Reset control byte
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 1000

  ' Lift
  ramp = 18
  array(Base) = 152
  array(Bicep) = 116
  array(Elbow) = 68
  array(Wrist) = 26
  array(WristRotate) = 95
  array(Gripper) = 85
  ctrlByte = %00000000        ' Reset control byte
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 1000
  RETURN


' Put the current block into bin 1 (red)
PutBlockBin1:
  ramp = 12
  ' Move over to bins
  array(Base) = 119
  array(Bicep) = 116
  array(Elbow) = 68
  array(Wrist) = 26
  array(WristRotate) = 95
  array(Gripper) = 85
  ctrlByte = %00000000
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 500

  ' Down to bin 1
  array(Base) = 114
  array(Bicep) = 86
  array(Elbow) = 77
  array(Wrist) = 33
  array(WristRotate) = 95
  array(Gripper) = 85
  ctrlByte = %00000000
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 1000

  ' Drop
  ramp = 8
  array(Gripper) = 100
  ctrlByte = %00000000
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 500

  ' Setup for next block
  GOSUB SetupArm
  PAUSE 1000
  RETURN


' Put the current block into bin 2 (green)
PutBlockBin2:
  ramp = 12
  ' Move over to bins
  array(Base) = 119
  array(Bicep) = 116
  array(Elbow) = 68
  array(Wrist) = 26
  array(WristRotate) = 70
  array(Gripper) = 85
  ctrlByte = %00000000        ' Reset control byte
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 500

  ' Down to bin 2
  array(Base) = 99
  array(Bicep) = 87
  array(Elbow) = 79
  array(Wrist) = 26
  array(WristRotate) = 70
  array(Gripper) = 85
  ctrlByte = %00000000        ' Reset control byte
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 1000

  ' Drop
  ramp = 8
  array(Gripper) = 100
  ctrlByte = %00000000        ' Reset control byte
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 500

  ' Setup for next block
  GOSUB SetupArm
  PAUSE 1000
  RETURN


' Put the current block into bin 3 (blue)
PutBlockBin3:
  ramp = 12
  ' Move over to bins
  array(Base) = 80
  array(Bicep) = 116
  array(Elbow) = 68
  array(Wrist) = 26
  array(WristRotate) = 70
  array(Gripper) = 85
  ctrlByte = %00000000
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 1000

  ' Down to bin 3
  array(Base) = 80
  array(Bicep) = 87
  array(Elbow) = 79
  array(Wrist) = 26
  array(WristRotate) = 70
  array(Gripper) = 85
  ctrlByte = %00000000
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 1000

  ' Drop
  ramp = 8
  array(Gripper) = 100
  ctrlByte = %00000000
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 500

  ' Setup for next block
  GOSUB SetupArm
  PAUSE 1500
  RETURN


' Put the current block into bin 4 (yellow)
PutBlockBin4:
  ramp = 12
  ' Move over to bins
  array(Base) = 65
  array(Bicep) = 116
  array(Elbow) = 68
  array(Wrist) = 26
  array(WristRotate) = 70
  array(Gripper) = 85
  ctrlByte = %00000000
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 2000

  ' Down to bin 4
  array(Base) = 65
  array(Bicep) = 77
  array(Elbow) = 100
  array(Wrist) = 22
  array(WristRotate) = 70
  array(Gripper) = 85
  ctrlByte = %00000000
  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 1200

  ' Drop
  ramp = 8
  array(Gripper) = 100
  ctrlByte = %00000000
  gripperBit = 1
  GOSUB Move_Arm_Joints
  PAUSE 500

  ' Setup for next block
  GOSUB SetupArm
  PAUSE 2000
  RETURN


' Initial arm setup position
SetupArm:
  ramp = 12
  array(Base) = 152
  array(Bicep) = 100
  array(Elbow) = 90
  array(Wrist) = 30
  array(WristRotate) = 95
  array(Gripper) = 140
  ctrlByte = %00000000

  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1

  GOSUB Move_Arm_Joints
  RETURN


' Arm rest position
RestArm:
  ramp = 16
  array(Base) = 98
  array(Bicep) = 120
  array(Elbow) = 90
  array(Wrist) = 30
  array(WristRotate) = 95
  array(Gripper) = 140
  ctrlByte = %00000000

  baseBit = 1
  bicepBit = 1
  elbowBit = 1
  wristBit = 1
  wristRotateBit = 1
  gripperBit = 1

  GOSUB Move_Arm_Joints
  RETURN


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


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


Convert_Degrees_To_PSCUnits:
  servoPos = ((array(pscChannel) * 56) / 10) + 250
RETURN


Convert_PSCUnits_To_Degrees:
  array(pscChannel) = ((servoPos - 250) * 10) / 56
RETURN


Write_PSC:
  SEROUT PSC,Baud,["!SC",pscChannel, ramp,
         servoPos.LOWBYTE, servoPos.HIGHBYTE, CR]
RETURN