AutonomousModeFastOneBall::AutonomousModeFastOneBall()
{
  AddSequential(new AutonomousLowGearCommand());
  AddParallel(new OperatorHighCommand());
  AddSequential(new AutonomousDriveCommand(1.0f, TURN_CORRECTION, 2.2));
  AddSequential(new DriveLaunchReleaseCommand());
}

AutonCheval::AutonCheval() : CommandGroup("AutonCheval")
{

	// Add Commands here:
	// e.g. AddSequential(new Command1());
	//      AddSequential(new Command2());
	// these will run in order.

	// To run multiple commands at the same time,
	// use AddParallel()
	// e.g. AddParallel(new Command1());
	//
	//AddSequential(new Command2());
	// Command1 and Command2 will run in parallel.

	// A command group will require all of the subsystems that each member
	// would require.
	// e.g. if Command1 requires chassis, and Command2 requires arm,
	// a CommandGroup containing them would require both the chassis and the
	// arm.
	AddSequential(new DriveForwardReach());
	AddSequential(new ArmLowerLowGoal());
	AddSequential(new ArmLowerLowGoal());
	AddSequential(new DriveWait());
	AddSequential(new DriveForwardReach());
	//AddSequential(new ArmRaise());

}

LowNoShoot::LowNoShoot()
{
	// Add Commands here:
	// e.g. AddSequential(new Command1());
	//      AddSequential(new Command2());
	// these will run in order.

	// To run multiple commands at the same time,
	// use AddParallel()
	// e.g. AddParallel(new Command1());
	//      AddSequential(new Command2());
	// Command1 and Command2 will run in parallel.

	// A command group will require all of the subsystems that each member
	// would require.
	// e.g. if Command1 requires chassis, and Command2 requires arm,
	// a CommandGroup containing them would require both the chassis and the
	// arm.
	AddSequential(new ResetGyro());
	AddSequential(new FoldIntakeOut(), 1);
	AddSequential(new TurnForAngle(.3, 0), .5);
	AddSequential(new DriveForTime(.45,0),3.75);
//	AddSequential(new AutoRollerIn(), 1);
	//AddSequential(new IntakeCommand(), .1);
	AddSequential(new DriveUntilClose(.4,94));
}

MoveRocketLevel2::MoveRocketLevel2() {
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
     // Add Commands here:
    // e.g. AddSequential(new Command1());
    //      AddSequential(new Command2());
    // these will run in order.

    // To run multiple commands at the same time,
    // use AddParallel()
    // e.g. AddParallel(new Command1());
    //      AddSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
              // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
        AddParallel(new ELRun(3));
        AddSequential(new WRRun(3));
        AddSequential(new EBRun(3));
              // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
        std::printf("2135: Moved to Rocket Level 2 Position\n");
 }

AlignVerticalAndHorizontal::AlignVerticalAndHorizontal(float angle)
{
	AddSequential(new GoalAlignVertical(101));
	AddSequential(new GoalAlign(angle));
	//AddSequential(new DriveCommandAuto(0,0,0,.5,0));

}

IntakePositionSequence::IntakePositionSequence(TurretState state, ReleaseState ball) {
	AddSequential(new SetArmPosition(c_armIntakePosition));
	AddParallel(new AutoIntake());
	AddParallel(new SetTurretCylinder(state));
	AddSequential(new BallRelease(ball));
	//trigger stuff for low bar. sensors are not on robot. cannot yet happen.
}

LiftContainerAuto::LiftContainerAuto() {
	AddParallel(new CloseClaw());
	AddParallel(new ResetToteNew());
	AddSequential(new ResetClaw());
	AddSequential(new CloseClaw());
	AddSequential(new ContainerPositionChange(5));
}

