Skip to content

Commit

Permalink
Merge branch 'main' into 204-teleop-place-sequence
Browse files Browse the repository at this point in the history
  • Loading branch information
@mmilunicmobile
mmilunicmobile committed Feb 22, 2025
2 parents cfe0464 + d661b46 commit a03b89c
Show file tree
Hide file tree
Showing 28 changed files with 466 additions and 202 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,7 +229,7 @@ public int getTagByTeam() {

public enum ArmHeight {
Home(Position.Home, Position.Home),
L1(Position.L1, Position.L1Prep),
L1(Position.L1, Position.L1),
L2(Position.L2, Position.L2Prep),
L3(Position.L3, Position.L3Prep),
L4(Position.L4, Position.L4Prep),
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,8 @@ public Robot() {
Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs"));
Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
PowerDistribution distribution =
new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
new PowerDistribution(
33, ModuleType.kRev); // Enables power distribution logging
} else {
setUseTiming(true); // Run as fast as possible
Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
Expand Down
115 changes: 75 additions & 40 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,6 @@
import frc.robot.subsystems.chute.ChuteIOSim;
import frc.robot.subsystems.chute.ChuteSubsystem;
import frc.robot.subsystems.climber.ClimberHeadIONeo550;
import frc.robot.subsystems.climber.ClimberHeadIOSim;
import frc.robot.subsystems.climber.ClimberIOSim;
import frc.robot.subsystems.climber.ClimberIOTalonFX;
import frc.robot.subsystems.climber.ClimberSubsystem;
import frc.robot.subsystems.elevator.ElevatorIOSim;
Expand All @@ -55,8 +53,8 @@
import frc.robot.subsystems.intake.IntakeSubsystem;
import frc.robot.subsystems.lights.LightsSubsystem;
import frc.robot.subsystems.swerve.CommandSwerveDrivetrain;
import frc.robot.subsystems.vision.DummyPhotonCamera;
import frc.robot.subsystems.vision.Vision;
import frc.robot.subsystems.vision.VisionIOLimelight;
import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
import frc.robot.subsystems.vision.VisionIOPhotonVisionSimML;
import frc.robot.subsystems.wrist.WristIONeo550;
Expand Down Expand Up @@ -107,23 +105,31 @@ public RobotContainer() {
vision =
new Vision(
drivetrain::addVisionMeasurement,
new VisionIOLimelight(
VisionConstants.camera0Name,
() -> drivetrain.getRobotPose().getRotation()),
new VisionIOLimelight(
VisionConstants.camera1Name,
() -> drivetrain.getRobotPose().getRotation()),
new VisionIOLimelight(
VisionConstants.camera2Name,
() -> drivetrain.getRobotPose().getRotation()));
gripperSubsystem = new GripperSubsystem(new GripperIOFalcon());
elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOTalonFX());
new DummyPhotonCamera(),
new DummyPhotonCamera(),
new DummyPhotonCamera());
// new VisionIOLimelight(
// VisionConstants.camera0Name,
// () -> drivetrain.getRobotPose().getRotation()),
// new VisionIOLimelight(
// VisionConstants.camera1Name,
// () -> drivetrain.getRobotPose().getRotation()),
// new VisionIOLimelight(
// VisionConstants.camera2Name,
// () -> drivetrain.getRobotPose().getRotation()));
gripperSubsystem =
new GripperSubsystem(new GripperIOFalcon()); // new GripperIOFalcon());
elevatorSubsystem =
new ElevatorSubsystem(new ElevatorIOTalonFX()); // new ElevatorIOTalonFX());
armSubsystem = new ArmSubsystem(new ArmPivotIOTalonFX());
wristSubsystem = new WristSubsystem(new WristIONeo550());
wristSubsystem = new WristSubsystem(new WristIONeo550()); // new WristIONeo550());
climberSubsystem =
new ClimberSubsystem(new ClimberIOTalonFX(), new ClimberHeadIONeo550());
new ClimberSubsystem(
new ClimberIOTalonFX(),
new ClimberHeadIONeo550()); // new ClimberIOTalonFX(), new
// ClimberHeadIONeo550());
lights = new LightsSubsystem();
chuteSubsystem = new ChuteSubsystem(new ChuteIONeo550());
chuteSubsystem = new ChuteSubsystem(new ChuteIONeo550()); // new ChuteIONeo550());

intakeSubsystem = new IntakeSubsystem(new IntakeRollerIOSim(), new FlipperIOSim());
} else {
Expand All @@ -148,7 +154,8 @@ public RobotContainer() {
armSubsystem = new ArmSubsystem(new ArmPivotIOSim());
wristSubsystem = new WristSubsystem(new WristIOSim());
intakeSubsystem = new IntakeSubsystem(new IntakeRollerIOSim(), new FlipperIOSim());
climberSubsystem = new ClimberSubsystem(new ClimberIOSim(), new ClimberHeadIOSim());
climberSubsystem =
new ClimberSubsystem(new ClimberIOTalonFX(), new ClimberHeadIONeo550());
lights = new LightsSubsystem();
chuteSubsystem = new ChuteSubsystem(new ChuteIOSim());
}
Expand All @@ -158,6 +165,10 @@ public RobotContainer() {
elevatorSubsystem, armSubsystem, wristSubsystem, chuteSubsystem);

auto = new Auto(drivetrain, this);

wristSubsystem.elevatorHeight = () -> elevatorSubsystem.getPosition();
wristSubsystem.armHeight = () -> armSubsystem.getPosition();

configureBindings();
// Establish the "Trajectory Field" Field2d into the dashboard
}
Expand Down Expand Up @@ -385,55 +396,62 @@ private void configureBindings() {
ARMWRIST.and(operatorController.getX()).whileTrue(wristSubsystem.turnWristLeft());
ARMWRIST.and(operatorController.getB()).whileTrue(wristSubsystem.turnWristRight());

CORAL.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.L4));
CORAL.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3));
CORAL.and(operatorController.getB()).onTrue(stateManager.moveToPosition(Position.L2));
CORAL.and(operatorController.getY())
.onTrue(stateManager.moveToPosition(Position.L4Prep))
.onFalse(stateManager.moveToPosition(Position.L4));
CORAL.and(operatorController.getX())
.onTrue(stateManager.moveToPosition(Position.L3Prep))
.onFalse(stateManager.moveToPosition(Position.L3));
CORAL.and(operatorController.getB())
.onTrue(stateManager.moveToPosition(Position.L2Prep))
.onFalse(stateManager.moveToPosition(Position.L2));
CORAL.and(operatorController.getA()).onTrue(stateManager.moveToPosition(Position.L1));

