'-----------------------------------
'Dynamixel AX Protocol V1.1 (DiosPro Only)
' Supports the following
'  AX-12
'  AX-S1
'----------------------------------------------
'V1.0 New Release
'V1.1 Added Support for Slower Return Delay
'     AXsetdelay command
'V1.2 Added Support for syncwrite
'     AXclearsync
'     AXaddsync
'     AXsendsync
'----------------------------------------------
'Connction Instructions
'     Conncet ports 8 and 9 together then connect them to the data lead
'     on the device.   The baud rate on the device should be the default
'     1,000,000 bps.  You must connect GND to the devices as well.  If you
'     are using the network to power the dios you will need to take the vdd
'     pin from the network and run it through a 5v regulator.  If you are
'     using the Dios Workboard to power the devices use VIN from the board.
'
'     Call AXinit() first to set things up then use the register read and
'     write commands to set the registers.
'
'-----------------
'Low Level Commands
'  AXinit
'  AXreadbyte
'  AXreadword
'  AXwritebyte
'  AXwriteword
'  AXsetbyte
'  AXsetword
'  AXaction
'  AXping
'  AXreset
'
'
'  Each Call will set the AXstatus variable to the following:
'   0 No Error (OK)
'   1 Input Voltage Error
'   2 Angle Limit Error
'   4 Overheat Error
'   8 Range Error
'  16 CheckSum Error
'  32 Overload Error
'  64 Command Error
' 128 TimeOut Error

'-----------------------------------------------------------
'  Setup for Dynamixel Protocol
'-----------------------------------------------------------
func AXinit()

   hsersetup baud,9,txon,start,clear,txpad,0
   pause 10

'AX12 Constants
   gconst AXModel_Number 0            'Word
   gconst AXVersion 2
   gconst AXID 3
   gconst AXBaud 4
   gconst AXDelay_Time 5
   gconst AXCW_Angle_Limit 6          'Word
   gconst AXCCW_Angle_Limit 8         'Word
   gconst AXTemp_Limit 11
   gconst AXLow_Volt_Limit 12
   gconst AXHigh_Volt_Limit 13
   gconst AXMax_Torque 14             'Word
   gconst AXReturn_Level 16
   gconst AXAlarm_LED 17
   gconst AXAlarm_Shutdown 18
   gconst AXDown_Cal 20 'Word
   gconst AXUp_Cal 22                 'Word
   gconst AXTorque_Enable 24
   gconst AXLED 25
   gconst AXCW_Compliance_Margin 26
   gconst AXCCW_Compliance_Margin 27
   gconst AXCW_Compliance_Slope 28
   gconst AXCCW_Compliance_Slope 29
   gconst AXGoal_Pos 30               'Word
   gconst AXMoving_Speed 32           'Word
   gconst AXTorque_Limit 34           'Word
   gconst AXPresent_Pos 36            'Word
   gconst AXPresent_Speed 38          'Word
   gconst AXPresent_Load 40           'Word
   gconst AXPresent_Volt 42
   gconst AXPresent_Temp 43
   gconst AXRegister_Instruction 44
   gconst AXMoving 46
   gconst AXMLock 46
   gconst AXPunch 48                  'Word

'AXS1 Constants
   gconst AXObstacleCompare 20
   gconst AXLightCompare 21
   gconst AXLeftIR 26
   gconst AXCenterIR 27
   gconst AXRightIR 28
   gconst AXLeftLum 29
   gconst AXCenterLum 30
   gconst AXRightLum 31
   gconst AXObsticleFlag 32
   gconst AXLumFlag 33
   gconst AXSoundData 35
   gconst AXSoundDataMax 36
   gconst AXSoundCount 37
   gconst AXSoundTime 38	'Word
   gconst AXBuzzerIndex 40
   gconst AXBuzzerTime 41
   gconst AXIRArrived 46
   gconst AXIRRX0 48
   gconst AXIRRX1 49
   gconst AXIRTX0 50
   gconst AXIRTX1 51
   gconst AXObsticleDetected 52
   gconst AXLightDetected 53


  global AXstatus as integer
  AXstatus = 0
endfunc





'-------------------------------------
' reset a device
'  Pass
'   Device
' 
'  Return
'   status
'-------------------------------------
func AXreset(unit as integer)
dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  dim v1 as integer

  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

  csum = unit + 8  ^ 255
  hserout 255,255,unit,6,csum

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 25 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,v1,csum
  'print runit," ",rlen," ",rerror," ",v1
 
  AXstatus = rerror
  exit AXstatus

timeout:
  AXstatus = 128
  exit 0
endfunc



'-------------------------------------
' Ping a device
'  Pass
'   Device
' 
'  Return
'   status
'-------------------------------------
func AXping(unit as integer)
dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  dim v1 as integer

  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

  csum = unit + 3  ^ 255
  hserout 255,255,unit,1,csum

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 25 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,v1,csum
  'print runit," ",rlen," ",rerror," ",v1
 
  AXstatus = rerror
  exit AXstatus

timeout:
  AXstatus = 128
  exit 0
endfunc


