package com.team254.frc2013.auto;

import com.team254.frc2013.commands.DriveAtSpeedCommand;
import com.team254.frc2013.commands.DriveProfiledCommand;
import com.team254.frc2013.commands.IndexerDownCommand;
import com.team254.frc2013.commands.ResetDriveEncodersCommand;
import com.team254.frc2013.commands.ResetGyroCommand;
import com.team254.frc2013.commands.RunIntakeCommand;
import com.team254.frc2013.commands.ShiftCommand;
import com.team254.frc2013.commands.ShootSequenceCommand;
import com.team254.frc2013.commands.ShooterOnCommand;
import com.team254.frc2013.commands.ShooterPresetCommand;
import com.team254.frc2013.commands.WaitCommand;
import com.team254.frc2013.subsystems.Shooter;
import com.team254.lib.control.impl.CustomProfile;
import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 * Scores three starting discs from the back of the pyramid, picks up four more, then scores them
 * from the front of the pyramid.

 * @author tom@team254.com (Tom Bottiglieri)
 * @author pat@team254.com (Patrick Fairbank)
 */
public class SevenDiscAutoMode extends CommandGroup {
  CustomProfile profile = new CustomProfile();

  public SevenDiscAutoMode() {
    /*
    addSequential(new ShooterOnCommand(true));

    addSequential(new IntakeRaiseCommand(IntakeRaiseCommand.INTAKE_DOWN));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new WaitCommand(0.2));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());

*/
    profile.addWaypoint(0, 1.0 * 12);
    profile.addWaypoint(1 * 12, 2.0 * 12);
    profile.addWaypoint(2 * 12, 2.0 * 12);
    profile.addWaypoint(3 * 12, 5.5 * 12);
    profile.addWaypoint(5.5 * 12, 5.5 * 12);
    profile.addWaypoint(6.5 * 12, 1.0 * 12);
    profile.addWaypoint(9.5 * 12, 1.0 * 12);
addSequential(new ShooterOnCommand(true));
addSequential(new IndexerDownCommand());
    addSequential(new ShiftCommand(false));
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new RunIntakeCommand(1));
    addSequential(new DriveAtSpeedCommand(10.5, 2.5, 0, 6));
    addSequential(new WaitCommand(.5));
    //addSequential(new ShooterPresetCommand(Shooter.PRESET_FRONT_PYRAMID));
    addSequential(new DriveProfiledCommand(6, 5, 2.3));

    addSequential(new RunIntakeCommand(0.0));
        addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
    /*
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    * */
    /*
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new DriveAtSpeedCommand(2.5, 2, 0, 20));
    //addSequential(new TurnAngleCommand(90, 5));
//    addSequential(new ShooterPresetCommand(Shooter.PRESET_FRONT_PYRAMID));
    addSequential(new DriveAtSpeedCommand(7, 6, 0, 5));
//    addSequential(new AutonIndexerCommand());
//    addSequential(new RunIntakeCommand(0.8));
    addSequential(new DriveAtSpeedCommand(9.5, 1.5, 0, 5));
//    addSequential(new RunIntakeCommand(0.8));
    addSequential(new DriveProfiledCommand(6, 6, 2.3));
    addSequential(new WaitCommand(3));
    addSequential(new DriveProfiledCommand(-5, 10, 5));
//    addSequential(new RunIntakeCommand(0.6));
//    addSequential(new ShootCommand());
//    addSequential(new LoadAndShootCommand());
//    addSequential(new LoadAndShootCommand());
//    addSequential(new LoadAndShootCommand());
**/
    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
 }
}

@@ -1571,17 +1571,17 @@
 oint(0, 
-1
+2
 .0 * 12)
@@ -1610,52 +1610,11 @@
 int(
-1 * 12, 2.0 * 12);%0A    profile.addWaypoint(2
+2.5
  * 1
@@ -1655,25 +1655,25 @@
 int(3 * 12, 
-5
+4
 .5 * 12);%0A  
@@ -1694,17 +1694,17 @@
 aypoint(
-5
+6
 .5 * 12,
@@ -1704,17 +1704,17 @@
 5 * 12, 
-5
+4
 .5 * 12)
@@ -1731,33 +1731,33 @@
 ile.addWaypoint(
-6
+7
 .5 * 12, 1.0 * 1
@@ -1741,35 +1741,33 @@
 point(7.5 * 12, 
-1.0
+2
  * 12);%0A    prof
@@ -1782,18 +1782,20 @@
 aypoint(
-9.
+10.7
 5 * 12, 
@@ -1796,25 +1796,25 @@
 * 12, 1.
-0
+5
  * 12);%0A
 addSeque
@@ -1805,16 +1805,20 @@
  * 12);%0A
+    
 addSeque
@@ -1852,16 +1852,20 @@
 true));%0A
+    
 addSeque
@@ -2076,24 +2076,31 @@
 ommand(1));%0A
+    /*%0A
     addSeque
@@ -2133,23 +2133,218 @@
 and(
-10.5, 2.
+3.0, 2.0, 0, 3));%0A    addSequential(new DriveAtSpeedCommand(7.5, 4.0, 0, 2));%0A    addSequential(new DriveAtSpeedCommand(10, 2.0, 0, 6));%0A    * */%0A    addSequential(new DriveProfiledCommand(10.2
 5, 
+1
 0, 6
+, profile
 ));%0A
@@ -2452,32 +2452,33 @@
 RONT_PYRAMID));%0A
+%0A
     addSequentia

Modify 7 disc