Fix interpolated second shot

src/main/deploy/pathplanner/2BallAutoBallDefense-StartMidTarmac-1BallSide.path

@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":5.61,"y":5.16},"prevControl":null,"nextControl":{"x":5.4285374393533585,"y":5.034997411766976},"holonomicAngle":-20.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.011847998379541,"y":4.467836783774834},"prevControl":{"x":5.181004180272515,"y":4.818231731981705},"nextControl":{"x":4.8498021046675,"y":4.132170289657036},"holonomicAngle":65.09523119190474,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.56043443732457,"y":3.2409178742408113},"prevControl":{"x":4.47941149046855,"y":3.6460326085209127},"nextControl":{"x":4.47941149046855,"y":3.6460326085209127},"holonomicAngle":90.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.919250344829804,"y":6.389238094931886},"prevControl":{"x":4.685577678064522,"y":5.990047289207863},"nextControl":{"x":5.197043305479015,"y":6.863801069374291},"holonomicAngle":175.03025927188963,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.8452268803271785,"y":7.465685817447585},"prevControl":{"x":5.613732746452834,"y":7.697179951321927},"nextControl":{"x":6.470528917410479,"y":6.84038378036429},"holonomicAngle":145.00797980144137,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.389238094931885,"y":6.111445134282674},"prevControl":{"x":5.798928053552308,"y":6.528134575256492},"nextControl":{"x":6.979548136311463,"y":5.6947556933088554},"holonomicAngle":161.56505117707795,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.817502242599422,"y":4.861376811361217},"prevControl":{"x":6.875375776068007,"y":4.757204451117763},"nextControl":null,"holonomicAngle":163.3007557660062,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":13.0,"maxAcceleration":3.0,"isReversed":null}

src/main/deploy/pathplanner/4BallAuto-StartTarmac-2BallSide.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":5.74,"y":2.78},"prevControl":null,"nextControl":{"x":5.9957436707293335,"y":2.1795184682961444},"holonomicAngle":30.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.6320336438141223,"y":2.2454930985811337},"prevControl":{"x":1.620458937120405,"y":2.2454930985811337},"nextControl":{"x":1.620458937120405,"y":2.2454930985811337},"holonomicAngle":60.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2963671496981648,"y":1.3195165630833965},"prevControl":{"x":1.4584130434083649,"y":1.365815389858628},"nextControl":{"x":1.1343212559879647,"y":1.273217736308165},"holonomicAngle":60.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.7246312973657008,"y":0.9375512421907299},"prevControl":{"x":1.8635277776884662,"y":1.365815389858628},"nextControl":{"x":1.8635277776884662,"y":1.365815389858628},"holonomicAngle":40.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.6807567286039053,"y":1.979274844625276},"prevControl":{"x":3.1135961006117623,"y":1.0880224292090535},"nextControl":{"x":4.1840666526365204,"y":2.770190439533669},"holonomicAngle":20.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.74,"y":2.78},"prevControl":{"x":5.488250129412112,"y":2.722126466531323},"nextControl":null,"holonomicAngle":30.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":13.0,"maxAcceleration":8.0,"isReversed":null}
+{"waypoints":[{"anchorPoint":{"x":5.74,"y":2.78},"prevControl":null,"nextControl":{"x":5.9957436707293335,"y":2.1795184682961444},"holonomicAngle":30.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.767425420842409,"y":1.6858372144531046},"prevControl":{"x":1.7558507141486916,"y":1.6858372144531046},"nextControl":{"x":1.7558507141486916,"y":1.6858372144531046},"holonomicAngle":60.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2963671496981648,"y":1.3195165630833965},"prevControl":{"x":1.4584130434083649,"y":1.365815389858628},"nextControl":{"x":1.1343212559879647,"y":1.273217736308165},"holonomicAngle":60.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.7246312973657008,"y":0.9375512421907299},"prevControl":{"x":1.8635277776884662,"y":1.365815389858628},"nextControl":{"x":1.8635277776884662,"y":1.365815389858628},"holonomicAngle":40.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.196242491611316,"y":3.04973250572025},"prevControl":{"x":3.463739612644983,"y":2.288521905271388},"nextControl":{"x":4.963548818704439,"y":3.84711054578883},"holonomicAngle":20.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.74,"y":2.78},"prevControl":{"x":5.488250129412112,"y":2.722126466531323},"nextControl":null,"holonomicAngle":30.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":13.0,"maxAcceleration":8.0,"isReversed":null}

