/*
 * 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.CommandGroup;
import edu.wpi.first.wpilibj.command.WaitCommand;
import storm2014.commands.DriveForward;
import storm2014.commands.LaunchWhenReady;
import storm2014.commands.SetArmPosition;
import storm2014.commands.SpinRoller;

/**
 *
 * @author Tim
 */
public class DriveAndShoot2Ball extends CommandGroup{
    
    public DriveAndShoot2Ball(){
        addSequential(new DriveForward(0.75, 4700));
        addSequential(new SetArmPosition(2));
        addSequential(new WaitCommand(0.4));
        addSequential(new LaunchWhenReady());
        addSequential(new SetArmPosition(0));
        addSequential(new WaitCommand(0.4));
        addSequential(new SpinRoller(1));
        addSequential(new SetArmPosition(1));
        addSequential(new WaitCommand(0.4));
        addSequential(new SetArmPosition(2));
        addSequential(new WaitCommand(0.4));
        addSequential(new LaunchWhenReady());
        
    }
    
}

@@ -263,21 +263,118 @@
 .Command
-Group
+;%0Aimport edu.wpi.first.wpilibj.command.CommandGroup;%0Aimport edu.wpi.first.wpilibj.command.PrintCommand
 ;%0Aimport
@@ -573,146 +573,362 @@
 ds.S
-pinRoller;%0A%0A/**%0A *%0A * @author Tim%0A */%0Apublic class DriveAndShoot2Ball extends CommandGroup%7B%0A    %0A    public DriveAndShoot2Ball()%7B%0A        
+etWinchPreset;%0Aimport storm2014.commands.Shift;%0Aimport storm2014.commands.SpinRoller;%0Aimport storm2014.subsystems.Catapult;%0A%0A/**%0A *%0A * @author Tim%0A */%0Apublic class DriveAndShoot2Ball extends CommandGroup%7B%0A    %0A    private Command _driveShoot() %7B%0A        CommandGroup driveShoot = new CommandGroup(%22Drive & shoot for 2 ball%22);%0A        %0A        driveShoot.
 addS
@@ -958,16 +958,13 @@
 ard(
-0.75, 47
+1, 15
 00))
@@ -965,32 +965,43 @@
 1500));%0A        
+driveShoot.
 addSequential(ne
@@ -1022,32 +1022,43 @@
 on(2));%0A        
+driveShoot.
 addSequential(ne
@@ -1063,35 +1063,33 @@
 new WaitCommand(
-0.4
+1
 ));%0A        addS
@@ -1076,32 +1076,43 @@
 nd(1));%0A        
+driveShoot.
 addSequential(ne
@@ -1113,32 +1113,111 @@
 ial(new 
-LaunchWhenReady(
+SetWinchPreset(Catapult.WINCH_FAR));%0A        driveShoot.addSequential(new PrintCommand(%22Firing%22
 ));%0A    
@@ -1212,32 +1212,43 @@
 ing%22));%0A        
+driveShoot.
 addSequential(ne
@@ -1253,72 +1253,204 @@
 new 
-SetArmPosition(0));%0A        addSequential(new WaitCommand(0.4));
+LaunchWhenReady());%0A        %0A        return driveShoot;%0A    %7D%0A    %0A    public DriveAndShoot2Ball() %7B%0A        addSequential(new Shift(true));%0A        %0A        addSequential(_driveShoot());%0A        
 %0A   
@@ -1529,17 +1529,68 @@
 osition(
-1
+2));%0A        addSequential(new DriveForward(-1, 1600
 ));%0A    
@@ -1615,35 +1615,33 @@
 new WaitCommand(
-0.4
+1
 ));%0A        addS
@@ -1651,39 +1651,35 @@
 ential(new S
-etArmPosition(2
+pinRoller(0
 ));%0A        
@@ -1700,23 +1700,24 @@
 new 
-WaitCommand(0.4
+SetArmPosition(0
 ));%0A
@@ -1746,36 +1746,79 @@
 new 
-LaunchWhenReady());%0A        
+DriveForward(1, 100));%0A        %0A        //addSequential(_driveShoot());
 %0A   

Tweak 2 ball auto