Skip to content

Commit

Permalink
Merge branch 'main' into LED-
Browse files Browse the repository at this point in the history
  • Loading branch information
@Jetblackdragon
Jetblackdragon authored Mar 19, 2024
2 parents 8f478b1 + a24da4b commit 9ee6373
Show file tree
Hide file tree
Showing 8 changed files with 207 additions and 155 deletions.
22 changes: 14 additions & 8 deletions src/main/java/frc/team2412/robot/Controls.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,12 @@
import frc.team2412.robot.commands.intake.FeederInCommand;
import frc.team2412.robot.commands.intake.IntakeRejectCommand;
import frc.team2412.robot.commands.launcher.FullTargetCommand;
import frc.team2412.robot.commands.launcher.ManualAngleCommand;
import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand;
import frc.team2412.robot.commands.launcher.SetAngleCommand;
import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand;
import frc.team2412.robot.commands.launcher.SetPivotCommand;
import frc.team2412.robot.subsystems.LauncherSubsystem;
import frc.team2412.robot.util.TrapAlign;

public class Controls {
public static class ControlConstants {
Expand All @@ -47,8 +49,9 @@ public static class ControlConstants {
// Launcher
private final Trigger launcherAmpPresetButton;
private final Trigger launcherSubwooferPresetButton;
private final Trigger launcherLowerPresetButton;
// private final Trigger launcherPodiumPresetButton;
// private final Trigger launcherTrapPresetButton;
private final Trigger launcherTrapPresetButton;
private final Trigger launcherLaunchButton;

private final Subsystems s;
Expand All @@ -60,9 +63,10 @@ public Controls(Subsystems s) {

launcherAmpPresetButton = codriveController.x();
launcherSubwooferPresetButton = codriveController.a();
launcherLowerPresetButton = codriveController.y();
// launcherPodiumPresetButton = codriveController.povLeft();
// launcherTrapPresetButton = codriveController.y();
launcherLaunchButton = codriveController.leftBumper();
launcherTrapPresetButton = codriveController.rightTrigger();
launcherLaunchButton = codriveController.rightBumper();
// intake controls (confirmed with driveteam)
driveIntakeInButton = driveController.a();
driveIntakeStopButton = driveController.b();
Expand Down Expand Up @@ -114,7 +118,7 @@ private void bindDrivebaseControls() {
() -> Rotation2d.fromRotations(driveController.getRightX())));
driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro));
driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels));
// driveController
// driveController x
// .back()
// .onTrue(
// new InstantCommand(
Expand Down Expand Up @@ -145,12 +149,14 @@ private void bindLauncherControls() {
CommandScheduler.getInstance()
.setDefaultCommand(
s.launcherSubsystem,
new SetAngleCommand(
new ManualAngleCommand(
s.launcherSubsystem,
() ->
MathUtil.applyDeadband(codriveController.getLeftY(), 0.1)
* LauncherSubsystem.ANGLE_MAX_SPEED));

launcherLowerPresetButton.onTrue(
new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE));
launcherSubwooferPresetButton.onTrue(
new SetAngleLaunchCommand(
s.launcherSubsystem,
Expand All @@ -166,8 +172,8 @@ private void bindLauncherControls() {
s.launcherSubsystem,
LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
LauncherSubsystem.AMP_AIM_ANGLE));
// launcherTrapPresetButton.onTrue(
// TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
launcherTrapPresetButton.onTrue(
TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));

codriveController.b().whileTrue(s.launcherSubsystem.run(s.launcherSubsystem::stopLauncher));

Expand Down
10 changes: 4 additions & 6 deletions src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,7 @@

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.team2412.robot.commands.launcher.SetAngleCommand;
import frc.team2412.robot.commands.launcher.SetLaunchSpeedCommand;
import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand;
import frc.team2412.robot.commands.launcher.StopLauncherCommand;
import frc.team2412.robot.subsystems.LauncherSubsystem;

Expand All @@ -15,12 +14,11 @@ public LauncherDiagnosticCommand(LauncherSubsystem launcherSubsystem) {
this.launcherSubsystem = launcherSubsystem;
this.Angle = launcherSubsystem.getAngle();
addCommands(
new SetAngleCommand(launcherSubsystem, () -> 45),
new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 10),
new WaitCommand(2),
new SetAngleCommand(launcherSubsystem, () -> 90),
new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 20),
new WaitCommand(1),
new SetAngleCommand(launcherSubsystem, () -> Angle),
new SetLaunchSpeedCommand(launcherSubsystem, 100),
new SetAngleLaunchCommand(launcherSubsystem, 500, Angle),
new WaitCommand(5),
new StopLauncherCommand(launcherSubsystem));
}
Expand Down
12 changes: 5 additions & 7 deletions ...ot/commands/launcher/SetAngleCommand.java → ...commands/launcher/ManualAngleCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,21 +5,19 @@
import java.util.function.DoubleSupplier;
// this command adjusts the angle using a speed that is input by the user

