Skip to content
This repository has been archived by the owner on Jan 10, 2025. It is now read-only.

Commit

Permalink
Formatting fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@github-actions
github-actions[bot] committed Aug 29, 2024
1 parent 5d2e2c7 commit 5c7921d
Show file tree
Hide file tree
Showing 6 changed files with 76 additions and 59 deletions.
26 changes: 19 additions & 7 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,9 @@
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.Arm.Setpoint;
import frc.robot.subsystems.CommandSwerveDrivetrain.RotationSetpoint;
import frc.robot.subsystems.Climber;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.CommandSwerveDrivetrain.RotationSetpoint;
import frc.robot.subsystems.Index;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.LED;
Expand Down Expand Up @@ -115,11 +115,15 @@ public Robot() {
.withTimeout(2)
.andThen(Commands.parallel(m_shooter.stop(), m_index.stop())),
m_arm.maintain()))));
NamedCommands.registerCommand("Arm", Commands.deferredProxy(() -> m_arm.goToSetpointAuto(Setpoint.kSpeaker).withTimeout(1.2)));
NamedCommands.registerCommand(
"Arm",
Commands.deferredProxy(() -> m_arm.goToSetpointAuto(Setpoint.kSpeaker).withTimeout(1.2)));
NamedCommands.registerCommand(
"Intake", Commands.deferredProxy(() -> m_superstructure.intake()));
NamedCommands.registerCommand("OTFArm", m_arm.moveToPosition(0.7528).andThen(m_arm.maintain()));
NamedCommands.registerCommand("Spinup", Commands.deferredProxy(() -> m_shooter.spinup(500).andThen(m_shooter.maintain())));
NamedCommands.registerCommand(
"Spinup",
Commands.deferredProxy(() -> m_shooter.spinup(500).andThen(m_shooter.maintain())));
NamedCommands.registerCommand("Passthrough", Commands.deferredProxy(() -> m_index.shootAuto()));

// m_autoChooser.setDefaultOption("Center1425", m_drivetrain.getAutoPath("Center1425"));
Expand Down Expand Up @@ -178,22 +182,30 @@ public Robot() {

m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake));
m_driver.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
m_driver.leftBumper().onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kAmp, m_drive, m_scheduler));
m_driver.rightBumper().onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kSpeaker, m_drive, m_scheduler));
m_driver
.leftBumper()
.onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kAmp, m_drive, m_scheduler));
m_driver
.rightBumper()
.onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kSpeaker, m_drive, m_scheduler));

m_codriver.leftBumper().whileTrue(m_index.shoot());
m_codriver.rightBumper().whileTrue(m_superstructure.outake());
m_codriver
.leftTrigger()
.whileTrue(
Commands.parallel(
m_arm.goToSetpoint(Setpoint.kSpeaker, m_codriver.leftTrigger()).andThen(m_arm.maintain()),
m_arm
.goToSetpoint(Setpoint.kSpeaker, m_codriver.leftTrigger())
.andThen(m_arm.maintain()),
m_shooter.spinup(1500).andThen(m_shooter.maintain())));
m_codriver
.rightTrigger()
.whileTrue(
Commands.parallel(
m_arm.goToSetpoint(Setpoint.kAmp, m_codriver.rightTrigger()).andThen(m_arm.maintain()),
m_arm
.goToSetpoint(Setpoint.kAmp, m_codriver.rightTrigger())
.andThen(m_arm.maintain()),
m_shooter.applyVolts(8)));

m_codriverX.whileTrue(m_superstructure.intake()).onFalse(m_intake.stop());
Expand Down
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,7 @@ public enum Setpoint {
new CANSparkMax(Constants.armFollowerPort, MotorType.kBrushless);
private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort);

private final PIDController m_autopid =
new PIDController(10, Constants.armI, Constants.armD);
private final PIDController m_autopid = new PIDController(10, Constants.armI, Constants.armD);
private final PIDController m_pid =
new PIDController(Constants.armP, Constants.armI, Constants.armD);
private final ArmFeedforward m_feedforward =
Expand Down Expand Up @@ -225,7 +224,7 @@ public Command goToSetpoint(Setpoint setpoint, Trigger holdTrigger) {
// case kShuttling -> tempPosition = 0.2;
// }
//
// final double position = tempPosition;
// final double position = tempPosition;
//
// Command moveCommand = moveToPosition(position);
//
Expand All @@ -237,9 +236,9 @@ public Command goToSetpoint(Setpoint setpoint, Trigger holdTrigger) {
// goToSetpoint(Setpoint.kStowed).schedule();
// }
// }
// ).until(() -> !holdTrigger.getAsBoolean()).andThen(goToSetpoint(Setpoint.kStowed));
// ).until(() -> !holdTrigger.getAsBoolean()).andThen(goToSetpoint(Setpoint.kStowed));
// return moveCommand.andThen(holdOrReturnCommand);
}
}

