' ========================================================================= ' ' File...... cdavenger.sxb ' Purpose... cdavenger robot ' Author.... David Mcanulty ' E-mail.... dave000@hellspark.com ' ' ========================================================================= ' ------------------------------------------------------------------------- ' Program Description ' ------------------------------------------------------------------------- ' ------------------------------------------------------------------------- ' Device Settings ' ------------------------------------------------------------------------- ID "CDAVENGR" DEVICE SX28,OSCHS3,BOR42 FREQ 50_000_000 STACK 16 'New Orig changed settings 'DEVICE SX28, OSCXT2, BOR42 'FREQ 20_000_000 'Old device settings 'DEVICE SX28,TURBO,BANKS8,OSCHS3,OPTIONX,BOR42 'FREQ 50_000_000 ' ------------------------------------------------------------------------- ' IO Pins ' ------------------------------------------------------------------------- ' Low voltage side of breadboard ' outputs to motor controller chips White_CD_Eject PIN RA.0 OUTPUT 'IXDN #1 White_CD_Inject PIN RA.1 OUTPUT 'IXDN #1 Black_CD_Eject PIN RA.2 OUTPUT 'IXDN #2 Black_CD_Inject PIN RA.3 OUTPUT 'IXDN #2 ' leds (on CDs) for debuging White_CD_LED PIN RB.0 OUTPUT Black_CD_LED PIN RB.1 OUTPUT ' High volatage side of breadboard ' Outputs to servo lifter motors White_CD_Lift PIN RC.7 OUTPUT 'Servo 1 Black_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 250 '#ms to leave the cdrom drive motors on Lift_Delay CON 120 '#ms to leave the lifter motors on Loop_Delay CON 100 '#ms to delay logic loop Extended CON 0 'When drive arm is fully exteneded sensor==0 Motor_Off CON 0 'Motor activation logic Motor_On CON 1 'Motor activation logic Lift_Right CON 75 'Servo logic 750uS aka .75mS Lift_Left 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 10 'Times to loop reverse after obstruction ' ------------------------------------------------------------------------- ' Variables ' ------------------------------------------------------------------------- White_CD_Arm_Status VAR byte Black_CD_Arm_Status VAR byte Counter VAR byte '-------------------------------------------------------------------------- ' DEBUGGER VARIABLE WATCH STATEMENTS '-------------------------------------------------------------------------- WATCH RA,8,ubin WATCH RB,8,ubin WATCH RC,8,ubin WATCH White_CD_Eject WATCH White_CD_Inject WATCH Black_CD_Eject WATCH Black_CD_Inject WATCH White_CD_LED WATCH Black_CD_LED WATCH White_CD_Lift WATCH Black_CD_Lift WATCH CD_Extension_Status WATCH White_CD_Arm_Status,8,udec WATCH Black_CD_Arm_Status,8,udec ' ------------------------------------------------------------------------- ' INTERRUPT ' ------------------------------------------------------------------------- ' ' ' ========================================================================= PROGRAM Start ' ========================================================================= ' ------------------------------------------------------------------------- ' 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 drive BACKWARDS SUB 1,1 'Pushed BOT backwards with specified 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 White_CD_LED = LED_On 'To indicate initial powerup/reset Black_CD_LED = LED_On 'To indicate initial powerup/reset DELAY_MS 1000 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 speed 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