498 lines
18 KiB
Java
498 lines
18 KiB
Java
// 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 frc.robot;
|
|
|
|
import frc.robot.constants.ManipulatorPivotConstants;
|
|
import frc.robot.constants.ClimberPivotConstants;
|
|
import frc.robot.constants.ElevatorConstants;
|
|
import frc.robot.constants.ManipulatorConstants;
|
|
import frc.robot.constants.OIConstants;
|
|
import frc.robot.constants.VisionConstants;
|
|
import frc.robot.subsystems.ManipulatorPivot;
|
|
import frc.robot.subsystems.Vision;
|
|
import frc.robot.subsystems.ClimberPivot;
|
|
import frc.robot.subsystems.ClimberRollers;
|
|
import frc.robot.subsystems.Drivetrain;
|
|
import frc.robot.subsystems.Elevator;
|
|
import frc.robot.subsystems.Manipulator;
|
|
|
|
import java.util.function.IntSupplier;
|
|
|
|
import com.pathplanner.lib.auto.AutoBuilder;
|
|
import com.pathplanner.lib.auto.NamedCommands;
|
|
import com.pathplanner.lib.events.EventTrigger;
|
|
import com.pathplanner.lib.path.EventMarker;
|
|
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.math.util.Units;
|
|
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.Commands;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
|
|
public class RobotContainer {
|
|
private ClimberPivot climberPivot;
|
|
|
|
private ClimberRollers climberRollers;
|
|
|
|
private Drivetrain drivetrain;
|
|
|
|
private Elevator elevator;
|
|
//private ElevatorSysID elevator;
|
|
|
|
private Manipulator manipulator;
|
|
|
|
private ManipulatorPivot manipulatorPivot;
|
|
|
|
private CommandXboxController driver;
|
|
private CommandXboxController operator;
|
|
|
|
private SendableChooser<Command> autoChooser;
|
|
|
|
private IntSupplier closestTag;
|
|
|
|
public RobotContainer() {
|
|
climberPivot = new ClimberPivot();
|
|
|
|
climberRollers = new ClimberRollers();
|
|
|
|
drivetrain = new Drivetrain();
|
|
|
|
elevator = new Elevator();
|
|
//elevator = new ElevatorSysID();
|
|
|
|
manipulator = new Manipulator();
|
|
|
|
manipulatorPivot = new ManipulatorPivot();
|
|
|
|
configureNamedCommands();
|
|
|
|
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
|
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
|
|
|
autoChooser = AutoBuilder.buildAutoChooser();
|
|
|
|
closestTag = drivetrain::getClosestTag;
|
|
|
|
configureButtonBindings();
|
|
//elevatorSysIDBindings();
|
|
//elevatorOnlyBindings();
|
|
|
|
configureShuffleboard();
|
|
}
|
|
|
|
/*private void elevatorSysIDBindings() {
|
|
elevator.setDefaultCommand(elevator.maintainPosition());
|
|
|
|
operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward));
|
|
operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse));
|
|
operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward));
|
|
operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
|
|
}*/
|
|
|
|
private void configureButtonBindings() {
|
|
//Default commands
|
|
climberPivot.setDefaultCommand(
|
|
climberPivot.runPivot(() -> 0)
|
|
);
|
|
|
|
climberRollers.setDefaultCommand(
|
|
climberRollers.runRoller(() -> 0)
|
|
);
|
|
|
|
drivetrain.setDefaultCommand(
|
|
drivetrain.drive(
|
|
() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
|
() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
|
driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
|
|
() -> true
|
|
)
|
|
);
|
|
|
|
elevator.setDefaultCommand(
|
|
elevator.maintainPosition()
|
|
);
|
|
|
|
|
|
manipulatorPivot.setDefaultCommand(
|
|
manipulatorPivot.maintainPosition()
|
|
);
|
|
|
|
|
|
manipulator.setDefaultCommand(
|
|
manipulator.runUntilCollected(
|
|
() -> 0.0
|
|
)
|
|
);
|
|
|
|
//Driver inputs
|
|
|
|
driver.start().whileTrue(
|
|
drivetrain.setXCommand()
|
|
);
|
|
|
|
driver.rightTrigger().whileTrue(
|
|
manipulator.runManipulator(() -> 0.35, true)
|
|
);
|
|
|
|
driver.leftTrigger().whileTrue(
|
|
manipulator.runUntilCollected(() -> 0.75)
|
|
);
|
|
|
|
driver.start().and(driver.back()).onTrue(
|
|
startingConfig()
|
|
);
|
|
|
|
driver.y().whileTrue(drivetrain.zeroHeading());
|
|
|
|
driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
|
|
|
|
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
|
|
|
|
driver.start().whileTrue(drivetrain.resetToVision());
|
|
|
|
driver.rightBumper().whileTrue(
|
|
drivetrain.goToPose(
|
|
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
|
|
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
|
|
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
|
|
)
|
|
);
|
|
|
|
driver.leftBumper().whileTrue(
|
|
drivetrain.goToPose(
|
|
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
|
|
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
|
|
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
|
|
)
|
|
);
|
|
|
|
//Operator inputs
|
|
operator.povUp().onTrue(
|
|
safeMoveManipulator(
|
|
ElevatorConstants.kL4Position,
|
|
ManipulatorPivotConstants.kL4Position
|
|
)
|
|
);
|
|
|
|
operator.povRight().onTrue(
|
|
safeMoveManipulator(
|
|
ElevatorConstants.kL3Position,
|
|
ManipulatorPivotConstants.kL3Position
|
|
)
|
|
);
|
|
|
|
operator.povLeft().onTrue(
|
|
safeMoveManipulator(
|
|
ElevatorConstants.kL2Position,
|
|
ManipulatorPivotConstants.kL2Position
|
|
)
|
|
);
|
|
|
|
operator.povDown().onTrue(
|
|
safeMoveManipulator(
|
|
ElevatorConstants.kL1Position,
|
|
ManipulatorPivotConstants.kL1Position
|
|
)
|
|
);
|
|
|
|
operator.back().onTrue(elevator.homeCommand());
|
|
|
|
operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
|
|
|
|
operator.a().onTrue(
|
|
safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition)
|
|
);
|
|
|
|
operator.x().onTrue(
|
|
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
|
|
.alongWith(manipulator.runManipulator(() -> 0.85, false))
|
|
.until(() -> driver.a().getAsBoolean())
|
|
);
|
|
|
|
operator.b().onTrue(
|
|
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
|
|
.alongWith(manipulator.runManipulator(() -> 0.85, false))
|
|
.until(() -> driver.a().getAsBoolean())
|
|
);
|
|
|
|
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
|
|
.alongWith(manipulator.runManipulator(() -> 0.85, false))
|
|
.until(() -> driver.a().getAsBoolean())
|
|
);
|
|
|
|
}
|
|
private void configureNamedCommands() {
|
|
new EventTrigger("Lift L4").whileTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
|
new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
|
|
|
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
|
|
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2).andThen(manipulator.runUntilCollected(() -> 0)));
|
|
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runUntilCollected(() -> 0)).withTimeout(0.5));
|
|
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)
|
|
.andThen(Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
|
|
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
|
|
}
|
|
|
|
//creates tabs and transforms them on the shuffleboard
|
|
private void configureShuffleboard() {
|
|
ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
|
|
ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
|
|
ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
|
|
|
|
Shuffleboard.selectTab(OIConstants.kAutoTab);
|
|
|
|
autoTab.add("Auto Selection", autoChooser)
|
|
.withSize(2, 1)
|
|
.withPosition(0, 0)
|
|
.withWidget(BuiltInWidgets.kComboBoxChooser);
|
|
|
|
sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
|
|
.withSize(2, 1)
|
|
.withPosition(0, 0)
|
|
.withWidget(BuiltInWidgets.kGraph);
|
|
|
|
sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
|
|
.withSize(2, 1)
|
|
.withPosition(2, 0)
|
|
.withWidget(BuiltInWidgets.kTextView);
|
|
|
|
sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
|
|
.withSize(2, 1)
|
|
.withPosition(2, 1)
|
|
.withWidget(BuiltInWidgets.kTextView);
|
|
|
|
sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
|
|
.withSize(2, 1)
|
|
.withPosition(0, 1)
|
|
.withWidget(BuiltInWidgets.kTextView);
|
|
|
|
sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
|
|
.withSize(1, 1)
|
|
.withPosition(4, 0)
|
|
.withWidget(BuiltInWidgets.kBooleanBox);
|
|
|
|
sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
|
|
.withSize(1, 1)
|
|
.withPosition(4, 1)
|
|
.withWidget(BuiltInWidgets.kBooleanBox);
|
|
|
|
sensorTab.addDouble("ElevMotor1", elevator::getMotor1)
|
|
.withWidget(BuiltInWidgets.kGraph);
|
|
|
|
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
|
|
.withWidget(BuiltInWidgets.kGraph);
|
|
|
|
sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
|
|
.withSize(1, 1)
|
|
.withPosition(5, 0)
|
|
.withWidget(BuiltInWidgets.kTextView);
|
|
|
|
sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
|
|
.withSize(1, 1)
|
|
.withPosition(5, 1)
|
|
.withWidget(BuiltInWidgets.kTextView);
|
|
|
|
sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
|
|
.withSize(1, 1)
|
|
.withPosition(5, 0)
|
|
.withWidget(BuiltInWidgets.kTextView);
|
|
|
|
sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
|
|
.withSize(1, 1)
|
|
.withPosition(5, 1)
|
|
.withWidget(BuiltInWidgets.kTextView);
|
|
|
|
sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
|
|
|
|
sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
|
|
|
|
sensorTab.addDouble("dt distance", drivetrain::driveDistance);
|
|
|
|
sensorTab.addDouble("velocity", drivetrain::getVelocity);
|
|
|
|
sensorTab.addDouble("heading", drivetrain::getHeading);
|
|
|
|
|
|
//sensorTab.add("odometry", drivetrain::getPose);
|
|
|
|
apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
|
|
.withSize(1,1).withPosition(1,1);
|
|
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
|
|
.withSize(2,1).withPosition(2,1);
|
|
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
|
|
.withSize(3,1).withPosition(3,1);
|
|
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
|
|
.withSize(4,1).withPosition(4,1);
|
|
apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
|
|
.withSize(6,1).withPosition(5,1);
|
|
apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
|
|
.withSize(7,1).withPosition(6,1);
|
|
|
|
apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
|
|
.withSize(1,1).withPosition(1,2);
|
|
apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
|
|
.withSize(1,1).withPosition(2,2);
|
|
apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
|
|
.withSize(1,1).withPosition(3,2);
|
|
apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
|
|
.withSize(1,1).withPosition(4,2);
|
|
apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
|
|
.withSize(1,1).withPosition(5,2);
|
|
apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
|
|
.withSize(1,1).withPosition(6,2);
|
|
|
|
apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
|
|
.withSize(1,1).withPosition(4,4);
|
|
|
|
}
|
|
|
|
public Command getAutonomousCommand() {
|
|
return autoChooser.getSelected();
|
|
}
|
|
|
|
/**
|
|
* Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
|
|
* @return Moves the elevator and arm, then intakes coral
|
|
*/
|
|
@SuppressWarnings("unused")
|
|
private Command coralIntakeRoutine() {
|
|
return moveManipulator(
|
|
ElevatorConstants.kCoralIntakePosition,
|
|
ManipulatorPivotConstants.kCoralIntakePosition
|
|
)
|
|
.andThen(manipulator.runUntilCollected(() -> .5));
|
|
}
|
|
|
|
/**
|
|
* Moves the elevator and arm to the constant setpoints and runs the manipulator until collected
|
|
*
|
|
* @param l2 Is the algae on L2? (True = L2, False = L3)
|
|
* @return Moves the elevator and arm then intakes algae
|
|
*/
|
|
@SuppressWarnings("unused")
|
|
private Command algaeIntakeRoutine(boolean l2) {
|
|
return moveManipulator(
|
|
l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition,
|
|
l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
|
|
)
|
|
.andThen(manipulator.runUntilCollected(() -> 1));
|
|
}
|
|
|
|
/**
|
|
* Moves the elevator and arm in different order based on target positions
|
|
*
|
|
* @param elevatorPosition The target position of the elevator
|
|
* @param armPosition The target rotation of the arm
|
|
* @return Moves the elevator and arm to the setpoints using the most efficient path
|
|
*/
|
|
private Command moveManipulator(double elevatorPosition, double armPosition) {
|
|
// If the elevator current and target positions are above the brace, or the arm current and target position is in
|
|
// front of the brace, move together
|
|
if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) {
|
|
return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
|
|
// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
|
|
// then the elevator, then the arm again
|
|
} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) {
|
|
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
|
.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);
|
|
// If the arm is behind the brace, move the arm first, then the elevator
|
|
} else if (!manipulatorPivot.isMotionSafe()) {
|
|
return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
|
|
// Catch all command that's safe regardless of arm and elevator positions
|
|
} else {
|
|
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
|
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Moves the elevator and arm in customizeable ways
|
|
*
|
|
* @param elevatorPosition The target elevator position
|
|
* @param armPosition The target arm position
|
|
* @param elevatorFirst Does the elevator move first? (True = Elevator first, False = Arm first)
|
|
* @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
|
|
* @return Moves the elevator and arm to the setpoints
|
|
*/
|
|
private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
|
|
|
|
/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
|
|
armPosition = MathUtil.clamp(
|
|
armPosition,
|
|
0,
|
|
ManipulatorPivotConstants.kRotationLimit
|
|
);
|
|
}*/
|
|
|
|
return Commands.either(
|
|
Commands.either(
|
|
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
|
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
|
() -> sequential
|
|
),
|
|
Commands.either(
|
|
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
|
|
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
|
|
() -> sequential
|
|
),
|
|
() -> elevatorFirst
|
|
);
|
|
}
|
|
|
|
@SuppressWarnings("unused")
|
|
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
|
|
if(!isL4){
|
|
return Commands.sequence(
|
|
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
|
|
elevator.goToSetpoint(() -> elevatorPosition),
|
|
manipulatorPivot.goToSetpoint(() -> armPosition));
|
|
|
|
}else{
|
|
return Commands.sequence(
|
|
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
|
|
elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
|
|
Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
* Moves the arm and elevator in a safe way.
|
|
*
|
|
* @param elevatorPosition The target position of the elevator
|
|
* @param armPosition The target rotation of the arm
|
|
* @return Moves the elevator and arm to the setpoints
|
|
*/
|
|
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
|
|
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
|
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
|
|
elevator.maintainPosition());*/
|
|
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
|
|
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
|
|
.raceWith(elevator.maintainPosition()));
|
|
}
|
|
|
|
private Command moveWithAlgae(double elevatorPosition, double armPosition) {
|
|
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
|
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
|
|
elevator.maintainPosition());*/
|
|
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
|
|
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
|
|
.raceWith(elevator.maintainPosition()));
|
|
}
|
|
|
|
@SuppressWarnings("unused")
|
|
private Command startingConfig() {
|
|
return moveManipulatorUtil(0, 0, false, true)
|
|
.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
|
|
}
|
|
} |