GOSUB userfunctions WAKELOCK 1 !--------------------------------------------------- !select sensors -------------------------------- ARRAY.LOAD sens $[],"accel", "gyro", "light" ARRAY.LOAD sens [],0 ,0 , 0 SENSORS.LIST tmp $[] ARRAY.LENGTH sel, tmp $[] FOR j = 1 TO 3 flag_break = 0 FOR i = 1 TO sel IF is_in ( sens $[j], LOWER$(tmp$[i]) ) THEN sens [j] = val (right $(tmp $[ i ], 1)) flag_break = 1 ENDIF IF flag_break THEN f_n.break NEXT NEXT SENSORS.OPEN sens[1] :1, sens[2] :1, sens[3] :1 !--------------------------------------------------- !params -------------------------------------- flagori = 0 refw = 1280 refh = 780 scax = 3 % pixel per loop/step dtDesH = 50 % desired/nominal loopTime[ms] dtDesL = 25 % desired loopTime when in background nx = 1080 dx = 1 fszx = nx fszy = 640 offx = (refw-nx)/2 offy = refh/2 ndivx = 18 ndivy = 16 nGraph = 9 DIM graph [nGraph] % list -ptr DIM _graph [ngraph] % polyline -ptr DIM scay [ngraph] % y-scale(s) DIM val [ngraph] % tmp values DIM valF [ngraph] % tmp values filtered DIM gra$ [nGraph] % legend text DIM hide [nGraph] % show/hide graphs DIM kF [nGraph] % filter const scay[1] = - 16 scay[2] = - scay[1] scay[3] = - scay[1] scay[4] = - 24 scay[5] = scay[4] scay[6] = scay[4] scay[7] = -3.2 scay[8] = -fszy/ndivy / 10 scay[9] = -fszy/ndivy / 10 gra$[1] = "accel_x "+STR$(ROUND(fszy/ndivy/-scay[1],1))+" [m/s^2] /div" gra$[2] = " _y " gra$[3] = " _z " gra$[4] = "omega_x "+STR$(ROUND(fszy/ndivy/-scay[4],1))+" [rad/s] /div" gra$[5] = " _y " gra$[6] = " _z " gra$[7] = "light "+STR$(ROUND(fszy/ndivy/-scay[7],1))+" [lx] /div" gra$[8] = "updateTime "+STR$(ROUND(fszy/ndivy/-scay[8],1))+" [ms] /div" gra$[9] = "renderTime "+STR$(ROUND(fszy/ndivy/-scay[9],1))+" [ms] /div" hide [1] = 0 hide [2] = 0 hide [3] = 0 hide [4] = 0 hide [5] = 0 hide [6] = 0 hide [7] = 0 hide [8] = 0 hide [9] = 0 ! kF = 0 means no filtering !! kF [1] = 0 kF [2] = 0 kF [3] = 0.2 kF [4] = 0 kF [5] = 0 kF [6] = 0 kF [7] = 0.3 kF [8] = 0 kF [9] = 0 IF !sens[1] THEN FOR i=1 TO 3 gra$[i] = ">no accel sensor<" hide [i] = 1 NEXT ENDIF IF !sens[2] THEN FOR i=4 TO 6 gra$[i] = ">no gyro sensor<" hide [i] = 1 NEXT ENDIF IF !sens[3] THEN gra$[7] = ">no liggt sensor<" hide [7] = 1 ENDIF !--------------------------------------------------- !create LIST(s) -------------------------------- LIST.CREATE n, graph[1] LIST.ADD graph[1] ,0, 0 FOR j = 1 TO nx/scax+scax LIST.ADD graph[1] , j*scax, 0 NEXT LIST.ADD graph[1] , fszx ,0, fszx , fszy/2 ,0, fszy/2 FOR i = 2 TO nGraph LIST.CREATE n, graph[i] LIST.ADD.LIST graph[i] , graph[1] NEXT !--------------------------------------------------- !go gr... ------------------------------------- GR.OPEN 255, 0, 0, 30, 1, flagori GR.SCREEN w, h GR.SCALE w/refw, h/800 GR.SET.ANTIALIAS O !--------------------------------------------------- !grid --------------------------------------- GR.SET.STROKE 2 grid = polygrid ( fszx ,fszy , ndivx, ndivy ) GR.COLOR 255, 40 , 40 ,70, 0 GR.POLY nn, grid, offx, offy- fszy/2 !--------------------------------------------------- ! graphs --------------------------------------- ARRAY.LOAD c [], ~ 255 , 255 , 000 , 000 , 0 ,~ 255 , 000 , 255 , 000 , 0 ,~ 255 , 000 , 000 , 255 , 0 ,~ 255 , 200 , 100 , 000 , 0 ,~ 255 , 200 , 000 , 200 , 0 ,~ 255 , 100 , 220 , 220 , 0 ,~ 255 , 120 , 150 , 220 , 0 ,~ 255 , 220 , 220 , 100 , 0 ,~ 255 , 225 , 225 , 225 , 0 GR.SET.STROKE 2 FOR i = 0 TO nGraph -1 GR.COLOR c[i*5+1],c[i*5+2],c[i*5+3],c[i*5+4],c[i*5+5] GR.POLY _graph[i+1] , graph[i+1] , offx, offy IF hide [i+1] THEN gr.hide _graph[i+1] NEXT !--------------------------------------------------- ! mask / curtain / cover rect ------------------ mask = mask (offx, offy-fszy/2, offx+fszx, offy+fszy/2) GR.COLOR 255 , 000 , 000 , 000 , 1 GR.POLY nn, mask, 0, 0 GR.SET.STROKE 3 GR.COLOR 140, 150 , 250 , 150 , 1 GR.LINE curtain , offx, offy-fszy/2, offx, offy+fszy/2 GR.SET.STROKE 3 GR.COLOR 255, 160 , 160 , 160 , 0 GR.RECT nn , offx, offy-fszy/2, offx+fszx, offy+fszy/2 !--------------------------------------------------- !text ----------------------------- GR.TEXT.SIZE 18 GR.TEXT.TYPEFACE 2 GR.COLOR 255, 255 , 255, 255, 1 txt3$ = "x-axis, nominal: "+ str $(ROUND( (fszx/ndivx)/ (scax/(dtdesh/1000) ) , 2)) +"[s] /div " GR.TEXT.DRAW nn, offx, offy+fszy/2 +20, txt3$ GR.TEXT.ALIGN 3 FOR i= 1 TO ndivy-1 GR.TEXT.DRAW nn, offx-10, offy+fszy/2 -i*(fszy/ndivy)+3, int $(i-ndivy/2) NEXT GR.TEXT.ALIGN 2 FOR i= 1 TO ndivx-1 GR.TEXT.DRAW nn, offx+ i*(fszx/ndivx)+5 , offy-fszy/2 -10 , int $(i) NEXT GR.TEXT.ALIGN 1 !--------------------------------------------------- ! legend ----------------------------- GR.TEXT.SIZE 16 GR.BITMAP.CREATE bmp1, fszx , fszy/ndivy*3 off1 = 20 off2 = 330 fac = 30 o2 = -off2 GR.BITMAP.DRAWINTO.START bmp1 GR.COLOR 20, 255 , 255 , 255 , 1 GR.RECT nn, 0,0, fszx , fszy/ndivy*3 FOR i = 0 TO ngraph-1 IF ! MOD(i, 3) THEN o2+=off2 bb = MOD(i, 3) py = fac*bb+5+off1 GR.COLOR 255, 255 , 255, 255, 1 GR.TEXT.DRAW nn, 40 + o2 , py , gra$[i+1] GR.COLOR c[i*5+1],c[i*5+2],c[i*5+3],c[i*5+4],c[i*5+5] GR.LINE nn, 10 + o2 , py-5 , 30 + o2 , py -5 NEXT GR.BITMAP.DRAWINTO.END GR.BITMAP.DRAW nn, bmp1, offx, offy +fszy/2- fszy/ndivy*3 !--------------------------------------------------- ! main loop ---------------------------------- PRINT "number grObj: "; getGrObjCount() c1 = nx/scax c2 = offx - scax _1 = 1 _2 = 2 _3 = 3 _4 = 4 _5 = 5 _6 = 6 _7 = 7 _8 = 8 _9 = 9 flagRec = 0 IF flagRec THEN TEXT.OPEN w,fid,"xxx.txt" ARRAY.LOAD vib [], 0, 15 dtdes = dtdesH tic = CLOCK() ticGlobal = tic DO SENSORS.READ sens[1] , val[_1] , val[_2] , val[_3] SENSORS.READ sens[2] , val[_4] , val[_5] , val[_6] SENSORS.READ sens[3] , val[_7] , dummy, dummy LET val [_8] = toc1 LET val [_9] = toc2 LET ptr = ptr+_1 IF ptr> c1 THEN ptr=1 FOR i = 1 TO nGraph IF kF[i] THEN GOSUB filter LIST.REPLACE graph[i] , ptr*_2, val[i] * scay[i] NEXT GR.MODIFY curtain, "x1", c2+ptr*scax , "x2", c2+ptr*scax IF flagRec THEN gosub writeData IF !BACKGROUND() THEN dtdes=dtdesH ELSE dtdes=dtdesL !if BACKGROUND() THEN if val [9]> dtdesL*1.4 then vibrate vib [],-1 LET toc1 = CLOCK() - tic IF !BACKGROUND() THEN GR.RENDER LET toc2 = CLOCK() - tic - toc1 PAUSE max (dtDes- (CLOCK()-tic) , _1) LET tic = CLOCK() UNTIL 0 !----------------------------------------------- !----------------------------------------------------- !----------------------------------------------------- !gosubs -------------------------------------- filter: LET valF[i] = valF[i] + (val[i]-valF[i])* kF[i] LET val [i] = valF[i] RETURN writeData: TEXT.WRITELN fid, tic;"\n";val[_1] ;"\n";val[_2] ;"\n"; val[_3] ;"\n"; val[_4] ;"\n"; val[_5] ;"\n"; val[_6] ;"\n"; val[_7] ;"\n"; val[_8] ;"\n"; val[_9] RETURN !------------------------------------------------- userfunctions: FN.DEF mask (le, to, ri, bo) GR.SCREEN wi, hi wi=wi*2 hi=hi*2 LIST.CREATE n, mask LIST.ADD mask, le, to, ri, to, ri, bo, le, bo, le, to LIST.ADD mask, 0 , 0 , 0 , hi, wi, hi, wi ,0, 0 , 0 LIST.ADD mask, le, to FN.RTN mask FN.END FN.DEF polygrid (lx,ly, ndivx, ndivy ) dx = lx / ndivx dy = ly / ndivy LIST.CREATE n, polyGrid FOR i= 1 TO ndivx-1 LIST.ADD polyGrid, i*dx, MOD(i, 2)*ly ,i*dx, MOD(i+1, 2)*ly NEXT LIST.ADD polyGrid, i*dx, 0 ,i*dx, dy FOR i= 1 TO ndivy-1 LIST.ADD polyGrid, MOD(i, 2) *lx, i*dy, MOD(i+1, 2) *lx,i*dy NEXT LIST.ADD polyGrid, 0 , i*dy ,dx , i*dy FN.RTN polyGrid FN.END FN.DEF getGrObjCount() GR.CIRCLE grObjCount,-5,-5, 0 FN.RTN grObjCount FN.END RETURN !-------------------------------------------------