csa_1hl.asm

电容感应程序

   add A, 3
   mov [CSA_1_bSnsMaskPtr], A ; Store starting position in an unused temp CSA_1_bSnsMaskPtr

; First find the starting location and size of the largest centroid
   mov [CSA_1_bBiggestCtrdStartPos], 0
   mov [CSA_1_bBiggestCtrdSize], 0
   mov [CSA_1_bCurCtrdStartPos], 0
   mov [CSA_1_bCurCtrdSize], 0

   mov A, [CSA_1_bStartIndex] 
   asl A ; multiply by two because we are using ints
   mov X, A

.LocateCtrd:
   RAM_SETPAGE_IDX >CSA_1_waSnsDiff
   mov A, [X+CSA_1_waSnsDiff]
   jnz .DiffIsNotZero
   mov A, [X+CSA_1_waSnsDiff+1]
   jnz .DiffIsNotZero
; The difference is zero, we either just ended a centroid or are between centroids
; First check the Current Size, if zero, we are in the middle of zeros, else, we just ended
.DifferenceIsZero:
   mov A, [CSA_1_bCurCtrdSize]
   jz .LocateCtrdNextSensor
; A centroid just ended.  Is it the largest centroid?  
   cmp A, [CSA_1_bBiggestCtrdSize]
   jc .ClearCurCtrdSize
; It is the biggest so far, store as biggest
   mov [CSA_1_bBiggestCtrdSize], [CSA_1_bCurCtrdSize]
   mov [CSA_1_bBiggestCtrdStartPos], [CSA_1_bCurCtrdStartPos]
.ClearCurCtrdSize:
   mov [CSA_1_bCurCtrdSize], 0
   jmp .LocateCtrdNextSensor
   
;The difference is not zero, we either just started, or are in the middle of a centroid
.DiffIsNotZero:
   mov A, [CSA_1_bCurCtrdSize]
   jnz .IncCtrdSize
; Centroid just began, store the start pos   
   mov A, [CSA_1_bCurPos]
   mov [CSA_1_bCurCtrdStartPos], A
.IncCtrdSize:   
   inc [CSA_1_bCurCtrdSize] 
; Find out the next position
.LocateCtrdNextSensor:   
   inc X
   inc X
   inc [CSA_1_bCurPos] 
   mov A, [CSA_1_bCurPos]
   cmp A, [CSA_1_bEndOfArray] ; Check for the end of the array
   jc .LocateCtrd
; Either at end of array, or diplexed
   tst [CSA_1_bDiplexInfo], 0xff ; check if diplexed
   jz .EndOfLocateCtrd
; Diplexed, so now find out if at end
   sub A, [CSA_1_bEndOfArray] ; subtract the size of the array
   add A, [CSA_1_bStartIndex] ; for comparison
   cmp A, [CSA_1_bEndOfArray] ; Check for the end of the array
   jnc .EndOfLocateCtrd
; Not the end of the diplexed array, find out offset of next position
   mov A, [CSA_1_bCurPos]
   sub A, [CSA_1_bStartIndex]
   add A, [CSA_1_bDiplexLSB]
   mov X, A
   mov A, [CSA_1_bDiplexMSB]
   adc A, 0 ; check if carry
   romx ; get the offset from the start
   add A, [CSA_1_bStartIndex]
   asl A ; because using ints
   mov X, A
   jmp .LocateCtrd
.EndOfLocateCtrd:
; Need to check if the current centroid is biggest
   mov A, [CSA_1_bCurCtrdSize]
   jz .CalculateCtrd
; There was a centroid at the end, is it biggest?
   cmp A, [CSA_1_bBiggestCtrdSize]
   jc .CalculateCtrd ; if two are the same size, last one wins
; It is the biggest so far, store as biggest
   mov [CSA_1_bBiggestCtrdSize], [CSA_1_bCurCtrdSize]
   mov [CSA_1_bBiggestCtrdStartPos], [CSA_1_bCurCtrdStartPos]

