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

Fix odemtry and shooter hood

This commit is contained in:
Tyler Wilcox 2026-03-24 20:53:19 -04:00
parent ea3ff7afb5
commit da438acf39
5 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit 8eb0a76b2646235f173cd524fbf7462256ab1696 Subproject commit aad75b9b559825aaf60ad5bb550bf8263d3d203d

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

@ -4,6 +4,7 @@
package frc.robot; package frc.robot;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle;
import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.InvertedValue;
@ -107,10 +108,10 @@ public final class Constants {
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
public static final Angle DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0); public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(100); public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(200); public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final Angle DEFLECTOR_SET_ANGLE3 = edu.wpi.first.units.Units.Degrees.of(300); public static final double DEFLECTOR_SET_ANGLE3 = 300;
public static final double DEFLECTOR_SERVO_RATIO = 1.0; public static final double DEFLECTOR_SERVO_RATIO = 1.0;
} }

3
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -61,6 +61,7 @@ public class AutoShootCommandChassisTurret extends Command{
turretSubsystem.setLauncher(2500); turretSubsystem.setLauncher(2500);
hopperSubsystem.spindexerOn(); hopperSubsystem.spindexerOn();
autoCenterController.calculate(-100);
// Change to HUB tracking pipeline // Change to HUB tracking pipeline
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER); LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER);
@ -83,7 +84,7 @@ public class AutoShootCommandChassisTurret extends Command{
Pose3d closest = botPose.nearest(tagPoses); Pose3d closest = botPose.nearest(tagPoses);
Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closest); Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closest);
Transform3d delta = botPose.minus(closest); Transform3d delta = botPose.minus(closest);
driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? chassisRotateSpeed : -chassisRotateSpeed); // May need to flip this driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? -chassisRotateSpeed : chassisRotateSpeed); // May need to flip this
} }
else // We have an april tag, center it to the camera frame else // We have an april tag, center it to the camera frame
{ {

5
src/main/java/frc/robot/commands/SetDeflectorCommand.java
View File

@ -5,15 +5,14 @@
package frc.robot.commands; package frc.robot.commands;
import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.TurretSubsystem;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. **/ /** An example command that uses an example subsystem. **/
public class SetDeflectorCommand extends Command { public class SetDeflectorCommand extends Command {
private final TurretSubsystem m_subsystem; private final TurretSubsystem m_subsystem;
private Angle deployAngle; private double deployAngle;
public SetDeflectorCommand(TurretSubsystem subsystem, Angle deployAngle) { public SetDeflectorCommand(TurretSubsystem subsystem, double deployAngle) {
m_subsystem = subsystem; m_subsystem = subsystem;
this.deployAngle = deployAngle; this.deployAngle = deployAngle;
} }

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

@ -81,7 +81,7 @@ public class TurretSubsystem extends TestableSubsystem {
SparkMaxConfig config = new SparkMaxConfig() SparkMaxConfig config = new SparkMaxConfig()
.apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
config.closedLoop.p(1).i(0).d(0.1).outputRange(0, 5000); config.closedLoop.p(1).i(0).d(0.1);
config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
launcherControler = launcherMotor.getClosedLoopController(); launcherControler = launcherMotor.getClosedLoopController();
@ -105,10 +105,11 @@ public class TurretSubsystem extends TestableSubsystem {
turretMotor.setControl(new PositionVoltage(position)); turretMotor.setControl(new PositionVoltage(position));
} }
public void setTargetElevation(Angle elev) public void setTargetElevation(double elev)
{ {
double angle = elev.in(Units.Degrees); double pos = Math.abs((elev - 300)/300);
hood.setAngle(300/angle); Logger.recordOutput("ShooterHood-Angle", pos);
hood.set(pos);
} }
public Command revSpeedCommand() public Command revSpeedCommand()
@ -147,6 +148,7 @@ public class TurretSubsystem extends TestableSubsystem {
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run // This method will be called once per scheduler run
switch (currentState) { switch (currentState) {
case HOMING: case HOMING: