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)
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 PIDCommand(Command):
"""This class defines a Command which interacts heavily with a PID loop.
It provides some convenience methods to run an internal PIDController.
It will also start and stop said PIDController when the PIDCommand is
first initialized and ended/interrupted.
"""
def __init__(self, p, i, d, period=None, f=0.0, name=None):
"""Instantiates a PIDCommand that will use the given p, i and d values.
It will use the class name as its name unless otherwise specified.
It will also space the time between PID loop calculations to be equal
to the given period.
:param p: the proportional value
:param i: the integral value
:param d: the derivative value
:param period: the time (in seconds) between calculations (optional)
:param f: the feed forward value
:param name: the name (optional)
"""
super().__init__(name)
self.controller = PIDController(p, i, d, f, self.returnPIDInput,
self.usePIDOutput, period)
def getPIDController(self):
"""Returns the PIDController used by this PIDCommand.
Use this if you would like to fine tune the pid loop.
Notice that calling setSetpoint(...) on the controller
will not result in the setpoint being trimmed to be in
the range defined by setSetpointRange(...).
:returns: the PIDController used by this PIDCommand
"""
return self.controller
def _initialize(self):
self.controller.enable()
def _end(self):
self.controller.disable()
def _interrupted(self):
self._end()
def setSetpointRelative(self, deltaSetpoint):
"""Adds the given value to the setpoint.
If :meth:`setRange` was used, then the bounds will still be honored by
this method.
:param deltaSetpoint: the change in the setpoint
"""
self.setSetpoint(self.getSetpoint() + deltaSetpoint)
def setSetpoint(self, setpoint):
"""Sets the setpoint to the given value. If :meth:`setRange` was called,
then the given setpoint will be trimmed to fit within the range.
:param setpoint: the new setpoint
"""
self.controller.setSetpoint(setpoint)
def getSetpoint(self):
"""Returns the setpoint.
:returns: the setpoint
"""
return self.controller.getSetpoint()
def getPosition(self):
"""Returns the current position
:returns: the current position
"""
return self.returnPIDInput()
def returnPIDInput(self):
"""Returns the input for the pid loop.
It returns the input for the pid loop, so if this command was based
off of a gyro, then it should return the angle of the gyro
All subclasses of PIDCommand must override this method.
This method will be called in a different thread then the :class:`.Scheduler`
thread.
:returns: the value the pid loop should use as input
"""
raise NotImplementedError
def usePIDOutput(self, output):
"""Uses the value that the pid loop calculated. The calculated value
is the "output" parameter.
This method is a good time to set motor values, maybe something along
the lines of `driveline.tankDrive(output, -output)`.
All subclasses of PIDCommand should override this method.
This method will be called in a different thread then the Scheduler
thread.
:param output: the value the pid loop calculated
"""
pass
def initSendable(self, builder):
self.controller.initSendable(builder)
super().initSendable(builder)
builder.setSmartDashboardType("PIDCommand")