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 @@
+
\ No newline at end of file
diff --git a/.idea/workspace.xml b/.idea/workspace.xml
index 682050b..bf8941f 100644
--- a/.idea/workspace.xml
+++ b/.idea/workspace.xml
@@ -6,7 +6,8 @@
-
+
+
@@ -26,16 +27,53 @@
-
+
+
+
+
+
@@ -81,4 +150,34 @@
+
+
+
+<<<<<<< Updated upstream
+
+
+=======
+
+
+
+
+ jar://$USER_HOME$/.gradle/caches/modules-2/files-2.1/edu.wpi.first.wpilibj/wpilibj-java/2021.2.2/f19d9fb4592ce5c22c9061880b51089ec0b06bde/wpilibj-java-2021.2.2-sources.jar!/edu/wpi/first/wpilibj/IterativeRobotBase.java
+ 60
+
+
+
+
+
+
+
+>>>>>>> Stashed changes
+
\ No newline at end of file
diff --git a/src/main/java/com/github/mittyrobotics/DriveTrainSubsystem.java b/src/main/java/com/github/mittyrobotics/DriveTrainSubsystem.java
new file mode 100644
index 0000000..f0b0e55
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/DriveTrainSubsystem.java
@@ -0,0 +1,4 @@
+package com.github.mittyrobotics;
+
+public class DriveTrainSubsystem {
+}
diff --git a/src/main/java/com/github/mittyrobotics/DriveTrainSubsytem.java b/src/main/java/com/github/mittyrobotics/DriveTrainSubsytem.java
new file mode 100644
index 0000000..73a50f4
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/DriveTrainSubsytem.java
@@ -0,0 +1,8 @@
+package com.github.mittyrobotics;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveTrainSubsytem extends SubsystemBase {
+ public static DriveTrainSubsystem instance = null;
+
+}
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..9871447
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/OI.java
@@ -0,0 +1,31 @@
+package com.github.mittyrobotics;
+
+import edu.wpi.first.wpilibj.XboxController;
+//import edu.wpi.first.wpilibj.XboxController;
+
+public class OI {
+
+
+ private static OI instance;
+
+ private edu.wpi.first.wpilibj.XboxController XboxController;
+
+ public static OI getInstance() {
+ if (instance == null) {
+ instance = new OI();
+ }
+ return instance;
+
+ }
+
+ public XboxController getXox edu.wpi.first.wpilibj.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..f94ea81 100644
--- a/src/main/java/com/github/mittyrobotics/Robot.java
+++ b/src/main/java/com/github/mittyrobotics/Robot.java
@@ -1,10 +1,12 @@
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 static com.github.mittyrobotics.Constants.*;
//Java automatically runs this class, and calls the various functions.
@@ -18,24 +20,31 @@ public class Robot extends TimedRobot {
* INITIALIZE CLASSES HERE
*/
+ TrapezoidProfile.State start;
+ TrapezoidProfile.State end;
+ TrapezoidProfile.Constraints constraints;
+ TrapezoidProfile profile;
+ TrapezoidProfile.State profileOutput;
+ Talon falcon = new Talon(0);
+ PIDController controller;
+ Encoder encoder;
+ double t;
- @Override
- public void robotInit() {
- }
-
- //Runs periodically during teleoperated mode
- /*
- * WRITE YOUR DRIVE CODE HERE
- */
- @Override
- public void teleopPeriodic() {
+ @Override
+ public void robotInit() {
+ start = new TrapezoidProfile.State(0, 0);
+ TrapezoidProfile.State end = new TrapezoidProfile.State(5.0, 0);
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 0.2);
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, end, start);
+ TrapezoidProfile.State profileOutput = profile.calculate(0.2);
+ double kp = 1.0, ki = 2.0, kd = 3.0;
+ controller = new PIDController(kp, ki, kd);
}
-
- //Runs when antonomous mode (robot runs on its own) first activated via the desktop application
+ //Runs when antonomous mode (robot runs on its own) first activated via the desktop application
@Override
public void autonomousInit() {
@@ -44,7 +53,7 @@ public void autonomousInit() {
//Runs when teleoperated mode (robot controlled by driver) is first activated
@Override
public void teleopInit() {
-
+ t = 0;
}
//Runs when test mode is activated
@@ -70,4 +79,14 @@ public void autonomousPeriodic() {
public void testPeriodic() {
}
-}
\ No newline at end of file
+
+ @Override
+ public void teleopPeriodic() {
+ encoder = new Encoder(Constants.ENCODER_IDS[0], Constants.ENCODER_IDS[1]);
+ TrapezoidProfile.State setpoint = profile.calculate(t);
+ controller.setSetpoint(setpoint.velocity);
+ controller.calculate(encoder.getRate());
+ t += 0.02;
+ }
+}
+
diff --git a/src/main/java/com/github/mittyrobotics/TankDriveCommand/TankDriveCommand.java b/src/main/java/com/github/mittyrobotics/TankDriveCommand/TankDriveCommand.java
new file mode 100644
index 0000000..41e5f40
--- /dev/null
+++ b/src/main/java/com/github/mittyrobotics/TankDriveCommand/TankDriveCommand.java
@@ -0,0 +1,26 @@
+package com.github.mittyrobotics.TankDriveCommand;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class TankDriveCommand extends CommandBase {
+
+ public void initialize() {
+
+ }
+
+ @Override
+ public void execute() {
+
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+
+ }
+
+ @Override
+ public boolean isFinished() {
+
+ }
+
+}