Changeset 3070


Ignore:
Timestamp:
Jul 18, 2012, 11:45:40 PM (3 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.