Added Commands and Subsystems

src/main/java/frc/robot/commands/DriveCommand.java

@@ -8,52 +8,47 @@
 
 import java.util.function.DoubleSupplier;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 /** An example command that uses an example subsystem. */
 public class DriveCommand extends Command {
   @SuppressWarnings("PMD.UnusedPrivateField")
-  private final DifferentialDrive diffDrive;
-  private final DoubleSupplier dumpSumpForward;
-  private final DoubleSupplier dumpSumpRight;
-
-
+  //private final Subsystems m_subsystem;
+  private final DifferentialDrive tankDrive;
+  private final DoubleSupplier rightSupplier;
+  private final DoubleSupplier forwardSupplier;
   /**
    * Creates a new ExampleCommand.
    *
-   * @param DifferentialDrive The subsystem used by this command.
+   * @param subsystem The subsystem used by this command.
    */
-  public DriveCommand(DifferentialDrive diffDrive, DoubleSupplier dumpSumpForward, DoubleSupplier dumpSumpRight) {
-    this.diffDrive = diffDrive;
-    this.dumpSumpForward = dumpSumpForward;
-    this.dumpSumpRight = dumpSumpRight;
-
+  public DriveCommand (DifferentialDrive drive, DoubleSupplier right, DoubleSupplier forward) {
+    tankDrive = drive;
+    rightSupplier = right;
+    forwardSupplier = forward;
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(diffDrive);
+    addRequirements(drive);
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() { 
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    diffDrive.drive(dumpSumpForward.getAsDouble(), dumpSumpRight.getAsDouble());
-    SmartDashboard.putBoolean("DriveTrain Active: ", true);
+    tankDrive.drive(forwardSupplier.getAsDouble(), rightSupplier.getAsDouble());
+
   }
+
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {
-  diffDrive.drive(0,0);
-    SmartDashboard.putBoolean("DriveTrain Active: ", false);
-}
+  public void end(boolean interrupted) {}
+
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-
-
     return true;
   }
-}
+}

src/main/java/frc/robot/commands/IntakeCommand.java

@@ -1,47 +1,43 @@
-package frc.robot.commands;
-import frc.robot.subsystems.Intake;
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-import java.util.function.BooleanSupplier;
+package frc.robot.commands;
 
+import frc.robot.subsystems.Subsystems;
 import edu.wpi.first.wpilibj2.command.Command;
 
-public class IntakeCommand extends Command {
-    private final Intake m_subsystem;
-    private final boolean isOut;
-    private BooleanSupplier finished;
-
-    public IntakeCommand(Intake m_subsystem, boolean isOut, BooleanSupplier finished){
-        this.m_subsystem = m_subsystem;
-        this.isOut = isOut;
-        this.finished = finished;
-        addRequirements(this.m_subsystem);
-    }
-    @Override
-    public void execute() {
-        if (isOut) {
-            executeOuttake();
-        } else {
-            executeIntake();
-        }
-    }
-
-    @Override
-    public boolean isFinished() {
-        return finished.getAsBoolean();
-    }
-
-    @Override
-    public void end(boolean isInterrupted) {
-        m_subsystem.stopIntake();
-    }
-    public void executeIntake() {
-        m_subsystem.intakeFuel();
-    }
-    public void executeOuttake() {
-        m_subsystem.outtakeFuel();
-    }
-
-    public void stop() {
-        m_subsystem.stopIntake();
-    }
+/** An example command that uses an example subsystem. */
+public class Commands extends Command {
+  @SuppressWarnings("PMD.UnusedPrivateField")
+  private final Subsystems m_subsystem;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public Commands(Subsystems subsystem) {
+    m_subsystem = subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(subsystem);
+  }
+
+  // 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() {}
+
+  // 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 false;
+  }
 }

src/main/java/frc/robot/commands/ShooterCommand.java

@@ -4,54 +4,40 @@
 
 package frc.robot.commands;
 
-import frc.robot.subsystems.Shooter;
-import frc.robot.subsystems.Intake;
-
-import java.util.function.BooleanSupplier;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.subsystems.Subsystems;
 import edu.wpi.first.wpilibj2.command.Command;
 
 /** An example command that uses an example subsystem. */
