highlanders fin

src/main/java/frc4388/robot/RobotContainer.java

@@ -49,6 +49,7 @@ import frc4388.robot.commands.climber.RunLevelerWithJoystick;
 import frc4388.robot.commands.drive.DriveStraightToPositionMM;
 import frc4388.robot.commands.drive.DriveWithJoystick;
 import frc4388.robot.commands.drive.PlaySongDrive;
+import frc4388.robot.commands.drive.SkipSong;
 import frc4388.robot.commands.drive.TurnDegrees;
 import frc4388.robot.commands.intake.RunIntakeWithTriggers;
 import frc4388.robot.commands.shooter.CalibrateShooter;
@@ -136,7 +137,7 @@ public class RobotContainer {
     public RobotContainer() {
         /* Passing Drive and Pneumatics Subsystems */
         m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
-        m_robotDrive.passRequiredSubsystem(m_robotPneumatics);
+        m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter);
 
         m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
         m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
@@ -184,8 +185,8 @@ public class RobotContainer {
     private void configureButtonBindings() {
         /* Test Buttons */
         // A driver test button
-        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new InstantCommand(() -> m_robotDrive.tankDriveVelocity(1, -1), m_robotDrive));
+        /*new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+            .whileHeld(new InstantCommand(() -> m_robotDrive.tankDriveVelocity(1, -1), m_robotDrive));*/
 
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
@@ -289,7 +290,7 @@ public class RobotContainer {
 
         // Meg
         new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH)
-            .whenPressed(new PlaySongDrive(m_robotDrive))
+            .whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter))
             .whenReleased(new InterruptSubystem(m_robotDrive));
 
         // Shooter Manual
@@ -298,22 +299,21 @@ public class RobotContainer {
             .whenReleased(new ShooterManual(false));
 
         // Goal Shooter Position
-        new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON)
-            .whileHeld(new ShooterGoalPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
-            .whenReleased(new InterruptSubystem(m_robotShooter))
-            .whenReleased(new InterruptSubystem(m_robotShooterHood))
-            .whenReleased(new InterruptSubystem(m_robotShooterAim));
+        new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON)
+            .whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter))
+            .whenReleased(new InterruptSubystem(m_robotDrive));
 
         // Trench Shooter Position
-        new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON)
+        new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON)
             .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
             .whenReleased(new InterruptSubystem(m_robotShooter))
             .whenReleased(new InterruptSubystem(m_robotShooterHood))
             .whenReleased(new InterruptSubystem(m_robotShooterAim));
+            //.whenPressed(new SkipSong(m_robotDrive, 1));
     }
 
     public void buildAutos() {
-        resetOdometry(new Pose2d(0, 0, new Rotation2d(180)));
+        //resetOdometry(new Pose2d(0, 0, new Rotation2d(180)));
 
         String[] sixBallAutoMiddlePaths = new String[]{
             "SixBallMidComplete"

src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java

@@ -94,7 +94,7 @@ public class DriveWithJoystick extends CommandBase {
     } 
   */
     
-    m_drive.driveWithInput(moveOutput, steerOutput);
+    m_drive.driveWithInput(-moveOutput, steerOutput);
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java

@@ -9,6 +9,7 @@ package frc4388.robot.commands.drive;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Shooter;
 
 public class PlaySongDrive extends CommandBase {
   private Drive m_drive;
@@ -16,10 +17,10 @@ public class PlaySongDrive extends CommandBase {
   /**
    * Creates a new PlaySongDrive.
    */
-  public PlaySongDrive(Drive subsystem) {
+  public PlaySongDrive(Drive subsystem, Shooter shooter) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    addRequirements(m_drive);
+    addRequirements(m_drive, shooter);
   }
 
   // Called when the command is initially scheduled.

src/main/java/frc4388/robot/commands/drive/SkipSong.java

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Drive;
+
+public class SkipSong extends CommandBase {
+  Drive m_drive;
+  int m_index;
+  
+  /**
+   * Creates a new SkipSong.
+   */
+  public SkipSong(Drive m_robotDrive, int index) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = m_robotDrive;
+    m_index = index;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    String[] songs = m_drive.songsStrings;
+    String song = m_drive.m_currentSong;
+
+    for (int i = 0; i < songs.length; i++) {
+      if (songs[i] == song) {
+        m_drive.selectSong(songs[i + m_index]);
+        break;
+      }
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -54,6 +54,7 @@ public class Drive extends SubsystemBase {
 
   /* Pneumatics Subsystem */
   public Pneumatics m_pneumaticsSubsystem;
+  Shooter m_shooter;
 
   /* Low Gear Gains */
   public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
@@ -96,7 +97,8 @@ public class Drive extends SubsystemBase {
   SendableChooser<String> m_songChooser = new SendableChooser<String>();
 
   /* Misc */
-  String m_currentSong = "";
+  public String m_currentSong = "";
+  public String[] songsStrings;
 
   /**
    * Add your docs here.
@@ -283,11 +285,12 @@ public class Drive extends SubsystemBase {
     /* Create chooser to choose song to play */
     File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
     System.err.println(songsDir.getPath());
-    String[] songsStrings = songsDir.list();
+    songsStrings = songsDir.list();
     for (String songString : songsStrings) {
       m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
     }
     Shuffleboard.getTab("Songs").add(m_songChooser);
+    selectSong(songsStrings[0]);
 
     /* Start counting time */
     m_lastTimeMs = System.currentTimeMillis();
@@ -306,8 +309,10 @@ public class Drive extends SubsystemBase {
    * 
    * @param subsystem Subsystem needed.
    */
-  public void passRequiredSubsystem(Pneumatics subsystem) {
+  public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) {
     m_pneumaticsSubsystem = subsystem;
+    m_shooter = shooter;
+    m_orchestra.addInstrument(m_shooter.m_shooterFalcon);
   }
 
   public void updateTime() {
@@ -913,6 +918,7 @@ public class Drive extends SubsystemBase {
       if (m_currentSong != m_songChooser.getSelected()){
         m_currentSong = m_songChooser.getSelected();
         selectSong(m_currentSong);
+
         //System.err.println(m_currentSong);
       }
     } catch (Exception e) {
@@ -920,4 +926,6 @@ public class Drive extends SubsystemBase {
       // e.printStackTrace(System.err);
     }
   }
+
+
 }

src/main/java/frc4388/utility/controller/JoystickManualButton.java

@@ -34,6 +34,7 @@ public class JoystickManualButton extends Button {
 
     m_joystick = joystick;
     m_buttonNumber = buttonNumber;
+    m_buttonType = buttonType;
   }
 
   /**