src/main/java/frc/robot/Constants.java

@@ -47,11 +47,11 @@ public static class SubsystemEnableFlags {
         public static final boolean TRANSFER = true;
         public static final boolean INTAKE = true;
 
-        public static final boolean HANGER = false;
+        public static final boolean HANGER = true;
 
         public static final boolean DRIVETRAIN = true;
 
-        public static final boolean BALL_COLOR_SENSOR = false;
+        public static final boolean BALL_COLOR_SENSOR = true;
         public static final boolean BOTTOM_COLOR_SENSORS = false;
 
         public static final boolean IR_SENSORS = true;
@@ -83,31 +83,31 @@ public static class LimelightConstants {
 
     public static class TransferConstants {
         public static final double DEFAULT_TRANSFER_SPEED = 0.35; // In Percent 0.0 - 1.0
-        public static final double SHOOT_FORWARD_TRANSFER_SPEED = 0.5;
-        public static final double MANUAL_REVERSE_TRANSFER_SPEED = -0.35; // In Percent -1.0 - 0.0
+        public static final double SHOOT_FORWARD_TRANSFER_SPEED = 0.40;
+        public static final double MANUAL_REVERSE_TRANSFER_SPEED = -0.20; // In Percent -1.0 - 0.0
+        public static final double OUTTAKE_REVERSE_SPEED = -0.5;
 
         public static final int MAX_BALL_COUNT = 2;
         public static final int STARTING_BALL_COUNT = 1;
 
-        public static final int MIN_BALL_COLOR_PROXIMITY = 1500; // Raw Proximity value 0 - 2047 (0 being far away)
+        public static final int MIN_BALL_COLOR_PROXIMITY = 1000; // Raw Proximity value 0 - 2047 (0 being far away)
 
-        public static final Color RED_BALL_COLOR = new Color(1, 0, 0); // TODO: Set to Measured Ball Color
+        public static final Color RED_BALL_COLOR = new Color(0.54931640625, 0.33935546875, 0.112);
+        public static final Color BLUE_BALL_COLOR = new Color(0.142, 0.404296875, 0.454345703125);
 
-        public static final Color BLUE_BALL_COLOR = new Color(0, 0, 1); // TODO: Set to Measured Ball Color
+        public static final double MAX_BALL_COLOR_DEVIATION = 0.15;
 
-        public static final double MAX_BALL_COLOR_DEVIATION = 0.01;
-
-    }
+}
 
     public static class SwerveConstants {
         public static final boolean INVERT_TURN = true;
         public static final double DRIVETRAIN_MOTOR_DEADZONE_VOLTS = 0.4;
         public static final double DRIVETRAIN_TRACK_METERS = 0.4445;
         public static final double DRIVETRAIN_WHEELBASE_METERS = 0.4445;
 
-        public static final double X_ACCEL_RATE_LIMIT = 10;
+        public static final double X_ACCEL_RATE_LIMIT = 15;
         public static final double X_DECEL_RATE_LIMIT = 10;
-        public static final double Y_ACCEL_RATE_LIMIT = 10;
+        public static final double Y_ACCEL_RATE_LIMIT = 15;
         public static final double Y_DECEL_RATE_LIMIT = 10;
 
         public static final double GYRO_YAW_OFFSET = 45; // degrees //TODO: CHECK OFFSET is right, Intake is forward
@@ -351,7 +351,8 @@ public static class HangerConstants {
 
     public static class IntakeConstants {
         public static final double INTAKE_FORWARD_SPEED = 1; // In Percent 0.0 - 1.0
-        public static final double INTAKE_BACKWARD_SPEED = -0.5; // In Percent -1.0 - 0.0
+        public static final double INTAKE_BACKWARD_SPEED = -0.25; // In Percent -1.0 - 0.0
+        public static final double INTAKE_OUTTAKE_SPEED = -0.7;
     }
 
     public static class ShooterConstants {
@@ -391,9 +392,23 @@ public static class ShooterConstants {
                 new TrainingDataPoint(100, 123, 1.23, 110) // TODO: Change this to actual calibrated training (given test)
         ); // TODO: Create all training data
 
-        public static final List<TrainingDataPoint> SIMPLE_CALIB_TRAINING = Arrays.asList(
-                new TrainingDataPoint(0, 0, 0) //TODO: SET THIS
-        );
+        public static final List<TrainingDataPoint> SHOOTER_DATA = Arrays.asList(
+                // tuned 4/11 at 9:17:54 after 12pm on monday of easter break!!
+                    new TrainingDataPoint(60.40864, 000000, 2470),
+                    new TrainingDataPoint(75.33697, 000000, 2450),
+                    new TrainingDataPoint(92.11719, 070000, 2375),
+                    new TrainingDataPoint(107.1820, 120000, 2420),
+                    new TrainingDataPoint(114.4050, 120000, 2460),
+                    new TrainingDataPoint(122.4000, 120000, 2490),
+                    new TrainingDataPoint(138.0660, 125000, 2540),
+                    new TrainingDataPoint(154.0136, 160000, 2550),
+                    new TrainingDataPoint(177.1405, 190000, 2720),
+                    new TrainingDataPoint(206.1429, 210000, 2840)
+                );
+        public static final double SHOOTER_INTERPOLATION_MIN_VALUE = 60.5;
+        public static final double SHOOTER_INTERPOLATION_MAX_VALUE = 206;
+
+        public static final double TARGET_SHOOTING_WHILE_MOVING_ERROR = 0.01;
     }
     public static class LEDConstants {
         public static final double MIN_WAIT_TIME_BETWEEN_INSTRUCTIONS = 0.03;  // In Seconds

src/main/java/frc/robot/Robot.java

@@ -7,22 +7,13 @@
 
 package frc.robot;
 
-import com.revrobotics.ColorSensorV3;
-import edu.wpi.first.util.datalog.DataLog;
-import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.ScheduleCommand;
-import frc.robot.commands.hanger.HangerZeroRetract;
-import frc.robot.commands.shooter.ZeroHoodMotorCommand;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.hardware.Limelight;
 import frc.robot.helper.logging.RobotLogger;
-import frc.robot.subsystems.ColorsensorTestSubsystem;
-
-import java.awt.*;
-import java.util.logging.Logger;
 
 public class Robot extends TimedRobot {
   private static final RobotLogger logger = new RobotLogger(Robot.class.getCanonicalName());
@@ -44,11 +35,13 @@ public void robotPeriodic() {
 
   @Override
   public void disabledInit() {
+    Limelight.disable();
     logger.info("Robot Disabled");
   }
 
   @Override
   public void autonomousInit() {
+    Limelight.enable();
     logger.info("Auto Enabled");
     robotContainer.resetPose();
     autonomousCommand = robotContainer.getAutonomousCommand();
@@ -62,10 +55,12 @@ public void autonomousInit() {
 
   @Override
   public void teleopInit() {
+    Limelight.enable();
     logger.info("TeleOp Enabled");
     if (autonomousCommand != null) {
       autonomousCommand.cancel();
     }
+
   }
 
   @Override
@@ -77,8 +72,7 @@ public void testInit() {
     logger.info("Test Enabled");
     CommandScheduler.getInstance().cancelAll();
 
-    CommandScheduler.getInstance().schedule(new ZeroHoodMotorCommand(robotContainer.shooterSubsystem));
-    CommandScheduler.getInstance().schedule(new HangerZeroRetract(robotContainer.hangerSubsystem));
+    robotContainer.zeroSubsystems();
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -10,8 +10,7 @@
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.*;
 import edu.wpi.first.wpilibj2.command.button.Button;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -26,13 +25,14 @@
 import frc.robot.commands.intake.IntakeOn;
 import frc.robot.commands.intake.IntakeReverse;
 import frc.robot.commands.shooter.*;
+import frc.robot.commands.transfer.OuttakeFast;
 import frc.robot.commands.transfer.TransferIndexForward;
 import frc.robot.commands.transfer.TransferManualReverse;
+import frc.robot.commands.transfer.TransferShootForward;
 import frc.robot.hardware.Limelight;
 import frc.robot.helper.ControllerUtil;
 import frc.robot.helper.DPadButton;
 import frc.robot.helper.JoystickAnalogButton;
-import frc.robot.subsystems.ShooterSubsystem.ShooterLocationPreset;
 import frc.robot.subsystems.*;
 
 import java.awt.Robot;
@@ -54,7 +54,7 @@ public class RobotContainer {
     public SwerveDrive drivetrainSubsystem = null;
     private IntakeSubsystem intakeSubsystem = null;
 
-    public ShooterSubsystem shooterSubsystem = null;
+    private ShooterSubsystem shooterSubsystem = null;
     private TransferSubsystem transferSubsystem = null;
 
     public HangerSubsystem hangerSubsystem = null;
@@ -69,7 +69,6 @@ public RobotContainer() {
         LiveWindow.disableAllTelemetry();
         LiveWindow.setEnabled(false);
 
-
         // Initialize Active Subsystems
         if (LIMELIGHT)
             Limelight.init();
@@ -100,13 +99,6 @@ public RobotContainer() {
             configureHanger();
     }
 
-    /**
-     * Use this method to define your button->command mappings. Buttons can be created by
-     * instantiating a {@link GenericHID} or one of its subclasses ({@link
-     * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
-     * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-     */
-
     public Command getAutonomousCommand() {
         return AutoChooser.getCommand();
     }
@@ -174,14 +166,17 @@ private void configureDrivetrain() {
                 drivetrainSubsystem,
                 () -> -ControllerUtil.modifyAxis(driverController.getLeftY()) * SwerveConstants.MAX_VELOCITY_METERS_PER_SECOND,
                 () -> -ControllerUtil.modifyAxis(driverController.getLeftX()) * SwerveConstants.MAX_VELOCITY_METERS_PER_SECOND,
-                () -> -ControllerUtil.modifyAxis(operatorController.getLeftX()) * SwerveConstants.MAX_VELOCITY_METERS_PER_SECOND
+                () -> -ControllerUtil.modifyAxis(operatorController.getLeftX()) * SwerveConstants.MAX_VELOCITY_METERS_PER_SECOND,
+                transferSubsystem::isShooting
         );
 
         if(LIMELIGHT) {
             // Left Bumper Enables Auto Align
             driverLeftBumper.whenPressed(
                 autoAlign
             );
+
+            driverLeftBumper.whenHeld(new SetShooterPIDFromInterpolation(shooterSubsystem, transferSubsystem::isShooting, driverController));
         }
 
         //Any Significant Movement in driver's X interrupt auto align
@@ -190,58 +185,46 @@ private void configureDrivetrain() {
     }
 
     private void configureShooter() {
-
         DPadButton dPadUp = new DPadButton(operatorController, DPadButton.Direction.UP);
-        DPadButton dPadRight = new DPadButton(operatorController, DPadButton.Direction.RIGHT);
-        DPadButton dPadLeft = new DPadButton(operatorController, DPadButton.Direction.LEFT);
-
-        JoystickAnalogButton operatorRightTrigger = new JoystickAnalogButton(operatorController, XboxController.Axis.kRightTrigger.value);
-        operatorRightTrigger.setThreshold(0.1);
 
-        dPadRight.whenPressed(new SetShooterPreset(shooterSubsystem, ShooterLocationPreset.LAUNCHPAD));
-        dPadLeft.whenPressed(new SetShooterPreset(shooterSubsystem, ShooterLocationPreset.TARMAC_VERTEX));
 
         dPadUp.whenHeld(new ZeroHoodMotorCommand(shooterSubsystem));
 
-        operatorRightTrigger.whenHeld(new SetShooterPIDVelocityFromState(
-                shooterSubsystem,
-                shooterSubsystem::getFlywheelShooterStateFromPreset,
-                operatorController));
-
 
         // Vibrations
         if (TRANSFER) {
             new Button(() -> transferSubsystem.getCurrentBallCount() >= MAX_BALL_COUNT).whenPressed(new WaitAndVibrateCommand(driverController, 0.1, 0.1));
         }
-
-        // Flywheel Vibration from the SetShooterPIDVelocityFromStateCommand
     }
 
     private void configureTransfer() {
-        JoystickAnalogButton operatorLeftTrigger  = new JoystickAnalogButton(operatorController, XboxController.Axis.kLeftTrigger.value);
+        JoystickAnalogButton driverLeftTrigger = new JoystickAnalogButton(driverController, XboxController.Axis.kLeftTrigger.value);
 
-        operatorLeftTrigger.whenHeld(new TransferIndexForward(transferSubsystem), false);
+      driverLeftTrigger.whenHeld(new TransferShootForward(transferSubsystem, shooterSubsystem), false);
+//        operatorLeftTrigger.whenHeld(new TransferIndexForward(transferSubsystem), false);
     }
 
     private void configureIntake() {
         JoystickButton driverRightBumper = new JoystickButton(driverController, XboxController.Button.kRightBumper.value);
-        JoystickButton operatorBButton = new JoystickButton(operatorController, XboxController.Button.kB.value);
+        JoystickButton driverBButton = new JoystickButton(driverController, XboxController.Button.kB.value);
+        JoystickButton driverYButton = new JoystickButton(driverController, XboxController.Button.kY.value);
         JoystickButton operatorRightBumper = new JoystickButton(operatorController, XboxController.Button.kRightBumper.value);
 
         // Operator's Intake Up Button
-        operatorRightBumper.whenPressed(new IntakeOff(intakeSubsystem));
-
-
-        driverRightBumper.whenHeld(new IntakeOn(intakeSubsystem)); // TODO: bad
+        operatorRightBumper.whenHeld(new IntakeOn(intakeSubsystem));
+        driverRightBumper.whenHeld(new IntakeOn(intakeSubsystem));
 
-        if (TRANSFER)
-            operatorBButton.whenHeld(
+        if (TRANSFER) {
+            driverBButton.whenHeld(
                     new ParallelCommandGroup(
                             new IntakeReverse(intakeSubsystem),
                             new TransferManualReverse(transferSubsystem)
-                    )
+                    ), false
             );
 
+            driverYButton.whenHeld(new OuttakeFast(transferSubsystem, intakeSubsystem));
+        }
+
 
 
     }
@@ -262,27 +245,14 @@ private void configureHanger() {
                 .whenActive(new HangerZeroRetract(hangerSubsystem));
     }
 
-    public void resetPose() {
-        drivetrainSubsystem.resetOdometry(new Pose2d());
-    }
-
-    private static double deadband(double value, double deadband) {
-        return (Math.abs(value) > deadband) ? value : 0.0;
+    public void zeroSubsystems() {
+        if (SHOOTER)
+            CommandScheduler.getInstance().schedule(new ZeroHoodMotorCommand(shooterSubsystem));
+        if (HANGER)
+            CommandScheduler.getInstance().schedule(new HangerZeroRetract(hangerSubsystem));
     }
 
-    private static double modifyAxis(double value) {
-        double deadband = 0.05;
-        value = deadband(value, deadband);
-
-        if (value == 0) {
-            return 0;
-        }
-
-        SmartDashboard.setDefaultNumber("Joystick Input Exponential Power", 3);
-//
-        double exp = SmartDashboard.getNumber("Joystick Input Exponential Power", 3);
-        value = Math.copySign(Math.pow((((1 + deadband)*value) - deadband), exp), value);
-
-        return value;
+    public void resetPose() {
+        drivetrainSubsystem.resetOdometry(new Pose2d());
     }
 }