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

Owl V.04

By: a guest on Jun 6th, 2012  |  syntax: None  |  size: 6.01 KB  |  hits: 37  |  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.  
  28. tInc = 80
  29. tC = tInc * 20_000
  30. tCtr = tInc * 1510
  31. tCw = tInc * 1300
  32. tCcw = tInc * 1700
  33.  
  34. OBJ
  35.   ping      :   "Ping"
  36.   Term      :   "FullDuplexSerial"
  37.   math      :   "SL32_INTEngine_2"
  38. VAR
  39.   long CompassStack[1000]
  40.   long GotoAngleStack[100]
  41.   long Angle
  42.   long range
  43.   long Target
  44.   long x
  45.   long y
  46.   long z
  47.   long t
  48.   long b
  49.  
  50.   byte NE
  51.   byte SE
  52.   byte SW
  53.   byte NW
  54.  
  55. Pub Main
  56.  
  57. term.start(31, 30, 0, 115200)
  58.  
  59. cognew(Compass, @CompassStack)
  60. cognew(GotoAngle, @GotoAngleStack)
  61.  
  62. Target := 0
  63.  
  64. Repeat
  65.   If ||(Target - Angle) < 4                          'Target == Angle
  66.     Target += 5
  67.     If Target > 359
  68.        Target := 0
  69.     waitcnt ((clkfreq/2) + cnt)
  70.     range := ping.Inches(ping_pin)
  71.  
  72.     term.str(string("Ping Range:", 32))
  73.     term.dec(range)
  74.     term.tx(13)
  75.  
  76.     term.str(string("Heading in Degrees:", 32))
  77.     term.dec(Angle)
  78.     term.tx(13)
  79.  
  80.     term.str(string("Heading Target:", 32))
  81.     term.dec(Target)
  82.     term.tx(13)
  83.     term.tx(13)
  84.  
  85. Pub GotoAngle
  86. ctra[30..26] := %00100
  87. ctra[8..0] := servoPin
  88.  
  89. frqa := 1
  90. dira[servoPin]~~
  91. t := cnt
  92.  
  93. repeat
  94.   If (Target - Angle) > 3
  95.      b := 1505
  96.      phsa := -(b*tInc)
  97.      t += tC
  98.      waitcnt(t)
  99.   elseif (Target - Angle) < -3
  100.      b := 1520
  101.      phsa := -(b*tInc)
  102.      t += tC
  103.      waitcnt(t)
  104.   else
  105.      b := 1510
  106.      phsa := -(b*tInc)
  107.      t += tC
  108.      waitcnt(t)
  109.  
  110. PUB Compass
  111.  
  112.   waitcnt(clkfreq/100_000 + cnt)      'Wait while compass has time to startup.
  113.  
  114.   setcont                             'sets measurements to go continuously
  115.  
  116.   repeat                              'Repeat indefinitely
  117.  
  118.      setpointer(OUTPUT_X_MSB)         'Start with Register OUT_X_MSB
  119.      getRaw                           'Gather raw data from compass
  120.      Angle := aziadjust
  121.      waitcnt(clkfreq/80 + cnt)
  122.  
  123. PUB aziadjust  : value
  124.  
  125. {{ Converts the Azimuth to Degrees from 0 - 360. }}
  126.  
  127. NW~
  128. NE~
  129. SE~
  130. SW~
  131.  
  132.   if  x =< 0
  133.         if azimuth =< 0
  134.            value := AZ_A
  135.            NW := 1
  136.         else
  137.             NW~
  138.  
  139.         if azimuth > 0
  140.            value := AZ_D
  141.            SW := 1
  142.         else
  143.             SW~
  144.  
  145.   if x > 0
  146.         if azimuth =< 0
  147.           value := AZ_B
  148.           NE := 1
  149.         else
  150.            NE~
  151.  
  152.  
  153.         if azimuth > 0
  154.           value := AZ_C
  155.           SE := 1
  156.         else
  157.             SE~
  158.  
  159.   value := 1 #> value <# 360
  160.  
  161.   value := (value + 270) // 360
  162.  
  163. PUB SetCont
  164.  
  165. {{ Sets the Compass to Continuous output mode.}}
  166.  
  167.   start
  168.   send(WRITE_DATA)
  169.   send(MODE)
  170.   send($00)
  171.   stop
  172.  
  173. PUB SetPointer(Register)
  174.  
  175. {{ Start pointer at user specified Register. }}
  176.  
  177. start
  178. send(WRITE_DATA)
  179. send(Register)
  180. stop
  181.  
  182. PUB GetRaw
  183.  
  184. {{ Get raw data from continous output.}}
  185.  
  186.   start
  187.   send(READ_DATA)
  188.   x := ((receive(true) << 8) | receive(true))            'RegisterA and RegisterB
  189.   z := ((receive(true) << 8) | receive(true))
  190.   y := ((receive(true) << 8) | receive(false))
  191.   stop
  192.   ~~x
  193.   ~~z
  194.   ~~y
  195.   x := x
  196.   z := z
  197.   y := y
  198.  
  199. PRI Azimuth
  200.  
  201.     'Azimuth = arcTan(y/x)
  202.  
  203.            result := math.arctan(y,x)
  204.  
  205. PRI AZ_A        'NE
  206.  
  207.                result := ||azimuth
  208.  
  209. PRI AZ_B        'SE
  210.  
  211.                result := azimuth  + 180
  212.  
  213. PRI AZ_C        'SW
  214.  
  215.                result := azimuth  + 180
  216.  
  217. PRI AZ_D        'NW
  218.  
  219.                result := -azimuth + 360
  220.  
  221. PRI send(value) ' I²C Send data - 4 Stack Longs
  222.  
  223.   value := ((!value) >< 8)
  224.  
  225.   repeat 8
  226.     dira[dataPin]  := value
  227.     dira[clockPin] := false
  228.     dira[clockPin] := true
  229.     value >>= 1
  230.  
  231.   dira[dataPin]  := false
  232.   dira[clockPin] := false
  233.   result         := !(ina[dataPin])
  234.   dira[clockPin] := true
  235.   dira[dataPin]  := true
  236.  
  237. PRI receive(aknowledge) ' I²C receive data - 4 Stack Longs
  238.  
  239.   dira[dataPin] := false
  240.  
  241.   repeat 8
  242.     result <<= 1
  243.     dira[clockPin] := false
  244.     result         |= ina[dataPin]
  245.     dira[clockPin] := true
  246.  
  247.   dira[dataPin]  := aknowledge
  248.   dira[clockPin] := false
  249.   dira[clockPin] := true
  250.   dira[dataPin]  := true
  251.  
  252. PRI start ' 3 Stack Longs
  253.  
  254.   outa[dataPin]  := false
  255.   outa[clockPin] := false
  256.   dira[dataPin]  := true
  257.   dira[clockPin] := true
  258.  
  259. PRI stop ' 3 Stack Longs
  260.  
  261.   dira[clockPin] := false
  262.   dira[dataPin]  := false
  263.  
  264. DAT
  265.  
  266. Azm     byte "Azimuth = ",0
  267. XRaw       byte "X = ",0
  268. YRaw       byte "Y = ",0
  269. ZRaw       byte "Z = ",0
create a new version of this paste RAW Paste Data