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
# First TX-PDO (motor is sending)
# Mapping table
n.wu32(0x1A00, 0, 0) # delete mapping table first
n.wu32(0x1A00, 1, 0x30020020) # set first object (status register):
# object 0x3002.0 (300200), length = 32bits (0x20h)
n.wu32(0x1A00, 0, 1) # write numbers of objects into the table = 2
# CobId
n.wu32(0x1800, 1, 0x80000000) # delete the old CobId first (Bit 31=1)
n.wu32(0x1800, 1, 0x180 + NODE_ID) # set new CobId
# Tansmissiontype
n.wu32(0x1800, 2, 255) # asynchronous (see above)
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')
