REM Send Accel values using an A6 message and 
REM receive the message back from AB

REM Scalling of Accel values Jeff T way to
REM see what the scaled numbers look like compared
REM to floating point ones

Dim xlbl as Label, ylbl as Label, zlbl as Label
Dim xdisplay as Label
Dim ydisplay as Label
Dim zdisplay as Label

! Create Labels to display scaled values
DIM xsc as LABEL
DIM ysc as LABEL
DIM zsc as LABEL

! Create Labels to display scaled values rx back
DIM xscrx as LABEL
DIM yscrx as LABEL
DIM zscrx as LABEL


DIM Message~(16)
DIM RxMessage~(16)  
DIM lasttime AS  DOUBLE
DIM tPause AS DOUBLE

DIM msgcount AS INTEGER
DIM chkerr AS INTEGER
DIM lblmsgs AS LABEL
DIM lblerrs AS LABEL

xcenter = Graphics.Width/2

lblmsgs = Graphics.newLabel(xcenter,300,400)
lblmsgs.setFont("Monospaced",24,1)
lblmsgs.setText("*")
lblerrs = Graphics.newLabel(xcenter,340,400)
lblerrs.setFont("Monospaced",24,1)
lblerrs.setText("*")




DIM quit AS Button
quit = Graphics.newButton(xcenter,400)
quit.setTitle("Quit")

REM Direct connection ip address
Comm.openTCPIP(1, "192.168.1.3", $2616)

xlbl = MakeLabel(xcenter-100,20,"X:")
ylbl = MakeLabel(xcenter-100,60,"Y:")
zlbl = MakeLabel(xcenter-100,100,"Z:")


xdisplay = Graphics.newLabel(xcenter,20,200)
xdisplay.setFont("Monospaced",24,1)
xdisplay.setText("*")
ydisplay = Graphics.newLabel(xcenter,60,200)
ydisplay.setFont("Monospaced",24,1)
ydisplay.setText("*")
zdisplay = Graphics.newLabel(xcenter,100,200)
zdisplay.setFont("Monospaced",24,1)
zdisplay.setText("*")


REM ADD SCALED DISPLAY LABELS
xsc = Graphics.newLabel(xcenter+220,20,100)
xsc.setFont("Monospaced",24,1)
xsc.setText("*")
xscrx = Graphics.newLabel(xcenter+340,20,100)
xscrx.setFont("Monospaced",24,1)
xscrx.setText("*")
ysc = Graphics.newLabel(xcenter+220,60,100)
ysc.setFont("Monospaced",24,1)
ysc.setText("*")
yscrx = Graphics.newLabel(xcenter+340,60,100)
yscrx.setFont("Monospaced",24,1)
yscrx.setText("*")
zsc = Graphics.newLabel(xcenter+220,100,100)
zsc.setFont("Monospaced",24,1)
zsc.setText("*")
zscrx = Graphics.newLabel(xcenter+340,100,100)
zscrx.setFont("Monospaced",24,1)
zscrx.setText("*")

REM ****** SET RATES ***********
sensors.setAccelRate(0.1)
tpause = 0.10

lasttime = 0.0

chkerr = 0
msgcount = 0

! Lock the screen in the current orientation.
orientation = 1 << (System.orientation - 1)
!System.setAllowedOrientations(orientation)
!Only allow landscape home on right
System.setAllowedOrientations(4)


!Pass 1 to go full screen and in this
!mode setAllowedOrientations works

Graphics.setFont("Monospaced",18,1)

REM USE FULL SCREEN
System.showGraphics(1)
REM SHOW GRAPHICS WITH DEBUGGER TOOLBAR
!System.showGraphics

SUB TxMessage(numbytes AS BYTE)
  FOR i% = 1 TO numbytes 
   PUT #1,, Message~(i%)
  NEXT
END SUB


SUB SendMsgA6
!Send known bytes 
  Message~(1) = $A6
  Message~(2) = 7
