Program Examples:CANOpen:DS402 CAN Drive Setup

From SoftMC-Wiki
Revision as of 08:36, 23 March 2015 by Rasty (talk | contribs)
Jump to: navigation, search

The following example demonstrates the setup of a CANopen drive for operation in Cyclic Synchronous Position mode. 

program
  
  bus[0].ctim=4000
  call can_reset ' reset can controller
' NMT PREOP state  
  bus[0].phase = 0
  sleep 300
'----------------------- IS setup -------------------------
  ax1.dadd = 1 ' drive address is "1"
  call Axis_Setup(ax1)
' NMT OP state
  bus[0].phase = 4
  sleep 100
  call CAN_SDO_WRITE(ax1.dadd,0x6060,0,8,8)' interpolation  position mode
  Print "Opmode =",CAN_SDO_READ(Ax1.dadd,0x6061,0) 
end program


sub Axis_Setup(Ax as generic axis)

  attach Ax  
  Ax.en=0
  Ax.pedel=5

  call CAN_SDO_WRITE(Ax.dadd , 0x60c2, 1 , 8 , bus[0].ctim/1000) '''Drive cycle time
  call CAN_SDO_WRITE(Ax.dadd , 0x60c2, 2 , 8 , -3) '''msecs
  CALL  IntergratedStepperMapPDO(Ax.dadd) ' map PDOs
  Ax.MotionBusVelocityBase=1000 
  Ax.MotionBusVelocityScale=0 

  Ax.pfac = 4096 ' one motor revolution is 4096 counts of position command (object 0x607a.0)
  Ax.vfac = Ax.pfac / 1000/60 'velicity is RPM
  Ax.afac = Ax.vfac / 1000 
  Ax.jfac = Ax.afac / 1000 
  Ax.pemax = 10
  Ax.Amax=100000
  Ax.dmax=100000
  Ax.jmax=1000000
  Ax.Acc=100000
  Ax.dec=100000
  Ax.jerk=1000000
'reset fault
  call CAN_SDO_WRITE(Ax.dadd, 0x6040, 0,16, 128)
  sleep 20
  call CAN_SDO_WRITE(Ax.dadd, 0x6040, 0,16, 0)
  detach Ax

end sub

'===================================================================
' Confuguration of drive:
'RPDO #1 :0x6040.0-control word, 0x607A.0-pcmd , 0x6071.0-tcmd
'RPDO #2 :0x60FE.1 - Digital Ouputs
'RPDO #3 :---
'RPDO #4 :---
'=================================================================== 
'TPDO #1 :0x6041.0-status word ,0x60640.0-pfb 0x60780.0-tfb
'TPDO #2 :0x60fd.0 - digila inputs, 0x6074.0 - Torque Demand
'TPDO #3 :---
'TPDO #4 :---
'===================================================================
sub IntergratedStepperMapPDO(byval drvAddress as long)
dim RPD01_COB as const long = 0x200
dim RPD02_COB as const long = 0x300
dim RPD03_COB as const long = 0x400
dim RPD04_COB as const long = 0x500
dim TPD01_COB as const long = 0x180
dim TPD02_COB as const long = 0x280
dim TPD03_COB as const long = 0x380
dim TPD04_COB as const long = 0x480
'erase MC papping
      SET_CONTROL_PDO_DADD(drvAddress,0,0,0)
      SET_STATUS_PDO_DADD(drvAddress,0,0,0)
      SET_PCMD_PDO_DADD(drvAddress,0,0,0)
      SET_PFB_PDO_DADD(drvAddress,0,0,0)
      SET_VCMD_PDO_DADD(drvAddress,0,0,0)
      SET_VFB_PDO_DADD(drvAddress,0,0,0)
      SET_TCMD_PDO_DADD(drvAddress,0,0,0)
      SET_TDCMD_PDO_DADD(drvAddress,0,0,0)
      SET_CFB_PDO_DADD(drvAddress,0,0,0)
'----------------------- Drive RPDO part -----------------------------------
      '***************** drive dynamic mapping of RPDO #1 ******************
      'disable RPDO #1 - required
      call CAN_SDO_WRITE(drvAddress,0x1400,01,32,0x80000000 + drvAddress + RPD01_COB)       
'set number of elements in RPDO #1 to zero - required
      call CAN_SDO_WRITE(drvAddress,0x1600,00,8,0) 
'-----------------------------------------------------------------------------------
'control word
	  call CAN_SDO_WRITE(drvAddress,0x1600,01,32,0x60400010) 'control word
      SET_CONTROL_PDO_DADD(drvAddress,RPD01_COB , 0 , 2) '16 bit
'-----------------------------------------------------------------------------------
'pcmd
      call CAN_SDO_WRITE(drvAddress,0x1600,02,32,0x607A0020) 'pcmd 
      SET_PCMD_PDO_DADD(drvAddress,RPD01_COB,2,4)   '32 bit
'-----------------------------------------------------------------------------------
'tcmd
      call CAN_SDO_WRITE(drvAddress,0x1600,03,32,0x60710010) 'TCMD
      SET_TCMD_PDO_DADD(drvAddress,RPD01_COB,6, 2)
