Add line following

build.gradle

@@ -47,11 +47,11 @@ repositories {
 
 // Somewhere after the deploy block, and before the dependencies block
 // https://github.com/pjreiniger/SnobotSim/wiki/Setting-Up-The-Simulator
-// apply plugin: com.snobot.simulator.plugin.SnobotSimulatorPlugin
+apply plugin: com.snobot.simulator.plugin.SnobotSimulatorPlugin
 
-// configurations {
-//   snobotSimCompile
-// }
+configurations {
+  snobotSimCompile
+}
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.
@@ -65,7 +65,7 @@ dependencies {
   compile 'com.moandjiezana.toml:toml4j:0.7.2'
 
   // SnobotSim
-  // snobotSimCompile snobotSimJava()
+  snobotSimCompile snobotSimJava()
 }
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')

src/main/java/frc/robot/Drivetrain.java

@@ -3,6 +3,7 @@
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.loggable.LoggableTalonSRX;
 import frc.robot.logging.DataLogger;
 import frc.robot.logging.Loggable;
@@ -24,6 +25,10 @@ public class Drivetrain implements Loggable {
   private LoggableTalonSRX rightTalon;
   private LoggableTalonSRX rightSlave;
 
+  private DigitalInput leftLine;
+  private DigitalInput midLine;
+  private DigitalInput rightLine;
+
   /**
    * Constructor
    *
@@ -86,10 +91,10 @@ public void arcadeDrive(double xDrive, double yDrive) {
   /**
    * Drives the robot with an tank style drive
    *
-   * @param xDrive The speed to drive the left drivetrain (ranges
-   *               from -1.0 to +1.0)
-   * @param yDrive The speed to drive the right drivetrain (ranges
-   *               from -1.0 to +1.0)
+   * @param xDrive The speed to drive the left drivetrain (ranges from -1.0 to
+   *               +1.0)
+   * @param yDrive The speed to drive the right drivetrain (ranges from -1.0 to
+   *               +1.0)
    */
   public void tankDrive(double leftDrive, double rightDrive) {
     this.driveLeft(leftDrive);
@@ -122,6 +127,28 @@ public void log(DataLogger logger) {
     rightSlave.log(logger);
   }
 
+  /**
+   * Uses outside line sensors to follow the line.
+   *
+   * @param maxMotorPercent maximum motor percent
+   */
+  public void followLine(double maxMotorPercent) {
+    double leftMotorPower = 1;
+    double rightMotorPower = 1;
+    if (leftLine.get() && !rightLine.get()) {
+      leftMotorPower = 0.75;
+    }
+    if (!leftLine.get() && rightLine.get()) {
+      rightMotorPower = 0.75;
+    }
+    if (leftLine.get() && rightLine.get()) {
+      leftMotorPower = 0;
+      rightMotorPower = 0;
+    }
+    driveLeft(leftMotorPower * maxMotorPercent);
+    driveRight(rightMotorPower * maxMotorPercent);
+  }
+
   public Double getOutputCurrent(WPI_TalonSRX talon) {
     if (RuntimeDetector.isSimulation()) {
       return 0.0;

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

@@ -316,9 +316,15 @@ private void humanControl() {
       if (driver.getBumper(GenericHID.Hand.kRight)) {
         speedInput = -speedInput;
       }
-    }
 
-    drive.arcadeDrive(turnInput, speedInput);
+      // System.out.printf("Speed: %f\n", speedInput);
+      // Call line following
+      if (driver.getXButton()) {
+        followLine(0.8);
+      } else {
+        drive.arcadeDrive(turnInput, speedInput);
+      }
+    }
 
     manipulation.lift(operator.getY(Hand.kLeft));
     scoring.tilt(operator.getY(Hand.kRight));
@@ -349,6 +355,7 @@ private void humanControl() {
 
     double collectionSpeed = speedRight - speedLeft;
     scoring.roll(collectionSpeed);
+
   }
 
   /**
@@ -377,4 +384,36 @@ public void testPeriodic() {
   public void disabledInit() {
     dataLogger.close();
   }
+
+  public void followLine(double maxMotorPercent) {
+    double leftMotorPower = .1;
+    double rightMotorPower = .1;
+    double turnspeed = 0.0;
+    double turnmodifier = 0.3;
+    double forwardspeed = -0.15;
+
+    int l = (leftLine.get()) ? 1 : 0;
+    int m = (midLine.get()) ? 1 : 0;
+    int r = (rightLine.get()) ? 1 : 0;
+
+    String bin = String.format("%d%d%d", l, m, r);
+    System.out.println(bin);
+
+    if (bin.equals("100")) {
+      turnspeed -= turnmodifier;
+    } else if (bin.equals("110")) {
+      turnspeed -= turnmodifier;
+    } else if (bin.equals("001")) {
+      turnspeed += turnmodifier;
+    } else if (bin.equals("011")) {
+      turnspeed += turnmodifier;
+    } else if (bin.equals("101")) {
+      System.out.println("Subscribe to Pewdiepie");
+      leftMotorPower = 0;
+      rightMotorPower = 0;
+    } else {
+      System.out.println("Jordan");
+    }
+    drive.arcadeDrive(turnspeed, forwardspeed);
+  }
 }