Examples with Pose2d edu.wpi.first.math.geometry.Pose2d used on opensource projects

Example 1 with Pose2d

use of edu.wpi.first.math.geometry.Pose2d in project 2022-Rapid-React by Manchester-Central.

the class AimToGoal method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    var thetaSpeed = 0.0;
    if (m_camera.hasTarget()) {
        thetaSpeed = m_camera.getXAngle() * 0.25;
    } else {
        var currentPose = m_swerveDrive.getPose();
        var relativeLocation = currentPose.relativeTo(new Pose2d(k_goalLocation, Rotation2d.fromDegrees(0)));
        var angle = Math.atan(relativeLocation.getY() / relativeLocation.getX());
        var rotation = new Rotation2d(angle).minus(m_swerveDrive.getRotation());
        rotation = currentPose.getX() > k_goalLocation.getX() ? rotation.rotateBy(Rotation2d.fromDegrees(180)) : rotation;
        // rotation.getDegrees();
        thetaSpeed = 0;
    }
    thetaSpeed = MathUtil.clamp(thetaSpeed, -Constants.MaxORPS * 0.5, Constants.MaxORPS * 0.5);
    m_swerveDrive.moveRobotRelative(0, 0, thetaSpeed);
}

Example 2 with Pose2d

use of edu.wpi.first.math.geometry.Pose2d in project 2022-Rapid-React by Manchester-Central.

the class SwerveDrive method updateOdometry.

public void updateOdometry(double x, double y, double angle) {
    updateGyroAdjustmentAngle(angle);
    m_odometry.resetPosition(new Pose2d(x, y, getRotation()), getRotation());
}

Example 3 with Pose2d

use of edu.wpi.first.math.geometry.Pose2d in project 2022-Rapid-React by Manchester-Central.

the class SwerveDrive method periodic.

@Override
public void periodic() {
    // This method will be called once per scheduler run
    m_odometry.update(getRotation(), getModuleStates());
    Pose2d pose = getPose();
    m_moduleFL.updatePosition(pose);
    m_moduleFR.updatePosition(pose);
    m_moduleBR.updatePosition(pose);
    m_moduleBL.updatePosition(pose);
    // pose = pose.transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(90)));
    m_field.setRobotPose(pose);
    SmartDashboard.putBoolean("Gyro/Calibrating", m_gyro.isCalibrating());
    SmartDashboard.putNumber("Gyro/Angle", m_gyro.getAngle());
    if (m_enableTuningPIDs) {
        tunePIDs();
    }
}

Example 4 with Pose2d

use of edu.wpi.first.math.geometry.Pose2d in project 6591-2022 by jafillmore.

the class RobotContainer method getAutonomousCommand.

/**
 * Use this to pass the autonomous command to the main {@link Robot} class.
 *
 * @return the command to run in autonomous
 */
public Command getAutonomousCommand() {
    // Create config for trajectory
    TrajectoryConfig config = new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, AutoConstants.kMaxAccelerationMetersPerSecondSquared).setKinematics(DriveConstants.kDriveKinematics);
    // An example trajectory to follow.  All units in meters.
    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(// Start at the origin facing the +X direction
    new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path
    List.of(new Translation2d(1, 1), new Translation2d(2, -1)), // End 3 meters straight ahead of where we started, facing forward
    new Pose2d(3, 0, new Rotation2d(0)), config);
    MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand(exampleTrajectory, m_robotDrive::getPose, DriveConstants.kFeedforward, DriveConstants.kDriveKinematics, // Position contollers
    new PIDController(AutoConstants.kPXController, 0, 0), new PIDController(AutoConstants.kPYController, 0, 0), new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints), // Needed for normalizing wheel speeds
    AutoConstants.kMaxSpeedMetersPerSecond, // Velocity PID's
    new PIDController(DriveConstants.kPFrontLeftVel, 0, 0), new PIDController(DriveConstants.kPRearLeftVel, 0, 0), new PIDController(DriveConstants.kPFrontRightVel, 0, 0), new PIDController(DriveConstants.kPRearRightVel, 0, 0), m_robotDrive::getCurrentWheelSpeeds, // Consumer for the output motor voltages
    m_robotDrive::setDriveMotorControllersVolts, m_robotDrive);
    // Reset odometry to the starting pose of the trajectory.
    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
    // Run path following command, then stop at the end.
    return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
}

Example 5 with Pose2d

use of edu.wpi.first.math.geometry.Pose2d in project 2022RobotCode by AusTINCANsProgrammingTeam.

the class AutonModes method getRamseteCommands.

private Command[] getRamseteCommands(Trajectory... trajectories) {
    Command[] ramseteCommands = new Command[trajectories.length];
    for (int i = 0; i < trajectories.length; i++) {
        RamseteCommand r = new RamseteCommand(trajectories[i], driveBaseSubsystem::getPose, new RamseteController(Constants.ramseteB, // ramsete follower to follow trajectory
        Constants.ramseteZeta), Constants.driveKinematics, driveBaseSubsystem::acceptWheelSpeeds, driveBaseSubsystem);
        // first ramsete command needs to have driveBase reset odometry to match that of pathweaver
        if (i == 0) {
            Pose2d p = trajectories[i].getInitialPose();
            ramseteCommands[i] = r.beforeStarting(() -> driveBaseSubsystem.resetOdometry(p));
        } else {
            ramseteCommands[i] = r;
        }
    }
    return ramseteCommands;
}