LowShot::LowShot() {
  AddParallel(new PositionDrive(40));
  AddSequential(new SetWinchPosition(1, true, 2));

  AddParallel(new LowGoalShoot(2));
  AddParallel(new Feed(2));
  AddSequential(new SetWheelsTwist(2));

  AddParallel(new SetWinchPosition(2, false, 4));
  AddSequential(new StopShoot());
  // Add Commands here:
  // e.g. AddSequential(new Command1());
  //      AddSequential(new Command2());
  // these will run in order.

  // To run multiple commands at the same time,
  // use AddParallel()
  // e.g. AddParallel(new Command1());
  //      AddSequential(new Command2());
  // Command1 and Command2 will run in parallel.

  // A command group will require all of the subsystems that each member
  // would require.
  // e.g. if Command1 requires chassis, and Command2 requires arm,
  // a CommandGroup containing them would require both the chassis and the
  // arm.
}

DriveUnderPort::DriveUnderPort()
{


	AddSequential(new DriveSetDistance(100));
	AddSequential(new lowerEverything());
	AddSequential(new DriveSetDistance(25));
	AddSequential(new LiftLoader());
	AddSequential(new DriveSetDistance(100));


	// Add Commands here:
	// e.g. AddSequential(new Command1());
	//      AddSequential(new Command2());
	// these will run in order.

	// To run multiple commands at the same time,
	// use AddParallel()
	// e.g. AddParallel(new Command1());
	//      AddSequential(new Command2());
	// Command1 and Command2 will run in parallel.

	// A command group will require all of the subsystems that each member
	// would require.
	// e.g. if Command1 requires chassis, and Command2 requires arm,
	// a CommandGroup containing them would require both the chassis and the
	// arm.
}

AutoPos1Level2Ship::AutoPos1Level2Ship() {
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
     // Add Commands here:
    // e.g. AddSequential(new Command1());
    //      AddSequential(new Command2());
    // these will run in order.

    // To run multiple commands at the same time,
    // use AddParallel()
    // e.g. AddParallel(new Command1());
    //      AddSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
              // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
                AddParallel(new EBBump(false));
        AddParallel(new ELRun(0));
        AddParallel(new WRRun(0));
        AddSequential(new AutoDriveDist(0));
        AddSequential(new AutoDriveTurn(0));
        AddParallel(new INDelivery(false));
        AddSequential(new AutoStop());
              // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
 }

CollectMode::CollectMode() : CommandGroup("CollectMode")
{
    AddParallel(new StopShooter());
    AddSequential(new LowerInjectorAndOpenFingers());
    AddSequential(new StartCollector());
    AddSequential(new LowerBridge());
}

AutoPosANYMove::AutoPosANYMove() {
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
     // Add Commands here:
    // e.g. AddSequential(new Command1());
    //      AddSequential(new Command2());
    // these will run in order.

    // To run multiple commands at the same time,
    // use AddParallel()
    // e.g. AddParallel(new Command1());
    //      AddSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
	frc2135::RobotConfig* config = frc2135::RobotConfig::GetInstance();
	double	cmdDistance;

	config->GetValueAsDouble("AutoPosANYMove", cmdDistance, 97.25);
	std::printf("2135: Auto Pos ANY Move - Init %5.3f inches\n", cmdDistance);

              // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
                AddSequential(new AutoDriveDist(cmdDistance));
			AddSequential(new AutoStop());
              // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
 }

AutonomousPlayLeft::AutonomousPlayLeft() {
    // Add Commands here:
    // e.g. AddSequential(new Command1());
    //      AddSequential(new Command2());
    // these will run in order.

    AddParallel(new CameraUp(true));
    AddSequential(new ShootNow(true, 1.5,false));
    AddSequential(new DriveDistance(false,-16,125,false));
    //AddSequential(new WaitTime(1.0));
    AddSequential(new DriveDistance(true,85,125,false));
    //AddSequential(new WaitTime(1.0));
    AddParallel(new ShootNow(true, 10.0,true));
    AddSequential(new DriveDistance(false,18,125,false));
    AddSequential(new CameraUp(false));
    // To run multiple commands at the same time,
    // use AddParallel()
    // e.g. AddParallel(new Command1());
    //      AddSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
}

