print 'Resetting node...'
if node.reset(5000) == False:
print 'Boot Timeout'
sys.exit(1)
print 'Node state after reset: ' + str(node.state)
node.state = CANopenNode.STATES.OPERATIONAL
print 'Node state : ' + str(node.state)
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
