57 lines
1.8 KiB
Python
57 lines
1.8 KiB
Python
def speed_fields_valid(motor):
|
|
"""
|
|
Check if the motor speed fields are valid:
|
|
VMAX >= VELO >= VBAS
|
|
"""
|
|
vbas = motor.read_field('basespeed')
|
|
velo = motor.read_field('speed')
|
|
vmax = motor.read_field('maxspeed')
|
|
assert vmax >= velo >= vbas
|
|
assert vbas >= 0
|
|
assert velo >= 0
|
|
assert vmax >= 0
|
|
|
|
# If the motor is not a speed variable drive, VMAX == VELO == VBAS should hold true
|
|
if motor.read_field('can_set_speed') == 0:
|
|
assert vmax == velo == vbas
|
|
|
|
|
|
def set_speed(motor, new_speed):
|
|
old_speed = motor.read_field('speed')
|
|
try:
|
|
motor.write_field('speed', new_speed)
|
|
if motor.read_field('can_set_speed') != 0:
|
|
if new_speed > motor.read_field('maxspeed'):
|
|
assert motor.read_field('speed') == motor.read_field('maxspeed')
|
|
elif new_speed < motor.read_field('basespeed'):
|
|
assert motor.read_field('speed') == motor.read_field('basespeed')
|
|
else:
|
|
assert motor.read_field('speed') == new_speed
|
|
else:
|
|
assert motor.read_field('speed') == old_speed
|
|
finally:
|
|
motor.write_field('speed', old_speed)
|
|
|
|
|
|
def set_minspeed_maxspeed_meanspeed(motor):
|
|
set_speed(motor, motor.read_field('maxspeed'))
|
|
set_speed(motor, motor.read_field('basespeed'))
|
|
set_speed(motor, 0.5 * (motor.read_field('basespeed') + motor.read_field('maxspeed')))
|
|
|
|
|
|
def set_invalid_speed_above_min_below_max(motor):
|
|
set_speed(motor, motor.read_field('maxspeed') + 1)
|
|
set_speed(motor, motor.read_field('basespeed') - 1)
|
|
|
|
|
|
def set_speed_and_move(motor, new_speed, target):
|
|
old_speed = motor.read_field('speed')
|
|
try:
|
|
motor.write_field('speed', new_speed)
|
|
motor.move_and_wait(target)
|
|
assert motor.at_target(target)
|
|
finally:
|
|
motor.write_field('speed', old_speed)
|
|
|
|
|