Auton

src/main/java/org/usfirst/frc4904/auton/AutonRoutine.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4904.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AutonRoutine extends SequentialCommandGroup {
+    public ArrayList<Command> commandsList;
+
+    @Override
+    public SequentialCommandGroup andThen(Command... commands) {
+        SequentialCommandGroup group = super.andThen(commands);
+        for (Command command : commands) {
+            commandsList.add(command);
+        }
+        return group;
+    }
+
+    public ArrayList<Command> getCommandsList() {
+        return commandsList;
+    }
+
+    /**
+     * Used for the spline visualizer.
+     * 
+     * @return List of all commands with trajectories
+     */
+    public ArrayList<Command> getSplinesList() {
+        ArrayList<Command> splinesCommands = new ArrayList<Command>();
+        for (Command command : commandsList) {
+            if (command instanceof SimpleSplines) {
+                splinesCommands.add(command);
+            }
+        }
+        return splinesCommands;
+    }
+}

src/main/java/org/usfirst/frc4904/auton/JustMove.java

@@ -0,0 +1,11 @@
+package org.usfirst.frc4904.auton;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance;
+
+class JustMove extends AutonRoutine {
+
+    public JustMove() {
+        this.andThen(new ChassisMoveDistance(RobotMap.Component.chassis, 10, null)); // TODO: what motioncontroller?
+    }
+}

src/main/java/org/usfirst/frc4904/auton/JustShoot.java

@@ -0,0 +1,12 @@
+package org.usfirst.frc4904.auton;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.commands.Shoot;
+
+class JustShoot extends AutonRoutine {
+
+    public JustShoot() {
+        andThen(new Shoot(RobotMap.Component.indexer, RobotMap.Component.shooter, 0.5));
+    }
+
+}

src/main/java/org/usfirst/frc4904/auton/PickupAndShootCenter.java

@@ -0,0 +1,31 @@
+package org.usfirst.frc4904.auton;
+
+import java.util.Arrays;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.commands.BoxShot;
+import org.usfirst.frc4904.robot.commands.FlywheelSpinDown;
+import org.usfirst.frc4904.robot.commands.RunIntake;
+import org.usfirst.frc4904.robot.commands.vision.VisionMoveHighPort;
+import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines;
+
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+
+class PickupAndShootCenter extends AutonRoutine {
+
+        public PickupAndShootCenter() {
+                Trajectory goingToPowerCells = RobotMap.Component.sensorChassis
+                                .generateQuinticTrajectory(Arrays.asList(Poses.currentPos, Poses.centerCollectStart));
+                SimpleSplines approachSpline = new SimpleSplines(RobotMap.Component.sensorChassis, goingToPowerCells);
+                Trajectory collect = RobotMap.Component.sensorChassis.generateQuinticTrajectory(
+                                Arrays.asList(Poses.centerCollectStart, Poses.centerCollectEnd));
+                SimpleSplines collectSpline = new SimpleSplines(RobotMap.Component.sensorChassis, collect);
+                addCommands(approachSpline,
+                                new ParallelCommandGroup(new RunIntake(RobotMap.Component.intake), collectSpline),
+                                new VisionMoveHighPort(),
+                                new ParallelCommandGroup(
+                                                new BoxShot(RobotMap.Component.indexer, RobotMap.Component.shooter),
+                                                new FlywheelSpinDown(RobotMap.Component.flywheel)));
+        }
+}

src/main/java/org/usfirst/frc4904/auton/PickupAndShootSide.java

@@ -0,0 +1,31 @@
+package org.usfirst.frc4904.auton;
+
+import java.util.Arrays;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.commands.BoxShot;
+import org.usfirst.frc4904.robot.commands.FlywheelSpinDown;
+import org.usfirst.frc4904.robot.commands.RunIntake;
+import org.usfirst.frc4904.robot.commands.vision.VisionMoveHighPort;
+import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines;
+
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+
+class PickupAndShootSide extends AutonRoutine {
+
+        public PickupAndShootSide() {
+                Trajectory goingToPowerCells = RobotMap.Component.sensorChassis
+                                .generateQuinticTrajectory(Arrays.asList(Poses.currentPos, Poses.sideCollectStart));
+                SimpleSplines approachSpline = new SimpleSplines(RobotMap.Component.sensorChassis, goingToPowerCells);
+                Trajectory collect = RobotMap.Component.sensorChassis
+                                .generateQuinticTrajectory(Arrays.asList(Poses.sideCollectStart, Poses.sideCollectEnd));
+                SimpleSplines collectSpline = new SimpleSplines(RobotMap.Component.sensorChassis, collect);
+                addCommands(approachSpline,
+                                new ParallelCommandGroup(new RunIntake(RobotMap.Component.intake), collectSpline),
+                                new VisionMoveHighPort(),
+                                new ParallelCommandGroup(
+                                                new BoxShot(RobotMap.Component.indexer, RobotMap.Component.shooter),
+                                                new FlywheelSpinDown(RobotMap.Component.flywheel)));
+        }
+}

src/main/java/org/usfirst/frc4904/auton/Poses.java

