use of edu.wpi.first.math.geometry.Translation2d 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));
}
use of edu.wpi.first.math.geometry.Translation2d 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;
}
use of edu.wpi.first.math.geometry.Translation2d in project RapidReact2022 by robototes.
the class AutonomousCommand method getAutonomousCommand.
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config = new TrajectoryConfig(AutoConstants.MAX_SPEED_METERS_PER_SECOND, AutoConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED).setKinematics(AutoConstants.driveKinematics);
// creating trajectory path (right now is a square)
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, Rotation2d.fromDegrees(0)), List.of(new Translation2d(1, 0), new Translation2d(1, 1), new Translation2d(0, 1)), new Pose2d(0, 0.0, Rotation2d.fromDegrees(0)), config);
// creates thetacontroller (rotation)
ProfiledPIDController thetaController = new ProfiledPIDController(0.000000005, 0, 0, AutoConstants.K_THETA_CONTROLLER_CONSTRAINTS);
thetaController.enableContinuousInput(-Math.PI, Math.PI);
exampleTrajectory.relativeTo(drivebaseSubsystem.getPoseAsPoseMeters());
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(exampleTrajectory, // Functional interface to feed supplier
drivebaseSubsystem::getPoseAsPoseMeters, AutoConstants.driveKinematics, // Position controllers
new PIDController(AutoConstants.PX_CONTROLLER, 0, 0), new PIDController(AutoConstants.PY_CONTROLLER, 0, 0), thetaController, drivebaseSubsystem::updateModules, drivebaseSubsystem);
return swerveControllerCommand;
}
use of edu.wpi.first.math.geometry.Translation2d 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);
}
use of edu.wpi.first.math.geometry.Translation2d in project RapidReact2022-340 by Greater-Rochester-Robotics.
the class SwerveDrive method getAngleOfTarget.
/**
* Estimates the closest angle to the target,
* given that the odometry is working
* @return angle in radians (not restricted to PI to -PI)
*/
public double getAngleOfTarget() {
// get the current angle
double currentAngle = getGyroInRad();
// if the pose hasn't been set return current angle.
if (!hasPoseBeenSet) {
return currentAngle;
}
// Calculating current angle between -pi and pi
double absoluteCurrentAngle = currentAngle % Constants.TWO_PI;
if (absoluteCurrentAngle > Math.PI) {
absoluteCurrentAngle -= 2 * (Math.PI);
} else if (absoluteCurrentAngle < -Math.PI) {
absoluteCurrentAngle += 2 * Math.PI;
}
// Finds where the center of the field is with respect to the robot
Translation2d target = driveOdometry.getPoseMeters().getTranslation().minus(Constants.FIELD_CENTER);
// based on that, find the angle of the above Tanslation2d object
double desiredAngle = Math.atan2(target.getY(), target.getX());
// Calculate the robot's target angle given the continuous angle of the gyroscope
return currentAngle - absoluteCurrentAngle + desiredAngle;
}
Aggregations