Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

work on elevator manual, vision, and manipulator

Browse Source
This commit is contained in:
Tylr-J42 2025-02-10 22:06:12 -05:00
parent 56980d3772
commit 96ad0ba088
4 changed files with 58 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/main/java/frc/robot/RobotContainer.java
View File

@ -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() {

23
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -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) {

4
src/main/java/frc/robot/subsystems/ManipulatorPivot.java
View File

@ -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);
}
/**

57
src/main/java/frc/robot/subsystems/Vision.java
View File

@ -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();
}
}