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 CsvTrajectoryCommand(_CsvTrajectoryCommand):
def __init__(self, fnom):
super().__init__(fnom)
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.ctrl_heading.setContinuous(True)
self.max_velocity_fps = 11
self.max_velocity_encps = self.drivetrain.fps_to_encp100ms(self.max_velocity_fps)
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_encps, self.max_velocity_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_velocity_encps, self.max_velocity_encps)
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
self.logger = DataLogger("csv_trajectory1.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("i", lambda: self.i)
self.timer.start()
self.i = 0
#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
(_, _, _, self.target_v_l, self.target_v_r, self.target_a_l,
self.target_a_r, self.target_heading) = self.get_trajectory_point_enc(self.i)
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.ctrl_heading.setSetpoint(self.target_heading)
self.drivetrain.feed()
self.logger.log()
self.i += 1
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):
pass
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)