upload code

Co-authored-by: Team841Aaron <[email protected]>

src/main/java/com/team841/Swerve23/Constants/SC.java

@@ -0,0 +1,19 @@
+package com.team841.Swerve23.Constants;
+
+public class SC {
+  public static class Intake {
+    public static final int currentLimit = 60; // in amps
+  }
+
+  public static class Arm {
+    public static final int currentLimit = 60; // in amps
+
+    public static class pidGains {
+      public static final Double kP = 0.0;
+      public static final Double kI = 0.0;
+      public static final Double kD = 0.0;
+      public static final Double tolerance = 0.0;
+      public static final Double kIz = 0.0;
+    }
+  }
+}

src/main/java/com/team841/Swerve23/Constants/SubsystemManifest.java

@@ -1,7 +1,11 @@
 package com.team841.Swerve23.Constants;
 
 import com.team841.Swerve23.Drive.Drivetrain;
+import com.team841.Swerve23.Superstructure.Arm;
+import com.team841.Swerve23.Superstructure.Intake;
 
 public final class SubsystemManifest {
   public static final Drivetrain drivetrain = Swerve.drivetrain;
+  public static final Intake intake = new Intake();
+  public static final Arm arm = new Arm();
 }

src/main/java/com/team841/Swerve23/Robot.java

@@ -1,7 +1,7 @@
 package com.team841.Swerve23;
 
 import com.team841.Swerve23.Constants.ConstantsIO;
-import com.team841.Swerve23.Constants.ConstantsIO.robotStates; 
+import com.team841.Swerve23.Constants.ConstantsIO.robotStates;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;

src/main/java/com/team841/Swerve23/Superstructure/Arm.java

@@ -1,22 +1,41 @@
 package com.team841.Swerve23.Superstructure;
 
 import com.revrobotics.CANSparkMax;
-import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.CANSparkMax.ControlType;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.SparkMaxPIDController;
 import com.team841.Swerve23.Constants.ConstantsIO;
-
+import com.team841.Swerve23.Constants.SC;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Arm extends SubsystemBase {
 
-  private final CANSparkMax armMotor = new CANSparkMax(ConstantsIO.CANID.kArm, MotorType.kBrushless);
+  private final CANSparkMax armMotor =
+      new CANSparkMax(ConstantsIO.CANID.kArm, MotorType.kBrushless);
 
   public SparkMaxPIDController armPID = armMotor.getPIDController();
 
-  public Arm() {}
+  public Arm() {
+    armMotor.restoreFactoryDefaults();
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
+    armMotor.setSmartCurrentLimit(SC.Arm.currentLimit);
+
+    armPID.setP(SC.Arm.pidGains.kP);
+    armPID.setI(SC.Arm.pidGains.kI);
+    armPID.setD(SC.Arm.pidGains.kD);
+    armPID.setIZone(SC.Arm.pidGains.kIz);
+
+    armPID.setFeedbackDevice(armMotor.getEncoder());
+  }
+
+  public Command armToPosition(double angle) {
+    return Commands.run(() -> armPID.setReference(angle, ControlType.kPosition))
+        .withInterruptBehavior(Command.InterruptionBehavior.kCancelSelf)
+        .handleInterrupt(() -> armMotor.set(0));
   }
+
+  @Override
+  public void periodic() {}
 }

src/main/java/com/team841/Swerve23/Superstructure/ArmFactory.java

@@ -0,0 +1,10 @@
+package com.team841.Swerve23.Superstructure;
+
+import com.team841.Swerve23.Constants.SubsystemManifest;
+
+public class ArmFactory {
+
+  private Arm s_arm = SubsystemManifest.arm;
+
+  ;
+}

src/main/java/com/team841/Swerve23/Superstructure/Intake.java

@@ -3,19 +3,47 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.team841.Swerve23.Constants.ConstantsIO;
-
+import com.team841.Swerve23.Constants.SC;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Intake extends SubsystemBase {
 
-  private final CANSparkMax intakeOneMotor = new CANSparkMax(ConstantsIO.CANID.kIntakeOne, MotorType.kBrushless);
-
-  private final CANSparkMax intakeTwoMotor = new CANSparkMax(ConstantsIO.CANID.kIntakeTwo, MotorType.kBrushless);
+  private final CANSparkMax intakeOneMotor =
+      new CANSparkMax(ConstantsIO.CANID.kIntakeOne, MotorType.kBrushless);
 
-  public Intake() {}
+  private final CANSparkMax intakeTwoMotor =
+      new CANSparkMax(ConstantsIO.CANID.kIntakeTwo, MotorType.kBrushless);
 
+  public Intake() {
 
-  @Override
-  public void periodic() {
+    intakeOneMotor.restoreFactoryDefaults();
+    intakeTwoMotor.restoreFactoryDefaults();
+
+    intakeOneMotor.setSmartCurrentLimit(SC.Intake.currentLimit);
+    intakeTwoMotor.setSmartCurrentLimit(SC.Intake.currentLimit);
+
+    intakeTwoMotor.follow(intakeOneMotor, true);
+
+    setIntakeBrakes(true);
+  }
+
+  public void setIntakeBrakes(boolean on) {
+    intakeOneMotor.setIdleMode(on ? CANSparkMax.IdleMode.kBrake : CANSparkMax.IdleMode.kCoast);
+    intakeTwoMotor.setIdleMode(on ? CANSparkMax.IdleMode.kBrake : CANSparkMax.IdleMode.kCoast);
   }
+
+  public void setIntakeMotor(Double speed) {
+    intakeOneMotor.set(speed);
+  }
+
+  public void intake() {
+    setIntakeMotor(1.0);
+  }
+
+  public void outTake() {
+    setIntakeMotor(-1.0);
+  }
+
+  @Override
+  public void periodic() {}
 }