.CalculateCtrd:
   mov A, [CSA_1_bBiggestCtrdSize]
   jz .COM_Error
   mov A, [CSA_1_bDiplexInfo] ; check if diplexed
   jz .COM_Init
   cmp [CSA_1_bBiggestCtrdSize], 2
   jc .COM_Error ; for diplexing, one or less is too small
   cmp A, [CSA_1_bBiggestCtrdSize]
   jc .COM_Error ; for diplexing, check if centroid is too large

.COM_Init:
   mov A, [CSA_1_bBiggestCtrdStartPos] ; Use for current position, may be diplexed
   mov [CSA_1_bCurPos], A
   mov [CSA_1_wDenom+MSB], 0 ; Clear the numerator and denominator
   mov [CSA_1_wDenom+LSB], 0
   mov [CSA_1_dNumer+MMSB], 0
   mov [CSA_1_dNumer+MLSB], 0
   mov [CSA_1_dNumer+LMSB], 0
   mov [CSA_1_dNumer+LLSB], 0

.COM_NextPosition:
   mov A, [CSA_1_bCurPos]
   cmp A, [CSA_1_bEndOfArray]
   jnc .COM_CheckDiplex
   asl A
   mov X, A
   jmp .COM_AddElement
; Must be diplexed, check for safe measure
.COM_CheckDiplex:
   tst [CSA_1_bDiplexInfo], 0xff ; check if diplexed
   jz .COM_Compute
; Find out offset of next position
   mov A, [CSA_1_bCurPos]
   sub A, [CSA_1_bStartIndex]
   add A, [CSA_1_bDiplexLSB]
   mov X, A
   mov A, [CSA_1_bDiplexMSB]
   adc A, 0 ; check if carry
   romx ; get the offset from the start
   add A, [CSA_1_bStartIndex]
   asl A ; because using ints
   mov X, A
.COM_AddElement:
; Subtract the noise from the difference, this will yield a more accurate result
   RAM_SETPAGE_IDX >CSA_1_waSnsDiff
   sub [X+CSA_1_waSnsDiff+LSB], NOISE_THRESHOLD
   sbb [X+CSA_1_waSnsDiff+MSB], 0
; Store a copy of the sensor difference in CSA_1_dMultTempX
   mov A, [X+CSA_1_waSnsDiff+LSB]
   mov [CSA_1_dMultTempX+LLSB], A
; Add LSB to denominator
   add [CSA_1_wDenom+LSB], A
   mov A, [X+CSA_1_waSnsDiff]
   mov [CSA_1_dMultTempX+LMSB], A
   mov [CSA_1_dMultTempX+MLSB], 0
   mov [CSA_1_dMultTempX+MMSB], 0
; Add MSB to denominator
   adc [CSA_1_wDenom], A
   mov A, [CSA_1_bCurPos]
   sub A, [CSA_1_bStartIndex] ; we need offset from beginning of group
   mov [CSA_1_wDivBtwSns+MSB], 0 
   mov [CSA_1_wDivBtwSns+LSB], A
   
   call .MultiplyNumeratorWhole

   inc [CSA_1_bCurPos]
   dec [CSA_1_bBiggestCtrdSize]
   jz .COM_Compute
   jmp .COM_NextPosition

