fixed problems

Patribots4738 · Dec 18, 2023 · 3929014 · 3929014
1 parent 1e741ac
commit 3929014
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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);
     }