import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder;
+ import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
-import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;
+import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode;
+import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest;
+import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget;
+ import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
// Goal Velocity / Double theCircumfrence
private double shooterTargetSpeed = 0;
- private double feederPower = 0;
- public double shooterPower = 0.5;
- private double feederPower = 0;
-
+
+ public boolean shooterAtMaxSpeed = false;
+ public boolean ballDetected = false;
//Velocity in rotations per second
VelocityVoltage voltageRequest = new VelocityVoltage(0);
- private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
+ private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
+
+ double powerModifier;
+ // for tracking current phase: used to adjust the setting
+ private FlywheelPhase phase;
+
+ private double torqueCurrentDebounceTime = 0.5;
+
+ VelocityDutyCycle velocityDutyCycle = new VelocityDutyCycle(0);
+ TorqueCurrentFOC torqueCurrentFOC = new TorqueCurrentFOC(0);
+
+ /*
+ * Debouncers take in certain statement. If it is false: it checks for how long (the first parameter)
+ * If it is long enough then return True.
+ */
+ private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling);
+
+ @AutoLogOutput private long launchCount = 0;
+ private boolean lastTorqueCurrentControl = false;
+
+ public enum FlywheelPhase {
+ BANG_BANG,
+ CONSTANT_TORQUE,
+ START_UP
+ }
+
public Shooter(){
- TalonFXConfiguration config = new TalonFXConfiguration();
- config.Slot0.kP = ShooterConstants.kP; //tune p value
- config.Slot0.kI = ShooterConstants.kI;
- config.Slot0.kD = ShooterConstants.kD;
- config.Slot0.kV = ShooterConstants.kV; //Maximum rps = 100 --> 12V/100rps
-
- config.TorqueCurrent.PeakReverseTorqueCurrent = ShooterConstants.SHOOTER_MAX_TORQUE_CURRENT; // we are making this a BANG BANG controller for talon fx
- config.MotorOutput.PeakForwardDutyCycle = ShooterConstants.MOTOR_BANG_BANG_MAX; // bang bang up
- // tune this
- config.MotorOutput.PeakReverseDutyCycle = ShooterConstants.MOTOR_BANG_BANG_MIN; // bang bang down
+ updateInputs();
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Slot0.kP = 0.15; //tune p value
+ config.Slot0.kI = 0;
+ config.Slot0.kD = 0.0;
+ config.Slot0.kV = 0.125; //Maximum rps = 100 --> 12V/100rps
shooterMotorLeft.getConfigurator().apply(config);
shooterMotorRight.getConfigurator().apply(config);
import org.littletonrobotics.junction.Logger;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
- import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
- import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
- import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.sim.TalonFXSimState;
/* ---------------- Constants ---------------- */
- private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE);
- private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE);
+ private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-180); // Change this later to the actual values
+ private static final double MAX_ANGLE_RAD = Units.degreesToRadians(180);
- private static final double MAX_VEL_RAD_PER_SEC = 15;
- //private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now
- // private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
+ private static final double MAX_VEL_RAD_PER_SEC = 600;
private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
- private static final double VERSA_RATIO = 5.0;
- private static final double TURRET_RATIO = 140.0 / 10.0;
- private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO;
+ private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO;
- private static final PIDController positionPID = new PIDController(15, 0, 0.25);
- //private static final PIDController longVelocityPID = new PIDController(15, 0, 1.0);
- private static final PIDController velocityPID = new PIDController(0.0, 0.0, 0.0);
+ private static final PIDController positionPID = new PIDController(15.0, 0, 0.0);
+ // private static final PIDController positionPID = new PIDController(15, 0, 0.25);
+ private static final PIDController velocityPID = new PIDController(0.2, 0.0, 0.0);
+ private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN);
+ private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN);
+ private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02
+ , 0.02);
+
private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0));
// private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0.010);
- private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, (1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt) * 0.8, 0);
+ private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt * 2.0, 0);
private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
-
/* ---------------- Constructor ---------------- */
public Turret() {
lastFrameVelocity = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble());
- // var request = velocityRequest.withVelocity(Units.radiansToRotations(targetVelocity)).withEnableFOC(false);
Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
- Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO);
+ Logger.recordOutput("Turret/setpointDeg", Units.radiansToDegrees(setpoint.position));
+
+ // --- Visualization ---
+ ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
+
+ updateInputs();
+ Logger.processInputs("Turret", inputs);
+
+ // SmartDashboard.putNumber("Encoder Left", encoderLeft.get());
+ // SmartDashboard.putNumber("Encoder Right", encoderRight.get());
+ Logger.recordOutput("Turret/setpointDeg", Units.radiansToDegrees(motorGoalRotations) / GEAR_RATIO);
+ //Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO);
+
+ // --- Position + velocity feedforward (MA-style) ---
+ // motor.setControl(request);
+ // motor.clearStickyFaults();
+
+ // --- Visualization ---
+ ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
}
/* ---------------- Simulation ---------------- */
package frc.robot.subsystems.turret;
+ import edu.wpi.first.math.geometry.Rotation3d;
+ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
+ import frc.robot.constants.Constants;
+ import frc.robot.constants.swerve.DriveConstants;
public class TurretConstants {
- public static double MAX_ANGLE = 180;
- public static double MIN_ANGLE = -180;
+ public static double MAX_ANGLE = 200;
+ public static double MIN_ANGLE = -200;
public static double MAX_VELOCITY = 10000000; // m/s
public static double MAX_ACCELERATION = 10000000; // m/s^2
- public static double TURRET_WIDTH = Units.feetToMeters(1.0);
+ public static double TURRET_WIDTH = Units.inchesToMeters(6.4);
public static double TURRET_RADIUS = TURRET_WIDTH / 2;
- public static double ROTATIONAL_VELOCITY_CONSTANT = 0.2;
+ public static double TURRET_TEETH_COUNT = 140.0; // the turret teeth count
+ public static double TURRET_GEAR_RATIO = 36.81818182;
+ public static int LEFT_ENCODER_TEETH = 15; // gear teeth
+ public static int RIGHT_ENCODER_TEETH = 22; // read above
+ public static int ENCODER_COUNT_TOTAL = 8192; // how many intervals it can have, like clicks on a clock chat gpt explained to me
- public static Translation2d DISTANCE_FROM_ROBOT_CENTER = new Translation2d(0, 0);
+ public static double LEFT_ENCODER_OFFSET = 9.52; // degrees 9.52 rot
+ public static double RIGHT_ENCODER_OFFSET = 6.53; // degrees 6.53 rot
- Translation3d TURRET_TO_CENTER_OF_ROBOT = new Translation3d(Units.inchesToMeters(4.875), -Units.inchesToMeters(26.5/2 - 11.375), 0.32);
- Translation3d CAMERA_TO_CENTER_OF_TURRET = new Translation3d(0.177, Units.inchesToMeters(1.0/8.0), 0.325);
- Rotation3d CAMERA_ROTATION = new Rotation3d(Units.degreesToRadians(-10), Units.degreesToRadians(-13), Units.degreesToRadians(-25));
+ public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters
- }
+ public static double CRT_TOLERANCE = 0.01;
+ }