diff --git a/images/swerve final.mp4 b/images/swerve final.mp4 new file mode 100644 index 0000000..25c8b44 Binary files /dev/null and b/images/swerve final.mp4 differ diff --git a/src/main/java/frc/robot/commands/AutonPathExample.java b/src/main/java/frc/robot/commands/AutonPathExample.java index 6a2cd5e..9249050 100644 --- a/src/main/java/frc/robot/commands/AutonPathExample.java +++ b/src/main/java/frc/robot/commands/AutonPathExample.java @@ -33,9 +33,10 @@ public AutonPathExample(SwerveDrivetrain drivetrain) { new Pose2d(0, 0, new Rotation2d(0)), List.of( new Translation2d(1, 2), - new Translation2d(3, 1) + new Translation2d(3, 1), + new Translation2d(5,4) ), - new Pose2d(4, 2, Rotation2d.fromDegrees(180.0)), + new Pose2d(4, 2, Rotation2d.fromDegrees(540.0)), swerveDrivetrain.getTrajectoryConfig()); swerveDrivetrain.getField().getObject("traj").setTrajectory(trajectory); diff --git a/src/main/java/frc/robot/commands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveJoystickCommand.java index 7bfd742..5e7433a 100644 --- a/src/main/java/frc/robot/commands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveJoystickCommand.java @@ -62,7 +62,7 @@ public void execute() { ChassisSpeeds chassisSpeeds; if (fieldOrientedFunc.get()) { chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - vY, vX, vW, drivetrain.getSimRotation2d()); + -vY, vX, vW, drivetrain.getSimRotation2d()); } else { chassisSpeeds = new ChassisSpeeds(vX, vY, vW);