Romi krishna

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);
+    }
+}

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;
+    }
+}

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() {