From d39914ecb3f8f76da85e69d8e0d48ce93986139a Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 11 Feb 2020 16:55:04 -0700 Subject: [PATCH] Make it Work --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/commands/PlaySongDrive.java | 9 +++++-- .../java/frc4388/robot/subsystems/Drive.java | 25 +++++++++++-------- .../frc4388/robot/subsystems/Storage.java | 4 +-- 5 files changed, 26 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 04e18c3..f4f8c2b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,7 +21,7 @@ public final class Constants { public static final class DriveConstants { /* Drive Train IDs */ public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; - public static final int DRIVE_RIGHT_FRONT_CAN_ID = 8; + public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; public static final int DRIVE_LEFT_BACK_CAN_ID = 3; public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; public static final int PIGEON_ID = 6; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b259b06..ac94b19 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -99,7 +99,7 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whileHeld(new PlaySongDrive(m_robotDrive)); + .whenPressed(new PlaySongDrive(m_robotDrive)); //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) // .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/PlaySongDrive.java b/src/main/java/frc4388/robot/commands/PlaySongDrive.java index 79eb580..5e0e6b4 100644 --- a/src/main/java/frc4388/robot/commands/PlaySongDrive.java +++ b/src/main/java/frc4388/robot/commands/PlaySongDrive.java @@ -25,13 +25,18 @@ public class PlaySongDrive extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - m_drive.playSong(); + m_drive.m_rightFrontMotor.set(0); + m_drive.m_leftFrontMotor.set(0); + m_drive.m_rightBackMotor.set(0); + m_drive.m_leftBackMotor.set(0); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.err.println("Playing " + m_drive.m_orchestra.isPlaying()); + m_drive.playSong(); + //System.err.println("Playing " + m_drive.m_orchestra.isPlaying()); + //m_drive.m_driveTrain.feedWatchdog(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 7ad5551..9bcf610 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -56,7 +56,7 @@ public class Drive extends SubsystemBase { public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); public Orchestra m_orchestra = new Orchestra(); - public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + //public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -91,7 +91,7 @@ public class Drive extends SubsystemBase { /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); m_rightFrontMotor.setInverted(true); - m_driveTrain.setRightSideInverted(false); + //m_driveTrain.setRightSideInverted(false); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); @@ -239,10 +239,10 @@ public class Drive extends SubsystemBase { */ m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS); - //m_orchestra.addInstrument(m_leftBackMotor); - //m_orchestra.addInstrument(m_leftFrontMotor); - //m_orchestra.addInstrument(m_rightBackMotor); + m_orchestra.addInstrument(m_leftBackMotor); m_orchestra.addInstrument(m_rightFrontMotor); + m_orchestra.addInstrument(m_rightBackMotor); + m_orchestra.addInstrument(m_leftFrontMotor); File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs"); System.err.println(songsDir.getPath()); @@ -253,6 +253,7 @@ public class Drive extends SubsystemBase { Shuffleboard.getTab("Songs").add(m_songChooser); } + String currentSong = ""; @Override public void periodic() { try { @@ -279,7 +280,11 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - selectSong(m_songChooser.getSelected()); + if (currentSong != m_songChooser.getSelected()){ + currentSong = m_songChooser.getSelected(); + selectSong(currentSong); + System.err.println(currentSong); + } } catch (Exception e) { System.err.println("Error in the Drive Subsystem"); //e.printStackTrace(System.err); @@ -348,7 +353,7 @@ public class Drive extends SubsystemBase { * Add your docs here. */ public void driveWithInput(double move, double steer){ - m_driveTrain.arcadeDrive(move, steer); + //m_driveTrain.arcadeDrive(move, steer); } /** @@ -364,7 +369,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_driveTrain.feedWatchdog(); + //m_driveTrain.feedWatchdog(); } /** @@ -380,7 +385,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_driveTrain.feedWatchdog(); + //m_driveTrain.feedWatchdog(); } /** @@ -395,7 +400,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_driveTrain.feedWatchdog(); + //m_driveTrain.feedWatchdog(); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index a7d1bfb..e94f238 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -47,9 +47,9 @@ public class Storage extends SubsystemBase { boolean beam_on = m_beamSensors[0].get(); if (beam_on) { - System.err.println("Beam on"); + //System.err.println("Beam on"); } else { - System.err.println("Beam off"); + //System.err.println("Beam off"); } }