From 96215d97340b055f4f6f2d10b129552b15bbc8f8 Mon Sep 17 00:00:00 2001
From: Meryem Esa <meresa14@gmail.com>
Date: Wed, 10 Feb 2016 19:37:37 -0800
Subject: [PATCH] instead of just one lidar, add one left and one right

---
 .../team3501/robot/subsystems/DriveTrain.java | 47 +++++++++++++------
 1 file changed, 33 insertions(+), 14 deletions(-)

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index be0103cf..bbca72fc 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -2,6 +2,7 @@ package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.GyroLib;
+import org.usfirst.frc.team3501.robot.Lidar;
 import org.usfirst.frc.team3501.robot.MathLib;
 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
@@ -15,11 +16,8 @@ import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
 public class DriveTrain extends PIDSubsystem {
-  // Encoder PID Proportional Constants P, I, and D
-  private static double EP = 0.013, EI = 0.000015, ED = -0.002;
-
-  // Gyro PID Constants P, I, and D
-  private static double GP = 0.018, GI = 0.000015, GD = 0;
+  private static double kp = 0.013, ki = 0.000015, kd = -0.002;
+  private static double gp = 0.018, gi = 0.000015, gd = 0;
   private static double pidOutput = 0;
 
   // PID Controller tolerances for the error
@@ -32,6 +30,10 @@ public class DriveTrain extends PIDSubsystem {
   private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
 
   private Encoder leftEncoder, rightEncoder;
+
+  public static Lidar leftLidar;
+  public static Lidar rightLidar;
+
   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
   private RobotDrive robotDrive;
 
@@ -61,6 +63,10 @@ public class DriveTrain extends PIDSubsystem {
     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
 
     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+    leftLidar = new Lidar(I2C.Port.kOnboard);
+    rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second
+                                               // lidar
     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
@@ -82,6 +88,7 @@ public class DriveTrain extends PIDSubsystem {
         Constants.DriveTrain.LEFT_REVERSE);
     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
         Constants.DriveTrain.RIGHT_REVERSE);
+    Constants.DriveTrain.inverted = false;
   }
 
   @Override
@@ -118,6 +125,14 @@ public class DriveTrain extends PIDSubsystem {
     rightEncoder.reset();
   }
 
+  public double getLeftLidarDistance() {
+    return leftLidar.pidGet();
+  }
+
+  public double getsRightLidarDistance() {
+    return rightLidar.pidGet();
+  }
+
   public double getRightSpeed() {
     return rightEncoder.getRate(); // in inches per second
   }
@@ -205,10 +220,10 @@ public class DriveTrain extends PIDSubsystem {
   /*
    * Method is a required method that the PID Subsystem uses to return the
    * calculated PID value to the driver
-   * 
+   *
    * @param Gives the user the output from the PID algorithm that is calculated
    * internally
-   * 
+   *
    * Body: Uses the output, does some filtering and drives the robot
    */
   @Override
@@ -221,8 +236,7 @@ public class DriveTrain extends PIDSubsystem {
         output = Math.signum(output) * 0.3;
       left = output;
       right = output + drift * kp / 10;
-    }
-    else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
+    } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
       left = output;
       right = -output;
     }
@@ -237,7 +251,7 @@ public class DriveTrain extends PIDSubsystem {
 
   /*
    * Checks the drive mode
-   * 
+   *
    * @return the current state of the robot in each state
    * Average distance from both sides of tank drive for Encoder Mode
    * Angle from the gyro in GYRO_MODE
@@ -262,14 +276,19 @@ public class DriveTrain extends PIDSubsystem {
   public void drive(double left, double right) {
     robotDrive.tankDrive(-left, -right);
     // dunno why but inverted drive (- values is forward)
+    if (!Constants.DriveTrain.inverted)
+      robotDrive.tankDrive(-left,
+          -right);
+    else
+      robotDrive.tankDrive(right, left);
   }
 
   /*
    * constrains the distance to within -100 and 100 since we aren't going to
    * drive more than 100 inches
-   *
+   * 
    * Configure Encoder PID
-   *
+   * 
    * Sets the setpoint to the PID subsystem
    */
   public void driveDistance(double dist, double maxTimeOut) {
@@ -310,10 +329,10 @@ public class DriveTrain extends PIDSubsystem {
 
   /*
    * Turning method that should be used repeatedly in a command
-   * 
+   *
    * First constrains the angle to within -360 and 360 since that is as much as
    * we need to turn
-   * 
+   *
    * Configures Gyro PID and sets the setpoint as an angle
    */
   public void turnAngle(double angle) {
-- 
2.30.2