From 3929014ea5faf6ce457ef83c115e7a3180121b91 Mon Sep 17 00:00:00 2001 From: rin-star Date: Mon, 18 Dec 2023 11:40:53 -0800 Subject: [PATCH] fixed problems --- src/main/java/frc/robot/commands/Drive.java | 9 ++++++++- src/main/java/frc/robot/subsystems/Claw.java | 3 +-- src/main/java/frc/robot/subsystems/Swerve.java | 8 ++++---- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index e10465e..23a2060 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -64,13 +64,20 @@ public void initialize() { public void execute() { double x = xSupplier.getAsDouble(); double y = ySupplier.getAsDouble(); + double rotation = rotationSupplier.getAsDouble(); + boolean fieldRelative = fieldRelativeSupplier.getAsBoolean(); + if (shouldMirror.getAsBoolean()) { x *= -1; y *= -1; } - use the following variables to drive the swerve using swerve.drive + swerve.drive(x, y, rotation, fieldRelative); + + //use the following variables to drive the swerve using swerve.drive + } + @Override public void end(boolean interrupted) { swerve.drive(0, 0, 0, false); diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 70eceb5..ed82804 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -11,8 +11,7 @@ public class Claw extends SubsystemBase { public Claw() { - set claw can id - claw = new Neo(/*can id goes here*/); + claw = new Neo(1); claw.restoreFactoryDefaults(); claw.setSmartCurrentLimit(30); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 74b9498..1a061c1 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Timer; import frc.robot.util.ADIS16470_IMU; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -98,8 +99,9 @@ public Swerve() { @Override public void periodic() { - Update the poseEstimator using the current timestamp (from DriverUI.java), the gyro angle, and the current module states + //Update the poseEstimator using the current timestamp (from DriverUI.java), the gyro angle, and the current module states + poseEstimator.updateWithTime(Timer.getFPGATimestamp(), getGyroAngle(), getModulePositions()); if (FieldConstants.IS_SIMULATION) { for (MAXSwerveModule mod : swerveModules) { @@ -148,9 +150,7 @@ public void drive(double xSpeed, double ySpeed, double rotSpeed, boolean fieldRe rotSpeed *= (DriveConstants.DYNAMIC_MAX_ANGULAR_SPEED * speedMultiplier); SwerveModuleState[] swerveModuleStates = - DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates( - - ); + DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rotSpeed)); setModuleStates(swerveModuleStates); }