PROGRAM "SAScantest.vnm" 1

TO init
;  MAKE can CANBus(0,64,0)
  MAKE can CANBus(1,64,0) ; hardware i/f: 0=Polled J12 1-2, 1=IRQ J12 2-3
  						  ; Bit Rate : 500K = 32, 250K = 64, 125K = 128
  						  ; 0 = 11 Bit CAN, 1 = 29 bit CAN
  MAKE CANint Digital(24)
  MAKE backlight PulseWidthOut (19,100,1,1,0)
  MAKE LCD GraphicsLCD(28)
  LCD.value(1,$81) LCD.value(1) := 24
  LCD.value(1) := $A6
  backlight.On
  backlight.Width := 50
  wALL := LCD.Window (0, 0, 128, 64, 0)    ;All
  wTop := LCD.Window (0, 48, 127, 63, 0)
  wBot := LCD.Window(0, 0, 127, 47, 0)
  MAKE YLED Digital (145)
  MAKE GLED Digital (144)
;  CAN.Value ( $0C ) := $3C ; BFPCTRL

;Data portion of the frame.
	MAKE data ARRAY (8,8, 00, 00, 00, 04, 00, 00, 00, 00 )
;  serial.handshake := 0
END

TO main

  START EVERY 200 LCD.Update
  LED.Off
  PRINT TO wBot, FONT 3
  can.On      ; Go to Normal mode of MCP2515.

  can_Rx    ;Do the CAN comms
END

;Simple polled CAN dialogue.
TO can_Rx
  LOCAL MsgCtr, MyTimer

  MsgCtr := 0
  MyTimer := NEW Timer(500)
  MyTimer.Go
  OldDir := 0
  Accel := 0
  Pos := 0
  PRINT TO wBot, CLS, CR, CR, CENTRE, "STOPPED", CR
  FOREVER
  [
; 	AWAIT canint.asserted OR MyTimer.Done

 ;   AWAIT canint.asserted ;Wait for interrupt.
;    WHILE canint.asserted ;Polled Mode Onlyv - Process interrupts while they last.
;    [
;    	can.Update
;    ]

	AWAIT can.Queue OR MyTimer.Done
    IF can.Queue          ;Any incoming frames to deal with?
    [
      LED.On
  	  MsgCtr :=  MsgCtr + 1
      IF can.Queue < 28   ;Handle Buffer Overrun
      [
	  	YLED.Off
;     PRINT TO wAll, can.Queue:2,":"
;	      PRINT TO wBot, CR, ~can.Look(0):3,":"
;	      REPEAT can.look(1)
;	      	PRINT TO wBot, ~can.Look(4+INDEX0):-2
;	      PRINT TO wBot, CR

;SLOW	      REPEAT can.look(1)
;SLOW	      	PRINT ~can.Look(4+INDEX0):-2
;SLOW	      PRINT CR

;	      PRINT ~can.Look(0):3,":", ~????can.Look(3):-8, ~????(can.Look(3)+4):-8, CR
	      Accel := ($1000 - (can.look(6)*256 + can.look(7)))/2
	      Pos := ($1000 - (can.look(4)*256 + can.look(5)))/2
		  IF Accel > 16 [
		  	NewDir := 1
		  ]Else[
			  IF Accel < -16
			  	NewDir := -1
			  else
			    NewDir := 0
		  ]
		  IF can.look(6) = $3F [
		  	NewDir := -2
		  	Accel := 0
		  ]
		  IF can.look(4) = $3F Pos := 0
		  If NewDir <> OldDir [
			OldDir := NewDir
			SELECT CASE NewDir
				CASE 1 PRINT TO wBot, CLS, FONT 3, CR, CR, CENTRE, "ANTICLOCKWISE", CR
				CASE -1 PRINT TO wBot, CLS, FONT 3, CR, CR, CENTRE, "CLOCKWISE", CR
			  	CASE -2 PRINT TO wBot, CLS, FONT 3, CR, CR, CENTRE, "INVALID", CR
				CASE ELSE PRINT TO wBot, CLS, FONT 3, CR, CR, CENTRE, "STOPPED", CR
		  ]
	  ]Else
	  	YLED.On
      can.Get             ;Discard current frame so we can view the next one.
      LED.Off
    ]
  	  If MyTimer.Done [
		  MyTimer.Go
		  wTop.Lock
      	  PRINT TO wTop, CLS, FONT 3, " Frames Per Sec: ", MsgCtr*2:-3," ", CR
      	  PRINT TO wTop, CENTRE, "Acc:",Accel:4:0," Pos:", Pos:4:0
		  wTop.UnLock
		  If MsgCtr = 0 [
			  OldDir := 0
			  Accel := 0
			  Pos := 0
			  PRINT TO wBot, CLS, FONT 3, CR, CR, CENTRE, "NO DATA", CR
		  ]Else
			  can.Put($46, data.Address, 8 )
		  MsgCtr := 0
	  ]
  ]

END

PROGRAM END