Distance measurements

simgui-ds.json

@@ -99,8 +99,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "78696e70757401000000000000000000",
+      "guid": "Keyboard0"
-      "useGamepad": true
     }
   ]
 }

src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto

@@ -23,7 +23,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "ChassisAlignShoot"
+                  "name": "TurretShoot"
                 }
               }
             ]

src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path

@@ -32,12 +32,12 @@
     },
     {
       "anchor": {
-        "x": 2.9483700538099855,
+        "x": 2.677383349903843,
         "y": 4.00221686746988
       },
       "prevControl": {
-        "x": 2.5986833068220334,
+        "x": 2.4274139067638107,
-        "y": 4.024072289156626
+        "y": 4.006125511257969
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -139,7 +139,7 @@ public final class Constants {
         public static final int TURRET_LIMIT_CHANNEL = 9;
         public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
         public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
-        public static final double HOMING_SPEED = .35;
+        public static final double HOMING_SPEED = .05;
         public static final double TURRET_GEAR_RATIO = 6;
 
         public static final int HOOD_SERVO_CHANNEL = 0;

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

@@ -55,6 +55,7 @@ public class RobotContainer {
 
         NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
         NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake());
+        NamedCommands.registerCommand("TurretShoot", new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
 
         turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
     }
@@ -82,8 +83,9 @@ public class RobotContainer {
         //m_driverController.a().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
         m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
         // m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+        m_driverController.a().whileTrue(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
         m_driverController.x().whileTrue(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
-        m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem));
+        m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem));
         m_driverController.y().onTrue(intakeSubsystem.deploy());
     }
 

src/main/java/frc/robot/Utils.java

@@ -95,14 +95,14 @@ public class Utils {
     }
 
     public static double getHoodAngleForDistance(double distanceInMeters){
-        if(distanceInMeters >= 0){
+        if(distanceInMeters >= 3){
-            return 0;
+            return 200;
         }
         return 0;
     }
 
     public static double getLauncherRPMForDistance(double distanceInMeters){ 
-        return (667.557*distanceInMeters) + 1024.699;
+        return Math.min((667.557*distanceInMeters) + 1024.699, 6000);
     }
 
     /**

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

@@ -17,6 +17,8 @@ import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
+import frc.robot.Constants.ShootingRegion;
+import frc.robot.Utils;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 
@@ -27,7 +29,7 @@ public class AutoTurretCommand extends Command {
     private TurretSubsystem turretSubsystem;
     private DriveSubsystem driveSubsystem;
 
-    private int[] shootingTagFilters;
+    private Alliance alliance;
 
     public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
         this.turretSubsystem = turretSubsystem;
@@ -43,13 +45,12 @@ public class AutoTurretCommand extends Command {
 
         DriverStation.getAlliance().ifPresent((aliance) -> {
             if (aliance == Alliance.Blue) {
-                shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
                 turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
+                aliance = Alliance.Blue;
             } else {
-                shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
                 turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
+                aliance = Alliance.Red;
             }
-            Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose());
         });
 
     }
@@ -57,6 +58,16 @@ public class AutoTurretCommand extends Command {
     @Override
     public void execute() {
 
+        ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), DriverStation.getAlliance().get());
+
+        if(shootingRegion != ShootingRegion.NON_SHOOTING_REGION){
+            Pose3d targetPose = Utils.findShootingTarget(shootingRegion, DriverStation.getAlliance().get());
+
+            turretSubsystem.setTurretTargetPose(targetPose);
+        }
+
+        Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose());
+
         Pose3d botPose = driveSubsystem.getBotPose();
         Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
                 new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));

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

@@ -2,6 +2,7 @@ package frc.robot.commands;
 
 import org.littletonrobotics.junction.Logger;
 
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -11,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants;
-import frc.robot.Constants.ShootingRegion;
 import frc.robot.Utils;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
@@ -28,7 +28,6 @@ public class AutoTurretShootCommand extends Command{
     private Command waitCommand = Commands.waitSeconds(1.5);
 
     private Alliance alliance;
-
     public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
         this.hopperSubsystem = hopperSubsystem;
         this.turretSubsystem = turretSubsystem;
@@ -47,16 +46,11 @@ public class AutoTurretShootCommand extends Command{
 
     @Override
     public void execute() {
-
-        ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), alliance);
-
-        // if(shootingRegion == )
-
         double distance = turretSubsystem.getTurretTargetPose().toPose2d().getTranslation().getDistance(driveSubsystem.getBotPose().getTranslation().toTranslation2d());
         Logger.recordOutput("AutoTurretShoot-DistanceToTarget(Meters)", distance);
 
-        turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(0));
+        turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(distance));
-        turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(0));
+        turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(distance));
 
         if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
             hopperSubsystem.feedKicker();
@@ -78,6 +72,15 @@ public class AutoTurretShootCommand extends Command{
 
         turretSubsystem.setLauncher(0);
         turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
+
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
+            } else {
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
+            }
+        });
+
         // turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
     }
 

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -206,14 +206,12 @@ public class TurretSubsystem extends TestableSubsystem {
 
     if(turretState == TurretState.HOMING)
     {
-      turretMotor.set(0.05);
-
       if(turretLimitSwitch.get())
       { // Turret has tripped the switch
         turretMotor.stopMotor();
         turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
         if(!Robot.isReal()){
-          turretOffsetInRotations = 1.5; // Flip turret around
+          turretOffsetInRotations = 0; // Flip turret around
         }
         turretState = TurretState.READY;
         forwardLimit = (1.5 + turretOffsetInRotations);
@@ -301,7 +299,7 @@ Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotation
     }
 
     turretMotor.setControl(turretControlMode.withPosition(posWithOffset));
-    turretSim.setRawRotorPosition(posWithOffset);
+    turretSim.setRawRotorPosition(-posWithOffset);
   }
 
   public boolean isTurretAtTarget(){