1 files changed, 33 insertions, 18 deletions
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c4c2329..efff2b8 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -3,7 +3,6 @@ package frc.robot;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
-import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
@@ -16,6 +15,8 @@ import frc.robot.util.IMUGyro;
 public class Constants
 {
     public static final GenericHID controller = new GenericHID(0);
+    public static final GenericHID controller2 = new GenericHID(1);
+
     public static final double InputSmoothing = 0.1;
 
     public static final double DriveMaxSpeed = 5; // m/s
@@ -45,36 +46,50 @@ public class Constants
     public static final Encoder encoderR = new Encoder(4, 5);
 
     // for Intake
-    public static final CANSparkMax intakeT = null;//new CANSparkMax(41, MotorType.kBrushless);
-    public static final CANSparkMax intakeD = null;//new CANSparkMax(42, MotorType.kBrushless);
-    // for Shooter
-    public static final CANSparkMax shooterT = null;//new CANSparkMax(61, MotorType.kBrushless);
-    public static final CANSparkMax shooterD = null;//new CANSparkMax(62, MotorType.kBrushless);
+    public static final int intakeT_ID = 32;
+    public static final int intakeD_ID = 31;
+
+    // for Shooter    CommandScheduler.getInstance().run();
 
-    // for Arm]
+    public static final int shooterT_ID = 21;
+    public static final int shooterD_ID = 22;
+
+    // for Arm
     public static final double ArmGearReduction = 60;
 
-    public static final ArmFeedforward arm_feedforward = new ArmFeedforward(0, 0, 0);
+    public static final double ArmEpsilon = 3 * Math.PI/180;
+
+    public static final double ArmkS = 0.4;
+    public static final double ArmkG = 0.495;
+    public static final double ArmkV = 0;
+
+    public static final double arm_initial_position = -43.0 * Math.PI / 180.0;
     public static final TrapezoidProfile.Constraints arm_constraints = 
-        new TrapezoidProfile.Constraints(0, 0);
+        new TrapezoidProfile.Constraints(7, 5);
 
-    public static final double ArmkP = 0.0;
+    public static final double ArmkP = 11.5;
     public static final double ArmkI = 0.0;
-    public static final double ArmkD = 0.0;
+    public static final double ArmkD = 0.4;
 
-    public static final CANSparkMax armL = null;//new CANSparkMax(51, MotorType.kBrushless);
-    public static final CANSparkMax armR = null;//new CANSparkMax(52, MotorType.kBrushless);
+    public static final int armL_ID = 41;
+    public static final int armR_ID = 42;
 
     // for Chain 
-    public static final double ChainEncoderScale = 2600;
+    public static final double ChainEncoderScale = 3000;
+
+    public static final double ChainEpsilon = 0.05;
 
     public static final TrapezoidProfile.Constraints chain_constraints = 
-        new TrapezoidProfile.Constraints(0.5, 0.5);
+        new TrapezoidProfile.Constraints(5, 5);
 
     public static final double ChainkP = 45.0;
     public static final double ChainkI = 0.0;
     public static final double ChainkD = 0.0;
-    
-    public static final CANSparkMax chain = new CANSparkMax(50, MotorType.kBrushed);
-    public static final Encoder chain_encoder = new Encoder(8, 9);
+
+    public static final int chain_ID = 50;
+    public static final int[] chain_encoder_PINS = {8, 9};
+
+    // Hooks
+    public static final int hookL_ID = 62;
+    public static final int hookR_ID = 61;
 }