;************************************************************************** ;Name: Robot Library ;Version: 0.1b ;Author: Dan Kohn ; Comments By: Dan Kohn & Adrian Austin ;Date of Mod): March, 15, 2006 ; ;IO Table: ; ;PortA PA7 OC1 / Gripper ; PA6 OC2 / RIGHT Motor (PWM for servo) \ ; PA5 OC3 / LEFT Motor (PWM for servo) / looking from the back to the front ; PA4 OC4 / Multiplexed Servo Output ;mike code gripper motor ; PA3 OC5 / USED FOR TRACE - BUFFALO ; PA2 IC1 / ; PA1 IC2 / Sonar Echo ; PA0 IC3/Clock for barcode ; ;PortE PE7 Data for barcode ; PE6 code for the limit switch works here ; PE5 ; PE4 ; PE3 ADR4/Line Track 3 left \ ; PE2 ADR3/Line Track 2 center - looking from the back of robot ; PE1 ADR2/Line Track 1 Right / ; PE0 ; ; ;PortD PD5 ; PD4 ; PD3 ; PD2 ; ; OUTPUT Circuit ;$B580 BIT0 LSB Servo Multiplexer ; BIT1 | ; BIT2 MSB Servo Multiplexer ; BIT3 ; BIT4 ; BIT5 ; BIT6 ; BIT7 ; ; Input Circuit // not currently in use ;$B590 BIT0 ; BIT1 ; BIT2 ; BIT3 ; BIT4 ; BIT5 ; BIT6 ; BIT7 ; ; Input Circuit // not currently in use ;$B5A0 BIT0 ; BIT1 ; BIT2 ; BIT3 ; BIT4 ; BIT5 ; BIT6 ; BIT7 ;************************************************************************** ORG $2000 include Hc11.inc ;definds all HC11 Registers JMP MAIN ;Barcode Equates IC3IV EQU $00E2 Barcode RMB 5 BC_count RMB 1 Boxcode RMB 1 outa equ $ffb8 ;Buffalo Equates out2bsp equ $ffc1 ; prints what is pointed to by x to the screen outcrlf equ $ffc4 ; outputs a carrage return and line feed out1bsp equ $ffbe ; prints what is pointed to by x to the screen ;Arm Motor Variables and Equates ;mike code TOC4IV EQU $00D6 On_cnts_G RMB 2 Off_cnts_G RMB 2 ;Left Motor Variables and Equates TOC3IV EQU $00D9 On_cnts_L RMB 2 Off_cnts_L RMB 2 ;Limit Switch Control Byte LS1 rmb 1 ;Line Track Control Byte LTCB rmb 1 ;Line Track Level LTL equ $94 ; comparison number for more black and less is white? ;multiplexer values cs0 equ $B580 On_cnts_A RMB 2 Off_cnts_A RMB 2 On_cnts_B RMB 2 Off_cnts_B RMB 2 ;Sonar variables Sonar_start rmb 2 Sonar_end rmb 2 Sonar_ans rmb 2 ;Right Motor Variables and Equates TOC2IV EQU $00DC On_cnts_R RMB 2 Off_cnts_R RMB 2 ;Gripper (TOC1init) Variables and Equates TOC1IV EQU $00DF On_cnts_Gr rmb 2 Off_cnts_Gr rmb 2 ;Timer Overflow Counter TOFIV EQU $00D0 Sec_Temp rmb 1 ;counter 32.77mSec [0-15] Sec_count rmb 1 ;counter 0.1Sec [0-255] plane_temp rmb 2 ;counter 32.77ms [0-30] plane1_cd rmb 2 ;countdown timer - plane 1 [180 -> 0 Sec] plane2_cd rmb 2 ;countdown timer - plane 2 [240 -> 0 Sec] plane3_cd rmb 2 ;countdown timer - plane 3 [300 -> 0 Sec] MAIN jsr go_stop ;Set up On/Off times for stop ;jsr TOC4_init ;init gripper motor jsr TOC3_init ;init left motor jsr TOC2_init ;init right motor jsr TOF_init ;init Timer Overflow (for 0.5 sec Timer) jsr A2D ;init A/D PE0..PE3 jsr TOC1_init ;inint TOC1_inint for the gripper ; jsr gripO ldaa #40 jsr pause jsr test ;jsr gripO ;ldaa #40 ;jsr pause ;Log: jsr grippC ;jmp Log ;ldx #on_cnts_Gr ;jsr out2bsp ;ldx #off_cnts_Gr ;jsr out2bsp ;jsr Start ;leave the block ;jsr grippC ; grips the first block ;jsr LeaveShoot ; moves robot from shoot to the plane 1 intersection ; spot to check if need to go to 1st plane ;jsr Plane1 ; moves the plane to plane one and dumps and returns ; to the 1st t intersection ;jsr gripO ;jsr Shoot2 ; this moves the robot back to the shoot ;jsr LeaveShoot ; moves from the shoot back to the 1st t intersection ;jsr Plane2 ; moves from t intersection an back to t intersection ;jsr Shoot2 ; moves back to the shoot ;jsr LeaveShoot ; moves back to t intersection ;jsr Plane3 ; moves from the t intersection and back from the 3rd plane ;jsr Shoot2 ; moves back to the shoot jsr go_stop jmp $e000 ;******************************************************************* ; test section ;****************************************************************** test: psha pshb pshx jsr gripO ;open the gripper from the schute jsr Start ; moves to the shoot jsr grippC ldaa #02 jsr pause ; grabs the first block jsr go_reverse ; backs up ldaa #15 jsr pause jsr leaveshoot jsr plane1 ; moves the block to the t intersection pulx pulb pula rts ;*************************************************************************** ; this section will grab block and check and move to the appropriate box ;*************************************************************************** block_check: psha pshb pshx jsr grippC ; grab the block jsr go_reverse ; back up and take block out of stack ldaa #03 jsr pause ; do barcode scanner here ldaa Boxcode cmpa #$01 bne NO_2_Plane jsr LeaveShoot jsr plane1 jsr Shoot2 jmp end_check NO_2_Plane: ldaa Boxcode cmpa #$02 bne No_3_Plane jsr LeaveShoot jsr plane2 jsr shoot2 jmp end_check NO_3_Plane: jsr leaveshoot jsr plane3 end_check: pulx pulb pula ; here light te led for hey i am done rts ;;************************************************************************** ; this section moves the robot to the block shoot ;************************************************************************** Start: psha jsr go_forward ;Leaving Block ldaa #10 jsr pause jsr LineTrack ; this line moves forward until the 1st plus intersection ldaa #10 jsr pause jsr go_forward ; moves forward so line tracking will work ldaa #10 jsr pause jsr Linetrackspecial ; moves forward to the 1st plus intersection pula rts ;************************************************************************** ; this section moves the robot away from the shoot to the intersection for ; turning to plane 1 ;************************************************************************** LeaveShoot: psha jsr clockw ;180 Right Turn to leave the Package shoot jsr LineTrack ; line track back to the 1st plus jsr go_forward ; this section makes the first turn ldaa #5 ; * jsr pause ; * jsr go_stop ldaa #5 jsr pause ; * jsr counterclck ; * jsr go_stop ; * ldaa #5 ; * jsr pause ; * jsr LineTrack ; moves robot to the 1st t intersection ; ends this section pula rts ;************************************************************************** ; this section moves the robot to the first plane and then moves it back to the 1st t intersection ;************************************************************************** Plane1: psha jsr go_forward ; moves forward so turn will work properly ldaa #7 jsr pause jsr go_stop ldaa #5 jsr pause jsr clockw ;turns right until hits white line for the first plane jsr go_stop ldaa #20 jsr pause jsr Linetrack ;Stops at 2nd plus by Loading Zone 1 jsr go_forward ldaa #3 jsr pause jsr Linetrack jsr Left90 ; turns to the airplane jsr go_stop ; right her is where droping will occur ldaa #25 jsr pause jsr go_Open ; for the simple robot must remove ldaa #10 ; and replace with carrage dump section jsr pause ;* jsr counterclck jsr linetrack ; moves the robot forward to the 2nd plus jsr go_forward ; this section moves the robot back to the ldaa #5 jsr pause jsr linetrack jsr go_forward ldaa #7 jsr pause jsr counterclck jsr go_stop ldaa #5 jsr Linetrack ; jsr pause ; pula rts ;************************************************************************** ; this section goes back to the shoot from the t intersection ; ; ;************************************************************************** Shoot2: psha jsr Linetrack jsr go_forward ldaa #8 jsr pause jsr Linetrack jsr clockw ;Goes back to Shoot jsr go_stop ; ldaa #8 jsr pause jsr Linetrackspecial jsr go_stop ldaa #10 jsr pause jsr go_close ldaa #20 jsr pause pula rts ; ;************************************************************************** ; this section moves from the t intersection ; to the second plane and back to the 1st t intersection ;************************************************************************** Plane2: psha jsr go_forward ldaa #5 jsr pause jsr Linetrack ; moves the robot forward until you get to the 3rd plus ; intersection jsr go_forward ldaa #5 jsr pause jsr clockw ;will trun right until hits the white line agian jsr go_stop ldaa #5 jsr pause jsr Linetrack ;Stops at 4th plus by Loading Zone 1 jsr go_forward ldaa #10 jsr pause jsr Linetrack ; moves forward until side of board ldaa #5 jsr pause jsr Left90 ; turn left to go towards the plane jsr go_stop ldaa #5 jsr pause jsr go_Open ; this section for the simple robot ldaa #10 ; this is where the carrage drop occurs jsr pause jsr counterclck ; jsr linetrack ; move to the 4rd plus intersection jsr go_forward ldaa #5 jsr pause jsr linetrack ; here stops at the 3rd plus intersection jsr go_forward ; moves the robot forward to get line track off the plus ldaa #9 jsr pause jsr counterclck ; turns the robot to the left until hits the white line ; back towards the t intersection jsr Linetrack ; moves the robot back to the t intersection jsr go_forward ; ldaa #5 jsr pause pula rts ;****************************************************************************************************** ; this section moves from the 1st t intersection to the 3rd plane and to the 1st t intersection ;****************************************************************************************************** Plane3: jsr go_forward ; moves forward to make linetrack work ldaa #5 jsr pause jsr Linetrack ; moves forward until 3th plus jsr go_forward ; moves forward so line track will work ldaa #5 jsr pause jsr counterclck ; turning left until white line agian jsr Linetrack ; moves to the 5th plus sign jsr go_forward ; ldaa #5 jsr pause jsr Linetrack ; moves to the side of the board jsr Right90 ; turns to move to the plane jsr go_stop ldaa #5 jsr pause ;sonar goes here jsr go_Open ; simple robot code here where comp code goes ldaa #10 ; * jsr pause ; this section moves the robot back to the line jsr clockw ; turns the robot back towards the 5th plus sign jsr linetrack ; moves the robot to the 5th plus sign jsr go_forward ; moves robot past the 5th plus sign ldaa #5 jsr pause jsr linetrack ; moves robot to the 3rd plus sign jsr go_forward ldaa #10 jsr pause jsr clockw ; turns back to the t intersection jsr Linetrack ; moves to the t intersection jsr go_forward ; ldaa #5 jsr pause pula rts ;************************************************************************** ;Name: LineTrack ;Author: Dominic Lawrence ;Date: unknown ;Modifications: ; ; PE3 ADR4/Line Track 3 left \ ; PE2 ADR3/Line Track 2 center - looking from the back of robot ; PE1 ADR2/Line Track 1 Right / ; DK 3/4/06 ; add tests for LTCB=6 and LTCB = 3 to improve line ; following. ; LTL equ $94 ; comparison number for more black and less is white? ;Process: First it calls LT_CONT that sets the LTCB. Then LTCB is ; tested and the appropriate sub routine is called to ; turn turned on/off the drive motors accordingly. ;************************************************************************** LineTrack: psha pshx Lt_loop: jsr Lt_cont FirstT: ldaa LTCB ; checks the center sensors cmpa #$02 beq FForward cmpa #$04 ; checks sensors for if to far left beq RTurn cmpa #$06 beq Rturn cmpa #$01 ; checks sensors for if to far right beq LTurn cmpa #$03 beq LTurn cmpa #$07 ; stops the robot if all three sensors are white beq SStop jsr go_forward jmp Lt_loop RTurn: jsr go_right ; jumps to sub that moves robot to the right jmp Lt_loop LTurn: jsr go_left ; jumps to sub that moves robot to the left jmp Lt_loop FForward: jsr go_Forward ; jumps to sub that moves robot forward jmp Lt_loop SStop: jsr go_stop ; jumps to stop sub ldaa #10 jsr pause pulx pula rts ;******************************* ; this one is special ; a line track that will work to pick up the blocks ;************************************************** lineTrackSpecial: psha pshx Lt_loop1: jsr Lt_cont ldaa PORTE ; grab info from port e anda #$40 ; compare to the down setting cmpa #$0 bne SStop ; if down stop FirstT1: ldaa LTCB ; checks the center sensors cmpa #$02 beq FForward_s cmpa #$04 ; checks sensors for if to far left beq RTurn_s cmpa #$06 beq Rturn_s cmpa #$01 ; checks sensors for if to far right beq LTurn_s cmpa #$03 beq LTurn_s cmpa #$07 ; stops the robot if all three sensors are white beq SStop_s jsr go_forward jmp Lt_loop1 RTurn_s: jsr go_right ; jumps to sub that moves robot to the right jmp Lt_loop1 LTurn_s: jsr go_left ; jumps to sub that moves robot to the left jmp Lt_loop1 FForward_s: jsr go_Forward ; jumps to sub that moves robot forward jmp Lt_loop1 SStop_s: jsr go_stop ; jumps to stop sub ldaa #10 jsr pause pulx pula rts ;************************************************************************** ;Name: Lt_cont - Line Track Control Word ;Author: Dominic Lawrence ;Date: unknown ;Modifications: ; AA 03/??/06 ; Made into a separate sub routine ; PE3 ADR4/Line Track 3 left \ ; PE2 ADR3/Line Track 2 center - looking from the back of robot ; PE1 ADR2/Line Track 1 Right / ;Function: Takes the inputs for each linetrack sensor and compares ; it to the LTL (Line Track Limit). If it is below LTL the ; sensor is inidcating it is on the line and the ; corresponding bit is set in LTCB (Line Track Control Byte). ; ; LTCB = xxxxx000 ; LTL equ $94 comparison number for more black and less is white? ;On Return: LTCL is set as per above ;************************************************************************** Lt_cont: psha pshx Ldaa #$0 ; getting the right side info and if wrong setting Staa LTCB ; LTCB to reflect this so linetrack sub routine will know Ldaa Adr2 ;* Cmpa #LTL ;* BHS CheckCenter;* Ldx #LTCB ;* Bset 0,x #$04 ;* CheckCenter: Ldaa Adr3 ; get the Center info and if wrong setting Cmpa #$9F ; LTCB to reflect this so Linetrack sub routine will know BHS CheckLeft ;* Ldx #LTCB ;* Bset 0,x #$02 ;* CheckLeft: Ldaa Adr4 ;getting the left side info and if wrong setting cmpa #LTL ; LTCB to reflect this so line track sub routine will know BHS lt_exit ;* Ldx #LTCB ;* Bset 0,x #$01 ;* lt_exit: pulx pula rts ;************************************************************************** ; This section will cause the robot to turn to the right until it hits a ; white line with the line tracking sensors ;************************************************************************** clockw: psha jsr go_right ldaa #5 jsr pause clockw_Loop: jsr LT_cont ldaa LTCB cmpa #$02 bne clockw_Loop pula rts ;************************************************************************** ; This section will cause the robot to turn to the Left until it hits a ; white line with the line tracking sensors ;************************************************************************* counterclck: psha jsr go_left ldaa #5 jsr pause counter_Loop: jsr LT_cont ldaa LTCB cmpa #$02 bne counter_Loop pula rts ;************************************************************************** ;Name: Drive Motor Functions ;Author: Dan Kohn ;Date: Dec 9, 2005 ;Functions: Each separtate function will do the indicated function ; to the robot drive motors ; ; go_Stop ; go_Left ; go_Right ; go_Forward ; go_Reverse ; never enter at robot_do! (Stack will be corrupted) ;************************************************************************** go_Stop pshx ldx #Stop bra robot_do go_Left pshx ldx #Left bra robot_do go_Right pshx ldx #Right bra robot_do go_Forward pshx ldx #Forward bra robot_do go_Reverse pshx ldx #Reverse bra robot_do robot_do ldd 0,x std On_cnts_R ldd 2,x std Off_cnts_R ldd 4,x std On_cnts_L ldd 6,x std Off_cnts_L pulx rts ;Table for Drive Motor Functions ; On_cnts_R Off_cnts_R On_cnts_L Off_cnts_L Stop fdb 3000, 37000, 3000, 37000 REVERSE fdb 2000, 38000, 4000, 36000 FORWARD fdb 4000, 36000, 2000, 38000 RIGHT fdb 2000, 38000, 2000, 38000 LEFT fdb 4000, 36000, 4000, 36000 ;Servo Motor notes: ; 1ms on time = 2000 counts = forward ; 2ms on time = 4000 counts = reverse ; total counts for a 50hz square wave (20mSec - suggested for servos) is 40000 counts ;************************************************************************** ;Name: A2D ;Author: Dan Kohn ;Date: Jan 5, 2006 ;Functions: Set up A/D to read PE0..PE3 continuously and ; place the converted values in ADR1..ADR4 ; ;On call: NONE ; ;On Return; A/D is turned on and running continously ;************************************************************************** A2D: pshx psha ldx #option ;setup A/D bset 0,x #$80 ;Turn on A/D - ADPU bclr 0,x #$40 ;Use E clock ldaa #$30 ;Set up to read PE0..PE3 staa ADCTL ;Continuously pula pulx rts ;************************************************************************** ;Name: Multiplexed Motor Functions ;Author: Michael mcneill ;Date: Feb 9, 2006 ;Functions: Each separtate function will do the indicated function ; to the robot drive motors ; ; go_Gstop ; go_Open ; go_Close ; go_Forward ; go_Reverse ; ; never enter at robot_go! (Stack will be corrupted) ;************************************************************************** ;Gripper: go_Gstop pshx ldx #Gstop bra robot_go go_Open pshx ldx #Open bra robot_go go_Close pshx ldx #Close bra robot_go robot_go ldd 0,x ; std On_cnts_Gr ; ldd 2,x ; std Off_cnts_Gr pulx rts ;****************************************** ; code for movement of the robot arm ;************************************* go_Astop pshx ldx #Astop bra robot_arm go_Up pshx ldx #UP bra robot_arm go_Down pshx ldx #Close bra robot_arm robot_arm ldd 0,x std On_cnts_A ldd 2,x std Off_cnts_A pulx rts go_Bstop pshx ldx #Bstop bra robot_bin go_Clockw pshx ldx #bin_Clockw bra robot_bin go_Counterw pshx ldx #bin_Counterw bra robot_bin robot_bin ldd 0,x std On_cnts_B ldd 2,x std Off_cnts_B pulx rts ;Table for Multiplexed Servo ; On_cnts_G Off_cnts_G CLOSE fdb 4000, 36000 Open fdb 2000, 38000 Gstop fdb 3000, 37000 ;Table for arm motor Functions ; On_cnts_A Off_cnts_A UP fdb 2000, 18000 DOWN fdb 4500, 14800 Astop fdb 3000, 17000 ;Table for bin motor Functions ; On_cnts_B Off_cnts_B bin_Clockw fdb 2000, 18000 ;Clockwise maybe bin_Counterw fdb 4500, 14800 ;Counterwise maybe Bstop fdb 3000, 17000 ;Servo Motor notes: ; 1ms on time = 2000 counts = forward ; 2ms on time = 4000 counts = reverse ; total counts for a 50hz square wave (20mSec - suggested for servos) is 40000 counts ;************************************************************************** ;this section closes the gripper with a small section at the beginning ;that deals with a use through a multiplexer ; ;************************************************************************** gripperC: psha ;LDAA #$0000 ;this section for a multiplexer ;STAA cs0 ;* ;ldaa #1 ;* ;jsr pause ;* jsr go_Close ldaa #3 jsr pause jsr go_GStop ldaa #2 jsr pause pula rts ;************************************************************************** ;this section sets the gripper to open ;this includes a small section that controls the gripper ;through a multiplexer ;************************************************************************** gripperO: psha ; ; LDAA #$0000 ; STAA cs0 ; ldaa #1 ; jsr pause jsr go_Open ; call sub that Opens the gripper hand ldaa #3 jsr pause jsr go_GStop ldaa #2 jsr pause pula rts ;************************************************************************** ;Name: Pause ;Author: Dan Kohn ;Date: Dec 19, 2005 ;Functions: Pause for multiple of 0.1 sec ; (setup for a 0.1 sec counter) ; ;On call: Reg A Contains the number of 0.1 sec to pause ; ;On Return; Sec_Count and Sec_Temp were changed (cleared at start of ; pause. ;************************************************************************** Pause: psha pshb ldab #$00 stab SEC_TEMP stab SEC_COUNT PauseL1: ldab sec_count cba bne PauseL1 pulb pula rts ;************************************************************************** ;************************************************************************** Left90: psha jsr go_left ldaa #9 jsr pause pula rts ;************************************************************************** ;************************************************************************** Right90: psha jsr go_right ldaa #9 jsr pause pula rts ;************************************************************************** ;Name: TOF_init ;Author: Dan Kohn ;Date: Dec 14, 2005 ;Functions: Initializes the TOF Function ; (setup for a 0.5 sec counter) ;************************************************************************** TOF_init psha pshx pshy SEI LDAA #$7E ;setting the interrupt vector table STAA TOFIV LDD #TOF_CNT STD TOFIV+1 ldaa #$80 ;clear TOF flag staa TFLG2 LDX #TMSK2 ;turn on TOF interrupt bset 0,x #$80 cli ldy #0 ;clear 1 sec counter for planes sty plane_temp ldy #180 ;set plane 1's time for 180sec sty plane1_cd ldy #240 ;set plane 2's time for 240sec sty plane2_cd ldy #360 ;set plane 3's time for 300sec sty plane3_cd puly pulx pula rts ;************************************************************************** ;Name: TOF_CNT ;Author: Dan Kohn ;Date: Dec 14, 2005 ;Functions: count every 0.1 sec. ; ;On Return: Count of 0.1 sec intervals is in SEC_count ; Count of 32.77mSec intervals is in Sec_Temp ;************************************************************************** TOF_CNT ldaa #$80 staa TFLG2 ldaa Sec_temp ;count # of times overflow occures (32.77ms) inca staa Sec_temp cmpa #03 ;if ~0.1sec has NOT gone by -> exit blo plane_time ; [3*32.77mSec = 0.0983100 Sec] ldaa #$00 ;if ~0.1sec has gone by -> clear Sec_temp staa Sec_temp ;and add one to the Sec_count ldaa Sec_count inca staa Sec_count plane_time: ldy plane_temp ;count # of times overflow occures (32.77ms) iny sty plane_temp cpy #30 ;if ~1sec has NOT gone by -> exit blo tof_exit ; [30*32.77mSec = .98310 Sec] ldy #$00 ;if ~0.1sec has gone by -> clear Sec_temp sty plane_temp ;and add one to the Sec_count ldy plane1_cd ;count down timer for plane 1 cpy #0 beq pt_2 dey sty plane1_cd pt_2: ldy plane2_cd ;count down timer for plane 2 cpy #0 beq pt_3 dey sty plane2_cd pt_3: ldy plane3_cd ;count down timer for plane 3 cpy #0 beq TOF_exit dey sty plane3_cd TOF_exit RTI ;************************************************************************** ;Name: TOC2_init ;Author: Dan Kohn ;Date: Dec 8, 2005 ;Functions: Initalize the Right Motor (OC2) ; ;On Call: go_stop MUST be called before initalizing ;************************************************************************** TOC2_init psha pshb pshx SEI LDAA #$7E ;setting the interrupt vector table STAA TOC2IV LDD #TOC2RT STD TOC2IV+1 LDD TCNT ;sets the first time of TOC2 ADDD On_cnts_R STD TOC2 LDAA #$40 ;intializing the registers staa TFLG1 ldx #TMSK1 BSET 0,X #$40 LDX #TCTL1 ;toggles the output BSET 0,X #$40 BCLR 0,x #$80 CLI ;turning on the interrupts pulx pulb pula rts ;************************************************************************** ;Name: TOC2IV ;Author: Gregory Reeves Jr. ;Date: July 30, 2005 ;Modified: Dan Kohn Dec 8, 2005 For Dual Output Compares ;Function: Interrupt for PWM for Right Motor ;On Call On_cnts_R and Off_cnts_R ;************************************************************************** TOC2RT LDAA #$40 ;resets the OC2F bit STAA TFLG1 LDAA PORTA ;check oc2 pin for on or off state ANDA #$40 BNE ON_2 LDD TOC2 ;sets up for the off time sequence ADDD Off_cnts_R STD TOC2 BRA EXIT_2 ON_2 LDD TOC2 ;sets up for the next ontime sequence ADDD On_cnts_R STD TOC2 EXIT_2 RTI ;return from the interrupt ;************************************************************************** ;Name: TOC3_init ;Author: Dan Kohn ;Date: Dec 8, 2005 ;Functions: Initalize the Left Motor (OC3) ; ;On Call: go_stop MUST be called before initalizing ;************************************************************************** TOC3_init psha pshb pshx SEI LDAA #$7E ;setting the interrupt vector table STAA TOC3IV LDD #TOC3RT STD TOC3IV+1 LDD TCNT ;sets the first time of TOC2 ADDD On_cnts_L STD TOC3 LDAA #$20 ;intializing the registers staa TFLG1 ldx #TMSK1 BSET 0,X #$20 LDX #TCTL1 ;toggles the output BSET 0,X #$10 BCLR 0,x #$20 CLI ;turning on the interrupts pulx pulb pula rts ;************************************************************************** ;Name: TOC3IV ;Author: Gregory Reeves Jr. ;Date: July 30, 2005 ;Modified: Dan Kohn Dec 8, 2005 For Dual Output Compares ;Function: Interrupt for PWM for Left Motor ;On Call On_cnts_L and Off_cnts_L ;************************************************************************** TOC3RT LDAA #$20 ;resets the OC3F bit STAA TFLG1 LDAA PORTA ;check oc3 pin for on or off state ANDA #$20 BNE ON_3 LDD TOC3 ;sets up for the off time sequence ADDD Off_cnts_L STD TOC3 BRA EXIT_3 ON_3 LDD TOC3 ;sets up for the next ontime sequence ADDD On_cnts_L STD TOC3 EXIT_3 RTI ;return from the interrupt ;************************************************************************** ;Name: TOC3_init ;Author: Michael McNeill, Jr. ;Date: ?? ;Functions: Initalize the Multiplexed Servo (OC4) ; ;On Call: go_Gstop MUST be called before initalizing ;************************************************************************** TOC4_init psha pshb pshx SEI LDAA #$7E ;setting the interrupt vector table STAA TOC4IV LDD #TOC4RT STD TOC4IV+1 LDD TCNT ;sets the first time of TOC4 ADDD On_cnts_G STD TOC4 LDAA #$10 ;intializing the registers staa TFLG1 ldx #TMSK1 BSET 0,X #$10 LDX #TCTL1 ;toggles the output BSET 0,X #$4 BCLR 0,x #$8 CLI ;turning on the interrupts pulx pulb pula rts ;************************************************************************** ;Name: TOC4IV ;Author: Michael McNeill, Jr. ;Date: Feb 1, 2006 ;Function: Interrupt for PWM for Gripper Motor ;On Call On_cnts_G and Off_cnts_G ;************************************************************************** TOC4RT LDAA #$10 STAA TFLG1 LDAA PORTA ANDA #$10 BNE ON_4 LDD TOC4 ADDD off_cnts_G STD TOC4 BRA EXIT_4 ON_4 LDD TOC4 ;sets up for the next ontime sequence ADDD On_cnts_G STD TOC4 EXIT_4 RTI ;return from the interrupt ;************************************************************************************************** ; ;Name: Scanner ;Author: Michael McNeill, Jr. ;Date: ?? ;Functions: Clears the Scanner buffer, then waits for the interrupt to ; recieve 33 bits. It then convers the returned bit pattern into plane ; number based on the table below ; ; Scanner reading $26 to $03 ; $1e to $02 ; $16 to $01 ; ;Process: the barcode scanner is read using the falling edge of PA0 (which is ; attached to the barcode clock). When a falling edge is detected, it ; reads the barcode data line (comming in on PE7). This data is placed ; into the buffer from the left and shifted to the right. Each character ; is actually 10 bits long. The first bit coming in is a START BIT, then ; 8 bits of data, then a PARITY BIT then a STOP bit. ; ; so the finish data buffer will look like this: ; ; Byte 4 3 2 1 0 ; Bits 7654321076543210765432107654321076543210 ; ; sPxxxxxxxxSsPxxxxxxxxSsPxxxxxxxxS ; 100010011001111110000010001001100 ; \ / ; data representing plane # ; ; Data shifted --> ; ; s=stop ; P=Parity ; S=START ; ; the data is then compared to the values returned for planes 1,2,3 and ; converted to the plane number. If the scan is bad, $FF will be the result. ; This value is then stored in BOXCODE. ; ;On Call: None ; ;On Return: BOXCODE containes the plane number indicated by the barcode read ($FF ; indicates an error when scanned). ;******************************************************************************************* Scanner: psha jsr BC_clear ldaa #$00 staa sec_count sc_0: ldaa sec_count ; watchdog timer for six seconds cmpa #60 ;* exits after 6 seconds if 33 bits not recieved bhs SC_3 ;* will return $ff as barcode error code lDAA BC_count cmpa #33 BLO sc_0 Ldaa Barcode+3 ;Barcode to read #$26 to plane 3 cmpa #$26 Bne sc_1 ldaa #$03 staa Boxcode jmp sc_exit Sc_1 Ldaa Barcode+3 ;Barcode to read #$1e to plane 2 cmpa #$1e Bne sc_2 ldaa #$02 staa Boxcode jmp sc_exit Sc_2 Ldaa Barcode+3 ;Barcode to read #$16 to plane 1 cmpa #$16 Bne sc_3 ldaa #$01 staa Boxcode jmp sc_exit Sc_3 ldaa #$ff ;flag for bad scan staa Boxcode sc_exit pula rts ;******************************************************************************* ;Name: IC3_init ;Author: Michael McNeill, Jr. ;Date: ?? ;Functions: This initializes IC3 to capture a falling edge of the ; clock for the barcode reader. ; ;******************************************************************************** IC3_init: psha pshx sei ldaa #$7E staa IC3IV LDD #IC3RT STD IC3IV+1 LDX #TCTL2 Bset 0,X #$02 ;set for falling edge Bclr 0,X #$01 LDAA #$01 STAA TFLG1 ;clr flag LDX #TMSK1 ;turn on INT Bset 0,X #$01 cli pulx pula rts ;************************************************************************** ;Name: IC3RT - Barcode interrupt ;Author: Dan Kohn ;Date: ?? ;Function: This interrupt is triggered on a FALLING EDGE of the ; bar code clk signal. That that time, the data line is ; captured and placed into the CARRY BIT which is then ; shifted from the left into the barcode buffer. ; ; [see SCANNER routine for more info] ;************************************************************************** IC3RT: ldaa #$01 ;resets the IC3 bit staa TFLG1 Ldaa portE ;check IC3 pin for on or off state anda #$80 BEQ BC_High sec BRA BC_skip BC_High clc BC_Skip ROR Barcode ROR Barcode+1 ROR Barcode+2 ROR Barcode+3 ROR Barcode+4 INC BC_count RTI ;return from the interrupt ;************************************************************************** ;Name: BC_Clear - Clear Bar Code buffer ;Author: Dan Kohn ;Date: ?? ;Function: Clear the buffer for the bar code buffer (fill with all ones) ;************************************************************************** BC_clear psha LDAA #$FF STAA Barcode STAA Barcode+1 STAA Barcode+2 STAA Barcode+3 STAA Barcode+4 LDAA #$0 STAA BC_count Pula RTS ;***************************************************** ;************************************************************************** ;Name: TOC1_init ;Author: michael mcneill ;Date: 4/15/06 ;Functions: Initalize the toc1_init ; ;On Call: go_stop MUST be called before initalizing ;************************************************************************** TOC1_init psha pshb pshx SEI LDAA #$7E ;setting the interrupt vector table STAA TOC1IV LDD #TOC1RT STD TOC1IV+1 LDD TCNT ;sets the first time of TOC1 ADDD On_cnts_GR STD TOC1 LDAA #$80 ;intializing the registers staa TFLG1 ldx #TMSK1 BSET 0,X #$80 LDAA #$80 STAA oc1m LDx #pactl bset 0,x #$80 CLI ;turning on the interrupts pulx pulb pula rts ;************************************************************************** ;************************************************************************** ;Name: TOCIRT ;Author: Michael McNeill, Jr. ;Date: 3/15/06 ;Function: Interrupt for PWM for Gripper Motor ;On Call On_cnts_Gr and Off_cnts_Gr ;************************************************************************** TOC1RT: LDAA #$80 STAA TFLG1 LDAA PORTA ANDA #$80 Bne toc1_high toc1_low: LDD TOC1 ADDD off_cnts_Gr STD TOC1 ldx #oc1d ;set pin to go high next time TOC=TCNT bset 0,x #$80 BRA EXIT_1 toc1_high: LDD TOC1 ;sets up for the next ontime sequence ADDD on_cnts_Gr STD TOC1 ldx #oc1d ;set pin to go low next time TOC=TCNT bclr 0,x #$80 EXIT_1: RTI ;return from the interrupt ;************************************************************************************************** ;Name: grip0 ;Author: Michael McNeill, Jr. ;Date: 3/15/06 ;Function: Interrupt for PWM for Gripper Motor ;On Call On_cnts_Gr ;Gripper (TOC1init) Variables and Equates: ;gripppe open ; ; ; ;************************************************************************************************************ gripO: pshy ldy #36400 sty off_cnts_Gr ldy #3600 sty on_cnts_Gr puly rts ;************************************************************************************************** ;Name: gripC ;Author: Michael McNeill, Jr. ;Date: 3/15/06 ;Function: Interrupt for PWM for Gripper Motor ;On Call OFF_cnts_Gr ;Gripper (TOC1init) Variables and Equates: ;gripppe close ; ; ; ;************************************************************************************************************ grippC: pshy ldy #35400 sty off_cnts_Gr ldy #4600 sty on_cnts_Gr puly rts ;************************************************************************************************************