Merge pull request #11 from primo4586/ClimbingAndTrap

Climbing and trap

src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedLeftMotorClimbing.java

@@ -0,0 +1,28 @@
+// 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.
+
+package frc.robot.commands.ClimbingCommands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.ClimbingSubsystem;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class SetSpeedLeftMotorClimbing extends InstantCommand {
+  private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance();
+  double speed;
+  public SetSpeedLeftMotorClimbing(double speed) {
+      this.addRequirements(climbingSubsystem);
+      this.speed = speed;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.climbingSubsystem.setSpeedClimbing(speed);
+
+  }
+}

src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedMotorsClimbing.java

@@ -0,0 +1,28 @@
+// 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.
+
+package frc.robot.commands.ClimbingCommands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.ClimbingSubsystem;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class SetSpeedMotorsClimbing extends InstantCommand {
+  private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance();
+  double speed;
+  public SetSpeedMotorsClimbing(double speed) {
+    this.addRequirements(climbingSubsystem);
+    this.speed = speed;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.climbingSubsystem.setSpeedClimbing(speed);
+
+  }
+}

src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedRightMotorClimbing.java

@@ -0,0 +1,29 @@
+// 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.
+
+package frc.robot.commands.ClimbingCommands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.ClimbingSubsystem;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class SetSpeedRightMotorClimbing extends InstantCommand {
+  private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance();
+  double speed;
+  public SetSpeedRightMotorClimbing(double speed) {
+    this.addRequirements(climbingSubsystem);
+    this.speed = speed;
+
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.climbingSubsystem.setSpeedClimbing(speed);
+
+  }
+}

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

@@ -0,0 +1,30 @@
+// 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.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.SubsystemTrap;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class TrapSetSpeed extends InstantCommand {
+  private final SubsystemTrap subsystemTrap = SubsystemTrap.getInstance();
+  double speed;
+  public TrapSetSpeed(double speed) {
+    this.addRequirements(subsystemTrap);
+    this.speed = speed;
+
+
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.subsystemTrap.setSpeed(speed);
+
+  }
+}

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

@@ -0,0 +1,50 @@
+// 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.
+
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import static frc.robot.Constants.climbingConstants.*;
+
+public class ClimbingSubsystem extends SubsystemBase {
+  /** Creates a new ClimbingSubsystem. */
+  private CANSparkMax m_climbingRight;
+  private CANSparkMax m_climbingLeft;
+
+  private static ClimbingSubsystem instance;
+
+  public static ClimbingSubsystem getInstance() {
+    if (instance == null) {
+      instance = new ClimbingSubsystem();
+    }
+    return instance;
+  }
+
+  private ClimbingSubsystem() {
+    m_climbingLeft = new CANSparkMax(M_CLIMBINGLEFT_MOTOR_ID, MotorType.kBrushless);
+    m_climbingRight = new CANSparkMax(M_CLIMBINGRIGHT_MOTOR_ID, MotorType.kBrushless);
+
+  }
+
+  public void setSpeedClimbing(double speed) {
+    m_climbingLeft.set(speed);
+    m_climbingRight.set(speed);
+  }
+
+  public void setRightSpeed(double speed) {
+    m_climbingRight.set(speed);
+  }
+
+  public void setLeftSpeed(double speed) {
+    m_climbingLeft.set(speed);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

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

@@ -0,0 +1,39 @@
+// 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.
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import static frc.robot.Constants.trapConstants.*;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SubsystemTrap extends SubsystemBase {
+  private WPI_TalonSRX m_trapMotor;
+
+  private static SubsystemTrap instance;
+
+  public static SubsystemTrap getInstance() {
+    if (instance == null) {
+      instance = new SubsystemTrap();
+    }
+    return instance;
+  }
+
+  /** Creates a new SubsystemTrap. */
+  private SubsystemTrap() {
+    m_trapMotor = new WPI_TalonSRX(TRAP_MOTOR_ID);
+
+  }
+
+  public void setSpeed(double speed) {
+    m_trapMotor.set(speed);
+
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}