fix build errors, add drive command deadbands

StuyPulse · Oct 20, 2023 · be02942 · be02942
1 parent 39246b7
commit be02942
Show file tree

Hide file tree

Showing 5 changed files with 34 additions and 21 deletions.
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -8,7 +8,7 @@
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.constants.Ports;
-import com.stuypulse.robot.subsystems.SwerveDrive;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.util.BootlegXbox;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;

diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
@@ -4,13 +4,18 @@
 
 import com.stuypulse.robot.constants.Settings.Driver.Drive;
 import com.stuypulse.robot.constants.Settings.Driver.Turn;
-import com.stuypulse.robot.constants.Settings.Swerve.Drive.*;
-import com.stuypulse.robot.subsystems.SwerveDrive;
+import com.stuypulse.robot.constants.Settings.Driver.Turn.GyroFeedback;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.stuylib.control.Controller;
+import com.stuypulse.stuylib.control.angle.AngleController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
 import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.math.Angle;
 import com.stuypulse.stuylib.streams.IStream;
 import com.stuypulse.stuylib.streams.vectors.VStream;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class SwerveDriveDrive extends CommandBase {
@@ -23,6 +28,7 @@ public class SwerveDriveDrive extends CommandBase {
     private final Gamepad driver;
 
     private Optional<Rotation2d> holdAngle;
+    private AngleController gyroFeedback;
 
     public SwerveDriveDrive(Gamepad driver) {
         this.driver = driver;
@@ -32,10 +38,9 @@ public SwerveDriveDrive(Gamepad driver) {
         turn = IStream.create(driver::getRightX);       
 
         holdAngle = Optional.empty();
+        gyroFeedback = new AnglePIDController(GyroFeedback.P, GyroFeedback.I, GyroFeedback.D);
 
         addRequirements(swerve);
-
-
     }
 
     @Override
@@ -53,26 +58,30 @@ private boolean isWithinDriveDeadband() {
 
    @Override
     public void execute() {
-        swerve.drive(speed.get(), turn.get());
-
+        double angularVel = turn.get();
 
         if(isWithinTurnDeadband()){
             if(holdAngle.isEmpty()) {
                 holdAngle = Optional.of(swerve.getGyroAngle());
             }
-
-        }
-        /*if (isWithinDriveDeadband()) {
-            if () {
-
-            }
-            if () {
-
+
+            if(GyroFeedback.GYRO_FEEDBACK_ENABLED.get() && !isWithinDriveDeadband()) {
+                angularVel = -gyroFeedback.update(
+                    Angle.fromRotation2d(holdAngle.get()),
+                    Angle.fromRotation2d(swerve.getGyroAngle())
+                );
             }
         }
+        else {
+            holdAngle = Optional.empty(); 
+        }
 
+        if(driver.getRawStartButton() || driver.getRawSelectButton()) {
+            swerve.setXMode();
+        }
         else {
+            swerve.drive(speed.get(), angularVel);
+        }
 
-        }*/
     }
 }
diff --git a/...uypulse/robot/subsystems/SwerveDrive.java → .../robot/subsystems/swerve/SwerveDrive.java b/...uypulse/robot/subsystems/SwerveDrive.java → .../robot/subsystems/swerve/SwerveDrive.java
@@ -1,4 +1,4 @@
-package com.stuypulse.robot.subsystems;
+package com.stuypulse.robot.subsystems.swerve;
 
 import com.kauailabs.navx.frc.AHRS;
 import com.stuypulse.robot.constants.Ports;
@@ -24,6 +24,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class SwerveDrive extends SubsystemBase {
+
     public final static SwerveDrive instance;
 
     static {
@@ -40,15 +41,13 @@ public static SwerveDrive getInstance() {
     }
 
     private final SwerveModuleImpl[] modules;  
-    private final FieldObject2d[] moduleObjects;
     private final SwerveDriveKinematics kinematics;
     private final AHRS gyro;
 
     public SwerveDrive(SwerveModuleImpl... modules) {    
         this.modules = modules;
         kinematics = new SwerveDriveKinematics(getModuleOffsets());
         gyro = new AHRS(SPI.Port.kMXP);
-        moduleObjects = new FieldObject2d[modules.length];
     }
 
     /**
@@ -97,7 +96,7 @@ public ChassisSpeeds getChassisSpeeds() {
      * @return The original state if the speed is greater than the deadband, otherwise a state with zero speed
      */
     public SwerveModuleState filterModuleState(SwerveModuleState state) {
-        if (Math.abs(state.speedMetersPerSecond) > Swerve.MODULE_SPEED_DEADBAND.get()) {
+        if (Math.abs(state.speedMetersPerSecond) > Swerve.MODULE_VELOCITY_DEADBAND.get()) {
             return state;
         }
 

diff --git a/...ypulse/robot/subsystems/SwerveModule.java → ...robot/subsystems/swerve/SwerveModule.java b/...ypulse/robot/subsystems/SwerveModule.java → ...robot/subsystems/swerve/SwerveModule.java
@@ -1,3 +1,4 @@
+package com.stuypulse.robot.subsystems.swerve;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -10,4 +11,7 @@ public abstract class SwerveModule extends SubsystemBase {
     public abstract SwerveModulePosition getModulePosition();
 
     public abstract void setState(SwerveModuleState state);
+
+    @Override
+    public abstract void periodic();
 }
diff --git a/...se/robot/subsystems/SwerveModuleImpl.java → ...t/subsystems/swerve/SwerveModuleImpl.java b/...se/robot/subsystems/SwerveModuleImpl.java → ...t/subsystems/swerve/SwerveModuleImpl.java
@@ -1,4 +1,4 @@
-package com.stuypulse.robot.subsystems;
+package com.stuypulse.robot.subsystems.swerve;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
@@ -20,6 +20,7 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
 public class SwerveModuleImpl extends SwerveModule {
+
     // data
     private final String id;
     private final Translation2d translationOffset;