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

Example 1 with Rotation2d

use of edu.wpi.first.math.geometry.Rotation2d 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 Rotation2d

use of edu.wpi.first.math.geometry.Rotation2d 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 3 with Rotation2d

use of edu.wpi.first.math.geometry.Rotation2d in project rapid-react-robot by wando-advanced-robotics.

the class DriveSystem method driveToPosition.

public Pose2d driveToPosition(double xMeters, double yMeters) {
    Translation2d translation = new Translation2d(xMeters, yMeters);
    Rotation2d angle = Rotation2d.fromDegrees(Math.atan(yMeters / xMeters) * (180 / Math.PI));
    if (angle.getDegrees() < 0) {
        angle = Rotation2d.fromDegrees(360 + angle.getDegrees());
    }
    System.out.println("Angle: " + angle.getDegrees());
    targetPosition = new Pose2d(translation, angle);
    if (targetPosition.getRotation().getDegrees() > robotAngle.getDegrees()) {
        if (targetPosition.getRotation().getDegrees() - robotAngle.getDegrees() > 180) {
            turnDirection = Direction.LEFT;
        } else if (targetPosition.getRotation().getDegrees() - robotAngle.getDegrees() <= 180) {
            turnDirection = Direction.RIGHT;
        }
    } else if (targetPosition.getRotation().getDegrees() <= robotAngle.getDegrees()) {
        if (targetPosition.getRotation().getDegrees() - robotAngle.getDegrees() < -180) {
            turnDirection = Direction.LEFT;
        } else if (targetPosition.getRotation().getDegrees() - robotAngle.getDegrees() >= -180) {
            turnDirection = Direction.RIGHT;
        }
    } else {
        turnDirection = Direction.LEFT;
    }
    System.out.println(turnDirection);
    // if (targetPosition.getY() >= 0) {
    // driveDirection = Direction.FORWARD;
    // }
    // else if (targetPosition.getY() < 0) {
    // driveDirection = Direction.BACKWARD;
    // }
    // else {
    // driveDirection = Direction.FORWARD;
    // }
    driveDirection = Direction.FORWARD;
    return targetPosition;
}

Example 4 with Rotation2d

use of edu.wpi.first.math.geometry.Rotation2d in project RapidReact2022 by ligerbots.

the class TrajectoryPlotter method plotWaypoints.

public void plotWaypoints(int index, List<Translation2d> waypoints) {
    String indexStr = "";
    if (index > 0) {
        indexStr = String.valueOf(index);
        m_maxWaypoints = Math.max(m_maxWaypoints, index);
    }
    final Rotation2d rot = Rotation2d.fromDegrees(0);
    List<Pose2d> poses = new ArrayList<Pose2d>();
    for (Translation2d t : waypoints) {
        poses.add(new Pose2d(t, rot));
    }
    m_field2d.getObject("waypoints" + indexStr).setPoses(poses);
}

Example 5 with Rotation2d

use of edu.wpi.first.math.geometry.Rotation2d in project FRC2022 by 2202Programming.

the class IntakeCentricDrive method calculate.

void calculate() {
    // Get the x speed. We are inverting this because Xbox controllers return
    // negative values when we push forward.
    xSpeed = xspeedLimiter.calculate(dc.getVelocityX()) * DriveTrain.kMaxSpeed;
    ySpeed = yspeedLimiter.calculate(dc.getVelocityY()) * DriveTrain.kMaxSpeed;
    rot = rotLimiter.calculate(dc.getXYRotation()) * DriveTrain.kMaxAngularSpeed;
    filteredBearing = bearingFilter.calculate(getJoystickBearing().getRadians());
    // Clamp speeds/rot from the Joysticks
    xSpeed = MathUtil.clamp(xSpeed, -Constants.DriveTrain.kMaxSpeed, Constants.DriveTrain.kMaxSpeed);
    ySpeed = MathUtil.clamp(ySpeed, -Constants.DriveTrain.kMaxSpeed, Constants.DriveTrain.kMaxSpeed);
    rot = MathUtil.clamp(rot, -Constants.DriveTrain.kMaxAngularSpeed, Constants.DriveTrain.kMaxAngularSpeed);
    // set goal of angle PID to be commanded bearing (in degrees) from joysticks
    m_targetAngle = new Rotation2d(filteredBearing);
    // from -Pi to Pi
    Rotation2d m_currentAngle = drivetrain.getPose().getRotation();
    m_angleError = m_targetAngle;
    m_angleError.minus(m_currentAngle);
    // PID already tuned in degrees
    intakeAnglePid.setSetpoint(m_targetAngle.getDegrees());
    rot = intakeAnglePid.calculate(m_currentAngle.getDegrees());
    output_states = kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_currentAngle));
}