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)