diff --git a/src/main/deploy/choreo/test.traj b/src/main/deploy/choreo/test.traj new file mode 100644 index 0000000..631ce9d --- /dev/null +++ b/src/main/deploy/choreo/test.traj @@ -0,0 +1,32 @@ +{ + "name":"test", + "version":3, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.8321799635887146 m", "val":0.8321799635887146}, "y":{"exp":"0.7060884237289429 m", "val":0.7060884237289429}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.659694671630859 m", "val":4.659694671630859}, "y":{"exp":"0.6415539979934692 m", "val":0.6415539979934692}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.725978374481201 m", "val":2.725978374481201}, "y":{"exp":"4.064524173736572 m", "val":4.064524173736572}, "heading":{"exp":"-33.321136223151115 mrad", "val":-0.03332113622315112}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.7410621643066406 m", "val":2.7410621643066406}, "y":{"exp":"5.301389694213867 m", "val":5.301389694213867}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":null, + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 969c521..4f19ffe 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,6 +29,8 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Superstructure.SuperState; +import frc.robot.components.cancoder.CANcoderIO; +import frc.robot.components.cancoder.CANcoderIOSim; import frc.robot.components.canrange.CANrangeIOReal; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; @@ -51,6 +53,8 @@ import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.shooter.TurretIO; +import frc.robot.subsystems.shooter.TurretIOSim; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; @@ -349,7 +353,18 @@ public Robot() { ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getCompHood(), canivore, 11) : new HoodIOSim( - canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11)); + canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11), + ROBOT_MODE == RobotMode.REAL + // TODO id's + ? new TurretIO() + : new TurretIOSim(), + ROBOT_MODE == RobotMode.REAL + ? new CANcoderIO(30, TurretSubsystem.getCancoder24tConfigs(), canivore) + : new CANcoderIOSim(30, TurretSubsystem.getCancoder24tConfigs(), canivore), + ROBOT_MODE == RobotMode.REAL + ? new CANcoderIO(30, TurretSubsystem.getCancoder26tConfigs(), canivore) + : new CANcoderIOSim(30, TurretSubsystem.getCancoder26tConfigs(), canivore)); + // TODO climber break; } climber = @@ -471,10 +486,11 @@ public Robot() { }) .onTrue( Commands.runOnce(() -> addAutos()) - .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) - .ignoringDisable(true)); + .alongWith( + leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0) + .withTimeout(1.0) + .ignoringDisable(true))); // TODO tbh idk if the leds will work here - // Add autos when first connecting to DS new Trigger( () -> diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index be47ad1..bf59b56 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -284,6 +284,7 @@ private void addCommands() { intake.rest(), indexer.rest(), shooter.score( + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), () -> AutoAim.getCompensatedSOTMShotData( swerve.getPose(), @@ -296,6 +297,7 @@ private void addCommands() { intake.rest(), indexer.kick(), shooter.score( + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), () -> AutoAim.getCompensatedSOTMShotData( swerve.getPose(), @@ -308,6 +310,7 @@ private void addCommands() { intake.rest(), indexer.kick(), shooter.score( + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), () -> AutoAim.getCompensatedSOTMShotData( swerve.getPose(), @@ -319,6 +322,7 @@ private void addCommands() { intake.intake(), indexer.kick(), shooter.score( + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), () -> AutoAim.getCompensatedSOTMShotData( swerve.getPose(), diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIO.java b/src/main/java/frc/robot/components/cancoder/CANcoderIO.java index d3aa988..5d3967e 100644 --- a/src/main/java/frc/robot/components/cancoder/CANcoderIO.java +++ b/src/main/java/frc/robot/components/cancoder/CANcoderIO.java @@ -4,17 +4,41 @@ package frc.robot.components.cancoder; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; // for cancoders that aren't on a swerve module (eg arm, intake) -public interface CANcoderIO { - // TODO wherever you use this, create an alert for connected or not +public class CANcoderIO { + @AutoLog public static class CANcoderIOInputs { public boolean connected = false; public Rotation2d cancoderPositionRotations = new Rotation2d(); } - public void updateInputs(CANcoderIOInputs inputs); + protected final CANcoder cancoder; + + private final StatusSignal cancoderAbsolutePositionRotations; + + public CANcoderIO(int cancoderID, CANcoderConfiguration config, CANBus canbus) { + cancoder = new CANcoder(cancoderID, canbus); + cancoderAbsolutePositionRotations = cancoder.getAbsolutePosition(); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, cancoderAbsolutePositionRotations); + cancoder.getConfigurator().apply(config); + cancoder.optimizeBusUtilization(); + } + + public void updateInputs(CANcoderIOInputs inputs) { + BaseStatusSignal.refreshAll(cancoderAbsolutePositionRotations); + + inputs.connected = BaseStatusSignal.isAllGood(cancoderAbsolutePositionRotations); + inputs.cancoderPositionRotations = + Rotation2d.fromRotations(cancoderAbsolutePositionRotations.getValueAsDouble()); + } } diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIOReal.java b/src/main/java/frc/robot/components/cancoder/CANcoderIOReal.java deleted file mode 100644 index 43f2667..0000000 --- a/src/main/java/frc/robot/components/cancoder/CANcoderIOReal.java +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.components.cancoder; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Angle; - -public class CANcoderIOReal implements CANcoderIO { - private final CANcoder cancoder; - - private final StatusSignal cancoderAbsolutePositionRotations; - - public CANcoderIOReal(int cancoderID, CANcoderConfiguration config, CANBus canbus) { - cancoder = new CANcoder(cancoderID, canbus); - cancoderAbsolutePositionRotations = cancoder.getAbsolutePosition(); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, cancoderAbsolutePositionRotations); - cancoder.getConfigurator().apply(config); - cancoder.optimizeBusUtilization(); - } - - @Override - public void updateInputs(CANcoderIOInputs inputs) { - BaseStatusSignal.refreshAll(cancoderAbsolutePositionRotations); - - inputs.connected = BaseStatusSignal.isAllGood(cancoderAbsolutePositionRotations); - inputs.cancoderPositionRotations = - Rotation2d.fromRotations(cancoderAbsolutePositionRotations.getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java b/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java new file mode 100644 index 0000000..20943ca --- /dev/null +++ b/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java @@ -0,0 +1,27 @@ +package frc.robot.components.cancoder; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.sim.CANcoderSimState; +import edu.wpi.first.math.geometry.Rotation2d; + +public class CANcoderIOSim extends CANcoderIO { + private final CANcoderSimState sim; + + private double positionRotations; + + public CANcoderIOSim(int cancoderID, CANcoderConfiguration config, CANBus canbus) { + super(cancoderID, config, canbus); + this.sim = cancoder.getSimState(); + } + + public void setSimValues(double positionRotations) { + this.positionRotations = positionRotations; + } + + public void updateInputs(CANcoderIOInputs inputs) { + sim.setRawPosition(positionRotations); + + inputs.cancoderPositionRotations = Rotation2d.fromRotations(positionRotations); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 73f83f6..7c42e06 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -18,7 +18,7 @@ public interface Shooter extends Subsystem { * Sets hood angle and flywheel velocity based on distance from hub from the shot map + current * pose */ - public Command score(Supplier shotDataSupplier); + public Command score(Supplier robotPoseSupplier, Supplier shotDataSupplier); /** * Sets hood angle and flywheel velocity based on distance from hub from the feed map + current diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 864f880..3b45694 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -86,7 +86,7 @@ public Command testShoot() { }); } - public Command score(Supplier shotDataSupplier) { + public Command score(Supplier robotPoseSupplier, Supplier shotDataSupplier) { return this.run( () -> { hoodSetpoint = shotDataSupplier.get().hoodAngle(); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java new file mode 100644 index 0000000..0d5f589 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -0,0 +1,121 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; + +public class TurretIO { + public static double TURRET_GEAR_RATIO = (42.0 / 12.0) * (32.0 / 16.0) * (85.0 / 10.0); + + public static double CANCODER_ONE_TO_TURRET_GEAR_RATIO = (24.0 / 32.0) * (10.0 / 85.0); + + // idk + public static Rotation2d TURRET_MIN_ROTATIONS = Rotation2d.fromRotations(0.0); + public static Rotation2d TURRET_MAX_ROTATIONS = Rotation2d.fromRotations(0.8); + + // todo ID? + protected final TalonFX motor = new TalonFX(40, "*"); + + @AutoLog + public static class TurretIOInputs { + public double angularVelocityRotationsPerSec = 0.0; + public Rotation2d positionRotations = new Rotation2d(); + public double statorCurrentAmps = 0.0; + public double supplyCurrentAmp = 0.0; + public double voltage = 0.0; + public double tempCelsius = 0.0; + } + + private final StatusSignal angularVelocityRotationsPerSec = motor.getVelocity(); + private final StatusSignal positionRotations = motor.getPosition(); + private final StatusSignal supplyCurrentAmps = motor.getSupplyCurrent(); + private final StatusSignal statorCurrentAmps = motor.getStatorCurrent(); + private final StatusSignal voltage = motor.getMotorVoltage(); + private final StatusSignal tempCelcius = motor.getDeviceTemp(); + + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + private MotionMagicVoltage motionMagic = new MotionMagicVoltage(0.0); + + // todo + private Rotation2d turretSetpoint = Rotation2d.kZero; + + public TurretIO() { + + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.Feedback.SensorToMechanismRatio = TURRET_GEAR_RATIO; + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kV = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + motor.getConfigurator().apply(config); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + angularVelocityRotationsPerSec, + positionRotations, + voltage, + statorCurrentAmps, + supplyCurrentAmps, + tempCelcius); + motor.optimizeBusUtilization(); + } + + public void updateInputs(TurretIOInputs inputs) { + BaseStatusSignal.refreshAll( + positionRotations, + angularVelocityRotationsPerSec, + voltage, + statorCurrentAmps, + supplyCurrentAmps, + tempCelcius); + + inputs.positionRotations = Rotation2d.fromRotations(positionRotations.getValueAsDouble()); + inputs.angularVelocityRotationsPerSec = angularVelocityRotationsPerSec.getValueAsDouble(); + inputs.voltage = voltage.getValueAsDouble(); + inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); + inputs.supplyCurrentAmp = supplyCurrentAmps.getValueAsDouble(); + inputs.tempCelsius = tempCelcius.getValueAsDouble(); + } + + public void setTurretPosition(Rotation2d positionAngle) { + turretSetpoint = positionAngle; + motor.setControl( + motionMagic.withPosition( + MathUtil.clamp( + positionAngle.getRotations(), + TURRET_MIN_ROTATIONS.getRotations(), + TURRET_MAX_ROTATIONS.getRotations()))); + } + + public void resetTurretPosition(Rotation2d turretRotation) { + motor.setPosition(turretRotation.getRotations()); + } + + @AutoLogOutput(key = "Shooter/Turret/Setpoint") + public Rotation2d getTurretSetpoint() { + return turretSetpoint; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java new file mode 100644 index 0000000..47703aa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class TurretIOSim extends TurretIO { + TalonFXSimState motorSim; + + private final DCMotorSim physicsSim; + + private final double simLoopPeriod = 0.002; // 2 ms + private Notifier simNotifier = null; + private double lastSimTime = 0.0; + + public TurretIOSim() { + physicsSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.01, TurretIO.TURRET_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)); + + motorSim = motor.getSimState(); + motorSim.setMotorType(MotorType.KrakenX44); + motorSim.Orientation = ChassisReference.Clockwise_Positive; + + simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + final double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; + + motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); + + physicsSim.setInputVoltage(motorSim.getMotorVoltage()); + physicsSim.update(deltaTime); + + motorSim.setRawRotorPosition( + physicsSim.getAngularPositionRad() * (TurretIO.TURRET_GEAR_RATIO)); + motorSim.setRotorVelocity( + physicsSim.getAngularVelocityRPM() * TurretIO.TURRET_GEAR_RATIO); + }); + + simNotifier.startPeriodic(simLoopPeriod); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index eedea8c..466d40b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,6 +4,8 @@ package frc.robot.subsystems.shooter; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; @@ -12,6 +14,11 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Superstructure; +import frc.robot.components.cancoder.CANcoderIO; +import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; +import frc.robot.utils.FieldUtils; +import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; @@ -21,6 +28,7 @@ /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { + /** Creates a new TurretSubsystem. */ public static double HOOD_GEAR_RATIO = 58.96875; @@ -33,17 +41,32 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; double currentFilterValue = 0.0; - HoodIO hoodIO; - HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + private CANcoderIO cancoder24t; + private CANcoderIO cancoder26t; + private CANcoderIOInputsAutoLogged cancoder24tInputs = new CANcoderIOInputsAutoLogged(); + private CANcoderIOInputsAutoLogged cancoder26tInputs = new CANcoderIOInputsAutoLogged(); + + private HoodIO hoodIO; + private HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + private FlywheelIO flywheelIO; + private FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - FlywheelIO flywheelIO; - FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + private TurretIO turretIO; + private TurretIOInputsAutoLogged turretInputs = new TurretIOInputsAutoLogged(); private LinearFilter currentFilter = LinearFilter.movingAverage(10); - public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO) { + public TurretSubsystem( + FlywheelIO flywheelIO, + HoodIO hoodIO, + TurretIO turretIO, + CANcoderIO cancoder24t, + CANcoderIO cancoder26t) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; + this.turretIO = turretIO; + this.cancoder24t = cancoder24t; + this.cancoder26t = cancoder26t; } private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); @@ -55,9 +78,33 @@ public void periodic() { Logger.processInputs("Shooter/Flywheel", flywheelInputs); hoodIO.updateInputs(hoodInputs); Logger.processInputs("Shooter/Hood", hoodInputs); - + turretIO.updateInputs(turretInputs); + Logger.processInputs("Shooter/Turret", turretInputs); + cancoder24t.updateInputs(cancoder24tInputs); + Logger.processInputs("Shooter/Turret Cancoder24t", cancoder24tInputs); + cancoder26t.updateInputs(cancoder26tInputs); + Logger.processInputs("Shooter/Turret Cancoder26t", cancoder26tInputs); currentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); - // This method will be called once per scheduler run + } + + public static CANcoderConfiguration getCancoder24tConfigs() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.0; + + return config; + } + + public static CANcoderConfiguration getCancoder26tConfigs() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.0; + + return config; } @Override @@ -72,6 +119,12 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar .getDistance(feedTarget.get().getTranslation())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + turretIO.setTurretPosition( + AutoAim.getTurretTargetRotation( + FeedTargets.getFeedTarget(Superstructure.getFeedTarget()) + .getPose() + .getTranslation(), + robotPoseSupplier.get())); }); } @@ -81,6 +134,7 @@ public Command rest() { () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); // TODO: TUNE TUCKED POSITION IF NEEDED flywheelIO.setFlywheelVoltage(0.0); + turretIO.setTurretPosition(TurretIO.TURRET_MIN_ROTATIONS); }); } @@ -90,9 +144,35 @@ public Command spit() { () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); flywheelIO.setMotionProfiledFlywheelVelocity(20); + turretIO.setTurretPosition(TurretIO.TURRET_MIN_ROTATIONS); }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } + public Rotation2d getAbsoluteTurretRotations() { + // give valaues between 0 and 1 + Rotation2d cancoder1 = cancoder24tInputs.cancoderPositionRotations; + Rotation2d cancoder2 = cancoder26tInputs.cancoderPositionRotations; + + // if can one is bigger than can 2 its simply can1-can2 + // otherwise can1 + 1 - can2 because we want how much behind can1 it is + double diffRotations = (cancoder1.getRotations() - cancoder2.getRotations()) % 1; + // keeping track of how many total rots can1 is doing using the diff with can2 + // 26/2 because gear difference of 2 + double absoluteRotationsCan1 = diffRotations * (26.0 / 2.0); + + // turret maxes out at less then 1 rotation which is like 11 can1 rotations anyways and it + // should work up to there + // multiply abs can1 rots by the gear ratio + double turretRotations = absoluteRotationsCan1 * TurretIO.CANCODER_ONE_TO_TURRET_GEAR_RATIO; + + return Rotation2d.fromRotations(turretRotations); + } + + @AutoLogOutput(key = "Shooter/Turret/Turret Absolute Rotation") + public Rotation2d getTurretRotation() { + return getAbsoluteTurretRotations(); + } + @Override @AutoLogOutput(key = "Shooter/At Vel Setpoint") public boolean atFlywheelVelocitySetpoint() { @@ -106,7 +186,35 @@ public boolean atFlywheelVelocitySetpoint() { @AutoLogOutput(key = "Shooter/Hood/At Setpoint") public boolean atHoodSetpoint() { return MathUtil.isNear( - hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); + hoodInputs.hoodPositionRotations.getDegrees(), getHoodSetpoint().getDegrees(), 1); + } + + @Override + @AutoLogOutput(key = "Shooter/Turret/At Setpoint") + public boolean isFacingTarget() { + return MathUtil.isNear( + getAbsoluteTurretRotations().getDegrees(), getTurretSetpoint().getDegrees(), 2); + } + + @Override + @AutoLogOutput(key = "Shooter/Hood/Setpoint") + public Rotation2d getHoodSetpoint() { + return hoodIO.getHoodSetpoint(); + } + + @AutoLogOutput(key = "Shooter/Turret/Setpoint") + public Rotation2d getTurretSetpoint() { + return turretIO.getTurretSetpoint(); + } + + @AutoLogOutput(key = "Shooter/Turret/Cancoder 24t position") + public Rotation2d getTurretCancoder24tPosition() { + return cancoder24tInputs.cancoderPositionRotations; + } + + @AutoLogOutput(key = "Shooter/Turret/Cancoder 26t position") + public Rotation2d getTurretCancoder26tPosition() { + return cancoder26tInputs.cancoderPositionRotations; } @Override @@ -127,50 +235,20 @@ public Command testShoot() { () -> { hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + turretIO.setTurretPosition(TurretIO.TURRET_MIN_ROTATIONS); }); } - public Command score(Supplier shotDataSupplier) { + public Command score(Supplier robotPoseSupplier, Supplier shotDataSupplier) { return this.run( () -> { - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); + ShotData shotData = + AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); + hoodIO.setHoodPosition(shotData.hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + turretIO.setTurretPosition( + AutoAim.getTurretTargetRotation( + FieldUtils.getCurrentHubTranslation(), robotPoseSupplier.get())); }); } - - @Override - public Rotation2d getHoodSetpoint() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getHoodSetpoint'"); - } - - @Override - public boolean isFacingTarget() { - return false; // TODO turret facing hub - } - // public boolean isFacingTarget() { - // switch (Superstructure.getShotTarget()) { // ugh maybe this should be in robot.java - // case SCORE: - // return isFacingHub(); - // case FEED: - // return isFacingFeedTarget(); - // default: - // return false; - // } - // } - - // public boolean isFacingHub() { - // Rotation2d target = AutoAim.getVirtualHubYaw(getVelocityFieldRelative(), getPose()); - // return MathUtil.isNear( - // target.getRadians(), getPose().getRotation().getRadians(), 0.174533); // 10 degrees - // } - - // public boolean isFacingFeedTarget() { - // Translation2d feedTarget = - // FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose().getTranslation(); - // Rotation2d target = AutoAim.getTargetRotation(feedTarget, getPose()); - // return MathUtil.isNear( - // target.getRadians(), getPose().getRotation().getRadians(), 0.174533); // 10 degrees - // } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 8a333fe..e75dccc 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -89,6 +89,13 @@ public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPos return rot; } + // this should also adjust for like the turret offset from the robot rotation but like I don't + // know what that is + public static Rotation2d getTurretTargetRotation(Translation2d target, Pose2d robotPose) { + Rotation2d rot = getTargetRotation(target, robotPose).minus(robotPose.getRotation()); + return rot; + } + public static Rotation2d getVirtualHubYaw(ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose) { return getVirtualTargetYaw( FieldUtils.getCurrentHubTranslation(), fieldRelativeSpeeds, robotPose);