diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d58160e..1994a4f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -144,7 +144,7 @@ public final class Constants { } public static final class VisionConstants { - public static final String NAME = "Aarav is poopy"; + public static final String NAME = "photonCamera"; public static final int LIME_HIXELS = 640; public static final int LIME_VIXELS = 480; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b6fb4c..ddf68d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,10 +7,6 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; - -import edu.wpi.first.wpilibj.PWM; -import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -21,6 +17,7 @@ import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; +import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.commands.PivotCommand; @@ -50,6 +47,8 @@ public class RobotContainer { public final Claw m_robotClaw = new Claw(m_robotMap.servo); + public final Limelight m_limeLight = new Limelight(); + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index cace0a8..629c903 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -38,16 +38,4 @@ public class AutoBalance extends PelvicInflammatoryDisease { if (Math.abs(getError()) < 3) out2 = 0; drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); } - - @Override - public void initialize() { - super.initialize(); - // this.gyro.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } } diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java index c811a3e..289a56c 100644 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/LimeAlign.java @@ -4,31 +4,39 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.AbhiIsADumbass; -public class LimeAlign extends CommandBase { - public LimeAlign(SwerveDrive drive) { - addRequirements(drive); - } +public class LimeAlign extends PelvicInflammatoryDisease { - // Called when the command is initially scheduled. - @Override - public void initialize() { + SwerveDrive drive; + Limelight lime; + + + public LimeAlign(SwerveDrive drive, Limelight lime) { + super(0.1, 0.0, 0.0, 0.0, 10); + + this.drive = drive; + this.lime = lime; + addRequirements(drive, lime); } - // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public double getError() { + try { + return lime.getTargetPoints().get(0).x - (VisionConstants.LIME_HIXELS / 2); + } catch (AbhiIsADumbass abhiIsADumbass) { + abhiIsADumbass.printStackTrace(); + return 0; + } + } - // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true); } } diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 6c54024..a3c9e6c 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -4,7 +4,6 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; @@ -26,12 +25,13 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { /** produces the error from the setpoint */ public abstract double getError(); + /** figure it out bitch */ public abstract void runWithOutput(double output); // Called when the command is initially scheduled. @Override - public void initialize() { + public final void initialize() { output = 0; } @@ -39,7 +39,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { + public final void execute() { double error = getError(); cumError += error * .02; // 20 ms double delta = error - prevError; @@ -54,7 +54,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Returns true when the command should end. @Override - public boolean isFinished() { + public final boolean isFinished() { return Math.abs(getError()) < tolerance; } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index a940777..e8b8756 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -14,14 +14,14 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; -import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.AbhiIsADumbass; public class Limelight extends SubsystemBase { private PhotonCamera cam; - /** Creates a new LImelight. */ + + /** Creates a new Limelight. */ public Limelight() { cam = new PhotonCamera(VisionConstants.NAME); }