' =========================================================================
'
'   File...... cdavenger.sxb
'   Purpose... cdavenger robot
'   Author.... David Mcanulty
'   E-mail.... dave000@hellspark.com
'
' =========================================================================


' -------------------------------------------------------------------------
' Program Description
' -------------------------------------------------------------------------

' This revision has obstacle avoidance disabled to help with tweaking of
' drive systems.

' -------------------------------------------------------------------------
' Device Settings
' -------------------------------------------------------------------------

ID                "CDAVENGR"
DEVICE            SX28,OSCHS3,BOR42    'Brownout restart at 4.2v
FREQ              50_000_000           '50Mhz
STACK             16                   'Stack size in bytes

' -------------------------------------------------------------------------
' IO Pins
' -------------------------------------------------------------------------

' Low voltage side of breadboard (BOTTOM)
' outputs to motor controller chips
Black_CD_Inject   PIN   RA.0  OUTPUT 'IXDN #1
Black_CD_Eject    PIN   RA.1  OUTPUT 'IXDN #1
White_CD_Inject   PIN   RA.2  OUTPUT 'IXDN #2
White_CD_Eject    PIN   RA.3  OUTPUT 'IXDN #2

' leds for debuging
White_CD_LED      PIN   RB.0  OUTPUT 
Black_CD_LED      PIN   RB.1  OUTPUT
Reset_Led         PIN   RB.2  OUTPUT

' High volatage side of breadboard (TOP)
' Outputs to servo lifter motors
Black_CD_Lift     PIN   RC.7  OUTPUT 'Servo 1
White_CD_Lift     PIN   RC.6  OUTPUT 'Servo 2


'Input from cdrom extension sensors (both tied together)
'CD_Extension_Status  PIN   RC.5  INPUT '5v + = extended with pulldown to gnd


' -------------------------------------------------------------------------
' Constants
' -------------------------------------------------------------------------

Motor_Delay    CON   1000    '#ms to leave the cdrom drive motors on
Lift_Delay     CON    200    '#ms to leave the lifter motors on 
Loop_Delay     CON    100	  '#ms to delay logic loop (controls movement speed)
Extended       CON      0    'When drive arm is fully exteneded sensor==0 (we get gnd)
Motor_Off      CON      0    'Motor activation logic
Motor_On       CON      1    'Motor activation logic
Lift_Left      CON     75    'Servo logic 750uS aka .75mS 
Lift_Right     CON    225    'Servo logic 2250uS aka 2.25mS 
LED_Off        CON      0
LED_ON         CON      1
White_CD       CON      1    'Identifier for subs
Black_CD       CON      2    'Identifier for subs
Obstructed     CON      5    'Times to loop reverse after obstruction

' -------------------------------------------------------------------------
' Variables
' -------------------------------------------------------------------------

White_CD_Arm_Status VAR  byte
Black_CD_Arm_Status VAR  byte
Counter             VAR  byte

' =========================================================================
  PROGRAM Start   'Jump to this label on powerup/init/reset
' =========================================================================

' -------------------------------------------------------------------------
' Subroutine Declarations
' -------------------------------------------------------------------------

DELAY_MS       SUB   1, 2  'delay in milliseconds, sub prevents duplicate asm code
CLEAR_ALL_IO   SUB   0,0   'resets all drive motors and leds
FORWARD        SUB   1,1   'Pulls BOT forward with specified CDROM drive
BACKWARDS      SUB   1,1   'Pushed BOT backwards with specified CDROM drive
EJECT          SUB   1,1   'Ejects CDROM arm
INJECT         SUB   1,1   'Retracts CDROM arm
LIFT           SUB   1,1   'Lifts specified Drive
DROP           SUB   1,1   'Lowers Specified Drive
COLLISIONS     SUB   0,0   'Handle drive tray collisions

' -------------------------------------------------------------------------
' Program Code
' -------------------------------------------------------------------------

Start:   'Powerup initialization code here
   PLP_A = %1111_1111   'Pullup unused inputs 0=pullup
   PLP_B = %1111_1111
   PLP_C = %1111_1111

   CLEAR_ALL_IO
   'Blink Reset LED To indicate initial powerup/reset
   For counter=0 to 5  '5cycles of 100ms-ON and 100ms-off = 5Seconds
      Reset_Led = LED_On
      DELAY_MS 100   
      Reset_Led = LED_Off
      DELAY_MS 100   
   NEXT

   'Get all motors to a sane state (lift, retract,lower)
   LIFT White_CD        
   LIFT Black_CD
   INJECT White_CD
   INJECT Black_CD
   DROP White_CD
   DROP Black_CD

Main:
   DO
      CLEAR_ALL_IO         'clear all motors drivers and status leds
      'COLLISIONS           'subroutine to deal with drive tray collisions
      White_CD_Arm_Status = FORWARD White_CD
      Black_CD_Arm_Status = FORWARD Black_CD
      DELAY_MS Loop_Delay  'Controls the movemment rate of the bot
   Loop
