Merge branch 'auton-math'

Author: WilsonBerkow

src/com/stuypulse/frc2017/robot/commands/auton/ApproachAllianceHopperFromBoiler.java

@@ -11,9 +11,8 @@
  *
  */
 public class ApproachAllianceHopperFromBoiler extends CommandGroup {
-	//TODO: Math (not priority)
-	private static final double TURN_TO_HOPPER = -1;
-    private static final double DRIVE_TO_HOPPER = -1;
+	private static final double TURN_TO_HOPPER = -45;
+    private static final double DRIVE_TO_HOPPER = 77;
 
     public ApproachAllianceHopperFromBoiler() {
         int direction;

src/com/stuypulse/frc2017/robot/commands/auton/ApproachHPFromBoilerGearCommand.java

@@ -12,10 +12,9 @@
  */
 public class ApproachHPFromBoilerGearCommand extends CommandGroup {
     public static final double BOILER_GEAR_TO_NEUTRAL_ZONE_ANGLE = 60;
-    public static final double BOILER_GEAR_TO_NEUTRAL_ZONE_DISTANCE = 374; // TODO: maybe subtract 12 inches
-    public static final double BOILER_GEAR_NEUTRAL_ZONE_TO_HP_ANGLE = -45;
-    //TODO: Math (priority)
-    public static final double BOILER_GEAR_NEUTRAL_ZONE_TO_HP_DISTANCE = -1;
+    public static final double BOILER_GEAR_TO_NEUTRAL_ZONE_DISTANCE = 71;
+    public static final double BOILER_GEAR_NEUTRAL_ZONE_TO_HP_ANGLE = -43;
+    public static final double BOILER_GEAR_NEUTRAL_ZONE_TO_HP_DISTANCE = 361;
 
 	public ApproachHPFromBoilerGearCommand() {
 		int direction;

src/com/stuypulse/frc2017/robot/commands/auton/ApproachHPFromHPGearCommand.java

@@ -12,7 +12,7 @@
  */
 public class ApproachHPFromHPGearCommand extends CommandGroup {
     public static final double HP_GEAR_TO_NEUTRAL_ZONE_ANGLE = -60;
-    public static final double HP_GEAR_TO_NEUTRAL_ZONE_DISTANCE = 374; // TODO: maybe subtract 12 inches here
+    public static final double HP_GEAR_TO_NEUTRAL_ZONE_DISTANCE = 362; //374- 12 inches deviation
 
 	public ApproachHPFromHPGearCommand() {
 		// Add Commands here:

src/com/stuypulse/frc2017/robot/commands/auton/ApproachHPFromMiddleGearCommand.java

@@ -11,12 +11,10 @@
  *
  */
 public class ApproachHPFromMiddleGearCommand extends CommandGroup {
-	//TODO: Math (priority)
     public static final double MIDDLE_GEAR_TO_AIRSHIP_HP_SIDE_ANGLE = -90;
-    public static final double MIDDLE_GEAR_TO_AIRSHIP_HP_SIDE_DISTANCE = -1;
+    public static final double MIDDLE_GEAR_TO_AIRSHIP_HP_SIDE_DISTANCE = 46.6;//TODO: check value
     public static final double AIRSHIP_HP_SIDE_TO_NEUTRAL_ZONE_ANGLE = 90;
-    public static final double AIRSHIP_HP_SIDE_TO_NEUTRAL_ZONE_DISTANCE = -1;
-
+    public static final double AIRSHIP_HP_SIDE_TO_NEUTRAL_ZONE_DISTANCE = 362; //374- 12 inches deviation
 	public ApproachHPFromMiddleGearCommand() {
 		int direction;
 		if (DriverStation.getInstance().getAlliance() == DriverStation.Alliance.Red) {

src/com/stuypulse/frc2017/robot/commands/auton/ApproachNeutralHopperFromBoiler.java

@@ -12,10 +12,10 @@
  */
 public class ApproachNeutralHopperFromBoiler extends CommandGroup {
 
-    double TURN_AWAY_FROM_BOILER = 45;
-    double MOVE_TOWARD_HOPPER = 206.3;
+    double TURN_AWAY_FROM_BOILER = -45;
+    double MOVE_TOWARD_HOPPER = 249.3;
     double TURN_TO_HOPPER = 90;
-    double MOVE_TO_HOPPER = 112;
+    double MOVE_TO_HOPPER = 77;
 
     public ApproachNeutralHopperFromBoiler() {
         int direction;

src/com/stuypulse/frc2017/robot/commands/auton/MiddleGearMobilityMinimalCommand.java

@@ -10,12 +10,11 @@
  *
  */
 public class MiddleGearMobilityMinimalCommand extends CommandGroup {
-    //TODO: Math (priority)
-    private double FIRST_MOBILITY_TURN_DISTANCE = -1;
-    private double FIRST_MOBILITY_TURN_ANGLE = -1;
-    private double SECOND_MOBILITY_TURN_DISTANCE = -1;
-    private double SECOND_MOBILITY_TURN_ANGLE = -1;
-    private double FINAL_MOBILITY_TURN_DISTANCE = -1;
+    private double FIRST_MOBILITY_TURN_DISTANCE = 53.3; //baseline-robot length
+    private double FIRST_MOBILITY_TURN_ANGLE = -90;
+    private double SECOND_MOBILITY_TURN_DISTANCE = 61.1;
+    private double SECOND_MOBILITY_TURN_ANGLE = 90;
+    private double FINAL_MOBILITY_TURN_DISTANCE = 52; //robot length + 12 inches deviation
 
     public  MiddleGearMobilityMinimalCommand() {
         // Add Commands here:

src/com/stuypulse/frc2017/robot/commands/auton/MobilityMinimalCommand.java

@@ -8,8 +8,8 @@
  *
  */
 public class MobilityMinimalCommand extends CommandGroup {
-	//TODO: Math (priority)
-    public static final double MOBILITY_TO_BASELINE_DISTANCE = -1;
+
+    public static final double MOBILITY_TO_BASELINE_DISTANCE = 93.3;
 
     public  MobilityMinimalCommand() {
         // Add Commands here:

src/com/stuypulse/frc2017/robot/commands/auton/MobilityToHPCommand.java

@@ -8,8 +8,7 @@
  *
  */
 public class MobilityToHPCommand extends CommandGroup {
-	//TODO: Math (priority)
-    public static final double MOBILITY_TO_NEUTRAL_ZONE_DISTANCE = -1;
+    public static final double MOBILITY_TO_NEUTRAL_ZONE_DISTANCE = 415.3;//427.3 - 12 inches deviation
 
     public MobilityToHPCommand() {
 

src/com/stuypulse/frc2017/robot/commands/auton/ScoreMiddleGearFromBoilerCommand.java

@@ -13,12 +13,11 @@
  *
  */
 public class ScoreMiddleGearFromBoilerCommand extends CommandGroup {
-    private static final double BACK_UP_FROM_BOILER_DISTANCE = -127.3;
-    private static final double TURN_AWAY_FROM_BOILER = -45;
-    //TODO: Set numbers from testing
-    private static final double MOVE_TOWARDS_LIFT = -1; // :(
+    private static final double BACK_UP_FROM_BOILER_DISTANCE = -131.9;
+    private static final double TURN_AWAY_FROM_BOILER = 135;
+    private static final double MOVE_TOWARDS_LIFT = 80.7;
     private static final double TURN_TOWARDS_LIFT = 90;
-    private static final double MOVE_TO_LIFT = -1; // :(
+    private static final double MOVE_TO_LIFT = 21;
 
     public ScoreMiddleGearFromBoilerCommand() {
         int direction;
@@ -37,7 +36,7 @@ public ScoreMiddleGearFromBoilerCommand() {
         addSequential(new DriveForwardEncodersCommand(ScoreMiddleGearCommand.MIDDLE_GEAR_REVERSE_DISTANCE));
         addSequential(new GearPusherRetractGearCommand());
         addSequential(new GearTrapTrapGearCommand());
-        
+
         // Add Commands here:
         // e.g. addSequential(new Command1());
         //      addSequential(new Command2());