Code:
;Control NOWFOREVER spindle speed
O<SetSpindleSpeed> sub
;fill buffer with parameters to set spindle speed
#<speed_lo> = SHR[[#<_spindlespeed>*100/60],8]
#<speed_hi> = AND[[#<_spindlespeed>*100/60],255]
(array,4000,1,#<_vfd_addr>,16,9,1,0,1,2,#<speed_lo>,#<speed_hi>)
O<Crc16> call [4002] [#4000-1]
#<crc> = [#<_return>]
O<test> if [EXISTS[#<_rs485_debug>]]
(print,crc=#<crc,0>)
O<test> endif
;add the crc16 value to the end of the buffer
#[4001+#4000] = AND[#<crc>,255]
#[4002+#4000] = SHR[#<crc>,8]
#4000 = [#4000+2]
;print request buffer if debug is on
O<test> if [EXISTS[#<_rs485_debug>]]
#<size> = [#4000]
(print,Request Size=#<size,0>)
#<i> = 0
O<print1> repeat [#4000]
#<q> = [#[4001+#<i>]]
(print,#<q,0>)
#<i> = [#<i>+1]
O<print1> endrepeat
O<test> endif
;zero the return count and send the buffer to the i2c bus
#<count> = 0
O<loop1> do
#<err> = 0
#4100 = 0
M66 H38 Q4001 E#4000 R4100 D4 ;set the spindle speed
O<test> if [EXISTS[#<_rs485_debug>]]
#<size> = [#4100]
(print,Response Size=#<size,0>)
#<i> = 0
O<print1> repeat [#4100]
#<q> = [#[4101+#<i>]]
(print,#<q,0>)
#<i> = [#<i>+1]
O<print1> endrepeat
O<test> endif
#<count> = [#<count>+1]
O<error1> if [ACTIVE[] AND [#4100 NE 3]]
O<test1> if [#<count EQ 5]
(dlgname,Spindle Error, opt=1)
(dlg,Failed to set spindle speed, typ=label, x=20, w=280, color=0xffa500)
(dlg,Response size wrong, typ=label, x=20, w=280)
(dlgshow)
M2
O<test1> else
#<err> = 1
O<test1> endif
O<error1> endif
;check the crc16 of the response
O<Crc16> call [4101] [#4100-2]
#<crc> = [#<_return>]
O<error2> if [ACTIVE[] AND [[#4102 NE AND[#<crc>,255]] OR [#4103 NE SHR[#<crc>,8]]]]
O<test2> if [#<count EQ 5]
(dlgname,Spindle Error, opt=1)
(dlg,Failed to set spindle speed, typ=label, x=20, w=280, color=0xffa500)
(dlg,Response crc wrong, typ=label, x=20, w=280)
(dlgshow)
M2
O<test2> else
#<err> = 1
O<test2> endif
O<error2> endif
;check that the request didn't have a bad crc
O<error3> if [ACTIVE[] AND [#4101 NE 0]]
O<test3> if [#<count EQ 5]
(dlgname,Spindle Error, opt=1)
(dlg,Failed to set spindle speed, typ=label, x=20, w=280, color=0xffa500)
(dlg,Request crc wrong, typ=label, x=20, w=280)
(dlgshow)
M2
O<test3> else
#<err> = 1
O<test3> endif
O<error3> endif
O<loop1> while [#<err> EQ 1]
;pause to give time for the rs485 vfd comms to complete
G04 P0.02
;now reqest for the vfd response
#4000 = 2
#<w> = 0
O<wait> do
G04 P0.01
;zero the return count and send the buffer to the i2c bus
#4100 = 0
M66 H38 Q4000 E1 R4100 D10 ; request the response data
O<test> if [EXISTS[#<_rs485_debug>]]
#<size> = [#4100]
(print,Response Size=#<size,0>)
#<i> = 0
O<print2> repeat [#4100]
#<q> = [#[4101+#<i>]]
(print,#<q,0>)
#<i> = [#<i>+1]
O<print2> endrepeat
O<test> endif
;check the crc16 of the response
O<test> if [#4101 EQ 0]
O<Crc16> call [4102] [#4100-3]
O<test> else
O<Crc16> call [4101] [#4100-2]
O<test> endif
#<crc> = [#<_return>]
O<error2> if [ACTIVE[] AND [[#[4100+#4100-1] NE AND[#<crc>,255]] OR [#[4100+#4100] NE SHR[#<crc>,8]]]]
O<test4> if [#<w> EQ 5]
(dlgname,Spindle Error, opt=1)
(dlg,Failed to set spindle speed, typ=label, x=20, w=280, color=0xffa500)
(dlg,Response crc wrong, typ=label, x=20, w=280)
(dlgshow)
M2
O<test4> else
#4101 = 1
O<test4> endif
O<error2> endif
#<w> = [#<w>+1]
O<test> if [ACTIVE[] AND [#<w> EQ 5]]
(dlgname,Spindle Error, opt=1)
(dlg,Failed to set spindle speed, typ=label, x=20, w=280, color=0xffa500)
(dlg,Response timeout, typ=label, x=20, w=280)
(dlgshow)
M2
O<test> endif
O<wait> while [#4101 NE 0]
O<test> if [EXISTS[#<_rs485_debug>]]
(print,Wait loops=#<w,0>)
O<test> endif
O<SetSpindleSpeed> endsub