' ========================================================================= ' ' File...... cdavenger.sxb ' Purpose... cdavenger robot ' Author.... David Mcanulty ' E-mail.... dave000@hellspark.com ' ' ========================================================================= ' ------------------------------------------------------------------------- ' Program Description ' ------------------------------------------------------------------------- ' This revision just does initliazation startup to enable uC debuging ' ------------------------------------------------------------------------- ' 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 (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 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 ' ========================================================================= ' ------------------------------------------------------------------------- ' 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 Reset_Led = LED_On 'To indicate initial powerup/reset DELAY_MS 10000 Reset_Led = LED_Off LIFT White_CD 'Get motors to a sane state, all retracted 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 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