Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
refactor(arm): Add limit switch.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Mar 1, 2024
1 parent d54237b commit 3a9b578
Show file tree
Hide file tree
Showing 3 changed files with 20 additions and 21 deletions.
29 changes: 15 additions & 14 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,10 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.lib.Telemetry;
import frc.robot.arm.Arm;
import frc.robot.auto.Auto;
import frc.robot.climber.Climber;
import frc.robot.intake.Intake;
import frc.robot.odometry.Odometry;
import frc.robot.shooter.Shooter;
import frc.robot.swerve.DriveCommand;
import frc.robot.swerve.Swerve;

Expand All @@ -19,19 +16,25 @@ public class RobotContainer {
/** Instance variable for the robot container singleton. */
public static RobotContainer instance = null;

private final Arm arm = Arm.getInstance();
private final Auto auto = Auto.getInstance();
private final Climber climber = Climber.getInstance();
private final Intake intake = Intake.getInstance();
private final Odometry odometry = Odometry.getInstance();
private final Shooter shooter = Shooter.getInstance();
private final Swerve swerve = Swerve.getInstance();
private final Arm arm;
private final Auto auto;
private final Intake intake;
private final Odometry odometry;
private final Swerve swerve;

private final CommandXboxController driverController = new CommandXboxController(0);
private final CommandXboxController operatorController = new CommandXboxController(1);
private final CommandXboxController driverController, operatorController;

/** Creates a new instance of the robot container. */
private RobotContainer() {
arm = Arm.getInstance();
auto = Auto.getInstance();
intake = Intake.getInstance();
odometry = Odometry.getInstance();
swerve = Swerve.getInstance();

driverController = new CommandXboxController(0);
operatorController = new CommandXboxController(1);

initializeTelemetry();
configureBindings();
}
Expand Down Expand Up @@ -75,8 +78,6 @@ private void configureBindings() {

operatorController.rightTrigger().whileTrue(auto.shootNote()).whileFalse(auto.stow());

// TODO When stowing, the wrist encoder drifts by ~30% of the shoulder velocity
// TODO This **will** throw off wrist angles for the rest of the match
operatorController.a().whileTrue(arm.amp()).whileFalse(auto.stow());
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/arm/ArmFactory.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,8 @@
public class ArmFactory {

public static LimitSwitchIO createLimitSwitch() {
// if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM))
// return new LimitSwitchIOPWM();
if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM))
return new LimitSwitchIODigital();

return new LimitSwitchIOSim();
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,12 @@

import edu.wpi.first.wpilibj.DigitalInput;

public class LimitSwitchIOPWM implements LimitSwitchIO {
public class LimitSwitchIODigital implements LimitSwitchIO {

private final DigitalInput digitalInput;

public LimitSwitchIOPWM() {
int port = 0;

digitalInput = new DigitalInput(port);
public LimitSwitchIODigital() {
digitalInput = new DigitalInput(4);
}

@Override
Expand Down

0 comments on commit 3a9b578

Please sign in to comment.