print('* Drive Initialization.')
print('\tClear Faults.')
node.wu16(0x6040,0x80)
print('\tMotor parameters Setup.')
node.wu16(0x6402,1) # motor type
node.wu16(0x6410,1,7000) # cont. current limit, ma
node.wu16(0x6410,2,8000) # out current limit, ma
node.wu8(0x6410,3,1) # pole paires
node.wu16(0x6410,4,3000) # max speed in current mode
node.wu16(0x6410,5,400) # Thermal constant
print('\tPosition Sensor Setup.')
node.wu16(0x2210, 1, 500) # enc. pulse n
node.wu16(0x2210, 2, 1) # pos. sensor type
print('\tCurrent Regulator Gains Setup.')
node.ws16(0x60F6, 1, 1250) # P
node.ws16(0x60F6, 2, 300) # I
print('\tSpeed Regulator Gains Setup.')
node.ws16(0x60F9, 1, 1700) # P
node.ws16(0x60F9, 2, 200) # I
print('\tPosition Regulator Gains Setup.')
node.ws16(0x60FB, 1, 350) # P
node.ws16(0x60FB, 2, 10) # I
node.ws16(0x60FB, 3, 400) # D
print('\tClear Faults.')
node.wu16(0x6040,0x80)
#---------------------------
# TESTS
#---------------------------
if len(sys.argv) > 1 and sys.argv[1] == 'c':
print('* Current Mode Test.')
