class TurnProfiledleft(wpilib.command.Command):
"""
velocity closed loop inside the talon,
in here, listen to gyro and build a motion profile to drive velocity
"""
def __init__(self, angle):
super().__init__("TurnProfiled")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
# degrees
self.profiler = TrapezoidalProfile(cruise_v=80,
a=65,
target_pos=angle,
tolerance=5)
self.timer = wpilib.Timer()
def initialize(self):
self.drivetrain.zeroNavx()
self.drivetrain.config_parameters(p=2.28, f=1.88, i=0, d=450)
self.timer.start()
def execute(self):
dt = self.timer.get()
self.timer.reset()
pos = self.drivetrain.getAngle()
self.profiler.calculate_new_velocity(pos, dt)
self.drivetrain.set_turn_velocity(self.profiler.current_target_v)
def isFinished(self):
return False
def end(self):
self.drivetrain.off()
self.timer.stop()
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 __init__(self, angle):
super().__init__("TurnProfiled")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
# degrees
self.profiler = TrapezoidalProfile(cruise_v=80,
a=65,
target_pos=angle,
tolerance=5)
self.timer = wpilib.Timer()
def test_cruise_velocity_positive2():
"""
cruise velocity must always be positive
"""
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=0,
tolerance=.5)
with pytest.raises(AssertionError):
profiler.setCruiseVelocityScale(-1)
def test_isFinished1(target_pos, current_pos, expectedResult):
"""
when not moving, velocity=0, we are done when our current position is within tolerance of the target position
"""
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=target_pos,
tolerance=.5,
current_target_v=0)
assert profiler.isFinished(current_pos) == expectedResult
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_isFinished2():
"""
when moving, we are not done even when our current position is within tolerance of the target position because
overshoot has happened
"""
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=0,
tolerance=.5,
current_target_v=10)
assert profiler.isFinished(0) == False
def test_target_velocity_may_be_negative():
"""
target position may be negative
"""
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=-1000,
tolerance=.5)
profiler.calculate_new_velocity(0, 0.01)
assert profiler.current_target_v < 0
assert profiler.current_a < 0
def test_cruise_velocity_positive3():
"""
cruise velocity must always be positive
"""
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=0,
tolerance=.5)
# i will hurt anyone who actually does this
profiler._cruise_v = -10
with pytest.raises(AssertionError):
profiler.calculate_new_velocity(0, 0.01)
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_enc = distance_ft * self.drivetrain.ratio
self.period = 0.02
self.ctrl_heading = PIDController(
Kp=0, Ki=0, Kd=0, Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity = self.drivetrain.fps_to_encp100ms(3)
self.max_acceleration = self.drivetrain.fps2_to_encpsp100ms(3)
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity,
a=self.max_acceleration,
target_pos=self.dist_enc,
tolerance=(3/12.)*self.drivetrain.ratio, # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity,
a=self.max_acceleration,
target_pos=-self.dist_enc,
tolerance=(3/12.)*self.drivetrain.ratio, # 3 inches
)
self.ctrl_l = DriveController(
Kp=0, Kd=0,
Ks=1.293985, Kv=0.014172, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage(),
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_velocity, self.max_velocity)
self.ctrl_r = DriveController(
Kp=0, Kd=0,
Ks=1.320812, Kv=0.013736, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage(),
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_velocity, self.max_velocity)
def test_target_position_may_be_negative():
"""
target position may be negative
"""
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=-1000,
tolerance=.5)
def test_profiler():
# 3 m/s, 4 m/s^2, 18 m, 0.5 m
profiler = TrapezoidalProfile(cruise_v=3, a=4, target_pos=18, tolerance=0.5)
assert math.isclose(profiler.current_target_v, 0)
profiler.calculate_new_velocity(0, 0.02)
assert math.isclose(profiler.current_target_v, 0.08)
profiler.calculate_new_velocity(0.1, 0.02)
assert math.isclose(profiler.current_target_v, 0.16)
profiler.calculate_new_velocity(17.99, 0.02)
assert math.isclose(profiler.current_target_v, 0.08)
profiler.calculate_new_velocity(18.00, 0.02)
assert math.isclose(profiler.current_target_v, 0.00)
def test_cruise_velocity_positive1():
"""
cruise velocity must always be positive
"""
with pytest.raises(AssertionError):
profiler = TrapezoidalProfile(cruise_v=-10,
a=20,
target_pos=0,
tolerance=.5)
class ProfiledForward(wpilib.command.Command):
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_ft = distance_ft
self.dist_enc = distance_ft * self.drivetrain.ratio
self.timer = Timer()
self.period = self.getRobot().getPeriod()
self.ctrl_heading = PIDController(
Kp=0,
Ki=0,
Kd=0,
Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity_fps = 3 # ft/sec
self.max_v_encps = self.drivetrain.fps_to_encp100ms(
self.max_velocity_fps)
self.max_acceleration = 3 # ft/sec^2
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=-self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.ctrl_l = DriveController(
Kp=0,
Kd=0,
Ks=1.293985,
Kv=0.014172,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_v_encps, self.max_v_encps)
self.ctrl_r = DriveController(
Kp=0,
Kd=0,
Ks=1.320812,
Kv=0.013736,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_v_encps, self.max_v_encps)
self.target_v_l = 0
self.target_v_r = 0
self.target_a_l = 0
self.target_a_r = 0
self.pos_ft_l = 0
self.pos_ft_r = 0
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
self.logger = DataLogger("profiled_forward.csv")
self.drivetrain.init_logger(self.logger)
self.logger.add("profile_vel_r", lambda: self.target_v_r)
self.logger.add("profile_vel_l", lambda: self.target_v_l)
self.logger.add("pos_ft_l", lambda: self.pos_ft_l)
self.logger.add("target_pos_l", lambda: self.profiler_l.target_pos)
self.logger.add("adist_l", lambda: self.profiler_l.adist)
self.logger.add("err_l", lambda: self.profiler_l.err)
self.logger.add("choice_l", lambda: self.profiler_l.choice)
self.logger.add("adist_r", lambda: self.profiler_l.adist)
self.logger.add("err_r", lambda: self.profiler_l.err)
self.logger.add("choice_r", lambda: self.profiler_l.choice)
self.timer.start()
#print ('pdf init')
def execute(self):
self.pos_ft_l = self.drivetrain.getLeftEncoder(
) / self.drivetrain.ratio
self.pos_ft_r = self.drivetrain.getRightEncoder(
) / self.drivetrain.ratio
#print ('pdf exec ', self.timer.get())
self.profiler_l.calculate_new_velocity(self.pos_ft_l, self.period)
self.profiler_r.calculate_new_velocity(self.pos_ft_r, self.period)
self.target_v_l = self.drivetrain.fps_to_encp100ms(
self.profiler_l.current_target_v)
self.target_v_r = self.drivetrain.fps_to_encp100ms(
self.profiler_r.current_target_v)
self.target_a_l = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_l.current_a)
self.target_a_r = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_r.current_a)
self.ctrl_l.setSetpoint(self.target_v_l)
self.ctrl_l.setAccelerationSetpoint(self.target_a_l)
self.ctrl_r.setSetpoint(self.target_v_r)
self.ctrl_r.setAccelerationSetpoint(self.target_a_r)
#self.drivetrain.setLeftMotor(self.ctrl_l.calculateFeedForward())
#self.drivetrain.setRightMotor(self.ctrl_r.calculateFeedForward())
self.logger.log()
self.drivetrain.feed()
def isFinished(self):
return (abs(self.pos_ft_l - self.dist_ft) < 1 / .3
and self.profiler_l.current_target_v == 0
and self.profiler_l.current_a == 0
and self.profiler_r.current_target_v == 0
and self.profiler_r.current_a == 0)
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
self.logger.flush()
#print ('pdf end')
def correct_heading(self, correction):
self.profiler_l.setCruiseVelocityScale(1 + correction)
self.profiler_r.setCruiseVelocityScale(1 - correction)
class ProfiledForward(wpilib.command.Command):
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_enc = distance_ft * self.drivetrain.ratio
self.period = 0.02
self.ctrl_heading = PIDController(
Kp=0, Ki=0, Kd=0, Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity = self.drivetrain.fps_to_encp100ms(3)
self.max_acceleration = self.drivetrain.fps2_to_encpsp100ms(3)
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity,
a=self.max_acceleration,
target_pos=self.dist_enc,
tolerance=(3/12.)*self.drivetrain.ratio, # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity,
a=self.max_acceleration,
target_pos=-self.dist_enc,
tolerance=(3/12.)*self.drivetrain.ratio, # 3 inches
)
self.ctrl_l = DriveController(
Kp=0, Kd=0,
Ks=1.293985, Kv=0.014172, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage(),
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_velocity, self.max_velocity)
self.ctrl_r = DriveController(
Kp=0, Kd=0,
Ks=1.320812, Kv=0.013736, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage(),
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_velocity, self.max_velocity)
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
def execute(self):
pos_l = self.drivetrain.getLeftEncoder()
pos_r = self.drivetrain.getRightEncoder()
self.profiler_l.calculate_new_velocity(pos_l, self.period)
self.profiler_r.calculate_new_velocity(pos_r, self.period)
self.ctrl_l.setSetpoint(self.profiler_l.current_target_v)
self.ctrl_l.setAccelerationSetpoint(self.profiler_l.current_a)
self.ctrl_r.setSetpoint(self.profiler_r.current_target_v)
self.ctrl_r.setAccelerationSetpoint(self.profiler_r.current_a)
def isFinished(self):
return (
self.profiler_l.current_target_v == 0 and
self.profiler_l.current_a == 0 and
self.profiler_r.current_target_v == 0 and
self.profiler_r.current_a == 0)
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
def correct_heading(self, correction):
correction = 1 + correction
self.profiler_l.setCruiseVelocityScale(correction)
self.profiler_r.setCruiseVelocityScale(-correction)