-
Notifications
You must be signed in to change notification settings - Fork 0
/
HM55B Compass Module Asm.spin
275 lines (223 loc) · 26 KB
/
HM55B Compass Module Asm.spin
1
{{''****************************************''* HM55B Compass Module DEMO V1.2 *''* Author: Beau Schwabe *''* Copyright (c) 2008 Parallax, Inc. * ''* See end of file for terms of use. * ''**************************************** ┌────┬┬────┐ ┌────│1 6│── +5V P0 = Enable │ 1K │ ├┴──┴┤ │ P1 = Clock P2 ┻──│2 │ /\ │ 5│── P0 P2 = Data │ │/ \│ │ VSS ──│3 └────┘ 4│── P1 └──────────┘ }}VARlong cog,Enable,Clock,Data,HM55B_x,HM55B_y,HM55B_thetaPUB stop'' Stop driver - frees a cog if cog cogstop(cog~ - 1)PUB start(EnablePin,ClockPin,DataPin):okay'' Start driver - starts a cog'' returns false if no cog available Enable := EnablePin Clock := ClockPin Data := DataPin okay := cog := cognew(@HM55B, @Enable)PUB x return HM55B_x PUB y return HM55B_yPUB theta return HM55B_theta DATHM55B org mov t1, par rdlong t2, t1 mov EnablePin_mask, #1 shl EnablePin_mask, t2 add t1, #4 rdlong t2, t1 mov ClockPin_mask, #1 shl ClockPin_mask, t2 add t1, #4 rdlong t2, t1 mov DataPin_mask, #1 shl DataPin_mask, t2 add t1, #4 mov HM55B_x_, t1 add t1, #4 mov HM55B_y_, t1 add t1, #4 mov HM55B_theta_, t1 or outa, EnablePin_mask 'Pre-Set Enable pin HIGH or dira, EnablePin_mask 'Set Enable pin as an OUTPUT call #ClockTheEnable mov t4, #4 'Reset Compass mov t3, Reset call #SHIFTOUT call #ClockTheEnable mov t4, #4 'Start Compass Measurement mov t3, Measure call #SHIFTOUTStatus call #ClockTheEnable mov t4, #4 'Get Status mov t3, Report call #SHIFTOUT mov t4, #4 call #SHIFTIN and t3, #$0F sub Report, t3 wz,nr 'Exit loop when status is ready if_nz jmp #Status mov t4, #11 'Read Compass x value call #SHIFTIN and t3, DataMask mov x_, t3 mov t4, #11 'Read Compass y value call #SHIFTIN and t3, DataMask mov y_, t3 call #ClockTheEnable test x_, TestMask wz if_nz or x_, NegMask test y_, TestMask wz if_nz or y_, NegMask neg y_, y_ mov cx, x_ mov cy, y_ call #cordic shr ca, #19 mov Theta_, ca mov t1, HM55B_x_ ' Write x data back wrlong x_, t1 mov t1, HM55B_y_ ' Write y data back wrlong y_, t1 mov t1, HM55B_Theta_ ' Write theta data back wrlong Theta_, t1 jmp #HM55B '------------------------------------------------------------------------------------------------------------------------------SHIFTOUT 'SHIFTOUT Entry andn outa, DataPin_mask 'Pre-Set Data pin LOW or dira, DataPin_mask 'Set Data pin as an OUTPUT andn outa, ClockPin_mask 'Pre-Set Clock pin LOW or dira, ClockPin_mask 'Set Clock pin as an OUTPUTMSBFIRST_ ' Send Data MSBFIRST mov t5, #%1 ' Create MSB mask ; load t5 with "1" shl t5, t4 ' Shift "1" N number of bits to the left. shr t5, #1 ' Shifting the number of bits left actually puts ' us one more place to the left than we want. To ' compensate we'll shift one position right. MSB_Sout test t3, t5 wc ' Test MSB of DataValue muxc outa, DataPin_mask ' Set DataBit HIGH or LOW shr t5, #1 ' Prepare for next DataBit call #Clk ' Send clock pulse djnz t4, #MSB_Sout ' Decrement t4 ; jump if not Zero andn outa, DataPin_mask ' Force DataBit LOWSHIFTOUT_ret ret'------------------------------------------------------------------------------------------------------------------------------SHIFTIN 'SHIFTIN Entry andn dira, DataPin_mask 'Set Data pin as an INPUT andn outa, ClockPin_mask 'Pre-Set Clock pin LOW or dira, ClockPin_mask 'Set Clock pin as an OUTPUTMSBPOST_ ' Receive Data MSBPOSTMSBPOST_Sin call #Clk ' Send clock pulse test DataPin_mask, ina wc ' Read Data Bit into 'C' flag rcl t3, #1 ' rotate "C" flag into return value djnz t4, #MSBPOST_Sin ' Decrement t4 ; jump if not ZeroSHIFTIN_ret ret '------------------------------------------------------------------------------------------------------------------------------Clk or outa, ClockPin_mask ' Set ClockPin HIGH andn outa, ClockPin_mask ' Set ClockPin LOWClk_ret ret '------------------------------------------------------------------------------------------------------------------------------ClockTheEnable or outa, EnablePin_mask ' Set EnablePin HIGH andn outa, EnablePin_mask ' Set EnablePin LOWClockTheEnable_ret ret'------------------------------------------------------------------------------------------------------------------------------' Perform CORDIC cartesian-to-polar conversioncordic abs cx,cx wc if_c neg cy,cy mov ca,#0 rcr ca,#1 movs :lookup,#table mov t1,#0 mov t2,#20:loop mov dx,cy wc sar dx,t1 mov dy,cx sar dy,t1 sumc cx,dx sumnc cy,dy:lookup sumc ca,table add :lookup,#1 add t1,#1 djnz t2,#:loopcordic_ret rettable long $20000000 long $12E4051E long $09FB385B long $051111D4 long $028B0D43 long $0145D7E1 long $00A2F61E long $00517C55 long $0028BE53 long $00145F2F long $000A2F98 long $000517CC long $00028BE6 long $000145F3 long $0000A2FA long $0000517D long $000028BE long $0000145F long $00000A30 long $00000518'------------------------------------------------------------------------------------------------------------------------------' Initialized dataReset long %0000Measure long %1000Report long %1100DataMask long %00000000_00000000_00000111_11111111TestMask long %00000000_00000000_00000010_00000000NegMask long %11111111_11111111_11111100_00000000x_ long 0y_ long 0theta_ long 0' Uninitialized datat1 res 1t2 res 1t3 res 1t4 res 1t5 res 1EnablePin_mask res 1ClockPin_mask res 1DataPin_mask res 1HM55B_x_ res 1HM55B_y_ res 1HM55B_theta_ res 1dx res 1dy res 1cx res 1cy res 1ca res 1{{┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐│ TERMS OF USE: MIT License │ ├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤│Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation │ │files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, ││modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software││is furnished to do so, subject to the following conditions: ││ ││The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.││ ││THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE ││WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR ││COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ││ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. │└──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘}}