'-------------------------------------
' Read a byte from a device register
'  Pass
'   Device
'   Address
' 
'  Return
'   Byte
'-------------------------------------
func AXreadbyte(unit as integer,addr as integer)
  dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  dim v1 as integer

  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

  csum = unit + 6 + addr + 1  ^ 255
  hserout 255,255,unit,4,2,addr,1,csum

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 25 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,v1,csum
  'print runit," ",rlen," ",rerror," ",v1
 
  AXstatus = rerror
  exit v1

timeout:
  AXstatus = 128
  exit 0


endfunc


'-------------------------------------
' Read a word from a device register
'  Pass
'   Device
'   Address
' 
'  Return
'   word
'-------------------------------------
func AXreadword(unit as integer,addr as integer)
  dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  dim v1 as integer
  dim v2 as integer
  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

  csum = unit + 6 + addr + 2  ^ 255
  hserout 255,255,unit,4,2,addr,2,csum

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 55 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,v1,v2,csum
  'print runit," ",rlen," ",rerror," ",v2*256+v1
 
  AXstatus = rerror
  exit v2*256+v1

timeout:
  AXstatus = 128
  exit 0

endfunc


'-------------------------------------
' Write a word to a device register
'  Pass
'   Device
'   Address
'   Value
'
'  Returns status and sets AXstatus
'  
'-------------------------------------
func AXwriteword(unit as integer,addr as integer,value as integer)
  dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

  csum = unit + 8 + addr + value.byte(0)+value.byte(1) ^ 255
  hserout 255,255,unit,5,3,addr,value.byte(0),value.byte(1),csum

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 25 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,csum
  'print runit," ",rlen," ",rerror
  AXstatus = rerror
  exit rerror

timeout:
  AXstatus = 128
  exit 128

endfunc



'------------------------------------------------
'Set the return delay of a device.
'  Pass
'   Device
'   Delay  0-255
-------------------------------------------------
func AXsetdelay(unit as integer,delay as integer)
    dim stat as integer
    stat = AXwritebyte(unit,5,delay)
    exit stat
endfunc




'-------------------------------------
' Write a byte to a device register
'  Pass
'   Device
'   Address
'   Value
'
'  Returns status and sets AXstatus
'  
'-------------------------------------
func AXwritebyte(unit as integer,addr as integer,value as integer)
  dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

  csum = unit + 7 + addr + value ^ 255
  hserout 255,255,unit,4,3,addr,value,csum

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 25 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,csum
  'print runit," ",rlen," ",rerror
 
  AXstatus = rerror
  exit rerror

timeout:
  AXstatus = 128
  exit 128

endfunc

'-------------------------------------
' Sets a word to be written to a device register with the AXaction command
'  Pass
'   Device
'   Address
'   Value
'
'  Returns status and sets AXstatus
'  
'-------------------------------------
func AXsetword(unit as integer,addr as integer,value as integer)
  dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

  csum = unit + 9 + addr + value.byte(0)+value.byte(1) ^ 255
  hserout 255,255,unit,5,4,addr,value.byte(0),value.byte(1),csum

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 25 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,csum
  'print runit," ",rlen," ",rerror
 
  AXstatus = rerror
  exit rerror

timeout:
  AXstatus = 128
  exit 128

endfunc


'-------------------------------------
' Sets a byte to be written to a device register with the AXaction command
'  Pass
'   Device
'   Address
'   Value
'
'  Returns status and sets AXstatus
'
'-------------------------------------
func AXsetbyte(unit as integer,addr as integer,value as integer)
  dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

  csum = unit + 8 + addr + value ^ 255
  hserout 255,255,unit,4,4,addr,value,csum

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 25 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,csum
  'print runit," ",rlen," ",rerror
 
  AXstatus = rerror
  exit rerror

timeout:
  AXstatus = 128
  exit 128

endfunc

'-------------------------------------
' Sets a byte to be written to a device register with the AXaction command
'  No arguments
'
'  Returns status and sets AXstatus
'  
'-------------------------------------
func AXaction()
  dim csum as integer
  dim x as integer
  dim idx as integer
  dim runit as integer
  dim rlen as integer
  dim rerror as integer
  dim tcount as integer
  csum=0
  TXSTA.bit(5)=0
  hsersetup clear

 
  hserout 255,255,254,2,5,250

  TXSTA.bit(5)=0

  'First Get Transmited data
  idx = 0
  tcount  = 0
loop:
  inc tcount
  if tcount > 10 then goto timeout
  getpacket loop,idx,x,x,x,x,x,x

  idx = 0
  tcount = 0
loop1:
  inc tcount
  if tcount > 25 then goto timeout
  getpacket loop1,idx,x,x,runit,rlen,rerror,csum
  print runit," ",rlen," ",rerror
 
  AXstatus = rerror
  exit rerror

timeout:
  AXstatus = 128
  exit 128

endfunc








'====================================================================
'Syncwrite support
'====================================================================
'Note that syncwrite commands use DiosPro Bank 7 to hold sync data
'  Each command uses 5 bytes so you can store 50 device commands.

'-----------------------------------
' Send the Sync data to network
'-----------------------------------
startasmcommand AXsendsync()

