diff --git a/src/main/java/frc/robot/autonomous/Actions.java b/src/main/java/frc/robot/autonomous/Actions.java index 2b4a950b..c3d66adc 100644 --- a/src/main/java/frc/robot/autonomous/Actions.java +++ b/src/main/java/frc/robot/autonomous/Actions.java @@ -5,6 +5,7 @@ import frc.robot.gamepieces.GamePieceController; import frc.robot.logging.RobotLogManager; import frc.robot.gamepieces.AbstractLayers.IndexerAL; +import frc.robot.gamepieces.AbstractLayers.IntakeAL; import java.text.DecimalFormat; @@ -21,47 +22,45 @@ public class Actions { private static GamePieceController gamePieceController = GamePieceController.getInstance(); private static double mirrorTurns = 1.0; - + public static void startOnLeft() { mirrorTurns = -1.0; } - + public static void startOnRight() { mirrorTurns = 1.0; } - + public static void startInCenter() { mirrorTurns = 1.0; } - + public static final Action nothing() { String actionText = "Do Nothing"; - return new Action(actionText, - () -> drive.isStopped(), - () -> drive.arcadeDrive(0, 0, false)); + return new Action(actionText, () -> drive.isStopped(), () -> drive.arcadeDrive(0, 0, false)); + } + + public static final Action drive(double duration) { + String actionText = "Do Nothing"; + return new Action(actionText, new ActionGroup.Duration(duration), () -> drive.arcadeDrive(1, 0, false)); } public static final Action Shoot() { String actionText = "shoot"; - + IndexerAL indexer = IndexerAL.getInstance(); - return new Action(actionText, - () -> indexer.isBallInChamber(), + return new Action(actionText, () -> indexer.isBallInChamber(), () -> gamePieceController.setAutomousFireWhenReady(true)); } public static Action wait(double duration) { String actionText = "Do Nothing"; - return new Action(actionText, - new ActionGroup.Duration(duration), - () -> drive.arcadeDrive(0, 0)); + return new Action(actionText, new ActionGroup.Duration(duration), () -> drive.arcadeDrive(0, 0)); } public static final Action nothingForever() { String actionText = "Do Nothing"; - return new Action(actionText, - () -> false, - () -> drive.moveLinearFeet(0)); + return new Action(actionText, () -> false, () -> drive.moveLinearFeet(0)); } public static ActionGroup doNothing() { @@ -71,16 +70,11 @@ public static ActionGroup doNothing() { } public static Action print(String message) { - return new Action( - "Print custom message", - new ActionGroup.RunOnce(() -> LOGGER.info(message))); + return new Action("Print custom message", new ActionGroup.RunOnce(() -> LOGGER.info(message))); } - public static Action zeroDistance() { - return new Action( - "Zeroing the distance", - new ActionGroup.RunOnce(() -> drive.zero())); + return new Action("Zeroing the distance", new ActionGroup.RunOnce(() -> drive.zero())); } /** @@ -90,22 +84,49 @@ public static Action zeroDistance() { */ public static Action moveDistanceForward(double distance) { String actionText = "Move forward " + distance + " feet"; - //TODO determine the mult values - double timeAmt = (distance - 0.112)/10.3596; - return new Action(actionText, - new ActionGroup.Duration(timeAmt), - () -> drive.arcadeDrive(0.8,0)); + return new Action(actionText, new ActionGroup.ReachDistance(distance), () -> drive.arcadeDrive(0.8, 0)); + } + + /** + * + * @param distance moves robot in feet. + * @return + */ + public static ActionGroup moveDistanceForwardAndGather(double distance) { + String actionGroupText = "Move forward " + distance + " feet"; + ActionGroup mode = new ActionGroup(actionGroupText); + mode.addAction( new Action("", new ActionGroup.ReachDistance(distance), new ActionGroup.ConcurrentActions( + () -> drive.arcadeDrive(0.8, 0), () -> IntakeAL.callForward()))); + //TODO i dont know if this just does this once and stops; + mode.addAction(new Action("",()->false,()->IntakeAL.callRollerStop())); + return mode; + } + + /** + * + * @param distance moves robot in feet. + * @param rotation enter positive degrees for left turn and enter negative + * degrees for right turn. + * @return + */ + public static Action arc(double speed, double rotation) { + String actionText = "Move forward " + speed + " feet"; + ActionGroup.MultiCondition condition = new ActionGroup.MultiCondition(new ActionGroup.ReachAngle(rotation), + new ActionGroup.ReachDistance(speed)); + + return new Action(actionText, condition, () -> drive.arcadeDrive(speed, rotation)); } /** * - * @param rotationInDegrees Rotates robot in radians. Enter rotation amount in Degrees. + * + * @param rotationInDegrees Rotates robot in radians. Enter rotation amount in + * Degrees. * */ public static Action moveturn(double rotationInDegrees) { String actionText = "Rotate " + rotationInDegrees + " degrees."; - return new Action(actionText, - new ActionGroup.ReachAngle(rotationInDegrees), + return new Action(actionText, new ActionGroup.ReachAngle(rotationInDegrees), // reach distance was here instead of reachAngle () -> drive.rotateByAngle(rotationInDegrees)); } @@ -113,8 +134,7 @@ public static Action moveturn(double rotationInDegrees) { public static boolean moveDistanceComplete(double distance) { double distanceMoved = drive.absoluteDistanceMoved(); - LOGGER.debug("Distances - Target: {} Moved: {}", - df.format(Math.abs(distance)), df.format(distanceMoved)); + LOGGER.debug("Distances - Target: {} Moved: {}", df.format(Math.abs(distance)), df.format(distanceMoved)); if (distanceMoved >= (Math.abs(distance) - RobotMap.POSITION_ALLOWED_ERROR)) { LOGGER.info("Finished moving {} feet", df.format(distanceMoved)); return true; @@ -124,14 +144,22 @@ public static boolean moveDistanceComplete(double distance) { } } + /** + * + * @param distance distance to move in feet + */ public static ActionGroup move(double distance) { String actionGroupText = "Move forward " + distance + " feet"; ActionGroup mode = new ActionGroup(actionGroupText); - mode.addAction(zeroDistance()); mode.addAction(moveDistanceForward(distance)); return mode; } + /** + * + * @param degrees enter positive degrees for left turn and enter negative + * degrees for right turn. + */ public static ActionGroup turn(double degrees) { String actionGroupText = "Turn " + degrees + " degrees"; ActionGroup mode = new ActionGroup(actionGroupText); @@ -154,14 +182,13 @@ public static ActionGroup DoNothing() { } public static ActionGroup shootGroup() { - String actionGroupText = "doing nothing"; + String actionGroupText = "shoots"; ActionGroup mode = new ActionGroup(actionGroupText); mode.addAction(Shoot()); mode.addAction(moveDistanceForward(10)); return mode; } - public static ActionGroup crossAutoLine() { String actionGroupText = "Go straight to cross the auto line."; ActionGroup mode = new ActionGroup(actionGroupText); @@ -169,7 +196,25 @@ public static ActionGroup crossAutoLine() { mode.addActions(move(10.0)); return mode; } - + + public static Action getAngleFromTarget() { + String actionText = "sense angle from target"; + return new Action(actionText, // + () -> true, // if an angle has been sensed + () -> { + }// sense for an angle + ); + } + + public static ActionGroup aimAndShoot() { + String actionGroupText = "aims at the target and shoot"; + double thing; + ActionGroup mode = new ActionGroup(actionGroupText); + + return mode; + + } + // TEST ACTIONS public static ActionGroup fourFootSquare() { @@ -186,4 +231,116 @@ public static ActionGroup fourFootSquare() { return mode; } + // + public static ActionGroup collectAndShoot() { + // stuff + String actionGroupText = "align with trench and go back to score"; + ActionGroup mode = new ActionGroup(actionGroupText); + mode.addActions(move(11)); + mode.addActions(turn(90)); + // TODO create this + mode.addAction(getAngleFromTarget()); + mode.addActions(turn(-90)); + // mode.addActions(modeForwardByTheAmountINeedPlus5.66Feet()); TODO + // formula is tan(theta)*10.16+5.66 + mode.addActions(turn(-90)); + mode.addActions(move(16.06)); + mode.addActions(turn(-160.59)); + mode.addActions(move(17.03)); + mode.addActions(turn(-19.41)); + // TODO make these align ans shoot + mode.addAction(Shoot()); + + return mode; + } + + /** + * turns right, moves closer to the power port and shoots at the port. Then it + * turns and moves pas the line and out of the way + */ + public static ActionGroup shootLB() { + // stuff + String actionGroupText = "shoot from loading bay"; + ActionGroup mode = new ActionGroup(actionGroupText); + // TODO make these based off of camera readings + mode.addActions(turn(-62.78)); + mode.addActions(move(6.56)); + mode.addActions(turn(5.78)); + // TODO make these align and shoot + mode.addAction(Shoot()); + // move out of the way after shot + mode.addActions(turn(57.4)); + mode.addActions(move(-10)); + return mode; + } + + /** + * shoots and moves back out of the way + */ + public static ActionGroup shootPP() { + // stuff + String actionGroupText = "shoot from power port"; + ActionGroup mode = new ActionGroup(actionGroupText); + // TODO make these align and shoot + mode.addAction(Shoot()); + // move back after shot + mode.addActions(move(-10)); + return mode; + } + + /** + * turns a little left and shoots, then turns back and drives back + */ + public static ActionGroup shootPS1() { + // stuff + String actionGroupText = "shoot from player station 1"; + ActionGroup mode = new ActionGroup(actionGroupText); + mode.addActions(turn(28.43)); + // TODO make these align and shoot + mode.addAction(Shoot()); + mode.addActions(turn(-28.43)); + // move out of the way after shot + mode.addActions(move(-10)); + + return mode; + } + + /** + * if the robots shooter is broken, then it moves back out of the way + * immediately + */ + public static ActionGroup noShoot() { + // stuff + String actionGroupText = "dont shoot and back up"; + ActionGroup mode = new ActionGroup(actionGroupText); + mode.addActions(move(-10)); + return mode; + } + + // TODO change names + public static ActionGroup leftSide() { + String actionGroupText = "shoot from left side"; + double thing; + ActionGroup mode = new ActionGroup(actionGroupText); + mode.addAction(Shoot()); + mode.addActions(turn(-150)); + mode.addActions(moveDistanceForwardAndGather(16)/* suck TODO */); + mode.addActions(move(-9)); + mode.addActions(turn(-160)); + mode.addAction(Shoot()); + return mode; + } + + public static ActionGroup rightSide() { + String actionGroupText = "shoot from right side"; + ActionGroup mode = new ActionGroup(actionGroupText); + mode.addActions(move(-12)); + mode.addActions(turn(42)); + mode.addActions(moveDistanceForwardAndGather(7)); + mode.addActions(move(-2)); + mode.addActions(turn(-45)); + mode.addAction(Shoot()); + return mode; + } + } diff --git a/src/main/java/frc/robot/autonomous/MatchConfiguration.java b/src/main/java/frc/robot/autonomous/MatchConfiguration.java index 8815314a..232ca462 100644 --- a/src/main/java/frc/robot/autonomous/MatchConfiguration.java +++ b/src/main/java/frc/robot/autonomous/MatchConfiguration.java @@ -10,7 +10,7 @@ import org.apache.logging.log4j.Logger; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -/** +/** * This class determines the robots position during the beginning of the game. * */ @@ -18,39 +18,35 @@ public class MatchConfiguration { private static MatchConfiguration instance; - private static final Logger LOGGER - = RobotLogManager.getMainLogger(MatchConfiguration.class.getName()); + private static final Logger LOGGER = RobotLogManager.getMainLogger(MatchConfiguration.class.getName()); public enum TeamColor { - UNKNOWN, - RED, - BLUE; + UNKNOWN, RED, BLUE; } public enum Side { - UNKNOWN, - LEFT, - RIGHT; + UNKNOWN, LEFT, RIGHT; } private TeamColor teamColor; - private String autoMode = "None"; + private String autoMode = "None"; private ActionGroup autonomous; - private String[] autolist= {"None","Shoot Basic"}; + private String[] autolist = { "None", "Shoot Basic", "Collect And Shoot", "Shoot from LB", "Shoot From PP", + "Shoot From PS1", "Get Out Of The Way", "Shoot From Left", "Shoot From Right" }; private SendableChooser chooser = new SendableChooser(); private MatchConfiguration() { teamColor = TeamColor.UNKNOWN; - for(String s : autolist) { - chooser.addOption(s,s); + for (String s : autolist) { + chooser.addOption(s, s); } - SmartDashboard.putData("Auto Chooser", chooser); + SmartDashboard.putData("Auto Chooser", chooser); } @@ -62,7 +58,7 @@ public static MatchConfiguration getInstance() { } public void setAllianceColor() { - Alliance color; + Alliance color; color = DriverStation.getInstance().getAlliance(); @@ -75,7 +71,7 @@ public void setAllianceColor() { } else { LOGGER.info("Alliance not found"); teamColor = TeamColor.UNKNOWN; - } + } } public void setAutoModeAndStartPosition() { @@ -85,32 +81,48 @@ public void setAutoModeAndStartPosition() { public ActionGroup AutoDecisionTree() { autonomous = Actions.doNothing(); - - // if (autoMode.startsWith("Left")) { - // Actions.startOnLeft(); - // } else if (autoMode.startsWith("Right")) { - // Actions.startOnRight(); - // } else { - // Actions.startInCenter(); - // } + // if (autoMode.startsWith("Left")) { + // Actions.startOnLeft(); + // } else if (autoMode.startsWith("Right")) { + // Actions.startOnRight(); + // } else { + // Actions.startInCenter(); + // } - switch(autoMode) { + switch (autoMode) { case "None": autonomous = Actions.doNothing(); - break; + break; case "Shoot Basic": autonomous = Actions.shootGroup(); break; + case "Collect And Shoot": + autonomous = Actions.collectAndShoot(); + break; + case "Shoot from LB": + autonomous = Actions.shootLB(); + break; + case "Shoot From PP": + autonomous = Actions.shootPP(); + break; + case "Shoot From PS1": + autonomous = Actions.shootPS1(); + break; + case "Get Out Of The Way": + autonomous = Actions.noShoot(); + case "Shoot From Left": + autonomous = Actions.leftSide(); + case "Shoot From Right": + autonomous = Actions.rightSide(); default: - Actions.doNothing(); + Actions.doNothing(); } autonomous.enable(); return autonomous; - } - + } public void load() { LOGGER.debug("Loading game info."); diff --git a/src/main/java/frc/robot/gamepieces/AbstractLayers/IntakeAL.java b/src/main/java/frc/robot/gamepieces/AbstractLayers/IntakeAL.java index ba77cdf0..c04d9468 100644 --- a/src/main/java/frc/robot/gamepieces/AbstractLayers/IntakeAL.java +++ b/src/main/java/frc/robot/gamepieces/AbstractLayers/IntakeAL.java @@ -76,7 +76,7 @@ public static void callDown() { IntakeAL.getInstance().setDown(); } - public static void callFoward() { + public static void callForward() { IntakeAL.getInstance().setForward(); } diff --git a/src/main/java/frc/robot/gamepieces/States/IntakeState.java b/src/main/java/frc/robot/gamepieces/States/IntakeState.java index 3c94c514..95e25f62 100644 --- a/src/main/java/frc/robot/gamepieces/States/IntakeState.java +++ b/src/main/java/frc/robot/gamepieces/States/IntakeState.java @@ -53,7 +53,7 @@ public void setIntakeArm(IntakerArm arm, IntakerRollers rollers) { IntakeAL.callBackward(); break; case ROLLERS_OUT: - IntakeAL.callFoward(); + IntakeAL.callForward(); break; default: case ROLLERS_OFF: @@ -82,7 +82,7 @@ public void setIntakeRoller(IntakerRollers rollers) { IntakeAL.callBackward(); break; case ROLLERS_OUT: - IntakeAL.callFoward(); + IntakeAL.callForward(); break; default: case ROLLERS_OFF: