/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.stuy.commands;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 * @author Kevin Wang
 */
public class Autonomous extends CommandBase {

    /* For inches to fender */
    private static final int DIST_FENDER_TO_KEY = 110;
    private static final double BUMPER_EDGE_TO_WHEEL_CENTER = 9.5;
    private static final int BOT_LENGTH_WITH_BUMPERS = 44;
    private static final double DIST_ALLIANCESTATION_TO_BRIDGE = 280.4;
    public static final double FENDER_DEPTH = 38.5;
    private static final double TOLERANCE = 2;
    public static final double INCHES_TO_FENDER = DIST_FENDER_TO_KEY - (BOT_LENGTH_WITH_BUMPERS - BUMPER_EDGE_TO_WHEEL_CENTER);

    /* For inches to bridge from fender */
    public static final double INCHES_TO_BRIDGE = DIST_ALLIANCESTATION_TO_BRIDGE - FENDER_DEPTH - BOT_LENGTH_WITH_BUMPERS -TOLERANCE;
    
    public static final double INCHES_FROM_EDGE_TO_SONAR = 13;
    
    public static final double RAMPING_DISTANCE = 5;
    public static final double RAMPING_CONSTANT = 10/9;
    
    public Autonomous() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        new DrivetrainSetGear(true).start();
        Command autonCommand;
        switch(CommandBase.oi.getAutonSetting()){
            case 0:
                autonCommand = new AutonSetting1();
                break;
            case 1:
                autonCommand = new AutonSetting2();
                break;
            case 2:
                autonCommand = new AutonSetting3();
                break;
            case 3:
                autonCommand = new AutonSetting4();
                break;
            case 4:
                autonCommand = new AutonSetting5();
                break;
            default:
                autonCommand = new AutonSetting6();
                break;
        }
        autonCommand.start();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}

@@ -165,16 +165,59 @@
 .Command
+Group;%0Aimport edu.stuy.commands.CommandBase
 ;%0A%0A/**%0A 
@@ -283,20 +283,21 @@
  Command
-Base
+Group
  %7B%0A%0A    
@@ -1163,20 +1163,16 @@
 = 10/9;%0A
-    
 %0A    pub
@@ -1202,207 +1202,22 @@
     
-// Use requires() here to declare subsystem dependencies%0A        // eg. requires(chassis);%0A    %7D%0A%0A    // Called just before this Command runs the first time%0A    protected void initialize() %7B%0A        
+addSequential(
 new 
@@ -1243,48 +1243,11 @@
 rue)
-.start(
 );%0A
-        Command autonCommand;%0A
     
@@ -1325,38 +1325,37 @@
            a
-utonCommand = 
+ddSequential(
 new AutonSet
@@ -1361,16 +1361,17 @@
 tting1()
+)
 ;%0A      
@@ -1420,38 +1420,37 @@
            a
-utonCommand = 
+ddSequential(
 new AutonSet
@@ -1456,16 +1456,17 @@
 tting2()
+)
 ;%0A      
@@ -1515,38 +1515,37 @@
            a
-utonCommand = 
+ddSequential(
 new AutonSet
@@ -1551,16 +1551,17 @@
 tting3()
+)
 ;%0A      
@@ -1610,38 +1610,37 @@
            a
-utonCommand = 
+ddSequential(
 new AutonSet
@@ -1646,16 +1646,17 @@
 tting4()
+)
 ;%0A      
@@ -1705,38 +1705,37 @@
            a
-utonCommand = 
+ddSequential(
 new AutonSet
@@ -1741,16 +1741,17 @@
 tting5()
+)
 ;%0A      
@@ -1809,22 +1809,21 @@
    a
-utonCommand = 
+ddSequential(
 new 
@@ -1841,556 +1841,43 @@
 g6()
-;%0A                break;%0A        %7D%0A        autonCommand.start(
 );%0A    
-%7D%0A%0A
     
-// Called repeatedly when this Command is scheduled to run%0A    protected void execute() %7B%0A    %7D%0A%0A    // Make this return true when this Command no longer needs to run execute()%0A    protected boolean isFinished() %7B%0A        return false;%0A    %7D%0A%0A    // Called once after isFinished returns true%0A    protected void end() %7B%0A    %7D%0A%0A    // Called when another command which requires one or more of the same%0A    // subsystems is scheduled to run%0A    protected void interrupted() %7B
+        break;%0A        %7D
 %0A   

Revert "Change Autonomous command to check for the auton setting only when auton begins."