From 1955885124693c471eb601c4bfd32a501c4c75d8 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 20 Feb 2024 00:46:44 -0700 Subject: [PATCH] Started changing intake class from spark max to talon; Not done limit switch or robot container stuff; (im actually tweakin) --- .../java/frc4388/robot/RobotContainer.java | 20 ++--- .../robot/commands/Intake/ArmIntakeIn.java | 4 +- .../java/frc4388/robot/subsystems/Intake.java | 89 ++++++++++++++++++- 3 files changed, 100 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 51c0419..9b06a59 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -185,16 +185,16 @@ public class RobotContainer { /* Auto Recording */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - () -> getDeadbandedDriverController().getLeftX(), - () -> getDeadbandedDriverController().getLeftY(), - () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY(), - () -> getDeadbandedOperatorController().getLeftBumper(), - () -> getDeadbandedOperatorController().getRightBumper(), - "TwoNotePrt1.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // () -> getDeadbandedOperatorController().getLeftBumper(), + // () -> getDeadbandedOperatorController().getRightBumper(), + // "TwoNotePrt1.txt")) + // .onFalse(new InstantCommand()); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index 1952c2d..9b63f58 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -28,8 +28,8 @@ public class ArmIntakeIn extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - robotIntake.pidOut(); - robotIntake.spinIntakeMotor(); + robotIntake.talonPIDOut(); + robotIntake.talonSpinIntakeMotor(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 503db7c..eae9b05 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -6,6 +6,22 @@ package frc4388.robot.subsystems; import java.util.function.BooleanSupplier; +import javax.swing.text.Position; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.can.TalonSRXPIDSetConfiguration; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.ForwardLimitValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import com.ctre.phoenix6.signals.ReverseLimitValue; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkLimitSwitch; @@ -13,6 +29,7 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.CAN; import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; @@ -22,14 +39,26 @@ import frc4388.utility.Gains; public class Intake extends SubsystemBase { + //NEO private CANSparkMax intakeMotor; private CANSparkMax pivot; private SparkPIDController m_spedController; - private static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; private SparkLimitSwitch forwardLimit; private SparkLimitSwitch reverseLimit; private SparkLimitSwitch intakeforwardLimit; private SparkLimitSwitch intakereverseLimit; + + //Talon + private TalonFX talonIntake; + private TalonFX talonPivot; + private CANcoder encoder; + + private HardwareLimitSwitchConfigs l; + + TalonFXConfiguration doodooController = new TalonFXConfiguration(); + + + public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; private BooleanSupplier sup = () -> true; private BooleanSupplier dup = () -> false; @@ -37,6 +66,7 @@ public class Intake extends SubsystemBase { private double smartDashboardOuttakeValue; /** Creates a new Intake. */ + //For NEO public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { this.intakeMotor = intakeMotor; this.pivot = pivot; @@ -65,6 +95,63 @@ public class Intake extends SubsystemBase { SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } + //For Talon + public Intake(TalonFX talonIntake, TalonFX talonPivot) { + this.talonIntake = talonIntake; + this.talonPivot = talonPivot; + + talonIntake.getConfigurator().apply(new TalonFXConfiguration()); + talonPivot.getConfigurator().apply(new TalonFXConfiguration()); + + talonIntake.setNeutralMode(NeutralModeValue.Brake); + talonPivot.setNeutralMode(NeutralModeValue.Brake); + + talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); + talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs()); + + + + doodooController.Slot0.kP = armGains.kP; + doodooController.Slot1.kI = armGains.kI; + doodooController.Slot2.kD = armGains.kD; + + // in init function, set slot 0 gains + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 0.05; // An error of 0.5 rotations results in 12 V output + slot0Configs.kI = 0.0; // no output for integrated error + slot0Configs.kD = 0.0; // A velocity of 1 rps results in 0.1 V output + + talonPivot.getConfigurator().apply(slot0Configs); + + + } + + // ! Talon Methods + public void talonPIDIn() { + PositionVoltage request = new PositionVoltage(0).withSlot(0); + talonPivot.setControl(request.withPosition(53)); //TODO: Find actual value + } + + public void talonPIDOut() { + PositionVoltage request = new PositionVoltage(53).withSlot(53); + talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value + } + + public void talonHandoff() { + talonIntake.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + } + + public void talonSpinIntakeMotor() { + talonIntake.set(IntakeConstants.INTAKE_SPEED); + } + + public boolean getTalonIntakeLimitSwitchState() { + return false; + } + + + + // ! NEO Methods //hanoff public void spinIntakeMotor() { intakeMotor.set(IntakeConstants.INTAKE_SPEED);