AXsscsum equ OPP2
AXsslen equ OPP3
AXsstemp equ OPP4

'Clear Check sum
  clrf AXsscsum

  bcf TXSTA,5


'Read count
   lfsr	FSR0,0x700
   movff POSTINC0,AXsslen


   movlw .5
   mulwf AXsslen
   movlw .4
   addwf PRODL,f  'PRODL contains packet len

'Calc some Check sum values
   movlw .254
   addwf AXsscsum,f
   movf PRODL,w
   addwf AXsscsum,f
   movlw .131
   addwf AXsscsum,f
   movlw .30
   addwf AXsscsum,f
   movlw .4
   addwf AXsscsum,f

'At this point we can send header
   
   hserout 255
   hserout 255
   hserout 254
   hserout PRODL
   hserout 131
   hserout 30
   hserout 4

AXsendsyncloop:

   movff POSTINC0,AXsstemp
   movf AXsstemp,w
   addwf AXsscsum,f
   hserout AXsstemp

   movff POSTINC0,AXsstemp
   movf AXsstemp,w
   addwf AXsscsum,f
   hserout AXsstemp

   movff POSTINC0,AXsstemp
   movf AXsstemp,w
   addwf AXsscsum,f
   hserout AXsstemp

   movff POSTINC0,AXsstemp
   movf AXsstemp,w
   addwf AXsscsum,f
   hserout AXsstemp

   movff POSTINC0,AXsstemp
   movf AXsstemp,w
   addwf AXsscsum,f
   hserout AXsstemp

   decfsz AXsslen,f
   goto AXsendsyncloop


'Finalize check sum
   movlw .255
   xorwf AXsscsum,f
   hserout AXsscsum

'Wait till last byte has been transmitted
AXsendsyncwait:
   btfss TXSTA,TRMT
      goto AXsendsyncwait

   nop
   bcf TXSTA,5
   movff RXHEAD,RXTAIL
   
endasmcommand





'------------------------------------------------
'Add a device pos and speed to syncwrite memeory
'------------------------------------------------
'Pass device,pos,speed
'------------------------------------------------
startasmcommand AXaddsync(exp,exp,exp)

'First calculate mem offset


   lfsr	FSR0,0x700
   movlw .5
   mulwf INDF0
   incf PRODL
   movff PRODL,OPP6
   incf INDF0,f
 

   

   call getexpression
   call AXaddsyncwrite_AS

   call getexpression
   call AXaddsyncwrite_AS
   movff ACUMEH,ACUMEL
   call AXaddsyncwrite_AS

   call getexpression
   call AXaddsyncwrite_AS
   movff ACUMEH,ACUMEL
   call AXaddsyncwrite_AS   

   goto mainloop


AXaddsyncwrite_AS:   
'Now setup write to that addr

   lfsr FSR0,0x780
   movlw	.128
   xorwf	OPP6,w
   movff   ACUMEL,PLUSW0
   incf OPP6,f
   return

endasmcommand



'-----------------------------------------
'Clears Syncwrite memory
'-----------------------------------------
startasmcommand AXclearsync()
   lfsr	FSR0,0x700
   movlw .0
   movwf INDF0 
endasmcommand




'====================================================================
'High Level Functions
'====================================================================

'-----------------------------------------------------
'Play a note on the AXS1 Buzzer
' Pass
'  device
'  note 0-255 (see S1 Manual)
'  Time in units  1=.1 second  50=5 seconds.  50 is the max
'-----------------------------------------------------
func AXbuzzer(dev as integer,note as integer,time as integer) as integer
  AXwritebyte(dev,AXBuzzerTime,time)
  AXwritebyte(dev,AXBuzzerIndex,note)
endfunc


'-----------------------------------------------------------
'Play a note on the AXS1 Buzzer and wait for it to finish
' Pass
'  device
'  note 0-255 (see S1 Manual)
'  Time in units  1=.1 second  50=5 seconds.  50 is the max
'-----------------------------------------------------------
func AXbuzzerwait(dev as integer,note as integer,time as integer) as integer
  dim st as integer
  
  AXwritebyte(dev,AXBuzzerTime,time)
  AXwritebyte(dev,AXBuzzerIndex,note)
  
Loop:
  st = AXreadbyte(dev,AXBuzzerTime)
  if st = 0 then exit 0
  if st = -1 then exit 1000
  goto Loop
endfunc

'-----------------------------------------------------
'Play a note on the AXS1 Buzzer with program control over time
' Pass
'  device
'  note 0-255 (see S1 Manual)
'  Time in milliseconds
'-----------------------------------------------------
func AXbuzzercustom(dev as integer,note as integer,time as integer)
     AXwritebyte(dev,AXBuzzerTime,254)
     AXwritebyte(dev,AXBuzzerIndex,note)
     pause time
     AXwritebyte(dev,AXBuzzerTime,0)
endfunc


'-----------------------------------------------------------
'Play a special set of notes
' Pass
'  device
'  sequence ID 0-26
'-----------------------------------------------------------
func AXbuzzerspecial(dev as integer,id as integer) as integer
   dim st as integer
   st=AXbuzzerwait(dev,id,255)
   exit st
endfunc