Skip to content

Commit

Permalink
PHR 3/2
Browse files Browse the repository at this point in the history
  • Loading branch information
@ControlsNarwhal
ControlsNarwhal committed Mar 3, 2024
1 parent d30d2fc commit 302f8a3
Show file tree
Hide file tree
Showing 10 changed files with 52 additions and 30 deletions.
6 changes: 0 additions & 6 deletions src/main/deploy/pathplanner/autos/leave.auto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,6 @@
"data": {
"pathName": "leave"
}
},
{
"type": "named",
"data": {
"name": "Shoot"
}
}
]
}
Expand Down
4 changes: 2 additions & 2 deletions src/main/deploy/pathplanner/paths/midBottom-note2.3.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,11 @@
{
"anchor": {
"x": 7.8,
"y": 4.1
"y": 4.05
},
"prevControl": {
"x": 6.3,
"y": 4.1
"y": 4.05
},
"nextControl": null,
"isLocked": false,
Expand Down
4 changes: 2 additions & 2 deletions src/main/deploy/pathplanner/paths/note1.2-note2.3.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,11 +33,11 @@
{
"anchor": {
"x": 7.8,
"y": 4.1
"y": 4.05
},
"prevControl": {
"x": 6.8,
"y": 4.1
"y": 4.05
},
"nextControl": null,
"isLocked": false,
Expand Down
4 changes: 2 additions & 2 deletions src/main/deploy/pathplanner/paths/note2.3-midBottom.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,12 @@
{
"anchor": {
"x": 7.8,
"y": 4.1
"y": 4.05
},
"prevControl": null,
"nextControl": {
"x": 6.3,
"y": 4.1
"y": 4.05
},
"isLocked": false,
"linkedName": "note2.3"
Expand Down
4 changes: 2 additions & 2 deletions src/main/deploy/pathplanner/paths/note2.3-middle.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,12 @@
{
"anchor": {
"x": 7.8,
"y": 4.1
"y": 4.05
},
"prevControl": null,
"nextControl": {
"x": 4.8,
"y": 4.1
"y": 4.05
},
"isLocked": false,
"linkedName": "note2.3"
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/team3128/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -304,7 +304,7 @@ public static class ShooterConstants {
public static final double CURRENT_TEST_EXPECTED_CURRENT = 0;

public static final double SHOOTER_TEST_PLATEAU = 1;
public static final double SHOOTER_TEST_TIMEOUT = 1.7;
public static final double SHOOTER_TEST_TIMEOUT = 1.8;
}

public static class ClimberConstants {
Expand Down
14 changes: 9 additions & 5 deletions src/main/java/frc/team3128/RobotContainer.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package frc.team3128;

import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
Expand Down Expand Up @@ -48,7 +50,7 @@ public class RobotContainer {
private Intake intake;
private Leds leds;

// private NAR_Joystick rightStick;
private NAR_ButtonBoard judgePad;
private NAR_ButtonBoard buttonPad;

public static NAR_XboxController controller;
Expand All @@ -71,7 +73,7 @@ public RobotContainer() {
climber.addClimberTests();
intake.addIntakeTests();

// rightStick = new NAR_Joystick(1);
// judgePad = new NAR_ButtonBoard(1);
controller = new NAR_XboxController(2);
buttonPad = new NAR_ButtonBoard(3);

Expand All @@ -88,13 +90,13 @@ public RobotContainer() {
private void configureButtonBindings() {
controller.getButton(XboxButton.kB).onTrue(runOnce(()-> swerve.resetEncoders()));

controller.getButton(XboxButton.kRightBumper).onTrue(rampUp(ShooterConstants.MAX_RPM, 25)).onFalse(shoot(ShooterConstants.MAX_RPM, 25)); //Ram Shot
controller.getButton(XboxButton.kRightBumper).onTrue(rampUp(ShooterConstants.MAX_RPM, 27)).onFalse(shoot(ShooterConstants.MAX_RPM, 25)); //Ram Shot
controller.getButton(XboxButton.kRightTrigger).onTrue(rampUpContinuous()).onFalse(autoShoot()); //Auto Shoot
controller.getButton(XboxButton.kY).onTrue(rampUp(5000, 15)).onFalse(feed(5000, 15,155)); //Feed Shot
controller.getButton(XboxButton.kX).onTrue(intake.intakePivot.pivotTo(-55)).onFalse(ampShoot()); //Amp Shot

controller.getButton(XboxButton.kA).onTrue(sequence(runOnce(()-> intake.isRetracting = false), intake.intakePivot.pivotTo(-150), climber.climbTo(Climber.Setpoint.EXTENDED))); //Extend Climber
controller.getButton(XboxButton.kBack).onTrue(sequence(climber.setClimber(-0.25), waitSeconds(1), climber.setClimber(-1), waitUntil(()->climber.isClimbed()), climber.setClimber(0))); //Retract Climber
controller.getButton(XboxButton.kBack).onTrue(sequence(climber.setClimber(-0.35), waitSeconds(1), climber.setClimber(-1), waitUntil(()->climber.isClimbed()), climber.setClimber(0))); //Retract Climber

controller.getButton(XboxButton.kLeftTrigger).onTrue(intake.intake(Intake.Setpoint.EXTENDED)); //Extend Intake
controller.getButton(XboxButton.kLeftBumper).onTrue(intake.retract(false)); //Retract Intake
Expand All @@ -117,6 +119,8 @@ private void configureButtonBindings() {
CmdSwerveDrive.setTurnSetpoint(Robot.getAlliance() == Alliance.Red ? 270 : 90);
}));

// judgePad.getButton(1).onTrue(runOnce(()-> swerve.resetOdometry(new Pose2d(5, 5, Rotation2d.fromDegrees(180)))));

// rightStick.getButton(1).onTrue(runOnce(()-> swerve.zeroGyro(0)));
// rightStick.getButton(3).onTrue(new CmdSysId("Rotation", (Double radiansPerSec) -> swerve.drive(new Translation2d(), radiansPerSec, true), ()-> Units.radiansToDegrees(swerve.getRobotVelocity().omegaRadiansPerSecond), swerve));
// rightStick.getButton(4).onTrue(runOnce(()-> swerve.resetOdometry(new Pose2d(1.35, FocalAimConstants.speakerMidpointY, Rotation2d.fromDegrees(180)))));
Expand Down Expand Up @@ -192,7 +196,7 @@ public void initDashboard() {
Leds.getInstance().setLedColor(Colors.ERROR);
}
else {
Log.info("Colors", "Errors configuring: " + NAR_CANSpark.getNumFailedConfigs());
Log.recoverable("Colors", "Errors configuring: " + NAR_CANSpark.getNumFailedConfigs());
Leds.getInstance().setLedColor(Colors.CONFIGURED);
}
}
Expand Down
14 changes: 11 additions & 3 deletions src/main/java/frc/team3128/commands/CmdManager.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package frc.team3128.commands;

import common.hardware.input.NAR_XboxController;
import common.utility.Log;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ScheduleCommand;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
Expand Down Expand Up @@ -33,6 +35,7 @@ public static Command vibrateController(){

public static Command autoShoot() {
return sequence(
runOnce(()-> DriverStation.reportWarning("AutoShoot: CommandStarting", false)),
parallel(
rampUp(),
swerve.turnInPlace().asProxy().withTimeout(1)
Expand All @@ -41,7 +44,8 @@ public static Command autoShoot() {
),
intake.intakeRollers.outtakeNoRequirements(),
waitSeconds(0.1),
neutral(false)
neutral(false),
runOnce(()-> DriverStation.reportWarning("AutoShoot: CommandEnding", false))
);
}

Expand All @@ -57,10 +61,12 @@ public static Command ampShoot() {

public static Command shoot(double rpm, double height){
return sequence(
runOnce(()-> DriverStation.reportWarning("Shoot: CommandStarting", false)),
rampUp(rpm, height),
intake.intakeRollers.outtakeNoRequirements(),
waitSeconds(0.1),
neutral(false)
neutral(false),
runOnce(()-> DriverStation.reportWarning("Shoot: CommandEnding", false))
);
}

Expand Down Expand Up @@ -115,6 +121,7 @@ public static Command rampUpContinuous(double rpm, DoubleSupplier height) {

public static Command neutral(boolean shouldStall){
return sequence(
runOnce(()-> DriverStation.reportWarning("Neutral: CommandStarting", false)),
intake.intakeRollers.runNoRequirements(0),
shooter.setShooter(0),
climber.climbTo(Climber.Setpoint.RETRACTED),
Expand All @@ -128,7 +135,8 @@ public static Command neutral(boolean shouldStall){
waitSeconds(0.5),
climber.reset()
)
)
),
runOnce(()-> DriverStation.reportWarning("Neutral: CommandEnding", false))
);
}
}
4 changes: 3 additions & 1 deletion src/main/java/frc/team3128/subsystems/Climber.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

import static frc.team3128.Constants.ClimberConstants.*;
import static frc.team3128.Constants.ClimberConstants.SETPOINT_TEST_TIMEOUT_EXTEND;
Expand Down Expand Up @@ -59,6 +60,7 @@ public void initShuffleboard() {
NAR_Shuffleboard.addSendable("Commands", "Shooter", Shooter.getInstance(), 0, 2);
NAR_Shuffleboard.addSendable("Commands", "IntakePivot", Intake.getInstance().intakePivot, 0, 3);
NAR_Shuffleboard.addSendable("Commands", "IntakeRollers", Intake.getInstance().intakeRollers, 0, 4);
NAR_Shuffleboard.addSendable("Commands", "CommandScheduler", CommandScheduler.getInstance(), 3, 0);

}

Expand Down Expand Up @@ -136,7 +138,7 @@ public boolean isNeutral() {
}

public boolean isClimbed() {
return getMeasurement() < -1.0;
return getMeasurement() < -5.0;
}

public State getRunningState() {
Expand Down
26 changes: 20 additions & 6 deletions src/main/java/frc/team3128/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,17 +4,18 @@
import common.core.subsystems.ManipulatorTemplate;
import common.core.subsystems.PivotTemplate;
import common.hardware.motorcontroller.NAR_Motor.Neutral;
import common.utility.Log;
import common.utility.narwhaldashboard.NarwhalDashboard.State;
import common.utility.tester.CurrentTest;
import common.utility.tester.Tester;
import common.utility.tester.Tester.*;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.util.Units;
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.InstantCommand;
import frc.team3128.Constants.LedConstants.Colors;
import frc.team3128.commands.CmdManager;

import static edu.wpi.first.wpilibj2.command.Commands.*;
import static frc.team3128.Constants.IntakeConstants.*;

Expand Down Expand Up @@ -48,6 +49,11 @@ protected void configMotors() {
PIVOT_MOTOR.setNeutralMode(Neutral.COAST);
}

@Override
public void useOutput(double output, double setpoint) {
PIVOT_MOTOR.setVolts(MathUtil.clamp(output, -12, 12));
}

public Command pivotNoRequirements(double setpoint) {
return new InstantCommand(()-> startPID(setpoint));
}
Expand Down Expand Up @@ -102,11 +108,13 @@ public Command miniOuttake() {

public Command serialize() {
return sequence(
runOnce(()-> DriverStation.reportWarning("Serialize: CommandStarting", false)),
runManipulator(-0.1),
waitUntil(()-> !hasObjectPresent()),
runManipulator(0.1),
waitUntil(()-> hasObjectPresent()),
runManipulator(0)
runManipulator(0),
runOnce(()-> DriverStation.reportWarning("Serialize: CommandEnding", false))
);
}

Expand Down Expand Up @@ -152,6 +160,7 @@ private Intake(){

public Command retract(boolean shouldStall) {
return sequence(
runOnce(()-> DriverStation.reportWarning("Retract: CommandStarting", false)),
runOnce(()-> Leds.getInstance().setLedColor(Colors.PIECE)),
// CmdManager.vibrateController(),
runOnce(()-> isRetracting = true),
Expand All @@ -170,27 +179,32 @@ public Command retract(boolean shouldStall) {
intakePivot.reset(0),
runOnce(()-> Leds.getInstance().setDefaultColor())
)
)
),
runOnce(()-> DriverStation.reportWarning("Retract: CommandEnding", false))
);
}

public Command intake(Setpoint setpoint) {
return sequence(
runOnce(()-> DriverStation.reportWarning("Intake: CommandStarting", false)),
runOnce(()-> isRetracting = true),
intakeRollers.runManipulator(INTAKE_POWER),
intakePivot.pivotTo(setpoint.angle),
intakeRollers.intake(),
retract(true)
retract(true),
runOnce(()-> DriverStation.reportWarning("Intake: CommandEnding", false))
);
}

public Command outtake() {
return sequence (
runOnce(()-> DriverStation.reportWarning("Outtake: CommandStarting", false)),
intakePivot.pivotTo(Setpoint.EXTENDED.angle),
waitUntil(()-> intakePivot.atSetpoint()),
intakeRollers.outtake(),
waitSeconds(0.5),
retract(false)
retract(false),
runOnce(()-> DriverStation.reportWarning("Outtake: CommanedEnding", false))
);
}

Expand Down

0 comments on commit 302f8a3

Please sign in to comment.