FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Distance measurements

This commit is contained in:
Tyler Wilcox 2026-04-16 15:58:14 -04:00
parent ed6ebb67c1
commit 9a58b54277
9 changed files with 41 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
simgui-ds.json
View File

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

2
src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto
View File

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

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

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

2
src/main/java/frc/robot/Constants.java
View File

@ -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;

4
src/main/java/frc/robot/RobotContainer.java
View File

@ -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());
}

6
src/main/java/frc/robot/Utils.java
View File

@ -95,14 +95,14 @@ public class Utils {
}
public static double getHoodAngleForDistance(double distanceInMeters){
if(distanceInMeters >= 0){
return 0;
if(distanceInMeters >= 3){
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);
}
/**

19
src/main/java/frc/robot/commands/AutoTurretCommand.java
View File

@ -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))));

21
src/main/java/frc/robot/commands/AutoTurretShootCommand.java
View File

@ -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.setLauncher(Utils.getLauncherRPMForDistance(0));
turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(distance));
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
}

6
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -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(){