If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *
If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 79ff5fd..78484b6 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,35 +4,30 @@
package frc.robot;
-import java.time.LocalDateTime;
-import java.time.format.DateTimeFormatter;
-import java.util.Optional;
-import org.littletonrobotics.urcl.URCL;
-
-import com.ctre.phoenix6.SignalLogger;
-
import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.lib.math.FiringSolutions;
+
import frc.lib.math.FiringSolutionsV3;
+import org.littletonrobotics.urcl.URCL;
+
+import java.time.LocalDateTime;
+import java.time.format.DateTimeFormatter;
+import java.util.Optional;
+
/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the
- * name of this class or
- * the package after creating this project, you must also update the
- * build.gradle file in the
- * project.
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the project.
*/
public class Robot extends TimedRobot {
public static final CTREConfigs ctreConfigs = new CTREConfigs();
@@ -43,7 +38,7 @@ public class Robot extends TimedRobot {
private static boolean redAlliance;
- //PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev);
+ PowerDistribution PDH;
/**
* This function is run when the robot is first started up and should be used for any
@@ -55,6 +50,8 @@ public void robotInit() {
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
+ PDH = new PowerDistribution(1, ModuleType.kRev);
+
// Disable LiveWindow since we don't use it
LiveWindow.disableAllTelemetry();
LiveWindow.setEnabled(false);
@@ -62,7 +59,10 @@ public void robotInit() {
DriverStation.silenceJoystickConnectionWarning(true);
// Starts recording to data log
- DataLogManager.start("/media/sda1/logs/", DateTimeFormatter.ofPattern("yyyy-MM-dd__HH-mm-ss").format(LocalDateTime.now())+".wpilog");
+ DataLogManager.start(
+ "/media/sda1/logs/",
+ DateTimeFormatter.ofPattern("yyyy-MM-dd__HH-mm-ss").format(LocalDateTime.now())
+ + ".wpilog");
// Record both DS control and joystick data
DriverStation.startDataLog(DataLogManager.getLog());
@@ -73,17 +73,21 @@ public void robotInit() {
URCL.start();
// Log data from all CTRE devices
- SignalLogger.setPath("/media/sda1/logs/");
- SignalLogger.start();
+ // SignalLogger.setPath("/media/sda1/logs/");
+ // SignalLogger.start();
// Output command scheduler to dashboard
SmartDashboard.putData(CommandScheduler.getInstance());
// Access PhotonVision dashboard when connected via usb TODO make work
- // PortForwarder.add(5800, "10.17.10.11", 5800);
+ PortForwarder.add(5800, "10.17.10.11", 5800);
- // idk if this is useful
- System.gc();
+ SmartDashboard.putData(PDH);
+
+ redAlliance = checkRedAlliance();
+
+ // idk if this is useful
+ // System.gc();
}
/**
@@ -100,7 +104,7 @@ public void robotPeriodic() {
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
- //SmartDashboard.putData(PDH);
+ SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime());
}
/** Gets the current alliance, true is red */
@@ -108,22 +112,28 @@ public static boolean getAlliance() {
return redAlliance;
}
- public boolean checkRedAlliance() {
+ public static boolean checkRedAlliance() {
Optional
+ * Does NOT have optimization, meaning wheels have to be facing same direction
+ */
+ public void voltageDrive(double Volts) {
mDriveMotor.setControl(driveCharacteriztionControl.withOutput(Volts));
}
- public double getMotorVoltage(){
+ /** Get drive motor voltage */
+ public double getMotorVoltage() {
return mDriveMotor.getMotorVoltage().getValue();
}
- public double getMotorVelocity(){
- return mDriveMotor.getVelocity().getValue();
+ /** IN METERS PER SECOND */
+ public double getMotorVelocity() {
+ return Conversions.RPSToMPS(
+ mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference);
}
-}
\ No newline at end of file
+}
diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java
new file mode 100644
index 0000000..8a2917a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AimBot.java
@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.lib.math.FiringSolutionsV3;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.subsystems.IntexerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.SwerveSubsystem;
+
+public class AimBot extends Command {
+
+ private ShooterSubsystem shooter;
+ private SwerveSubsystem swerveSubsystem;
+ private IntexerSubsystem intexer;
+ private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04);
+ private double speed;
+ private Timer timer = new Timer();
+
+ /** Creates a new AimBot. */
+ public AimBot(
+ ShooterSubsystem shooterSubsystem,
+ SwerveSubsystem swerve,
+ IntexerSubsystem intexer,
+ double speed) {
+ this.shooter = shooterSubsystem;
+ this.speed = speed;
+ this.intexer = intexer;
+ this.swerveSubsystem = swerve;
+ addRequirements(shooterSubsystem, swerve);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ shooter.setShooterVelocity(speed);
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ Pose2d pose = swerveSubsystem.getPose();
+ ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds();
+
+ double offset;
+ if (Robot.getAlliance()) {
+ if (pose.getRotation().getRadians() > 0) {
+ offset = -Math.toRadians(180);
+ } else {
+ offset = Math.toRadians(180);
+ }
+ } else {
+ offset = 0;
+ }
+
+ double rotationVal = rotationPID.calculate(
+ pose.getRotation().getRadians() + offset,
+ FiringSolutionsV3.getAngleToMovingTarget(
+ pose.getX(),
+ pose.getY(),
+ FiringSolutionsV3.speakerTargetX,
+ FiringSolutionsV3.speakerTargetY,
+ currentSpeed.vxMetersPerSecond,
+ currentSpeed.vyMetersPerSecond,
+ pose.getRotation().getRadians()));
+
+ swerveSubsystem.drive(
+ new Translation2d(0, 0).times(Constants.Swerve.maxSpeed),
+ rotationVal * Constants.Swerve.maxAngularVelocity,
+ true,
+ false);
+
+ if (shooter.isShooterAtSpeed() && rotationPID.getPositionError() <= .035) {
+ timer.reset();
+ timer.start();
+ intexer.setShooterIntake(.9);
+ }
+
+ shooter.setWristByAngle(shooter.getCalculatedAngleToSpeaker());
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ swerveSubsystem.setChassisSpeeds(new ChassisSpeeds(0, 0, 0));
+ intexer.setShooterIntake(0);
+ shooter.setWristSpeedManual(0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return !intexer.shooterBreak() && timer.get() > .2;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/ElevationManual.java b/src/main/java/frc/robot/commands/ElevationManual.java
index 4ae7d4c..57bda1e 100644
--- a/src/main/java/frc/robot/commands/ElevationManual.java
+++ b/src/main/java/frc/robot/commands/ElevationManual.java
@@ -4,18 +4,22 @@
package frc.robot.commands;
-import java.util.function.DoubleSupplier;
-
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.Constants;
import frc.robot.subsystems.ElevatorSubsystem;
+import java.util.function.DoubleSupplier;
+
public class ElevationManual extends Command {
private ElevatorSubsystem m_elevatorSubsystem;
double m_speed;
DoubleSupplier axis;
Boolean locked = false;
double lockedValue = 0.0;
+ double lastElevatorSetpoint = 0.0;
public ElevationManual(ElevatorSubsystem elevate, DoubleSupplier control) {
m_elevatorSubsystem = elevate;
@@ -25,23 +29,25 @@ public ElevationManual(ElevatorSubsystem elevate, DoubleSupplier control) {
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
- double value = axis.getAsDouble();
+ lastElevatorSetpoint = m_elevatorSubsystem.getSetpoint();
+ double value = axis.getAsDouble();
+ value = MathUtil.applyDeadband(value, Constants.stickDeadband);
value = Math.pow(value, 3);
- if (Math.abs(value) > .05) { // Crime zone
- m_elevatorSubsystem.setManualOverride(true);
- m_elevatorSubsystem.ManSpin(value);
- } else {
- m_elevatorSubsystem.setManualOverride(false);
- lockedValue = m_elevatorSubsystem.getEncoderValue();
- }
+ // if (Math.abs(value) > .1) { // Crime zone
+ m_elevatorSubsystem.ManSpin(value);
+ /* } else {
+ if (!m_elevatorSubsystem.locked){
+ lastElevatorSetpoint = m_elevatorSubsystem.getPosition();
+ m_elevatorSubsystem.setPositionWithEncoder(lastElevatorSetpoint);
+ }
+ }*/
SmartDashboard.putBoolean("locked", locked);
SmartDashboard.putNumber("locked value", lockedValue);
@@ -49,8 +55,7 @@ public void execute() {
// Called once the command ends or is interrupted.
@Override
- public void end(boolean interrupted) {
- }
+ public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
diff --git a/src/main/java/frc/robot/commands/ElevatorSet.java b/src/main/java/frc/robot/commands/ElevatorSet.java
index bae921b..5a8f0a0 100644
--- a/src/main/java/frc/robot/commands/ElevatorSet.java
+++ b/src/main/java/frc/robot/commands/ElevatorSet.java
@@ -5,6 +5,7 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.robot.subsystems.ElevatorSubsystem;
public class ElevatorSet extends Command {
@@ -21,7 +22,7 @@ public ElevatorSet(ElevatorSubsystem elevator, double position) {
// Called when the command is initially scheduled.
@Override
public void initialize() {
- //m_elevator.setManualOverride(true);
+ // m_elevator.setManualOverride(true);
}
// Called every time the scheduler runs while the command is scheduled.
@@ -32,9 +33,7 @@ public void execute() {
// Called once the command ends or is interrupted.
@Override
- public void end(boolean interrupted) {
- m_elevator.stopHere();
- }
+ public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
diff --git a/src/main/java/frc/robot/commands/FIREEE.java b/src/main/java/frc/robot/commands/FIREEE.java
index 24fe217..641d227 100644
--- a/src/main/java/frc/robot/commands/FIREEE.java
+++ b/src/main/java/frc/robot/commands/FIREEE.java
@@ -5,6 +5,7 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.robot.subsystems.IntexerSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
@@ -21,13 +22,12 @@ public FIREEE(ShooterSubsystem shooterSub, IntexerSubsystem intex) {
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
- if (shooter.shooterAtSpeed()){
+ if (shooter.isShooterAtSpeed()) {
intexer.setShooterIntake(.9);
}
}
diff --git a/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java b/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java
new file mode 100644
index 0000000..8f50a4b
--- /dev/null
+++ b/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.subsystems.IntexerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class FIREEFORACERTAINAMOUNTOFTIME extends Command {
+ private ShooterSubsystem shooter;
+ private IntexerSubsystem intexer;
+ private double time;
+ private Timer timer = new Timer();
+
+ public FIREEFORACERTAINAMOUNTOFTIME(
+ ShooterSubsystem shooterSub, IntexerSubsystem intex, double time) {
+ shooter = shooterSub;
+ intexer = intex;
+ this.time = time;
+ // Use addRequirements() here to declare subsystem dependencies.
+ addRequirements(intex);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ timer.reset();
+ timer.start();
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ intexer.setShooterIntake(.9);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ intexer.setShooterIntake(0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return timer.get() > time;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooter.java b/src/main/java/frc/robot/commands/IntakeThroughShooter.java
index 5dab49c..bcefdb8 100644
--- a/src/main/java/frc/robot/commands/IntakeThroughShooter.java
+++ b/src/main/java/frc/robot/commands/IntakeThroughShooter.java
@@ -7,6 +7,7 @@
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.robot.Constants;
import frc.robot.subsystems.IntexerSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
@@ -18,7 +19,8 @@ public class IntakeThroughShooter extends Command {
Joystick controller;
/** Creates a new IntakeFromShooter. */
- public IntakeThroughShooter(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) {
+ public IntakeThroughShooter(
+ ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) {
shooter = shooterSub;
intexer = intex;
this.controller = controller;
@@ -29,30 +31,29 @@ public IntakeThroughShooter(ShooterSubsystem shooterSub, IntexerSubsystem intex,
// Called when the command is initially scheduled.
@Override
public void initialize() {
+ shooter.setWristByAngle(.66);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
shooter.setShooterVelocity(-1000);
- intexer.setALL(-.5);
- shooter.setWristPosition(.66);
+ intexer.setALL(Constants.Intake.outakeSpeed);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM);
- shooter.setWristPosition(Constants.Shooter.intakeAngleRadians);
+ shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians);
intexer.setALL(0);
}
-
+
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (intexer.intakeBreak()) {
controller.setRumble(RumbleType.kBothRumble, 0.75);
- intexer.setIntakeThroughShooterPart2Status(true);
return true;
} else {
return false;
diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java
index 0d7f116..42d258a 100644
--- a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java
+++ b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java
@@ -6,7 +6,9 @@
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.robot.Constants;
import frc.robot.subsystems.IntexerSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
@@ -17,9 +19,11 @@ public class IntakeThroughShooterPart2 extends Command {
private boolean finishPlease = false;
Joystick controller;
+ public final Timer timer = new Timer();
/** Creates a new IntakeFromShooterPart2. */
- public IntakeThroughShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) {
+ public IntakeThroughShooterPart2(
+ ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) {
shooter = shooterSub;
intexer = intex;
this.controller = controller;
@@ -30,15 +34,17 @@ public IntakeThroughShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem i
// Called when the command is initially scheduled.
@Override
public void initialize() {
+ timer.reset();
+ timer.start();
controller.setRumble(RumbleType.kBothRumble, 0);
+ shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
shooter.setShooterVelocity(0);
- intexer.setALL(.35);
- shooter.setWristPosition(Constants.Shooter.intakeAngleRadians);
+ intexer.setALL(Constants.Intake.noteInsideSpeed);
}
// Called once the command ends or is interrupted.
@@ -52,7 +58,7 @@ public void end(boolean interrupted) {
// Returns true when the command should end.
@Override
public boolean isFinished() {
- if (!intexer.intakeThroughShooterPart2isReady || intexer.shooterBreak()) {
+ if (intexer.shooterBreak() || timer.get() > 1.5) {
return true;
} else {
return false;
diff --git a/src/main/java/frc/robot/commands/IntexBestHex.java b/src/main/java/frc/robot/commands/IntexBestHex.java
index 945a74c..8a6e674 100644
--- a/src/main/java/frc/robot/commands/IntexBestHex.java
+++ b/src/main/java/frc/robot/commands/IntexBestHex.java
@@ -4,10 +4,12 @@
package frc.robot.commands;
-import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.lib.math.FiringSolutionsV3;
+import frc.robot.Constants;
import frc.robot.subsystems.IntexerSubsystem;
public class IntexBestHex extends Command {
@@ -35,17 +37,17 @@ public void initialize() {
public void execute() {
if (in) { // Hood logic to run forwards or backwards
if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet
- intexer.setALL(.4);
- } else if (!intexer.intakeBreak() && intexer.shooterBreak()) { // Stop note if at shooter
+ intexer.setALL(Constants.Intake.noteInsideSpeed);
+ } else if (!intexer.intakeBreak()
+ && intexer.shooterBreak()) { // Stop note if at shooter
controller.setRumble(RumbleType.kBothRumble, 0.75);
intexer.setALL(0);
} else { // Note is not in robot
- intexer.setFrontIntake(.75);
+ intexer.setFrontIntake(Constants.Intake.noteOutsideSpeed);
}
} else { // Outtake
- intexer.setALL(-0.5);
+ intexer.setALL(Constants.Intake.outakeSpeed);
}
-
}
// Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java
index 43bf8c1..67889da 100644
--- a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java
+++ b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java
@@ -5,44 +5,52 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.Constants;
import frc.robot.subsystems.IntexerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
public class IntexForAutosByAutos extends Command {
private IntexerSubsystem intexer;
+ private ShooterSubsystem shooter;
/** Creates a new IntexForAutosByAutos. */
- public IntexForAutosByAutos(IntexerSubsystem intexerSub) {
+ public IntexForAutosByAutos(IntexerSubsystem intexerSub, ShooterSubsystem shooterSubsystem) {
this.intexer = intexerSub;
- addRequirements(intexerSub);
+ this.shooter = shooterSubsystem;
+ addRequirements(intexerSub, shooterSubsystem);
}
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
+ shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians);
+
if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet
- intexer.setALL(.4);
- } else if (!intexer.intakeBreak() && intexer.shooterBreak()) { // Stop note if at shooter
+ intexer.setALL(Constants.Intake.noteInsideSpeed);
+ } else if (intexer.shooterBreak()) { // Stop note if at shooter
intexer.setALL(0);
} else { // Note is not in robot
- intexer.setFrontIntake(.75);
+ intexer.setFrontIntake(Constants.Intake.noteOutsideSpeed);
+ intexer.setShooterIntake(Constants.Intake.noteInsideSpeed);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
+ intexer.setALL(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
- if (!intexer.intakeBreak() && intexer.shooterBreak()) {
+ if (intexer.shooterBreak()) {
return true;
} else {
return false;
diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java
index 2e6d3ea..1e44b33 100644
--- a/src/main/java/frc/robot/commands/ManRizzt.java
+++ b/src/main/java/frc/robot/commands/ManRizzt.java
@@ -4,42 +4,59 @@
package frc.robot.commands;
-import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
-
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.Constants;
import frc.robot.subsystems.ShooterSubsystem;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
public class ManRizzt extends Command {
- ShooterSubsystem shooterSubsystem;
+ ShooterSubsystem m_shooterSubsystem;
private DoubleSupplier speed;
BooleanSupplier setAngle;
+ double lastWristSetpoint = 0.0;
+ boolean wristIsLocked = false;
public ManRizzt(ShooterSubsystem subsystem, DoubleSupplier speed, BooleanSupplier setAngle) {
// Use addRequirements() here to declare subsystem dependencies.
- shooterSubsystem = subsystem;
+ m_shooterSubsystem = subsystem;
this.setAngle = setAngle;
this.speed = speed;
SmartDashboard.putNumber("Set Wrist Angle", 0);
- addRequirements(shooterSubsystem);
+ addRequirements(m_shooterSubsystem);
}
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
@Override
public void execute() {
- double speedValue = speed.getAsDouble() * 0.5;
-
- speedValue = Math.pow(speed.getAsDouble(), 3);
- if (setAngle.getAsBoolean()){
- shooterSubsystem.setWristPosition(SmartDashboard.getNumber("Set Wrist Angle", 0));
+ double speedValue = MathUtil.applyDeadband(speed.getAsDouble(), Constants.stickDeadband);
+ speedValue = Math.pow(speedValue, 3);
+
+ if (setAngle.getAsBoolean()) {
+ // m_shooterSubsystem.setWristByAngle(SmartDashboard.getNumber("Set Wrist Angle", 0));
} else {
- shooterSubsystem.manualWristSpeed(speedValue);
+ if (Math.abs(speedValue) > .0) {
+ wristIsLocked = false;
+ m_shooterSubsystem.setWristSpeedManual(speedValue);
+ } else {
+ if (m_shooterSubsystem.isZeroed) {
+ if (!wristIsLocked) {
+ m_shooterSubsystem.setWristByAngle(
+ m_shooterSubsystem.getCurrentShooterAngle());
+ wristIsLocked = true;
+ }
+ } else {
+ m_shooterSubsystem.setWristSpeedManual(0);
+ }
+ }
}
}
@@ -50,6 +67,6 @@ public boolean isFinished() {
@Override
public void end(boolean interrupted) {
- shooterSubsystem.manualWristSpeed(0);
+ // m_shooterSubsystem.setWristSpeedManual(0);
}
}
diff --git a/src/main/java/frc/robot/commands/MissileLock.java b/src/main/java/frc/robot/commands/MissileLock.java
index 9839f15..6063198 100644
--- a/src/main/java/frc/robot/commands/MissileLock.java
+++ b/src/main/java/frc/robot/commands/MissileLock.java
@@ -5,6 +5,7 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.lib.math.FiringSolutionsV3;
import frc.robot.Constants;
import frc.robot.subsystems.ShooterSubsystem;
@@ -24,17 +25,23 @@ public MissileLock(ShooterSubsystem shooterSub, String target) {
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (target == "amp") {
- shooter.PointShoot(shooter.getCalculatedAngleToAmp(),
- FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity()));
+ if (shooter.outsideAllianceWing) {
+ shooter.PointShoot(
+ Math.toRadians(55),
+ FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity()));
+ } else {
+ shooter.setShooterVelocity(
+ FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity()));
+ }
} else {
- shooter.PointShoot(shooter.getCalculatedAngleToSpeaker(),
+ shooter.PointShoot(
+ shooter.getCalculatedAngleToSpeaker(),
FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity()));
}
}
@@ -42,8 +49,10 @@ public void execute() {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
- shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM);
- shooter.setWristPosition(Constants.Shooter.intakeAngleRadians);
+ // shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM);
+ if (target != "amp" || shooter.outsideAllianceWing) {
+ shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians);
+ }
}
// Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java
new file mode 100644
index 0000000..ba2b011
--- /dev/null
+++ b/src/main/java/frc/robot/commands/NoteSniffer.java
@@ -0,0 +1,116 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.subsystems.IntexerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.SwerveSubsystem;
+import frc.robot.subsystems.VisionSubsystem;
+
+import org.photonvision.targeting.PhotonPipelineResult;
+
+public class NoteSniffer extends Command {
+
+ private SwerveSubsystem swerveSubsystem;
+ private VisionSubsystem vision;
+ private IntexerSubsystem intexer;
+ private ShooterSubsystem shooter;
+ private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04);
+ private Timer timer = new Timer();
+ private boolean noteInside = false;
+ private double translationVal = .35;
+
+ /** Creates a new IntakeWithVision. */
+ public NoteSniffer(
+ SwerveSubsystem swerve,
+ VisionSubsystem vision,
+ IntexerSubsystem intexer,
+ ShooterSubsystem shooter) {
+ this.swerveSubsystem = swerve;
+ this.vision = vision;
+ this.intexer = intexer;
+ this.shooter = shooter;
+ addRequirements(swerve, intexer);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ timer.reset();
+ timer.start();
+ translationVal = .35;
+ noteInside = false;
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians);
+
+ PhotonPipelineResult result = vision.getLatestResultN();
+ double rotationVal;
+ boolean overMidfield = Robot.getAlliance()
+ ? (16.54 - swerveSubsystem.getPose().getX()) > 8.3
+ : swerveSubsystem.getPose().getX() > 8.3;
+
+ if (intexer.intakeBreak() || overMidfield) {
+ noteInside = true;
+ translationVal = 0;
+ rotationVal = 0;
+ }
+
+ if (result.hasTargets() && !noteInside) { // Lock robot towards detected note
+ double yawToNote = Math.toRadians(result.getBestTarget().getYaw())
+ + swerveSubsystem.getGyroYaw().getRadians();
+
+ SmartDashboard.putNumber("Note Yaw", yawToNote);
+
+ rotationVal = rotationPID.calculate(
+ yawToNote, swerveSubsystem.getGyroYaw().getRadians());
+
+ intexer.setFrontIntake(Constants.Intake.noteOutsideSpeed);
+ intexer.setShooterIntake(Constants.Intake.noteInsideSpeed);
+ } else {
+ rotationVal = 0;
+ }
+
+ // if (shooter.getDistanceToSpeaker() < 2.5){
+ swerveSubsystem.drive(
+ new Translation2d(translationVal, 0).times(Constants.Swerve.maxSpeed),
+ rotationVal * Constants.Swerve.maxAngularVelocity,
+ false,
+ false);
+ /* } else {
+ intexer.setALL(-.5);
+ swerveSubsystem.drive(
+ new Translation2d(-0.2, 0).times(Constants.Swerve.maxSpeed),
+ 0 * Constants.Swerve.maxAngularVelocity,
+ false,
+ false);
+ }*/
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ intexer.setALL(0);
+ swerveSubsystem.setChassisSpeeds(new ChassisSpeeds(0, 0, 0));
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return intexer.shooterBreak() || timer.get() > 2;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/ResetNoteInShooter.java b/src/main/java/frc/robot/commands/ResetNoteInShooter.java
new file mode 100644
index 0000000..4fb5633
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ResetNoteInShooter.java
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.Constants;
+import frc.robot.subsystems.IntexerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class ResetNoteInShooter extends Command {
+ private ShooterSubsystem shooter;
+ private IntexerSubsystem intexer;
+
+ Joystick controller;
+
+ /** Creates a new IntakeFromShooter. */
+ public ResetNoteInShooter(
+ ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) {
+ shooter = shooterSub;
+ intexer = intex;
+ this.controller = controller;
+ // Use addRequirements() here to declare subsystem dependencies.
+ addRequirements(shooterSub, intex);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ shooter.setWristByAngle(.66);
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ intexer.setALL(Constants.Intake.outakeSpeed);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians);
+ intexer.setALL(0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ if (intexer.intakeBreak()) {
+ controller.setRumble(RumbleType.kBothRumble, 0.75);
+ return true;
+ } else {
+ return false;
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java b/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java
new file mode 100644
index 0000000..67a6cd5
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.Constants;
+import frc.robot.subsystems.IntexerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class ResetNoteInShooterPart2 extends Command {
+ private ShooterSubsystem shooter;
+ private IntexerSubsystem intexer;
+
+ Joystick controller;
+ public final Timer timer = new Timer();
+
+ /** Creates a new IntakeFromShooterPart2. */
+ public ResetNoteInShooterPart2(
+ ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) {
+ shooter = shooterSub;
+ intexer = intex;
+ this.controller = controller;
+ // Use addRequirements() here to declare subsystem dependencies.
+ addRequirements(shooterSub, intex);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ timer.reset();
+ timer.start();
+ controller.setRumble(RumbleType.kBothRumble, 0);
+ shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians);
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ intexer.setALL(Constants.Intake.noteInsideSpeed);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ intexer.setALL(0);
+ intexer.setIntakeThroughShooterPart2Status(false);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ if (intexer.shooterBreak() || timer.get() > 1.5) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/commands/RizzLevel.java b/src/main/java/frc/robot/commands/RizzLevel.java
index 02eead3..392b7d0 100644
--- a/src/main/java/frc/robot/commands/RizzLevel.java
+++ b/src/main/java/frc/robot/commands/RizzLevel.java
@@ -5,6 +5,7 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.robot.subsystems.ShooterSubsystem;
public class RizzLevel extends Command {
@@ -23,30 +24,22 @@ public RizzLevel(ShooterSubsystem noteAccelerator, double angleInRADIANS) {
// Called when the command is initially scheduled.
@Override
public void initialize() {
-
+ shooter.setWristByAngle(angle);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
- public void execute() {
- if (shooter.isZeroed){
- shooter.setWristPosition(angle);
- }
- }
+ public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
- //shooter.manualWristSpeed(0);
+ // shooter.manualWristSpeed(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
- if (shooter.isZeroed){
- return shooter.getAngle() >= (angle - Math.toRadians(.5)) && shooter.getAngle() < (angle + Math.toRadians(.5));
- } else {
- return true;
- }
+ return true;
}
}
diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java
index d78ab6b..38885e4 100644
--- a/src/main/java/frc/robot/commands/TeleopSwerve.java
+++ b/src/main/java/frc/robot/commands/TeleopSwerve.java
@@ -1,32 +1,33 @@
package frc.robot.commands;
-import frc.lib.math.FiringSolutions;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
import frc.lib.math.FiringSolutionsV3;
import frc.robot.Constants;
import frc.robot.Robot;
+import frc.robot.subsystems.IntexerSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.SwerveSubsystem;
import frc.robot.subsystems.VisionSubsystem;
-import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
-
import org.photonvision.targeting.PhotonPipelineResult;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
public class TeleopSwerve extends Command {
private SwerveSubsystem swerveSubsystem;
private VisionSubsystem vision;
private ShooterSubsystem shooterSubsystem;
+ private IntexerSubsystem intexerSubsystem;
private DoubleSupplier translationSup;
private DoubleSupplier strafeSup;
@@ -36,16 +37,33 @@ public class TeleopSwerve extends Command {
private BooleanSupplier shooterOverrideAmp;
private BooleanSupplier shooterOverrideSpeaker;
private BooleanSupplier intakeOverride;
+ private BooleanSupplier intakeOverrideNoMove;
private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04);
private Joystick controller;
- public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsystem shooter, DoubleSupplier translationSup, DoubleSupplier strafeSup,
- DoubleSupplier rotationSup, BooleanSupplier robotCentricSup, BooleanSupplier shooterOverrideAmp, BooleanSupplier shooterOverrideSpeaker, BooleanSupplier intake, Joystick controller) {
+ private boolean noteInside = false;
+
+ public TeleopSwerve(
+ SwerveSubsystem swerve,
+ VisionSubsystem vision,
+ ShooterSubsystem shooter,
+ IntexerSubsystem intexer,
+ DoubleSupplier translationSup,
+ DoubleSupplier strafeSup,
+ DoubleSupplier rotationSup,
+ BooleanSupplier robotCentricSup,
+ BooleanSupplier shooterOverrideAmp,
+ BooleanSupplier shooterOverrideSpeaker,
+ BooleanSupplier intake,
+ BooleanSupplier intakeNoMove,
+ Joystick controller) {
+
this.swerveSubsystem = swerve;
this.vision = vision;
this.shooterSubsystem = shooter;
+ this.intexerSubsystem = intexer;
addRequirements(swerve);
this.translationSup = translationSup;
@@ -55,70 +73,136 @@ public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsy
this.shooterOverrideAmp = shooterOverrideAmp;
this.shooterOverrideSpeaker = shooterOverrideSpeaker;
this.intakeOverride = intake;
+ this.intakeOverrideNoMove = intakeNoMove;
this.controller = controller;
- SmartDashboard.putData(rotationPID);
+ SmartDashboard.putData("Lock On Rotation PID", rotationPID);
}
@Override
public void execute() {
Pose2d pose = swerveSubsystem.getPose();
boolean robotCentric = robotCentricSup.getAsBoolean();
+ boolean openLoop = true;
PhotonPipelineResult result = vision.getLatestResultN();
/* Get Values, Deadband */
- double translationVal = MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband);
+ double translationVal =
+ MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband);
double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband);
double rotationVal;
+ double offset;
+ double ampLockOffset;
+
+ if (Robot.getAlliance()) {
+ ampLockOffset = 16.54 - 5;
+ if (pose.getRotation().getRadians() > 0) {
+ offset = -Math.toRadians(180);
+ } else {
+ offset = Math.toRadians(180);
+ }
+ } else {
+ ampLockOffset = 5;
+ offset = 0;
+ }
+
/* Exponential Drive */
translationVal = Math.copySign(Math.pow(translationVal, 2), translationVal);
strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal);
if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker
- if (shooterSubsystem.getDistanceToSpeaker() >= 3.5){
+ if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5) {
controller.setRumble(RumbleType.kBothRumble, 0.5);
} else {
controller.setRumble(RumbleType.kBothRumble, 0);
}
+
ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds();
- rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(),
- FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY,
+ rotationVal = rotationPID.calculate(
+ pose.getRotation().getRadians() + offset,
+ FiringSolutionsV3.getAngleToMovingTarget(
+ pose.getX(),
+ pose.getY(),
+ FiringSolutionsV3.speakerTargetX,
+ FiringSolutionsV3.speakerTargetY,
currentSpeed.vxMetersPerSecond,
currentSpeed.vyMetersPerSecond,
- FiringSolutions.getAngleToSpeaker(pose.getX(), pose.getY())));
-
+ pose.getRotation().getRadians()));
+ openLoop = false;
+
} else if (shooterOverrideAmp.getAsBoolean()) { // Lock robot angle to amp
ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds();
+ openLoop = false;
+
+ if ((Robot.getAlliance() && pose.getX() < 16.54 - 5)
+ ^ (!Robot.getAlliance() && pose.getX() > 5)) {
+
+ rotationVal = rotationPID.calculate(
+ pose.getRotation().getRadians() + offset,
+ FiringSolutionsV3.getAngleToMovingTarget(
+ pose.getX(),
+ pose.getY(),
+ FiringSolutionsV3.ampTargetX,
+ FiringSolutionsV3.ampTargetY,
+ currentSpeed.vxMetersPerSecond,
+ currentSpeed.vyMetersPerSecond,
+ pose.getRotation().getRadians()));
+ } else {
+ rotationVal =
+ rotationPID.calculate(pose.getRotation().getRadians(), Math.toRadians(-90));
+ }
- rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(),
- FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY,
- currentSpeed.vxMetersPerSecond,
- currentSpeed.vyMetersPerSecond,
- FiringSolutions.getAngleToSpeaker(pose.getX(), pose.getY())));
+ } else if (intakeOverride.getAsBoolean()
+ && result.hasTargets()
+ && !noteInside) { // Lock robot towards detected note
- } else if (intakeOverride.getAsBoolean() && result.hasTargets()) { // Lock robot towards detected note
- double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + swerveSubsystem.getGyroYaw().getRadians();
+ double yawToNote = Math.toRadians(result.getBestTarget().getYaw())
+ + swerveSubsystem.getGyroYaw().getRadians();
SmartDashboard.putNumber("Note Yaw", yawToNote);
- rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians());
+ rotationVal = rotationPID.calculate(
+ yawToNote, swerveSubsystem.getGyroYaw().getRadians());
+
+ translationVal = 0.35;
+ robotCentric = true;
+ openLoop = false;
+
+ } else if (intakeOverrideNoMove.getAsBoolean() && result.hasTargets() && !noteInside) {
+ double yawToNote = Math.toRadians(result.getBestTarget().getYaw())
+ + swerveSubsystem.getGyroYaw().getRadians();
+
+ SmartDashboard.putNumber("Note Yaw", yawToNote);
+
+ openLoop = false;
+ rotationVal = rotationPID.calculate(
+ yawToNote, swerveSubsystem.getGyroYaw().getRadians());
+
} else {
- rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband);
+ rotationVal =
+ MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband);
rotationVal = Math.copySign(Math.pow(rotationVal, 2), rotationVal);
controller.setRumble(RumbleType.kBothRumble, 0);
+ noteInside = false;
}
- if (Robot.getAlliance() && !robotCentric){ // Invert field oriented for always blue origin
+ if (Robot.getAlliance() && !robotCentric) { // Invert field oriented for always blue origin
translationVal = -translationVal;
strafeVal = -strafeVal;
}
+ if (intexerSubsystem.intakeBreak()) {
+ translationVal = 0;
+ rotationVal = 0;
+ noteInside = true;
+ }
+
/* Drive */
swerveSubsystem.drive(
new Translation2d(translationVal, strafeVal).times(Constants.Swerve.maxSpeed),
rotationVal * Constants.Swerve.maxAngularVelocity,
!robotCentric,
- true);
+ openLoop);
}
-}
\ No newline at end of file
+}
diff --git a/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java b/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java
index 2e1c3bd..ec1f75d 100644
--- a/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java
+++ b/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java
@@ -5,10 +5,13 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.robot.subsystems.IntexerSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
public class ToBreakOrNotToBreak extends Command {
IntexerSubsystem intexer;
+ LEDSubsystem ledSubsystem;
/** Creates a new ToBreakOrNotToBreak. */
public ToBreakOrNotToBreak(IntexerSubsystem intexer) {
@@ -19,13 +22,12 @@ public ToBreakOrNotToBreak(IntexerSubsystem intexer) {
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
- if (!intexer.shooterBreak()){
+ if (!intexer.shooterBreak()) {
intexer.setShooterIntake(.5);
} else {
intexer.setShooterIntake(0);
diff --git a/src/main/java/frc/robot/commands/ZeroWrist.java b/src/main/java/frc/robot/commands/ZeroRizz.java
similarity index 91%
rename from src/main/java/frc/robot/commands/ZeroWrist.java
rename to src/main/java/frc/robot/commands/ZeroRizz.java
index 5e70ce4..6dcd871 100644
--- a/src/main/java/frc/robot/commands/ZeroWrist.java
+++ b/src/main/java/frc/robot/commands/ZeroRizz.java
@@ -6,15 +6,16 @@
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
+
import frc.robot.Constants;
import frc.robot.subsystems.ShooterSubsystem;
-public class ZeroWrist extends Command {
+public class ZeroRizz extends Command {
ShooterSubsystem shooter;
public final Timer timer = new Timer();
/** Creates a new ZeroWrist. */
- public ZeroWrist(ShooterSubsystem shooter) {
+ public ZeroRizz(ShooterSubsystem shooter) {
this.shooter = shooter;
addRequirements(shooter);
// Use addRequirements() here to declare subsystem dependencies.
@@ -30,7 +31,7 @@ public void initialize() {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
- shooter.manualWristSpeed(.45);
+ shooter.setWristSpeedManual(.45);
}
// Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index 62563fa..4703e22 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -4,19 +4,18 @@
package frc.robot.subsystems;
-import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.Slot1Configs;
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.LaserCan.RangingMode;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
-import au.grapplerobotics.ConfigurationFailedException;
-import au.grapplerobotics.LaserCan;
-import au.grapplerobotics.LaserCan.RangingMode;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -25,60 +24,50 @@
public class ElevatorSubsystem extends SubsystemBase {
// Devices
- public TalonFX m_elevatorLeft = new TalonFX(20); // left leader
- public TalonFX m_elevatorRight = new TalonFX(21);
- public LaserCan lasercan = new LaserCan(22);
+ public TalonFX m_elevatorLeft; // left leader
+ public TalonFX m_elevatorRight;
+ public LaserCan lasercan;
// Falcon stuff
- private final PositionDutyCycle m_requestPosition = new PositionDutyCycle(0);
private final PositionDutyCycle lockPosition = new PositionDutyCycle(0);
private final PIDController elevatorPID = new PIDController(0, 0, 0);
- private final Slot1Configs encoderConfigSlot1 = new Slot1Configs();
// Constants IN METERS
private final double spoolCircumference = 0.0508;
private final double gearRatio = 17.33;
- private final double maxHeight = .8;
// Vars
private double revolutionCount;
private double currentHeight;
private double setHeight;
- private boolean laser;
- LaserCan.Measurement measurement;
+ private LaserCan.Measurement measurement;
public boolean manualOverride = false;
public boolean locked = false;
public ElevatorSubsystem() {
+ m_elevatorLeft = new TalonFX(20); // left leader
+ m_elevatorRight = new TalonFX(21);
+ lasercan = new LaserCan(22);
+
// Falcon setup
- m_elevatorLeft.setNeutralMode(NeutralModeValue.Brake);
+ TalonFXConfiguration elevatorConfigs = new TalonFXConfiguration();
+ elevatorConfigs.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ elevatorConfigs.Slot0.kP = 0.09;
+ elevatorConfigs.Slot1.kP = 1;
+ elevatorConfigs.Slot0.GravityType = GravityTypeValue.Elevator_Static;
+ elevatorConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ elevatorConfigs.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.25;
+ elevatorConfigs.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.25;
+ elevatorConfigs.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.25;
+
+ m_elevatorLeft.getConfigurator().apply(elevatorConfigs);
+ m_elevatorRight.getConfigurator().apply(elevatorConfigs);
m_elevatorRight.setControl(new Follower(m_elevatorLeft.getDeviceID(), true));
- // PID
- Slot0Configs encoderConfigSlot0 = new Slot0Configs();
- ClosedLoopRampsConfigs closedloop = new ClosedLoopRampsConfigs();
-
- closedloop.withDutyCycleClosedLoopRampPeriod(.5).withTorqueClosedLoopRampPeriod(0.5);
- encoderConfigSlot0.kP = .09;
- encoderConfigSlot0.kI = .0;
- encoderConfigSlot0.kD = .0;
- encoderConfigSlot0.kV = .0;
- encoderConfigSlot0.kG = .0;
- encoderConfigSlot0.GravityType = GravityTypeValue.Elevator_Static;
-
- encoderConfigSlot1.kP = 1;
- encoderConfigSlot1.kI = .0;
- encoderConfigSlot1.kD = .0;
- encoderConfigSlot1.kV = .01;
-
-
- m_elevatorLeft.getConfigurator().apply(encoderConfigSlot0, 0.050);
- m_elevatorLeft.getConfigurator().apply(closedloop);
-
// laser can pid shenanigans
- elevatorPID.setP(2.5);
- elevatorPID.setI(0);
+ elevatorPID.setP(3);
+ elevatorPID.setI(0.5);
elevatorPID.setD(0);
elevatorPID.setTolerance(0.02);
@@ -92,8 +81,6 @@ public ElevatorSubsystem() {
SmartDashboard.putData("Elevator PID", elevatorPID);
m_elevatorLeft.setPosition(0);
-
- laser = false;
}
@Override
@@ -105,60 +92,54 @@ public void periodic() {
SmartDashboard.putNumber("Set Height", setHeight);
SmartDashboard.putNumber("LaserCan Meters", getHeightLaserCan());
SmartDashboard.putBoolean("LaserCan failure", lasercanFailureCheck());
- SmartDashboard.putNumber("Elevator Left Supply Current", m_elevatorLeft.getSupplyCurrent().getValueAsDouble());
- SmartDashboard.putNumber("Elevator Right Supply Current",
- m_elevatorRight.getSupplyCurrent().getValueAsDouble());
- SmartDashboard.putNumber("LaserCan Ambient", measurement.ambient);
+ SmartDashboard.putNumber(
+ "Elevator Left Supply Current",
+ m_elevatorLeft.getSupplyCurrent().getValueAsDouble());
+ SmartDashboard.putNumber(
+ "Elevator Right Supply Current",
+ m_elevatorRight.getSupplyCurrent().getValueAsDouble());
+ SmartDashboard.putNumber("LaserCan Ambient", measurement != null ? measurement.ambient : 0);
revolutionCount = m_elevatorLeft.getPosition().getValueAsDouble();
- if (!manualOverride){ // Flagitious logic
- if (!locked){
- stopHere();
- locked = true;
- }
- } else {
- locked = false;
- }
-
- //FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this
+ // FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this
}
- public void setManualOverride(boolean value){
+ public void setElevatorSpeedManual(double value) {
+ m_elevatorLeft.set(value);
+ }
+
+ public void setManualOverride(boolean value) {
manualOverride = value;
}
- public double getEncoderValue(){
+ public double getEncoderValue() {
return revolutionCount;
}
- public void setToEncoder(double value){
+ public void setPositionWithEncoder(double value) {
+ locked = true;
m_elevatorLeft.setControl(lockPosition.withPosition(value));
}
- public void stopHere(){
+ public void stopHere() {
+ locked = true;
m_elevatorLeft.setControl(lockPosition.withPosition(revolutionCount).withSlot(0));
}
/** Get height from motor encoder */
public double getHeightEncoder() {
- return (revolutionCount / gearRatio) * (spoolCircumference * Math.PI) + 0.2;
+ return (revolutionCount / gearRatio) * (spoolCircumference * Math.PI);
}
/** Set height IN METERS. Will run off LaserCan but will switch to encoder if it fails */
public void setHeight(double height) {
+ locked = true;
setHeight = height;
- if (!lasercanFailureCheck()) { // Run off LaserCan
- m_elevatorLeft.set(elevatorPID.calculate(getHeightLaserCan(), height));
- } else { // Run off encoder
- double rot = (height / (spoolCircumference * Math.PI)) * gearRatio;
- if (getHeightEncoder() < maxHeight) {
- m_elevatorLeft.setControl(m_requestPosition.withPosition(rot).withSlot(1));
- }
- }
+ m_elevatorLeft.set(elevatorPID.calculate(getHeight(), height));
}
/** Get height IN METERS. Will run off LaserCan but will switch to encoder if it fails */
- public double getHeight(){
+ public double getHeight() {
if (!lasercanFailureCheck()) { // Run off LaserCan
return getHeightLaserCan();
} else { // Run off encoder
@@ -171,6 +152,7 @@ public boolean atHeight() {
}
public void ManSpin(double percent) {
+ locked = false;
m_elevatorLeft.set(percent);
}
@@ -178,11 +160,6 @@ public void resetElevatorEncoder() {
m_elevatorLeft.setPosition(0);
}
- //lasercan methods
- public void useLaserCan(boolean laserCanOn) {
- laser = laserCanOn;
- }
-
/** Get height from LaserCan IN METERS */
public double getHeightLaserCan() {
return currentHeight;
@@ -204,4 +181,11 @@ public boolean lasercanFailureCheck() {
}
}
+ public double getSetpoint() {
+ return m_elevatorLeft.getClosedLoopReference().getValue();
+ }
+
+ public double getPosition() {
+ return m_elevatorLeft.getPosition().getValueAsDouble();
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/IntexerSubsystem.java b/src/main/java/frc/robot/subsystems/IntexerSubsystem.java
index 258296a..f85cad4 100644
--- a/src/main/java/frc/robot/subsystems/IntexerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntexerSubsystem.java
@@ -4,28 +4,37 @@
package frc.robot.subsystems;
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.revrobotics.CANSparkBase;
-import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class IntexerSubsystem extends SubsystemBase {
// Devices
- private CANSparkBase left = new CANSparkMax(30, MotorType.kBrushless);
- private CANSparkBase right = new CANSparkMax(31, MotorType.kBrushless);
- private CANSparkBase shooterIntake = new CANSparkMax(10, MotorType.kBrushless);
+ private CANSparkBase left;
+ private CANSparkBase right;
+ private CANSparkBase shooterIntake;
public boolean intakeThroughShooterPart2isReady = false;
+ public boolean resetNoteInShooterPart2isReady = false;
/** Front Intake */
- private DigitalInput breakingBeam = new DigitalInput(2);
+ private DigitalInput breakingBeam;
/** Shooter Intake */
- private DigitalInput beamKamen = new DigitalInput(1);
+ private DigitalInput beamKamen;
public IntexerSubsystem() {
+ left = new CANSparkMax(30, MotorType.kBrushless);
+ right = new CANSparkMax(31, MotorType.kBrushless);
+ shooterIntake = new CANSparkMax(10, MotorType.kBrushless);
+
+ beamKamen = new DigitalInput(1);
+ breakingBeam = new DigitalInput(2);
+
left.setIdleMode(IdleMode.kCoast);
right.setIdleMode(IdleMode.kCoast);
shooterIntake.setIdleMode(IdleMode.kCoast);
@@ -49,7 +58,7 @@ public void setALL(double speed) {
shooterIntake.set(speed);
}
}
-
+
public void setFrontIntake(double speed) {
if (speed == 0) {
left.stopMotor();
@@ -61,7 +70,7 @@ public void setFrontIntake(double speed) {
}
public void setShooterIntake(double speed) {
- if (speed == 0){
+ if (speed == 0) {
shooterIntake.stopMotor();
} else {
shooterIntake.set(speed);
@@ -76,10 +85,14 @@ public boolean shooterBreak() {
return !beamKamen.get();
}
- public void setIntakeThroughShooterPart2Status(boolean value){
+ public void setIntakeThroughShooterPart2Status(boolean value) {
intakeThroughShooterPart2isReady = value;
}
+ public void resetNoteInShooterPart2Status(boolean value) {
+ resetNoteInShooterPart2isReady = value;
+ }
+
@Override
public void periodic() {
SmartDashboard.putBoolean("Intake Beam Break", intakeBreak());
diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java
index 45beeb0..5331fac 100644
--- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java
@@ -5,51 +5,260 @@
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc.robot.Constants;
import frc.robot.Robot;
public class LEDSubsystem extends SubsystemBase {
- public DigitalOutput AllianceColor = new DigitalOutput(2); // Alliance color
- public DigitalOutput FoundNote = new DigitalOutput(3); // Sends if the camera 'OnionRing' sees a note
- public DigitalOutput Aimed = new DigitalOutput(4); // Sends if the outtake is aimed
- public DigitalOutput NoteInIntake = new DigitalOutput(5); // Sends if the Note is in the intake/outtake
- public DigitalOutput NoteOuttaked = new DigitalOutput(6); // Sends if the Note has been outtaked
+ public DigitalOutput WalterLight = new DigitalOutput(3); // Bit 1 (1)
+ public DigitalOutput JesseBlinkman = new DigitalOutput(4); // Bit 2 (2)
+ public DigitalOutput GusBling = new DigitalOutput(5); // Bit 3 (4)
+ public DigitalOutput SkylerBright = new DigitalOutput(6); // Bit 4 (8)
+
+ // Output bits to the LEDs
+ public DigitalOutput[] bits = {WalterLight, JesseBlinkman, GusBling, SkylerBright
+ }; // Actual Outputs
+ private boolean[] output = new boolean[4]; // Computed Outputs
+
+ // Booleans used for Easy input
+ private Boolean hasNote = false;
+ private Boolean chargingOuttake = false;
+ private Boolean atSpeed = false;
+
+ public Boolean[] inputBooleans = {
+ false, // Amp -0 Rainbow Pattern #1
+ false, // Source -1 Rainbow Pattern #2
+ false, // Climb -2 Rainbow Pattern #3
+ false, // Blank/DC -3 None
+ false, // Note Detected -4 White Solid
+ false, // Charging -5 Green Pulse (HasNote)
+ false, // At Speed -6 Green BLink (HasNote)
+ false, // Charging -7 Magenta Pulse (NoNote)
+ false, // At Speed -8 Magenta BLink (NoNote)
+ false, // Note in Intake -9 Orange Blink
+ false, // Note in Shooter -10 Orange Solid
+ false, // Alliance Color -11 Red Pulse
+ false // Alliance Color -12 Blue Pulse
+ };
+
+ // Use subsystems
+ VisionSubsystem vision;
+ ShooterSubsystem shooter;
+ IntexerSubsystem intexer;
+ SwerveSubsystem swerve;
+
+ /** Creates a new LEDSubsystem. */
+ public LEDSubsystem(
+ VisionSubsystem m_VisionSubsystem,
+ ShooterSubsystem m_ShooterSubsystem,
+ IntexerSubsystem m_IntexerSubsystem,
+ SwerveSubsystem m_SwerveSubsystem) {
+ this.vision = m_VisionSubsystem;
+ this.shooter = m_ShooterSubsystem;
+ this.intexer = m_IntexerSubsystem;
+ this.swerve = m_SwerveSubsystem;
+ }
+
+ @Override
+ public void periodic() {
+ set(); // Decimal Phase
+ encoder(); // Transition Phase
+ send(); // Send Phase
+ }
+
+ private void set() { // Decimal phase
+ var results = vision.getLatestResultN();
+
+ // Note Detected
+ if (results.hasTargets()) {
+ NoteDetected(true);
+ } else {
+ NoteDetected(false);
+ }
+
+ // Driver Station Connected
+ if (DriverStation.isDSAttached()) {
+ inputBooleans[1] = false;
+ } else {
+ inputBooleans[1] = true;
+ }
+
+ if (Robot.checkRedAlliance()) {
+ inputBooleans[11] = true;
+ inputBooleans[12] = false;
+ } else {
+ inputBooleans[11] = false;
+ inputBooleans[12] = true;
+ }
+
+ // HasNote for ChargingOuttake and AtSpeed
+ if (hasNote) { // Converts from simple inputs to boolean
+ if (chargingOuttake) {
+ inputBooleans[5] = true;
+ inputBooleans[6] = false;
+ } else if (atSpeed) {
+ inputBooleans[5] = false;
+ inputBooleans[6] = true;
+ }
+ inputBooleans[7] = false;
+ inputBooleans[8] = false;
+ } else {
+ if (chargingOuttake) {
+ inputBooleans[7] = true;
+ inputBooleans[8] = false;
+ } else if (atSpeed) {
+ inputBooleans[7] = false;
+ inputBooleans[8] = true;
+ }
+ inputBooleans[5] = false;
+ inputBooleans[6] = false;
+ }
+
+ // Check if pathfinding
+ if (SwerveSubsystem.followingPath) {
+ inputBooleans[0] = true;
+ } else {
+ inputBooleans[0] = false;
+ }
+
+ // Check beam breaks
+ if (intexer.intakeBreak()) {
+ inputBooleans[9] = true; // Intake
+ inputBooleans[10] = false; // Shooter
+ hasNote = true;
+ } else if (intexer.shooterBreak()) {
+ inputBooleans[9] = false; // Intake
+ inputBooleans[10] = true; // Shooter
+ hasNote = true;
+ } else {
+ inputBooleans[9] = false; // Intake
+ inputBooleans[10] = false; // Shooter
+ hasNote = false;
+ }
+
+ // Charging or At Speed with Note or without Note
+ if (hasNote) { // Has Note
+ if (shooter.isShooterAtSpeed()) { // At speed
+ inputBooleans[5] = false;
+ inputBooleans[6] = true;
+ } else if (shooter.getVelocity() > Constants.Shooter.idleSpeedRPM + 500) { // Charging
+ inputBooleans[5] = true;
+ inputBooleans[6] = false;
+ }
+ inputBooleans[7] = false;
+ inputBooleans[8] = false;
+ } else { // No Note
+ if (shooter.isShooterAtSpeed()) { // At speed
+ inputBooleans[7] = false;
+ inputBooleans[8] = true;
+ } else if (shooter.getVelocity() > Constants.Shooter.idleSpeedRPM + 500) { // Charging
+ inputBooleans[7] = true;
+ inputBooleans[8] = false;
+ }
+ inputBooleans[5] = false;
+ inputBooleans[6] = false;
+ }
+
+ SmartDashboard.putBooleanArray("Input Booleans", inputBooleans);
+ }
+
+ private void encoder() { // Transition phase
+ // Decimal to Binary
+ int pin_amount = 4; // Amount of pins
+
+ int trueIndex = 0; // Index of selected LED sequence
+
+ for (int i = 0;
+ i < inputBooleans.length;
+ i++) { // Picks the first true sequence based on priority
+ if (inputBooleans[i]) {
+ trueIndex = i;
+ break;
+ }
+ }
- VisionSubsystem vision;
+ String binaryString = Integer.toBinaryString(trueIndex); // Cast Number to Binary
+ int length = binaryString.length(); // Find length of Binary (How many bits)
+ String finalString; // Final binary string
- /** Creates a new LEDSubsystem. */
- public LEDSubsystem(VisionSubsystem m_VisionSubsystem) {
- this.vision = m_VisionSubsystem;
- }
+ // Adds the amount of 0's needed for acurate transcription. Ex. 0x01 -> 0x0001
+ if (length < pin_amount) {
+ int zerosToAdd = pin_amount - length;
+ StringBuilder paddedStringBuilder = new StringBuilder();
- @Override
- public void periodic() {
- var results = vision.getLatestResultN();
+ for (int i = 0; i < zerosToAdd; i++) {
+ paddedStringBuilder.append("0");
+ }
- if (results.hasTargets()) {
- FoundNote(true);
- } else {
- FoundNote(false);
+ paddedStringBuilder.append(binaryString);
+ String paddedBinaryString = paddedStringBuilder.toString();
+
+ finalString = paddedBinaryString;
+ } else {
+ finalString = binaryString;
+ }
+
+ SmartDashboard.putString("Binary Output", finalString);
+
+ for (int i = pin_amount - 1; i >= 0; i--) {
+ output[i] = finalString.charAt(i) == '1';
+ }
+ }
+
+ private void send() { // Send Phase - "Redundancy is bliss"
+ for (int i = 0; i < output.length; i++) {
+ bits[i].set(output[i]);
+ }
+ SmartDashboard.putBooleanArray("DigitalOutputs", output);
+ }
+
+ // SET FUNCTIONS
+ public void ampTele(boolean amp) {
+ inputBooleans[0] = amp;
+ }
+
+ public void sourceTele(boolean source) {
+ inputBooleans[1] = source;
+ }
+
+ public void climbTele(boolean climb) {
+ inputBooleans[2] = climb;
}
- }
- public void setAllianceColor() {
- AllianceColor.set(Robot.getAlliance()); // True is Red, False is Blue
- }
+ public void HasNote(boolean hasNote) {
+ this.hasNote = hasNote;
+ }
+
+ public void ChargingOuttake(boolean chargingOuttake) {
+ this.chargingOuttake = chargingOuttake;
+ }
- public void FoundNote(boolean foundNote) {
- FoundNote.set(foundNote);
- }
+ public void ReadyToFire(boolean fire) {
+ this.atSpeed = fire;
+ }
- public void Aimed(boolean aimed) {
- Aimed.set(aimed);
- }
+ public void NoteInIntake(boolean noteInIntake) {
+ inputBooleans[9] = noteInIntake;
+ }
- public void NoteInIntake(boolean noteInIntake) {
- NoteInIntake.set(noteInIntake);
- }
+ public void NoteInShooter(boolean noteInShooter) {
+ inputBooleans[10] = noteInShooter;
+ }
- public void NoteOuttaked(boolean noteOuttaked) {
- NoteOuttaked.set(noteOuttaked);
- }
+ public void NoteDetected(boolean note) {
+ inputBooleans[4] = note;
+ }
+
+ /* DISCLAIMER: DISCONTINUED */
+ public void setAllianceColor() {
+ inputBooleans[11] = Robot.getAlliance(); // True is Red
+ inputBooleans[12] = !Robot.getAlliance(); // False is Blue
+ }
+
+ public void Disconnected(boolean disconnected) { // NOT CONFIRMED TO BE IN FINAL
+ inputBooleans[3] = disconnected;
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 5a38ac1..8aa7656 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -4,30 +4,33 @@
package frc.robot.subsystems;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.lib.math.FiringSolutionsV3;
-import frc.lib.math.Interpolations;
import com.revrobotics.CANSparkBase;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkPIDController;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.*;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.*;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc.lib.math.FiringSolutionsV3;
+import frc.lib.math.Interpolations;
+import frc.robot.Constants;
+import frc.robot.Robot;
public class ShooterSubsystem extends SubsystemBase {
// Devices
- private CANSparkBase m_Wrist = new CANSparkMax(13, MotorType.kBrushless);
- private CANSparkBase shootaTop = new CANSparkMax(11, MotorType.kBrushless); // leader
- private CANSparkBase shootaBot = new CANSparkMax(12, MotorType.kBrushless);
+ private CANSparkBase m_Wrist;
+ private CANSparkBase shootaTop; // leader
+ private CANSparkBase shootaBot;
private RelativeEncoder m_VelocityEncoder;
private RelativeEncoder m_VelocityEncoder2;
@@ -40,29 +43,50 @@ public class ShooterSubsystem extends SubsystemBase {
private SparkPIDController topPID;
// PID Constants
- private double velocityP = 0.00018;
+ private double velocityP = 0.0001;
private double velocityI = 5.2e-7;
- private double velocityD = 1.5;
+ private double velocityD = 0;
- private double positionP = 2;
+ private double positionP = 1;
private double positionI = 0;
private double positionD = 0;
// Vars
+ /** In m/s */
private double shooterVelocity = 12.1;
+
private double shooterAngleToSpeaker, shooterAngleToAmp;
- private Boolean ENCFAIL = false;
+ private boolean ENCFAIL = false;
public boolean isZeroed = false;
- private double angleOffset = 68.2; // IN RADIANS
+ public boolean wristIsLocked = false;
+ /** IN RADIANS */
+ private double angleOffset = Constants.Shooter.angleOffsetManual;
+
+ private double distanceToMovingSpeakerTarget = .94;
+ public double lastWristAngleSetpoint = 0.0;
+ public boolean manualOverride = false;
+ public double wristAngleUpperBound;
+ public double wristAngleLowerBound;
+
+ public boolean outsideAllianceWing = false;
+
+ // Constants
+ private final double wristAngleMax = 0.0;
+ private final double wristAngleMin = 0.0;
private final Timer speedTimer = new Timer();
- private final int m_WristCurrentMax = 84;
private final Interpolations interpolation = new Interpolations();
- private double distanceToMovingSpeakerTarget = .94;
+ private final int m_WristCurrentMax = 84;
private SwerveSubsystem swerveSubsystem;
+ private ElevatorSubsystem elevatorSubsystem;
- public ShooterSubsystem(SwerveSubsystem swerve) {
+ public ShooterSubsystem(SwerveSubsystem swerve, ElevatorSubsystem elevator) {
swerveSubsystem = swerve;
+ elevatorSubsystem = elevator;
+
+ m_Wrist = new CANSparkMax(13, MotorType.kBrushless);
+ shootaTop = new CANSparkMax(11, MotorType.kBrushless); // leader
+ shootaBot = new CANSparkMax(12, MotorType.kBrushless);
// Encoders
m_VelocityEncoder = shootaTop.getEncoder();
@@ -76,7 +100,7 @@ public ShooterSubsystem(SwerveSubsystem swerve) {
m_Wrist.restoreFactoryDefaults();
m_Wrist.setIdleMode(IdleMode.kBrake);
- m_Wrist.setInverted(true);
+ m_Wrist.setInverted(false);
shootaBot.setInverted(false);
@@ -95,14 +119,14 @@ public ShooterSubsystem(SwerveSubsystem swerve) {
m_Wrist.burnFlash();
shootaBot.burnFlash();
shootaTop.burnFlash();
-
+
m_pidWrist = new PIDController(positionP, positionI, positionD);
m_VelocityEncoder.setMeasurementPeriod(20);
SmartDashboard.putNumber("set velocity", shooterVelocity);
- SmartDashboard.putData(m_pidWrist);
+ SmartDashboard.putData("Wrist PID", m_pidWrist);
SmartDashboard.putData(this);
SmartDashboard.putNumber("Set Slip Offset", FiringSolutionsV3.slipPercent);
@@ -118,29 +142,40 @@ public ShooterSubsystem(SwerveSubsystem swerve) {
@Override
public void periodic() {
// This method will be called once per scheduler run
-/*
- pidSpdP = SmartDashboard.getNumber("Velo P", pidSpdP);
- pidSpdI = SmartDashboard.getNumber("Velo I", pidSpdI);
- pidSpdD = SmartDashboard.getNumber("Velo D", pidSpdD);
-Minor whoopsie if these guys were causing loop overruns
- botPID.setP(pidSpdP, 0);
- botPID.setI(pidSpdI, 0);
- botPID.setD(pidSpdD, 0);
-
- topPID.setP(pidSpdP, 0);
- topPID.setI(pidSpdI, 0);
- topPID.setD(pidSpdD, 0);
-*/
+ /*
+ if (velocityP != SmartDashboard.getNumber("Velo P", velocityP)){
+ velocityP = SmartDashboard.getNumber("Velo P", velocityP);
+ topPID.setP(velocityP, 0);
+ botPID.setP(velocityP, 0);
+ }
+
+ if (velocityI != SmartDashboard.getNumber("Velo I", velocityI)){
+ velocityI = SmartDashboard.getNumber("Velo I", velocityI);
+ topPID.setI(velocityI, 0);
+ botPID.setI(velocityI, 0);
+ }
+
+ if (velocityD != SmartDashboard.getNumber("Velo D", velocityD)){
+ velocityD = SmartDashboard.getNumber("Velo D", velocityD);
+ topPID.setD(velocityD, 0);
+ botPID.setD(velocityD, 0);
+ }
+ */
shooterVelocity = SmartDashboard.getNumber("set velocity", shooterVelocity);
- FiringSolutionsV3.slipPercent = SmartDashboard.getNumber("Set Slip Offset", FiringSolutionsV3.slipPercent);
- FiringSolutionsV3.speakerTargetZ = SmartDashboard.getNumber("Set Target Z", FiringSolutionsV3.speakerTargetZ);
+ FiringSolutionsV3.slipPercent =
+ SmartDashboard.getNumber("Set Slip Offset", FiringSolutionsV3.slipPercent);
+ FiringSolutionsV3.speakerTargetZ =
+ SmartDashboard.getNumber("Set Target Z", FiringSolutionsV3.speakerTargetZ);
- SmartDashboard.putNumber("Top - Bottom error", m_VelocityEncoder.getVelocity() - m_VelocityEncoder2.getVelocity());
- SmartDashboard.putNumber("Current Angle Radians", getAngle());
+ SmartDashboard.putNumber(
+ "Top - Bottom error",
+ m_VelocityEncoder.getVelocity() - m_VelocityEncoder2.getVelocity());
+ SmartDashboard.putNumber("Current Angle Radians", getCurrentShooterAngle());
SmartDashboard.putNumber("Current Velocity", getVelocity());
- SmartDashboard.putBoolean("shooter at speed", shooterAtSpeed());
- SmartDashboard.putNumber("Current Angle Degrees", Units.radiansToDegrees(getAngle()));
+ SmartDashboard.putBoolean("shooter at speed", isShooterAtSpeed());
+ SmartDashboard.putNumber(
+ "Current Angle Degrees", Units.radiansToDegrees(getCurrentShooterAngle()));
// check for encoder failure
if (m_WristEncoder.isConnected()) {
@@ -156,27 +191,68 @@ public void periodic() {
SmartDashboard.putNumber("Flywheel Right Current", shootaBot.getOutputCurrent());
SmartDashboard.putNumber("Wrist Current", m_Wrist.getOutputCurrent());
SmartDashboard.putBoolean("is Wrist Stalled", isWristMotorStalled());
+
+ if (isZeroed) {
+ if (!manualOverride) {
+ m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), lastWristAngleSetpoint));
+ }
+
+ // Implement whenever build stops throwing
+ /*
+ * if (getCurrentShooterAngle() > wristAngleUpperBound){
+ * m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(),
+ * wristAngleUpperBound));
+ * } else if (getCurrentShooterAngle() < wristAngleLowerBound){
+ * m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(),
+ * wristAngleLowerBound));
+ * }
+ */
+ }
}
- /** Check if wrist motor is exceeding stall current, used for zeroing */
- public boolean isWristMotorStalled() {
- if (m_Wrist.getOutputCurrent() > m_WristCurrentMax) {
- return true;
+ /** in RADIANs units MATTER */
+ public double getCurrentShooterAngle() {
+ if (!ENCFAIL) {
+ return ((m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset;
} else {
- return false;
+ return ((m_PositionEncoder.getPosition() * 2 * Math.PI) / 100);
}
}
- /** Reset wrist encoder to given value */
- public void setWristEncoderPosition(double newPosition) {
- m_WristEncoder.setPositionOffset(newPosition);
+ public double getCalculatedAngleToAmp() {
+ return shooterAngleToAmp;
}
- /** Get whether shooter is at target speed */
- public boolean shooterAtSpeed() { // Copied from Hudson but made it better
+ public double getCalculatedAngleToSpeaker() {
+ return shooterAngleToSpeaker;
+ }
+
+ public double getCalculatedVelocity() {
+ return shooterVelocity;
+ }
+
+ public double getDistanceTo(double x, double y) {
+ return FiringSolutionsV3.getDistanceToTarget(
+ swerveSubsystem.getPose().getX(), swerveSubsystem.getPose().getY(), x, y);
+ }
+
+ public double getDistanceToSpeakerWhileMoving() {
+ return distanceToMovingSpeakerTarget;
+ }
+
+ /** Shooter velocity in RPM */
+ public double getVelocity() {
+ return m_VelocityEncoder.getVelocity();
+ }
+
+ public double getWristRotations() {
+ return m_PositionEncoder.getPosition();
+ }
+ /** Get whether shooter is at target speed */
+ public boolean isShooterAtSpeed() { // Copied from Hudson but made it better
// if error less than certain amount start the timer
- if (Math.abs(getVelocity() - FiringSolutionsV3.convertToRPM(shooterVelocity)) < 50) {
+ if (Math.abs(getVelocity() - FiringSolutionsV3.convertToRPM(shooterVelocity)) < 80) {
speedTimer.start();
} else {
speedTimer.reset();
@@ -190,20 +266,21 @@ public boolean shooterAtSpeed() { // Copied from Hudson but made it better
}
}
- /** Shooter velocity in RPM */
- public double getVelocity() {
- return m_VelocityEncoder.getVelocity();
- }
-
- /** in RADIANs units MATTER */
- public double getAngle() {
- if (!ENCFAIL) {
- return ((-m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset;
+ /** Check if wrist motor is exceeding stall current, used for zeroing */
+ public boolean isWristMotorStalled() {
+ if (m_Wrist.getOutputCurrent() > m_WristCurrentMax) {
+ return true;
} else {
- return ((m_PositionEncoder.getPosition() * 2 * Math.PI) / 100);
+ return false;
}
}
+ public void PointShoot(double PointAngle, double launchVelocity) {
+ setWristByAngle(PointAngle);
+ botPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity);
+ topPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity);
+ }
+
public void resetWristEncoders(double newOffset) {
angleOffset = newOffset;
m_WristEncoder.reset();
@@ -211,88 +288,171 @@ public void resetWristEncoders(double newOffset) {
isZeroed = true;
}
- /** IN RADIANS */
- public void setWristPosition(double angle) {
- if (isZeroed){
- m_Wrist.set(m_pidWrist.calculate(getAngle(), angle));
- }
+ /** Wrist Encoder Reset */
+ public void restartWristEncoders() {
+ m_WristEncoder.reset();
+ m_PositionEncoder.setPosition(0);
+ }
+
+ public void setManualOverride(boolean value) {
+ manualOverride = value;
}
/** In rotations per minute */
- public void setShooterVelocity(double velocity) {
+ public void setOffsetVelocity(double velocity) {
if (velocity == 0) {
shootaBot.stopMotor();
shootaTop.stopMotor();
} else {
- botPID.setReference(velocity, CANSparkMax.ControlType.kVelocity);
- topPID.setReference(velocity, CANSparkMax.ControlType.kVelocity);
+ botPID.setReference(
+ velocity - SmartDashboard.getNumber("Top Bottom Offset", 400),
+ CANSparkMax.ControlType.kVelocity);
+ topPID.setReference(
+ velocity + SmartDashboard.getNumber("Top Bottom Offset", 400),
+ CANSparkMax.ControlType.kVelocity);
}
}
/** In rotations per minute */
- public void SetOffsetVelocity(double velocity) {
+ public void setShooterVelocity(double velocity) {
if (velocity == 0) {
shootaBot.stopMotor();
shootaTop.stopMotor();
} else {
- botPID.setReference(velocity - SmartDashboard.getNumber("Top Bottom Offset", 400), CANSparkMax.ControlType.kVelocity);
- topPID.setReference(velocity + SmartDashboard.getNumber("Top Bottom Offset", 400), CANSparkMax.ControlType.kVelocity);
+ botPID.setReference(velocity, CANSparkMax.ControlType.kVelocity);
+ topPID.setReference(velocity, CANSparkMax.ControlType.kVelocity);
}
}
- public void PointShoot(double PointAngle, double launchVelocity) {
- setWristPosition(PointAngle);
- botPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity);
- topPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity);
+ /** Reset wrist encoder to given value */
+ public void setWristEncoderOffset(double newPosition) {
+ m_WristEncoder.setPositionOffset(newPosition);
}
- public void manualWristSpeed(double speed) {
- m_Wrist.set(speed);
+ public void setWristAngleLowerBound(double wristAngleLowerBound) {
+ if (wristAngleLowerBound < wristAngleMin) {
+ wristAngleLowerBound = wristAngleMin;
+ } else if (wristAngleLowerBound > wristAngleUpperBound) {
+ wristAngleLowerBound = wristAngleUpperBound;
+ } else {
+ this.wristAngleLowerBound = wristAngleLowerBound;
+ }
}
- public double getCalculatedVelocity() {
- return shooterVelocity;
+ public void setWristAngleUpperBound(double wristAngleUpperBound) {
+ if (wristAngleUpperBound > wristAngleMax) {
+ wristAngleUpperBound = wristAngleMax;
+ } else if (wristAngleUpperBound < wristAngleLowerBound) {
+ wristAngleUpperBound = wristAngleLowerBound;
+ } else {
+ this.wristAngleUpperBound = wristAngleUpperBound;
+ }
}
- public double getCalculatedAngleToAmp() {
- return shooterAngleToAmp;
+ /** IN RADIANS */
+ public void setWristByAngle(double angle) {
+ manualOverride = false;
+ if (isZeroed) {
+ // manualOverride = false;
+ updateWristAngleSetpoint(angle);
+ }
}
- public double getCalculatedAngleToSpeaker() {
- return shooterAngleToSpeaker;
+ /** IN ROTATIONS */
+ public void setWristByRotations(double newPosition) {
+ if (isZeroed) {
+ m_Wrist.set(m_pidWrist.calculate(getWristRotations(), newPosition));
+ }
}
- public double getDistanceToSpeaker () {
- return distanceToMovingSpeakerTarget;
+ public void setWristSpeedManual(double speed) {
+ manualOverride = true;
+ m_Wrist.set(speed);
+ }
+
+ public void setWristToBrake() {
+ m_Wrist.setIdleMode(IdleMode.kBrake);
+ }
+
+ public void setWristToCoast() {
+ m_Wrist.setIdleMode(IdleMode.kCoast);
+ }
+
+ public void updateWristAngleSetpoint(double angle) {
+ if (angle != lastWristAngleSetpoint) {
+ lastWristAngleSetpoint = angle;
+ }
}
public void updateShooterMath() { // Shooter Math
+
Pose2d pose = swerveSubsystem.getPose();
ChassisSpeeds chassisSpeeds = swerveSubsystem.getChassisSpeeds();
- distanceToMovingSpeakerTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY,
- chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, pose.getRotation().getRadians());
+ distanceToMovingSpeakerTarget = FiringSolutionsV3.getDistanceToMovingTarget(
+ pose.getX(),
+ pose.getY(),
+ FiringSolutionsV3.speakerTargetX,
+ FiringSolutionsV3.speakerTargetY,
+ chassisSpeeds.vxMetersPerSecond,
+ chassisSpeeds.vyMetersPerSecond,
+ pose.getRotation().getRadians());
FiringSolutionsV3.updateSpeakerR(distanceToMovingSpeakerTarget);
- //shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR();
- shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget));
+ // shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR();
+
+ if (elevatorSubsystem.getHeight() > 0.3) {
+ shooterAngleToSpeaker =
+ Math.toRadians(interpolation.getShooterAngleFromInterpolationElevatorUp(
+ distanceToMovingSpeakerTarget));
+ } else {
+ shooterAngleToSpeaker = Math.toRadians(
+ interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget));
+ }
+
+ if ((Robot.getAlliance() && pose.getX() < 16.54 - 5)
+ ^ (!Robot.getAlliance() && pose.getX() > 5)) {
+ outsideAllianceWing = true;
+ } else {
+ outsideAllianceWing = false;
+ }
- SmartDashboard.putNumber("Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity));
+ SmartDashboard.putNumber(
+ "Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity));
SmartDashboard.putNumber("Calculated Angle Radians", shooterAngleToSpeaker);
SmartDashboard.putNumber("Calculated Angle Degrees", Math.toDegrees(shooterAngleToSpeaker));
- SmartDashboard.putNumber("distance", FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY));
+ SmartDashboard.putNumber(
+ "distance",
+ FiringSolutionsV3.getDistanceToTarget(
+ pose.getX(),
+ pose.getY(),
+ FiringSolutionsV3.speakerTargetX,
+ FiringSolutionsV3.speakerTargetY));
SmartDashboard.putNumber("R", FiringSolutionsV3.getSpeakerR());
SmartDashboard.putNumber("C", FiringSolutionsV3.C(distanceToMovingSpeakerTarget));
- SmartDashboard.putNumber("quarticA", FiringSolutionsV3.quarticA(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ));
- SmartDashboard.putNumber("quarticC", FiringSolutionsV3.quarticC(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ));
- SmartDashboard.putNumber("quarticE", FiringSolutionsV3.quarticE(distanceToMovingSpeakerTarget));
+ SmartDashboard.putNumber(
+ "quarticA",
+ FiringSolutionsV3.quarticA(
+ distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ));
+ SmartDashboard.putNumber(
+ "quarticC",
+ FiringSolutionsV3.quarticC(
+ distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ));
+ SmartDashboard.putNumber(
+ "quarticE", FiringSolutionsV3.quarticE(distanceToMovingSpeakerTarget));
SmartDashboard.putNumber("Distance to Moving Target", distanceToMovingSpeakerTarget);
SmartDashboard.putNumber("robot vx", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("robot vy", chassisSpeeds.vyMetersPerSecond);
- double distanceToMovingAmpTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY,
- chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, pose.getRotation().getRadians());
+ double distanceToMovingAmpTarget = FiringSolutionsV3.getDistanceToMovingTarget(
+ pose.getX(),
+ pose.getY(),
+ FiringSolutionsV3.ampTargetX,
+ FiringSolutionsV3.ampTargetY,
+ chassisSpeeds.vxMetersPerSecond,
+ chassisSpeeds.vyMetersPerSecond,
+ pose.getRotation().getRadians());
FiringSolutionsV3.updateAmpR(distanceToMovingAmpTarget);
diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
index d9304e4..9e79f9c 100644
--- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -1,23 +1,16 @@
package frc.robot.subsystems;
-import frc.robot.SwerveModule;
-import frc.robot.Constants;
-import frc.robot.Robot;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-
-import java.util.Optional;
-import java.util.function.BooleanSupplier;
-
-import org.photonvision.EstimatedRobotPose;
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.PathPlannerLogging;
import com.pathplanner.lib.util.ReplanningConfig;
import edu.wpi.first.math.Matrix;
@@ -25,6 +18,9 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
@@ -37,18 +33,25 @@
import edu.wpi.first.units.Units;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
-import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import static edu.wpi.first.units.Units.Meters;
-import static edu.wpi.first.units.Units.MetersPerSecond;
-import static edu.wpi.first.units.Units.Volts;
-import static edu.wpi.first.units.MutableMeasure.mutable;
+import frc.lib.math.FiringSolutionsV3;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.SwerveModule;
+
+import org.photonvision.EstimatedRobotPose;
+
+import java.util.Optional;
+import java.util.function.BooleanSupplier;
public class SwerveSubsystem extends SubsystemBase {
@@ -59,7 +62,8 @@ public class SwerveSubsystem extends SubsystemBase {
// Vars
private final VisionSubsystem vision;
private final SwerveDrivePoseEstimator swerveOdomEstimator;
- SwerveModuleState[] swerveModuleStates;
+ private SwerveModuleState[] swerveModuleStates;
+ public static boolean followingPath = false;
// Characterization stuff
private final MutableMeasure