Parallax Penguin, Руководство пользователя

Страница: 22 / 24

Copyright © Parallax Inc. Penguin Robot (#27313-27316) v1.4 2/16/2010 Page 22 of 23
' Parallax Penguin Robot
' Penguin-CompassTest.bpx
' {$STAMP BS2px}
' {$PBASIC 2.5}
Compass PIN 15 ' Transceives to/from Din/Dout
Clock PIN 13 ' Sends pulses to HM55B's Clock
Enable PIN 14 ' Controls HM55B's /Enable
Reset CON %0000 ' Reset command for HM55B
Measure CON %1000 ' Start measurement command
Report CON %1100 ' Get status/axis values command
Ready CON %1100 ' 11 -> Done, 00 -> no errors
NegMask CON %1111100000000000 ' For 11-bit negative to 16-bits
x VAR Word ' x-axis data
y VAR Word ' y-axis data
Heading VAR x ' Store Heading measurement
status VAR x ' Status flags
DEBUG CLS, "Heading:"
DO
GOSUB ReadCompass
DEBUG CRSRXY, 9, 0, DEC Heading, "° "
PAUSE 150
LOOP
ReadCompass: ' Compass module subroutine
HIGH Enable ' Send reset command to HM55B
LOW Enable
SHIFTOUT Compass, Clock, MSBFIRST, [Reset\4]
HIGH Enable ' HM55B start measurement command
LOW Enable
SHIFTOUT Compass, Clock, MSBFIRST, [Measure\4]
status = 0 ' Clear previous status flags
DO ' Status flag checking loop
HIGH Enable: LOW Enable ' Measurement status command
SHIFTOUT Compass, Clock, MSBFIRST, [Report\4]
SHIFTIN Compass, Clock, MSBPOST, [Status\4]' Get Status
LOOP UNTIL status = Ready ' Exit loop when status is ready
SHIFTIN Compass,Clock,MSBPOST,[x\11,y\11] ' Get x & y axis values
HIGH Enable ' Disable module
IF (y.BIT10 = 1) THEN y = y | NegMask ' Store 11-bits as signed word
IF (x.BIT10 = 1) THEN x = x | NegMask ' Repeat for other axis
Heading = x ATN -y ' Convert x and y to brads
Heading = Heading */ 360 ' Convert brads to degrees
RETURN