operatorController.getLeftTrigger().onTrue(stateManager.moveToPosition(Position.Climb));
operatorController.getLeftTrigger().onTrue(stateManager.moveToPosition(Position.Start));

bindPlaceSeq(operatorController.getY(), Position.L4Prep, Position.L4, 0.1);

bindPlaceSeq(operatorController.getX(), Position.L3Prep, Position.L3, 0.1);

bindPlaceSeq(operatorController.getB(), Position.L2Prep, Position.L2, 0.1);

bindPlaceSeq(operatorController.getA(), Position.L1Prep, Position.L1, 0.1);
operatorController.getA().onTrue(stateManager.moveToPosition(Position.L1));

CORAL.and(operatorController.getStart())
.onTrue(stateManager.moveToPosition(Position.Source));

CORAL.and(operatorController.getDPadDown())
.onTrue(stateManager.moveToPosition(Position.Home));
CORAL.and(operatorController.getDPadUp())
.onTrue(stateManager.moveToPosition(Position.Handoff));
.onTrue(stateManager.moveToPosition(Position.HandoffPrep))
.onFalse(stateManager.moveToPosition(Position.Handoff));
CORAL.and(operatorController.getDPadLeft()).onTrue(chuteSubsystem.moveChuteUp());
CORAL.and(operatorController.getDPadRight()).onTrue(chuteSubsystem.moveChuteDown());

ALGAE.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.NetAlgae));
// ALGAE.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.NetAlgae));
ALGAE.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3Algae));
ALGAE.and(operatorController.getB()).onTrue(stateManager.moveToPosition(Position.L2Algae));
ALGAE.and(operatorController.getA())
.onTrue(stateManager.moveToPosition(Position.Processor));
ALGAE.and(operatorController.getStart())
.onTrue(stateManager.moveToPosition(Position.Processor));
.onTrue(stateManager.moveToPosition(Position.GroundAlgae));
ALGAE.and(operatorController.getDPadDown())
.onTrue(stateManager.moveToPosition(Position.Home));
.onTrue(stateManager.moveToPosition(Position.AlgaeHome));
ALGAE.and(operatorController.getDPadUp())
.onTrue(stateManager.moveToPosition(Position.Handoff));
ALGAE.and(operatorController.getDPadLeft())
.onTrue(stateManager.moveToPosition(Position.Quick34));
ALGAE.and(operatorController.getDPadRight())
.onTrue(stateManager.moveToPosition(Position.Quick23));

operatorController.getBack().onTrue(wristSubsystem.flipWristPosition());
// operatorController.getBack().onTrue(wristSubsystem.flipWristPosition());

