diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3e30175..2781eb9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,12 +7,10 @@ package frc4388.robot; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - import edu.wpi.first.math.geometry.Translation2d; - -import frc4388.utility.LEDPatterns; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import frc4388.utility.Gains; +import frc4388.utility.LEDPatterns; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -40,10 +38,10 @@ public final class Constants { public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 222.7145469; - public static final double FRONT_RIGHT_ROT_OFFSET = 255.0464068;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; - public static final double BACK_LEFT_ROT_OFFSET = -277.4284884; - public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625; + public static final double FRONT_LEFT_ROT_OFFSET = 220; + public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -279.08349884; + public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656; } public static final class IDs { @@ -67,7 +65,7 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.1, 0.2, 0, 0.0); + public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); } public static final class AutoConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8e64dcf..8e8bcea 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -133,8 +133,8 @@ public class RobotContainer { ); private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) + // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( @@ -361,12 +361,12 @@ public class RobotContainer { .onFalse(new InstantCommand()); // ! /* Speed */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .whileTrue(new InstantCommand(() -> diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java new file mode 100644 index 0000000..86bc7b2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java @@ -0,0 +1,107 @@ +// package frc4388.robot.commands.Autos; + +// import java.io.File; +// import java.util.ArrayList; +// import java.util.HashMap; + +// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import frc4388.robot.commands.Swerve.JoystickPlayback; +// import frc4388.robot.commands.Swerve.neoJoystickPlayback; +// import frc4388.robot.subsystems.SwerveDrive; +// import frc4388.utility.controller.VirtualController; + +// public class neoPlaybackChooser { +// private final SendableChooser m_teamChosser = new SendableChooser(); +// private final SendableChooser m_possitionChosser = new SendableChooser(); +// private final SendableChooser m_autoNameChosser = new SendableChooser(); + +// private final SwerveDrive m_swerve; +// private final VirtualController[] m_controllers; +// // private final ArrayList> m_choosers = new ArrayList<>(); +// // private SendableChooser m_playback = null; +// private final ArrayList m_widgets = new ArrayList<>(); +// // private final HashMap m_commandPool = new HashMap<>(); + +// // private final File m_dir = new File("/home/lvuser/autos/"); +// // private int m_cmdNum = 0; + +// // commands +// private Command m_noAuto = new InstantCommand(); + +// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { +// m_teamChosser.addOption("Red", "red"); +// m_teamChosser.setDefaultOption("Blue", "blue"); +// m_teamChosser.addOption("Nuetral", "nuetral"); +// m_possitionChosser.addOption("AMP", "amp"); +// m_possitionChosser.setDefaultOption("Center", "center"); +// m_possitionChosser.addOption("Source", "source"); +// m_swerve = swerve; +// m_controllers = controllers; +// } + +// public neoPlaybackChooser addOption(String name, String option) { +// m_autoNameChosser.addOption(name, option); +// return this; +// } + +// // public PlaybackChooser buildDisplay() { +// // for (int i = 0; i < 10; i++) { +// // appendCommand(); +// // } +// // m_playback = m_choosers.get(0); +// // nextChooser(); + +// // // ! This does not work, why? +// // Shuffleboard.getTab("Auto Chooser") +// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) +// // .withPosition(4, 0); +// // return this; +// // } + +// // This will be bound to a button for the time being +// public void render() { +// // var chooser = new SendableChooser(); +// // // chooser.setDefaultOption("No Auto", m_noAuto); + +// // m_choosers.add(chooser); +// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") +// .add("Command: " + m_choosers.size(), chooser) +// .withSize(4, 1) +// .withPosition(0, m_choosers.size() - 1) +// .withWidget(BuiltInWidgets.kSplitButtonChooser) +// .withWidget(BuiltInWidgets.kComboBoxChooser); + + +// m_widgets.add(widget); +// } + +// // public void nextChooser() { +// // SendableChooser chooser = m_choosers.get(m_cmdNum++); + +// // String[] dirs = m_dir.list(); + +// // if(dirs != null){ // Fix funny error +// // for (String auto : dirs) { +// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); +// // } +// // } + + +// // for (var cmd_name : m_commandPool.keySet()) { +// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); +// // } +// // } + +// public String autoName() { +// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; +// } + +// public Command getCommand() { +// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); +// } +// } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 986ed14..e8d48e2 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -333,7 +333,7 @@ public class Intake extends SubsystemBase { resetArmPosition(); - SmartDashboard.putNumber("Pivot Position", getArmPos()); + //SmartDashboard.putNumber("Pivot Position", getArmPos()); smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index dc5e0e7..44ce9ad 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -55,8 +55,8 @@ public class Limelight extends SubsystemBase { isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; - SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); - SmartDashboard.putNumber("speakerDistance", distance); + //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); + //SmartDashboard.putNumber("speakerDistance", distance); } public Pose2d getPose() { diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 026e213..ba8ed12 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -99,7 +99,7 @@ public class Shooter extends SubsystemBase { // This method will be called once per scheduler run // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); - smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + //smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed. // Else if the robot is not near the speaker, then set the speed back to idle. diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d6ae2e4..f979565 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -218,7 +218,7 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); + // SmartDashboard.putNumber("Gyro", getGyroAngle()); } public void shiftDown() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index cfdd7ab..7e112f9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,17 +4,22 @@ package frc4388.robot.subsystems; +import javax.swing.text.StyleContext.SmallAttributeSet; + +import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; @@ -24,7 +29,8 @@ public class SwerveModule extends SubsystemBase { private WPI_TalonFX driveMotor; private WPI_TalonFX angleMotor; private CANCoder encoder; - //private ConfigurableDouble offsetGetter; + private int selfid; + private ConfigurableDouble offsetGetter; private static int swerveId = 0; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; @@ -33,7 +39,8 @@ public class SwerveModule extends SubsystemBase { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; - // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); + this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); + this.selfid = swerveId; swerveId++; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); angleConfig.slot0.kP = swerveGains.kP; @@ -45,17 +52,24 @@ public class SwerveModule extends SubsystemBase { angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleConfig); - + + //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + reset(0); encoder.configMagnetOffset(offset); driveMotor.setSelectedSensorPosition(0); driveMotor.config_kP(0, 0.2); } - //@Override - // public void periodic() { - // encoder.configMagnetOffset(offsetGetter.get()); - // } + @Override + public void periodic() { + //encoder.configMagnetOffset(offsetGetter.get()); + //SmartDashboard.putString("Error Code: " + selfid, getstuff()); + // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees()); + // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel()); + // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos()); + // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel()); + } /** * Get the drive motor of the SwerveModule * @return the drive motor of the SwerveModule @@ -165,4 +179,9 @@ public class SwerveModule extends SubsystemBase { public double getVoltage() { return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); } + + // public String getstuff() { + // encoder.getPosition(); + // return "" + encoder.getLastError().value; + // } }