/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

package storm2014.commands.autonomous;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.PrintCommand;
import edu.wpi.first.wpilibj.command.WaitCommand;
import storm2014.commands.DriveForward;
import storm2014.commands.FireCatapult;
import storm2014.commands.Launch;
import storm2014.commands.PreFire;
import storm2014.commands.SetArmPosition;
import storm2014.commands.Shift;
import storm2014.commands.control.Conditional;
import storm2014.commands.control.DoNothing;
import storm2014.subsystems.Catapult;
import storm2014.subsystems.VisionSystem;

/**
 *
 * @author Tim
 */
public class DriveAndShoot extends CommandGroup{
    
    private boolean foundHotTarget;
    
    private Command _waitAndDetect(){
        CommandGroup _waitAndDetect = new CommandGroup("Waits for hot goal to switch at start of match, then scans for hot goal");
        _waitAndDetect.addSequential(new WaitCommand(0.75));
        _waitAndDetect.addParallel(new Command() {
            protected void initialize() {
                foundHotTarget = VisionSystem.foundHotTarget();
            }
            protected void execute() {
                foundHotTarget = foundHotTarget || VisionSystem.foundHotTarget();
            }
            protected boolean isFinished() {
                return false;
            }
            protected void end() {}
            protected void interrupted() {}
        },1.25);
        return _waitAndDetect;
    }   
    
    public DriveAndShoot(){
        addParallel(_waitAndDetect());
        addSequential(new Shift(true));
        addSequential(new DriveForward(1, 4200));
        addSequential(new Conditional(new WaitCommand(.25), new WaitCommand(3)) { //may lower wait time on the waitcommand
            protected boolean condition() {
                return foundHotTarget;
            }
        });
        addSequential(new SetArmPosition(2));
        addParallel(new PreFire());
        addSequential(new WaitCommand(1.25));
        addSequential(new Launch());
    }
    
}

@@ -1163,9 +1163,9 @@
 d(0.
-7
+2
 5));
@@ -1652,17 +1652,17 @@
     %7D,1.
-2
+7
 5);%0A    
@@ -1805,24 +1805,70 @@
 ift(true));%0A
+        addSequential(new WaitCommand(0.25));%0A
         addS
@@ -1960,18 +1960,18 @@
 ommand(.
-25
+01
 ), new W
@@ -2178,17 +2178,17 @@
 osition(
-2
+1
 ));%0A    
@@ -2215,24 +2215,70 @@
 PreFire());%0A
+        addSequential(new SetArmPosition(2));%0A
         addS
@@ -2307,11 +2307,11 @@
 and(
-1.2
+0.7
 5));

Tweak Hot auto