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

import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.WaitCommand;

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

    public static final double FENDER_DEPTH = 38.5;

    public static final double CONVEY_AUTO_TIME = 4;



    //TODO: update with testing
    //Time to delay is (distance to travel)/rate + 0.2
    // 0.2 is a guess for the amount of time to reach max speed
    
    // distance is ~50" or ~4', rate is ~10 ft/s. Time is 0.4s, Your Mileage May Vary
    public static final double t_farKeyToBridge = 0.6;

    // distance is ~136" or ~12', rate is 10 ft/s. Time is 1.2s, Your Mileage May Vary
    public static final double t_closeKeyToBridge = 1.4;


    public Autonomous() {
        addSequential(new DrivetrainSetGear(true));
        addSequential(new WaitCommand(CommandBase.oi.getDelayTime()));
        switch (CommandBase.oi.getAutonSetting()) {
            case 0:
                addSequential(new AutonSetting0());
                break;
            case 1:
                addSequential(new AutonSetting1());
                break;
            case 2:
                addSequential(new AutonSetting2());
                break;
            case 3:
                addSequential(new AutonSetting3());
                break;
            case 4:
                addSequential(new AutonSetting4());
                break;
            case 5:
                addSequential(new AutonSetting5());
                break;
            default:
                break;
        }
    }
}

@@ -468,16 +468,17 @@
 elay is 
+%5B
 (distanc
@@ -500,16 +500,24 @@
 te + 0.2
+%5D * 1.25
 %0A    // 
@@ -573,16 +573,153 @@
 x speed%0A
+    // 1.25:  add 25%25 more time to account for friction%0A    //    drive train speed is based on free speed; friction will make it slower%0A
     %0A   
@@ -853,17 +853,18 @@
 dge = 0.
-6
+75
 ;%0A%0A    /
@@ -999,17 +999,18 @@
 dge = 1.
-4
+75
 ;%0A%0A%0A    

Increase back-up time by 25% to account for higher load on drivetrain