diff --git a/.idea/gradle.xml b/.idea/gradle.xml
index ba1ec5c..611e7c8 100644
--- a/.idea/gradle.xml
+++ b/.idea/gradle.xml
@@ -1,5 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
+  <component name="GradleMigrationSettings" migrationVersion="1" />
   <component name="GradleSettings">
     <option name="linkedExternalProjectsSettings">
       <GradleProjectSettings>
diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
index 6febd6e..899bb36 100644
--- a/.idea/jarRepositories.xml
+++ b/.idea/jarRepositories.xml
@@ -26,5 +26,15 @@
       <option name="name" value="WPILocal" />
       <option name="url" value="file:$USER_HOME$/wpilib/2021/maven" />
     </remote-repository>
+    <remote-repository>
+      <option name="id" value="WPILocal" />
+      <option name="name" value="WPILocal" />
+      <option name="url" value="file:/$USER_HOME$/Public/wpilib/2021/maven" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="WPILocal" />
+      <option name="name" value="WPILocal" />
+      <option name="url" value="file:/$PROJECT_DIR$/../../../../../../Public/wpilib/2021/maven" />
+    </remote-repository>
   </component>
 </project>
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
index 3e79c5f..5821b2f 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -4,5 +4,5 @@
   <component name="FrameworkDetectionExcludesConfiguration">
     <file type="web" url="file://$PROJECT_DIR$" />
   </component>
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_11" project-jdk-name="11" project-jdk-type="JavaSDK" />
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="11" project-jdk-type="JavaSDK" />
 </project>
\ No newline at end of file
diff --git a/.idea/runConfigurations.xml b/.idea/runConfigurations.xml
new file mode 100644
index 0000000..797acea
--- /dev/null
+++ b/.idea/runConfigurations.xml
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="RunConfigurationProducerService">
+    <option name="ignoredProducers">
+      <set>
+        <option value="com.android.tools.idea.compose.preview.runconfiguration.ComposePreviewRunConfigurationProducer" />
+      </set>
+    </option>
+  </component>
+</project>
\ No newline at end of file
diff --git a/.idea/workspace.xml b/.idea/workspace.xml
index 682050b..f9ac5cc 100644
--- a/.idea/workspace.xml
+++ b/.idea/workspace.xml
@@ -5,8 +5,11 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="34a8bc03-e56d-48a3-9c6f-5fe36900ebe8" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/src/main/java/com/github/mittyrobotics/TrapezoidProfile.java" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/jarRepositories.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/jarRepositories.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/modules.xml" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/tko-romi.iml" beforeDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/src/main/java/com/github/mittyrobotics/RomiGyro.java" beforeDir="false" afterPath="$PROJECT_DIR$/src/main/java/com/github/mittyrobotics/RomiGyro.java" afterDir="false" />
     </list>
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -26,16 +29,41 @@
         </task>
         <projects_view>
           <tree_state>
-            <expand />
+            <expand>
+              <path>
+                <item name="" type="6a2764b6:ExternalProjectsStructure$RootNode" />
+                <item name="tko-romi" type="f1a62948:ProjectNode" />
+              </path>
+            </expand>
             <select />
           </tree_state>
         </projects_view>
       </state>
     </system>
   </component>
+  <component name="FileTemplateManagerImpl">
+    <option name="RECENT_TEMPLATES">
+      <list>
+        <option value="Class" />
+      </list>
+    </option>
+  </component>
   <component name="Git.Settings">
     <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
   </component>
