"edu.wpi.first.math.**.proto.*",
"edu.wpi.first.math.**.struct.*",
],
- "java.dependency.enableDependencyCheckup": false,
+ "java.dependency.enableDependencyCheckup": false
}
case WaffleHouse: // AKA Betabot
turret = new Turret();
shooter = new Shooter();
- // hood = new Hood();
break;
case SwerveCompetition: // AKA "Vantage"
// Add 180 degrees to the rotation bc robot is backwards
drivepose = new Pose2d(
currentPose.getTranslation(),
- currentPose.getRotation()
- );
+ currentPose.getRotation());
ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
drivepose.exp(
new Twist2d(
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.turret.Turret;
-import frc.robot.util.FieldZone;
-import frc.robot.util.Vision.TurretVision;
public class SimpleAutoShoot extends Command {
private Turret turret;
private Drivetrain drivetrain;
- private TurretVision turretVision;
private Shooter shooter;
private double fieldAngleRad;
private double turretSetpoint;
private double adjustedSetpoint;
- private double yawToTagCamera;
private double yawToTag;
- private boolean turretVisionEnabled = false;
private boolean SOTM = true;
private Translation2d drivepose;
- private double lastPos = 0;
private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
private double lastTurretAngle = 0;
this.turret = turret;
this.drivetrain = drivetrain;
this.shooter = shooter;
- drivepose = drivetrain.getPose().getTranslation();
-
+ drivepose = drivetrain.getPose().getTranslation();
+
addRequirements(turret);
}
public void updateTurretSetpoint(Translation2d drivepose) {
-
- //FieldZone currentZone = getZone(drivepose);
+
+ // FieldZone currentZone = getZone(drivepose);
Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
double D_y;
double D_x;
- double timeToGoal = 0.0;
-
+
// If the robot is moving, adjust the target position based on velocity
if (SOTM) {
ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
double xVel = fieldRelVel.vxMetersPerSecond;
double yVel = fieldRelVel.vyMetersPerSecond;
-
+
+ double distance = target.getDistance(drivepose);
+ double speed = Math.hypot(xVel, yVel);
+ double timeToGoal = speed > 0.1 ? distance / speed : 0.0;
+
D_y = target.getY() - drivepose.getY() - timeToGoal * yVel;
D_x = target.getX() - drivepose.getX() - timeToGoal * xVel;
} else {
fieldAngleRad = Math.atan2(D_y, D_x);
// Calculate robot heading and adjust for reverse drive
- double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment
+ double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive
+ // adjustment
// Calculate turret setpoint (angle relative to robot heading)
turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0);
// System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
}
- public void updateYawToTag(){
+ public void updateYawToTag() {
// Calculate the yaw offset to the tag
double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY();
double D_x = FieldConstants.getHubTranslation().getX() - drivetrain.getPose().getX();
public void initialize() {
// Initialize setpoint calculation and set the initial goal for the turret
updateTurretSetpoint(drivepose);
- // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2));
turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0);
}
@Override
public void execute() {
- //shooter.setShooterPower(ShotInterpolation.shooterPowerMap.get(FieldConstants.getHubTranslation().toTranslation2d().getDistance(drivetrain.getPose().getTranslation())));
// Continuously update setpoints and adjust based on vision if available
drivepose = drivetrain.getPose().getTranslation();
updateTurretSetpoint(drivepose);
updateYawToTag();
- // double turretVelocity =
- // turretAngleFilter.calculate(
- // new Rotation2d(Units.degreesToRadians(turretSetpoint)).minus(new Rotation2d(Units.degreesToRadians(lastTurretAngle))).getRadians() / Constants.LOOP_TIME);
-
double velocityAdjustment = 0;
- double turretAcceleration = ((-drivetrain.getAngularRate(2)) - (lastFrameVelocity)) / Constants.LOOP_TIME;
if (Math.abs(lastTurretAngle - turretSetpoint) > 90) {
velocityAdjustment = -drivetrain.getAngularRate(2) * 1.4;
}
Logger.recordOutput("Spinny accel", drivetrain.getAngularRate(2));
Logger.recordOutput("Original Turret Setpoint", turretSetpoint);
-
- turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) - velocityAdjustment);
- // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), (-drivetrain.getAngularRate(2)) + turretAcceleration * 0.3);
- //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity);
+
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)),
+ -drivetrain.getAngularRate(2) - velocityAdjustment);
lastTurretAngle = turretSetpoint;
lastFrameVelocity = drivetrain.getAngularRate(2);
// Set the turret to a safe position when the command ends
turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
}
-
- public boolean leftSide(Translation2d drivepose) {
- if (drivepose.getY() > (FieldConstants.FIELD_WIDTH / 2)) {
- return true;
- } else {
- return false;
- }
- }
-
- public FieldZone getZone(Translation2d drivepose) {
- return FieldConstants.getZone(drivepose);
- }
-}
-
+}
\ No newline at end of file
public class AcquireGamePiece extends SequentialCommandGroup {
/**
* Intakes a game piece
- *
+ *
* @param gamePiece The supplier for the game piece to intake
- * @param drive The drivetrain
+ * @param drive The drivetrain
*/
- public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive){
+ public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive) {
+ // TODO: Replace DoNothing with next year's intake command
addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive)));
}
}
\ No newline at end of file
public class Constants {
- // constants:
+ // constants:
public static final double GRAVITY_ACCELERATION = 9.8;
public static final double ROBOT_VOLTAGE = 12.0;
public static final CANBus RIO_CAN = new CANBus("rio");
public static final CANBus SUBSYSTEM_CANIVORE_CAN = new CANBus("CANivoreSub");
- // Logging
+ // Logging
public static final boolean USE_TELEMETRY = true;
public static enum Mode {
public static final double DEFAULT_DEADBAND = 0.00005;
public static final double TRANSLATIONAL_DEADBAND = 0.01;
-
+
public static final double ROTATION_DEADBAND = 0.01;
-
+
public static final double HEADING_DEADBAND = 0.05;
public static final double HEADING_SLEWRATE = 10;
- //Modes
+ // Modes
public static final Mode SIM_MODE = Mode.SIM;
public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE;
public class FieldConstants {
/** Width of the field [meters] */
- public static final double FIELD_LENGTH = Units.inchesToMeters(57*12 + 6+7.0/8.0);
+ public static final double FIELD_LENGTH = Units.inchesToMeters(57 * 12 + 6 + 7.0 / 8.0);
/** Height of the field [meters] */
- public static final double FIELD_WIDTH = Units.inchesToMeters(26*12 + 5);
+ public static final double FIELD_WIDTH = Units.inchesToMeters(26 * 12 + 5);
- /**Apriltag layout for 2026 REBUILT */
+ /** Apriltag layout for 2026 REBUILT */
public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
public static final double RED_BORDER = Units.inchesToMeters(180);
public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.75;
/** Location of hub target */
- public static final Translation3d HUB_BLUE =
- new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035, Units.inchesToMeters(72));
-
- public static final Translation3d HUB_RED =
- new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72));
-
- public static final Translation3d NEUTRAL_LEFT =
- new Translation3d(FIELD_LENGTH/2, LEFT_SIDE_TARGET, 0);
+ public static final Translation3d HUB_BLUE = new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035,
+ Units.inchesToMeters(72));
- public static final Translation3d NEUTRAL_RIGHT =
- new Translation3d(FIELD_LENGTH/2, RIGHT_SIDE_TARGET, 0);
+ public static final Translation3d HUB_RED = new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 + 20),
+ 4.035 + .67, Units.inchesToMeters(72));
- public static final Translation3d ALLIANCE_LEFT_BLUE =
- new Translation3d(BLUE_BORDER + 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
+ public static final Translation3d NEUTRAL_LEFT = new Translation3d(FIELD_LENGTH / 2, LEFT_SIDE_TARGET, 0);
- public static final Translation3d ALLIANCE_RIGHT_BLUE =
- new Translation3d(BLUE_BORDER + 5, RIGHT_SIDE_TARGET, 0);
+ public static final Translation3d NEUTRAL_RIGHT = new Translation3d(FIELD_LENGTH / 2, RIGHT_SIDE_TARGET, 0);
+ // previous hub + a few feet further back
+ public static final Translation3d ALLIANCE_LEFT_BLUE = new Translation3d(BLUE_BORDER + 5, LEFT_SIDE_TARGET, 0);
- public static final Translation3d ALLIANCE_LEFT_RED =
- new Translation3d(RED_BORDER + 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
+ public static final Translation3d ALLIANCE_RIGHT_BLUE = new Translation3d(BLUE_BORDER + 5, RIGHT_SIDE_TARGET, 0);
- public static final Translation3d ALLIANCE_RIGHT_RED =
- new Translation3d(RED_BORDER + 5, RIGHT_SIDE_TARGET, 0);
+ // previous hub + a few feet further back
+ public static final Translation3d ALLIANCE_LEFT_RED = new Translation3d(RED_BORDER + 5, LEFT_SIDE_TARGET, 0);
+
+ public static final Translation3d ALLIANCE_RIGHT_RED = new Translation3d(RED_BORDER + 5, RIGHT_SIDE_TARGET, 0);
public static final double BlueAllianceLine = BLUE_BORDER; // That's the distance from one side to the blue bump
- public static final double RedAllianceLine = RED_BORDER; //
+ public static final double RedAllianceLine = RED_BORDER; // That's the distance from one side to the red bump
public static Translation3d getHubTranslation() {
if (Robot.getAlliance() == Alliance.Blue) {
public static FieldZone getZone(Translation2d drivepose) {
double x = drivepose.getX();
double y = drivepose.getY();
- if(x < FieldConstants.RedAllianceLine) { // inside red
+ if (x < FieldConstants.RedAllianceLine) { // inside red
if (Robot.getAlliance() == Alliance.Red) {
return FieldZone.ALLIANCE;
} else {
import java.util.function.BooleanSupplier;
-import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = ()->false;
+ private final BooleanSupplier slowModeSupplier = () -> false;
private Shooter shooter;
private Turret turret;
private Hood hood;
private Intake intake;
private Spindexer spindexer;
- private Pose2d alignmentPose = null;
- private Command turretAutoShoot;
private Command autoShoot;
private boolean intakeBoolean = true;
- public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, Spindexer spindexer) {
+ public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake,
+ Spindexer spindexer) {
super(drive);
this.shooter = shooter;
this.turret = turret;
this.spindexer = spindexer;
}
- public void configureControls() {
+ public void configureControls() {
// Reset the yaw. Mainly useful for testing/driver practice
driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)
- )));
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
// Cancel commands
- driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{
+ driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(()->null);
+ getDrivetrain().setDesiredPose(() -> null);
CommandScheduler.getInstance().cancelAll();
}));
// Align wheels
driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand(
- ()->getDrivetrain().setStateDeadband(false),
- getDrivetrain()::alignWheels,
- interrupted->getDrivetrain().setStateDeadband(true),
- ()->false, getDrivetrain()).withTimeout(2));
+ () -> getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted -> getDrivetrain().setStateDeadband(true),
+ () -> false, getDrivetrain()).withTimeout(2));
// Intake
- if(intake != null){
- driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{
- if(intakeBoolean){
+ if (intake != null) {
+ driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
intake.setSetpoint(IntakeConstants.INTAKE_ANGLE);
intake.setFlyWheel();
intakeBoolean = false;
- }
- else{
+ } else {
intake.setSetpoint(IntakeConstants.STOW_ANGLE);
intake.stopFlyWheel();
}
}
// Auto shoot
- if(turret != null){
+ if (turret != null) {
driver.get(PS5Button.SQUARE).onTrue(
- new InstantCommand(()->{
- if (autoShoot != null && autoShoot.isScheduled()){
+ new InstantCommand(() -> {
+ if (autoShoot != null && autoShoot.isScheduled()) {
autoShoot.cancel();
- } else{
+ } else {
autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
CommandScheduler.getInstance().schedule(autoShoot);
}
- })
- );
+ }));
}
-
}
@Override
return false;
}
- public void startRumble(){
+ public void startRumble() {
driver.rumbleOn();
}
- public void endRumble(){
+ public void endRumble() {
driver.rumbleOff();
}
}
import frc.robot.util.ClimbArmSim;
public class Climb extends SubsystemBase {
-
+
private double startingPosition = 0;
- //Motors
+ // Motors
// TODO: tune better once design is finalized
private final PIDController pid = new PIDController(0.4, 4, 0.04);
private final DCMotor climbGearBox = DCMotor.getKrakenX60(1);
private TalonFXSimState encoderSim;
- //Mechism2d display
+ // Mechism2d display
private final Mechanism2d simulationMechanism = new Mechanism2d(3, 3);
private final MechanismRoot2d mechanismRoot = simulationMechanism.getRoot("Climb", 1.5, 1.5);
private final MechanismLigament2d simLigament = mechanismRoot.append(
- new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite))
- );
+ new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite)));
private final double climbGearRatio = 54 / 10 * 60 / 18; // gear ratio of 18
private ClimbArmSim climbSim;
public Climb() {
if (isSimulation()) {
encoderSim = motorLeft.getSimState();
- encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition)*climbGearRatio);
+ encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition) * climbGearRatio);
climbSim = new ClimbArmSim(
- climbGearBox,
- climbGearRatio,
- 0.1,
- 0.127,
- 0, //min angle
- Units.degreesToRadians(90), //max angle
- true,
- Units.degreesToRadians(startingPosition),
- 60
- );
+ climbGearBox,
+ climbGearRatio,
+ 0.1,
+ 0.127,
+ 0, // min angle
+ Units.degreesToRadians(90), // max angle
+ true,
+ Units.degreesToRadians(startingPosition),
+ 60);
climbSim.setIsClimbing(true);
}
pid.setIZone(1);
pid.setSetpoint(Units.degreesToRadians(startingPosition));
- motorLeft.setPosition(Units.degreesToRotations(startingPosition)*climbGearRatio);
- motorRight.setPosition(Units.degreesToRotations(startingPosition)*climbGearRatio);
+ motorLeft.setPosition(Units.degreesToRotations(startingPosition) * climbGearRatio);
+ motorRight.setPosition(Units.degreesToRotations(startingPosition) * climbGearRatio);
SmartDashboard.putData("PID", pid);
SmartDashboard.putData("Climb Display", simulationMechanism);
}
@Override
- public void periodic() {
+ public void periodic() {
double motorPosition = motorLeft.getPosition().getValueAsDouble();
- double currentPosition = Units.rotationsToRadians(motorPosition/climbGearRatio);
+ double currentPosition = Units.rotationsToRadians(motorPosition / climbGearRatio);
power = pid.calculate(currentPosition);
/**
* Sets the motor to an angle from 0-90 deg
+ *
* @param angle in degrees
*/
public void setAngle(double angle) {
/**
* Gets the current position of the motor in degrees
+ *
* @return The angle in degrees
*/
public double getAngle() {
/**
* Turns the motor to 90 degrees (extended positiion)
*/
- public void extend(){
+ public void extend() {
double extendAngle = 90;
setAngle(extendAngle);
}
/**
* Turns the motor to 0 degrees (climb position)
*/
- public void climb(){
+ public void climb() {
setAngle(startingPosition);
}
- public boolean isSimulation(){
+ public boolean isSimulation() {
return RobotBase.isSimulation();
}
}
\ No newline at end of file
import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
-public class Hood extends SubsystemBase implements HoodIO{
- private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+public class Hood extends SubsystemBase implements HoodIO {
+ private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
- private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE);
+ private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE);
private double MAX_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MAX_ANGLE);
private double MAX_VEL_RAD_PER_SEC = 25;
private double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
- private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO;
+ private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO;
- private static final PIDController positionPID = new PIDController(6, 0, 0.2);
+ private static final PIDController positionPID = new PIDController(6.0, 0, 0.2);
- private final TrapezoidProfile profile = new TrapezoidProfile(
- new TrapezoidProfile.Constraints(
- MAX_VEL_RAD_PER_SEC,
- MAX_ACCEL_RAD_PER_SEC2));
- private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0);
+ private final TrapezoidProfile profile = new TrapezoidProfile(
+ new TrapezoidProfile.Constraints(
+ MAX_VEL_RAD_PER_SEC,
+ MAX_ACCEL_RAD_PER_SEC2));
+ private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1,
+ 1. / DCMotor.getFalcon500(1).KvRadPerSecPerVolt, 0);
private State setpoint = new State();
private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE));
private double goalVelocityRadPerSec = 0.0;
- private static double kP = 2.0;
- private static double kD = 0.2;
+ private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
- private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
-
- public Hood(){
+ public Hood() {
motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO);
- motor.setNeutralMode(NeutralModeValue.Coast);
-
- motor.getConfigurator().apply(
- new Slot0Configs()
- .withKP(kP)
- .withKD(kD));
+ try {
+ Thread.sleep(100);
+ } catch (InterruptedException e) {
+ Thread.currentThread().interrupt();
+ }
+ motor.setNeutralMode(NeutralModeValue.Coast);
TalonFXConfiguration config = new TalonFXConfiguration();
+ config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+ motor.getConfigurator().apply(config);
+
+ setpoint = new State(getPositionRad() / GEAR_RATIO, 0.0);
+
+ SmartDashboard.putData("max", new InstantCommand(
+ () -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0)));
+ SmartDashboard.putData("medium",
+ new InstantCommand(() -> setFieldRelativeTarget(
+ new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)),
+ 0)));
+ SmartDashboard.putData("min", new InstantCommand(
+ () -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0)));
+ }
- motor.getConfigurator().apply(
- new com.ctre.phoenix6.configs.TalonFXConfiguration() {
- {
- MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
- }
- });
-
- setpoint = new State(getPositionRad() / GEAR_RATIO, 0.0);
-
- SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0)));
- SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0)));
- SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0)));
- }
-
- public double getPositionRad(){
- return Units.rotationsToRadians(motor.getPosition().getValueAsDouble());
- }
+ public double getPositionRad() {
+ return Units.rotationsToRadians(motor.getPosition().getValueAsDouble());
+ }
- public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) {
+ public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) {
goalAngle = angle;
goalVelocityRadPerSec = velocityRadPerSec;
}
- @Override
- public void periodic() {
+ @Override
+ public void periodic() {
updateInputs();
- State goalState = new State(
+ State goalState = new State(
MathUtil.clamp(goalAngle.getRadians(), MIN_ANGLE_RAD, MAX_ANGLE_RAD),
goalVelocityRadPerSec);
double targetVelocity;
- double motorSetpointPosition = (setpoint.position) * GEAR_RATIO;
+ double motorSetpointPosition = (setpoint.position) * GEAR_RATIO;
targetVelocity = positionPID.calculate(
motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
motorSetpointPosition);
-
+
targetVelocity += Units.rotationsToRadians(motorVelRotPerSec);
double voltage = feedForward.calculate(targetVelocity);
motor.setVoltage(voltage);
- Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue());
+ Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue());
Logger.recordOutput("Hood/velocitySetpoint", targetVelocity / GEAR_RATIO);
Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(setpoint.position) / GEAR_RATIO);
SmartDashboard.putData("Hood PID", positionPID);
- SmartDashboard.putNumber("Turret Position Deg", Units.radiansToDegrees(getPositionRad()) / GEAR_RATIO);
+ SmartDashboard.putNumber("Hood Position Deg", Units.radiansToDegrees(getPositionRad()) / GEAR_RATIO);
Logger.processInputs("Hood", inputs);
}
- @Override
+ @Override
public void updateInputs() {
inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO;
inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / GEAR_RATIO;
public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters
- // public static final double MAX_VELOCITY = 5; // rad/s
public static final double MAX_VELOCITY = 0.30; // rad/s
public static final double MAX_ACCELERATION = 30; // rad/s^2
public static final double MAX_ANGLE = 82; // degrees
- public static final double MIN_ANGLE = 58.5; // degrees
-
- public static final double START_ANGLE = 90-22.9; // degrees
+ public static final double MIN_ANGLE = 58.5; // degrees
// Arena dimensions
public static final double TARGET_HEIGHT = 2.44; // meters
public static final Translation2d TRANSLATION_TARGET = new Translation2d(0, 0);
public static final Rotation2d ROTATION_TARGET_ANGLE = new Rotation2d();
// Other
- public static final double INITIAL_VELOCTIY = 14.9; // meters per second
-
- // Testing purposes
- public static final double START_DISTANCE = 2; // meters
+ public static final double INITIAL_VELOCITY = 14.9; // meters per second
// Calibration Purposes
public static final double CURRENT_SPIKE_THRESHHOLD = 10.0; // amps
public interface HoodIO {
@AutoLog
- public static class HoodIOInputs{
+ public static class HoodIOInputs {
public double positionDeg = HoodConstants.MAX_ANGLE;
public double velocityRadPerSec = 0.0;
public double motorCurrent = 0.0;