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()