Merge branch 'feat/wrist' of https://github.com/Iron-Panthers/2023-Of…

…fseason-Reboot into feat/wrist
Iron-Panthers · Nov 29, 2023 · 1f7619c · 1f7619c
2 parents 823ff57 + b869883
commit 1f7619c
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 21 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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