Merge pull request #129 from 2491-NoMythic/event_10k

Event 10k

src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto

@@ -5,7 +5,7 @@
       "x": 0.6842044526319189,
       "y": 6.690138362483131
     },
-    "rotation": -125.0
+    "rotation": -116.0
   },
   "command": {
     "type": "sequential",

src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto

@@ -23,18 +23,6 @@
             "name": "shooterOn"
           }
         },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "CloseMidPickupAuto"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "hardStop"
-          }
-        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto

@@ -5,7 +5,7 @@
       "x": 0.7607347530216694,
       "y": 4.480646518544946
     },
-    "rotation": 121.7
+    "rotation": 116.0
   },
   "command": {
     "type": "sequential",

src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 7.156417275797516,
-        "y": 1.7934317350116178
+        "x": 6.9848151084202525,
+        "y": 2.113187808819271
       },
       "prevControl": {
-        "x": 6.046263311717348,
-        "y": 1.3552130649799718
+        "x": 5.8746611443400845,
+        "y": 1.674969138787625
       },
       "nextControl": null,
       "isLocked": false,
@@ -31,7 +31,7 @@
   "rotationTargets": [
     {
       "waypointRelativePos": 0.6,
-      "rotationDegrees": -157.01,
+      "rotationDegrees": -168.42994609958126,
       "rotateFast": true
     }
   ],
@@ -45,7 +45,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -159.86,
+    "rotation": -165.06858282186255,
     "rotateFast": false
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.951779274060725,
-        "y": 1.6143455862325808
+        "x": 2.7779158761164506,
+        "y": 2.34690443283615
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 1.412869986233799
       },
       "prevControl": {
-        "x": 4.733816875041158,
-        "y": 2.313223391878882
+        "x": 4.783983565594652,
+        "y": 1.645754560785515
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/CloseAmpPickupFromMidAuto.path

@@ -31,7 +31,7 @@
   "rotationTargets": [
     {
       "waypointRelativePos": 0.15,
-      "rotationDegrees": -95.24734689547573,
+      "rotationDegrees": -91.0,
       "rotateFast": true
     }
   ],
@@ -56,7 +56,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -101.30993247402034,
+    "rotation": -91.98723203197048,
     "rotateFast": false
   },
   "reversed": false,

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

@@ -385,16 +385,16 @@ private void configureBindings() {
         new WaitCommand(0.5),
         new InstantCommand(()->intake.setNoteHeld(false))
         );
-        SmartDashboard.putNumber("Indexer Amp Speed", shooterAmpSpeed);
+        SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed);
       SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup(
-        new InstantCommand(()->shooter.shootWithSupplier(()->SmartDashboard.getNumber("amp RPS", shooterAmpSpeed), true), shooter),
+        new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter),
         new MoveMeters(driveTrain, 0.015, 0.5, 0, 0),
         new InstantCommand(driveTrain::pointWheelsInward, driveTrain),
         new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(15), angleShooterSubsystem),
-        new WaitUntil(()->(Math.abs(shooter.getLSpeed()-SmartDashboard.getNumber("amp RPS", shooterAmpSpeed))<1)&&(Math.abs(shooter.getRSpeed()-SmartDashboard.getNumber("amp RPS", shooterAmpSpeed))<1)),
+        new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<1)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<1)),
         new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)),
         new WaitCommand(0.1),
-        new InstantCommand(()->indexer.magicRPSSupplier(()->SmartDashboard.getNumber("Indexer Amp Speed", 150)), indexer),
+        new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer),
         new WaitCommand(0.5),
         new InstantCommand(()->intake.setNoteHeld(false))
       );
@@ -604,7 +604,6 @@ public void teleopPeriodic() {
     }
     SmartDashboard.putBoolean("is note seen", RobotState.getInstance().IsNoteSeen);
 		SmartDashboard.putBoolean("shooter in range", RobotState.getInstance().ShooterInRange);
-		SmartDashboard.putBoolean("shooter ready", RobotState.getInstance().ShooterReady);
   }
 
   public void logPower(){

src/main/java/frc/robot/commands/AimShooter.java

@@ -3,6 +3,7 @@
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.settings.Constants.Field;
@@ -61,6 +62,7 @@ public void execute() {
 		} else if (AmpSup.getAsBoolean()) {
 			angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE);
 		} else if(humanPlayerSupplier.getAsBoolean()) {
+			frc.robot.subsystems.RobotState.getInstance().lightsReset = true;
 			angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE);
 		} else if (groundIntakeSup.getAsBoolean()){
 			angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.GROUND_INTAKE_SHOOTER_ANGLE);

src/main/java/frc/robot/commands/CollectNote.java

@@ -125,7 +125,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    drivetrain.pointWheelsInward();
+    // drivetrain.pointWheelsInward();
     runsInvalid = 0;
     closeNote = false;
   }

