def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
self.sd = NetworkTables.getTable("SmartDashboard")
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
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 getPose(self) -> Optional[Pose2d]:
if not self.targetExists():
return None
data = self.pose_data
# Turn limelight pose information into a wpilib Pose2d
rot = Rotation2d.fromDegrees((data[4] + 180) % 360)
x = 15 * math.cos(rot.degrees()) + (-data[2])
y = 15 * math.cos(90 - rot.degrees()) + data[0]
# Change unit from inches to meters
return Pose2d(x / 39.37, y / 39.37, rot)
def setup_trajectory(self, trajectory: Trajectory):
self.controller = RamseteController()
self.left_controller.reset()
self.right_controller.reset()
self.prev_time = 0
self.speed = DifferentialDriveWheelSpeeds()
self.controller.setTolerance(
Pose2d(0.05, 0.05, Rotation2d(math.radians(5))))
self.initial_state = self.trajectory.sample(0)
speeds = ChassisSpeeds()
speeds.vx = self.initial_state.velocity
speeds.omega = self.initial_state.velocity * self.initial_state.curvature
self.prevSpeeds = self.kinematics.toWheelSpeeds(speeds)
def generate_trajectories(options, robot_class):
"""
Generate trajectory from waypoints.
:param options: Options for the entry point being called
:param robot_class: The class of the robot being run
"""
print('Generating Trajectories...', end='')
generated_trajectories: Dict[str, str] = {}
traj_data: TrajectoryData
for key, traj_data in TRAJECTORIES.items():
new_trajectories = {}
if not traj_data.field_relative:
start_pos: StartingPosition
for start_pos in StartingPosition:
# The subtracted Pose2d here is the ORIGIN of the field (The power port).
# All trajectories are now relative to the POWER PORT.
transformed_trajectory = generate_trajectory(
traj_data).transformBy(start_pos.value - Pose2d())
new_trajectories[
f'{key}-{start_pos.name}'] = transformed_trajectory
else:
# Transforms Pathweaver trajectories which are relative to the field's top left coordinate
# to being relative to the POWER PORT
new_trajectories[key] = generate_trajectory(traj_data)
# All trajectories should be POWER PORT relative at this point
generated_trajectories.update({
k: TrajectoryUtil.serializeTrajectory(v)
for k, v in new_trajectories.items()
})
print('Done')
print('Writing Trajectories...', end='')
with open(PICKLE_FILE, 'wb') as f:
pickle.dump(generated_trajectories, f)
print('Done')
def test_swerve4_odometry(s4: SwerveDrive4Kinematics):
odometry = SwerveDrive4Odometry(s4, Rotation2d(0))
odometry.resetPosition(Pose2d(), Rotation2d.fromDegrees(90))
state = SwerveModuleState(5)
odometry.updateWithTime(
0,
Rotation2d.fromDegrees(90),
SwerveModuleState(),
SwerveModuleState(),
SwerveModuleState(),
SwerveModuleState(),
)
pose = odometry.updateWithTime(0.1, Rotation2d.fromDegrees(90), state,
state, state, state)
assert pose.translation().x == pytest.approx(0.5)
assert pose.translation().y == pytest.approx(0.0)
assert pose.rotation().degrees() == pytest.approx(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)))
class Waypoints:
MIN_FIELD_X = 0
MAX_FIELD_X = 54 * units.meters_per_foot
MIN_FIELD_Y = 0
MAX_FIELD_Y = 27 * units.meters_per_foot
INITATION_LINE_X = 10 * units.meters_per_foot
OFFSET = 180
START_LAWFUL = Pose2d(INITATION_LINE_X, 7.3, Rotation2d.fromDegrees(180))
TRENCH_RUN_LAWFUL = Pose2d(8.3, 7.3, Rotation2d.fromDegrees(180))
START_STEAL = Pose2d(INITATION_LINE_X, 0.7, Rotation2d.fromDegrees(180))
TRENCH_RUN_STEAL = Pose2d(6.2, 0.7, Rotation2d.fromDegrees(180))
START_CENTER = Pose2d(INITATION_LINE_X, 5.8, Rotation2d.fromDegrees(180))
RENDEZVOUS_TWO_BALLS = Pose2d(6.3, 5.5, Rotation2d.fromDegrees(120))
IDEAL_SHOOT = Pose2d(INITATION_LINE_X + 2 * units.meters_per_foot, 5.8,
Rotation2d.fromDegrees(180))
class StartingPosition(Enum):
# LEFT is one foot off the LEFT wall relative to the Power port (and facing the power port).
LEFT = Pose2d(3.2, -0.6468, Rotation2d.fromDegrees(180))
CENTER = Pose2d(3.2, 0, Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
RIGHT = Pose2d(3.2, -3.25,
Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
@dataclass
class TrajectoryData:
"""Hold metadata for a wpilib trajectory"""
start: Pose2d
interior_waypoints: List[Translation2d]
end: Pose2d
config: TrajectoryConfig = TrajectoryConfig(MAX_GENERATION_VELOCITY,
MAX_GENERATION_ACCELERATION)
constraints: Tuple[TrajectoryConstraint, ...] = DEFAULT_CONSTRAINTS
kinematics: DifferentialDriveKinematics = KINEMATICS
field_relative: bool = True
reverse: bool = False
# The POWER PORT's Pose relative to the top left coordinate of the field as provided by Path weaver.
POWER_PORT = Pose2d(0.2, -2.437, Rotation2d())
# Starting positions are relative to the field's top left coordinate facing down the field
class StartingPosition(Enum):
# LEFT is one foot off the LEFT wall relative to the Power port (and facing the power port).
LEFT = Pose2d(3.2, -0.6468, Rotation2d.fromDegrees(180))
CENTER = Pose2d(3.2, 0, Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
RIGHT = Pose2d(3.2, -3.25,
Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
# ALL trajectory points should be relative to the field's top left coordinate down the field
# Trajectories that aren't field relative are generated with respect to ALL Starting Positions
# E.x. "charge-LEFT", "charge-CENTER", "charge-RIGHT"
# We do this to allow for trajectory chaining, which requires field-relativity
def getAveragedPose(self) -> Pose2d:
return Pose2d(self.avg_x, self.avg_y,
Rotation2d.fromDegrees(self.avg_rot))
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
self.sd = NetworkTables.getTable("SmartDashboard")
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
#Create the DriveTrain
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
#Create The Encoders
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.f_l_encoder.setSimDevice(0)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.r_l_encoder.setSimDevice(1)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.f_r_encoder.setSimDevice(2)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.r_r_encoder.setSimDevice(3)
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
# Now that we have created the ability to see wheel speeds, chassis speeds and our position
# Let us start to use feedforward to try to lock our robot into a specific speed.
self.feedForward = SimpleMotorFeedforwardMeters(kS=0.194,
kV=.5,
kA=0.457)
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)