+  <component name="GitSEFilterConfiguration">
+    <file-type-list>
+      <filtered-out-file-type name="LOCAL_BRANCH" />
+      <filtered-out-file-type name="REMOTE_BRANCH" />
+      <filtered-out-file-type name="TAG" />
+      <filtered-out-file-type name="COMMIT_BY_MESSAGE" />
+    </file-type-list>
+  </component>
+  <component name="HighlightingSettingsPerFile">
+    <setting file="file://$PROJECT_DIR$/src/main/java/com/github/mittyrobotics/Constants.java" root0="FORCE_HIGHLIGHTING" />
+    <setting file="file://$PROJECT_DIR$/src/main/java/com/github/mittyrobotics/RomiRobot2.java" root0="FORCE_HIGHLIGHTING" />
+    <setting file="file://$PROJECT_DIR$/src/main/java/com/github/mittyrobotics/Robot.java" root0="FORCE_HIGHLIGHTING" />
+  </component>
   <component name="ProjectId" id="1popUgtFRoEscWY2dSrfhCdDagz" />
   <component name="ProjectLevelVcsManager" settingsEditedManually="true" />
   <component name="ProjectViewState">
@@ -51,8 +79,14 @@
     <property name="project.structure.last.edited" value="Project" />
     <property name="project.structure.proportion" value="0.0" />
     <property name="project.structure.side.proportion" value="0.0" />
+    <property name="run.code.analysis.last.selected.profile" value="pProject Default" />
     <property name="settings.editor.selected.configurable" value="preferences.lookFeel" />
   </component>
+  <component name="RecentsManager">
+    <key name="CopyClassDialog.RECENTS_KEY">
+      <recent name="com.github.mittyrobotics" />
+    </key>
+  </component>
   <component name="RunManager">
     <configuration default="true" type="ArquillianJUnit" factoryName="" nameIsGenerated="true">
       <option name="arquillianRunConfiguration">
@@ -81,4 +115,16 @@
   <component name="TypeScriptGeneratedFilesManager">
     <option name="version" value="3" />
   </component>
+  <component name="Vcs.Log.Tabs.Properties">
+    <option name="TAB_STATES">
+      <map>
+        <entry key="MAIN">
+          <value>
+            <State />
+          </value>
+        </entry>
+      </map>
+    </option>
+    <option name="oldMeFiltersMigrated" value="true" />
+  </component>
 </project>
\ No newline at end of file
diff --git a/src/main/java/com/github/mittyrobotics/DriveTrain.java b/src/main/java/com/github/mittyrobotics/DriveTrain.java
new file mode 100644
index 0000000..84c1334
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/DriveTrain.java
@@ -0,0 +1,32 @@
+package com.github.mittyrobotics;
+
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveTrain extends SubsystemBase {
+
+    public static DriveTrain instance = null;
+    public static DriveTrain getInstance(){
+        if(instance==null){
+            instance = new DriveTrain();
+        }
+        return instance;
+    }
+    Spark sparkLeft, sparkRight;
+
+    public void initHardware(){
+        //initialize stuff here
+        sparkLeft = new Spark(Constants.LEFT_MOTOR_ID);
+        sparkRight = new Spark(Constants.RIGHT_MOTOR_ID);
+    }
+    public void setMotors(double val){
+        sparkLeft.set(val);
+        sparkRight.set(val);
+    }
+    public void setSparkLeft(double val){
+        sparkLeft.set(val);
+    }
+    public void setSparkRight(double val){
+        sparkRight.set(val);
+    }
+}
diff --git a/src/main/java/com/github/mittyrobotics/OI.java b/src/main/java/com/github/mittyrobotics/OI.java
new file mode 100644
index 0000000..88c51f1
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/OI.java
@@ -0,0 +1,20 @@
+package com.github.mittyrobotics;
+import edu.wpi.first.wpilibj.XboxController;
+public class OI {
+    private static OI instance;
+    private XboxController xboxController;
+
+    public static OI getInstance(){
+        if(instance == null){
+            instance = new OI();
+        }
+        return instance;
+    }
+
+    public XboxController getXboxController(){
+        if(xboxController == null){
+            xboxController = new XboxController(0);
+        }
+        return xboxController;
+    }
+}
diff --git a/src/main/java/com/github/mittyrobotics/Robot.java b/src/main/java/com/github/mittyrobotics/Robot.java
index 63ba14c..4941afc 100644
--- a/src/main/java/com/github/mittyrobotics/Robot.java
+++ b/src/main/java/com/github/mittyrobotics/Robot.java
@@ -1,29 +1,62 @@
 package com.github.mittyrobotics;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
