Changeset 3070
 Timestamp:
 Jul 18, 2012, 11:45:40 PM (3 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

cpu/arm/olpc/roller.fth
r2740 r3070 20 20 d# 8 value #fractionbits 21 21 : fraction* (  ) * #fractionbits >>a ; 22 : 1/2 (  n ) 1 #fractionbits 1 lshift ;22 : 1/2 (  n ) 1 #fractionbits 1 lshift ; 23 23 : >fraction ( n  fraction ) #fractionbits lshift ; 24 24 : a/b>fraction ( num denom  fraction ) swap #fractionbits lshift swap / ; … … 81 81 d# 40 constant balldiameter 82 82 83 : maxx (  n ) screenwh drop balldiameter  >fraction ; 84 : maxy (  n ) screenwh nip balldiameter  >fraction ; 83 : maxxraw (  n ) screenwh drop balldiameter  ; 84 : maxyraw (  n ) screenwh nip balldiameter  ; 85 86 : maxx (  n ) maxxraw >fraction ; 87 : maxy (  n ) maxyraw >fraction ; 85 88 d# 400 >fraction constant maxz 86 89 87 90 : 3swap ( x1 y1 z1 x2 y2 z2  x2 y2 z2 x1 y1 z1 ) 5 roll 5 roll 5 roll ; 88 91 : 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 3vector 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 : xyzmagnitude ( x y z  magnitude ) 100 rot abs rot abs rot abs 101 2dup > if swap then ( x miny,z maxy,z ) 102 rot 2dup > if swap then ( a b max ) 103 rot + 3 / + ( magnitude ) 104 ; 89 105 90 106 [ifdef] notdef … … 106 122 \ Compute new laptop velocity 107 123 vell damping xyz* xyz+ to vell ( ) 108 ;109 110 \ This is a simple approximation for the magnitude of a 3vector111 \  largest component plus 1/3 the sum of the two smaller components.112 \ It works surprisingly well, especially since we use it to estimate113 \ drag, which is a complicated phenomenon for which the formula is114 \ only an approximation anyway.115 116 : xyzmagnitude ( x y z  magnitude )117 rot abs rot abs rot abs118 2dup > if swap then ( x miny,z maxy,z )119 rot 2dup > if swap then ( a b max )120 rot + 3 / + ( magnitude )121 124 ; 122 125 … … 250 253 begin ( updatelaptop ) updateball d# 50 ms key? until 251 254 texton 255 ; 256 257 : filteredacceleration (  accx,y,z ) 258 0 0 0 259 d# 16 0 do netacceleration xyz+ loop 260 1 d# 16 a/b>fraction xyz* 261 ; 262 263 d# 573 constant deg*10/rad 264 d# 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 pushdecimal 270 dup abs <# u# [char] . hold u#s swap sign u#> type 271 ." " 272 popbase 273 then 274 ; 275 276 0 value xsq 277 0 value gsq 278 279 : arcsinstep ( 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 arcsinstep d# 20 / ( x n' ) \ x^5 / 5! term 287 arcsinstep d# 06 / ( x n' ) \ x^3 / 3! term 288 deg*10/rad + ( x n' ) \ x term 289 gravity */ 290 ; 291 292 : xcenter (  x ) screenwh drop 2/ ; 293 : ycenter (  y ) screenwh nip 2/ ; 294 295 : putball ( x y z  ) 3dup posb redraw to posb ; 296 : putcentered ( offsetx offsety zsize  ) 297 >r ( x y r: z ) 298 swap xcenter + 0 max maxxraw min >fraction ( y x' r: z ) 299 swap ycenter + 0 max maxyraw min >fraction ( x' y' r: z ) 300 r> putball 301 ; 302 303 : showcentermark (  ) 304 0 setfg xcenter ballradius + ycenter ballradius + d# 16 circleat 305 ; 306 307 0 0 0 3value calxyz 308 : updateangle (  ) 309 filteredacceleration ( accx,y,z ) 310 calxyz xyz drop ( x' y' ) 311 0 0 atxy 312 ." Degrees X " over arcsin*10 .decidegrees ." Y " dup arcsin*10 .decidegrees ( x' y' ) 313 swap 2/ swap 2/ 314 d# 0 putcentered 315 showcentermark 316 \ ." X " rot 5 .r ." Y " swap 5 .r ." Z " 5 .r 317 ; 318 319 : ?zero (  ) 320 rotatebutton? if 321 filteredacceleration to calxyz 322 calxyz xyzmagnitude to gravity 323 0 1 atxy ." Zeroing" 324 begin rotatebutton? 0= until 325 (cr ." " 326 then 327 ; 328 : clinometer (  ) 329 initaccelerometer 330 initball 331 cursoroff 332 cleardrawing 333 d# 40 0 atxy ." Rotate button zeroes" 334 begin 335 ?zero 336 ( updatelaptop ) updateangle d# 50 ms 337 key? until 338 cursoron 339 ; 340 : level (  ) 341 clinometer 252 342 ; 253 343
Note: See TracChangeset
for help on using the changeset viewer.