Merge pull request #40 from CyberCoyotes/W2-FlywheelAndOrchestra

W2 flywheel and orchestra

src/main/java/frc/robot/Constants.java

@@ -16,4 +16,9 @@ public final class Constants {
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
   }
+  public static class SystemConstants {
+    public static final int leftFlywheelCAN = 16;
+    public static final int rightFlywheelCAN = 15;
+    public static final int bassGuitar = 7;
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -5,59 +5,62 @@
 package frc.robot;
 
 import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.DualFlyWheelSubsystem;
+import frc.robot.subsystems.OrchestraSubsystem;
+import frc.robot.subsystems.OrchestraSubsystem.Song;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-
-/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
- * subsystems, commands, and trigger mappings) should be declared here.
- */
+
 public class RobotContainer {
-  // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
 
-  // Replace with CommandPS4Controller or CommandJoystick if needed
+  //#region Devices 
+  TalonFX flywheelLeft, flywheelRight, bassGuitar;   
+  //#endregion
+
+  //#region Subsystems
+  DualFlyWheelSubsystem flywheel;
+  OrchestraSubsystem daTunes;
+  //#endregion Subsystems
+
   private final CommandXboxController m_driverController =
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
+    new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+
+    flywheelLeft = new TalonFX(Constants.SystemConstants.leftFlywheelCAN);
+    flywheelRight = new TalonFX(Constants.SystemConstants.rightFlywheelCAN);
+    //Orchestra
+    bassGuitar = new TalonFX(Constants.SystemConstants.bassGuitar);
+
+    flywheel = new DualFlyWheelSubsystem(flywheelLeft, flywheelRight);
+    flywheel.SetStatePower(0.2);
+    flywheel.SetRatio(-1);
+
+    daTunes = new OrchestraSubsystem(new TalonFX[]{bassGuitar});
+    daTunes.SetTune(Song.ONE_ONE_FIVE );
+
+
     // Configure the trigger bindings
     configureBindings();
   }
 
-  /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
-   * predicate, or via the named factories in {@link
-   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
-   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
-   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
-   * joysticks}.
-   */
+  // tl;dr: Trigger class for simple booleans
   private void configureBindings() {
-    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    new Trigger(m_exampleSubsystem::exampleCondition)
-        .onTrue(new ExampleCommand(m_exampleSubsystem));
-
-    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
-    // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
+
+    //WOW This is bad but oh well
+    m_driverController.y().onTrue(new InstantCommand( () -> flywheel.Toggle(), flywheel));
+    m_driverController.b().onTrue(new InstantCommand( () -> daTunes.Play(), daTunes));
+    m_driverController.a().onTrue(new InstantCommand( () -> daTunes.Shud(), daTunes));
+
+
   }
-
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return Autos.exampleAuto(m_exampleSubsystem);
+    return new WaitCommand(2);
   }
 }

src/main/java/frc/robot/subsystems/DualFlyWheelSubsystem.java

@@ -0,0 +1,69 @@
+package frc.robot.subsystems;
+import com.ctre.phoenix6.*;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** Serves as a base for any flywheel system driven by 2 motors. Fire and forget, enabling and diabling will be done by singular calls.\
+ * Set configs before creating this class/subsystem.
+ * {@link #SetStatePower} is used to set the main power.
+ */
+public class DualFlyWheelSubsystem extends SubsystemBase {
+    private TalonFX m_main;
+    private TalonFX m_sub;
+    /**The state percentage */
+    private double percentage;
+    /**The multiplier that converts from "primary speed" to "secondary speed"*/
+    private double ratio = 1;
+    private DutyCycleOut mainDutyCycle;
+    private DutyCycleOut subDutyCycle;
+
+    public DualFlyWheelSubsystem(TalonFX main, TalonFX sub) {
+        this.m_main = main;
+        this.m_sub = sub;
+        //default to off
+        m_main.setControl(mainDutyCycle = new DutyCycleOut(0));
+        m_sub.setControl(subDutyCycle = new DutyCycleOut(0)); 
+    }
+    /**
+     * Sets the power the primary motor will use. Does not enable or disable.
+     */
+    private void SetMotorPowers(double arg)
+    {
+        mainDutyCycle.Output = arg;
+        subDutyCycle.Output = -arg * ratio;
+
+        m_main.setControl(mainDutyCycle);
+        m_sub.setControl(subDutyCycle);
+    }
+    /**
+     *Sets the multiplier that converts from "primary speed" to "secondary speed". Inversion is automatic; supplying a value of -1 here will make both motors identical.
+     */
+    public void SetRatio(double arg)
+    {
+       ratio = arg;
+    }
+    public void SetStatePower(double percent)
+    {
+        // percent = Math.max(0, Math.min(percent,1));
+        this.percentage = percent;
+        mainDutyCycle = new DutyCycleOut(percentage);
+    }
+    public void Enable()
+    {
+       SetMotorPowers(percentage);
+    }
+    public void Disable()
+    {
+       SetMotorPowers(0);
+    }
+    public void Toggle()
+    {
+        double set =  Math.abs( mainDutyCycle.Output - percentage);
+       SetMotorPowers(set);
+    }
+}

src/main/java/frc/robot/subsystems/OrchestraSubsystem.java

@@ -0,0 +1,46 @@
+package frc.robot.subsystems;
+
+import java.util.ArrayList;
+import com.ctre.phoenix6.Orchestra;
+import com.ctre.phoenix6.controls.MusicTone;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class OrchestraSubsystem extends SubsystemBase{
+
+ArrayList<TalonFX> instruments;
+Orchestra m_orchestra;
+
+public OrchestraSubsystem(TalonFX[] motors) {
+
+m_orchestra = new Orchestra();
+//add instruments
+    for (int i = 0; i < motors.length; i++ ) {
+     m_orchestra.addInstrument(motors[i], i);
+     motors[i].setControl(new MusicTone(i));
+    }}
+
+// Attempt to load the chrp
+
+public void SetTune(Song song)
+{
+
+    m_orchestra.loadMusic(song.name()+ ".chrp");
+}
+public void Play()
+{
+    m_orchestra.play();
+}
+public void Shud()
+{
+    m_orchestra.stop();
+}
+
+public enum Song{
+ICE_CREAM,
+E1M1,
+ONE_ONE_FIVE,
+ULTRA_INSTINCT,
+SANS
+}
+}