From e290e263a5ba902312005cc192b306acd01c0e55 Mon Sep 17 00:00:00 2001
From: ProgrammingSR <programming@saintsrobotics.com>
Date: Fri, 16 Feb 2024 17:23:41 -0800
Subject: [PATCH] chore: sent stuff to constants

---
 src/main/java/frc/robot/Constants.java        | 33 +++++++++++++++++++
 src/main/java/frc/robot/IntakeConstants.java  |  6 ----
 src/main/java/frc/robot/RobotContainer.java   | 20 ++++++++---
 .../frc/robot/subsystems/IntakeSubsystem.java | 30 +++++++----------
 .../robot/subsystems/ShooterSubsystem.java    |  1 +
 5 files changed, 61 insertions(+), 29 deletions(-)
 create mode 100644 src/main/java/frc/robot/Constants.java
 delete mode 100644 src/main/java/frc/robot/IntakeConstants.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
new file mode 100644
index 0000000..e54b19f
--- /dev/null
+++ b/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,33 @@
+// 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;
+
+public final class Constants {
+
+
+//Intake PID and Encoder Constants
+    public static class IntakeConstants {
+        public static final double kIntakeDroppedAngle = 9.0;
+        public static final double kIntakeRaisedAngle = 9.0;
+        public static final int kIntakeMotorID = 0;
+        public static final int kArmMotorID = 0;
+        public static final double kIntakeP = 0;
+        public static final double kIntakeI = 0;
+        public static final double kIntakeD = 0;
+        public static final double kArmP = 0;
+        public static final double kArmI = 0;
+        public static final double kArmD = 0;
+        public static final int kArmEncoderCh = 0;
+    }
+
+    //Shooter subsystem speed constants
+    public static class ShooterConstants {
+        public static final double kSpinSpeedTrue = 0.75;
+        public static final double kSpinSpeedFalse = 0;
+
+
+    }
+}
+
diff --git a/src/main/java/frc/robot/IntakeConstants.java b/src/main/java/frc/robot/IntakeConstants.java
deleted file mode 100644
index 8fc71b7..0000000
--- a/src/main/java/frc/robot/IntakeConstants.java
+++ /dev/null
@@ -1,6 +0,0 @@
-package frc.robot;
-
-public class IntakeConstants {
-    public static final double IntakeDroppedAngle = 9.0;
-    public static final double IntakeRaisedAngle = 9.0;
-}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 18ca027..44b12f6 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -10,10 +10,13 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.Constants.ShooterConstants;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
 public class RobotContainer {
+  //The robots subsystems are defined here
   public final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
   public final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
   private boolean IntakeDropped = false;
@@ -22,9 +25,13 @@ public class RobotContainer {
   private XboxController controller = new XboxController(0);
   public RobotContainer() {
     configureBindings();
-    m_IntakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),m_IntakeSubsystem));
+    m_IntakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.kIntakeDroppedAngle : IntakeConstants.kIntakeRaisedAngle),m_IntakeSubsystem));
   }
 
+
+  /**
+   * checks for intake button on controller every tick
+   */
   public void periodic(){
     if(controller.getAButton()){
       lastAButton = true;
@@ -36,13 +43,16 @@ public void periodic(){
     }
   }
 
+  /**
+   * Use this method to define your button->command mappings.
+   */
   private void configureBindings() {
     new JoystickButton(controller, Button.kB.value)
-      .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(0.75), m_ShooterSubsystem))
-      .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem));
+      .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedTrue), m_ShooterSubsystem))
+      .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedFalse), m_ShooterSubsystem));
     new JoystickButton(controller, Button.kA.value)
-      .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-0.75), m_ShooterSubsystem))
-      .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem));
+      .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-ShooterConstants.kSpinSpeedTrue), m_ShooterSubsystem))
+      .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedFalse), m_ShooterSubsystem));
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 6408f88..794a666 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -11,21 +11,17 @@
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeConstants;
 
+//creates new motors and pid controllers lmao
 public class IntakeSubsystem extends SubsystemBase {
-  //TODO: add constants
-  CANSparkFlex intakeMotor = new CANSparkFlex(0, MotorType.kBrushless);
-  CANSparkFlex armMotor = new CANSparkFlex(0, MotorType.kBrushless);
+  CANSparkFlex intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
+  CANSparkFlex armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless);
   
-
-  PIDController intakeVeloPID = new PIDController(0,0,0);
-  PIDController armPID = new PIDController(0,0,0);
-  DutyCycleEncoder armEncoder = new DutyCycleEncoder(0);
+  PIDController intakeVeloPID = new PIDController(IntakeConstants.kIntakeP,IntakeConstants.kIntakeI,IntakeConstants.kIntakeD);
+  PIDController armPID = new PIDController(IntakeConstants.kArmP,IntakeConstants.kArmI,IntakeConstants.kArmD);
+  DutyCycleEncoder armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh);
   
-
-
-
-
   /** Creates a new intake. */
   public IntakeSubsystem() {
     
@@ -41,19 +37,17 @@ public void load(double speed, double angle){
     tiltToAngle(angle);
 
   }
-    /**
-     * 
-  *@param angle in radians
-  *@everyone join vc we are playing gartic phone
-  */
 
+  /**
+   * 
+   * @param angle motor to apply to intake
+   * 
+   */
   public void tiltToAngle(double angle) {
     double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle);
     armMotor.set(motorPower);
   }
   
-
-
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 8ff662b..1ea808c 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -8,6 +8,7 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
 public class ShooterSubsystem extends SubsystemBase {
   /** Creates a new ShooterSubsystem. */
   CANSparkFlex m_bottom = new CANSparkFlex(20, MotorType.kBrushless);