-import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.controller.PIDController;
-
-
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import com.github.mittyrobotics.util.Compressor;
 //Java automatically runs this class, and calls the various functions.
 /*
  *  YOUR WIFI MUST BE CONNECTED TO ROMI FOR THIS TO WORK
  */
 public class Robot extends TimedRobot {
 
+    Spark SparkLeft, SparkRight;
+    DigitalInput digitalInput1;
+    DigitalInput input2;
+    DigitalInput input3;
+    DigitalInput input4;
+    XboxController controller;
+    RomiGyro gyro;
+    boolean clicked;
+    boolean released;
     //Runs when the robot is first started up and should be used for any initialization code
     /*
+
      *  INITIALIZE CLASSES HERE
      */
-
-
+    TrapezoidProfile.State start; //start variable from type trapezoidprofile.state
+    TrapezoidProfile.State end; //end var
+    TrapezoidProfile.Constraints constraints; //constraints var from .constraints
+    TrapezoidProfile profile; //profile
+    int counter;
+    double kp, ki, kd;
+    PIDController control  = new PIDController(kp, ki, kd);
+    Encoder encoder;
+    DoubleSolenoid s;
+    boolean deadZone;
+    DoubleSolenoid p;
 
     @Override
     public void robotInit() {
-
-
+        SparkLeft = new Spark(Constants.LEFT_MOTOR_ID;
+        SparkRight = new Spark(Constants.RIGHT_MOTOR_ID);
+        SparkLeft.setInverted(true);
+//        digitalInput1 = new DigitalInput(0);
+//        input2 = new DigitalInput(1);
+//        input3 = new DigitalInput(2);
+//        input4 = new DigitalInput(3);
+        controller =  new XboxController(0);
+//        gyro = new RomiGyro();
+//        clicked = controller.getAButtonPressed();
+//        released = controller.getAButtonReleased();
+//        start = new TrapezoidProfile.State(0, 0);
+//        end = new TrapezoidProfile.State(1.0, 0);
+        constraints = new TrapezoidProfile.Constraints(0.2, 0.2);
+//        profile = new TrapezoidProfile(constraints, end, start);
+//        encoder = new Encoder(Constants.ENCODER_IDS[0], Constants.ENCODER_IDS[1]);
+//        s = new DoubleSolenoid(1,2);
+        deadZone = true;
+        DriveTrain.getInstance().initHardware();
     }
 
     //Runs periodically during teleoperated mode
@@ -33,6 +66,67 @@ public void robotInit() {
     @Override
     public void teleopPeriodic() {
 
+
+        if (clicked){
+            SparkLeft.set(OI.getInstance().getXboxController().getY(GenericHID.Hand.kLeft);
+            SparkRight.set(OI.getInstance().getXboxController().getX(GenericHID.Hand.kLeft);
+        }
+        TrapezoidProfile.State profileOutput = profile.calculate(0.02*counter);
+        control.setSetpoint(profileOutput.position);
+        double output = control.calculate(encoder.getDistance());
+        SparkLeft.set(output);
+        SparkRight.set(output);
+        counter++;
+
+        if(OI.getInstance().getXboxController().getAButton()){
+            s.set(DoubleSolenoid.Value.kForward);
+        }
+        else if(OI.getInstance().getXboxController().getBButton()){
+            s.set(DoubleSolenoid.Value.kReverse);
+        }
+        else if(OI.getInstance().getXboxController().getXButton()){
+            p.set(DoubleSolenoid.Value.kForward);
+        }
+        else if(OI.getInstance().getXboxController().getYButton()){
+            p.set(DoubleSolenoid.Value.kReverse);
+        }
+
+        if(clicked==false){
+            clicked=false;
+        }
+        if(digitalInput1.get()) {
+            SparkLeft.set(0);
+            SparkRight.set(1);
+        }
+        else if(input2.get()){
+            SparkLeft.set(1);
+            SparkRight.set(0);
+        }
+        else if(input3.get()){
+            SparkLeft.set(1);
+            SparkRight.set(1);
+        }
+        else if(input4.get()) {
+            SparkLeft.set(-1);
+            SparkRight.set(-1);
+        }
+        if(controller.getAButton()){
+            while(gyro.getAngleZ()<=45){
+                SparkLeft.set(-0.5);
+                SparkRight.set(0.5);
+            }
+        }
+
+        if(clicked){
+            SparkLeft.set(controller.getY(GenericHID.Hand.kLeft));
+            SparkRight.set(controller.getY(GenericHID.Hand.kRight));
+        }
+        if(controller.getAButton() && gyro.getAngleZ() <= 45){
+            SparkLeft.set(-0.5);
+            SparkRight.set(0.5);
+        }
+
+        */
     }
 
     //Runs when antonomous mode (robot runs on its own) first activated via the desktop application
@@ -59,7 +153,7 @@ public void robotPeriodic() {
 
     }
 
-    //Runs periodically during autonomous mode
+    //Runs periodically during auto6nomous mode
     @Override
     public void autonomousPeriodic() {
 
diff --git a/src/main/java/com/github/mittyrobotics/Robot1.java b/src/main/java/com/github/mittyrobotics/Robot1.java
new file mode 100644
index 0000000..8be4242
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/Robot1.java
@@ -0,0 +1,80 @@
+package com.github.mittyrobotics;
+
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+//Java automatically runs this class, and calls the various functions.
+/*
+ *  YOUR WIFI MUST BE CONNECTED TO ROMI FOR THIS TO WORK
+ */
+public class Robot1 extends TimedRobot {
+
+    Spark SparkLeft, SparkRight;
+    double deadZone = 0.2;
+    boolean inDeadZone;
+    XboxController controller;
+    @Override
+    public void robotInit() {
+        DriveTrain.getInstance().initHardware();
+        SparkLeft = new Spark(Constants.LEFT_MOTOR_ID);
+        SparkRight = new Spark(Constants.RIGHT_MOTOR_ID);
+
+        //Runs periodically during teleoperated mode
+        /*
+         *  WRITE YOUR DRIVE CODE HERE
+         */
+        controller =  new XboxController(0);
+
+    }
+    @Override
+    public void teleopPeriodic() {
+        if(controller.getY(GenericHID.Hand.kLeft) > -deadZone && OI.getInstance().getXboxController().getY(GenericHID.Hand.kLeft) > deadZone){
+             DriveTrain.getInstance().setSparkLeft(OI.getInstance().getXboxController().getY(GenericHID.Hand.kLeft));
+             SparkLeft.set(controller.getY(GenericHID.Hand.kLeft));
+             SparkRight.set(controller.getY(GenericHID.Hand.kRight));
+            inDeadZone = false;
+        }
+        else if() {
+            inDeadZone = true;
+        }
+
+
+    }
+
+    //Runs when antonomous mode (robot runs on its own) first activated via the desktop application
+    @Override
+    public void autonomousInit() {
+
+    }
+
+    //Runs when teleoperated mode (robot controlled by driver) is first activated
+    @Override
+    public void teleopInit() {
+
+    }
+
+    //Runs when test mode is activated
+    @Override
+    public void testInit() {
+
+    }
+
+    //Runs whenever the robot is on, periodically: should be used for command schedulers
+    @Override
+    public void robotPeriodic() {
+
+    }
+
+    //Runs periodically during autonomous mode
+    @Override
+    public void autonomousPeriodic() {
+
+    }
+
+    //Runs periodically during test mode
+    @Override
+    public void testPeriodic() {
+
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/com/github/mittyrobotics/RomiRobot2.java b/src/main/java/com/github/mittyrobotics/RomiRobot2.java
new file mode 100644
index 0000000..85940dc
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/RomiRobot2.java
@@ -0,0 +1,148 @@
+package com.github.mittyrobotics;
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+public class RomiRobot2 extends TimedRobot {
+    Spark SparkLeft, SparkRight;
+    TrapezoidProfile.State start; //start variable from type trapezoidprofile.state
+    TrapezoidProfile.State end; //end var
+    TrapezoidProfile.Constraints constraints; //constraints var from .constraints
+    TrapezoidProfile profile; //profile
+    RomiGyro gyro;
+    int counter;
+    double kp, ki, kd;
+    PIDController control  = new PIDController(kp, ki, kd);
+    Encoder encoder;
+
+    public void robotInit() {
+        SparkLeft = new Spark(Constants.LEFT_MOTOR_ID);
+        SparkRight = new Spark(Constants.RIGHT_MOTOR_ID);
+        SparkLeft.setInverted(true);
+        gyro = new RomiGyro();
+        start = new TrapezoidProfile.State(0, 0);
+        end = new TrapezoidProfile.State(1.0, 0);
+        constraints = new TrapezoidProfile.Constraints(0.2, 0.2);
+        profile = new TrapezoidProfile(constraints, end, start);
+        encoder = new Encoder(Constants.ENCODER_IDS[0], Constants.ENCODER_IDS[1]);
+    }
+    public void teleopPeriodic() {
+
+        //straight 12 inches
+
+
+    }
+    public void challenge(){
+        //no whiles
+        //use variable so ++ when done with step6
+        int currDistance = 0;
+        currDistance += 12;
+        moveForward(12, currDistance);
+        changeDirection(90, true);
+        currDistance += 6;
+        moveForward(6,currDistance);
+        changeDirection(90,false);
+        currDistance += 12;
+        moveForward(12,currDistance);
+        changeDirection(120,true);
+        currDistance += 18;
+        moveForward(18,currDistance);
+        changeDirection(130,false);
+        currDistance += 36;
+        moveForward(36, currDistance);
+        changeDirection(90,true);
+        currDistance += 10;
+        moveForward(10, currDistance);
+        changeDirection(60,false);
+        currDistance += 36;
+        moveForward(36, currDistance);
+    }
+    public void challenge2(){
+        int currDistance = 0;
+        currDistance += 12;
+        moveForward(12, currDistance);
+        changeDirection(90, true);
+        currDistance += 6;
+        moveForward(6,currDistance);
+        changeDirection(90,false);
+        currDistance += 24;
+        moveForward(24,currDistance);
+        changeDirection(90,true);
+        currDistance += 15;
+        moveForward(15,currDistance);
+        changeDirection(80,false);
+        currDistance += 6;
+        moveForward(6, currDistance);
+        changeDirection(100,true);
+        currDistance += 24;
+        moveForward(24, currDistance);
+        changeDirection(110,false);
+        currDistance += 36;
+        moveForward(36, currDistance);
+        changeDirection(70,false);
+        currDistance += 30;
+        moveForward(30, currDistance);
+    }
+    public void moveForward(int d_inch, int currDistance){
+        while(encoder.getDistance()<currDistance){
+            TrapezoidProfile.State profileOutput = profile.calculate(0.2*counter);
+            control.setSetpoint(d_inch*Constants.TICKS_PER_INCH);
+            double output = control.calculate(encoder.getDistance());
+            SparkLeft.set(output);
+            SparkRight.set(output);
+            counter++;
+        }
+    }
+    public void changeDirection(int a_degrees, boolean right){
+        if(right){
+            //gyro check if it is greater than a_degrees
+            while(gyro.getAngleZ()<a_degrees){
+                SparkLeft.set(1);
+                SparkRight.set(-1);
+            }
+        }
+        else{
+            //gyro check if it is greater than a_degrees
+            while(gyro.getAngleZ()<a_degrees){
+                SparkLeft.set(-1);
+                SparkRight.set(1);
+            }
+        }
+    }
+    public void autonomousInit() {
+
+
+
+    }
+
+    //Runs when teleoperated mode (robot controlled by driver) is first activated
+    @Override
+    public void teleopInit() {
+
+    }
+
+    //Runs when test mode is activated
+    @Override
+    public void testInit() {
+
+    }
+
+    //Runs whenever the robot is on, periodically: should be used for command schedulers
+    @Override
+    public void robotPeriodic() {
+
+    }
+
+    //Runs periodically during autonomous mode
+    @Override
+    public void autonomousPeriodic() {
+
+    }
+
+    //Runs periodically during test mode
+    @Override
+    public void testPeriodic() {
+
+    }
+}
+
diff --git a/src/main/java/com/github/mittyrobotics/TankDrive.java b/src/main/java/com/github/mittyrobotics/TankDrive.java
new file mode 100644
index 0000000..ef20851
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/TankDrive.java
@@ -0,0 +1,85 @@
+package com.github.mittyrobotics;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import com.github.mittyrobotics.OI;
+
+public class TankDrive extends CommandBase {
+
+
+
+    Spark SparkLeft, SparkRight;
+    DigitalInput input0;-
+    DigitalInput input1;
+    DigitalInput input2;
+    DigitalInput input3;
+    XboxController controller;
+    RomiGyro gyro;
+
+    public TankDrive(){
+        super();
+        setName("Tank Drive");
+
+    }
+    @Override
+    public void initialize() {
+        super.initialize();
+        SparkLeft = new Spark(Constants.LEFT_MOTOR_ID);
+        SparkRight = new Spark(Constants.RIGHT_MOTOR_ID);
+        SparkLeft.setInverted(true);
+        input0 = new DigitalInput(0);
+        input1 = new DigitalInput(1);
+        input2 = new DigitalInput(2);
+        input3 = new DigitalInput(3);
+        controller = new XboxController(0);
+        gyro = new RomiGyro();
+    }
+
+    @Override
+    public void execute() {
+        super.execute();
+        DriveTrain.getInstance().setSparkLeft(OI.getInstance().getXboxController().getY(GenericHID.Hand.kLeft));
+        DriveTrain.getInstance().setSparkRight(OI.getInstance().getXboxController().getY(GenericHID.Hand.kRight));
+        if(controller.getAButtonPressed()) {
+            if (input0.get()) {
+               //move left
+                SparkLeft.set(0);
+                SparkRight.set(1);
+            }
+            if (input1.get()) {
+                //move right
+                SparkLeft.set(1);
+                SparkRight.set(0);
+            }
+            if (input2.get()) {
+                //move forward
+                SparkLeft.set(1);
+                SparkRight.set(1);
+            }
+            if (input3.get()) {
+                SparkLeft.set(-1);
+                SparkRight.set(-1);
+            }
+        }
+        else{
+            while(gyro.getAngleX()<45){
+                SparkLeft.set(0.75);
+                SparkRight.set(0);
+            }
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        DriveTrain.getInstance().setSparkLeft(0);
+        DriveTrain.getInstance().setSparkRight(0);
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return OI.getInstance().getXboxController().getAButtonPressed();
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/com/github/mittyrobotics/TrapezoidProfile.java b/src/main/java/com/github/mittyrobotics/TrapezoidProfile.java
new file mode 100644
index 0000000..1aeb660
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/TrapezoidProfile.java
@@ -0,0 +1,45 @@
+package com.github.mittyrobotics;
+
+
+public class TrapezoidProfile {
+    private TrapezoidProfile.Constraints constraints;
+    private TrapezoidProfile.State desired;
+    private TrapezoidProfile.State current;
+    private double profUptime = 0;
+    private boolean complete = false;
+
+    public TrapezoidProfile(TrapezoidProfile.Constraints constraints, TrapezoidProfile.State desired, TrapezoidProfile.State initial){
+        this.constraints=constraints;
+        this.desired=desired;
+        this.current=initial;
+
+    }
+
+    public void
+
+//
+//    public TrapezoidProfile calculate(double seconds){
+//        return TrapezoidProfile.current;
+//    }
+
+
+    public class Constraints{
+        private double maxV;
+        private double maxA;
+
+        public Constraints(double maxV, double maxA){
+            this.maxV=maxV;
+            this.maxA=maxA;
+        }
+
+    }
+
+    public class State{
+        private double pos;
+        private double vel;
+        public State(double pos, double vel){
+            this.pos=pos;
+            this.vel=vel;
+        }
+    }
+}