From 8e4c083cf74939e98aabaf8aaf45976a148d0666 Mon Sep 17 00:00:00 2001
From: Cindy Zhang <cindyzyx9@gmail.com>
Date: Thu, 26 Jan 2017 19:36:14 -0800
Subject: [PATCH] fix bugs

---
 src/org/usfirst/frc/team3501/robot/Robot.java        |  4 ++--
 .../robot/commands/driving/DriveDistance.java        | 12 +++++++-----
 .../frc/team3501/robot/subsystems/DriveTrain.java    | 10 ++++++++++
 3 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index c2e4f99..a9dd0ff 100644
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -1,6 +1,6 @@
 package org.usfirst.frc.team3501.robot;
 
-import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
+import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
 import org.usfirst.frc.team3501.robot.subsystems.Intake;
 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
@@ -40,7 +40,7 @@ public class Robot extends IterativeRobot {
 
   @Override
   public void autonomousInit() {
-    Scheduler.getInstance().add(new TimeDrive(1.5, 0.4));
+    Scheduler.getInstance().add(new DriveDistance(25, 10));
   }
 
   @Override
diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
index 47fc46e..5c0c20b 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -1,6 +1,5 @@
 package org.usfirst.frc.team3501.robot.commands.driving;
 
-import org.usfirst.frc.team3501.robot.MathLib;
 import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
 import org.usfirst.frc.team3501.robot.utils.PIDController;
@@ -31,8 +30,11 @@ public class DriveDistance extends Command {
   private double driveI;
   private double driveD;
 
-  public DriveDistance(double distance, double motorVal) {
+  public DriveDistance(double distance, double maxTimeOut) {
     requires(driveTrain);
+    this.maxTimeOut = maxTimeOut;
+    this.target = distance;
+
     this.driveP = driveTrain.driveP;
     this.driveI = driveTrain.driveI;
     this.driveD = driveTrain.driveD;
@@ -69,12 +71,12 @@ public class DriveDistance extends Command {
           .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
     }
     // System.out.println("turn: " + xVal);
-    double leftDrive = MathLib.calcLeftTankDrive(-xVal, yVal);
-    double rightDrive = MathLib.calcRightTankDrive(xVal, -yVal);
+    double leftDrive = yVal - xVal;
+    double rightDrive = yVal + xVal;
 
     this.driveTrain.setMotorValues(leftDrive, rightDrive);
 
-    System.out.println(driveTrain.getAvgEncoderDistance());
+    driveTrain.printEncoderOutput();
     // System.out.println("motorval: " + yVal);
   }
 
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index ba5392a..44744c4 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -7,6 +7,7 @@ import org.usfirst.frc.team3501.robot.utils.BNO055;
 import com.ctre.CANTalon;
 
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.I2C.Port;
 import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
@@ -46,6 +47,10 @@ public class DriveTrain extends Subsystem {
 
     // ROBOT DRIVE
     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+    this.imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
+        BNO055.vector_type_t.VECTOR_EULER, Port.kOnboard, (byte) 0x28);
+    gyroZero = imu.getHeading();
   }
 
   public static DriveTrain getDriveTrain() {
@@ -98,6 +103,11 @@ public class DriveTrain extends Subsystem {
     return rightEncoder.getDistance();
   }
 
+  public void printEncoderOutput() {
+    System.out.println("left: " + getLeftEncoderDistance());
+    System.out.println("right: " + getRightEncoderDistance());
+  }
+
   public double getAvgEncoderDistance() {
     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
   }
-- 
2.30.2