diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3feda2b..809e3d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -93,6 +93,8 @@ public class RobotContainer { //elevator.runAssistedElevator(operator::getLeftY) // ); + elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition)); + manipulator.setDefaultCommand( manipulator.defaultCommand() ); @@ -246,7 +248,7 @@ public class RobotContainer { // then the elevator, then the arm again } else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) - .andThen(manipulatorPivot.goToSetpoint(armPosition, 2)); + .andThen(manipulatorPivot.goToSetpoint(armPosition)); // If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm } else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) { return moveManipulatorUtil(elevatorPosition, armPosition, true, true); @@ -256,7 +258,7 @@ public class RobotContainer { // Catch all command that's safe regardless of arm and elevator positions } else { return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) - .andThen(manipulatorPivot.goToSetpoint(armPosition, 2)); + .andThen(manipulatorPivot.goToSetpoint(armPosition)); } } @@ -280,13 +282,13 @@ public class RobotContainer { return Commands.either( Commands.either( - elevator.goToSetpoint(elevatorPosition, 2).andThen(manipulatorPivot.goToSetpoint(armPosition, 2)), - elevator.goToSetpoint(elevatorPosition, 2).alongWith(manipulatorPivot.goToSetpoint(armPosition, 2)), + elevator.goToSetpoint(elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)), + elevator.goToSetpoint(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)), () -> sequential ), Commands.either( - manipulatorPivot.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), - manipulatorPivot.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), + manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(elevatorPosition)), + manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(elevatorPosition)), () -> sequential ), () -> elevatorFirst @@ -303,7 +305,7 @@ public class RobotContainer { @SuppressWarnings("unused") private Command safeMoveManipulator(double elevatorPosition, double armPosition) { return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) - .andThen(manipulatorPivot.goToSetpoint(armPosition, 2)); + .andThen(manipulatorPivot.goToSetpoint(armPosition)); } private Command startingConfig() { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index be7ccde..4838295 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -13,10 +13,8 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ElevatorConstants; @@ -30,9 +28,6 @@ public class Elevator extends SubsystemBase { private DigitalInput bottomLimitSwitch; - private PIDController positionController; - //private PIDController velocityController; - private ElevatorFeedforward feedForward; public Elevator() { @@ -66,12 +61,6 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kBottomLimitSwitchID ); - positionController = new PIDController( - ElevatorConstants.kPositionControllerP, - ElevatorConstants.kPositionControllerI, - ElevatorConstants.kPositionControllerD - ); - /* velocityController = new PIDController( ElevatorConstants.kVelocityControllerP, @@ -148,6 +137,14 @@ public class Elevator extends SubsystemBase { } */ + public Command manualControl(DoubleSupplier speed){ + return run(() -> { + + elevatorMotor1.setVoltage(feedForward.calculate(0) + (speed.getAsDouble()/12)); + + }); + } + /** * Moves the elevator to a target destination (setpoint). * If the setpoint is 0, the elevator will creep down to hit the limit switch @@ -156,7 +153,7 @@ public class Elevator extends SubsystemBase { * @param timeout Time to achieve the setpoint before quitting * @return Sets motor voltage to achieve the target destination */ - public Command goToSetpoint(double setpoint, double timeout) { + public Command goToSetpoint(double setpoint) { double clampedSetpoint = MathUtil.clamp( setpoint, 0, @@ -170,7 +167,7 @@ public class Elevator extends SubsystemBase { ClosedLoopSlot.kSlot0, feedForward.calculate(0) ); - + }); /* if (clampedSetpoint == 0) { diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index b2160bb..9757772 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -122,7 +122,7 @@ public class ManipulatorPivot extends SubsystemBase { * @param timeout Time to achieve the setpoint before quitting * @return Sets motor voltage to achieve the target destination */ - public Command goToSetpoint(double setpoint, double timeout) { + public Command goToSetpoint(double setpoint) { double clampedSetpoint = MathUtil.clamp( setpoint, 0, @@ -139,7 +139,7 @@ public class ManipulatorPivot extends SubsystemBase { ); armMotor.setVoltage(voltsOut); - }).until(positionController::atSetpoint).withTimeout(timeout); + }).until(positionController::atSetpoint); } /** diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index e1fe524..8a70ac9 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -7,45 +7,62 @@ import edu.wpi.first.networktables.NetworkTableInstance; public class Vision{ - private DoubleSubscriber cam1GlobalPose; - private DoubleSubscriber cam1ClosestTag; - private BooleanSubscriber cam1TagDetected; + private DoubleSubscriber blackRobotRelativePose; + private DoubleSubscriber blackClosestTag; - private DoubleSubscriber cam1TimeStamp; + private DoubleSubscriber blackFramerate; - private DoubleSubscriber framerate; + private DoubleSubscriber orangeRobotRelativePose; + private DoubleSubscriber orangeClosestTag; + private DoubleSubscriber orangeFramerate; public Vision(){ NetworkTableInstance inst = NetworkTableInstance.getDefault(); - NetworkTable visionTable = inst.getTable("Fiducial"); + NetworkTable blackVisionTable = inst.getTable("black_Fiducial"); + NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial"); - cam1GlobalPose = visionTable.getDoubleTopic("cam1GlobalPose").subscribe(0.0); - cam1ClosestTag = visionTable.getDoubleTopic("cam1ClosestTag").subscribe(0.0); + blackRobotRelativePose = blackVisionTable.getDoubleTopic("blackRelativePose").subscribe(0.0); + blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0); - cam1TagDetected = visionTable.getBooleanTopic("cam1_visible").subscribe(false); + blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0); - framerate = visionTable.getDoubleTopic("fps").subscribe(0.0); + orangeRobotRelativePose = orangeVisionTable.getDoubleTopic("orangeRelativePose").subscribe(0.0); + orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0); + + orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0); } - public double getCam1GlobalPose(){ - return cam1GlobalPose.get(); + public double getBlackGlobalPose(){ + return blackRobotRelativePose.get(); } - public double getCam1ClosestTag(){ - return cam1ClosestTag.get(); + public double getBlackClosestTag(){ + return blackClosestTag.get(); } - public boolean getCam1TagDetected(){ - return cam1TagDetected.get(); + public double getBlackTimeStamp(){ + return blackRobotRelativePose.getLastChange(); } - public double getCam1TimeStamp(){ - return cam1GlobalPose.getLastChange(); + public double getBlackFPS(){ + return blackFramerate.get(); + } + + public double getOrangeGlobalPose(){ + return orangeRobotRelativePose.get(); } - public double getFPS(){ - return framerate.get(); + public double getOrangeClosestTag(){ + return orangeClosestTag.get(); + } + + public double getOrangeTimeStamp(){ + return orangeRobotRelativePose.getLastChange(); + } + + public double getOrangeFPS(){ + return orangeFramerate.get(); } }