diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3d4382b..94bf7fb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,8 +10,10 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Util.Constants.ClimberConstants.ClimberModes; public class Robot extends LoggedRobot { private Command m_autonomousCommand; @@ -62,6 +64,7 @@ public void disabledExit() { @Override public void autonomousInit() { + m_robotContainer.align_climber(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { @@ -79,6 +82,7 @@ public void autonomousExit() { @Override public void teleopInit() { + m_robotContainer.align_climber(); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c2edb2a..85ed6ad 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,8 +18,9 @@ public class RobotContainer { - private final Systems systems = new Systems(); - private final Intake intake = systems.getIntake(); + private Systems systems = new Systems(); + private Intake intake = systems.getIntake(); + private Climber climber = systems.getClimber(); private TitanController driver = new TitanController(ControllerConstants.driverPort, ControllerConstants.deadzone); private TitanController operator = new TitanController(ControllerConstants.operatorPort, ControllerConstants.deadzone); @@ -34,6 +35,9 @@ public class RobotContainer { private Trigger intakeCoral = driver.a(); + // Climber Controls + private Trigger climberAim = driver.b(); + public RobotContainer() { configureBindings(); @@ -41,11 +45,12 @@ public RobotContainer() { public void periodicSubsystems() { intake.periodic(); - + climber.periodic(); } public void periodic() { SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); + climber.periodic(); } @@ -57,11 +62,20 @@ private void configureOperatorControls() { // Intake Controls intakeCoral.whileTrue(intake.runIntakeCommand(IntakeModes.INTAKE)); + climberAim.onTrue(climber.runClimberCommand(ClimberModes.ALIGN)); + + } + public void configureBindings() { + configureOperatorControls(); } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } + + public void align_climber(){ + climber.alignClimber(); + } } diff --git a/src/main/java/frc/robot/Subsytems/Climber/Climber.java b/src/main/java/frc/robot/Subsytems/Climber/Climber.java index 5b45477..f86c620 100644 --- a/src/main/java/frc/robot/Subsytems/Climber/Climber.java +++ b/src/main/java/frc/robot/Subsytems/Climber/Climber.java @@ -20,7 +20,7 @@ public class Climber extends REVMechanism { private ClimberConfig config; private SparkMax motor; - public boolean attachted; + public boolean attached; public SysIdRoutine routine; private ClimberModes mode; @@ -42,8 +42,8 @@ public ClimberConfig() { } } - public Climber(SparkMax motor, boolean attachted) { - super(motor, attachted); + public Climber(SparkMax motor, boolean attached) { + super(motor, attached); ClimberConfig config = new ClimberConfig(); this.motor = motor; this.mode = ClimberModes.STOW; @@ -64,7 +64,7 @@ protected Config setConfig() { if (attached) { config.applySparkConfig(motor); } - return this.config; + return config; } @Override public void periodic() { @@ -112,4 +112,7 @@ public Command runClimberCommand(ClimberModes climberModes) { return run(() -> runEnum(climberModes)).withName("Climber.runEnum"); } + public void alignClimber(){ + runEnum(ClimberModes.ALIGN); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Subsytems/Manipulator/Manipulator.java b/src/main/java/frc/robot/Subsytems/Manipulator/Manipulator.java index 92c2b22..defcd7d 100644 --- a/src/main/java/frc/robot/Subsytems/Manipulator/Manipulator.java +++ b/src/main/java/frc/robot/Subsytems/Manipulator/Manipulator.java @@ -15,7 +15,7 @@ public class Manipulator extends REVMechanism { private ManipulatorConfig config; private SparkMax motor; - public Boolean attachted; + public boolean attached; private ManipulatorModes mode; private ManipulatorStates state; @@ -57,7 +57,7 @@ public Manipulator(SparkMax motor, boolean attached) { @Override protected Config setConfig() { - if (attachted) { + if (attached) { config.applySparkConfig(motor); } return this.config; diff --git a/src/main/java/frc/robot/Systems.java b/src/main/java/frc/robot/Systems.java index ca19657..b0327b7 100644 --- a/src/main/java/frc/robot/Systems.java +++ b/src/main/java/frc/robot/Systems.java @@ -7,6 +7,7 @@ import frc.robot.Subsytems.Elevator.Elevator; import frc.robot.Subsytems.Intake.Intake; import frc.robot.Subsytems.Manipulator.Manipulator; +import frc.robot.Subsytems.Climber.Climber; import frc.robot.Util.Constants; import frc.robot.Util.Constants.*; @@ -15,6 +16,7 @@ public class Systems { private Intake intake; private Elevator elevator; private Manipulator manipulator; + private Climber climber; /* Kraken X60s */ @@ -23,6 +25,7 @@ public class Systems { /* Neo 1.1s */ private SparkMax intakeMotor; + private SparkMax climberMotor; /* Neo 550s */ private SparkMax manipulatorMotor; @@ -36,12 +39,14 @@ public Systems() { /* Neo 1.1s */ intakeMotor = new SparkMax(IntakeConstants.id, MotorType.kBrushless); + climberMotor = new SparkMax(ClimberConstants.id, MotorType.kBrushless); /* Neo 550s */ manipulatorMotor = new SparkMax(ManipulatorConstants.id, MotorType.kBrushless); /*----------*/ intake = new Intake(intakeMotor, IntakeConstants.attached); + climber = new Climber(climberMotor, ClimberConstants.attached); elevator = new Elevator(elevatorLeft, elevatorRight, ElevatorConstants.attached); manipulator = new Manipulator(manipulatorMotor, ManipulatorConstants.attached); @@ -53,6 +58,10 @@ public Intake getIntake() { return intake; } + public Climber getClimber() { + return climber; + } + public Elevator getElevator() { return elevator; } diff --git a/src/main/java/frc/robot/Util/Constants.java b/src/main/java/frc/robot/Util/Constants.java index 18568c1..c49ebf5 100644 --- a/src/main/java/frc/robot/Util/Constants.java +++ b/src/main/java/frc/robot/Util/Constants.java @@ -239,9 +239,12 @@ public enum ElevatorStates { public static final boolean breakType = true; public static final boolean useFMaxRotation = true; public static final boolean useRMaxRotation = true; + public static final boolean canRangeAttached = true; - public static final int leftId = 3; - public static final int rightId = 4; + public static final int leftId = 14; + public static final int rightId = 15; + public static final int canCoderId = 00; + public static final int canRangeId = 16; public static final double gearRatio = 1 / 1; public static final Current forwardCurrentLimit = Units.Amps.of(0); public static final Current reverseCurrentLimit = Units.Amps.of(0); @@ -409,57 +412,4 @@ public enum ClimberModes { } } - - - public static class ClimberConstants { - - public enum ClimberStates { - STOW, - ALIGN, - CLIMB - } - - public static final boolean attached = true; - public static final boolean isInverted = false; - public static final int id = 2; - public static final double gearRatio = 1 / 1; - public static final Current supplyLimit = Units.Amps.of(0); - public static final Current stallLimit = Units.Amps.of(0); - public static final Angle offset = Units.Rotation.of(0); - public static final double maxForwardOutput = 0; - public static final double maxReverseOutput = 0; - - public static final IdleMode idleMode = IdleMode.kCoast; - public static final FeedbackSensor sensorType = FeedbackSensor.kPrimaryEncoder; - public static final MAXMotionPositionMode mm_positionMode = MAXMotionPositionMode.kMAXMotionTrapezoidal; - - public static final double p = 0; - public static final double i = 0; - public static final double d = 0; - public static final double maxIAccum = 0; - - public static final Angle stow_angle = Units.Rotation.of(0); - public static final Angle align_angle = Units.Rotation.of(0); - public static final Angle climb_angle = Units.Rotation.of(0); - public static final Angle error = Units.Rotation.of(0); - - public static final AngularVelocity mm_maxAccel = Units.RPM.of(0); - public static final AngularVelocity mm_velocity = Units.RPM.of(0); - public static final AngularVelocity mm_error = Units.RPM.of(0); - - public enum ClimberModes { - STOW(stow_angle), - ALIGN(align_angle), - CLIMB(climb_angle); - - public Angle angle; - - ClimberModes(Angle angle) { - this.angle = angle; - } - - } - - } - } diff --git a/src/main/java/frc/robot/Util/RobotConfig.java b/src/main/java/frc/robot/Util/RobotConfig.java new file mode 100644 index 0000000..87ff58f --- /dev/null +++ b/src/main/java/frc/robot/Util/RobotConfig.java @@ -0,0 +1,23 @@ +package frc.robot.Util; + +public final class RobotConfig { + + public static class RobotPropertiesConfig{ + public double mass = 68; + public double moi = 15; + } + + public static class BumberConfig{ + public double bumpWidth = 0.2; + public double bumpLength = 1.5; + public double xOffset = 1; + public double yOffset = 1; + } + + public static class ModuleConfig{ + public double wheelRadius = 0.25; + public double driveGearing = 6.12; + public double maxDriveSpeed = 5; + } + +}