.COM_Compute:
   mov A, [CSA_1_dNumer+LLSB] ; Move numerator to temp
   mov [CSA_1_dMultTempX+LLSB], A
   mov [CSA_1_dMultTempY+LLSB], A
   mov A, [CSA_1_dNumer+LMSB]
   mov [CSA_1_dMultTempX+LMSB], A
   mov [CSA_1_dMultTempY+LMSB], A
   mov A, [CSA_1_dNumer+MLSB]
   mov [CSA_1_dMultTempX+MLSB], A
   mov [CSA_1_dMultTempY+MLSB], A
   mov A, [CSA_1_dNumer+MMSB]
   mov [CSA_1_dMultTempX+MMSB], A
   mov [CSA_1_dMultTempY+MMSB], A

   mov [CSA_1_dNumer+MMSB], 0 ; Clear numerator
   mov [CSA_1_dNumer+MLSB], 0
   mov [CSA_1_dNumer+LMSB], 0
   mov [CSA_1_dNumer+LLSB], 0
   
   mov A, [CSA_1_bSnsMaskPtr] ; Retrieve starting position in index of the DivBtwSns
   push A
   index CSA_1_Group_Table ; get MSB of whole multiplier
   mov [CSA_1_wDivBtwSns+MSB], A
   pop A
   inc A
   push A
   index CSA_1_Group_Table ; get LSB of whole multiplier
   mov [CSA_1_wDivBtwSns+LSB], A
   call .MultiplyNumeratorWhole ; Multiplies by whole part
   pop A
   inc A
   index CSA_1_Group_Table ; get byte of fractional multiplier
   mov [CSA_1_wDivBtwSns+LSB], A
   call .MultiplyNumeratorFraction ; Multiplies by fractional part

; Now do the division of the numerator and denominator

; Round up the temp by half of the denominator (0.5 gets 1)
   mov A, [CSA_1_wDenom+MSB]
   mov [CSA_1_dMultTempX+MSB], A
   mov A, [CSA_1_wDenom+LSB]
   mov [CSA_1_dMultTempX+LSB], A
   and F, ~0x04 ; Clear carry bit if set
   rrc [CSA_1_dMultTempX+MSB] ; divide denominator by 2
   rrc [CSA_1_dMultTempX+LSB]
   mov A, [CSA_1_dMultTempX+LSB]
   add [CSA_1_dNumer+LLSB], A ; add 1/2 denominator 
   mov A, [CSA_1_dMultTempX+MSB]
   adc [CSA_1_dNumer+LMSB], A
   adc [CSA_1_dNumer+MLSB], 0
   adc [CSA_1_dNumer+MMSB], 0

; Compute the division of numerator divided by denominator
   asl [CSA_1_dNumer+LLSB] 
   rlc [CSA_1_dNumer+LMSB]	 
   rlc [CSA_1_dNumer+MLSB]	 
   rlc [CSA_1_dNumer+MMSB]
   mov X, 16	 
.DivideLoop:
; Subtract the divisor or denominator from the MMSB MLSB dividend or numerator
   mov A, [CSA_1_wDenom+LSB]
   sub [CSA_1_dNumer+MLSB], A 
   mov A, [CSA_1_wDenom+MSB]
   sbb [CSA_1_dNumer+MMSB], A
; Check if less than zero
   jnc .SetLSbToOneAndShift
.AddBackSetLSbToZeroAndShift:
; Less than zero so add back to dividend and shift a zero into the dividend
   mov A, [CSA_1_wDenom+LSB]
   add [CSA_1_dNumer+MLSB], A 
   mov A, [CSA_1_wDenom+MSB]
   adc [CSA_1_dNumer+MMSB], A
   asl [CSA_1_dNumer+LLSB] 
   rlc [CSA_1_dNumer+LMSB]	 
   rlc [CSA_1_dNumer+MLSB]	 
   rlc [CSA_1_dNumer+MMSB]
   dec X
   jnz .DivideLoop
   jmp .EndOfDivide
.SetLSbToOneAndShift:
   asl [CSA_1_dNumer+LLSB]
   or [CSA_1_dNumer+LLSB], 0x01
   rlc [CSA_1_dNumer+LMSB]	 
   rlc [CSA_1_dNumer+MLSB]	 
   rlc [CSA_1_dNumer+MMSB]
   dec X
   jnz .DivideLoop
.EndOfDivide:
; MMSB and MLSB have carry after shifted right once
   rrc [CSA_1_dNumer+MMSB]
   rrc [CSA_1_dNumer+MLSB]	 
; Load return value
   mov X, [CSA_1_wCtrdPos+MSB]
   mov A, [CSA_1_wCtrdPos+LSB]	 
   jmp .EndGetCtrdPos
.COM_Error:
; Load error return value
   mov A, 0xff
   mov X, 0xff
