diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7432df0..18c62d2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,7 +28,33 @@ public final class Constants { public static final double stickDeadband = 0.1; - + + public static final class IntakeArm{ //TODO: This must be tuned to specific robot + // mm + public static final int KMotorID = 0; + public static final double mmVelocity = 5.0; + public static final double mmAcceleration = 10.0; + public static final double mmJerk = 50; + public static final double KP = 24.0; + public static final double KD = 0.1; + public static final double KV = 0.12; + public static final double KS = 0.25; + public static final double PeakForwardVoltage = 11.5; + public static final double PeakReverseVoltage = -11.5; + public static final double SensorToMechanismRatio = 0; + public static final boolean ForwardSoftLimitEnable = true; + public static final double ForwardSoftLimitThreshold = 300; + public static final boolean ReverseSoftLimitEnable = true; + public static final double RevesrseSoftLimitThreshold = 0; + // not mm + public static final double minimumError = 0; + public static final double startingValue = 0; + public static final double zeroEncoderValue = 0; + public static final double intakeArmSpeed = 0; + // switch + public static final int switchID = 0; + + } public static final class Swerve { public static final int pigeonID = 10; public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java new file mode 100644 index 0000000..ee0a19f --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java @@ -0,0 +1,41 @@ +// 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.commands.IntakeArmCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.IntakeArmSubsystem; +import static frc.robot.Constants.IntakeArm.*; + +public class IntakeZero extends Command { + /** Creates a new SetSpeed. */ + private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance(); + public IntakeZero() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intakeArm.setSpeed(intakeArmSpeed); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intakeArm.setSpeed(0); + intakeArm.setEncoder(zeroEncoderValue); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return intakeArm.getSwitch(); + } +} diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java b/src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java new file mode 100644 index 0000000..75614c2 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java @@ -0,0 +1,39 @@ +// 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.commands.IntakeArmCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.IntakeArmSubsystem; + +public class MoveIntakeArmToDegree extends Command { + /** Creates a new MoveIntakeArmTo. */ + private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance(); + private double degree; + + public MoveIntakeArmToDegree(double degree) { + this.degree = degree; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intakeArm.moveArmTo(degree); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return intakeArm.checkIntakeArmPosion(); + } +} diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java new file mode 100644 index 0000000..e18843b --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java @@ -0,0 +1,26 @@ +// 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.commands.IntakeArmCommands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.IntakeArmSubsystem; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class SetSpeed extends InstantCommand { + private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance(); + private double speed; + public SetSpeed(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + this.speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intakeArm.setSpeed(speed); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java new file mode 100644 index 0000000..51cbb33 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java @@ -0,0 +1,97 @@ +// 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.subsystems; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import static frc.robot.Constants.IntakeArm.*; + +public class IntakeArmSubsystem extends SubsystemBase { + private TalonFX m_IntakeArmMotor; + private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0); + private final DigitalInput intakeSwitch = new DigitalInput(switchID); + // singleton + private static IntakeArmSubsystem instance; + + public static IntakeArmSubsystem getInstance() { + if (instance == null) { + instance = new IntakeArmSubsystem(); + } + return instance; + } + /** Creates a new IntakeArmSubsystem. */ + private IntakeArmSubsystem() { + this.m_IntakeArmMotor = new TalonFX(KMotorID); + TalonFXConfiguration configs = new TalonFXConfiguration(); + MotionMagicConfigs mm = new MotionMagicConfigs(); + + mm.MotionMagicCruiseVelocity = mmVelocity; + mm.MotionMagicAcceleration = mmAcceleration; + mm.MotionMagicJerk = mmJerk; + configs.MotionMagic = mm; + + configs.Slot0.kP = KP; + configs.Slot0.kD = KD; + configs.Slot0.kV = KV; + configs.Slot0.kS = KS; + + configs.Voltage.PeakForwardVoltage = PeakForwardVoltage; + configs.Voltage.PeakReverseVoltage = PeakReverseVoltage; + configs.Feedback.SensorToMechanismRatio = SensorToMechanismRatio; + + configs.SoftwareLimitSwitch.ForwardSoftLimitEnable = ForwardSoftLimitEnable; + configs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ForwardSoftLimitThreshold; + configs.SoftwareLimitSwitch.ReverseSoftLimitEnable = ReverseSoftLimitEnable; + configs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RevesrseSoftLimitThreshold; + + + // gives code to TalonFX + StatusCode status = StatusCode.StatusCodeNotInitialized; + for (int i = 0; i < 5; i++) { + status = m_IntakeArmMotor.getConfigurator().apply(configs); + if (status.isOK()) { + break; + } + } + if (!status.isOK()) { + System.out.println("Turret could not apply configs, error code: " + status.toString()); + } + + + m_IntakeArmMotor.setPosition(startingValue); + } + + // moving the arm + public void moveArmTo(double degrees){ + m_IntakeArmMotor.setControl(motionMagic.withPosition(degrees)); + } + //set speed + public void setSpeed(double speed){ + m_IntakeArmMotor.set(speed); + } + // set incoder idk + public void setEncoder (double encoderValue){ + m_IntakeArmMotor.setPosition(encoderValue); + } + // get if a switch is press + public boolean getSwitch(){ + return intakeSwitch.get(); + } + // check if intake arm is in place + public boolean checkIntakeArmPosion(){ + return Math.abs(m_IntakeArmMotor.getClosedLoopError().getValue()) < minimumError; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}