fix command names and get rid of unused stuff

diff --git a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
index a2f83d4233a23e8e16d0349b5ef9c1a4ee1118a4..356c71a82c8367e18aecf5047d4ec8017887acd6 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot.commands;
 
-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;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
@@ -19,6 +20,7 @@ public class AlignToScore extends CommandGroup {
   private final double DIST_CENTER_OF_MASS_TO_FRONT_OF_ROBOT = 0;
 
   private final double DEFAULT_SPEED = 0.5;
+  private final double maxTimeout = 5;
 
   // constants for position 1: low bar
   private final double POS1_DIST1 = 0;
@@ -56,42 +58,37 @@ public class AlignToScore extends CommandGroup {
     // position 1 is always the low bar
     case 1:
 
-      addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED));
-      addSequential(new TurnForAngle(POS1_TURN1));
-      addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED));
+      addSequential(new DriveDistance(POS1_DIST1, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS1_TURN1, maxTimeout));
+      addSequential(new DriveDistance(POS1_DIST2, DEFAULT_SPEED));
 
     case 2:
 
-      addSequential(new DriveForDistance(POS2_DIST1, DEFAULT_SPEED));
-      addSequential(new TurnForAngle(POS2_TURN1));
-      addSequential(new DriveForDistance(POS2_DIST2, DEFAULT_SPEED));
+      addSequential(new DriveDistance(POS2_DIST1, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS2_TURN1, maxTimeout));
+      addSequential(new DriveDistance(POS2_DIST2, DEFAULT_SPEED));
 
     case 3:
 
-      addSequential(new DriveForDistance(POS3_DIST1, DEFAULT_SPEED));
-      addSequential(new TurnForAngle(POS3_TURN1));
-      addSequential(new DriveForDistance(POS3_DIST2, DEFAULT_SPEED));
-      addSequential(new TurnForAngle(POS3_TURN2));
-      addSequential(new DriveForDistance(POS3_DIST3, DEFAULT_SPEED));
+      addSequential(new DriveDistance(POS3_DIST1, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS3_TURN1, maxTimeout));
+      addSequential(new DriveDistance(POS3_DIST2, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS3_TURN2, maxTimeout));
+      addSequential(new DriveDistance(POS3_DIST3, DEFAULT_SPEED));
 
     case 4:
 
-      addSequential(new DriveForDistance(POS4_DIST1, DEFAULT_SPEED));
-      addSequential(new TurnForAngle(POS4_TURN1));
-      addSequential(new DriveForDistance(POS4_DIST2, DEFAULT_SPEED));
-      addSequential(new TurnForAngle(POS4_TURN2));
-      addSequential(new DriveForDistance(POS4_DIST3, DEFAULT_SPEED));
+      addSequential(new DriveDistance(POS4_DIST1, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS4_TURN1, maxTimeout));
+      addSequential(new DriveDistance(POS4_DIST2, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS4_TURN2, maxTimeout));
+      addSequential(new DriveDistance(POS4_DIST3, DEFAULT_SPEED));
 
     case 5:
 
-      addSequential(new DriveForDistance(POS5_DIST1, DEFAULT_SPEED));
-      addSequential(new TurnForAngle(POS5_TURN1));
-      addSequential(new DriveForDistance(POS5_DIST2, DEFAULT_SPEED));
+      addSequential(new DriveDistance(POS5_DIST1, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS5_TURN1, maxTimeout));
+      addSequential(new DriveDistance(POS5_DIST2, DEFAULT_SPEED));
     }
   }
-
-  public static void calculatePath() {
-    double leftDistance = Robot.shooter.getLeftDistanceToTower();
-    double rightDistance = Robot.shooter.getRightDistanceToTower();
-  }
 }

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 918a9fca74ca5b96dfc8dae31593144fc3a0b8ca..0f7a79b2bdc7ea5c8c70b3ce2500c4f91738c605 100755 (executable)
--- 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;
+   * }
+   */
 }

diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/Punch.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/Punch.java
index deb2c04b4424577adb16e5efb8b58658bfb517a5..36ef785f7df0fca53a76748933a58e038bcd806a 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/shooter/Punch.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/Punch.java
@@ -8,7 +8,7 @@ public class Punch extends Command {
 
   @Override
   protected void initialize() {
-    Robot.shooter.punch();
+    Robot.shooter.extendPunch();
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
index 22239e49ee945c5bd0a9a163d9a9da7160233db2..0264c7578e5f34afb6ce0bdfea7abaab0a36fdec 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
@@ -2,9 +2,7 @@ package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.Lidar;
-import org.usfirst.frc.team3501.robot.MathLib;
 
-import edu.wpi.first.wpilibj.AnalogPotentiometer;
 import edu.wpi.first.wpilibj.CANTalon;
 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
@@ -29,12 +27,10 @@ public class Shooter extends Subsystem {
   private Lidar lidar;
 
   public Shooter() {
-    leftLidar = new AnalogPotentiometer(0);
-    rightLidar = new AnalogPotentiometer(0);
     shooter = new CANTalon(Constants.Shooter.PORT);
     angleAdjuster = new CANTalon(Constants.Shooter.ANGLE_ADJUSTER_PORT);
-    punch = new DoubleSolenoid(Constants.Shooter.PUNCH_FORWARD_PORT,
-        Constants.Shooter.PUNCH_REVERSE_PORT);
+    punch = new DoubleSolenoid(Constants.Shooter.PUNCH_FORWARD,
+        Constants.Shooter.PUNCH_REVERSE);
 
     encoder = new Encoder(Constants.Shooter.ENCODER_PORT_A,
         Constants.Shooter.ENCODER_PORT_B, false, EncodingType.k4X);
@@ -88,13 +84,4 @@ public class Shooter extends Subsystem {
   @Override
   protected void initDefaultCommand() {
   }
-
-  public double getLeftDistanceToTower() {
-    // TODO: find the method that actually gets distance
-    return leftLidar.get();
-  }
-
-  public double getRightDistanceToTower() {
-    return rightLidar.get();
-  }
 }