Added motion magic stuff

Looked at the CTRE website and went to github. Added the stuff we saw on there for motion magic. Needs to be tested to see if it's accurate.

src/main/java/frc4388/robot/Constants.java

@@ -24,6 +24,17 @@ public final class Constants {
 	    public static final int DRIVE_LEFT_BACK_CAN_ID = 3;
         public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
         public static final int PIGEON_ID = 6;
+
+        /* PID Constants */
+        public static final int SLOT_IDX = 0;
+
+        public static final int PID_LOOP_IDX = 0;
+
+        public static final int TIMEOUT_MS = 30;
+
+        static final Gains kGains = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);    
+        
+        public static final double ENCODER_TICKS_PER_REV = 2048;
     }
 
     public static final class LEDConstants {

src/main/java/frc4388/robot/Gains.java

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+/**
+ * Add your docs here.
+ */
+public class Gains {
+    public final double kP;
+    public final double kI;
+    public final double kD;
+    public final double kF;
+    public final int kIzone;
+    public final double kPeakOutput;
+
+    public Gains(double _kP, double _kI, double _kD, double _kF, int _kIzone, double _kPeakOutput)
+    {
+        kP = _kP;
+        kI = _kI;
+        kD = _kD;
+        kF = _kF;
+        kIzone = _kIzone;
+        kPeakOutput = _kPeakOutput;
+    }
+}

src/main/java/frc4388/robot/RobotContainer.java

@@ -69,6 +69,9 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
             .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+
+        new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
+            .whenPressed(() -> m_robotDrive.goToTargetPos());
     }
     
     /**

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -7,6 +7,8 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
@@ -22,11 +24,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
 
 import frc4388.robot.Constants.DriveConstants;
+import frc4388.utility.controller.XboxController;
 
 /**
  * Add your docs here.
  */
-public class Drive extends ProfiledPIDSubsystem {
+public class Drive extends SubsystemBase {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
@@ -42,9 +45,6 @@ public class Drive extends ProfiledPIDSubsystem {
    * Add your docs here.
    */
   public Drive() {
-    /*  */
-    super(new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(0, 0)), 0);
-
     /* factory default values */
     m_leftFrontMotor.configFactoryDefault();
     m_rightFrontMotor.configFactoryDefault();
@@ -65,6 +65,10 @@ public class Drive extends ProfiledPIDSubsystem {
     m_rightFrontMotor.setInverted(false);
     m_leftBackMotor.setInverted(InvertType.FollowMaster);
     m_rightBackMotor.setInverted(InvertType.FollowMaster);
+
+    /* Motor Encoders */
+    m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS);
+    m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS);
   }
 
   @Override
@@ -121,15 +125,12 @@ public class Drive extends ProfiledPIDSubsystem {
     m_pigeon.setAccumZAngle(0);
   }
 
-  @Override
-  protected void useOutput(double output, TrapezoidProfile.State setpoint) {
-    // TODO Auto-generated method stub
-    
-  }
+  // Motion Magic Testing
+  public void goToTargetPos()
+  {
+    double targetPos = XboxController.RIGHT_Y_AXIS * DriveConstants.ENCODER_TICKS_PER_REV * 10.0;
 
-  @Override
-  protected double getMeasurement() {
-    // TODO Auto-generated method stub
-    return 0;
+    m_leftFrontMotor.set(ControlMode.MotionMagic, targetPos);
+    m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos);
   }
 }