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