@@ -0,0 +1,45 @@
+package org.usfirst.frc4904.auton;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.util.Units;
+
+class Poses {
+
+        // All measurements are taken in inches and converted into meters, starting at
+        // the bottom left corner of the field (I hope)
+
+        // Useful constants
+        static final double TOP_OF_FIELD = Units.inchesToMeters(319);
+        static final double START_OF_FIELD = Units.inchesToMeters(28);
+        static final double WIDTH_OF_FIELD = Units.inchesToMeters(649);
+        static final double FIRST_CENTER_BALL = Units.inchesToMeters(122.63);
+
+        // y position of center ball is 94.66 + 43.65 inches
+        // see layout and marking diagram pages 4 and 5
+
+        static final double CENTER_BALL_POS_Y = Units.inchesToMeters(138.41);
+
+        static final double OFFSET = Units.inchesToMeters(5);
+        static final double STARTING_LINE = Units.inchesToMeters(120);
+
+        // x position of center ball
+        // from right side of field:
+        // 374.59+130.36 - 122.63
+        static final double centerBallposX = -1;
+
+        static final Pose2d centerCollectStart = new Pose2d(Units.inchesToMeters(227),
+                        Units.inchesToMeters((TOP_OF_FIELD - 94.66) - 50), new Rotation2d(0));
+        static final Pose2d centerCollectEnd = new Pose2d(Units.inchesToMeters(240),
+                        Units.inchesToMeters((TOP_OF_FIELD - 94.66) - 81), new Rotation2d(0));
+
+        static final Pose2d sideCollectStart = new Pose2d(Units.inchesToMeters(STARTING_LINE + 122.63 - OFFSET),
+                        Units.inchesToMeters(TOP_OF_FIELD - 94.66 + 66.91), new Rotation2d(0));
+        static final Pose2d sideCollectEnd = new Pose2d(Units.inchesToMeters(STARTING_LINE + 194.63 + OFFSET),
+                        Units.inchesToMeters(TOP_OF_FIELD - 94.66 + 66.91), new Rotation2d(0));
+        static Pose2d shootingPose = new Pose2d(Units.inchesToMeters(START_OF_FIELD),
+                        Units.inchesToMeters(TOP_OF_FIELD - 94.66), new Rotation2d(0));
+
+        static Pose2d currentPos = new Pose2d(Units.inchesToMeters(120 + START_OF_FIELD),
+                        Units.inchesToMeters(TOP_OF_FIELD / 2), new Rotation2d(0)); // TODO: Get from localization
+}

src/main/java/org/usfirst/frc4904/auton/ShootAndPickupSide.java

@@ -0,0 +1,32 @@
+package org.usfirst.frc4904.auton;
+
+import java.util.Arrays;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.commands.BoxShot;
+import org.usfirst.frc4904.robot.commands.FlywheelSpinDown;
+import org.usfirst.frc4904.robot.commands.RunIntake;
+import org.usfirst.frc4904.robot.commands.vision.VisionMoveHighPort;
+import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines;
+
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+
+class ShootAndPickupSide extends AutonRoutine {
+
+        public ShootAndPickupSide() {
+                Trajectory goingToPowerCells = RobotMap.Component.sensorChassis
+                                .generateQuinticTrajectory(Arrays.asList(Poses.shootingPose, Poses.sideCollectStart));
+                SimpleSplines approachSpline = new SimpleSplines(RobotMap.Component.sensorChassis, goingToPowerCells);
+                Trajectory collect = RobotMap.Component.sensorChassis
+                                .generateQuinticTrajectory(Arrays.asList(Poses.sideCollectStart, Poses.sideCollectEnd));
+                SimpleSplines collectSpline = new SimpleSplines(RobotMap.Component.sensorChassis, collect);
+                addCommands(new VisionMoveHighPort(),
+                                new ParallelCommandGroup(
+                                                new BoxShot(RobotMap.Component.indexer, RobotMap.Component.shooter),
+                                                new FlywheelSpinDown(RobotMap.Component.flywheel)),
+                                approachSpline,
+                                new ParallelCommandGroup(new RunIntake(RobotMap.Component.intake), collectSpline));
+        }
+
+}

src/main/java/org/usfirst/frc4904/auton/ShootandPickupCenter.java

@@ -0,0 +1,33 @@
+package org.usfirst.frc4904.auton;
+
+import java.util.Arrays;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.commands.BoxShot;
+import org.usfirst.frc4904.robot.commands.FlywheelSpinDown;
+import org.usfirst.frc4904.robot.commands.RunIntake;
+import org.usfirst.frc4904.robot.commands.vision.VisionMoveHighPort;
+import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines;
+
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+
+class ShootandPickupCenter extends AutonRoutine {
+
+        public ShootandPickupCenter() { // TODO: Splining from the shooting pose to the balls will probably just make it
+                                        // ram into the wall
+                Trajectory goingToPowerCells = RobotMap.Component.sensorChassis
+                                .generateQuinticTrajectory(Arrays.asList(Poses.shootingPose, Poses.centerCollectStart));
+                SimpleSplines approachSpline = new SimpleSplines(RobotMap.Component.sensorChassis, goingToPowerCells);
+                Trajectory collect = RobotMap.Component.sensorChassis.generateQuinticTrajectory(
+                                Arrays.asList(Poses.centerCollectStart, Poses.centerCollectEnd));
+                SimpleSplines collectSpline = new SimpleSplines(RobotMap.Component.sensorChassis, collect);
+
+                addCommands(new VisionMoveHighPort(),
+                                new ParallelCommandGroup(
+                                                new BoxShot(RobotMap.Component.indexer, RobotMap.Component.shooter),
+                                                new FlywheelSpinDown(RobotMap.Component.flywheel)),
+                                approachSpline,
+                                new ParallelCommandGroup(new RunIntake(RobotMap.Component.intake), collectSpline));
+        }
+}