!  Message~(3) = $01
!  Message~(4) = $02
!  Message~(5) = $04
  Message~(6) = $08
  Message~(7) = $10
  Message~(8) = $20
  FOR idx = 2 To 8
   checksum~ = checksum~ + Message~(idx)
  NEXT
  checksum~ = BITNOT(checksum~) +1
  Message~(9) = checksum~
  TxMessage(9)
  
END SUB 



SUB nullEvent(time AS Double)
!Measure Accel evey 0.1 sec
! Make a byte to hold scaled value
sc~ = 0
tmp$ = ""
IF lasttime = 0 THEN
 lasttime = time
ELSE
 IF time - lasttime > tpause THEN
   ac = sensors.accel
   
   PRINT $  tmp$ USING "+#.#####"; ac(1);

   xdisplay.setText(tmp$)
   sc~ = (ac(1) * 128) + 127
   PRINT $ tmp$ USING "####"; sc~
   xsc.setText(tmp$)
   Message~(3) = sc~
   PRINT $  tmp$ USING "+#.#####"; ac(2);

   ydisplay.setText(tmp$)
   sc~ = (ac(2) * 128) + 127
   PRINT $ tmp$ USING "####"; sc~
   ysc.setText(tmp$)
   Message~(4) = sc~  
   PRINT $  tmp$ USING "+#.#####"; ac(3);

   zdisplay.setText(tmp$)
   sc~ = (ac(3) * 128) + 127
   Message~(5) = sc~
   PRINT $ tmp$ USING "####"; sc~
   zsc.setText(tmp$)
      
   SendMsgA6
   lasttime = time
 END IF
END IF
 
 !! Receive the A6 message back from AB
   rxstate~ = 0
   idx~ = 1
    
   WHILE NOT EOF(1)
    GET #1,,rxch~
    SELECT CASE rxstate~
      CASE 0
        IF rxch~ = $A6 THEN
          RxMessage~(idx~) = rxch~
          idx~ = idx~ +1
          rxchksum~ = 0
          rxstate~ = 1
        END IF
      CASE 1
        RxMessage~(idx~) = rxch~
        idx~ = idx~ + 1
        bytesneeded~ = rxch~ -1
        bytecount~ = 0
        rxchksum~ = rxch~
        rxstate~ = 2
      CASE 2
        RxMessage~(idx~) = rxch~
        idx~ = idx~ + 1
        rxchksum~ = rxchksum~ + rxch~
        bytecount~ = bytecount~ + 1
        IF bytecount~ = bytesneeded~ THEN
          rxstate~ = 3
        END IF
      CASE 3
        RxMessage~(idx~) = rxch~
        chktest~ = rxchksum~ + rxch~
        chktest~ = chktest~ BITAND $FF
        IF chktest~ = 0 THEN
          msgcount = msgcount + 1
          print $ M$ "MsgCount = "; msgcount
          lblmsgs.setText(M$)
        ELSE
          chkerr = chkerr + 1
          print $ M$ "ERROR COUNT = "; chkerr
          lblerrs.setText(M$)
        END IF
        rxstate~ = 0
        idx~ = 1       

      END SELECT
      
  WEND 
  
REM Display Accel values received back
   PRINT $ tmp$ USING "####"; RxMessage~(3)
   xscrx.setText(tmp$)
   PRINT $ tmp$ USING "####"; RxMessage~(4)
   yscrx.setText(tmp$)
   PRINT $ tmp$ USING "####"; RxMessage~(5)
   zscrx.setText(tmp$)   
END SUB



FUNCTION MakeLabel(x,y,title as string) as Label
DIM lbl AS Label
lbl = Graphics.newLabel(x,y)
lbl.setText(title)
lbl.setAlignment(3)
lbl.setFont("Monospaced",24,1)
END FUNCTION

SUB touchUpInside(ctrl AS Button, when AS Double)
if ctrl = quit then
  STOP
end if
end sub