diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java new file mode 100644 index 0000000..565835f --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4388.robot.commands.auton; + +import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; +import org.usfirst.frc4388.robot.commands.DriveGyroReset; +import org.usfirst.frc4388.robot.commands.DriveSpeedShift; +import org.usfirst.frc4388.robot.commands.DriveStraightBasic; +import org.usfirst.frc4388.robot.commands.DriveStraightMP; +import org.usfirst.frc4388.robot.commands.DriveTurnBasic; +import org.usfirst.frc4388.robot.commands.ElevatorBasic; +import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed; +import org.usfirst.frc4388.robot.commands.IntakePosition; +import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; +import org.usfirst.frc4388.robot.subsystems.Carriage; +import org.usfirst.frc4388.robot.subsystems.Drive; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * + */ +public class CenterRight extends CommandGroup { + + public CenterRight() + { + addSequential(new DriveGyroReset()); + + addSequential(new IntakePosition(true)); + addSequential(new DriveStraightBasic(-10, 60, true, true, 0)); + addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(50, 60, true, true, 0)); + addSequential(new DriveTurnBasic(true, 28, 300, MPSoftwareTurnType.TANK)); + addSequential(new ElevatorBasic(20)); + addSequential(new DriveStraightBasic(25, 60, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + //addSequential(new WaitCommand(.1)); + addSequential(new IntakePosition(false)); + addSequential(new WaitCommand(.5)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java new file mode 100644 index 0000000..62713df --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java @@ -0,0 +1,46 @@ +package org.usfirst.frc4388.robot.commands.auton; + +import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; +import org.usfirst.frc4388.robot.commands.DriveGyroReset; +import org.usfirst.frc4388.robot.commands.DriveSpeedShift; +import org.usfirst.frc4388.robot.commands.DriveStraightBasic; +import org.usfirst.frc4388.robot.commands.DriveStraightMP; +import org.usfirst.frc4388.robot.commands.DriveTurnBasic; +import org.usfirst.frc4388.robot.commands.ElevatorBasic; +import org.usfirst.frc4388.robot.commands.IntakePosition; +import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; +import org.usfirst.frc4388.robot.subsystems.Carriage; +import org.usfirst.frc4388.robot.subsystems.Drive; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * + */ +public class LeftStartRightScore extends CommandGroup { + + public LeftStartRightScore() + { + addSequential(new DriveGyroReset()); + addSequential(new DriveSpeedShift(true)); + addSequential(new DriveSpeedShift(true)); + addSequential(new IntakePosition(true)); + addSequential(new DriveStraightBasic(215, 60, true, true, 0)); + addSequential(new ElevatorBasic(5)); + addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(205, 60, true, true, 0)); + addSequential(new ElevatorBasic(30)); + addSequential(new DriveStraightBasic(30, 60, true, true, 0)); + addSequential(new DriveTurnBasic(true, 125, 300, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(30, 60, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + //addSequential(new WaitCommand(.1)); + addSequential(new IntakePosition(false)); + addSequential(new WaitCommand(.2)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-10, 60, true, true, 0)); + //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java index ebc984e..e53b19d 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java @@ -21,8 +21,8 @@ import edu.wpi.first.wpilibj.command.WaitCommand; */ public class RightStartLeftScore extends CommandGroup { - public RightStartLeftScore() { - + public RightStartLeftScore() //////////TUNEDISH AND ALMOST GOOD + { addSequential(new DriveGyroReset()); addSequential(new DriveSpeedShift(true)); addSequential(new DriveSpeedShift(true)); @@ -30,10 +30,11 @@ public class RightStartLeftScore extends CommandGroup { addSequential(new DriveStraightBasic(-215, 60, true, true, 0)); addSequential(new ElevatorBasic(5)); addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(150, 60, true, true, 0)); + addSequential(new DriveStraightBasic(205, 60, true, true, 0)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(3, 60, true, true, 0)); + addSequential(new DriveStraightBasic(30, 60, true, true, 0)); + addSequential(new DriveTurnBasic(true, -125, 300, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(30, 60, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); @@ -41,6 +42,5 @@ public class RightStartLeftScore extends CommandGroup { addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); addSequential(new DriveStraightBasic(-10, 60, true, true, 0)); //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville - } }