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_profiler4():
"""
reverse velocity, triangle, no overshoot
"""
DT = 0.02
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=-4,
tolerance=.5,
current_target_v=0)
t = 0
pos = 0
if log_trajectory:
logger = DataLogger("test_profiler4.csv")
logger.log_while_disabled = 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.4599:
assert profiler.current_a == pytest.approx(-20,
0.01), "t = %f" % (t, )
if t == pytest.approx(0.46, 0.01):
assert profiler.current_target_v == pytest.approx(-9.2, 0.01)
assert profiler.current_a == pytest.approx(-20,
0.01), "t = %f" % (t, )
if 0.4601 < t < 0.92:
assert profiler.current_a == pytest.approx(20,
0.01), "t = %f" % (t, )
if log_trajectory:
logger.log()
logger.close()
assert t == pytest.approx(0.94, 0.01)
assert profiler.current_a == 0
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()
class ElevatorTest2(Command):
def __init__(self):
super().__init__("laksdjfkl")
self.elevator = self.getRobot().elevator
self.requires(self.elevator)
self.timer = wpilib.Timer()
self.done = False
self.state = 1
self.voltage = 0.75
def initialize(self):
self.logger = DataLogger('elevator-majig.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add(
"enc_pos1",
lambda: self.elevator.motor.getSelectedSensorVelocity(0))
self.logger.add("current_motor1",
lambda: self.elevator.motor.getOutputCurrent())
self.logger.add("current_follow1",
lambda: self.elevator.other_motor.getOutputCurrent())
self.logger.add("voltage_motor1",
lambda: self.elevator.motor.getMotorOutputVoltage())
self.logger.add(
"voltage_follow1",
lambda: self.elevator.other_motor.getMotorOutputVoltage())
self.logger.add("bvoltage_motor1",
lambda: self.elevator.motor.getBusVoltage())
self.logger.add("bvoltage_follow1",
lambda: self.elevator.other_motor.getBusVoltage())
self.logger.add(
"enc_pos2", lambda: self.elevator.other_right_motor.
getSelectedSensorVelocity(0))
self.logger.add("current_motor2",
lambda: self.elevator.right_motor.getOutputCurrent())
self.logger.add(
"current_follow2",
lambda: self.elevator.other_right_motor.getOutputCurrent())
self.logger.add(
"voltage_motor2",
lambda: self.elevator.right_motor.getMotorOutputVoltage())
self.logger.add(
"voltage_follow2",
lambda: self.elevator.other_right_motor.getMotorOutputVoltage())
self.logger.add("bvoltage_motor2",
lambda: self.elevator.right_motor.getBusVoltage())
self.logger.add(
"bvoltage_follow2",
lambda: self.elevator.other_right_motor.getBusVoltage())
self.timer.start()
def execute(self):
e = self.elevator.getEncoderPosition()
if self.state == 1:
self.elevator.ascend(self.voltage)
if e > 28000:
self.state = 2
self.elevator.descend(self.voltage)
if self.state == 2:
self.elevator.descend(self.voltage)
if e < 1000 and self.state == 2:
self.state = 1
self.voltage += 0.25
elif self.voltage > 1:
self.voltage = 0
self.done = True
self.logger.log()
def isFinished(self):
return self.done
def end(self):
self.elevator.hover()
self.logger.close()
from data_logger import DataLogger
from data_reader import *
from genetic_algorithm import *
pop_size = 100
gen = 100
Px = 0.7
Pm = 0.01
Tour = 5
selection = 'tournament'
for i in range(12, 21, 2):
reader = DataReader()
n, flow_matrix, distance_matrix = reader.read_data("data/had%s.dat" %
str(i))
logger = DataLogger('had%s' % str(i))
logger.write_header(pop_size, gen, Px, Pm, Tour, selection)
for j in range(0, 10):
genetic_algorithm = GeneticAlgorithm(n, pop_size, flow_matrix,
distance_matrix, Pm, Px, Tour,
logger, selection)
print('Had%s file best result: %s' % (i, genetic_algorithm.run(gen)))
logger.close()