'-----------------------------------------------------------------------------------
      'update number of elements in RPDO #1 - required
      call CAN_SDO_WRITE(drvAddress,0x1600,00,8,3)
      'enable RPDO #1 - required
      call CAN_SDO_WRITE(drvAddress,0x1400,01,32,RPD01_COB + drvAddress)
       '***************** drive dynamic mapping of RPDO #2 ******************
      'disable RPDO #2 - required
      call CAN_SDO_WRITE(drvAddress,0x1401,01,32,0x80000000 + drvAddress + RPD02_COB)       
     'update number of elements in RPDO #2 - required
      call CAN_SDO_WRITE(drvAddress,0x1601,00,8,0)
'-----------------------------------------------------------------------------------
' Digital outputs	  
     'update number of elements in RPDO #2 - required
      call CAN_SDO_WRITE(drvAddress,0x1601,01,32,0x60FE0120) 'outputs
      call CAN_SDO_WRITE(drvAddress,0x60FE,02,32,0xffffffff) 'outputs mask

      SET_DOUT_PDO_DADD(drvAddress,RPD02_COB , 0 , 4) '16 bit
      call CAN_SDO_WRITE(drvAddress,0x1601,00,8,1)
      call CAN_SDO_WRITE(drvAddress,0x1401,01,32,drvAddress + RPD02_COB)       
'-----------------------------------------------------------------------------------
 
      '***************** drive dynamic mapping of RPDO #3 ******************
      'disable RPDO #3 - required
       call CAN_SDO_WRITE(drvAddress,0x1402,01,32, 0x80000000 + drvAddress + RPD03_COB)
      '***************** drive dynamic mapping of RPDO #4 ******************
      'disable RPDO #4 - required
       call CAN_SDO_WRITE(drvAddress,0x1403,01,32, 0x80000000 + drvAddress + RPD04_COB)
'----------------------- Drive TPDO part -----------------------------------

      '***************** drive dynamic mapping of TPDO #1 ******************
      'disable TPDO #1 - required
      call CAN_SDO_WRITE(drvAddress,0x1800,01,32, 0x80000000 + drvAddress + TPD01_COB)
      'set number of elements in TPDO #1 to zero - required
      call CAN_SDO_WRITE(drvAddress,0x1A00,00,8,0)
      'set TPDO #1 mapping
'-----------------------------------------------------------------------------------
'status word
      call CAN_SDO_WRITE(drvAddress,0x1A00,01,32,0x60410010) 'status word 
      SET_STATUS_PDO_DADD(drvAddress,TPD01_COB,0, 2)
'-----------------------------------------------------------------------------------
'pfb
      call CAN_SDO_WRITE(drvAddress,0x1A00,02,32,0x60640020) 'pfb
      SET_PFB_PDO_DADD(drvAddress,TPD01_COB , 2 , 4)
'-----------------------------------------------------------------------------------
'current feedback
      call CAN_SDO_WRITE(drvAddress,0x1A00,03,32,0x60780010) 'tfb Current Actual Value
      SET_CFB_PDO_DADD(drvAddress,TPD01_COB , 6 , 2)
'-----------------------------------------------------------------------------------
      'update number of elements in TPDO #1 - required
      call CAN_SDO_WRITE(drvAddress,0x1A00,00,8, 3)
      'enable TPDO #1 - required
      call CAN_SDO_WRITE(drvAddress,0x1800,01,32,TPD01_COB + drvAddress)
      call CAN_SDO_WRITE(drvAddress,0x1800,02,8,1) ' enable cyclic transmission of PDO

      '***************** drive dynamic mapping of TPDO #2 ******************
      'disable TPDO #2 - required
      call CAN_SDO_WRITE(drvAddress,0x1801,01,32, 0x80000000 + drvAddress + TPD02_COB) 
     'set number of elements in TPDO #1 to zero - required
      call CAN_SDO_WRITE(drvAddress,0x1A01,00,8,0)
      'set TPDO #1 mapping
'-----------------------------------------------------------------------------------
'digital inputs
      call CAN_SDO_WRITE(drvAddress,0x1A01,01,32,0x60FD0020) 'digital inputs
      SET_DIN_PDO_DADD(drvAddress,TPD02_COB,0, 4)
'-----------------------------------------------------------------------------------
'Torque demand
      call CAN_SDO_WRITE(drvAddress,0x1A01,02,32,0x60740010) 'toque demand that drive produces to itself
      SET_TDCMD_PDO_DADD(drvAddress,TPD02_COB , 4 , 2)
'-----------------------------------------------------------------------------------
      'update number of elements in TPDO #1 - required
      call CAN_SDO_WRITE(drvAddress,0x1A01,00,8, 2)
      'enable TPDO #1 - required
      call CAN_SDO_WRITE(drvAddress,0x1801,01,32,TPD02_COB + drvAddress)

      '***************** drive dynamic mapping of TPDO #3 ******************
      'disable TPDO #3 - required
      call CAN_SDO_WRITE(drvAddress,0x1802,01,32, 0x80000000 + drvAddress + TPD03_COB) 
      '***************** drive dynamic mapping of TPDO #4 ******************
      'disable TPDO #4 - required
      call CAN_SDO_WRITE(drvAddress,0x1803,01,32, 0x80000000 + drvAddress + TPD04_COB) 
end sub

See Also

Retrieved from "http://softmc.servotronix.com/index.php?title=Program_Examples:CANOpen:DS402_CAN_Drive_Setup&oldid=125372"