diff --git a/src/org/team708/robot/AutoConstants.java b/src/org/team708/robot/AutoConstants.java index a49e6a7..27439ae 100644 --- a/src/org/team708/robot/AutoConstants.java +++ b/src/org/team708/robot/AutoConstants.java @@ -7,11 +7,12 @@ public final class AutoConstants { */ public static final double NINETY_DEGREE_TURN = 85.0; public static final double TURN_SPEED = 0.6; - public static final double CLAW_LENGTH = 18.0; // The length, in inches, of the claw + public static final double CLAW_LENGTH = 22.0; // The length, in inches, of the claw public static final double TOTE_TO_AUTOZONE_DISTANCE = 84.0; // inches public static final double ENCODER_SPEED = 0.45; public static final double INDEXER_UP_DISTANCE = 13.0; public static final double INTAKE_ONE_TOTE_TIME = 3.0; + public static final double FIELD_WIDTH = 310; /* * Container To Autozone @@ -33,8 +34,8 @@ public final class AutoConstants { * Container Tote Spin Tote */ public static final double TOTE_DISTANCE_ONE = 21.0; - public static final double TOTE_TWO_TURN_ANGLE = 159.0; - public static final double TOTE_DISTANCE_SECOND = 58.0; + public static final double TOTE_TWO_TURN_ANGLE = 152.0; + public static final double TOTE_DISTANCE_SECOND = 53.0; /* * Container Tote Tote @@ -50,7 +51,9 @@ public final class AutoConstants { /* * Three Totes */ - public static final double THREE_TOTE_DISTANCE = 36.0; + public static final double THREE_TOTE_CONTAINER_DISTANCE = 12.0; + public static final double THREE_TOTE_TOTE_DISTANCE = 24.0; + public static final double THREE_TOTE_SMACK_ANGLE = 60.0; /* * Deal With Container diff --git a/src/org/team708/robot/Constants.java b/src/org/team708/robot/Constants.java index c12aa84..b3488fe 100644 --- a/src/org/team708/robot/Constants.java +++ b/src/org/team708/robot/Constants.java @@ -47,7 +47,7 @@ public final class Constants { /* * Game Elements */ - public static final double TOTE_HEIGHT = 13.5; + public static final double TOTE_HEIGHT = 14.0; public static final double SCORE_TOTE = 1.0; /* diff --git a/src/org/team708/robot/Robot.java b/src/org/team708/robot/Robot.java index 0add027..19e117a 100644 --- a/src/org/team708/robot/Robot.java +++ b/src/org/team708/robot/Robot.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.team708.robot.commands.DoNothing; +import org.team708.robot.commands.autonomous.ShoveAll; //import org.team708.robot.commands.autonomous.ClearStep; //import org.team708.robot.commands.autonomous.HockeyStickClearToAutoZone; //import org.team708.robot.commands.autonomous.ThreeContainersToAuto; @@ -177,6 +178,7 @@ private void queueAutonomousModes() { autonomousMode.addObject("Container Tote", new ContainerToteByOptical()); autonomousMode.addObject("6) Container Tote Spin Tote", new ContainerToteSpinToteByOptical()); autonomousMode.addObject("7) Container Tote Tote", new ContainerToteToteByOptical()); + autonomousMode.addObject("8) Shove All", new ShoveAll()); // autonomousMode.addObject("Drive By IR", new DriveToIRDistance(5.0, // 0.0, 0.9, 0.1)); // autonomousMode.addObject("Drive in Square", new DriveInSquare()); diff --git a/src/org/team708/robot/commands/autonomous/ShoveAll.java b/src/org/team708/robot/commands/autonomous/ShoveAll.java index 91a3bcc..f2f0e3e 100644 --- a/src/org/team708/robot/commands/autonomous/ShoveAll.java +++ b/src/org/team708/robot/commands/autonomous/ShoveAll.java @@ -2,10 +2,9 @@ import org.team708.robot.AutoConstants; import org.team708.robot.commands.autonomous.steps.DriveOpticalAndEncoder; -import org.team708.robot.commands.clawElevator.IncrementClawOne; +import org.team708.robot.commands.claw.CloseClaw; import org.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance; import org.team708.robot.commands.drivetrain.TurnToDegrees; -import org.team708.robot.commands.intake.IntakeByTime; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -14,14 +13,11 @@ */ public class ShoveAll extends CommandGroup { - private double FIELD_WIDTH = 310; - - public ShoveAll() { + public ShoveAll() { - addParallel(new IntakeByTime(5.0)); - addSequential(new IncrementClawOne()); + addParallel(new CloseClaw()); - addSequential(new DriveStraightToEncoderDistance(FIELD_WIDTH)); + addSequential(new DriveStraightToEncoderDistance(AutoConstants.FIELD_WIDTH)); addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, AutoConstants.NINETY_DEGREE_TURN)); addSequential(new DriveOpticalAndEncoder(AutoConstants.TOTE_TO_AUTOZONE_DISTANCE)); diff --git a/src/org/team708/robot/commands/autonomous/optical/ThreeTotesByOptical.java b/src/org/team708/robot/commands/autonomous/optical/ThreeTotesByOptical.java index cda11be..5f47d16 100644 --- a/src/org/team708/robot/commands/autonomous/optical/ThreeTotesByOptical.java +++ b/src/org/team708/robot/commands/autonomous/optical/ThreeTotesByOptical.java @@ -2,6 +2,7 @@ import org.team708.robot.AutoConstants; import org.team708.robot.commands.autonomous.steps.DriveByOpticalToAuto; +import org.team708.robot.commands.autonomous.steps.DriveOpticalAndEncoder; import org.team708.robot.commands.claw.CloseClaw; import org.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance; import org.team708.robot.commands.drivetrain.TurnToDegrees; @@ -17,18 +18,23 @@ public class ThreeTotesByOptical extends CommandGroup { public ThreeTotesByOptical() { addParallel(new CloseClaw()); //pick up first tote and move to second - addSequential(new IndexerUp()); - addSequential(new DriveStraightToEncoderDistance(AutoConstants.THREE_TOTE_DISTANCE, AutoConstants.ENCODER_SPEED)); + addParallel(new IndexerUp()); + addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, -AutoConstants.THREE_TOTE_SMACK_ANGLE)); + addSequential(new DriveStraightToEncoderDistance(AutoConstants.THREE_TOTE_CONTAINER_DISTANCE, AutoConstants.ENCODER_SPEED)); + addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, AutoConstants.THREE_TOTE_SMACK_ANGLE)); + addSequential(new DriveStraightToEncoderDistance(AutoConstants.THREE_TOTE_TOTE_DISTANCE)); //pick up second tote and move to third - addSequential(new IndexerUp()); - addSequential(new DriveStraightToEncoderDistance(AutoConstants.THREE_TOTE_DISTANCE, AutoConstants.ENCODER_SPEED)); + addParallel(new IndexerUp()); + addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, -AutoConstants.THREE_TOTE_SMACK_ANGLE)); + addSequential(new DriveStraightToEncoderDistance(AutoConstants.THREE_TOTE_CONTAINER_DISTANCE, AutoConstants.ENCODER_SPEED)); + addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, AutoConstants.THREE_TOTE_SMACK_ANGLE)); + addSequential(new DriveStraightToEncoderDistance(AutoConstants.THREE_TOTE_TOTE_DISTANCE)); //pick up third tote and move to auto - addSequential(new IndexerUp()); + addParallel(new IndexerUp()); addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, AutoConstants.NINETY_DEGREE_TURN)); - addSequential(new DriveByOpticalToAuto()); - addSequential(new DriveStraightToEncoderDistance(AutoConstants.CLAW_LENGTH, AutoConstants.ENCODER_SPEED)); + addSequential(new DriveOpticalAndEncoder(AutoConstants.TOTE_TO_AUTOZONE_DISTANCE)); //turn 90 degrees counterclockwise addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, -AutoConstants.NINETY_DEGREE_TURN)); diff --git a/src/org/team708/robot/commands/drivetrain/TurnToDegrees.java b/src/org/team708/robot/commands/drivetrain/TurnToDegrees.java index 0369cda..b3b748f 100644 --- a/src/org/team708/robot/commands/drivetrain/TurnToDegrees.java +++ b/src/org/team708/robot/commands/drivetrain/TurnToDegrees.java @@ -12,6 +12,11 @@ public class TurnToDegrees extends Command { private double rotationSpeed; private double goalDegrees; + /** + * Constructor + * @param rotationSpeed + * @param goalDegrees + */ public TurnToDegrees(double rotationSpeed, double goalDegrees) { // Use requires() here to declare subsystem dependencies requires(Robot.drivetrain);