diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java index a6a5829..f9a94d6 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java @@ -33,13 +33,22 @@ public class CenterLeft extends CommandGroup { addSequential(new WaitCommand(.2)); addSequential(new DriveStraightBasic(80, 45, true, true, 0)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveTurnBasic(true, 17, 150, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(10, 45, true, true, 0)); + addSequential(new DriveTurnBasic(true, 12, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(12, 45, true, true, 0)); + //addParallel(new TimeoutBecaseYea()); + //addSequential(new WaitCommand(3)); + + + 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(-20, 45, true, true, 0)); addSequential(new ElevatorBasic(10)); addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java index 8481821..5e86855 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java @@ -34,12 +34,24 @@ public class CenterRight extends CommandGroup { addSequential(new DriveStraightBasic(80, 45, true, true, 0)); addSequential(new ElevatorBasic(25)); addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK)); + addParallel(new TimeoutBecaseYea()); addSequential(new DriveStraightBasic(25, 45, true, true, 0)); + addSequential(new WaitCommand(3)); + + + /* 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(-25, 45, true, true, 0)); addSequential(new ElevatorBasic(10)); addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java b/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java new file mode 100644 index 0000000..6fe0160 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java @@ -0,0 +1,67 @@ +package org.usfirst.frc4388.robot.commands.auton; + + +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.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.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * + */ +public class Cube2Right extends CommandGroup { + + public Cube2Right() { + addSequential(new DriveGyroReset()); + addSequential(new DriveSpeedShift(true)); + addSequential(new IntakePosition(true)); + + + addSequential(new DriveStraightBasic(-140, 50, true, true, 0)); + addSequential(new ElevatorBasic(30)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK)); + addSequential(new ElevatorBasic(30)); + addSequential(new DriveStraightBasic(8, 50, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + //addSequential(new WaitCommand(.1)); + addSequential(new IntakePosition(false)); + addSequential(new WaitCommand(.4)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-10, 50, true, true, 0)); + addSequential(new DriveTurnBasic(true, 40, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(-30, 50, true, true, 0)); + addSequential(new DriveTurnBasic(true, -10, 150, MPSoftwareTurnType.TANK)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); + addSequential(new DriveStraightBasic(30, 50, true, true, 0)); + addSequential(new IntakePosition(true)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-30, 50, true, true, 0)); + addSequential(new DriveTurnBasic(true, 30, 150, MPSoftwareTurnType.TANK)); + addSequential(new ElevatorBasic(70)); + addSequential(new DriveStraightBasic(-20, 20, true, true, 0)); + addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK)); + 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)); + + + + addSequential(new DriveSpeedShift(false)); + + + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java index 71a7c8b..d85eea3 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java @@ -42,7 +42,6 @@ public class RightSwitchAuton extends CommandGroup { addSequential(new DriveStraightBasic(-20, 50, true, true, 0)); addSequential(new DriveSpeedShift(false)); - //addSequential(new DriveStraightMP(50, 10, true, true, 0)); // 95 for 112" greenville - + } } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java index 47030f4..f4c03f4 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java @@ -28,10 +28,14 @@ public class ScaleFrom1 extends CommandGroup { addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-290, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-10, 45, true, true, 0)); + + addSequential(new DriveStraightBasic(-270, 45, true, true, 0)); addSequential(new ElevatorBasic(70)); + addSequential(new DriveStraightBasic(-30, 20, true, true, 0)); addSequential(new DriveTurnBasic(true, -90, 100, MPSoftwareTurnType.TANK)); //addSequential(new DriveStraightBasic(5, 20, true, true, 0)); + addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); @@ -42,7 +46,7 @@ public class ScaleFrom1 extends CommandGroup { addSequential(new DriveSpeedShift(false)); - //addSequential(new DriveStraightMP(50, 10, true, true, 0)); // 95 for 112" greenville + //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville } -} \ No newline at end of file +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java index a57c377..8ae5417 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java @@ -29,7 +29,7 @@ public class ScaleFrom3 extends CommandGroup { addSequential(new DriveStraightBasic(-10, 45, true, true, 0)); - //addSequential(new DriveTurnBasic(true, -2, 100, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(-270, 45, true, true, 0)); addSequential(new ElevatorBasic(70)); addSequential(new DriveStraightBasic(-30, 20, true, true, 0)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java b/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java new file mode 100644 index 0000000..ab260b5 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.robot.commands.auton; + + +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.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.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * + */ +public class TimeoutBecaseYea extends CommandGroup { + + public TimeoutBecaseYea() + { + addSequential(new WaitCommand(2)); + 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)); + + } +} \ No newline at end of file