.EndGetCtrdPos:
; Algorithm is finished, position is in [CSA_1_wCtrdPos]
   RAM_EPILOGUE RAM_USE_CLASS_3
   ret

; Multiplication algorithm (for whole numbers)
.MultiplyNumeratorWhole:
   mov A, 0x01
.CheckToAddNext2xLSB:
   push A
   and A, [CSA_1_wDivBtwSns+LSB] ; See if current 2^x needs added
   jz .SkipAddAndShiftLeftLSB
   mov A, [CSA_1_dMultTempX+LLSB]
   add [CSA_1_dNumer+LLSB], A
   mov A, [CSA_1_dMultTempX+LMSB]
   adc [CSA_1_dNumer+LMSB], A
   mov A, [CSA_1_dMultTempX+MLSB]
   adc [CSA_1_dNumer+MLSB], A
   mov A, [CSA_1_dMultTempX+MMSB]
   adc [CSA_1_dNumer+MMSB], A
.SkipAddAndShiftLeftLSB:
   asl [CSA_1_dMultTempX+LLSB] 
   rlc [CSA_1_dMultTempX+LMSB] 
   rlc [CSA_1_dMultTempX+MLSB] 
   rlc [CSA_1_dMultTempX+MMSB] 
   pop A
   asl A
   jnc .CheckToAddNext2xLSB
   mov A, [CSA_1_wDivBtwSns+MSB]
   jz .EndMultiplyNumeratorWhole ; only multiply by MSB if needed
   mov A, 0x01
.CheckToAddNext2xMSB:
   push A
   and A, [CSA_1_wDivBtwSns+MSB] ; See if current 2^x needs added
   jz .SkipAddAndShiftLeftMSB
   mov A, [CSA_1_dMultTempX+LLSB]
   add [CSA_1_dNumer+LLSB], A
   mov A, [CSA_1_dMultTempX+LMSB]
   adc [CSA_1_dNumer+LMSB], A
   mov A, [CSA_1_dMultTempX+MLSB]
   adc [CSA_1_dNumer+MLSB], A
   mov A, [CSA_1_dMultTempX+MMSB]
   adc [CSA_1_dNumer+MMSB], A
.SkipAddAndShiftLeftMSB:
   asl [CSA_1_dMultTempX+LLSB] 
   rlc [CSA_1_dMultTempX+LMSB] 
   rlc [CSA_1_dMultTempX+MLSB] 
   rlc [CSA_1_dMultTempX+MMSB] 
   pop A
   asl A
   jnc .CheckToAddNext2xMSB
.EndMultiplyNumeratorWhole:
   ret

; Multiplication algorithm (for fractional numbers)
.MultiplyNumeratorFraction:
   and F, ~0x04 ; clear carry bit if set
   rrc [CSA_1_dMultTempY+MMSB] 
   rrc [CSA_1_dMultTempY+MLSB] 
   rrc [CSA_1_dMultTempY+LMSB] 
   rrc [CSA_1_dMultTempY+LLSB] 
   mov A, 0x80
.CheckToAddNextHalf:
   push A
   and A, [CSA_1_wDivBtwSns+LSB]
   jz .SkipAddAndShiftRight
   mov A, [CSA_1_dMultTempY+LLSB]
   add [CSA_1_dNumer+LLSB], A
   mov A, [CSA_1_dMultTempY+LMSB]
   adc [CSA_1_dNumer+LMSB], A
   mov A, [CSA_1_dMultTempY+MLSB]
   adc [CSA_1_dNumer+MLSB], A
   mov A, [CSA_1_dMultTempY+MLSB]
   adc [CSA_1_dNumer+MMSB], A
.SkipAddAndShiftRight:   
   asr [CSA_1_dMultTempY+MMSB] 
   rrc [CSA_1_dMultTempY+MLSB] 
   rrc [CSA_1_dMultTempY+LMSB] 
   rrc [CSA_1_dMultTempY+LLSB] 
   pop A
   and F, ~0x04 ; clear carry bit if set
   rrc A
   jnc .CheckToAddNextHalf
   ret

.ENDSECTION
ENDIF