diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 867bf46..0f7cee8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,12 +3,12 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import frc.robot.Commands.SequentialCommands; + import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Commands.WristCommand; +import frc.robot.commands.SequentialCommands; import frc.robot.subsystems.WristSubsystem; /** @@ -20,7 +20,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final WristSubsystem wristSubsystem = new WristSubsystem(); - private final SequentialCommands squentialCommand = new SequentialCommands(wristSubsystem); + private final SequentialCommands squentialCommand = new SequentialCommands(wristSubsystem); // private final WristCommand wristCommand = new WristCommand(); private final CommandXboxController driverB = new CommandXboxController(Constants.Wrist.DRIVER_CONTROLLER_PORT); @@ -32,7 +32,7 @@ public RobotContainer() { configureButtonBindings(); // } else if (driverB.leftTrigger().getAsBoolean()) { - + // driverB.y().onTrue(new WristCommand(wristSubsystem, 0)); // driverB.a().onTrue(new WristCommand(wristSubsystem, 20)); // driverB.b().onTrue(new WristCommand(wristSubsystem, 40)); diff --git a/src/main/java/frc/robot/commands/SequentialCommands.java b/src/main/java/frc/robot/commands/SequentialCommands.java index f8786f7..cc95aa9 100644 --- a/src/main/java/frc/robot/commands/SequentialCommands.java +++ b/src/main/java/frc/robot/commands/SequentialCommands.java @@ -2,22 +2,19 @@ // 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 frc.robot.Commands.WristCommand; -import frc.robot.subsystems.WristSubsystem; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.CommandBase; +package frc.robot.commands; + import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.WristSubsystem; public class SequentialCommands extends SequentialCommandGroup { /** Creates a new TestCommand. */ public SequentialCommands(WristSubsystem wristSubsystem) { // Use addRequirements() here to declare subsystem dependencies. addCommands( - new WristCommand(wristSubsystem, 0), - new WristCommand(wristSubsystem, 20), - new WristCommand(wristSubsystem, 40) - ); + new WristCommand(wristSubsystem, 0), + new WristCommand(wristSubsystem, 20), + new WristCommand(wristSubsystem, 40)); } // // Called when the command is initially scheduled. diff --git a/src/main/java/frc/robot/commands/WristCommand.java b/src/main/java/frc/robot/commands/WristCommand.java index 4c25887..52587e7 100644 --- a/src/main/java/frc/robot/commands/WristCommand.java +++ b/src/main/java/frc/robot/commands/WristCommand.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.WristSubsystem; diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java index a380e67..156ea89 100644 --- a/src/main/java/frc/robot/subsystems/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/WristSubsystem.java @@ -9,16 +9,16 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.Constants.Wrist; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.Wrist; public class WristSubsystem extends SubsystemBase { /** Creates a new WristSubsystem. */ private double targetAngle; + private final ShuffleboardTab WristTab = Shuffleboard.getTab("Wrist"); private double currentAngle; @@ -50,10 +50,12 @@ public WristSubsystem() { public static double degreesToTicks(double angle) { return angle * ((Wrist.GEAR_RATIO * Wrist.TICKS)); } - public void setTargetAngle(double targetAngle){ + + public void setTargetAngle(double targetAngle) { this.targetAngle = targetAngle; pidController.setSetpoint(targetAngle); } + public static double ticksToDegrees(double ticks) { return (ticks / (Wrist.TICKS * Wrist.GEAR_RATIO)); } @@ -62,11 +64,13 @@ private double getCurrentAngle() { currentAngle = ticksToDegrees(wrist_motor.getSelectedSensorPosition()); return currentAngle; } - public boolean nearTargetAngle(){ - if(targetAngle-0.5<=getCurrentAngle() && getCurrentAngle()<=targetAngle+0.5)return true; + + public boolean nearTargetAngle() { + if (targetAngle - 0.5 <= getCurrentAngle() && getCurrentAngle() <= targetAngle + 0.5) + return true; return false; + } - } @Override public void periodic() { // calculates motor power