Merge remote-tracking branch 'origin/dev-2' into feat/intake2

Iron-Panthers · Nov 28, 2023 · c750031 · c750031
2 parents 8c27917 + a1e5bb3
commit c750031
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 30 deletions.
diff --git a/src/main/java/frc/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/Subsystems/ElevatorSubsystem.java
@@ -69,7 +69,7 @@ public ElevatorSubsystem() {
 
 
    motorPower = 0;
- pidController = new PIDController(0.4,0, 0.01);
+ pidController = new PIDController(0.34, 0, 0.02);
  //pidController.setTolerance(0.7,0.001);
    ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
    ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
@@ -111,12 +111,10 @@ public void setTargetHeight(double targetHeight) {
      pidController.setSetpoint(this.targetHeight); } 
 
    public double getCurrentHeight(){
-
-
      return ticksToInches(-left_motor.getSelectedSensorPosition());
    }
 public boolean nearTargetHeight(){
-   if(targetHeight-0.5<=getCurrentHeight() && getCurrentHeight()<=targetHeight+0.5)return true;
+   if(targetHeight - 0.5 <= getCurrentHeight() && getCurrentHeight() <= targetHeight + 0.5)return true;
    return false;
  }
  @Override
@@ -125,20 +123,18 @@ public void periodic() {
    motorPower = pidController.calculate(getCurrentHeight());
    if (!pidController.atSetpoint()){
      if (getCurrentHeight()<5){
-       left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower+0.02,-0.2,0.2)));
+       left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.2,0.2)));
      }
      else{
-       left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower+0.02,-0.5,0.5)));
+       left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.5,0.5)));
      }
-
    }
-
    // left_motor.set(TalonFXControlMode.PercentOutput, -(0.1));
  }
 
 
  @Override
  public void simulationPeriodic() {
    // This method will be called once per scheduler run during simulation
- }
+   }
  }
diff --git a/src/main/java/frc/robot/Commands/ElevatorBaseCommand b/src/main/java/frc/robot/Commands/ElevatorBaseCommand
@@ -14,16 +14,16 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
 public class ElevatorBaseCommand extends CommandBase {
  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
  private final ElevatorSubsystem elevatorSubsystem;
-public double targetHight;
+public double targetHeight;
 
 
  /**
   * Creates a new ExampleCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
- public ElevatorBaseCommand(ElevatorSubsystem elevatorSubsystem, double targetHight) {
-   this.targetHight = targetHight;
+ public ElevatorBaseCommand(ElevatorSubsystem elevatorSubsystem, double targetHeight) {
+   this.targetHeight = targetHeight;
    this.elevatorSubsystem = elevatorSubsystem;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(elevatorSubsystem);
@@ -33,7 +33,7 @@ public double targetHight;
  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
-   elevatorSubsystem.setTargetHeight(targetHight);
+   elevatorSubsystem.setTargetHeight(targetHeight);
 
 
 

diff --git a/src/main/java/frc/robot/Commands/ManualArmCommand b/src/main/java/frc/robot/Commands/ManualArmCommand
@@ -41,7 +41,7 @@ private DoubleSupplier speedSupplier;
  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
-double speed =speedSupplier.getAsDouble();
+double speed = speedSupplier.getAsDouble();
 elevatorSubsystem.setMotorPower(speed);
 
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -25,36 +25,23 @@ public RobotContainer() {
 
 
  private final CommandXboxController driverA = new CommandXboxController(0);
- private double targetHight;
+ private double targetHeight;
 
 
  /** The container for the robot. Contains subsystems, OI devices, and commands. */
  public RobotContainer() {
 
 
-   double[] levels={5d,10d,17d,8d,4d,7d,1d};
+   double[] levels = {5d,10d,17d,8d,4d,7d,1d};
 
 
    for(double i :levels){
-     targetHight = i;
+     targetHeight = i;
    }
 
 
    // Configure the button bindings
    configureButtonBindings();
-
-
-
-
-   driverA.rightTrigger().onTrue(
-     new ElevatorBaseCommand(elevatorSubsystem, 5).andThen(
-       new ElevatorBaseCommand(elevatorSubsystem, 10)).andThen(
-         new ElevatorBaseCommand(elevatorSubsystem, 17)).andThen(
-           new ElevatorBaseCommand(elevatorSubsystem, 8)).andThen(
-             new ElevatorBaseCommand(elevatorSubsystem, 4)).andThen(
-               new ElevatorBaseCommand(elevatorSubsystem, 7)).andThen(
-                 new ElevatorBaseCommand(elevatorSubsystem, 1)));
-
    driverA.y().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 20));
    driverA.x().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 15d));
    driverA.b().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 10d));