Changeset 3070


Ignore:
Timestamp:
Jul 18, 2012, 11:45:40 PM (2 years ago)
Author:
wmb
Message:

OLPC accelerometer demo - added "level" command to implement a clinometer precise to 0.1 degrees up to 45 degrees.

File:
1 edited

Legend:

Unmodified
Added
Removed
  • cpu/arm/olpc/roller.fth

    r2740 r3070  
    2020d# 8 value #fraction-bits 
    2121: fraction*  ( -- )  * #fraction-bits >>a  ; 
    22 : 1/2  ( -- n )  1 #fraction-bits 1- lshift  ; 
     22: 1/2   ( -- n )  1 #fraction-bits 1- lshift  ; 
    2323: >fraction  ( n -- fraction )  #fraction-bits lshift  ; 
    2424: a/b>fraction  ( num denom -- fraction )  swap #fraction-bits lshift  swap /  ; 
     
    8181d# 40 constant ball-diameter 
    8282 
    83 : maxx  ( -- n )  screen-wh drop  ball-diameter -  >fraction  ; 
    84 : maxy  ( -- n )  screen-wh nip   ball-diameter -  >fraction  ; 
     83: maxx-raw  ( -- n )  screen-wh drop  ball-diameter -  ; 
     84: maxy-raw  ( -- n )  screen-wh nip   ball-diameter -  ; 
     85 
     86: maxx  ( -- n )  maxx-raw  >fraction  ; 
     87: maxy  ( -- n )  maxy-raw  >fraction  ; 
    8588d#  400 >fraction constant maxz 
    8689 
    8790: 3swap  ( x1 y1 z1 x2 y2 z2 -- x2 y2 z2 x1 y1 z1 )  5 roll  5 roll  5 roll  ; 
    8891: 3over  ( x1 y1 z1 x2 y2 z2 -- x2 y2 z2 x1 y1 z1 )  5 pick  5 pick  5 pick  ; 
     92 
     93\ This is a simple approximation for the magnitude of a 3-vector 
     94\ - largest component plus 1/3 the sum of the two smaller components. 
     95\ It works surprisingly well, especially since we use it to estimate 
     96\ drag, which is a complicated phenomenon for which the formula is 
     97\ only an approximation anyway. 
     98 
     99: xyz-magnitude  ( x y z -- magnitude ) 
     100   rot abs rot abs rot abs 
     101   2dup >  if  swap  then      ( x min-y,z max-y,z ) 
     102   rot 2dup >  if  swap  then  ( a b max ) 
     103   -rot  + 3 /  +              ( magnitude ) 
     104; 
    89105 
    90106[ifdef] notdef 
     
    106122   \ Compute new laptop velocity 
    107123   vel-l damping xyz*  xyz+  to vel-l  ( ) 
    108 ; 
    109  
    110 \ This is a simple approximation for the magnitude of a 3-vector 
    111 \ - largest component plus 1/3 the sum of the two smaller components. 
    112 \ It works surprisingly well, especially since we use it to estimate 
    113 \ drag, which is a complicated phenomenon for which the formula is 
    114 \ only an approximation anyway. 
    115  
    116 : xyz-magnitude  ( x y z -- magnitude ) 
    117    rot abs rot abs rot abs 
    118    2dup >  if  swap  then      ( x min-y,z max-y,z ) 
    119    rot 2dup >  if  swap  then  ( a b max ) 
    120    -rot  + 3 /  +              ( magnitude ) 
    121124; 
    122125 
     
    250253   begin  ( update-laptop ) update-ball  d# 50 ms  key? until 
    251254   text-on 
     255; 
     256 
     257: filtered-acceleration  ( -- acc-x,y,z ) 
     258   0 0 0  
     259   d# 16 0 do  net-acceleration xyz+  loop 
     260   1 d# 16 a/b>fraction xyz* 
     261; 
     262 
     263d# 573 constant deg*10/rad 
     264d# 1460 value gravity 
     265: .decidegrees  ( degrees*10 -- ) 
     266   dup abs d# 450 >  if   ( degrees*10 ) 
     267      0<  if  ." <-45"  else  ." >45"  then 
     268   else 
     269      push-decimal 
     270      dup abs  <# u# [char] . hold u#s swap sign  u#> type 
     271      ."     " 
     272      pop-base 
     273   then 
     274; 
     275 
     2760 value xsq 
     2770 value gsq 
     278 
     279: arcsin-step  ( n -- n' )  deg*10/rad +  xsq gsq */  ; 
     280: arcsin*10  ( x -- degrees*10 ) 
     281   dup dup * to xsq       ( x ) 
     282   gravity dup * to gsq   ( x ) 
     283   \ Starting with this bias instead of 0 helps with roundoff error, 
     284   \ so the result is good to within 0.1 degree up to 45 degrees. 
     285   d# 7500                ( x n ) 
     286   arcsin-step d# 20 /    ( x n' )  \ x^5 / 5! term 
     287   arcsin-step d# 06 /    ( x n' )  \ x^3 / 3! term 
     288   deg*10/rad +           ( x n' )  \ x term 
     289   gravity */ 
     290; 
     291 
     292: x-center  ( -- x )  screen-wh drop 2/  ; 
     293: y-center  ( -- y )  screen-wh nip 2/  ; 
     294 
     295: put-ball  ( x y z -- )  3dup pos-b redraw  to pos-b  ; 
     296: put-centered  ( offset-x offset-y z-size -- ) 
     297   >r                   ( x y r: z ) 
     298   swap  x-center +  0 max  maxx-raw min >fraction  ( y x' r: z ) 
     299   swap  y-center +  0 max  maxy-raw min >fraction  ( x' y' r: z ) 
     300   r> put-ball 
     301; 
     302 
     303: show-center-mark  ( -- ) 
     304   0 set-fg  x-center ball-radius + y-center ball-radius +  d# 16  circleat 
     305; 
     306 
     3070 0 0 3value cal-xyz 
     308: update-angle  ( -- ) 
     309   filtered-acceleration   ( acc-x,y,z ) 
     310   cal-xyz xyz- drop       ( x' y' ) 
     311   0 0 at-xy 
     312   ." Degrees X " over arcsin*10 .decidegrees     ."   Y " dup arcsin*10 .decidegrees  ( x' y' ) 
     313   swap 2/  swap 2/ 
     314   d# 0  put-centered 
     315   show-center-mark 
     316   \ ." X " rot 5 .r  ."   Y " swap 5 .r  ."  Z "  5 .r 
     317; 
     318 
     319: ?zero  ( -- ) 
     320   rotate-button?  if 
     321      filtered-acceleration to cal-xyz 
     322      cal-xyz xyz-magnitude to gravity 
     323      0 1 at-xy ." Zeroing" 
     324      begin rotate-button? 0=  until 
     325      (cr ."        " 
     326   then 
     327; 
     328: clinometer  ( -- ) 
     329   init-accelerometer 
     330   init-ball 
     331   cursor-off 
     332   clear-drawing 
     333   d# 40 0  at-xy  ." Rotate button zeroes" 
     334   begin 
     335      ?zero 
     336     ( update-laptop ) update-angle  d# 50 ms  
     337   key? until 
     338   cursor-on 
     339; 
     340: level  ( -- ) 
     341   clinometer 
    252342; 
    253343 
Note: See TracChangeset for help on using the changeset viewer.