Merge pull request #4 from primo4586/Arm-intake

Arm intake

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-

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();
+  }
+}

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();
+  }
+}

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);
+  }
+}

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
+  }
+}