diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b952508..ab8a69f 100644 --- 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 index e1e56e6..6c31820 100644 --- 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 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); + } - }*/ } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java similarity index 97% rename from src/main/java/com/stuypulse/robot/subsystems/SwerveDrive.java rename to src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index b186d5c..d48251a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/SwerveDrive.java +++ b/src/main/java/com/stuypulse/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,7 +41,6 @@ public static SwerveDrive getInstance() { } private final SwerveModuleImpl[] modules; - private final FieldObject2d[] moduleObjects; private final SwerveDriveKinematics kinematics; private final AHRS gyro; @@ -48,7 +48,6 @@ 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/src/main/java/com/stuypulse/robot/subsystems/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModule.java similarity index 84% rename from src/main/java/com/stuypulse/robot/subsystems/SwerveModule.java rename to src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModule.java index c35f3a0..a3a9d6e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/SwerveModule.java +++ b/src/main/java/com/stuypulse/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/src/main/java/com/stuypulse/robot/subsystems/SwerveModuleImpl.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModuleImpl.java similarity index 98% rename from src/main/java/com/stuypulse/robot/subsystems/SwerveModuleImpl.java rename to src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModuleImpl.java index 158ac77..2a3ff34 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/SwerveModuleImpl.java +++ b/src/main/java/com/stuypulse/robot/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;