Don't like ads? PRO users don't see any ads ;-)
Public Pastes
Guest

Owl V.01

By: a guest on Jun 5th, 2012  |  syntax: None  |  size: 6.55 KB  |  hits: 45  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. CON
  2.   _clkmode = xtal1 + pll16x
  3.   _xinfreq = 5_000_000
  4.   servoPin = 19
  5.   ping_pin = 0
  6.   datapin  = 4  'SDA
  7.   clockPin = 5   'SCL
  8.  
  9.   WRITE_DATA     = $3C 'Used to perform a Write operation
  10.   READ_DATA      = $3D 'Used to perform a Read operation
  11.  
  12.   CNFG_A         = $00 'Read/Write Register, Sets Data Output Rate. Default = 15Hz & 8 samples per measurement
  13.                        '160Hz can be achieved by monitoring DRDY pin in single measurement mode.
  14.   CNFG_B         = $01 'Read/Write Register, Sets the Device Gain(230-1370 Gauss). Default = 1090 Gauss
  15.   MODE           = $02 'Read/Write Register, Selects the operating mode. Default = Single measurement
  16.                        'Send $3C $02 $00 on power up to change to continuous measurement mode.
  17.   OUTPUT_X_MSB   = $03 'Read Register, Output of X MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  18.   OUTPUT_X_LSB   = $04 'Read Register, Output of X LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  19.   OUTPUT_Z_MSB   = $05 'Read Register, Output of Z MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  20.   OUTPUT_Z_LSB   = $06 'Read Register, Output of Z LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  21.   OUTPUT_Y_MSB   = $07 'Read Register, Output of Y MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  22.   OUTPUT_Y_LSB   = $08 'Read Register, Output of Y LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  23.   STATUS         = $09 'Read Register, indicates device status.
  24.   ID_A           = $0A 'Read Register, (ASCII value H)
  25.   ID_B           = $0B 'Read Register, (ASCII value 4)
  26.   ID_C           = $0C 'Read Register, (ASCII value 3)
  27. OBJ
  28.   ping : "Ping"
  29.   Term      :   "FullDuplexSerial"
  30.   math      :   "SL32_INTEngine_2"
  31. VAR
  32.   long ServoStack[32]
  33.   long PingCompassStack[32]
  34.   long range
  35.   long x
  36.   long y
  37.   long z
  38.  
  39.   byte NE
  40.   byte SE
  41.   byte SW
  42.   byte NW
  43.  
  44. Pub Main
  45.   term.start(31, 30, 0, 9600)
  46.   cognew(Servo1, @ServoStack)
  47.   cognew(PingCompass, @PingCompassStack)
  48. PUB Servo1 | tInc, tC, tCtr, tCw, tCcw, t, b
  49.  
  50. ctra[30..26] := %00100
  51. ctra[8..0] := servoPin
  52.  
  53. frqa := 1
  54. dira[servoPin]~~
  55.  
  56. tInc := clkfreq/1_000_000
  57. tC := tInc * 20_000
  58. tCtr := tInc * 1510
  59. tCw := tInc * 1300
  60. tCcw := tInc * 1700
  61. t := cnt
  62.  
  63. repeat
  64.   repeat b from tCtr to tCw step (tInc * 4)
  65.     repeat 1
  66.       phsa := -b
  67.       t += tC
  68.       waitcnt(t)
  69.   repeat b from tCw to tCtr step (tInc * 4)
  70.     repeat 1
  71.       phsa := -b
  72.       t += tC
  73.       waitcnt(t)
  74.   repeat b from tCtr to tCcw step (tInc * 4)
  75.     repeat 1
  76.       phsa := -b
  77.       t += tC
  78.       waitcnt (t)
  79.   repeat b from tCcw to tCtr step (tInc * 4)
  80.     repeat 1
  81.       phsa := -b
  82.       t += tC
  83.       waitcnt (t)
  84.  
  85. PUB PingCompass
  86.  
  87.   waitcnt(clkfreq/100_000 + cnt)      'Wait while compass has time to startup.
  88.  
  89.   setcont                             'sets measurements to go continuously
  90.  
  91.   repeat                              'Repeat indefinitely
  92.  
  93.      setpointer(OUTPUT_X_MSB)         'Start with Register OUT_X_MSB
  94.  
  95.      getRaw                           'Gather raw data from compass
  96.      range := ping.Inches(ping_pin)  ' Get range in inches
  97.  
  98.      term.str(string("Ping Range:", 32))
  99.      term.dec(range)                  ' Display result
  100.      term.tx(13)
  101.  
  102.      RawTerm                          'Terminal window display X,Y,Z Raw Data
  103.      term.tx(13)
  104.  
  105.      term.str(string("Heading in Degrees:", 32))
  106.      term.dec(aziadjust)
  107.      term.tx(13)
  108.  
  109.      waitcnt(clkfreq + cnt)
  110.  
  111. PUB AzimuthTerm
  112.  
  113.  ''Terminal window display of calculated arcTan(y/x)
  114.  
  115.      term.str(string("This is the calculated azimuth:",11))
  116.      term.tx(13)
  117.      term.tx(13)
  118.      term.str(@Azm)
  119.      term.dec(azimuth)
  120.      term.tx(13)
  121.      term.tx(13)
  122. PUB RawTerm
  123.  
  124.   '' Terminal window display X,Y,Z Raw Data
  125.      term.str(string("Raw Compass Data:", 32))
  126.      term.str(@XRaw)
  127.      term.dec(x)
  128.      term.tx(32)
  129.      term.str(@YRaw)
  130.      term.dec(y)
  131.      term.tx(32)
  132.      term.str(@ZRaw)
  133.      term.dec(z)
  134.      term.tx(13)
  135.  
  136. PUB aziadjust  : value
  137.  
  138. {{ Converts the Azimuth to Degrees from 0 - 360. }}
  139.  
  140. NW~
  141. NE~
  142. SE~
  143. SW~
  144.  
  145.   if  x =< 0
  146.         if azimuth =< 0
  147.            value := AZ_A
  148.            NW := 1
  149.         else
  150.             NW~
  151.  
  152.         if azimuth > 0
  153.            value := AZ_D
  154.            SW := 1
  155.         else
  156.             SW~
  157.  
  158.   if x > 0
  159.         if azimuth =< 0
  160.           value := AZ_B
  161.           NE := 1
  162.         else
  163.            NE~
  164.  
  165.  
  166.         if azimuth > 0
  167.           value := AZ_C
  168.           SE := 1
  169.         else
  170.             SE~
  171.  
  172.   value := 1 #> value <# 360
  173.  
  174.   value := (value + 270) // 360
  175.  
  176. PUB SetCont
  177.  
  178. {{ Sets the Compass to Continuous output mode.}}
  179.  
  180.   start
  181.   send(WRITE_DATA)
  182.   send(MODE)
  183.   send($00)
  184.   stop
  185.  
  186. PUB SetPointer(Register)
  187.  
  188. {{ Start pointer at user specified Register. }}
  189.  
  190. start
  191. send(WRITE_DATA)
  192. send(Register)
  193. stop
  194.  
  195. PUB GetRaw
  196.  
  197. {{ Get raw data from continous output.}}
  198.  
  199.   start
  200.   send(READ_DATA)
  201.   x := ((receive(true) << 8) | receive(true))            'RegisterA and RegisterB
  202.   z := ((receive(true) << 8) | receive(true))
  203.   y := ((receive(true) << 8) | receive(false))
  204.   stop
  205.   ~~x
  206.   ~~z
  207.   ~~y
  208.   x := x
  209.   z := z
  210.   y := y
  211.  
  212. PRI Azimuth
  213.  
  214.     'Azimuth = arcTan(y/x)
  215.  
  216.            result := math.arctan(y,x)
  217.  
  218. PRI AZ_A        'NE
  219.  
  220.                result := ||azimuth
  221.  
  222. PRI AZ_B        'SE
  223.  
  224.                result := azimuth  + 180
  225.  
  226. PRI AZ_C        'SW
  227.  
  228.                result := azimuth  + 180
  229.  
  230. PRI AZ_D        'NW
  231.  
  232.                result := -azimuth + 360
  233.  
  234. PRI send(value) ' I²C Send data - 4 Stack Longs
  235.  
  236.   value := ((!value) >< 8)
  237.  
  238.   repeat 8
  239.     dira[dataPin]  := value
  240.     dira[clockPin] := false
  241.     dira[clockPin] := true
  242.     value >>= 1
  243.  
  244.   dira[dataPin]  := false
  245.   dira[clockPin] := false
  246.   result         := !(ina[dataPin])
  247.   dira[clockPin] := true
  248.   dira[dataPin]  := true
  249.  
  250. PRI receive(aknowledge) ' I²C receive data - 4 Stack Longs
  251.  
  252.   dira[dataPin] := false
  253.  
  254.   repeat 8
  255.     result <<= 1
  256.     dira[clockPin] := false
  257.     result         |= ina[dataPin]
  258.     dira[clockPin] := true
  259.  
  260.   dira[dataPin]  := aknowledge
  261.   dira[clockPin] := false
  262.   dira[clockPin] := true
  263.   dira[dataPin]  := true
  264.  
  265. PRI start ' 3 Stack Longs
  266.  
  267.   outa[dataPin]  := false
  268.   outa[clockPin] := false
  269.   dira[dataPin]  := true
  270.   dira[clockPin] := true
  271.  
  272. PRI stop ' 3 Stack Longs
  273.  
  274.   dira[clockPin] := false
  275.   dira[dataPin]  := false
  276.  
  277. DAT
  278.  
  279. Azm     byte "Azimuth = ",0
  280. XRaw       byte "X = ",0
  281. YRaw       byte "Y = ",0
  282. ZRaw       byte "Z = ",0
create a new version of this paste RAW Paste Data