From 87f4f1da7ce2160bd90fdfb711a34ff184b6ce34 Mon Sep 17 00:00:00 2001 From: Yair Lavie Date: Sat, 20 Jan 2024 00:59:22 +0200 Subject: [PATCH 1/2] delayed proggress creating TrapArmSubsystem and goInSwitch and goOutSwitch and setSpeedCommands and creating TrapArmConstants --- src/main/java/frc/robot/Constants.java | 6 ++ .../TrapArmCommands/goInSwitchCommand.java | 42 +++++++++++++ .../TrapArmCommands/goOutSwitchCommand.java | 42 +++++++++++++ .../TrapArmCommands/setSpeedCommand.java | 39 ++++++++++++ .../robot/subsystems/TrapArmSubsystem.java | 60 +++++++++++++++++++ 5 files changed, 189 insertions(+) create mode 100644 src/main/java/frc/robot/commands/TrapArmCommands/goInSwitchCommand.java create mode 100644 src/main/java/frc/robot/commands/TrapArmCommands/goOutSwitchCommand.java create mode 100644 src/main/java/frc/robot/commands/TrapArmCommands/setSpeedCommand.java create mode 100644 src/main/java/frc/robot/subsystems/TrapArmSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6320e0f..ec31365 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -302,4 +302,10 @@ public static class ShooterConstants { .put(2.1, 10.2); } + public static class TrapArmConstants{ + public static final int ArmMotorID = 10; + public static final int InnerSwitchID= 0; + public static final int OuterSwitchID= 10; + + } } diff --git a/src/main/java/frc/robot/commands/TrapArmCommands/goInSwitchCommand.java b/src/main/java/frc/robot/commands/TrapArmCommands/goInSwitchCommand.java new file mode 100644 index 0000000..6cc2f80 --- /dev/null +++ b/src/main/java/frc/robot/commands/TrapArmCommands/goInSwitchCommand.java @@ -0,0 +1,42 @@ +// 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.TrapArmCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.TrapArmSubsystem; + +public class goInSwitchCommand extends Command { + private final TrapArmSubsystem trapArmSubsystem= TrapArmSubsystem.getInstance(); + /** Creates a new goInSwitch. */ + public goInSwitchCommand() { + addRequirements(trapArmSubsystem); + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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) { + trapArmSubsystem.setSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (trapArmSubsystem.getInnerSwitch()){ + return true; + } + else{ + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/TrapArmCommands/goOutSwitchCommand.java b/src/main/java/frc/robot/commands/TrapArmCommands/goOutSwitchCommand.java new file mode 100644 index 0000000..e14b880 --- /dev/null +++ b/src/main/java/frc/robot/commands/TrapArmCommands/goOutSwitchCommand.java @@ -0,0 +1,42 @@ +// 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.TrapArmCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.TrapArmSubsystem; + +public class goOutSwitchCommand extends Command { + private final TrapArmSubsystem trapArmSubsystem= TrapArmSubsystem.getInstance(); + /** Creates a new goOutSwitch. */ + public goOutSwitchCommand() { + addRequirements(trapArmSubsystem); + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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) { + trapArmSubsystem.setSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (trapArmSubsystem.getOuterSwitch()){ + return true; + } + else{ + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/TrapArmCommands/setSpeedCommand.java b/src/main/java/frc/robot/commands/TrapArmCommands/setSpeedCommand.java new file mode 100644 index 0000000..37e026f --- /dev/null +++ b/src/main/java/frc/robot/commands/TrapArmCommands/setSpeedCommand.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.commands.TrapArmCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.TrapArmSubsystem; + +public class setSpeedCommand extends Command { + private final TrapArmSubsystem trapArmSubsystem= TrapArmSubsystem.getInstance(); + double speed; + /** Creates a new setSpeedCommand. */ + public setSpeedCommand(double speed) { + addRequirements(trapArmSubsystem); + this.speed = speed; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + trapArmSubsystem.setSpeed(speed); + } + + // 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/TrapArmSubsystem.java b/src/main/java/frc/robot/subsystems/TrapArmSubsystem.java new file mode 100644 index 0000000..3929b5e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TrapArmSubsystem.java @@ -0,0 +1,60 @@ +// 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.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import static frc.robot.Constants.TrapArmConstants.*; +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class TrapArmSubsystem extends SubsystemBase { + private TalonSRX m_TrapMotor; + DigitalInput innerSwitch; + DigitalInput outerSwitch; + + private static TrapArmSubsystem instance; + + public static TrapArmSubsystem getInstance() { + if (instance == null) { + instance = new TrapArmSubsystem(); + } + return instance; + } + + /** Creates a new TrapArmSubsystem. */ + public TrapArmSubsystem() { + this.m_TrapMotor = new TalonSRX(ArmMotorID); + this.innerSwitch = new DigitalInput(InnerSwitchID); + this.outerSwitch = new DigitalInput(OuterSwitchID); + } + + + public void setSpeed(double speed){ + m_TrapMotor.set(ControlMode.PercentOutput,speed); + } + + + public boolean getInnerSwitch(){ + return innerSwitch.get(); + } + + + public boolean getOuterSwitch(){ + return outerSwitch.get(); + } + + + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 24ebcd0a2366e014d2945f5fedf64040b96e79c5 Mon Sep 17 00:00:00 2001 From: Yair Lavie Date: Sat, 20 Jan 2024 13:47:06 +0200 Subject: [PATCH 2/2] doing changes changing setSpeedCommand to instantCommand and returning in goOutSwitch and goInSwitch the switches and changing building action to private --- .../TrapArmCommands/goInSwitchCommand.java | 7 +------ .../TrapArmCommands/goOutSwitchCommand.java | 7 +------ .../TrapArmCommands/setSpeedCommand.java | 18 ++---------------- .../frc/robot/subsystems/TrapArmSubsystem.java | 6 +----- 4 files changed, 5 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/commands/TrapArmCommands/goInSwitchCommand.java b/src/main/java/frc/robot/commands/TrapArmCommands/goInSwitchCommand.java index 6cc2f80..11d7759 100644 --- a/src/main/java/frc/robot/commands/TrapArmCommands/goInSwitchCommand.java +++ b/src/main/java/frc/robot/commands/TrapArmCommands/goInSwitchCommand.java @@ -32,11 +32,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (trapArmSubsystem.getInnerSwitch()){ - return true; - } - else{ - return false; - } + return trapArmSubsystem.getInnerSwitch(); } } diff --git a/src/main/java/frc/robot/commands/TrapArmCommands/goOutSwitchCommand.java b/src/main/java/frc/robot/commands/TrapArmCommands/goOutSwitchCommand.java index e14b880..47990bc 100644 --- a/src/main/java/frc/robot/commands/TrapArmCommands/goOutSwitchCommand.java +++ b/src/main/java/frc/robot/commands/TrapArmCommands/goOutSwitchCommand.java @@ -32,11 +32,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (trapArmSubsystem.getOuterSwitch()){ - return true; - } - else{ - return false; - } + return trapArmSubsystem.getOuterSwitch(); } } diff --git a/src/main/java/frc/robot/commands/TrapArmCommands/setSpeedCommand.java b/src/main/java/frc/robot/commands/TrapArmCommands/setSpeedCommand.java index 37e026f..110af6b 100644 --- a/src/main/java/frc/robot/commands/TrapArmCommands/setSpeedCommand.java +++ b/src/main/java/frc/robot/commands/TrapArmCommands/setSpeedCommand.java @@ -4,10 +4,10 @@ package frc.robot.commands.TrapArmCommands; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.TrapArmSubsystem; -public class setSpeedCommand extends Command { +public class setSpeedCommand extends InstantCommand { private final TrapArmSubsystem trapArmSubsystem= TrapArmSubsystem.getInstance(); double speed; /** Creates a new setSpeedCommand. */ @@ -22,18 +22,4 @@ public setSpeedCommand(double speed) { public void initialize() { trapArmSubsystem.setSpeed(speed); } - - // 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; - } } diff --git a/src/main/java/frc/robot/subsystems/TrapArmSubsystem.java b/src/main/java/frc/robot/subsystems/TrapArmSubsystem.java index 3929b5e..abc8401 100644 --- a/src/main/java/frc/robot/subsystems/TrapArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TrapArmSubsystem.java @@ -6,11 +6,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; import static frc.robot.Constants.TrapArmConstants.*; -import com.ctre.phoenix.motorcontrol.ControlMode; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -30,7 +26,7 @@ public static TrapArmSubsystem getInstance() { } /** Creates a new TrapArmSubsystem. */ - public TrapArmSubsystem() { + private TrapArmSubsystem() { this.m_TrapMotor = new TalonSRX(ArmMotorID); this.innerSwitch = new DigitalInput(InnerSwitchID); this.outerSwitch = new DigitalInput(OuterSwitchID);