diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 23aae84..f723e20 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.0, - "y": 0.0 + "x": 2.780674342105263, + "y": 5.112253289473684 }, "prevControl": null, "nextControl": { - "x": 0.25, - "y": 0.0 + "x": 2.7421875, + "y": 6.016694078947369 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.0, - "y": 0.0 + "x": 3.8294407894736837, + "y": 5.862746710526316 }, "prevControl": { - "x": 1.75, - "y": 0.0 + "x": 3.5794407894736837, + "y": 5.862746710526316 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 132.26189909327812 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bfceb05..e2f669e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -99,41 +99,41 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 + public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); private static final class ModuleSpecificConstants { - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); - private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125); + private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_WIDTH); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; - private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_WIDTH); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5); - private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.5); + private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; - private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_WIDTH); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25); - private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.07666015625); + private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_WIDTH); } public static final class IDs { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d9fcbd3..71b9b76 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import java.util.ArrayList; import java.util.List; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ba19149..f50c3f6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; import java.util.Optional; +import java.util.function.DoubleSupplier; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.CANcoder; @@ -69,8 +70,13 @@ public class SwerveDrive extends Subsystem { // Handle exception as needed config = null; } + // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( - () -> swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()), // Robot pose supplier + () -> { + var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()); + pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); + return pose;//getRotation().times(-1) + }, // Robot pose supplier swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() @@ -121,7 +127,7 @@ public class SwerveDrive extends Subsystem { if (fieldRelative) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) + .withVelocityY(-leftStick.getY()*speedAdjust) .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); // double rot = 0; @@ -159,8 +165,8 @@ public class SwerveDrive extends Subsystem { // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(-leftStick.getY()*speedAdjust) .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); }