From 6c72e0a830472f433d7d3a3fbba284abfafeb1f0 Mon Sep 17 00:00:00 2001
From: Kevin Zhang <icestormf1@gmail.com>
Date: Sun, 21 Feb 2016 14:34:14 -0800
Subject: [PATCH] Change turning to using arcade drive turning to be smoother

---
 src/org/usfirst/frc/team3501/robot/Robot.java | 29 ++++--------------
 .../robot/commands/auton/AimAndAlign.java     | 30 -------------------
 .../robot/commands/driving/TurnForTime.java   |  6 ++--
 .../team3501/robot/subsystems/DriveTrain.java |  7 ++++-
 4 files changed, 16 insertions(+), 56 deletions(-)
 delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/AimAndAlign.java

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 8beb817d..19fa056a 100644
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -26,7 +26,7 @@ public class Robot extends IterativeRobot {
   // Sendable Choosers send a drop down menu to the Smart Dashboard.
   SendableChooser positionChooser;
   SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense,
-      positionFourDefense, positionFiveDefense;
+  positionFourDefense, positionFiveDefense;
 
   @Override
   public void robotInit() {
@@ -104,28 +104,6 @@ public class Robot extends IterativeRobot {
   public void autonomousInit() {
     Scheduler.getInstance().run();
 
-    // // get options chosen from drop down menu
-    // Integer chosenPosition = (Integer) positionChooser.getSelected();
-    // Integer chosenDefense = 0;
-    //
-    // if (chosenPosition == 1)
-    // chosenDefense = (Integer) positionOneDefense.getSelected();
-    // else if (chosenPosition == 2)
-    // chosenDefense = (Integer) positionTwoDefense.getSelected();
-    // else if (chosenPosition == 3)
-    // chosenDefense = (Integer) positionThreeDefense.getSelected();
-    // else if (chosenPosition == 4)
-    // chosenDefense = (Integer) positionFourDefense.getSelected();
-    // else if (chosenPosition == 5)
-    // chosenDefense = (Integer) positionFiveDefense.getSelected();
-    //
-    // System.out.println("Chosen Position: " + chosenPosition);
-    // System.out.println("Chosen Defense: " + chosenDefense);
-  }
-
-  @Override
-  public void autonomousPeriodic() {
-    Scheduler.getInstance().run();
     // Scheduler.getInstance().add(new DriveDistance(24, 5));
     // Scheduler.getInstance().add(new DriveForTime(2, 0.3));
     // Scheduler.getInstance().add(new TurnForAngle(90, 5));
@@ -136,6 +114,11 @@ public class Robot extends IterativeRobot {
     // Scheduler.getInstance().add(new Turn180());
   }
 
+  @Override
+  public void autonomousPeriodic() {
+    Scheduler.getInstance().run();
+  }
+
   @Override
   public void teleopInit() {
     Scheduler.getInstance().add(new JoystickDrive());
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AimAndAlign.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AimAndAlign.java
deleted file mode 100755
index d304013f..00000000
--- a/src/org/usfirst/frc/team3501/robot/commands/auton/AimAndAlign.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class AimAndAlign extends Command {
-
-	public AimAndAlign() {
-	}
-
-	@Override
-	protected void initialize() {
-	}
-
-	@Override
-	protected void execute() {
-	}
-
-	@Override
-	protected boolean isFinished() {
-		return false;
-	}
-
-	@Override
-	protected void end() {
-	}
-
-	@Override
-	protected void interrupted() {
-	}
-}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java
index 368d1045..f0f2ecf6 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java
@@ -45,9 +45,11 @@ public class TurnForTime extends Command {
     timer.start();
 
     if (direction == Direction.RIGHT) {
-      Robot.driveTrain.drive(speed, -speed);
+      // Robot.driveTrain.drive(speed, -speed);
+      Robot.driveTrain.arcadeDrive(0, speed);
     } else if (direction == Direction.LEFT) {
-      Robot.driveTrain.drive(-speed, speed);
+      // Robot.driveTrain.drive(-speed, speed);
+      Robot.driveTrain.arcadeDrive(0, speed);
     }
   }
 
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 5fa52933..a7fa8eea 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -213,11 +213,12 @@ public class DriveTrain extends PIDSubsystem {
         output = Math.signum(output) * 0.3;
       left = output;
       right = output + drift * Constants.DriveTrain.kp / 10;
+      drive(left, right);
     } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
       left = output;
       right = -output;
+      arcadeDrive(0, output);
     }
-    drive(left, right);
     pidOutput = output;
   }
 
@@ -257,6 +258,10 @@ public class DriveTrain extends PIDSubsystem {
       robotDrive.tankDrive(right, left);
   }
 
+  public void arcadeDrive(double y, double twist) {
+    robotDrive.arcadeDrive(y, twist);
+  }
+
   /*
    * constrains the distance to within -100 and 100 since we aren't going to
    * drive more than 100 inches
-- 
2.30.2