goto Main   

' -------------------------------------------------------------------------
' Subroutine Code
' -------------------------------------------------------------------------
' Use: DELAY_MS msecs

SUB DELAY_MS
  Local_TempW VAR BYTE (2)
  IF __PARAMCNT = 1 THEN
    Local_TempW = __PARAM1      ' save byte value
  ELSE
    Local_TempW = __WPARAM12    ' save word value
  ENDIF
  PAUSE Local_tempW 
ENDSUB

SUB CLEAR_ALL_IO
   'Default motors to coast mode
   White_CD_Eject = Motor_Off
   White_CD_Inject = Motor_Off
   Black_CD_Eject = Motor_Off
   Black_CD_Inject = Motor_Off
   'Default leds off
   White_CD_LED = LED_Off
   Black_CD_LED = LED_Off
ENDSUB

SUB FORWARD
   Local_Drive VAR BYTE
   Local_Drive = __PARAM1

   'CD Forward Function
      LIFT Local_Drive  'To clear spike from carpet
      EJECT Local_Drive 'Stetching forward
      'if CD_Extension_Status = Extended THEN
         DROP Local_Drive    'Drop spike into carpet
         INJECT Local_Drive   'Pull bot forward 
      'else                    'We have an obstruction
      '   INJECT Local_Drive   'retract tray, no movement
      '   DROP Local_Drive    'drop bot
      '   RETURN Obstructed
      'endif
   RETURN 0 ' No obstruction
ENDSUB

SUB BACKWARDS
   Local_Drive VAR BYTE
   Local_Drive = __PARAM1

   EJECT Local_Drive 'eject the current side (pushing us backwards)
   LIFT Local_Drive  'uC lifts the current side
   INJECT Local_Drive'uC injects the current side (setting us up for another push)
   DROP Local_Drive  'uC drops the current side
ENDSUB

SUB LIFT
   Local_Drive VAR BYTE
   Local_Drive = __PARAM1
   
   if Local_Drive = 1 THEN
      White_CD_LED = LED_On
      For Counter=0 to Lift_Delay
         PULSOUT White_CD_Lift, Lift_Right	'Right
         DELAY_MS 20		'Servo pulse, must be 20ms
      NEXT  'end of for loop
      White_CD_LED = LED_Off
   else
      Black_CD_LED = LED_On
      For Counter=0 to Lift_Delay
         PULSOUT Black_CD_Lift, Lift_Right	'Right
         DELAY_MS 20		'Servo pulse, must be 20ms
      NEXT  'end of for loop
      Black_CD_LED = LED_Off
   endif 
ENDSUB

SUB DROP
   Local_Drive VAR BYTE
   Local_Drive = __PARAM1

   if Local_Drive = 1 THEN
      For Counter=0 to Lift_Delay
      PULSOUT White_CD_Lift, Lift_Left	'Left
      DELAY_MS 20		'Servo pulse, must be 20ms
      NEXT  'end of for loop
   else
      For Counter=0 to Lift_Delay
      PULSOUT Black_CD_Lift, Lift_Left	'Left
      DELAY_MS 20		'Servo pulse, must be 20ms
      NEXT  'end of for loop
   endif 
ENDSUB

SUB EJECT
   Local_Drive VAR BYTE
   Local_Drive = __PARAM1

   if Local_Drive = 1 THEN
      White_CD_LED = LED_On
      White_CD_Eject = Motor_On
      DELAY_MS Motor_Delay
      White_CD_Eject = Motor_Off
      White_CD_LED = LED_Off
   else
      Black_CD_LED = LED_On
      Black_CD_Eject = Motor_On
      DELAY_MS Motor_Delay
      Black_CD_Eject = Motor_Off
      Black_CD_LED = LED_Off
   endif
ENDSUB
 
SUB INJECT
   Local_Drive VAR BYTE
   Local_Drive = __PARAM1

   if Local_Drive = 1 THEN
      White_CD_LED = LED_On
      White_CD_Inject = Motor_On
      DELAY_MS Motor_Delay
      White_CD_Inject = Motor_Off
      White_CD_LED = LED_Off
   else
      Black_CD_LED = LED_On
      Black_CD_Inject = Motor_On
      DELAY_MS Motor_Delay
      Black_CD_Inject = Motor_Off
      Black_CD_LED = LED_Off
   endif
ENDSUB

SUB COLLISIONS
      do while White_CD_Arm_Status > 0  'If White CDROM is blocked, reverse
         BACKWARDS White_CD
         dec White_CD_Arm_Status
      loop
      
      do while Black_CD_Arm_Status > 0  'If Black CDROM is blocked, reverse
         BACKWARDS Black_CD
         dec Black_CD_Arm_Status
      loop
ENDSUB