Updated code

Freshman_Base_2019/src/main/java/frc/robot/Robot.java

@@ -1,103 +1,38 @@
 package frc.robot;
 
-//import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import com.kauailabs.navx.frc.AHRS;
-
 import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.SerialPort;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.Timer;
-import frc.robot.autos.AutoCompilation;
-import frc.robot.autos.RealAutos;
 import frc.robot.controls.DriverControls;
 import frc.robot.controls.OperatorControls;
-import frc.robot.subsystems.Drivetrain;
-import frc.robot.subsystems.HatchCollector;
-import frc.robot.subsystems.Intake;
-import frc.robot.subsystems.LineFollower;
-import frc.robot.subsystems.Shooter;
 
-import java.lang.Math;
 
 public class Robot extends TimedRobot {
-    public static Joystick driverJoy;
-    static Joystick operatorJoy;
-
-    public static boolean firstTime = true;
-    static boolean buttonEnable = false, buttonPrev = false;
-
-    final int firstDistance = 120;
-    final int secondDistance = 60;
-
-    static int autostage = 0;
-
-    public static AHRS NAVXgyro = new AHRS(SerialPort.Port.kMXP);
-
-    DriverControls driver = new DriverControls(2);
-    OperatorControls operator = new OperatorControls(1);
-
-    static Drivetrain drivingClass = new Drivetrain();
-    public static Intake spinningIntake = new Intake();
-    public static Shooter shootNow = new Shooter();
-    static HatchCollector maticsMatics = new HatchCollector();
-    static LineFollower lineFollower = new LineFollower();
-    static RealAutos autos = new RealAutos();
-    static AutoCompilation tryAutosAgain = new AutoCompilation();
 
-    boolean startAuto = false;
-    boolean startAutoSet = false;
+    private DriverControls driverControls;
+    private OperatorControls operatorControls;
 
-    boolean startTimeSet = false;
-    boolean startTimeSetTwo = false;
-    public static double startTime = 0;
-    static double goNow = 0;
-
-    double pi = Math.PI;
 
     @Override
     public void robotInit() {
-        driverJoy = new Joystick(0);
-        operatorJoy = new Joystick(1);
+        driverControls = new DriverControls(0);
+        operatorControls = new OperatorControls(1);
         CameraServer.getInstance().startAutomaticCapture();
-        NAVXgyro.reset();
-        drivingClass.initQuadrature();
     }
 
-	@Override
+    @Override
     public void autonomousInit() {
-
-        if (startTimeSet == false) {
-            startTime = Timer.getFPGATimestamp();
-            startTimeSet = true;
-        }
-
-        if (startTimeSetTwo == false) {
-            goNow = Timer.getFPGATimestamp();
-            startTimeSetTwo = true;
-        }
     }
 
     @Override
     public void autonomousPeriodic() {
-        if (!drivingClass.moveDistance(60, .25)) {
-            drivingClass.moveDistance(60, .25);
-        } else if (drivingClass.moveDistance(60, .25)) {
-            frc.robot.subsystems.Shooter.autoShoot();
-        }
+        driverControls.controls();
+        operatorControls.controls();
     }
 
     @Override
     public void teleopPeriodic() {
-
-        driver.controls();
-        operator.controls();        
-
-        Drivetrain.drive(driverJoy.getRawAxis(1) * 1, driverJoy.getRawAxis(2) * 1, driverJoy.getRawAxis(3) * 1);
-        spinningIntake.spinIntake(driverJoy.getRawButton(1), driverJoy.getRawButton(4), driverJoy.getRawButton(2));
-        shootNow.operatorShoot(operatorJoy.getRawAxis(1), operatorJoy.getRawAxis(5));
-        maticsMatics.openClaw(operatorJoy.getRawAxis(2));
-        maticsMatics.moveToSafety(operatorJoy.getRawButton(3), operatorJoy.getRawButton(1));
+        driverControls.controls();
+        operatorControls.controls();
     }
 
     @Override
@@ -107,6 +42,4 @@ public void testInit() {
     @Override
     public void testPeriodic() {
     }
-}
-
-// Need to put LED code into the regular code
+}

Freshman_Base_2019/src/main/java/frc/robot/controls/DriverControls.java

@@ -1,71 +1,51 @@
 package frc.robot.controls;
 
-//import frc.robot.Drivetrain;
-//import frc.robot.Intake;
-
-//TRY TOGGLE USING THE D-PAD???
+import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Intake;
 
 public class DriverControls extends LogitechControls {
-
-    double left;
-    double right;
-    double straight;
-
+
+    private double turningMultiplier = .75, slowTurningSpeed = .3;
+
     public DriverControls(int port) {
         super(port);
     }
 
-    public void driverControls() {
-        controls();
-    }
-
     public void controls() {
-    straight = 0;
-    left = 0;
-    right = 0;
-    if(getDPadLeft()) {
-        if(Math.abs(getLeftYAxis()) > 0.1) {
-            straight = getLeftYAxis();
-        }
-        if(getLeftTrigger() > 0.1) {
-            left = getLeftTrigger();
-        }
-        if(getRightTrigger() > 0.1) {
-            right = getRightTrigger();
-        }
+        /*
+         * Drivetrain 
+         * Left Y axis - forward and back 
+         * Triggers - left and right
+         * Bumpers - slower left and right
+         * A - run intake forward
+         * Y - run intake backwards
+         * B - stop intake
+         */
 
-            frc.robot.subsystems.Drivetrain.drive(straight, right, left);
-    }
-    else if (getDPadRight()) {
-        if(Math.abs(getLeftYAxis()) > 0.1) {
-            straight = -getLeftYAxis();
-        }
-        if(getLeftTrigger() > 0.1) {
-            left = -getLeftTrigger();
-        }
-        if(getRightTrigger() > 0.1) {
-            right = -getRightTrigger();
-        }
+        // Drivetrain
+        double leftYAxis = getLeftYAxis();
+        double rightTrigger = getRightTrigger();
+        double leftTrigger = getLeftTrigger();
 
-            frc.robot.subsystems.Drivetrain.drive(straight, right, left);
-    }   
-        if(getAButton()) {
-            frc.robot.subsystems.Intake.setSpeed(-1);
-        }
-        if(getBButton()) {
-            frc.robot.subsystems.Intake.stop();
-        }
-        if(getXButton()) {
-            frc.robot.subsystems.Intake.setSpeed(1);
-        }
-        if(!getAButton()) {
-            frc.robot.subsystems.Intake.stop();
+        boolean rightShoulderButton = getRightShoulderButton();
+        boolean leftShoulderButton = getLeftShoulderButton();
+
+        double speedStraight = Math.abs(leftYAxis) > .08 ? leftYAxis : 0;
+        double speedRight = rightTrigger > .15 ? rightTrigger * turningMultiplier
+                : rightShoulderButton ? slowTurningSpeed : 0;
+        double speedLeft = leftTrigger > .15 ? leftTrigger * turningMultiplier
+                : leftShoulderButton ? slowTurningSpeed : 0;
+        Drivetrain.drive(speedStraight, speedRight, speedLeft);
+
+        // Intake
+        if (getAButton()) {
+            Intake.run(false);
         }
-        if(!getBButton()) {
-            frc.robot.subsystems.Intake.stop();
+        if (getYButton()) {
+            Intake.run(true);
         }
-        if(!getXButton()) {
-            frc.robot.subsystems.Intake.stop();
+        if (getBButton()) {
+            Intake.stop();
         }
     }
-}
+}