fix stuff

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index c446a8687b22642a025abafecabfa9ecaf167717..515483c974c7131928737f4c24d241ddd4f9278d 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -1,5 +1,6 @@
 package org.usfirst.frc.team3501.robot;
 
+import org.usfirst.frc.team3501.robot.commandgroups.AutonGearThenBaselinePegCloseToBoiler;
 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
 import org.usfirst.frc.team3501.robot.subsystems.Intake;
 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
@@ -47,6 +48,8 @@ public class Robot extends IterativeRobot {
   @Override
   public void autonomousInit() {
     driveTrain.setHighGear();
+    Scheduler.getInstance()
+        .add(new AutonGearThenBaselinePegCloseToBoiler("RETRIEVAL", 0));
   }
 
   @Override
@@ -56,7 +59,7 @@ public class Robot extends IterativeRobot {
 
   @Override
   public void teleopInit() {
-
+    Scheduler.getInstance().removeAll();
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
index 6af22a452c7bfa5bc65162c5967467b0b4953776..2fcb8f3a8ee943365dd4d82d558564ba64824c7b 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
@@ -17,17 +17,15 @@ public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup
 
 {
 
-  private static final double ROBOT_LENGTH = 40.0;
-  private static final double ROBOT_WIDTH = 36.0;
+  private static final double ROBOT_LENGTH = 36.0;
+  private static final double ROBOT_WIDTH = 40.0;
 
-  private static final double DISTANCE_TO_PEG_EXTENDED_BOILER = 73.1657;
-  private static final double DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE = 114.3
-      + 17.625 - ((162 - (17.625 + 35.25) - 38.625) / Math.sqrt(3));
-  private static final double FIRST_LIMIT_LENGTH = 75.649;
-  private static final double SECOND_LIMIT_LENGTH = 162.8289106736;
+  private static final double STRAIGHT_DIST_FROM_BOILER = 76.8;
+  private static final double DISTANCE_TO_RETRIEVAL_PEG = 114.3 + 17.625
+      - ((162 - (17.625 + 35.25) - 38.625) / Math.sqrt(3));
   private static final double LENGTH_OF_PEG = 10.5;
   private static final double PEG_EXTENDED_BOILER = 117.518;
-  private static final double PEG_EXTENDED_RETRIEVAL_ZONE = (DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE
+  private static final double PEG_EXTENDED_RETRIEVAL_ZONE = (DISTANCE_TO_RETRIEVAL_PEG
       * 2) / Math.sqrt(3);
   private static final double DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE = 255.6765151902;
   private static final double MOTOR_VALUE_TURN = 0.5;// this is probably not
@@ -38,64 +36,57 @@ public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup
                              // arena in inches (can change)
 
   /***
-   * 
-   * @param team
+   *
+   * @param side
    *          which side to start on: "BLUE" for blue side and "RED" for red
    *          side
-   * @param distanceFromBoilerCorner
+   * @param distanceFromcorner
    *          distance from the corner of the boiler
    */
-  public AutonGearThenBaselinePegCloseToBoiler(String team,
-      int distanceFromBoilerCorner) {
-
-    this.distanceFromCorner = distanceFromBoilerCorner;
+  public AutonGearThenBaselinePegCloseToBoiler(String side,
+      int distanceFromcorner) {
     requires(Robot.getDriveTrain());
-    if (distanceFromBoilerCorner >= 0
-        && distanceFromBoilerCorner < FIRST_LIMIT_LENGTH) {
-      // boiler peg path
 
-      // drive straight into extended line of peg
-      addSequential(
-          new DriveDistance(
-              ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
-                  + DISTANCE_TO_PEG_EXTENDED_BOILER - (ROBOT_LENGTH / 2.0),
-              MOTOR_VALUE_FORWARD));
+    if (side.equals("BOILER")) {
+      addSequential(new DriveDistance(
+          131.6 - (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+              - ROBOT_LENGTH / 2,
+          5));
+      // addSequential(new DriveDistance(
+      // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
+      // + STRAIGHT_DIST_FROM_BOILER - (ROBOT_LENGTH / 2.0),
+      // MOTOR_VALUE_FORWARD));
       // position robot for optimal gear placement
-      addSequential(new TurnForAngle(60, Direction.LEFT, MOTOR_VALUE_TURN));
+      addSequential(new TurnForAngle(60, Direction.RIGHT, 5));
       // put gear on peg
-      addSequential(new DriveDistance(PEG_EXTENDED_BOILER
-          - (((distanceFromCorner + (ROBOT_WIDTH / 2.0)) * 2)
-              / Math.sqrt(3))
-          - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0),
-          MOTOR_VALUE_FORWARD));
+      addSequential(new DriveDistance(
+          2 * (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+              - ROBOT_LENGTH / 2 + 7,
+          5));
+      // addSequential(new DriveDistance(PEG_EXTENDED_BOILER
+      // - (((distanceFromCorner + (ROBOT_WIDTH / 2.0)) * 2) / Math.sqrt(3))
+      // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD));
 
       // does not include drift distance
-    } else if (distanceFromBoilerCorner > SECOND_LIMIT_LENGTH
-        && distanceFromBoilerCorner <= DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE) // mirror
-                                                                                   // of
-                                                                                   // right
-                                                                                   // peg
-                                                                                   // path
-    {
-      this.distanceFromCorner = DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE
-          - distanceFromBoilerCorner;
-      // Turning DISTANCE_FROM_BOILER_WALL into starting point from left side by
-      // reversing it
-      addSequential(
-          new DriveDistance(
-              ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
-                  + DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE
-                  - (ROBOT_LENGTH / 2.0),
-              MOTOR_VALUE_FORWARD));
-      addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
-      addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE
-          - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2)
-          - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0),
-          MOTOR_VALUE_FORWARD));
+    } else if (side.equals("RETRIEVAL")) {
+      addSequential(new DriveDistance(
+          131.6 - (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+              - ROBOT_LENGTH / 2,
+          5));
+      addSequential(new TurnForAngle(60, Direction.LEFT, 5));
+      addSequential(new DriveDistance(
+          2 * (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+              - ROBOT_LENGTH / 2,
+          5));
+      // addSequential(new DriveDistance(
+      // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
+      // + DISTANCE_TO_RETRIEVAL_PEG - (ROBOT_LENGTH / 2.0),
+      // MOTOR_VALUE_FORWARD));
+      // addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
+      // addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE
+      // - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2)
+      // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD));
       // does not include drift distance
-    } else {
-      System.out.println(
-          "DO NOT START HERE!!! MOVE CLOSER TO A WALL OR LOSE ALL AUTON POINTS!!");
     }
   }
 }

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 582cd80b69abb799a89a7b3afdcd56432d22a738..d297e66a1dcb1ecb55445b8291ce089d7d4f24ea 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -47,12 +47,13 @@ public class DriveDistance extends Command {
     this.driveTrain.resetEncoders();
     this.driveController.setSetPoint(this.target);
     this.zeroAngle = driveTrain.getAngle();
+    System.out.println("drive " + target);
   }
 
   @Override
   protected void execute() {
     double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
-    double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
+    double yVal = driveController.calcPID(driveTrain.getLeftEncoderDistance());
 
     double leftDrive = yVal - xVal;
     double rightDrive = yVal + xVal;

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
index 717f15151d5b8512218b2e865acade3403cd9ced..44434c694531aa65751e95f307fe740ffbf1378a 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
@@ -70,6 +70,7 @@ public class TurnForAngle extends Command {
     }
 
     this.driveTrain.setMotorValues(leftDrive, rightDrive);
+    System.out.println(this.driveTrain.getAngle() - this.zeroAngle);
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 32ec7b221ab237ef17747dfa46f47f8246ace31b..a0b75c6de0bffb327ea0bc2397cf30320a714900 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -14,8 +14,8 @@ import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
 public class DriveTrain extends Subsystem {
-  public static double driveP = 0.006, driveI = 0.0011, driveD = -0.002;
-  public static double turnP = 0.004, turnI = 0.0013, turnD = -0.005;
+  public static double driveP = 0.008, driveI = 0.0011, driveD = -0.002;
+  public static double turnP = 0.006, turnI = 0.0013, turnD = -0.005;
   public static double driveStraightGyroP = 0.01;
 
   public static final double WHEEL_DIAMETER = 4; // inches
@@ -77,8 +77,8 @@ public class DriveTrain extends Subsystem {
 
   // DRIVE METHODS
   public void setMotorValues(double left, double right) {
-    left = MathLib.restrictToRange(left, 0.0, 1.0);
-    right = MathLib.restrictToRange(right, 0.0, 1.0);
+    left = MathLib.restrictToRange(left, -1.0, 1.0);
+    right = MathLib.restrictToRange(right, -1.0, 1.0);
 
     frontLeft.set(left);
     rearLeft.set(left);