From c3660651ba036e0841ca97ec3a9fcda7609d15a2 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 7 Apr 2022 18:29:48 -0600 Subject: [PATCH 1/8] schtuff --- src/main/java/frc4388/robot/Robot.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 10 +++++----- .../java/frc4388/robot/subsystems/VisionOdometry.java | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 78ad394..bdfaaac 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -152,7 +152,7 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { - // m_robotContainer.m_robotVisionOdometry.setLEDs(false); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bc32f7b..8079e75 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -458,14 +458,14 @@ public class RobotContainer { ); } - private SequentialCommandGroup shoot(double storageRunTime, double timeout) { + private ParallelRaceGroup shoot(double storageRunTime, double timeout) { return new SequentialCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withTimeout(timeout), + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), new ParallelCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).until(TrackTarget::isTargetNotLocked), + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true) ) - ); + ).withTimeout(timeout + storageRunTime); } SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), @@ -496,7 +496,7 @@ public class RobotContainer { SequentialCommandGroup extendThenAimTurret() { return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), - new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true) // TODO: optimize time + new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret), 1.0, true) // TODO: optimize time ); } diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index fda2586..c5b6d7d 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -45,7 +45,7 @@ public class VisionOdometry extends SubsystemBase { public VisionOdometry(SwerveDrive drive, Turret shooter) { // do{ m_camera = new PhotonCamera(VisionConstants.NAME); - // }while(m_camera.getLatestResult().getLatencyMillis() == 0.d); + // } while (m_camera.getLatestResult().getLatencyMillis() == 0.d); m_drive = drive; m_shooter = shooter; From a9a7b1bc68818ac1dc50879dd817aa2992a6d44e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 7 Apr 2022 18:33:44 -0600 Subject: [PATCH 2/8] tried an LED thing (servo) --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 3 ++- src/main/java/frc4388/robot/subsystems/LED.java | 15 ++++++++++----- src/main/java/frc4388/utility/LEDPatterns.java | 4 ++++ 5 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 47b9ab5..9b2107e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -168,7 +168,7 @@ public final class Constants { public static final double STORAGE_SPEED = 1.0;//0.9; } public static final class LEDConstants { - public static final int LED_SPARK_ID = 3; + public static final int LED_SPARK_ID = 4;//3; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED; public static final LEDPatterns SHOOTING_PATTERN = LEDPatterns.SOLID_GREEN; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bc32f7b..5e1e159 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -89,7 +89,7 @@ public class RobotContainer { public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); - private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad + private final LED m_robotLED = new LED(m_robotMap.newLED); // ! no LED makes aarav sad public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 2545ecb..20f080e 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -53,7 +53,8 @@ public class RobotMap { } /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); +// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + public final Servo newLED = new Servo(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() {} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 7a24a0c..53796c4 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -6,6 +6,7 @@ package frc4388.robot.subsystems; import java.util.logging.Logger; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LEDConstants; @@ -18,13 +19,15 @@ import frc4388.utility.LEDPatterns; public class LED extends SubsystemBase { private LEDPatterns m_currentPattern; - private Spark m_LEDController; + private Servo newLED; + // private Spark m_LEDController; /** * Add your docs here. */ - public LED(Spark LEDController){ - m_LEDController = LEDController; + public LED(Servo newLED){ + // m_LEDController = LEDController; + this.newLED = newLED; setPattern(LEDConstants.DEFAULT_PATTERN); updateLED(); Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); @@ -39,7 +42,8 @@ public class LED extends SubsystemBase { * Add your docs here. */ public void updateLED(){ - m_LEDController.set(m_currentPattern.getValue()); + newLED.setRaw((int) m_currentPattern.percentToPWM()); + // m_LEDController.set(m_currentPattern.getValue()); } /** @@ -47,7 +51,8 @@ public class LED extends SubsystemBase { */ public void setPattern(LEDPatterns pattern){ m_currentPattern = pattern; - m_LEDController.set(m_currentPattern.getValue()); + newLED.setRaw((int) m_currentPattern.percentToPWM()); + // m_LEDController.set(m_currentPattern.getValue()); } /** diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/LEDPatterns.java index 6df032c..bc318d9 100644 --- a/src/main/java/frc4388/utility/LEDPatterns.java +++ b/src/main/java/frc4388/utility/LEDPatterns.java @@ -42,4 +42,8 @@ public enum LEDPatterns { public float getValue() { return id; } + + public float percentToPWM() { + return (1000 + (getValue() * 1000)); + } } \ No newline at end of file From a420b8f47aa1c77be76bc1406a152115160024c6 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 7 Apr 2022 18:40:23 -0600 Subject: [PATCH 3/8] LEDs --> PWM --- src/main/java/frc4388/robot/RobotMap.java | 3 ++- src/main/java/frc4388/robot/subsystems/LED.java | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 20f080e..38d121f 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -23,6 +23,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.CAN; import edu.wpi.first.networktables.NetworkTableType; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; @@ -54,7 +55,7 @@ public class RobotMap { /* LED Subsystem */ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - public final Servo newLED = new Servo(LEDConstants.LED_SPARK_ID); + public final PWM newLED = new Servo(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() {} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 53796c4..385e3ef 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -6,6 +6,7 @@ package frc4388.robot.subsystems; import java.util.logging.Logger; +import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -19,13 +20,13 @@ import frc4388.utility.LEDPatterns; public class LED extends SubsystemBase { private LEDPatterns m_currentPattern; - private Servo newLED; + private PWM newLED; // private Spark m_LEDController; /** * Add your docs here. */ - public LED(Servo newLED){ + public LED(PWM newLED){ // m_LEDController = LEDController; this.newLED = newLED; setPattern(LEDConstants.DEFAULT_PATTERN); From acfa52c8982a81e149832e41b76d2c5cff562660 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 7 Apr 2022 19:02:32 -0600 Subject: [PATCH 4/8] back to Spark LEDs --- .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 4 ++-- .../java/frc4388/robot/subsystems/LED.java | 18 +++++++++--------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 55fbbde..8079e75 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -89,7 +89,7 @@ public class RobotContainer { public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); - private final LED m_robotLED = new LED(m_robotMap.newLED); // ! no LED makes aarav sad + private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 38d121f..5e5da07 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -54,8 +54,8 @@ public class RobotMap { } /* LED Subsystem */ -// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - public final PWM newLED = new Servo(LEDConstants.LED_SPARK_ID); + public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); +// public final PWM newLED = new Servo(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() {} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 385e3ef..cb6f53e 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -20,15 +20,15 @@ import frc4388.utility.LEDPatterns; public class LED extends SubsystemBase { private LEDPatterns m_currentPattern; - private PWM newLED; - // private Spark m_LEDController; + // private PWM newLED; + private Spark m_LEDController; /** * Add your docs here. */ - public LED(PWM newLED){ - // m_LEDController = LEDController; - this.newLED = newLED; + public LED(Spark LEDController){ + m_LEDController = LEDController; + // this.newLED = newLED; setPattern(LEDConstants.DEFAULT_PATTERN); updateLED(); Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); @@ -43,8 +43,8 @@ public class LED extends SubsystemBase { * Add your docs here. */ public void updateLED(){ - newLED.setRaw((int) m_currentPattern.percentToPWM()); - // m_LEDController.set(m_currentPattern.getValue()); + // newLED.setRaw((int) m_currentPattern.percentToPWM()); + m_LEDController.set(m_currentPattern.getValue()); } /** @@ -52,8 +52,8 @@ public class LED extends SubsystemBase { */ public void setPattern(LEDPatterns pattern){ m_currentPattern = pattern; - newLED.setRaw((int) m_currentPattern.percentToPWM()); - // m_LEDController.set(m_currentPattern.getValue()); + // newLED.setRaw((int) m_currentPattern.percentToPWM()); + m_LEDController.set(m_currentPattern.getValue()); } /** From 713feb9c6b04ac7346872023232a1a09b133e95f Mon Sep 17 00:00:00 2001 From: Ryan Date: Fri, 8 Apr 2022 10:20:43 -0600 Subject: [PATCH 5/8] speed up 3 ball auto --- src/main/deploy/pathplanner/JMove2.path | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/deploy/pathplanner/JMove2.path b/src/main/deploy/pathplanner/JMove2.path index 5bd47dc..8b9f54e 100644 --- a/src/main/deploy/pathplanner/JMove2.path +++ b/src/main/deploy/pathplanner/JMove2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":7.66,"y":0.7},"prevControl":null,"nextControl":{"x":7.465685817447585,"y":2.0602977914816596},"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":10.6,"y":2.118171324950245},"prevControl":{"x":10.796770013793191,"y":3.8080785022329553},"nextControl":null,"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":7.66,"y":0.7},"prevControl":null,"nextControl":{"x":7.708937737734877,"y":2.3864441401993526},"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":11.0,"y":2.118171324950245},"prevControl":{"x":11.180129214388481,"y":3.8472372199577447},"nextControl":null,"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8079e75..95d4ee2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -496,14 +496,14 @@ public class RobotContainer { SequentialCommandGroup extendThenAimTurret() { return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), - new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret), 1.0, true) // TODO: optimize time + new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 0.5, true) // TODO: optimize time ); } ParallelDeadlineGroup idleDrumUntilShootingFirstBall() { return new ParallelDeadlineGroup( extendThenAimTurret(), - new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom) + new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(9000), m_robotBoomBoom) ); } @@ -527,7 +527,7 @@ public class RobotContainer { // ParallelCommandGroup intakeWithPath2AndTrackTarget = new ParallelCommandGroup(intakeWithPath2, weirdAutoShootingGroup3); ParallelDeadlineGroup intakeWithPath2AndIdleShooterAndAimTurret = new ParallelDeadlineGroup( - intakeWithPath2(4.2), + intakeWithPath2(2.8), new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom), new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true) ); @@ -560,10 +560,11 @@ public class RobotContainer { // ! THREE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE) SequentialCommandGroup threeBallAuto = new SequentialCommandGroup( idleDrumUntilShootingFirstBall(), - shoot(1.0), // TODO: optimize time + shoot(0.8), // TODO: optimize time brakeStorage(0.1), - intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget - shoot(2.3), // TODO: optimize time + intakeWithPathAndTrackTarget, + // intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget + shoot(0.8), // TODO: optimize time brakeStorage(0.1), intakeWithPath2AndIdleShooterAndAimTurret, shoot(4.0), // TODO: optimize time From fc903b1daee0c29a299d8b1a29bd5045f9dec747 Mon Sep 17 00:00:00 2001 From: Ryan Date: Fri, 8 Apr 2022 10:24:25 -0600 Subject: [PATCH 6/8] asd --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 95d4ee2..3559a0b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -496,7 +496,7 @@ public class RobotContainer { SequentialCommandGroup extendThenAimTurret() { return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), - new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 0.5, true) // TODO: optimize time + new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret), 0.5, true) // TODO: optimize time ); } From d9d0dcc8230cee22fd327a3f7c23c13a5a77cd9d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 8 Apr 2022 12:03:57 -0600 Subject: [PATCH 7/8] added trig solution for offset --- src/main/java/frc4388/robot/Constants.java | 3 +++ .../commands/ShooterCommands/TrackTarget.java | 17 +++++++++++------ 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9b2107e..8d03887 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -341,5 +341,8 @@ public final class Constants { public static final double EDGE_TO_CENTER = 20; public static final double LIMELIGHT_RADIUS = 8; public static final double SHOOTER_CORRECTION = 1.d; + + public static final double PIXELS_PER_DEGREE = LIME_HIXELS / H_FOV; + public static final double DEGREES_PER_PIXEL = 1 / PIXELS_PER_DEGREE; } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 9151974..03f6c8b 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -105,12 +105,8 @@ public class TrackTarget extends CommandBase { } // points = getFakePoints(); //// points = filterPoints(points); + Point average = VisionOdometry.averagePoint(points); - - double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; - output *= 2.1; - - m_turret.runTurretWithInput(output); // double position = m_turret.m_boomBoomRotateEncoder.getPosition(); // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || @@ -121,9 +117,18 @@ public class TrackTarget extends CommandBase { // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), // true); - double regressedDistance = getDistance(average.y); + + // ! offset trig solution + double desiredOffset = 10; // * inches + double angleOffset = Math.toDegrees(Math.atan(desiredOffset / regressedDistance)); // * degrees + double pixelOffset = angleOffset * VisionConstants.PIXELS_PER_DEGREE; + double output = ((average.x + pixelOffset) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + output *= 2.1; + + m_turret.runTurretWithInput(output); + // ! no longer a +30 lol -aarav double distAdj = SmartDashboard.getNumber("Distance Adjust", -35); velocity = m_boomBoom.getVelocity(regressedDistance + distAdj); From 820a93cad2f4d9fd85e921f82bec3512ea27eaff Mon Sep 17 00:00:00 2001 From: Ryan Date: Fri, 8 Apr 2022 13:35:49 -0600 Subject: [PATCH 8/8] back to +40 offset --- .../frc4388/robot/commands/ShooterCommands/TrackTarget.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 03f6c8b..82be61f 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -124,7 +124,7 @@ public class TrackTarget extends CommandBase { double angleOffset = Math.toDegrees(Math.atan(desiredOffset / regressedDistance)); // * degrees double pixelOffset = angleOffset * VisionConstants.PIXELS_PER_DEGREE; - double output = ((average.x + pixelOffset) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 2.1; m_turret.runTurretWithInput(output);