X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fauton%2FAlignToScore.java;h=0f7a79b2bdc7ea5c8c70b3ce2500c4f91738c605;hb=f11e620060e9ff7062b4409bbc4f14bb5bcf2e9d;hp=918a9fca74ca5b96dfc8dae31593144fc3a0b8ca;hpb=dfcc1b2194b4c82b295440af9b408b6b46f40e03;p=3501%2Fstronghold-2016

diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java
index 918a9fca..0f7a79b2 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java
@@ -1,6 +1,5 @@
 package org.usfirst.frc.team3501.robot.commands.auton;
 
-import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
 import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
 
@@ -116,31 +115,35 @@ public class AlignToScore extends CommandGroup {
     }
   }
 
-  public static double lidarCalculateAngleToTurn(int position,
-      double horizontalDistToGoal) {
-    double leftDist = Robot.driveTrain.getLeftLidarDistance();
-    double rightDist = Robot.driveTrain.getRightLidarDistance();
-
-    double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2);
-    double distToTower;
-    // TODO: figure out if we do want to shoot into the side goal if we are
-    // in position 1 or 2, or if we want to change that
-    if (position == 1 || position == 2) {
-      distToTower = Math
-          .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
-          - DIST_CASTLE_WALL_TO_SIDE_GOAL;
-    }
-
-    // TODO: figure out if we do want to shoot into the font goal if we are
-    // in position 3, 4, 5, or if we want to change that
-    else {
-      distToTower = Math
-          .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
-          - DIST_CASTLE_WALL_TO_SIDE_GOAL;
-    }
-
-    double angleToTurn = Math.atan(distToTower / horizontalDistToGoal);
-
-    return angleToTurn;
-  }
+  // following commented out method is calculations for path of robot in auton
+  // after passing through defense using two lidars
+  /*
+   * public static double lidarCalculateAngleToTurn(int position,
+   * double horizontalDistToGoal) {
+   * double leftDist = Robot.driveTrain.getLeftLidarDistance();
+   * double rightDist = Robot.driveTrain.getRightLidarDistance();
+   * 
+   * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2);
+   * double distToTower;
+   * // TODO: figure out if we do want to shoot into the side goal if we are
+   * // in position 1 or 2, or if we want to change that
+   * if (position == 1 || position == 2) {
+   * distToTower = Math
+   * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
+   * - DIST_CASTLE_WALL_TO_SIDE_GOAL;
+   * }
+   * 
+   * // TODO: figure out if we do want to shoot into the font goal if we are
+   * // in position 3, 4, 5, or if we want to change that
+   * else {
+   * distToTower = Math
+   * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
+   * - DIST_CASTLE_WALL_TO_SIDE_GOAL;
+   * }
+   * 
+   * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal);
+   * 
+   * return angleToTurn;
+   * }
+   */
 }