// Driver Align Bindings, for a different/later day
// CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(9, 0));

// Climb Bindings
leftDriveController.getLeftThumb().whileTrue(climberSubsystem.downPosition());
leftDriveController.getRightThumb().whileTrue(climberSubsystem.upPosition());
leftDriveController.getLeftThumb().whileTrue(climberSubsystem.moveClimberDownVoltage());
leftDriveController.getRightThumb().whileTrue(climberSubsystem.moveClimberUpVoltage());
leftDriveController.getBottomThumb().whileTrue(climberSubsystem.intakeCage());

// leftDriveController.getBottomThumb().whileTrue(alignToPiece());
Expand All @@ -445,15 +463,24 @@ private void configureBindings() {
// rightDriveController.getRightThumb().whileTrue(intakeSubsystem.openAndEject());

CORAL.and(rightDriveController.getLeftThumb())
.whileTrue(gripperSubsystem.intakeSpinCoral());
CORAL.and(rightDriveController.getRightThumb())
.whileTrue(
gripperSubsystem
.ejectSpinCoral()); // and(normalRelease)).whileTrue(gripperSubsystem.ejectSpinCoral());
.intakeSpinCoral()
.withDeadline(
Commands.waitSeconds(0.2)
.andThen(
Commands.waitUntil(
gripperSubsystem.HAS_PIECE))));
CORAL.and(rightDriveController.getRightThumb())
.whileTrue(gripperSubsystem.ejectSpinCoral());

ALGAE.and(rightDriveController.getBottomThumb())
.whileTrue(gripperSubsystem.intakeSpinAlgae());
ALGAE.and(rightDriveController.getTrigger()).whileTrue(gripperSubsystem.ejectSpinAlgae());
ALGAE.and(rightDriveController.getLeftThumb()).onTrue(gripperSubsystem.intakeSpinAlgae());
ALGAE.and(rightDriveController.getRightThumb())
.and(stateManager.PROCESSOR)
.whileTrue(gripperSubsystem.slowEjectSpinAlgae());
ALGAE.and(rightDriveController.getRightThumb())
.and(stateManager.PROCESSOR.negate())
.whileTrue(gripperSubsystem.ejectSpinAlgae());

