API tools faq
paste
Advertisement
Atdiy

Untitled

Atdiy
Oct 1st, 2012
48
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 7.55 KB | None | 0 0
raw download clone embed print report
  1. CON
  2.  
  3. _clkmode = xtal1 + pll16x
  4. _clkfreq = 80_000_000
  5.  
  6. servoPin = 19
  7. datapin = 4 'SDA
  8. clockPin = 5 'SCL
  9.  
  10. '' All available registers on the HMC5883 are listed below: (Check datasheet for detailed information)
  11.  
  12. WRITE_DATA = $3C 'Used to perform a Write operation
  13. READ_DATA = $3D 'Used to perform a Read operation
  14.  
  15. CNFG_A = $00 'Read/Write Register, Sets Data Output Rate. Default = 15Hz & 8 samples per measurement
  16. '160Hz can be achieved by monitoring DRDY pin in single measurement mode.
  17. CNFG_B = $01 'Read/Write Register, Sets the Device Gain(230-1370 Gauss). Default = 1090 Gauss
  18. MODE = $02 'Read/Write Register, Selects the operating mode. Default = Single measurement
  19. 'Send $3C $02 $00 on power up to change to continuous measurement mode.
  20. OUTPUT_X_MSB = $03 'Read Register, Output of X MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  21. OUTPUT_X_LSB = $04 'Read Register, Output of X LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  22. OUTPUT_Z_MSB = $05 'Read Register, Output of Z MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  23. OUTPUT_Z_LSB = $06 'Read Register, Output of Z LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  24. OUTPUT_Y_MSB = $07 'Read Register, Output of Y MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  25. OUTPUT_Y_LSB = $08 'Read Register, Output of Y LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  26. STATUS = $09 'Read Register, indicates device status.
  27. ID_A = $0A 'Read Register, (ASCII value H)
  28. ID_B = $0B 'Read Register, (ASCII value 4)
  29. ID_C = $0C 'Read Register, (ASCII value 3)
  30.  
  31.  
  32. tInc = 80
  33. tC = tInc * 20_000
  34. VAR
  35. long CompassStack[100]
  36. long Angle
  37. long Target
  38. long x
  39. long y
  40. long z
  41. long t
  42. long b
  43.  
  44. byte NE
  45. byte SE
  46. byte SW
  47. byte NW
  48.  
  49. OBJ
  50.  
  51. Term : "FullDuplexSerial"
  52. math : "SL32_INTEngine_2"
  53.  
  54. PUB Main
  55.  
  56. ctra[30..26] := %00100
  57. ctra[8..0] := servoPin
  58.  
  59. frqa := 1
  60. dira[servoPin]~~
  61. t := cnt
  62.  
  63. term.start(31, 30, 0, 115200) 'start a terminal Object (rxpin, txpin, mode, baud rate)
  64. cognew(Compass,@CompassStack)
  65.  
  66. Target := 0
  67.  
  68. Repeat
  69. If Angle >= 185
  70. b := 1450 'Clockwise
  71. phsa := -(b*tInc)
  72. t += tC
  73. waitcnt(t)
  74. elseif Angle <= 175
  75. b := 1550 'Counter clockwise
  76. phsa := -(b*tInc)
  77. t += tC
  78. waitcnt(t)
  79. else
  80. b := 1510
  81. phsa := -(b*tInc)
  82. t += tC
  83. waitcnt(t)
  84.  
  85.  
  86. PUB Compass
  87.  
  88. waitcnt(clkfreq/100_000 + cnt) 'Wait while compass has time to startup.
  89.  
  90. setcont 'sets
  91.  
  92. repeat 'Repeat indefinitely
  93.  
  94. setpointer(OUTPUT_X_MSB) 'Start with Register OUT_X_MSB
  95. getRaw 'Gather raw data from compass
  96.  
  97. term.tx(1) 'Set Terminal data at top of screen
  98. term.str(string("Heading in Degrees:"))
  99. term.dec(aziadjust)
  100. term.tx(13)
  101. term.tx(13)
  102.  
  103. PUB aziadjust : value
  104.  
  105. {{ Converts the Azimuth to Degrees from 0 - 360. }}
  106.  
  107. NW~
  108. NE~
  109. SE~
  110. SW~
  111.  
  112. if x =< 0
  113. if azimuth =< 0
  114. value := AZ_A
  115. NW := 1
  116. else
  117. NW~
  118.  
  119. if azimuth > 0
  120. value := AZ_D
  121. SW := 1
  122. else
  123. SW~
  124.  
  125. if x > 0
  126. if azimuth =< 0
  127. value := AZ_B
  128. NE := 1
  129. else
  130. NE~
  131.  
  132.  
  133. if azimuth > 0
  134. value := AZ_C
  135. SE := 1
  136. else
  137. SE~
  138.  
  139. value := 1 #> value <# 360
  140.  
  141. value := (value + 270) // 360
  142.  
  143.  
  144.  
  145. PUB Heading | t1, t2
  146.  
  147. '' Gives a heading in alpha numeric format. From 0 - 90 degrees for NE,NW,SE,SW directions.
  148. t1~
  149. t2~
  150. t1 := aziadjust
  151.  
  152. if NE == 1
  153.  
  154. term.str(@N)
  155. t2 := aziadjust
  156. term.dec(t2)
  157. term.str(@E)
  158. term.tx(11)
  159.  
  160. if SE == 1
  161.  
  162. term.str(@S)
  163. t2 := aziadjust - 180
  164. term.dec(||t2)
  165. term.str(@E)
  166. term.tx(11)
  167.  
  168. if SW == 1
  169.  
  170. term.str(@S)
  171. t2 := aziadjust - 180
  172. term.dec(t2)
  173. term.str(@W)
  174. term.tx(11)
  175.  
  176. if (NW == 1)
  177.  
  178. term.str(@N)
  179. t2 := 360 - aziadjust <#90
  180. if t2 == 90
  181. t2 := 0
  182. term.dec(t2)
  183. term.str(@W)
  184. term.tx(11)
  185.  
  186.  
  187. if (t1 == 0)
  188. term.tx(11)
  189. term.tx(8)
  190. term.tx(8)
  191. term.tx(8)
  192. term.str(@NORTH)
  193. term.tx(11)
  194.  
  195.  
  196. if t1 == 90
  197. term.tx(12)
  198. term.tx(8)
  199. term.tx(8)
  200. term.tx(8)
  201. term.tx(8)
  202. term.str(@EAST)
  203. term.tx(11)
  204.  
  205.  
  206. if t1 == 180
  207.  
  208. term.tx(12)
  209. term.tx(8)
  210. term.tx(8)
  211. term.tx(8)
  212. term.str(@SOUTH)
  213. term.tx(11)
  214.  
  215.  
  216. if t1 == 271
  217. term.tx(12)
  218. term.tx(8)
  219. term.tx(8)
  220. term.tx(8)
  221. term.tx(8)
  222. term.str(@WEST)
  223. term.tx(11)
  224.  
  225.  
  226. PUB SetCont
  227.  
  228. {{ Sets the Compass to Continuous output mode.}}
  229.  
  230. start
  231. send(WRITE_DATA)
  232. send(MODE)
  233. send($00)
  234. stop
  235.  
  236. PUB SetPointer(Register)
  237.  
  238. {{ Start pointer at user specified Register. }}
  239.  
  240. start
  241. send(WRITE_DATA)
  242. send(Register)
  243. stop
  244.  
  245. PUB GetRaw
  246.  
  247. {{ Get raw data from continous output.}}
  248.  
  249. start
  250. send(READ_DATA)
  251. x := ((receive(true) << 8) | receive(true)) 'RegisterA and RegisterB
  252. z := ((receive(true) << 8) | receive(true))
  253. y := ((receive(true) << 8) | receive(false))
  254. stop
  255. ~~x
  256. ~~z
  257. ~~y
  258. x := x
  259. z := z
  260. y := y
  261.  
  262. PRI Azimuth
  263.  
  264. 'Azimuth = arcTan(y/x)
  265.  
  266. result := math.arctan(y,x)
  267.  
  268. PRI AZ_A 'NE
  269.  
  270. result := ||azimuth
  271.  
  272. PRI AZ_B 'SE
  273.  
  274. result := azimuth + 180
  275.  
  276. PRI AZ_C 'SW
  277.  
  278. result := azimuth + 180
  279.  
  280. PRI AZ_D 'NW
  281.  
  282. result := -azimuth + 360
  283.  
  284. PRI send(value) ' I²C Send data - 4 Stack Longs
  285.  
  286. value := ((!value) >< 8)
  287.  
  288. repeat 8
  289. dira[dataPin] := value
  290. dira[clockPin] := false
  291. dira[clockPin] := true
  292. value >>= 1
  293.  
  294. dira[dataPin] := false
  295. dira[clockPin] := false
  296. result := !(ina[dataPin])
  297. dira[clockPin] := true
  298. dira[dataPin] := true
  299.  
  300. PRI receive(aknowledge) ' I²C receive data - 4 Stack Longs
  301.  
  302. dira[dataPin] := false
  303.  
  304. repeat 8
  305. result <<= 1
  306. dira[clockPin] := false
  307. result |= ina[dataPin]
  308. dira[clockPin] := true
  309.  
  310. dira[dataPin] := aknowledge
  311. dira[clockPin] := false
  312. dira[clockPin] := true
  313. dira[dataPin] := true
  314.  
  315. PRI start ' 3 Stack Longs
  316.  
  317. outa[dataPin] := false
  318. outa[clockPin] := false
  319. dira[dataPin] := true
  320. dira[clockPin] := true
  321.  
  322. PRI stop ' 3 Stack Longs
  323.  
  324. dira[clockPin] := false
  325. dira[dataPin] := false
  326.  
  327. DAT
  328.  
  329. E byte "E",0
  330. N byte "N",0
  331. S byte "S",0
  332. W byte "W",0
  333.  
  334.  
  335. Azm byte "Azimuth = ",0
  336. XRaw byte "X = ",0
  337. YRaw byte "Y = ",0
  338. ZRaw byte "Z = ",0
  339.  
  340. NORTH byte "NORTH",0
  341. SOUTH byte "SOUTH",0
  342. EAST byte "EAST",0
  343. WEST byte "WEST",0
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!