for i in range(500):
time.sleep(0.2)
print('Actual Current: {0} mA'.format(node.rs16(0x6078)))
except KeyboardInterrupt:
node.ws16(0x2030,0)
else:
print('* Velocity Mode Test.')
V = 1000
A = 500
D = 500
node.ws8(0x6060,3) # mode of operation
node.wu32(0x607F,4000) # max profile v rpm
node.wu32(0x6083,A) # profile acceleration
node.wu32(0x6084,D) # profile deceleration
node.wu32(0x6085,10000) # quick stop deceleration
node.ws16(0x6086,0) # motion profile type, 0 - trapez.
node.wu16(0x6040,0x6) # controlword (shutdown)
node.wu16(0x6040,0xF) # controlword (switch on)
for i in range(4):
if i%2 == 0:
node.ws32(0x60FF,V) # target velocity
else:
node.ws32(0x60FF,-V) # target velocity
node.wu16(0x6040,0xF) # controlword (switch on)
for i in range(20):
time.sleep(0.2)
print('Actual Velocity: {0}'.format(node.rs32(0x606C)))
node.wu16(0x6040,0x10F) # stop
node.wu16(0x6040,0x6) # stop
n.wu32(0x1800, 3, 200) # inhibit time in 100ys
# Desired velocity
n.ws32(0x3300,V) # desired velocity in rpm
# Start of the test program
n.wu32(0x3000,1) # reset error register
n.wu32(0x3004,1) # enable power stage
time.sleep(0.2) # 100ms delay for power stage to be enabled
#n.wu32( 0x3150, 0, 1) # open brake (just in case)
mc_trig = canio2.MsgDataTrigger(0x180+NODE_ID, 0x10, 0x10)
dest = REVOLUTIONS*ENCODER_RESOLUTION
while(1):
dest = - dest
print dest
waiter.add(mc_trig)
n.ws32(0x3791,dest) # relative positioning 4000 counts (hall or encoder)
if waiter.wait(10000) == False:
print('Motion Complete Timeout')
n.wu32(0x3000,2)
n.wu32(0x3004, 0) # disable power stage
time.sleep(0.2)
print 'Encoder position:', n.rs32(0x396A)
except:
print '\nError:', sys.exc_info()[0], sys.exc_info()[1]
finally:
print 'Stop drive.'
n.wu32(0x3000,2)
n.wu32(0x3004, 0) # disable power stage
