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;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
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 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 DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0);
public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(100);
public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(200);
public static final Angle DEFLECTOR_SET_ANGLE3 = edu.wpi.first.units.Units.Degrees.of(300);
public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final double DEFLECTOR_SET_ANGLE3 = 300;
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);
hopperSubsystem.spindexerOn();
autoCenterController.calculate(-100);
// Change to HUB tracking pipeline
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);
Logger.recordOutput("AutoAlign-ClosestAprilTagPose", 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
{

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

@ -5,15 +5,14 @@
package frc.robot.commands;
import frc.robot.subsystems.TurretSubsystem;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. **/
public class SetDeflectorCommand extends Command {
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;
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()
.apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
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);
launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
launcherControler = launcherMotor.getClosedLoopController();
@ -105,10 +105,11 @@ public class TurretSubsystem extends TestableSubsystem {
turretMotor.setControl(new PositionVoltage(position));
}
public void setTargetElevation(Angle elev)
public void setTargetElevation(double elev)
{
double angle = elev.in(Units.Degrees);
hood.setAngle(300/angle);
double pos = Math.abs((elev - 300)/300);
Logger.recordOutput("ShooterHood-Angle", pos);
hood.set(pos);
}
public Command revSpeedCommand()
@ -147,6 +148,7 @@ public class TurretSubsystem extends TestableSubsystem {
@Override
public void periodic() {
// This method will be called once per scheduler run
switch (currentState) {
case HOMING: