Skip to content

Commit

Permalink
fix: dio ports
Browse files Browse the repository at this point in the history
  • Loading branch information
rutmanz committed Aug 26, 2024
1 parent 78f1e1a commit 78ccab3
Show file tree
Hide file tree
Showing 7 changed files with 19 additions and 10 deletions.
8 changes: 6 additions & 2 deletions src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,12 @@
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DIO {
public static final int INTAKE_BEAM_BREAK = 6;
public static final int SHOOTER_BEAM_BREAK = 4;
public static final int INDEXER_BEAM_BREAK = 5;
public static final int TRAMP_BEAM_BREAK = 9;
}
public static final boolean IS_COMPETITION_ROBOT = true;
// Whether to pull PID constants from SmartDashboard
private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
Expand Down Expand Up @@ -89,7 +95,6 @@ public static class Auto {
}

public static class Indexer {
public static final int BEAM_BREAK_ID = IS_COMPETITION_ROBOT ? 7 : 8;
public static final int INTAKE_ID = 13;
public static final int FEEDER_ID = 15;

Expand Down Expand Up @@ -257,7 +262,6 @@ public enum ElevatorState {
}

public static class Tramp {
public static final int BEAM_BREAK_CHANNEL = 9;
public static final double GEAR_RATIO = 9.0;
public static final int MOTOR_ID = 17;
}
Expand Down
6 changes: 5 additions & 1 deletion src/main/java/org/team1540/robot2024/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import com.pathplanner.lib.commands.FollowPathCommand;
import com.pathplanner.lib.commands.PathfindingCommand;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
Expand All @@ -30,6 +31,8 @@
public class Robot extends LoggedRobot {
private Command autonomousCommand;
private RobotContainer robotContainer;
private DigitalInput intakeBeamBreak = new DigitalInput(Constants.DIO.INTAKE_BEAM_BREAK);
private DigitalInput shooterBeamBreak = new DigitalInput(Constants.DIO.SHOOTER_BEAM_BREAK);

/**
* This function is run when the robot is first started up and should be used for any
Expand Down Expand Up @@ -122,7 +125,8 @@ public void robotPeriodic() {
// the Command-based framework to work.
CommandScheduler.getInstance().run();
if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();

Logger.recordOutput("beambreak/shooter", shooterBeamBreak.get());
Logger.recordOutput("beambreak/intake", intakeBeamBreak.get());
// Update mechanism visualiser in sim
if (Robot.isSimulation()) MechanismVisualiser.periodic();
// robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.alternating(Color.kBlueViolet, Color.kGreen));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ public class IndexerIOSim implements IndexerIO {
private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI);
private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI);
// private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID);
private final DigitalInput indexerBeamBreak = new DigitalInput(Constants.DIO.INDEXER_BEAM_BREAK);
private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI, FEEDER_KD);
private boolean isClosedLoop = true;
private double intakeVoltage = 0.0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,14 @@
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.DigitalInput;
import org.team1540.robot2024.Constants;

import static org.team1540.robot2024.Constants.Indexer.*;

public class IndexerIOTalonFX implements IndexerIO {
private final TalonFX intakeMotor = new TalonFX(INTAKE_ID);
private final TalonFX feederMotor = new TalonFX(FEEDER_ID);
private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID);
private final DigitalInput indexerBeamBreak = new DigitalInput(Constants.DIO.INDEXER_BEAM_BREAK);

private final VoltageOut feederVoltageCtrlReq = new VoltageOut(0).withEnableFOC(true);
private final VelocityVoltage feederVelocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,11 @@
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
import org.team1540.robot2024.Constants;

import static org.team1540.robot2024.Constants.Tramp.BEAM_BREAK_CHANNEL;
import static org.team1540.robot2024.Constants.Tramp.GEAR_RATIO;

public class TrampIOSim implements TrampIO {
private final DCMotorSim sim = new DCMotorSim(DCMotor.getNEO(1), GEAR_RATIO, 0.025); //TODO: Fix MOI its probably wrong :D
private final DigitalInput beamBreak = new DigitalInput(BEAM_BREAK_CHANNEL);
private final DigitalInput beamBreak = new DigitalInput(Constants.DIO.TRAMP_BEAM_BREAK);
private double appliedVolts = 0.0;


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,12 @@
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.wpilibj.DigitalInput;
import org.team1540.robot2024.Constants;

import static org.team1540.robot2024.Constants.Tramp.*;

public class TrampIOSparkMax implements TrampIO {
private final DigitalInput beamBreak = new DigitalInput(BEAM_BREAK_CHANNEL);
private final DigitalInput beamBreak = new DigitalInput(Constants.DIO.TRAMP_BEAM_BREAK);
private final CANSparkMax motor = new CANSparkMax(MOTOR_ID, MotorType.kBrushless);
private final RelativeEncoder motorEncoder = motor.getEncoder();

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,12 @@
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.DigitalInput;
import org.team1540.robot2024.Constants;

import static org.team1540.robot2024.Constants.Tramp.BEAM_BREAK_CHANNEL;
import static org.team1540.robot2024.Constants.Tramp.MOTOR_ID;

public class TrampIOTalonFX implements TrampIO {
private final DigitalInput beamBreak = new DigitalInput(BEAM_BREAK_CHANNEL);
private final DigitalInput beamBreak = new DigitalInput(Constants.DIO.TRAMP_BEAM_BREAK);
private final TalonFX motor = new TalonFX(MOTOR_ID);
private final StatusSignal<Double> velocity = motor.getVelocity();
private final StatusSignal<Double> position = motor.getPosition();
Expand Down

0 comments on commit 78ccab3

Please sign in to comment.