src/main/java/frc/robot/commands/GroundIntake.java

@@ -14,6 +14,7 @@
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.RobotState;
 
 public class GroundIntake extends Command {
   /** Creates a new GroundIntake. */
@@ -33,19 +34,23 @@ public GroundIntake(IntakeSubsystem intake, IndexerSubsystem indexer, Drivetrain
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    if (intake.isNoteSeen()){
+      RobotState.getInstance().lightsReset = true;
+    }
     angleShooter.setDesiredShooterAngle(30);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    RobotState.getInstance().lightsReset = false;
     double mult = 0.43;
     double robotSpeed = Math.sqrt(Math.pow(driveTrain.getChassisSpeeds().vxMetersPerSecond, 2) + Math.pow(driveTrain.getChassisSpeeds().vyMetersPerSecond, 2));
     double rollerSpeed = (IntakeConstants.INTAKE_SPEED*IntakeConstants.INTAKE_MAX_VELOCITY - (mult * IntakeConstants.INTAKE_SPEED*IntakeConstants.INTAKE_MAX_VELOCITY)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IntakeConstants.INTAKE_SPEED*IntakeConstants.INTAKE_MAX_VELOCITY);
     // double rollerSpeed = ( - (mult * 9000)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * 10000);
     double sideSpeed =  (IntakeConstants.INTAKE_SIDE_SPEED - (mult * IntakeConstants.INTAKE_SIDE_SPEED)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IntakeConstants.INTAKE_SIDE_SPEED);
     double indexerSpeed = (IndexerConstants.INDEXER_INTAKE_SPEED- (mult * IndexerConstants.INDEXER_INTAKE_SPEED)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IndexerConstants.INDEXER_INTAKE_SPEED);
-    // intake.intakeYes(rollerSpeed, sideSpeed);
+      // intake.intakeYes(rollerSpeed, sideSpeed);
     intake.setVelocity(rollerSpeed);
     SmartDashboard.putNumber("GROUNDINTAKE/roller speed", rollerSpeed);
     intake.intakeSideWheels(sideSpeed);

src/main/java/frc/robot/commands/IndexCommand.java

@@ -153,11 +153,6 @@ public void execute() {
         }
       }
       boolean indexer = false;
-      if(shooter.validShot()&&!idleReving) {
-        RobotState.getInstance().ShooterReady = true;
-      } else {
-        RobotState.getInstance().ShooterReady = false;
-      }
       if(angleShooterSubsytem.validShot() && drivetrain.validShot() && shooter.validShot() && shooter.isReving() && !idleReving) {
         if (shootIfReadySupplier.getAsBoolean()) {
           indexer = true;
@@ -184,7 +179,6 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    RobotState.getInstance().ShooterReady = false;
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/IndicatorLights.java

@@ -4,6 +4,7 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Lights;
 import frc.robot.subsystems.RobotState;
@@ -14,6 +15,8 @@
 public class IndicatorLights extends Command {
 
   Lights lights;
+  Timer timer;
+  boolean noteWasIn;
  /**
  * This command controls the lights.  Middle Lights: if limelights are updating the odometry, these lights are green, otherwise they are red.
  * Side Lights: If a note is held, they are red if the shooter isn't rev'ed up, and green if they are rev'ed up. If a note is not held, they are off unless a note is seen by the intake
@@ -22,6 +25,8 @@ public class IndicatorLights extends Command {
 public IndicatorLights(Lights lights) {
     addRequirements(lights);
     this.lights = lights;
+    timer = new Timer();
+    timer.start();
   }
 
 
@@ -34,29 +39,42 @@ public void execute() {
     boolean noteSeenByLimelight = RobotState.getInstance().IsNoteSeen;
     boolean limelightsUpdated = RobotState.getInstance().LimelightsUpdated;
     boolean readyToShoot = noteInRobot&&limelightsUpdated;
-    boolean allSystemsGoodToGo = RobotState.getInstance().ShooterReady;
-
+
     if(limelightsUpdated) {
       lights.setMid(0, 40, 0);
     } else {
       lights.setMid(50, 0, 0);
     }
-
-    if(!noteInRobot) {
-      if(noteSeenByLimelight) {
-        lights.setSides(50, 50, 0);
+    double time  = timer.get();
+    boolean timerReset = RobotState.getInstance().lightsReset;
+    if(time < 2){
+      if(time%0.2<0.1) {
+        lights.setSides(255, 255, 255);
       } else {
         lights.setSides(0, 0, 0);
       }
     }
-    if(noteInRobot) {
-      if(allSystemsGoodToGo) {
-        lights.setSides(0, 50, 0);
-      } else {
-        lights.setSides(50, 0, 0);
+    else if(!noteInRobot) {
+      noteWasIn = false;
+      if(noteSeenByLimelight) {
+        lights.setSides(70, 35, 0);
+      } else {  
+        lights.setSides(0, 0, 0);
       }
     }
-
+    else{
+      if(!noteWasIn){
+        noteWasIn = true;
+        timer.reset();
+      }
+      lights.setProgress((RobotState.getInstance().ShooterError / 50)-0.2, 50, 0, 0, 0, 50, 0);
+    }
+    if(noteInRobot&&timerReset) {
+      if (time > 2){
+        timer.reset();
+      } 
+      RobotState.getInstance().lightsReset = false;
+    }
     lights.dataSetter();
   }
 

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

@@ -330,7 +330,7 @@ public static final class ClimberConstants{
 public static final class IndexerConstants{
   public static final int INDEXER_MOTOR = 11;
   public static final int CURRENT_LIMIT = 50;
-  public static final double INDEXER_INTAKE_SPEED = 1;//0.903 ;//speed to pick up at 10 ft/s
+  public static final double INDEXER_INTAKE_SPEED = 1*0.6;//0.903 ;//speed to pick up at 10 ft/s
   public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive
   public static final double INDEXER_SHOOTING_RPS = 90;
   public static final double INDEXER_SHOOTING_POWER = 1*0.6;
@@ -455,29 +455,35 @@ public final class PS4Operator{
   public final class Field{
 
     public static final double ROBOT_CENTER_TO_BUMBER = 0.419;
-    public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X = 2.79;//NOT fOUND
-    public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y = 4.33; //NOT FOUND
-    public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X = 13.99;
+    public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X = 2.945;//NOT fOUND
+    public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y = 4.585; //NOT FOUND
+    public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X = 13.94;
     public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_Y = 3.99;
     public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X-ROBOT_CENTER_TO_BUMBER;
     public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_Y+ROBOT_CENTER_TO_BUMBER;
     public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X+ROBOT_CENTER_TO_BUMBER;
     public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y-ROBOT_CENTER_TO_BUMBER;
 
-    public static final double ROBOT_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3;
-    public static final double ROBOT_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3;//16.38;//16.87; changed so that shots from the side wil aim to the opposite side, and bank in
-    public static final double SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213;//16.38;//16.87; changed so that shots from the side wil aim to the opposite side, and bank in
-    public static final double SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213;//16.38; changed so that shots from the side wil aim to the opposite side, and bank in
-    public static final double RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263;//5.46;
-    public static final double BLUE_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263;//5.46;
-    public static final double SPEAKER_Z = 2.08;//1.5;//1.8;//2.08; //height of opening. Changed so that the smaller spekeaker_x shots will still go in
+    public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213;
+    public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263;
+    public static final double RED_SPEAKER_Y = 5.58;
+    public static final double SHOOTER_RED_SPEAKER_X = 16.45;//changed so that shots from the side wil aim to the opposite side, and bank in
+    public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in
+
+    public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in
+    public static final double CALCULATED_BLUE_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263;
+    public static final double BLUE_SPEAKER_Y = 5.39;
+    public static final double SHOOTER_BLUE_SPEAKER_X = 0;
+    public static final double ROBOT_BLUE_SPEAKER_X =SHOOTER_BLUE_SPEAKER_X+0.6;
 
+    public static final double SPEAKER_Z = 2.08;//1.5;//1.8;//2.08; //height of opening. Changed so that the smaller spekeaker_x shots will still go in
+
     public static final double MAX_SHOOTING_DISTANCE = 9;
     public static final double SHORT_RANGE_SHOOTING_DIST = 3;
 
     public static final double AMPLIFIER_SHOOTER_ANGLE = 108;
     public static final double SUBWOOFER_ANGLE = 60;
-    public static final double PODIUM_SHOOTER_ANGLE = 36.3;
+    public static final double PODIUM_SHOOTER_ANGLE = 40;//36.3;
     public static final double FAR_STAGE_SHOOTER_ANGLE = 25.5;
     //24.5;
     public static final double OPPOSITE_STAGE_SHOOTER_ANGLE = 26;//24.5;
@@ -487,9 +493,9 @@ public final class Field{
     public static final double BLUE_FAR_STAGE_ROBOT_ANGLE = 184;
     public static final double RED_FAR_STAGE_ROBOT_ANGLE = -4;
     public static final double RED_OPPOSITE_STAGE_ROBOT_ANGLE = 32;//31
-    public static final double BLUE_OPPOSITE_STAGE_ROBOT_ANGLE = 149;
+    public static final double BLUE_OPPOSITE_STAGE_ROBOT_ANGLE = 148;
     public static final double RED_OVER_STAGE_PASS_ANGLE = 45;
-    public static final double BLUE_OVER_STAGE_PASS_ANGLE = -115;
+    public static final double BLUE_OVER_STAGE_PASS_ANGLE = 135;
 
     //angle at 60 for bounce techinque, didn't work
   }