From 8e643c1eef159f785b3d1cf22e3688df65679e13 Mon Sep 17 00:00:00 2001
From: mayabartels <mayabartels01@gmail.com>
Date: Fri, 10 Jan 2020 21:29:45 -0700
Subject: [PATCH] Changed extension for drive subsystem

Added some new parts to motion profiling, not complete shell for code yet
---
 .../java/frc4388/robot/subsystems/Drive.java  | 25 ++++++++++++++++---
 1 file changed, 22 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 4a369d9..66729cd 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -13,14 +13,18 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.sensors.PigeonIMU;
 
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
 
 import frc4388.robot.Constants.DriveConstants;
 
 /**
  * Add your docs here.
  */
-public class Drive extends SubsystemBase {
+public class Drive extends ProfiledPIDSubsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
@@ -35,7 +39,10 @@ public class Drive extends SubsystemBase {
   /**
    * Add your docs here.
    */
-  public Drive(){
+  public Drive() {
+    /*  */
+    super(new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(0, 0)), 0);
+
     /* factory default values */
     m_leftFrontMotor.configFactoryDefault();
     m_rightFrontMotor.configFactoryDefault();
@@ -78,7 +85,7 @@ public class Drive extends SubsystemBase {
     m_pigeon.getYawPitchRoll(ypr);
     return ypr[1];
   }
-  
+
   public double getGyroRoll() {
     double[] ypr = new double[3];
     m_pigeon.getYawPitchRoll(ypr);
@@ -89,4 +96,16 @@ public class Drive extends SubsystemBase {
     m_pigeon.setYaw(0);
     m_pigeon.setAccumZAngle(0);
   }
+
+  @Override
+  protected void useOutput(double output, TrapezoidProfile.State setpoint) {
+    // TODO Auto-generated method stub
+    
+  }
+
+  @Override
+  protected double getMeasurement() {
+    // TODO Auto-generated method stub
+    return 0;
+  }
 }