public Command goToSetpoint(Setpoint setpoint) {
double position = 0;
Expand Down
83 changes: 45 additions & 38 deletions src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@
import com.pathplanner.lib.auto.*;
import com.pathplanner.lib.commands.*;
import com.pathplanner.lib.util.*;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
Expand All @@ -46,12 +44,9 @@
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.LimelightHelpers;
import frc.robot.generated.TunerConstants;
Expand Down Expand Up @@ -428,44 +423,56 @@ public Command getAutoPath(String pathName) {
return AutoBuilder.buildAuto(pathName);
}

public Command goToSetpoint(RotationSetpoint setpoint, SwerveRequest.FieldCentric drivetrain, CommandScheduler scheduler) {
public Command goToSetpoint(
RotationSetpoint setpoint,
SwerveRequest.FieldCentric drivetrain,
CommandScheduler scheduler) {
// Use AtomicReference to hold the target position, so it can be accessed in lambdas
AtomicReference<Double> targetPosition = new AtomicReference<>(0.0);

// Determine the target position based on the setpoint
switch (setpoint) {
case kAmp:
targetPosition.set(-(Math.PI / 2)); // Example angle
break;
case kSpeaker:
targetPosition.set(Math.PI); // Facing forward
break;
default:
targetPosition.set(0.0);
break;
case kAmp:
targetPosition.set(-(Math.PI / 2)); // Example angle
break;
case kSpeaker:
targetPosition.set(Math.PI); // Facing forward
break;
default:
targetPosition.set(0.0);
break;
}

// Define the command as a lambda that runs continuously until the robot reaches the target position
return Commands.run(() -> {
// Continuously update the current position
double currentPosition = m_odometry.getEstimatedPosition().getRotation().getRadians() % (2 * Math.PI);
double rotationOutput = m_rotationController.calculate(currentPosition, targetPosition.get());

// Apply the rotational output to the drivetrain
drivetrain.withRotationalRate(rotationOutput);

// Debug print to verify execution
System.out.println("Moving towards setpoint...");

// Specify when the command should stop (when the target position is reached)
})
.until(() -> Math.abs(m_odometry.getEstimatedPosition().getRotation().getRadians() - targetPosition.get()) < 0.01)
.withTimeout(2)
.andThen(() -> {
// Stop the drivetrain when the target position is reached
drivetrain.withRotationalRate(0);
System.out.println("Target position reached.");
});
}

// Define the command as a lambda that runs continuously until the robot reaches the target
// position
return Commands.run(
() -> {
// Continuously update the current position
double currentPosition =
m_odometry.getEstimatedPosition().getRotation().getRadians() % (2 * Math.PI);
double rotationOutput =
m_rotationController.calculate(currentPosition, targetPosition.get());

// Apply the rotational output to the drivetrain
drivetrain.withRotationalRate(rotationOutput);

// Debug print to verify execution
System.out.println("Moving towards setpoint...");

// Specify when the command should stop (when the target position is reached)
})
.until(
() ->
Math.abs(
m_odometry.getEstimatedPosition().getRotation().getRadians()
- targetPosition.get())
< 0.01)
.withTimeout(2)
.andThen(
() -> {
// Stop the drivetrain when the target position is reached
drivetrain.withRotationalRate(0);
System.out.println("Target position reached.");
});
}
}
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/subsystems/Index.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,19 +40,19 @@ public Command shoot() {
// if (!m_hasNote.getAsBoolean()) {
// return runOnce(() -> {}).withName("Empty Index");
// } else {
return run(() -> m_motor.setVoltage(-8))
// .until(m_hasNote.negate())
.withName("Index PassThrough Teleop");
return run(() -> m_motor.setVoltage(-8))
// .until(m_hasNote.negate())
.withName("Index PassThrough Teleop");
// }
}

public Command shootAuto() {
// if (!m_hasNote.getAsBoolean()) {
// return runOnce(() -> {}).withName("Empty Index");
// } else {
return run(() -> m_motor.setVoltage(-8))
.until(m_hasNote.negate())
.withName("Index PassThrough Auto");
return run(() -> m_motor.setVoltage(-8))
.until(m_hasNote.negate())
.withName("Index PassThrough Auto");
// }
}

Expand Down
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/subsystems/LED.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,15 +34,14 @@ public Command hasNote() {
});
}

public Command intaking() {
public Command intaking() {
return defer(
() -> {
m_activeState.set("intaking");
return run(() -> m_blinkin.setSpeed(0.73));
});
}


public Command hotpink() {
return defer(
() -> {
Expand Down
2 changes: 1 addition & 1 deletion src/main/warp-paths-v1.chor
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

0 comments on commit 5c7921d

Please sign in to comment.