-public class ShooterCommand extends Command {
+public class Commands extends Command {
   @SuppressWarnings("PMD.UnusedPrivateField")
-  private final Shooter shooter;
-  private final Intake intake;
-  private final BooleanSupplier finished;
+  private final Subsystems m_subsystem;
+
   /**
    * Creates a new ExampleCommand.
    *
-   * @param Shooter The subsystem used by this command.
+   * @param subsystem The subsystem used by this command.
    */
-  public ShooterCommand(Shooter shooter, BooleanSupplier finished, Intake intake) {
-    this.shooter = shooter;
-    this.finished = finished;
-    this.intake = intake;
-    addRequirements(shooter); // Use addRequirements() here to declare subsystem dependencies.
+  public Commands(Subsystems subsystem) {
+    m_subsystem = subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(subsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-    intake.intakeFuel();
-    shooter.shootFuel();
-    SmartDashboard.putBoolean("Shooter Active: ", true);
-  }
+  public void execute() {}
+
+  // 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 finished.getAsBoolean();
-  }
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    shooter.stopShooter();
-    intake.stopIntake();
-    SmartDashboard.putBoolean("Shooter Active: ", false);
+    return false;
   }
 }

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

@@ -23,102 +23,30 @@
 
 
 public class DifferentialDrive extends SubsystemBase {
-  public final TalonFX frontLeft = new TalonFX(MotorIDs.kMotorFrontLeft);
-  public final TalonFX frontRight = new TalonFX(MotorIDs.kMotorFrontRight);
-  public final TalonFX backLeft = new TalonFX(MotorIDs.kMotorBackLeft);
-  public final TalonFX backRight = new TalonFX(MotorIDs.kMotorBackRight);
-
-  public final CANcoder leftEncoder = new CANcoder(SensorIDs.kEncoderLeftDriveBase, SensorIDs.kCANivore);
-  public final CANcoder rightEncoder = new CANcoder(SensorIDs.kEncoderRightDriveBase, SensorIDs.kCANivore);
-
-public final VelocityDutyCycle closedloop = new VelocityDutyCycle(0).withEnableFOC(false);
+  //0s not real - change later
+  public final TalonFX frontLeftMotor = new TalonFX(0);
+  public final TalonFX backLeftMotor = new TalonFX(0);
+  public final TalonFX frontRightMotor = new TalonFX(0);
+  public final TalonFX backRightMotor = new TalonFX(0);
 
-private final PigeonWrapper pigeon;
 
-  public DifferentialDrive(PigeonWrapper pigeon) {
-
-    this.pigeon = pigeon;
-
-    pigeon.getYaw();
-
-    TalonFXConfiguration driveConfig = new TalonFXConfiguration();
-
-    //PID value assignment
-    driveConfig.Slot0.kP = Constants.PIDConstants.Drive.kDriveP;
-    driveConfig.Slot0.kI = Constants.PIDConstants.Drive.kDriveI;
-    driveConfig.Slot0.kD = Constants.PIDConstants.Drive.kDriveD;
-    driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-
-    // Assigning encoder as feedback device
-    driveConfig.Feedback.FeedbackRemoteSensorID = leftEncoder.getDeviceID();
-    driveConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
-
-    frontLeft.getConfigurator().apply(driveConfig);
-    frontRight.getConfigurator().apply(driveConfig);
-    backLeft.getConfigurator().apply(driveConfig);
-    backRight.getConfigurator().apply(driveConfig);
-  }
-
-  // leftDrive controls the speed of the left side of the tank bot
- private void leftDrive(double speed){
-
-  //checking for speed in range
-  speed = MathUtil.clamp(speed, DriveConstants.kMinDriveVelocity, DriveConstants.kMaxDriveVelocity);
-
-  frontLeft.setControl(closedloop.withVelocity(speed * DriveConstants.kMaxDriveVoltage));
-  backLeft.setControl(closedloop.withVelocity(speed * DriveConstants.kMaxDriveVoltage));
-  SmartDashboard.putNumber("Left DriveTrain Speed: ", speed);
- }
-
-//We Love mr. skibidihorn and fvelocity - Zack philopino boy.
-
-// rightDrive controls the speed of the right side of the tank bot
- private void rightDrive(double speed){
-
-  //checking for speed in range
-  speed = MathUtil.clamp(speed, DriveConstants.kMinDriveVelocity, DriveConstants.kMaxDriveVelocity);
-
-  frontRight.setControl(closedloop.withVelocity(speed * DriveConstants.kMaxDriveVoltage));
-  backRight.setControl(closedloop.withVelocity(speed * DriveConstants.kMaxDriveVoltage));
-  SmartDashboard.putNumber("Right DriveTrain Speed: ", speed);
- }
-
- /**
-  * this is a helper method for the controller
-  * @param forward
-  * @param right
-  */
-  //drive controls the direction the tank bot moves
- public void drive(double forward, double right){
-  double leftSpeed = forward + right;
-  double rightSpeed = forward - right;
-  leftDrive(leftSpeed);
-  rightDrive(rightSpeed);
-}
-public void end(){
-  rightDrive(0);
-  leftDrive(0);
-}
-
-public Rotation2d getYaw(){
-  if (RobotBase.isSimulation){
-    return Rotation2d.fromRadians(simHeading);
-  }
-  return pigeon.getRotation2d();
+  public DifferentialDrive() {
+    leftSide.set(0);
+    rightSide.set(0);
 }
 
-public double getHeading(){
-  return pigeon.getYaw().getAsDouble();
-}
-
-public void resetPigeon(){
-  pigeon.setYaw(0);
-}
-/*
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-
-  }
-*/
-}
+private void leftSide(double speed){
+      frontLeftMotor.set(speed);
+      backLeftMotor.set(speed);
+    }
+    private void rightSide(double speed){
+      frontRightMotor.set(speed);
+      backRightMotor.set(speed);
+    }
+    public void drive(double forward, double right){
+      double rightSpeed = forward + right;
+      double leftSpeed = forward - right;
+      rightSide(Math.clamp(rightSpeed, -1.00,  1.00));
+      leftSide(Math.clamp(leftSpeed,  -1.00,  1.00));
+    }
+}