def __init__(self, kS, kV, trackwidth, trajectory):
'''
Creates a controller for following a PathWeaver trajectory.
__init__(self, kS: Volts, kV: Volts * Seconds / Meters, trackwidth: Meters, trajectory: wpilib.trajectory.Trajectory)
:param kS: The kS gain determined by characterizing the Robot's drivetrain
:param kV: The kV gain determined by characterizing the Robot's drivetrain
:param trackwidth: The horizontal distance between the left and right wheels of the tank drive.
:param trajectory: The trajectory to follow. This can be generated by PathWeaver, or made by hand.
'''
self.kS = kS
self.kV = kV
self.trajectory = trajectory
self.odometry = DifferentialDriveOdometry(
Rotation2d(radians(0)), self.trajectory.initialPose())
self.ramsete = RamseteController(2, 0.7)
self.drive_kinematics = DifferentialDriveKinematics(trackwidth)
self.are_wheel_speeds_zero = False
self.timer = Timer()
def reset(self, chassis, gyro):
'''
Re-initializes all of the data in the controller as if the path has not yet been executed.
This method MUST be called in teleopInit and autonomousInit directly before the controller is used.
reset(self, chassis: traits.DriveTrain, gyro: traits.Gyro)
:param chassis: An object that implements the DriveTrain trait. This object's encoders will be reset by this function
:param gyro: An object that implements the Gyro trait. This object's angle will be reset by this function
'''
# Assert the objects implement the proper traits
assert chassis.implements(DriveTrain)
assert gyro.implements(Gyro)
# The encoders and gyro need to be reset so that the Ramsete
# controller is fed data that looks new.
# By reseting these sensors, we look like weve never run the path at all!
chassis.reset_encoders()
gyro.reset()
self.are_wheel_speeds_zero = False
# The odometry object also needs to be re-initialized
# so that it forgets the state from the previous run
self.odometry = DifferentialDriveOdometry(
Rotation2d(radians(0)), self.trajectory.initialPose())
self.timer.start()
def __init__(self):
self.mode = self._Mode.Idle
self.odometry = DifferentialDriveOdometry(Rotation2d.fromDegrees(0))
self.desired_output = WheelState()
self.desired_velocity = WheelState()
self.feedforward = WheelState()
self.wheel_left = motorstate.MotorState()
self.wheel_right = motorstate.MotorState()
self.nt = NetworkTables.getTable(f"/components/chassis")
class Odometry:
kinematics: DifferentialDriveKinematics
navx: navx.AHRS
left_encoder: CANEncoder
right_encoder: CANEncoder
left_distance = ntproperty('/left distance/', 0)
right_distance = ntproperty('/right distance/', 0)
def getAngle(self):
"""Return the navx angle with counter-clockwise as positive"""
return -self.navx.getAngle()
def setup(self):
self.odometry = DifferentialDriveOdometry(
Rotation2d(math.radians(self.getAngle())))
def get_pose(self) -> Pose2d:
return self.odometry.getPose()
@feedback
def get_pose_string(self) -> str:
pose = self.get_pose()
return f'({pose.translation().X()}, {pose.translation().Y()}, {pose.rotation()}'
def get_distance(self, left=True):
if left:
return self.left_distance
else:
return -self.right_distance
def reset(self, new_pose: Pose2d = Pose2d()):
self.odometry.resetPosition(new_pose,
Rotation2d.fromDegrees(self.getAngle()))
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
@property
def left_rate(self):
return self.left_encoder.getVelocity()
@property
def right_rate(self):
return -self.right_encoder.getVelocity()
def execute(self):
self.odometry.update(Rotation2d(math.radians(self.getAngle())),
self.left_encoder.getPosition(),
-self.right_encoder.getPosition())
self.left_distance = self.left_encoder.getPosition()
self.right_distance = -self.right_encoder.getPosition()
def setup(self) -> None:
self.left_front.setInverted(False)
self.right_front.setInverted(True)
for motor in (
self.left_front,
self.left_rear,
self.right_front,
self.right_rear,
):
motor.setIdleMode(rev.IdleMode.kCoast)
self.left_rear.follow(self.left_front)
self.right_rear.follow(self.right_front)
self.left_encoder = rev.CANEncoder(self.left_front)
self.right_encoder = rev.CANEncoder(self.right_front)
rev_to_m = WHEEL_CIRCUMFERENCE / GEAR_RATIO
for enc in (self.left_encoder, self.right_encoder):
enc.setPositionConversionFactor(rev_to_m)
enc.setVelocityConversionFactor(rev_to_m / 60)
self.left_pid: rev.CANPIDController = self.left_front.getPIDController(
)
self.right_pid: rev.CANPIDController = self.right_front.getPIDController(
)
self.ff_calculator = SimpleMotorFeedforwardMeters(kS=0.194,
kV=2.79,
kA=0.457)
for pid in (self.left_pid, self.right_pid):
# TODO: needs tuning
pid.setP(5.19 / 10)
pid.setI(0)
pid.setD(0)
pid.setIZone(0)
pid.setFF(0)
pid.setOutputRange(-1, 1)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.odometry = DifferentialDriveOdometry(self._get_heading())
class Chassis:
left_rear: rev.CANSparkMax
left_front: rev.CANSparkMax
right_rear: rev.CANSparkMax
right_front: rev.CANSparkMax
imu: NavX
open_loop = magicbot.tunable(False)
vx = magicbot.will_reset_to(0.0)
vz = magicbot.will_reset_to(0.0)
def setup(self) -> None:
self.left_front.setInverted(False)
self.right_front.setInverted(True)
self.disable_brake_mode()
self.left_rear.follow(self.left_front)
self.right_rear.follow(self.right_front)
self.left_encoder = rev.CANEncoder(self.left_front)
self.right_encoder = rev.CANEncoder(self.right_front)
rev_to_m = WHEEL_CIRCUMFERENCE / GEAR_RATIO
for enc in (self.left_encoder, self.right_encoder):
enc.setPositionConversionFactor(rev_to_m)
enc.setVelocityConversionFactor(rev_to_m / 60)
self.left_pid: rev.CANPIDController = self.left_front.getPIDController(
)
self.right_pid: rev.CANPIDController = self.right_front.getPIDController(
)
self.ff_calculator = SimpleMotorFeedforwardMeters(kS=0.194,
kV=2.79,
kA=0.457)
for pid in (self.left_pid, self.right_pid):
# TODO: needs tuning
pid.setP(5.19 / 10)
pid.setI(0)
pid.setD(0)
pid.setIZone(0)
pid.setFF(0)
pid.setOutputRange(-1, 1)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.odometry = DifferentialDriveOdometry(self._get_heading())
# default_position on the field
self.reset_odometry(Pose2d(-3, 0, Rotation2d(math.pi)))
def execute(self) -> None:
# XXX: https://github.com/robotpy/robotpy-wpilib/issues/635
chassis_speeds = ChassisSpeeds()
chassis_speeds.vx = self.vx
chassis_speeds.omega = self.vz
speeds = self.kinematics.toWheelSpeeds(chassis_speeds)
left_ff = self.ff_calculator.calculate(speeds.left)
right_ff = self.ff_calculator.calculate(speeds.right)
if self.open_loop:
self.left_front.setVoltage(left_ff)
self.right_front.setVoltage(right_ff)
else:
self.left_pid.setReference(speeds.left,
rev.ControlType.kVelocity,
arbFeedforward=left_ff)
self.right_pid.setReference(speeds.right,
rev.ControlType.kVelocity,
arbFeedforward=right_ff)
self.odometry.update(
self._get_heading(),
self.left_encoder.getPosition(),
self.right_encoder.getPosition(),
)
def drive(self, vx: float, vz: float) -> None:
"""Sets the desired robot velocity.
Arguments:
vx: Forwards linear velocity in m/s.
vz: Angular velocity in rad/s, anticlockwise positive.
"""
self.vx = vx
self.vz = vz
def disable_closed_loop(self) -> None:
"""Run the drivetrain in open loop mode (feedforward only)."""
self.open_loop = True
def enable_closed_loop(self) -> None:
"""Run the drivetrain in velocity closed loop mode."""
self.open_loop = False
def enable_brake_mode(self) -> None:
for motor in (
self.left_front,
self.left_rear,
self.right_front,
self.right_rear,
):
motor.setIdleMode(rev.IdleMode.kBrake)
def disable_brake_mode(self) -> None:
for motor in (
self.left_front,
self.left_rear,
self.right_front,
self.right_rear,
):
motor.setIdleMode(rev.IdleMode.kCoast)
def _get_heading(self) -> Rotation2d:
"""Get the current heading of the robot from the IMU, anticlockwise positive."""
return Rotation2d(self.imu.getYaw())
def get_pose(self) -> Pose2d:
"""Get the current position of the robot on the field."""
return self.odometry.getPose()
@magicbot.feedback
def get_heading(self) -> float:
"""Get the current heading of the robot."""
return self.get_pose().rotation().radians()
@magicbot.feedback
def get_left_velocity(self) -> float:
return self.left_encoder.getVelocity()
@magicbot.feedback
def get_right_velocity(self) -> float:
return self.right_encoder.getVelocity()
def reset_odometry(self, pose: Pose2d) -> None:
"""Resets the odometry to start with the given pose.
This is intended to be called at the start of autonomous.
"""
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
self.odometry.resetPosition(pose, self._get_heading())
def find_field_angle(self, field_point: Translation2d) -> float:
"""Return the theoretical angle (in radians) to the target based on odometry"""
pose = self.get_pose()
rel = field_point - pose.translation()
rel_heading = Rotation2d(rel.y, rel.x) + pose.rotation()
return rel_heading.radians()
def find_power_port_angle(self) -> float:
return self.find_field_angle(POWER_PORT_POSITION)
class Path:
def __init__(self, kS, kV, trackwidth, trajectory):
'''
Creates a controller for following a PathWeaver trajectory.
__init__(self, kS: Volts, kV: Volts * Seconds / Meters, trackwidth: Meters, trajectory: wpilib.trajectory.Trajectory)
:param kS: The kS gain determined by characterizing the Robot's drivetrain
:param kV: The kV gain determined by characterizing the Robot's drivetrain
:param trackwidth: The horizontal distance between the left and right wheels of the tank drive.
:param trajectory: The trajectory to follow. This can be generated by PathWeaver, or made by hand.
'''
self.kS = kS
self.kV = kV
self.trajectory = trajectory
self.odometry = DifferentialDriveOdometry(
Rotation2d(radians(0)), self.trajectory.initialPose())
self.ramsete = RamseteController(2, 0.7)
self.drive_kinematics = DifferentialDriveKinematics(trackwidth)
self.are_wheel_speeds_zero = False
self.timer = Timer()
def is_done(self):
'''
is_done(self) -> bool
Returns whether or not the path is done.
'''
return self.are_wheel_speeds_zero
def reset(self, chassis, gyro):
'''
Re-initializes all of the data in the controller as if the path has not yet been executed.
This method MUST be called in teleopInit and autonomousInit directly before the controller is used.
reset(self, chassis: traits.DriveTrain, gyro: traits.Gyro)
:param chassis: An object that implements the DriveTrain trait. This object's encoders will be reset by this function
:param gyro: An object that implements the Gyro trait. This object's angle will be reset by this function
'''
# Assert the objects implement the proper traits
assert chassis.implements(DriveTrain)
assert gyro.implements(Gyro)
# The encoders and gyro need to be reset so that the Ramsete
# controller is fed data that looks new.
# By reseting these sensors, we look like weve never run the path at all!
chassis.reset_encoders()
gyro.reset()
self.are_wheel_speeds_zero = False
# The odometry object also needs to be re-initialized
# so that it forgets the state from the previous run
self.odometry = DifferentialDriveOdometry(
Rotation2d(radians(0)), self.trajectory.initialPose())
self.timer.start()
def follow(self, chassis, gyro):
'''
This updates the Path and drives the chassis to follow the path
update(self, chassis: traits.DriveTrain, gyro: traits.Gyro)
:param chassis: An object that implements the DriveTrain trait. When this function is called,
the object's motors will be driven to follow the last set trajectory.
:param gyro: An object that implements the gyro trait.
'''
# Assert the objects implement the proper traits
assert chassis.implements(DriveTrain)
assert gyro.implements(Gyro)
if self.is_done():
return
# Set the chassis to low gear for more precise movements
chassis.set_low_gear()
# If a trajectory has been set, run it
if self.trajectory is not None:
# Get the accumulated left and right distance of the chass
ld, rd = chassis.get_left_distance(), chassis.get_right_distance()
# Ramsete requires the counterclockwise angle of the Robot
angle = gyro.get_counterclockwise_degrees()
# Get the current position of the robot
current_pose = self.odometry.update(Rotation2d(radians(angle)), ld,
rd)
# Calculate the target position using the trajectory, and get the chassis wheel speeds
target_pose = self.trajectory.sample(self.timer.get())
chassis_speed = self.ramsete.calculate(current_pose, target_pose)
wheel_speeds = self.drive_kinematics.toWheelSpeeds(chassis_speed)
l, r = wheel_speeds.left, wheel_speeds.right
if abs(l) == 0 and abs(r) == 0:
self.are_wheel_speeds_zero = True
# Convert the left and right wheel speeds to volts using the characterized constants,
# and then convert those to percent values from -1 to 1
chassis.tank_drive((self.kS + l * self.kV) / 12,
(self.kS + r * self.kV) / 12)
class Chassis:
# chassis physical constants
BUMPER_WIDTH = 3.25 * units.meters_per_inch
ROBOT_WIDTH = 30 * units.meters_per_inch + BUMPER_WIDTH
ROBOT_LENGTH = 30 * units.meters_per_inch + BUMPER_WIDTH
ROBOT_MASS = 100 * units.kilograms_per_pound
TRACK_WIDTH = 24 * units.meters_per_inch
TRACK_RADIUS = TRACK_WIDTH / 2
WHEEL_DIAMETER = 6 * units.meters_per_inch
WHEEL_RADIUS = WHEEL_DIAMETER / 2
WHEEL_CIRCUMFERENCE = 2 * np.pi * WHEEL_RADIUS
GEAR_RATIO = (48 / 14) * (50 / 16) # 10.7142861
# conversions
RADIANS_PER_METER = (2 * np.pi * GEAR_RATIO) / WHEEL_CIRCUMFERENCE
METERS_PER_RADIAN = WHEEL_CIRCUMFERENCE / (2 * np.pi * GEAR_RATIO)
# motor config
LEFT_INVERTED = True
RIGHT_INVERTED = False
# motor coefs
KS = 0.149
KV = 2.4
KA = 0.234
# velocity pidf gains
VL_KP = 0.000363
VL_KI = 0
VL_KD = 0
VL_KF = 0
VR_KP = 0.000363
VR_KI = 0
VR_KD = 0
VR_KF = 0
# joystick
MAX_JOYSTICK_OUTPUT = 1
# static objeccts
feedforward_l = motorfeedforward.MotorFeedforward(KS, KV, KA)
feedforward_r = motorfeedforward.MotorFeedforward(KS, KV, KA)
kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
# required devices
dm_l: lazytalonfx.LazyTalonFX
dm_r: lazytalonfx.LazyTalonFX
ds_l: lazytalonfx.LazyTalonFX
ds_r: lazytalonfx.LazyTalonFX
imu: lazypigeonimu.LazyPigeonIMU
# constraints
MAX_VELOCITY = 3
class _Mode(Enum):
Idle = 0
PercentOutput = 1
Velocity = 2
def __init__(self):
self.mode = self._Mode.Idle
self.odometry = DifferentialDriveOdometry(Rotation2d.fromDegrees(0))
self.desired_output = WheelState()
self.desired_velocity = WheelState()
self.feedforward = WheelState()
self.wheel_left = motorstate.MotorState()
self.wheel_right = motorstate.MotorState()
self.nt = NetworkTables.getTable(f"/components/chassis")
def setup(self):
self.dm_l.setInverted(self.LEFT_INVERTED)
self.dm_r.setInverted(self.RIGHT_INVERTED)
self.dm_l.setRadiansPerUnit(self.RADIANS_PER_METER)
self.dm_r.setRadiansPerUnit(self.RADIANS_PER_METER)
self.dm_l.setPIDF(
0,
self.VL_KP,
self.VL_KI,
self.VL_KD,
self.VL_KF,
)
self.dm_r.setPIDF(
0,
self.VR_KP,
self.VR_KI,
self.VR_KD,
self.VR_KF,
)
def on_enable(self):
pass
def on_disable(self):
self.stop()
def stop(self) -> None:
self.mode = self._Mode.Idle
if wpilib.RobotBase.isSimulation():
self._setSimulationOutput(1, 0)
self._setSimulationOutput(3, 0)
def setOutput(self, output_l: float, output_r: float) -> None:
self.mode = self._Mode.PercentOutput
self.desired_output.left = output_l
self.desired_output.right = output_r
def setWheelVelocity(self, velocity_l: float, velocity_r: float) -> None:
self.mode = self._Mode.Velocity
self.desired_velocity.left = velocity_l
self.desired_velocity.right = velocity_r
def setChassisVelocity(self, velocity_x: float, velocity_y: float,
velocity_omega) -> None:
state = ChassisSpeeds(velocity_x, velocity_y, -velocity_omega)
velocity = self.kinematics.toWheelSpeeds(state)
self.setWheelVelocity(velocity.left, velocity.right)
def setTankDrive(self, throttle, rotation):
self.mode = self._Mode.PercentOutput
self.desired_output.left = throttle + rotation
self.desired_output.right = throttle - rotation
# self.desired_output.norm(self.MAX_JOYSTICK_OUTPUT)
def setForzaDrive(self, throttle, reverse, rotation):
if throttle <= 0:
throttle = -reverse
self.setTankDrive(throttle, rotation)
def ntPutLeftRight(self, key, value):
self.nt.putNumber(f"{key}_left", value.left)
self.nt.putNumber(f"{key}_right", value.right)
def updateNetworkTables(self):
"""Update network table values related to component."""
self.wheel_left.putNT(self.nt, "wheel_left")
self.wheel_right.putNT(self.nt, "wheel_right")
self.ntPutLeftRight("desired_output", self.desired_output)
self.ntPutLeftRight("desired_velocity", self.desired_velocity)
self.ntPutLeftRight("feedforward", self.feedforward)
def getHeading(self):
return self.getPose().rotation().radians()
def getPose(self):
if wpilib.RobotBase.isSimulation():
x = hal.simulation.SimDeviceSim("Field2D").getDouble("x").get()
y = hal.simulation.SimDeviceSim("Field2D").getDouble("y").get()
rot = hal.simulation.SimDeviceSim("Field2D").getDouble("rot").get()
rot = (units.angle_range(rot * units.radians_per_degree) *
units.degrees_per_radian)
return Pose2d(x, y, Rotation2d.fromDegrees(rot))
else:
return self.odometry.getPose()
def _setSimulationOutput(self, id, output):
hal.simulation.SimDeviceSim(f"Talon FX[{id}]").getDouble(
"Motor Output").set(output)
def _getSimulationPosition(self, id):
return (hal.simulation.SimDeviceSim(
f"Custom Talon FX[{id}]").getDouble("Position").get())
def execute(self):
dt = 0.02
self.wheel_left.update(self.dm_l.getPosition(), dt)
self.wheel_right.update(self.dm_r.getPosition(), dt)
if wpilib.RobotBase.isSimulation():
self.wheel_left.position = self._getSimulationPosition(1)
self.wheel_right.position = self._getSimulationPosition(3)
self.odometry.update(
Rotation2d(self.getHeading()),
self.wheel_left.position,
self.wheel_right.position,
)
if self.mode == self._Mode.Idle:
self.dm_l.setOutput(0)
self.dm_r.setOutput(0)
elif self.mode == self._Mode.PercentOutput:
self.dm_l.setOutput(self.desired_output.left)
self.dm_r.setOutput(self.desired_output.right)
elif self.mode == self._Mode.Velocity:
self.feedforward.left = (
self.feedforward_l.calculate(self.desired_velocity.left, dt) /
12)
self.feedforward.right = (
self.feedforward_r.calculate(self.desired_velocity.right, dt) /
12)
if not wpilib.RobotBase.isSimulation():
self.dm_l.setVelocity(
self.desired_velocity.left,
self.feedforward.left,
)
self.dm_r.setVelocity(
self.desired_velocity.right,
self.feedforward.right,
)
else:
self.dm_l.setOutput(self.desired_velocity.left /
self.MAX_VELOCITY)
self.dm_r.setOutput(self.desired_velocity.right /
self.MAX_VELOCITY)
self.updateNetworkTables()
def setup(self):
self.odometry = DifferentialDriveOdometry(
Rotation2d(math.radians(self.getAngle())))