public class SetAngleCommand extends Command {
public class ManualAngleCommand extends Command {
private final LauncherSubsystem launcherSubsystem;
private final DoubleSupplier launcherAngleSpeed;
private final DoubleSupplier launcherAngle;

public SetAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) {
public ManualAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) {
this.launcherSubsystem = launcherSubsystem;
this.launcherAngleSpeed = angleSpeed;
this.launcherAngle = angleSpeed;
addRequirements(launcherSubsystem);
}

@Override
public void execute() {
if (launcherAngleSpeed.getAsDouble() != 0.0) {
launcherSubsystem.setAngleSpeed(launcherAngleSpeed.getAsDouble());
}
launcherSubsystem.setAngleManual(launcherAngle.getAsDouble());
}

@Override
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ public SetAngleAmpLaunchCommand(LauncherSubsystem launcherSubsystem, double spee
public void initialize() {
launcherSubsystem.setAngle(launcherAngle);
launcherSubsystem.ampLaunch(launcherSpeed);
launcherSubsystem.manualSetpoint(launcherAngle);
}

@Override
Expand Down
26 changes: 26 additions & 0 deletions src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
package frc.team2412.robot.commands.launcher;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj2.command.Command;
import frc.team2412.robot.subsystems.LauncherSubsystem;

public class SetPivotCommand extends Command {
private final LauncherSubsystem launcherSubsystem;
private final double angle;

public SetPivotCommand(LauncherSubsystem launcherSubsystem, double angle) {
this.launcherSubsystem = launcherSubsystem;
this.angle = angle;
addRequirements(launcherSubsystem);
}

@Override
public void initialize() {
launcherSubsystem.setAngle(angle);
}

@Override
public boolean isFinished() {
return MathUtil.isNear(angle, launcherSubsystem.getAngle(), LauncherSubsystem.ANGLE_TOLERANCE);
}
}
149 changes: 69 additions & 80 deletions src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.team2412.robot.Robot;
import java.util.Map;

public class IntakeSubsystem extends SubsystemBase {
Expand Down Expand Up @@ -48,83 +49,17 @@ public class IntakeSubsystem extends SubsystemBase {
private final DigitalInput feederSensor;

// Shuffleboard
// speed
private final GenericEntry setIntakeInSpeedEntry =
Shuffleboard.getTab("Intake")
.addPersistent("Intake in speed - ", INTAKE_IN_SPEED)
.withSize(2, 1)
.withProperties(Map.of("Min", -1, "Max", 1))
.getEntry();

private final GenericEntry setIndexInSpeedEntry =
Shuffleboard.getTab("Intake")
.add("Index in speed - ", INDEX_UPPER_IN_SPEED)
.withSize(1, 1)
.getEntry();

private final GenericEntry setFeederInSpeedEntry =
Shuffleboard.getTab("Intake")
.add("Feeder in speed - ", FEEDER_IN_SPEED)
.withSize(1, 1)
.getEntry();

// temperature
private final GenericEntry intakeMotorFrontTemp =
Shuffleboard.getTab("Intake")
.add("Front Intake temp", 0)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView)
.getEntry();

private final GenericEntry intakeMotorBackTemp =
Shuffleboard.getTab("Intake")
.add("Back Intake temp", 0)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView)
.getEntry();

private final GenericEntry intakeMotorLeftTemp =
Shuffleboard.getTab("Intake")
.add("Left Intake temp", 0)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView)
.getEntry();
private final ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake");
// speed
private GenericEntry setIntakeInSpeedEntry;

private final GenericEntry intakeMotorRightTemp =
Shuffleboard.getTab("Intake")
.add("Right Intake temp", 0)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView)
.getEntry();
private GenericEntry setIndexInSpeedEntry;

