|
Block Signal Program Code |
This page contains the source code portion of the Basic Stamp 2p program that controls the block signals. Note that this program is written using the PBASIC 2.5 version. The complete code and documentation can be downloaded using this link: BlockSignal.zip
' ====================================================================== ' BlockSignal.bsp 12-15-2005 '{$PBASIC 2.5} '{$STAMP BS2p} ' I/O bit definitions BlockSel0 PIN 0 ' I/O bit Main P0, block address bit 0 BlockSel1 PIN 1 ' I/O bit Main P1, block address bit 1 BlockSel2 PIN 2 ' I/O bit Main P2, block address bit 2 BlockData PIN 3 ' I/O bit Main P3, selected block activity BlockEn1 PIN 4 ' I/O bit Main P4, chip enable 1 BlockEn2 PIN 5 ' I/O bit Main P5, chip enable 2 RunMode PIN 6 ' I/O bit Main P6, Cycle Led's for debug purposes LedPin PIN 7 ' I/O bit Main P7, Led heartbeat indicator SigL1_1 PIN 8 ' I/O bit Main P8 SigL1_2 PIN 9 ' I/O bit Main P9 SigL2_1 PIN 10 ' I/O bit Main P10 SigL2_2 PIN 11 ' I/O bit Main P11 SigL3_1 PIN 12 ' I/O bit Main P12 SigL3_2 PIN 13 ' I/O bit Main P13 SigL4_1 PIN 14 ' I/O bit Main P14 SigL4_2 PIN 15 ' I/O bit Main P15 SigL5_1 PIN 0 ' I/O bit Aux P0 SigL5_2 PIN 1 ' I/O bit Aux P1 SigL6_1 PIN 2 ' I/O bit Aux P2 SigL6_2 PIN 3 ' I/O bit Aux P3 SigL7_1 PIN 4 ' I/O bit Aux P4 SigL7_2 PIN 5 ' I/O bit Aux P5 SigL8_1 PIN 6 ' I/O bit Aux P6, Semaphore servo bit SigL8_2 PIN 7 ' I/O bit Aux P7, Semaphore lamp on/off bit SigL9_1 PIN 8 ' I/O bit Aux P8 SigL9_2 PIN 9 ' I/O bit Aux P9 SigL10_1 PIN 10 ' I/O bit Aux P10 SigL10_2 PIN 11 ' I/O bit Aux P11 SigL11_1 PIN 12 ' I/O bit Aux P12 SigL11_2 PIN 13 ' I/O bit Aux P13 SigL12_1 PIN 14 ' I/O bit Aux P14 SigL12_2 PIN 15 ' I/O bit Aux P15 ' Program constants DefaultDirPri CON %1111111110110111 DefaultDirAux CON %1111111111111111 InActive CON 0 Active CON 1 SigOff CON %00 SigRed CON %01 SigGrn CON %10 SigYel CON %11 RedOn CON 0 ' Yellow adjust. GrnOn CON 1 ' Yellow adjust. ' Program variables Idx VAR Nib ' Working counter BlockStat VAR Word ' Holds block detector input BlockB1 VAR BlockStat.BIT0 BlockB2 VAR BlockStat.BIT1 BlockB3 VAR BlockStat.BIT2 BlockB4 VAR BlockStat.BIT3 BlockB5 VAR BlockStat.BIT4 BlockB6 VAR BlockStat.BIT5 BlockB7 VAR BlockStat.BIT6 BlockB8 VAR BlockStat.BIT7 BlockB9 VAR BlockStat.BIT8 BlockB10 VAR BlockStat.BIT9 BlockLast VAR Word ' Holds previous block detector input BlockChipEn VAR Nib ' ReadBlock working location BlockChipLed VAR BlockChipEn.BIT3 LedCount VAR Byte ' Heartbeat working counter LedCountSet CON $C0 ' Heartbeat Led change rate YelCount VAR Nib ' Working location CheckSignal LastColor VAR Bit ' Last used toggle color ChkActive VAR Bit ' Test/exercise mode working variable SigColor1 VAR Byte ' L1 thru L4 signal color SigL1_L VAR SigColor1.BIT0 SigL1_H VAR SigColor1.BIT1 SigL2_L VAR SigColor1.BIT2 SigL2_H VAR SigColor1.BIT3 SigL3_L VAR SigColor1.BIT4 SigL3_H VAR SigColor1.BIT5 SigL4_L VAR SigColor1.BIT6 SigL4_H VAR SigColor1.BIT7 SigColor2 VAR Word ' L5 thru L12 signal color SigL5_L VAR SigColor2.BIT0 SigL5_H VAR SigColor2.BIT1 SigL6_L VAR SigColor2.BIT2 SigL6_H VAR SigColor2.BIT3 SigL7_L VAR SigColor2.BIT4 SigL7_H VAR SigColor2.BIT5 SigL8_L VAR SigColor2.BIT6 SigL8_H VAR SigColor2.BIT7 SigL9_L VAR SigColor2.BIT8 SigL9_H VAR SigColor2.BIT9 SigL10_L VAR SigColor2.BIT10 SigL10_H VAR SigColor2.BIT11 SigL11_L VAR SigColor2.BIT12 SigL11_H VAR SigColor2.BIT13 SigL12_L VAR SigColor2.BIT14 SigL12_H VAR SigColor2.BIT15 YellowSig1 VAR Byte ' Locations of yellow signals YellowSig2 VAR Word ' Locations of yellow signals YellowCom1 VAR Byte YellowCom2 VAR Word ' Locations of non-yellow signals YellowTmp2 VAR Word ' SigToggle working location YellowTmp1 VAR YellowTmp2.LOWBYTE YellowNew2 VAR Word ' SigToggle working location YellowNew1 VAR YellowNew2.LOWBYTE SemRed CON 220 ' Semaphore red position SemYel CON 110 ' Semaphore yellow position SemGrn CON 1 ' Semaphore green position SemOff CON 0 ' Semaphore off position L8SemColor VAR Byte ' Working location L8SemPos VAR Byte ' Current semaphore position L8MoveDelay VAR Nib ' Semaphore move delay L8DelayStart CON 4 ' Semaphore delay value '-------------------------------------------------------------------------- ' Initialize signals to all off. ProgramStart: AUXIO ' Select I/O bits 16 through 31 DIRS = DefaultDirAux ' Set default Aux I/O direction bits MAINIO ' Select I/O bits 0 through 15 DIRS = DefaultDirPri ' Set default I/O direction bits '-------------------------------------------------------------------------- ' Perform test loop if the RunMode bit is InActive. Cycle through the sequence ' red, green, and off three times. Then read block detector inputs. Display the ' block detector input states on L1 through L10 respectively; off for inactive, ' yellow for active. TestLoop: IF RunMode = InActive THEN FOR LedCount = 1 TO 3 AUXIO ' Select I/O bits 31 - 16 OUTS = %0101010100010101 ' Set color bits for red MAINIO ' Select I/O bits 15 - 0 OUTH = %01010101 ' Set color bits for red L8SemColor = SemRed ' Select semaphore color DO WHILE L8SemPos <> L8SemColor ' Move semaphore GOSUB Semaphore PAUSE 10 LOOP PAUSE 333 AUXIO ' Select I/O bits 31 - 16 OUTS = %1010101000101010 ' Set color bits for green MAINIO ' Select I/O bits 15 - 0 OUTH = %10101010 ' Set color bits for green L8SemColor = SemGrn ' Select semaphore color DO WHILE L8SemPos <> L8SemColor ' Move semaphore GOSUB Semaphore PAUSE 10 LOOP PAUSE 333 AUXIO ' Select I/O bits 31 - 16 OUTS = %0000000000000000 ' Set color bits for off MAINIO ' Select I/O bits 15 - 0 OUTH = %00000000 ' Set color bits for off PAUSE 333 NEXT GOSUB ReadBlock SigColor1 = 0 FOR Idx = 1 TO 4 ChkActive = BlockStat & 1 SigColor1 = SigColor1 >> 2 IF ChkActive = 1 THEN SigColor1 = SigColor1 | $C0 ENDIF BlockStat = BlockStat >> 1 NEXT SigColor2 = 0 FOR Idx = 5 TO 10 ChkActive = BlockStat & 1 SigColor2 = SigColor2 >> 2 IF ChkActive = 1 THEN SigColor2 = SigColor2 | $C000 ENDIF BlockStat = BlockStat >> 1 NEXT SigColor2 = SigColor2 >> 4 GOSUB YellowSig ' Identify yellow signals AUXIO ' Select I/O bits 31 - 16 OUTS = SigColor2 ' Set new signal colors MAINIO ' Select I/O bits 15 - 0 OUTH = SigColor1 ' Set new signal colors FOR LedCount = 0 TO $FF GOSUB CheckToggle PAUSE 5 NEXT TOGGLE LedPin GOTO TestLoop ENDIF L8SemColor = SemOff ' Turn off semaphore ' ---------------------------------------------------------------------- ' Main program loop. The CheckToggle routine is called often to handle ' yellow signal indications. The block detectors are read and processed ' when we change the heartbeat Led state. This check rate is fast enough ' and provides some debounce of block detector input. Also, when a ' semaphore move is NOT inprogress, the semaphore servo must be pulsed ' periodically to maintain its position. MainLoop: IF RunMode = Active THEN IF LedCount = 0 THEN TOGGLE LedPin LedCount = LedCountSet GOSUB ReadBlock GOSUB CheckToggle GOSUB ProcBlock IF L8SemPos = L8SemColor THEN GOSUB Semaphore ' Maintain servo position ENDIF ELSE IF L8SemPos <> L8SemColor THEN ' Semaphore change in progress? IF L8MoveDelay = 0 THEN ' Yes. Move delay zero? GOSUB Semaphore ' Yes, update semaphore position L8MoveDelay = L8DelayStart ' Reset move delay ELSE L8MoveDelay = L8MoveDelay - 1 ' Decrement move delay ENDIF ENDIF GOSUB CheckToggle LedCount = LedCount - 1 ENDIF GOTO MainLoop ENDIF GOTO ProgramStart ' ---------------------------------------------------------------------- ' This routine reads the block detector input data and stores it in the ' BlockStat variable. ReadBlock: BlockStat = 0 ' Clear previous contents FOR Idx = 0 TO 9 ' Step through all block detectors BlockChipEn = (Idx/8)+1 ' Compute chip enable bits 5 and 4 BlockChipLed = ~LedPin ' Include heartbeat led state OUTB = ~BlockChipEn ' Set chip enable bits 5 and 4 OUTA = Idx // 8 ' Set block input selection bits 2-0 IF BlockData = Active THEN BlockStat.LOWBIT(Idx) = 1 ' Mark block active ENDIF IF Idx = 4 OR Idx = 7 THEN GOSUB CheckToggle ENDIF NEXT BlockChipEn = $7 ' Reset chip enable bits 5 and 4 BlockChipLed = LedPin ' Include heartbeat led state OUTB = BlockChipEn ' Load I/O bits RETURN ' ---------------------------------------------------------------------- ' This routine processes the block detector input data if it has changed ' since the last loop. It stores new singal color settings in the SigColor1 ' and SigColor2 variable. It preserves the state of the semaphore lamp bit ' and then sets the BS2 I/O bits. ' ' When setting a signal to green, the color bits must first check and ' preserve a red or yellow set earlier in the logic tree (beginning with ' the BlockB4 code). Yellow settings must likewise preserve previous red ' color bits. The low bit of the signal color pair is check for a 1. These ' rules ensure that red indications have the highest priority, yellow the ' second, the green the lowest priority. ProcBlock: IF BlockStat <> BlockLast THEN ' Process if detectors have changed SigColor1 = 0 ' Clear all signal color bits SigColor2 = 0 ' Clear all signal color bits ' BlockB3 processing IF BlockB3 = Active THEN SigL1_L = 1 ' L1red SigL1_H = 0 ' L1red SigL2_L = 1 ' L2red SigL2_H = 0 ' L2red IF BlockB4 = InActive THEN SigL4_L = 1 ' L4yel SigL4_H = 1 ' L4yel IF BlockB3 = Active AND BlockB6 = Active THEN SigL3_L = 1 ' L3yel SigL3_H = 1 ' L3yel ELSE SigL3_L = 0 ' L3grn SigL3_H = 1 ' L3grn ENDIF ENDIF IF BlockB5 = InActive THEN SigL6_L = 1 ' L6yel SigL6_H = 1 ' L6yel IF BlockB3 = Active AND BlockB6 = Active THEN SigL5_L = 1 ' L5yel SigL5_H = 1 ' L5yel ELSE SigL5_L = 0 ' L5grn SigL5_H = 1 ' L5grn ENDIF ENDIF ELSE IF BlockB4 = InActive AND BlockB5 = InActive THEN IF BlockB1 = Active OR BlockB2 = Active THEN SigL1_L = 0 ' L1grn SigL1_H = 1 ' L1grn SigL2_L = 1 ' L2yel SigL2_H = 1 ' L2yel ENDIF ENDIF ENDIF ' BlockB4 processing IF BlockB4 = Active THEN SigL3_L = 1 ' L3red SigL3_H = 0 ' L3red SigL4_L = 1 ' L4red SigL4_H = 0 ' L4red IF BlockB3 = InActive THEN IF SigL1_L <> 1 THEN SigL1_L = 1 ' L1yel SigL1_H = 1 ' L1yel ENDIF IF SigL2_L <> 1 THEN SigL2_L = 0 ' L2grn SigL2_H = 1 ' L2grn ENDIF ENDIF IF BlockB6 = InActive THEN IF SigL7_L <> 1 THEN SigL7_L = 0 ' L7grn SigL7_H = 1 ' L7grn ENDIF IF SigL8_L <> 1 THEN SigL8_L = 1 ' L8yel SigL8_H = 1 ' L8yel ENDIF ENDIF ENDIF ' BlockB5 processing IF BlockB5 = Active THEN SigL5_L = 1 ' L5red SigL5_H = 0 ' L5red SigL6_L = 1 ' L6red SigL6_H = 0 ' L6red IF BlockB3 = InActive THEN IF SigL1_L <> 1 THEN SigL1_L = 1 ' L1yel SigL1_H = 1 ' L1yel ENDIF IF SigL2_L <> 1 THEN SigL2_L = 0 ' L2grn SigL2_H = 1 ' L2grn ENDIF ENDIF IF BlockB6 = InActive THEN IF SigL7_L <> 1 THEN SigL7_L = 0 ' L7grn SigL7_H = 1 ' L7grn ENDIF IF SigL8_L <> 1 THEN SigL8_L = 1 ' L8yel SigL8_H = 1 ' L8yel ENDIF ENDIF ENDIF ' BlockB6 processing IF BlockB6 = Active THEN SigL7_L = 1 ' L7red SigL7_H = 0 ' L7red SigL8_L = 1 ' L8red SigL8_H = 0 ' L8red IF BlockB7 = InActive THEN IF SigL9_L <> 1 THEN SigL9_L = 0 ' L9grn SigL9_H = 1 ' L9grn ENDIF IF SigL10_L <> 1 THEN SigL10_L = 1 ' L10yel SigL10_H = 1 ' L10yel ENDIF ENDIF IF BlockB8 = InActive THEN IF SigL11_L <> 1 THEN SigL11_L = 0 ' L11grn SigL11_H = 1 ' L11grn ENDIF IF SigL12_L <> 1 THEN SigL12_L = 1 ' L12yel SigL12_H = 1 ' L12yel ENDIF ENDIF IF BlockB4 = InActive THEN IF SigL3_L <> 1 THEN SigL3_L = 1 ' L3yel SigL3_H = 1 ' L3yel ENDIF IF BlockB3 = InActive THEN IF SigL4_L <> 1 THEN SigL4_L = 0 ' L4grn SigL4_H = 1 ' L4grn ENDIF ENDIF ENDIF IF BlockB5 = InActive THEN IF SigL5_L <> 1 THEN SigL5_L = 1 ' L5yel SigL5_H = 1 ' L5yel ENDIF IF BlockB3 = InActive THEN IF SigL6_L <> 1 THEN SigL6_L = 0 ' L6grn SigL6_H = 1 ' L6grn ENDIF ENDIF ENDIF ENDIF ' BlockB7 processing IF BlockB7 = Active THEN SigL9_L = 1 ' L9red SigL9_H = 0 ' L9red SigL10_L = 1 ' L10red SigL10_H = 0 ' L10red IF BlockB6 = InActive THEN IF SigL7_L <> 1 THEN SigL7_L = 1 ' L7yel SigL7_H = 1 ' L7yel ENDIF IF SigL8_L <> 1 THEN SigL8_L = 0 ' L8grn SigL8_H = 1 ' L8grn ENDIF ENDIF ELSE IF BlockB6 = InActive THEN IF BlockB9 = Active OR BlockB10 = Active THEN IF SigL9_L <> 1 THEN SigL9_L = 1 ' L9yel SigL9_H = 1 ' L9yel ENDIF IF SigL10_L <> 1 THEN SigL10_L = 0 ' L10grn SigL10_H = 1 ' L10grn ENDIF ENDIF ENDIF ENDIF ' BlockB8 processing IF BlockB8 = Active THEN SigL11_L = 1 ' L11red SigL11_H = 0 ' L11red SigL12_L = 1 ' L12red SigL12_H = 0 ' L12red IF BlockB6 = InActive THEN IF SigL7_L <> 1 THEN SigL7_L = 1 ' L7yel SigL7_H = 1 ' L7yel ENDIF IF SigL8_L <> 1 THEN SigL8_L = 0 ' L8grn SigL8_H = 1 ' L8grn ENDIF ENDIF ELSE IF BlockB6 = InActive THEN IF BlockB9 = Active OR BlockB10 = Active THEN IF SigL11_L <> 1 THEN SigL11_L = 1 ' L11yel SigL11_H = 1 ' L11yel ENDIF IF SigL12_L <> 1 THEN SigL12_L = 0 ' L12grn SigL12_H = 1 ' L12grn ENDIF ENDIF ENDIF ENDIF BlockLast = BlockStat ' Remember detector data GOSUB YellowSig AUXIO ' Select I/O bits 31 - 16 YellowNew2 = INS & $0080 ' Get current semaphore lamp bit YellowTmp2 = SigColor2 & $FF3F ' Reset semaphore bits OUTS = YellowTmp2 | YellowNew2 ' Set new colors and semaphore bit MAINIO ' Select I/O bits 15 - 0 OUTH = SigColor1 ' Set new signal colors ' Set the semaphore color servo position in working location based on the ' SigColor2 bits. IF SigL8_H = 0 AND SigL8_L = 0 THEN L8SemColor = SemOff ELSEIF SigL8_H = 0 AND SigL8_L = 1 THEN L8SemColor = SemRed ELSEIF SigL8_H = 1 AND SigL8_L = 0 THEN L8SemColor = SemGrn ELSE L8SemColor = SemYel ENDIF ENDIF RETURN ' ---------------------------------------------------------------------- ' This routine is used to locate the yellow color signals and set the ' YellowSig1, YellowSig2, YellowCom1, and YellowCom2 variables. These ' variables are used by the SigToggle routine. For yellow signals, the ' initial color is set to green. YellowSig: YellowSig1 = 0 FOR Idx = 0 TO 6 STEP 2 IF SigColor1.LOWBIT(Idx) = 1 AND SigColor1.LOWBIT(Idx + 1) = 1 THEN YellowSig1.LOWBIT(Idx) = SigColor1.LOWBIT(Idx) YellowSig1.LOWBIT(Idx + 1) = SigColor1.LOWBIT(Idx + 1) SigColor1.LOWBIT(Idx) = 0 ENDIF NEXT YellowCom1 = ~YellowSig1 YellowSig2 = 0 FOR idx = 0 TO 14 STEP 2 IF SigColor2.LOWBIT(Idx) = 1 AND SigColor2.LOWBIT(Idx + 1) = 1 THEN ' Signal L8 is a semaphore and is handled by separate code. Add/remove idx ' tests as required. IF idx <> 6 THEN YellowSig2.LOWBIT(Idx) = SigColor2.LOWBIT(Idx) YellowSig2.LOWBIT(Idx + 1) = SigColor2.LOWBIT(Idx + 1) SigColor2.LOWBIT(Idx) = 0 ENDIF ENDIF NEXT YellowCom2 = ~YellowSig2 RETURN ' ---------------------------------------------------------------------- ' This routine is used to adjust the red/green proportions of the yellow ' color. RedOn and GrnOn specify the time (count) for each color. CheckToggle: IF YelCount = 0 THEN GOSUB SigToggle IF LastColor = 0 THEN YelCount = RedOn ELSE YelCount = GrnOn ENDIF LastColor = ~LastColor ' Switch to other color ELSE YelCount = YelCount - 1 ENDIF RETURN ' ---------------------------------------------------------------------- ' This routine toggles the state of an I/O bit pair if they are being ' used to display a yellow signal. SigToggle: AUXIO ' Select I/O bits 31 - 16 YellowTmp2 = INS & YellowCom2 ' Save non-yellow bits YellowNew2 = ~INS ' Read and invert all bits YellowNew2 = YellowNew2 & YellowSig2 ' Mask for yellow signals OUTS = YellowTmp2 | YellowNew2 ' Set new states in I/O bits MAINIO ' Select I/O bits 15 - 0 YellowTmp1 = INH & YellowCom1 ' Save non-yellow bits YellowNew1 = ~INH ' Read and invert all bits YellowNew1 = YellowNew1 & YellowSig1 ' Mask for yellow signals OUTH = YellowTmp1 | YellowNew1 ' Set new states in I/O bits RETURN ' ---------------------------------------------------------------------- ' This routine handles the positioning of the L8 semaphore. Move the ' servo by 1 step if not at correct position. If in correct position, ' pulse servo when called to maintain its position. Semaphore: AUXIO ' Select I/O bits 31 - 16 IF L8SemPos <> L8SemColor THEN IF SigL8_2 = 1 THEN LOW SigL8_2 ' Turn off semaphore lamp ENDIF IF L8SemPos < L8SemColor THEN IF (L8SemPos + 1) > L8SemColor THEN L8SemPos = L8SemColor ELSE L8SemPos = L8SemPos + 1 ENDIF ELSE IF L8SemPos < 1 OR (L8SemPos - 1) < L8SemColor THEN L8SemPos = L8SemColor ELSE L8SemPos = L8SemPos - 1 ENDIF ENDIF PULSOUT SigL8_1, 1020 + (L8SemPos * 2) ' Set semaphore position IF L8SemPos = L8SemColor AND L8SemPos <> SemOff THEN HIGH SigL8_2 ' Turn on semaphore lamp ENDIF ELSE PULSOUT SigL8_1, 1020 + (L8SemPos * 2) ' Maintain position ENDIF MAINIO ' Select I/O bits 15 - 0 RETURN
Navigation: Block Signal D&B Home Buczynski.com Index
Copyright © 2006 Don Buczynski
San Diego, California