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.runAssistedElevator(operator::getLeftY)
// ); // );
elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
manipulator.setDefaultCommand( manipulator.setDefaultCommand(
manipulator.defaultCommand() manipulator.defaultCommand()
); );
@ -246,7 +248,7 @@ public class RobotContainer {
// then the elevator, then the arm again // then the elevator, then the arm again
} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { } else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) 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 // 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()) { } else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, armPosition, true, true); 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 // Catch all command that's safe regardless of arm and elevator positions
} else { } else {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) 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( return Commands.either(
Commands.either( Commands.either(
elevator.goToSetpoint(elevatorPosition, 2).andThen(manipulatorPivot.goToSetpoint(armPosition, 2)), elevator.goToSetpoint(elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)),
elevator.goToSetpoint(elevatorPosition, 2).alongWith(manipulatorPivot.goToSetpoint(armPosition, 2)), elevator.goToSetpoint(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)),
() -> sequential () -> sequential
), ),
Commands.either( Commands.either(
manipulatorPivot.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(elevatorPosition)),
manipulatorPivot.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(elevatorPosition)),
() -> sequential () -> sequential
), ),
() -> elevatorFirst () -> elevatorFirst
@ -303,7 +305,7 @@ public class RobotContainer {
@SuppressWarnings("unused") @SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) { private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition, 2)); .andThen(manipulatorPivot.goToSetpoint(armPosition));
} }
private Command startingConfig() { 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.MathUtil;
import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ElevatorConstants; import frc.robot.constants.ElevatorConstants;
@ -30,9 +28,6 @@ public class Elevator extends SubsystemBase {
private DigitalInput bottomLimitSwitch; private DigitalInput bottomLimitSwitch;
private PIDController positionController;
//private PIDController velocityController;
private ElevatorFeedforward feedForward; private ElevatorFeedforward feedForward;
public Elevator() { public Elevator() {
@ -66,12 +61,6 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kBottomLimitSwitchID ElevatorConstants.kBottomLimitSwitchID
); );
positionController = new PIDController(
ElevatorConstants.kPositionControllerP,
ElevatorConstants.kPositionControllerI,
ElevatorConstants.kPositionControllerD
);
/* /*
velocityController = new PIDController( velocityController = new PIDController(
ElevatorConstants.kVelocityControllerP, 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). * Moves the elevator to a target destination (setpoint).
* If the setpoint is 0, the elevator will creep down to hit the limit switch * 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 * @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination * @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( double clampedSetpoint = MathUtil.clamp(
setpoint, setpoint,
0, 0,
@ -170,7 +167,7 @@ public class Elevator extends SubsystemBase {
ClosedLoopSlot.kSlot0, ClosedLoopSlot.kSlot0,
feedForward.calculate(0) feedForward.calculate(0)
); );
}); });
/* /*
if (clampedSetpoint == 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 * @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination * @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( double clampedSetpoint = MathUtil.clamp(
setpoint, setpoint,
0, 0,
@ -139,7 +139,7 @@ public class ManipulatorPivot extends SubsystemBase {
); );
armMotor.setVoltage(voltsOut); 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{ public class Vision{
private DoubleSubscriber cam1GlobalPose; private DoubleSubscriber blackRobotRelativePose;
private DoubleSubscriber cam1ClosestTag; private DoubleSubscriber blackClosestTag;
private BooleanSubscriber cam1TagDetected;
private DoubleSubscriber cam1TimeStamp; private DoubleSubscriber blackFramerate;
private DoubleSubscriber framerate; private DoubleSubscriber orangeRobotRelativePose;
private DoubleSubscriber orangeClosestTag;
private DoubleSubscriber orangeFramerate;
public Vision(){ public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault(); 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); blackRobotRelativePose = blackVisionTable.getDoubleTopic("blackRelativePose").subscribe(0.0);
cam1ClosestTag = visionTable.getDoubleTopic("cam1ClosestTag").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(){ public double getBlackGlobalPose(){
return cam1GlobalPose.get(); return blackRobotRelativePose.get();
} }
public double getCam1ClosestTag(){ public double getBlackClosestTag(){
return cam1ClosestTag.get(); return blackClosestTag.get();
} }
public boolean getCam1TagDetected(){ public double getBlackTimeStamp(){
return cam1TagDetected.get(); return blackRobotRelativePose.getLastChange();
} }
public double getCam1TimeStamp(){ public double getBlackFPS(){
return cam1GlobalPose.getLastChange(); return blackFramerate.get();
}
public double getOrangeGlobalPose(){
return orangeRobotRelativePose.get();
} }
public double getFPS(){ public double getOrangeClosestTag(){
return framerate.get(); return orangeClosestTag.get();
}
public double getOrangeTimeStamp(){
return orangeRobotRelativePose.getLastChange();
}
public double getOrangeFPS(){
return orangeFramerate.get();
} }
} }