def test_profiler1():
"""
forward velocity, trapezoid, no overshoot
"""
DT = 0.02
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=50,
tolerance=.5,
current_target_v=0)
t = 0
pos = 0
if log_trajectory:
logger = DataLogger("test_profiler1.csv")
logger.log_while_disabled = True
logger.do_print = True
logger.add('t', lambda: t)
logger.add('pos', lambda: pos)
logger.add('v', lambda: profiler.current_target_v)
logger.add('a', lambda: profiler.current_a)
while not profiler.isFinished(pos):
if log_trajectory:
logger.log()
profiler.calculate_new_velocity(pos, DT)
pos += profiler.current_target_v * DT
t += DT
if t > 10:
if log_trajectory:
logger.close()
assert False, "sim loop timed out"
if t < 0.501:
assert profiler.current_a == pytest.approx(20,
0.01), "t = %f" % (t, )
if 0.501 < t < 5:
assert profiler.current_target_v == pytest.approx(
10., 0.01), "t = %f" % (t, )
assert profiler.current_a == 0, "t = %f" % (t, )
if 5 < t < 5.5 - 0.001:
assert profiler.current_a == pytest.approx(-20.,
0.01), "t = %f" % (t, )
if t == pytest.approx(5.50, 0.001):
assert profiler.current_a == pytest.approx(0.,
0.01), "t = %f" % (t, )
assert profiler.current_target_v == pytest.approx(
0., 0.01), "t = %f" % (t, )
if log_trajectory:
logger.log()
logger.close()
assert t == pytest.approx(5.52, 0.01)
assert profiler.current_a == 0
def test_StateSpaceDriveCommand(Notifier):
global log_trajectory
left_drive = DriveSide(
Ks=1.293985,
Kv=0.014172 * 63. / 0.3048,
Ka=0.005938 * 63. / 0.3048)
right_drive = DriveSide(
Ks=1.320812,
Kv=0.013736 * 63. / 0.3048,
Ka=0.005938 * 63. / 0.3048)
robot = Rockslide()
robot.robotInit()
robot.drivetrain.getLeftEncoder = getLeftEncoder = MagicMock()
robot.drivetrain.getRightEncoder = getRightEncoder = MagicMock()
robot.drivetrain.getVoltage = MagicMock(return_value=10)
command = StateSpaceDriveCommand("straight3m.tra")
command.initialize()
dt = robot.getPeriod()
t = 0
if log_trajectory:
logger = DataLogger("test_StateSpaceDriveCommand.csv")
logger.log_while_disabled = True
logger.do_print = False
logger.add('t', lambda: t)
logger.add('pos_l_m', lambda: left_drive.pos_m)
logger.add('pos_r_m', lambda: right_drive.pos_m)
logger.add('m_pos_l_m', lambda: command.y[0,0])
logger.add('m_pos_r_m', lambda: command.y[1,0])
logger.add('vel_l_mps', lambda: left_drive.v_mps)
logger.add('vel_r_mps', lambda: right_drive.v_mps)
logger.add('target_pos_l_m', lambda: command.r[0,0])
logger.add('target_pos_r_m', lambda: command.r[2,0])
logger.add('target_vel_l_mps', lambda: command.r[1,0])
logger.add('target_vel_r_mps', lambda: command.r[3,0])
logger.add('voltage', lambda: command.drivetrain.getVoltage())
logger.add('vpl', lambda: command.drivetrain.motor_lb.get())
logger.add('vpr', lambda: command.drivetrain.motor_rb.get())
while not command.isFinished():
logger.log()
getLeftEncoder.return_value = left_drive.pos_m * 630 / 0.3048
getRightEncoder.return_value = right_drive.pos_m * 630 / 0.3048
command.execute()
V = command.drivetrain.getVoltage()
vpl = command.drivetrain.motor_lb.get()
vpr = command.drivetrain.motor_rb.get()
left_drive.update(V * vpl, dt)
right_drive.update(V * vpr, dt)
t += dt
assert t < 10
command.end()
def test_ProfiledForward2(Notifier, sim_hooks):
global log_trajectory
robot = Rockslide()
robot.robotInit()
DT = robot.getPeriod()
robot.drivetrain.getLeftEncoder = getLeftEncoder = MagicMock()
robot.drivetrain.getRightEncoder = getRightEncoder = MagicMock()
getLeftEncoder.return_value = 0
getRightEncoder.return_value = 0
command = ProfiledForward(10)
command.initialize()
t = 0
pos_ft = 0
if log_trajectory:
logger = DataLogger("test_profiled_forward2.csv")
logger.log_while_disabled = True
logger.do_print = True
logger.add('t', lambda: t)
logger.add('pos', lambda: pos_ft)
logger.add('target_pos', lambda: command.dist_ft)
logger.add('v', lambda: command.profiler_l.current_target_v)
logger.add('max_v', lambda: command.max_v_encps)
logger.add('a', lambda: command.profiler_l.current_a)
logger.add('max_a', lambda: command.max_acceleration)
logger.add('voltage', lambda: command.drivetrain.getVoltage())
logger.add('vpl', lambda: command.drivetrain.motor_lb.get())
logger.add('adist', lambda: command.profiler_l.adist)
logger.add('err', lambda: command.profiler_l.err)
while t < 10:
if log_trajectory:
logger.log()
getLeftEncoder.return_value = pos_ft * command.drivetrain.ratio
getRightEncoder.return_value = -pos_ft * command.drivetrain.ratio
command.execute()
v = command.profiler_l.current_target_v
pos_ft += v * DT
t += DT
sim_hooks.time = t
if command.isFinished():
break
command.end()
if log_trajectory:
logger.log()
logger.close()
