diff --git a/src/main/java/frc4388/robot/commands/ArmCommand.java b/src/main/java/frc4388/robot/commands/PivotCommand.java similarity index 55% rename from src/main/java/frc4388/robot/commands/ArmCommand.java rename to src/main/java/frc4388/robot/commands/PivotCommand.java index 90ec94d..3f8235c 100644 --- a/src/main/java/frc4388/robot/commands/ArmCommand.java +++ b/src/main/java/frc4388/robot/commands/PivotCommand.java @@ -5,26 +5,17 @@ package frc4388.robot.commands; import frc4388.robot.subsystems.Arm; -import frc4388.robot.subsystems.Claw; -public class ArmCommand extends PelvicInflammatoryDisease { +public class PivotCommand extends PelvicInflammatoryDisease { private final Arm arm; - private final Claw claw; - private final boolean toggle; private final double target; /** Creates a new ArmCommand. */ - public ArmCommand(Arm arm, Claw claw, double target, boolean open) { + public PivotCommand(Arm arm, double target) { super(0.6, 0, 0, 0, 0); this.arm = arm; - this.claw = claw; - this.toggle = open; this.target = target; - addRequirements(arm, claw); - } - - public ArmCommand(Arm arm, Claw claw, double target) { - this(arm, claw, target, claw.isClawOpen()); + addRequirements(arm); } @Override @@ -36,9 +27,4 @@ public class ArmCommand extends PelvicInflammatoryDisease { public void runWithOutput(double output) { arm.setRotVel(output); } - - @Override - public void end(boolean interrupted) { - claw.setClaw(toggle); - } } diff --git a/src/main/java/frc4388/robot/commands/TeleCommand.java b/src/main/java/frc4388/robot/commands/TeleCommand.java new file mode 100644 index 0000000..02c8c27 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/TeleCommand.java @@ -0,0 +1,32 @@ +// 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 frc4388.robot.commands; + +import frc4388.robot.Constants.ArmConstants; +import frc4388.robot.subsystems.Arm; + +public class TeleCommand extends PelvicInflammatoryDisease { + private final Arm arm; + private final double target; + + /** Creates a new ArmCommand. */ + public TeleCommand(Arm arm, double target) { + super(0.6, 0, 0, 0, 0); + this.arm = arm; + this.target = target; + addRequirements(arm); + } + + @Override + public double getError() { + return (arm.getArmLength() - target) / + (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); + } + + @Override + public void runWithOutput(double output) { + arm.setTeleVel(output); + } +}