diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7ac73846..6d63bef8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -128,6 +128,7 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { //preferences are initialized IF they don't already exist on the Rio + Preferences.initBoolean("Brushes", false); Preferences.initBoolean("Intake", false); Preferences.initBoolean("Climber", false); Preferences.initBoolean("Shooter", false); diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 6c8b9e50..6307889b 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -254,9 +254,9 @@ private DriveConstants() { public static final double k_PICKUP_NOTE_tx_D = 0; } public static final class ShooterConstants{ - public static final int SHOOTER_R_MOTORID = 2491; - public static final int SHOOTER_L_MOTORID = 2491; - public static final int PITCH_MOTOR_ID = 2491; + public static final int SHOOTER_R_MOTORID = 10; + public static final int SHOOTER_L_MOTORID = 9; + public static final int PITCH_MOTOR_ID = 22; public static final double SHOOTER_MOTOR_POWER = 1; public static final double SHOOTER_AMP_POWER = 0.3; public static final double SHOOTING_SPEED_MPS = 10; @@ -273,7 +273,7 @@ public static final class ShooterConstants{ public static final double SHOOTER_ANGLE_TOLERANCE = 0.5; public static final double SHOOTER_HEIGHT = 0.1; public static final double ANGLE_TICKS_PER_DEGREE = 2491; - public static final double DEGREES_PER_ROTATION = 2491; + public static final double DEGREES_PER_ROTATION = 360; public static final double DISTANCE_MULTIPLIER = 0; public static final double OFFSET_MULTIPLIER = 1; public static final double MINIMUM_SHOOTER_ANGLE = 10;//still has to be found @@ -282,9 +282,9 @@ public static final class ShooterConstants{ //PID coefficients for shooter: - public static final double kP = 0.1; - public static final double kI = 1e-4; - public static final double kD = 1; + public static final double kP = 1; + public static final double kI = 0; + public static final double kD = 0; public static final double kIz = 0; public static final double kFF = 0; public static final double kMaxOutput = 1; @@ -313,12 +313,12 @@ public static final class ClimberConstants{ public static final double CLIMBER_RPM = 0.1; } public static final class IndexerConstants{ - public static final int INDEXER_MOTOR = 2491; + public static final int INDEXER_MOTOR = 23; public static final double INDEXER_SPEED = 2491; } public static final class IntakeConstants{ - public static final int INTAKE_1_MOTOR = 2491; - public static final int INTAKE_2_MOTOR = 2491; + public static final int INTAKE_1_MOTOR = 20; + public static final int INTAKE_2_MOTOR = 21; public static final int BRUSH_1_MOTOR = 2491; public static final int BRUSH_2_MOTOR = 2491; public static final int BRUSH_3_MOTOR = 2491; diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index d8cfe4e4..162f4498 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -13,8 +13,10 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkAbsoluteEncoder.Type; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; @@ -22,40 +24,45 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class AngleShooterSubsystem extends SubsystemBase { CANSparkMax pitchMotor; SparkPIDController pitchPID; - CANcoder angleEncoder; + SparkAbsoluteEncoder absoluteEncoder; double shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; public static Pose2d dtvalues; public static ChassisSpeeds DTChassisSpeeds; public AngleShooterSubsystem() { pitchMotor = new CANSparkMax(ShooterConstants.PITCH_MOTOR_ID, MotorType.kBrushless); - + pitchMotor.setInverted(true); pitchPID = pitchMotor.getPIDController(); - pitchPID.setFF(ShooterConstants.pitchFeedForward); - } + absoluteEncoder = pitchMotor.getAbsoluteEncoder(Type.kDutyCycle); + absoluteEncoder.setPositionConversionFactor(1); + absoluteEncoder.setInverted(false); + SmartDashboard.putData("set zero offset", new InstantCommand(()->absoluteEncoder.setZeroOffset(SmartDashboard.getNumber("shooter encoder zero offset", absoluteEncoder.getZeroOffset())))); + } + public void pitchShooter(double pitchSpeed) { pitchMotor.set(pitchSpeed); } - + public double getShooterAngle() { - return angleEncoder.getPosition().getValueAsDouble() * ShooterConstants.DEGREES_PER_ROTATION; + return absoluteEncoder.getPosition() * ShooterConstants.DEGREES_PER_ROTATION; } - + public static void setDTPose(Pose2d pose) { dtvalues = pose; } - + public static void setDTChassisSpeeds(ChassisSpeeds speeds) { DTChassisSpeeds = speeds; } - + public double calculateSpeakerAngleDifference() { double deltaX; shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; @@ -69,19 +76,19 @@ public double calculateSpeakerAngleDifference() { double deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y); double speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2)); SmartDashboard.putNumber("dist to speakre", speakerDist); - + double shootingTime = speakerDist / shootingSpeed; // calculates how long the note will take to reach the target double currentXSpeed = DTChassisSpeeds.vxMetersPerSecond; double currentYSpeed = DTChassisSpeeds.vyMetersPerSecond; Translation2d targetOffset = new Translation2d(currentXSpeed * shootingTime*OFFSET_MULTIPLIER, currentYSpeed * shootingTime*OFFSET_MULTIPLIER); // line above calculates how much our current speed will affect the ending // location of the note if it's in the air for ShootingTime - + // next 3 lines set where we actually want to aim, given the offset our shooting // will have based on our speed double offsetSpeakerY = Field.SPEAKER_Y + targetOffset.getY(); double offsetSpeakerX; - + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { offsetSpeakerX = Field.RED_SPEAKER_X + targetOffset.getX(); } else { @@ -92,7 +99,7 @@ public double calculateSpeakerAngleDifference() { double offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaX, 2) + Math.pow(offsetDeltaY, 2)); SmartDashboard.putString("offset amount", targetOffset.toString()); SmartDashboard.putString("offset speaker location", - new Translation2d(offsetSpeakerX, offsetSpeakerY).toString()); + new Translation2d(offsetSpeakerX, offsetSpeakerY).toString()); // getting desired robot angle double totalDistToSpeaker = Math .sqrt(Math.pow(offsetSpeakerdist, 2) + Math.pow(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT, 2)); @@ -106,12 +113,16 @@ public double calculateSpeakerAngleDifference() { desiredShooterAngle = ShooterConstants.MAXIMUM_SHOOTER_ANGLE; } SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle); - + double differenceAngle = (desiredShooterAngle - this.getShooterAngle()); SmartDashboard.putNumber("differenceAngleShooter", differenceAngle); - + return differenceAngle; } - + @Override + public void periodic() { + SmartDashboard.putNumber("shooter angle encoder position", absoluteEncoder.getPosition()); + SmartDashboard.putNumber("shooter encoder zero offset", absoluteEncoder.getZeroOffset()); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index 642b7283..e486a9a0 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -15,12 +16,10 @@ public class IndexerSubsystem extends SubsystemBase { public IndexerSubsystem() { m_IndexerMotor = new CANSparkMax(IndexerConstants.INDEXER_MOTOR, MotorType.kBrushless); + m_IndexerMotor.setInverted(false); + m_IndexerMotor.burnFlash(); m_DistanceSensor = m_IndexerMotor.getAnalog(Mode.kAbsolute); } - - public boolean isNoteIn() { - return m_DistanceSensor.getVoltage()>2; - } public void on() { m_IndexerMotor.set(IndexerConstants.INDEXER_SPEED); @@ -33,8 +32,12 @@ public void off() { public void reverse() { m_IndexerMotor.set(-IndexerConstants.INDEXER_SPEED); } + public boolean isNoteIn() { + return m_DistanceSensor.getVoltage()>2; + } + @Override public void periodic() { - SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage()); + SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage()); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 7ab90aa2..19d862eb 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -11,6 +11,9 @@ import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.Preferences; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.settings.Constants.IntakeConstants; public class IntakeSubsystem extends SubsystemBase { @@ -24,40 +27,46 @@ public class IntakeSubsystem extends SubsystemBase { double intakeRunSpeed; public IntakeSubsystem() { intake1 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless); - intake2 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless); - brush1 = new CANSparkMax(IntakeConstants.BRUSH_1_MOTOR, MotorType.kBrushless); - brush2 = new CANSparkMax(IntakeConstants.BRUSH_2_MOTOR, MotorType.kBrushless); - brush3 = new CANSparkMax(IntakeConstants.BRUSH_3_MOTOR, MotorType.kBrushless); - + intake2 = new CANSparkMax(IntakeConstants.INTAKE_2_MOTOR, MotorType.kBrushless); + if(Preferences.getBoolean("Brushes", false)) { + brush1 = new CANSparkMax(IntakeConstants.BRUSH_1_MOTOR, MotorType.kBrushless); + brush2 = new CANSparkMax(IntakeConstants.BRUSH_2_MOTOR, MotorType.kBrushless); + brush3 = new CANSparkMax(IntakeConstants.BRUSH_3_MOTOR, MotorType.kBrushless); + brush1.setIdleMode(IdleMode.kCoast); + brush2.setIdleMode(IdleMode.kCoast); + brush3.setIdleMode(IdleMode.kCoast); + } intake2.follow(intake1); intake2.setInverted(true); intake1.setIdleMode(IdleMode.kCoast); intake2.setIdleMode(IdleMode.kCoast); - brush1.setIdleMode(IdleMode.kCoast); - brush2.setIdleMode(IdleMode.kCoast); - brush3.setIdleMode(IdleMode.kCoast); + intake1.burnFlash(); + intake2.burnFlash(); } public void intakeYes(double intakeRunSpeed) { intake1.set(intakeRunSpeed); - brush1.set(intakeRunSpeed); - brush2.set(intakeRunSpeed); - brush3.set(intakeRunSpeed); + if(Preferences.getBoolean("Brushes", false)) { + brush1.set(intakeRunSpeed); + brush2.set(intakeRunSpeed); + brush3.set(intakeRunSpeed); + } } public void intakeNo(double intakeRunSpeed) { intake1.set(-intakeRunSpeed); - brush1.set(-intakeRunSpeed); - brush2.set(-intakeRunSpeed); - brush3.set(-intakeRunSpeed); - - + if(Preferences.getBoolean("Brushes", false)) { + brush1.set(-intakeRunSpeed); + brush2.set(-intakeRunSpeed); + brush3.set(-intakeRunSpeed); + } } public void intakeOff() { intake1.set(0); - brush2.set(0); - brush1.set(0); - brush3.set(0); + if(Preferences.getBoolean("Brushes", false)) { + brush2.set(0); + brush1.set(0); + brush3.set(0); + } } - } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 21ad8b34..41a2f35e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,7 +3,15 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; - import com.revrobotics.CANSparkMax; + import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder; @@ -17,19 +25,21 @@ import frc.robot.settings.Constants; import frc.robot.settings.Constants.ShooterConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import edu.wpi.first.wpilibj2.command.SubsystemBase; - import static frc.robot.settings.Constants.ShooterConstants.*; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import static frc.robot.settings.Constants.ShooterConstants.*; public class ShooterSubsystem extends SubsystemBase { - CANSparkMax shooterR; - CANSparkMax shooterL; + TalonFX shooterR; + TalonFX shooterL; double runSpeed; + TalonFXConfigurator configurator; double differenceAngle; double currentHeading; double m_DesiredShooterAngle; - SparkPIDController shooterPID; + Slot0Configs PIDconfigs = new Slot0Configs(); + SparkPIDController pitchPID; double kP = Constants.ShooterConstants.kP; double kI = Constants.ShooterConstants.kI; @@ -49,34 +59,25 @@ public class ShooterSubsystem extends SubsystemBase { int accumulativeTurns; /** Creates a new Shooter. */ - public ShooterSubsystem(double runSpeed) { - SparkPIDController shooterPID; - shooterR = new CANSparkMax(ShooterConstants.SHOOTER_R_MOTORID, MotorType.kBrushless); - shooterL = new CANSparkMax(ShooterConstants.SHOOTER_L_MOTORID, MotorType.kBrushless); - shooterR.restoreFactoryDefaults(); - shooterL.follow(shooterR); - shooterL.setInverted(true); - shooterR.setIdleMode(IdleMode.kCoast); - shooterL.setIdleMode(IdleMode.kCoast); - - - encoder1 = shooterR.getEncoder(SparkRelativeEncoder.Type.kQuadrature, 4096); - - shooterPID = shooterR.getPIDController(); - + public ShooterSubsystem(double runSpeed) { + shooterR = new TalonFX(ShooterConstants.SHOOTER_R_MOTORID); + shooterL = new TalonFX(ShooterConstants.SHOOTER_L_MOTORID); + shooterL.setInverted(true); + shooterR.setInverted(false); + shooterL.setControl(shooterR.getAppliedControl()); + shooterL.setNeutralMode(NeutralModeValue.Coast); + shooterR.setNeutralMode(NeutralModeValue.Coast); + PIDconfigs = new Slot0Configs(); + + configurator = shooterR.getConfigurator(); + pitchPID.setFF(pitchFeedForward); + + PIDconfigs.kP = kP; + PIDconfigs.kI = kI; + PIDconfigs.kD = kD; + PIDconfigs.kS = kFF; - shooterPID.setP(kP); - shooterPID.setI(kI); - shooterPID.setD(kD); - shooterPID.setIZone(kIz); - shooterPID.setFF(kFF); - shooterPID.setOutputRange(kMinOutput, kMaxOutput); - - - //Adjust the value runSpeed later. (SmartDashboard stuff) - shooterPID.setReference(runSpeed, ControlType.kVelocity); - SmartDashboard.putNumber("P Gain", Constants.ShooterConstants.kP); SmartDashboard.putNumber("I Gain", Constants.ShooterConstants.kI); SmartDashboard.putNumber("D Gain",Constants.ShooterConstants.kD); @@ -96,29 +97,20 @@ public ShooterSubsystem(double runSpeed) { double ve = SmartDashboard.getNumber("Set Velocity", 0); - if((p != kP)) {shooterPID.setP(p); kP = p; } - if((i != kI)) {shooterPID.setI(i); kI = i; } - if((d != kD)) {shooterPID.setD(d); kD = d; } - - if((iz != kIz)) {shooterPID.setIZone(iz); kIz = iz;} - if((ff != kFF)) {shooterPID.setFF(ff); kFF = ff;} - if((max != kMaxOutput) || (min != kMinOutput)) - { - shooterPID.setOutputRange(min, max); - kMinOutput = min; kMaxOutput = max; - } + if((p != kP)) {PIDconfigs.kP = p; kP = p; } + if((i != kI)) {PIDconfigs.kI = i; kI = i; } + if((d != kD)) {PIDconfigs.kD = d; kD = d; } - SmartDashboard.putNumber("Process Variable", encoder1.getPosition()); - + if((ff != kFF)) {PIDconfigs.kS = ff; kFF = ff;} + configurator.apply(PIDconfigs); } public void shootThing(double runSpeed) { shooterR.set(runSpeed); - shooterL.set(runSpeed); } public double getError() { - return Math.abs(shooterR.getAbsoluteEncoder(Type.kDutyCycle).getVelocity()-ShooterConstants.RUNNING_VELOCITY_RPS); + return Math.abs(shooterR.getVelocity().asSupplier().get()-ShooterConstants.RUNNING_VELOCITY_RPS); } public void turnOff(){ shooterR.set(0);