AutonomousDriveLowBarShootLow::AutonomousDriveLowBarShootLow() {
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
     // Add Commands here:
    // e.g. AddSequential(new Command1());
    //      AddSequential(new Command2());
    // these will run in order.

    // To run multiple commands at the same time,
    // use AddParallel()
    // e.g. AddParallel(new Command1());
    //      AddSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
              // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
                      // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
	AddSequential (new ShooterInitialize);
	AddSequential (new AutonomousMotionProfile(&AutonomousDrivePastLowBar_60ips));
	AddSequential (new AutonomousMotionProfile(&Rotate60Clockwise,&Rotate60Clockwise));
	AddSequential (new AutonomousFireBoulderLow());


 }

AutonomousCommand3::AutonomousCommand3() {
	//Working 1 bin autonomous; don't delete
	//This commented out code makes the robot move a recycling bin to the auto zone, uncomment if useful.
	/*
	AddParallel(new DriveForward250());
	AddSequential(new RollersIn());
	AddParallel(new DriveForward250());
	AddSequential(new ElevatorUp());
	AddSequential(new Turn90CounterClockwise());
	AddSequential(new DriveForward750());
	*/

	//This is REALLY tentative.
	//TODO Lot of testing
	/*
	AddParallel(new RollersIn());
	AddSequential(new DriveForward250());
	AddSequential(new Turn90CounterClockwise());
	AddSequential(new DriveForward250());
	*/

	// This code has the robot start facing the driver's station and picks up a can and drives backward into the auto zone.
	AddParallel(new DriveForward250());
	AddSequential(new RollersIn());
	AddSequential(new DriveForward100());
	AddSequential(new ElevatorUp());
	AddSequential(new DriveBackward1000());
	//AddSequential(new DriveForward250());
}

Launch::Launch(double speed)
{
	// Add Commands here:
	// e.g. AddSequential(new Command1());
	//      AddSequential(new Command2());
	// these will run in order.

	// To run multiple commands at the same time,
	// use AddParallel()
	// e.g. AddParallel(new Command1());
	//      AddSequential(new Command2());
	// Command1 and Command2 will run in parallel.

	// A command group will require all of the subsystems that each member
	// would require.
	// e.g. if Command1 requires chassis, and Command2 requires arm,
	// a CommandGroup containing them would require both the chassis and the
	// arm.
//	AddParallel(new FoldIntakeIn(), 1.5);
	AddSequential(new SpinUp(speed), 1.5);
//	AddSequential(new PrepLaunch(speed), 1.5);
	//AddSequential(new ReverseIntakeCommand(),1);
	AddSequential(new ServoExtend(), 0.5);
//	AddSequential(new WaitCommand(1));
	AddSequential(new StopSpeed());
	AddSequential(new ServoRetract(), 0.5);
}

MoveToDefense::MoveToDefense()
{
	log = Log::getInstance();
	log->write(Log::TRACE_LEVEL, "Moving to Defense");
	AddSequential(new AngleIntake(90.0));
	AddSequential(new DriveStraight(1.0, DriveStraight::SensorType::GYRO, 3.1)); //temporary time
}

MoveAround::MoveAround()
{
    AddSequential(new ChangeHeading(360));
    AddSequential(new ChangeDistance(36));
    AddSequential(new ChangeHeading(180));
    AddSequential(new ChangeDistance(36));
    AddSequential(new ChangeHeading(180));
}

void AutonomousCommand::Moat() {
    AddParallel(new Intake_Pos(-125865));//set intake to horizontal position while intaking pre-load ball
    AddSequential(new Roller_In(0.75, 0.5));//Intake preload ball
    AddParallel(new Roller_Stop());
    AddSequential(new AutonomousDrive(-1,0,2.5));//Drive Forward, full speed over defense
    AddSequential(new AutonomousDrive(0,0,0.5));//stop after 2.5 seconds
    AddSequential(new Intake_Pos(-24661));
}

Auto::Auto()
{
	AddParallel(new SetShooterMode<ShooterOn>());
	AddSequential(new AutonomousDrive<DriveStraight>(6, 0.8, 4500, 0)); // was 5200, before that 4100
	AddSequential(new AutonomousDrive<DriveTurn>(2, 0.8, 0, 33.0));
	AddSequential(new AutonomousDrive<DriveStraight>(3, 0.8, 450, 0));
	AddSequential(new ShootBall());
}