leftDriveController
.getTrigger()
Expand Down Expand Up @@ -481,19 +508,27 @@ private void configureBindings() {
// Technical Bindings

leftDriveController.getLeftBottomMiddle().onTrue(climberSubsystem.zeroClimberCommand());
rightDriveController
.getLeftBottomMiddle()
.onTrue(stateManager.moveToPosition(Position.Start));
leftDriveController.getLeftTopMiddle().whileTrue(climberSubsystem.climberTuneable());

rightDriveController
.getLeftTopLeft()
.onTrue(Commands.runOnce(() -> drivetrain.seedFieldCentric()));

leftDriveController.getLeftBottomLeft().whileTrue(intakeSubsystem.rollerTuneable());
leftDriveController.getLeftTopRight().whileTrue(intakeSubsystem.flipperTuneable());
// leftDriveController.getLeftBottomLeft().whileTrue(wristSubsystem.tunablePose());
// leftDriveController.getLeftTopRight().whileTrue(wristSubsystem.tuneableVoltage());

// leftDriveController.getLeftBottomLeft().whileTrue(intakeSubsystem.rollerTuneable());
// leftDriveController.getLeftTopRight().whileTrue(intakeSubsystem.flipperTuneable());

leftDriveController.getLeftBottomLeft().whileTrue(chuteSubsystem.moveChuteUp());
leftDriveController.getLeftTopRight().whileTrue(chuteSubsystem.moveChuteDown());

leftDriveController.getLeftBottomRight().onTrue(intakeSubsystem.zeroflipper());

leftDriveController.getLeftTopLeft().whileTrue(gripperSubsystem.gripperTuneable());

{
var tunableCommand =
Commands.runOnce(
Expand Down Expand Up @@ -522,7 +557,7 @@ private void configureBindings() {
SmartDashboard.putData(stateManager);
}

leftDriveController.getRightBottomLeft().onTrue(elevatorSubsystem.zeroElevatorCommand());
// leftDriveController.getRightBottomLeft().onTrue(elevatorSubsystem.zeroElevatorCommand());
}

private void bindPlaceSeq(Trigger button, Position prep, Position end, double timeout) {
Expand Down
15 changes: 7 additions & 8 deletions src/main/java/frc/robot/constants/ArmConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,10 +4,10 @@

public class ArmConstants {

public static final int ARM_PIVOT_MOTOR_ID = 10; // not correct motor ID
public static final String ARM_PIVOT_CANBUS = "CANivore";
public static final int ARM_PIVOT_MOTOR_ID = 10;
public static final String ARM_PIVOT_CANBUS = "rio";

public static final int ARM_THROUGHBORE_ENCODER_ID = 0;
public static final int ARM_THROUGHBORE_ENCODER_ID = 1;

// public static final Slot0Configs slot0Configs =
// new Slot0Configs()
Expand All @@ -27,13 +27,12 @@ public class ArmConstants {
// .withMotionMagicJerk(4);

public static final CurrentLimitsConfigs currentLimitConfigs = new CurrentLimitsConfigs();

public static final double ARM_KP = 5;
public static final double ARM_KP = 3.5;
public static final double ARM_KD = 0;
public static final double ARM_KI = 0;
public static final double ARM_KI = 0.25;

public static final double ARM_TOLERANCE = 0.01;

public static final double upperLimit = 100;
public static final double lowerLimit = 0;
public static final double upperLimit = 2.15;
public static final double lowerLimit = -0.6;
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/constants/ChuteConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,9 @@ public class ChuteConstants {

public static final double CHUTE_TOLERANCE = 0.01;

public static final double ChuteCurrent = 30;
public static final double ChuteCurrent = 40;

public static final double upperLimit = 100;
public static final double upperLimit = 10;
public static final double lowerLimit = 0;

public static final double ChuteCurrentTrigger = 15;
Expand Down
6 changes: 4 additions & 2 deletions src/main/java/frc/robot/constants/ClimberConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,11 @@ public class ClimberConstants {

public static final int CLIMBER_ID = 16;

public static final int CLIMBER_HEAD_ID = 0; // or 40? the electrical manual wasn't clear
public static final int CLIMBER_HEAD_ID = 40;

public static final String CANbus = "CANivore";
public static final String canbus = "rio";

public static final String WinchCanbus = "CANivore";

public static final double ClimberHeadCurrent = 30;
}
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/constants/ElevatorConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,16 @@ public class ElevatorConstants {
.withKD(0)
.withKG(0)
.withKI(0)
.withKP(0)
.withKP(2)
.withKS(0)
.withKV(0)
.withGravityType(GravityTypeValue.Elevator_Static);
;
public static final MotionMagicConfigs motionMagicConfigs =
new MotionMagicConfigs()
.withMotionMagicAcceleration(4) // these are guesses, come back here
.withMotionMagicCruiseVelocity(4) // also guess
.withMotionMagicJerk(4);
.withMotionMagicAcceleration(190 / .3) // these are guesses, come back here
.withMotionMagicCruiseVelocity(190) // also guess
.withMotionMagicJerk(0);

public static final int elevatorLeaderId = 9;

Expand All @@ -32,5 +32,5 @@ public class ElevatorConstants {
public static final CurrentLimitsConfigs currentLimit = new CurrentLimitsConfigs();

public static final double lowerLimit = 0;
public static final double upperLimit = 100;
public static final double upperLimit = 297;
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/constants/GripperConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ public class GripperConstants {
public static final CurrentLimitsConfigs currentLimit =
new CurrentLimitsConfigs().withStatorCurrentLimit(50);

public static final String canbus = "CANivore";
public static final String canbus = "rio";

public static final int gripperSensorChannel = 60;
public static final int gripperSensorChannel = 0;
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/constants/VisionConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,8 @@ public class VisionConstants {
AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

// Camera names, must match names configured on coprocessor
public static String camera0Name = "limelight-april";
public static String camera1Name = "limelight-april-right";
public static String camera0Name = "limelight-left";
public static String camera1Name = "limelight-right";
public static String camera2Name = "limelight-intake";
// public static String camera1Name = "camera_1";

Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/constants/WristConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +5,16 @@ public class WristConstants {
public static final int WRIST_MOTOR_ID = 11;
public static final String WRIST_CANBUS = "CANivore";

public static final int WRIST_THROUGHBORE_ENCODER_ID = 1;
public static final int WRIST_THROUGHBORE_ENCODER_ID = 0;

public static final double WRIST_KP = 5;
public static final double WRIST_KP = 8;
public static final double WRIST_KD = 0;
public static final double WRIST_KI = 0;

public static final double WRIST_TOLERANCE = 0.01;

public static final double WristCurrent = 0;

public static final double upperLimit = 100;
public static final double lowerLimit = 0;
public static final double upperLimit = 1.59;
public static final double lowerLimit = -1.58;
}
Loading

0 comments on commit a03b89c

Please sign in to comment.