private final GenericEntry indexMotorUpperTemp =
Shuffleboard.getTab("Intake")
.add("Upper Index temp", 0)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView)
.getEntry();

private final GenericEntry ingestMotorTemp =
Shuffleboard.getTab("Intake")
.add("Ingest temp", 0)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView)
.getEntry();

private final GenericEntry feederMotorTemp =
Shuffleboard.getTab("Intake")
.add("Feeder temp", 0)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView)
.getEntry();
private GenericEntry setFeederInSpeedEntry;

// sensor override
private final GenericEntry sensorOverride =
Shuffleboard.getTab("Intake")
.add("Override Sensors", false)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kToggleSwitch)
.getEntry();
private GenericEntry sensorOverride;

public IntakeSubsystem() {
intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless);
Expand All @@ -142,9 +77,7 @@ public IntakeSubsystem() {

resetMotors();

ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake");
shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1);
shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1);
initShuffleboard();
}

private void configureMotor(CANSparkBase motor, int currentLimit, boolean invert) {
Expand All @@ -166,9 +99,13 @@ private void resetMotors() {
configureMotor(intakeMotorRight, true);

configureMotor(ingestMotor, false);
configureMotor(indexMotorUpper, 40, true);
configureMotor(indexMotorUpper, 40, false);

configureMotor(feederMotor, 40, true);
configureMotor(feederMotor, 40, false);
indexMotorUpper
.getForwardLimitSwitch(com.revrobotics.SparkLimitSwitch.Type.kNormallyOpen)
.enableLimitSwitch(false);
indexMotorUpper.burnFlash();
}

public void intakeSet(double speed) {
Expand Down Expand Up @@ -241,6 +178,7 @@ public boolean getSensorOverride() {
return sensorOverride.getBoolean(false);
}


public boolean isIntakeOn() {
return (intakeMotorFront.get() > 0
|| indexMotorUpper.get() > 0
Expand All @@ -254,11 +192,62 @@ public void periodic() {
intakeMotorBackTemp.setDouble(intakeMotorBack.getMotorTemperature());
intakeMotorRightTemp.setDouble(intakeMotorRight.getMotorTemperature());
intakeMotorLeftTemp.setDouble(intakeMotorLeft.getMotorTemperature());

// logging

ingestMotorTemp.setDouble(ingestMotor.getMotorTemperature());
public void initShuffleboard() {
if (Robot.isDebugMode()) {
shuffleboardTab
.addDouble("Front Intake Motor Temp", () -> intakeMotorFront.getMotorTemperature())
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView);
shuffleboardTab
.addDouble("Back Intake Motor Temp", () -> intakeMotorBack.getMotorTemperature())
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView);
shuffleboardTab
.addDouble("Left Intake Motor Temp", () -> intakeMotorLeft.getMotorTemperature())
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView);
shuffleboardTab
.addDouble("Right Intake Motor Temp", () -> intakeMotorRight.getMotorTemperature())
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView);
shuffleboardTab
.addDouble("Ingest Motor Temp", () -> ingestMotor.getMotorTemperature())
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView);
shuffleboardTab
.addDouble("Index Motor Temp", () -> indexMotorUpper.getMotorTemperature())
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView);
shuffleboardTab
.addDouble("Feeder Motor Temp", () -> feederMotor.getMotorTemperature())
.withSize(1, 1)
.withWidget(BuiltInWidgets.kTextView);
}

indexMotorUpperTemp.setDouble(indexMotorUpper.getMotorTemperature());
shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1);
shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1);

feederMotorTemp.setDouble(feederMotor.getMotorTemperature());
setIntakeInSpeedEntry =
shuffleboardTab
.addPersistent("Intake in speed - ", INTAKE_IN_SPEED)
.withSize(2, 1)
.withProperties(Map.of("Min", -1, "Max", 1))
.getEntry();

setIndexInSpeedEntry =
shuffleboardTab.add("Index in speed - ", INDEX_UPPER_IN_SPEED).withSize(1, 1).getEntry();

setFeederInSpeedEntry =
shuffleboardTab.add("Feeder in speed - ", FEEDER_IN_SPEED).withSize(1, 1).getEntry();

sensorOverride =
Shuffleboard.getTab("Intake")
.add("Override Sensors", false)
.withSize(1, 1)
.withWidget(BuiltInWidgets.kToggleSwitch)
.getEntry();
}
}
Loading

0 comments on commit 9ee6373

Please sign in to comment.