Final worlds

src/main/java/frc/team555/robot/Robot.java

@@ -1,5 +1,6 @@
 package frc.team555.robot;
 
+
 import frc.team555.robot.core.PowerUpRobot;
 import frc.team555.robot.test.TestRobot;
 import frc.team555.robot.test.TestRobot2;

src/main/java/frc/team555/robot/auto/AutoStart.java

@@ -0,0 +1,11 @@
+package frc.team555.robot.auto;
+
+import org.montclairrobotics.sprocket.auto.states.ResetGyro;
+import org.montclairrobotics.sprocket.drive.steps.GyroCorrection;
+import org.montclairrobotics.sprocket.states.StateMachine;
+@Deprecated
+public class AutoStart extends StateMachine {
+    public AutoStart(GyroCorrection correction){
+        super(new ResetGyro(correction));
+    }
+}

src/main/java/frc/team555/robot/auto/AutoSwitch.java

@@ -0,0 +1,22 @@
+package frc.team555.robot.auto;
+
+import frc.team555.robot.utils.Side;
+import org.montclairrobotics.sprocket.auto.AutoMode;
+import org.montclairrobotics.sprocket.states.State;
+/*
+public abstract class AutoSwitch extends AutoMode {
+     /*protected static double POWER = 0.75;
+
+    // public AutoSwitch(String name, State... states) {
+        super(name, states);
+    }
+
+    public static AutoSwitch fromMiddle() {
+        return new AutoSwitchMid(Side.fromDriverStation()[0]);
+    }
+
+    public static AutoSwitch fromSide(Side pos) {
+        return new AutoSwitchLR(pos, Side.fromDriverStation()[0]);
+    }
+}
+*/

src/main/java/frc/team555/robot/auto/AutoSwitchLR.java

@@ -0,0 +1,33 @@
+package frc.team555.robot.auto;
+
+import frc.team555.robot.core.PowerUpRobot;
+import frc.team555.robot.utils.Side;
+import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro;
+import org.montclairrobotics.sprocket.auto.states.ResetGyro;
+import org.montclairrobotics.sprocket.geometry.Angle;
+import org.montclairrobotics.sprocket.utils.Input;
+/*
+class AutoSwitchLR extends AutoSwitch {
+    AutoSwitchLR(Side pos, Side target) {
+        super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")",
+                /* Reset gyro
+                new ResetGyro(PowerUpRobot.correction),
+                /* Drive forward 12 inches
+                new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+                /* Turn π/2 radians (toward the nearest wall)
+                new DriveEncoderGyro(0,(pos == Side.RIGHT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction),
+                /* Drive forward 36 inches
+                new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+                /* Turn π/2 radians (in the opposite direction)
+                new DriveEncoderGyro(0,(pos == Side.LEFT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction),
+                /* Drive forward 36 inches
+                new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+                /* Turn π/2 radians (in the opposite direction)
+                new DriveEncoderGyro(0,(pos == Side.LEFT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction),
+                /* Drive forward 12 inches OR 72 inches, depending on the target
+                new DriveEncoderGyro(pos == target ? 12 : 72, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+                /* Move the lift, turn, and drop the cube into the switch
+                new DropCubeSwitch(new Input<Side>(){public Side get(){return pos;}}, PowerUpRobot.correction)
+        );
+    }
+}*/

src/main/java/frc/team555/robot/auto/AutoSwitchMid.java

@@ -0,0 +1,26 @@
+package frc.team555.robot.auto;
+
+import frc.team555.robot.core.PowerUpRobot;
+import frc.team555.robot.utils.Side;
+import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro;
+import org.montclairrobotics.sprocket.auto.states.ResetGyro;
+import org.montclairrobotics.sprocket.geometry.Angle;
+import org.montclairrobotics.sprocket.utils.Input;
+/*
+class AutoSwitchMid extends AutoSwitch {
+
+    AutoSwitchMid(Side target) {
+        super("Cube to " + target.toString() + " Switch (M)",
+                /* Reset gyro
+                new ResetGyro(PowerUpRobot.correction),
+                /* Drive forward 12 inches
+                new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+                /* Turn π/2 radians (in the correct directon)
+                new DriveEncoderGyro(0,(target == Side.RIGHT ? +1 : -1) * POWER, Angle.QUARTER, true, PowerUpRobot.correction),
+                /* Drive forward 36 inches
+                new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+                /* Move the lift, turn, and drop the cube into the switch
+                new DropCubeSwitch(new Input<Side>(){public Side get(){return target;}}, PowerUpRobot.correction)
+        );
+    }
+}*/

src/main/java/frc/team555/robot/auto/AutoWillTest.java

@@ -0,0 +1,34 @@
+//package frc.team555.robot.auto;
+//
+//import frc.team555.robot.core.PowerUpRobot;
+//import frc.team555.robot.utils.Side;
+//import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro;
+//import org.montclairrobotics.sprocket.auto.states.ResetGyro;
+//import org.montclairrobotics.sprocket.geometry.Angle;
+//import org.montclairrobotics.sprocket.utils.Input;
+//
+//public class AutoWillTest extends AutoSwitch {
+//
+//    public AutoWillTest(Side pos, Side target) {
+//        super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")",
+//                /* Reset gyro */
+//                new ResetGyro(PowerUpRobot.correction),
+//                /* Drive forward 12 inches */
+//                new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+//                /* Turn π/2 radians (toward the nearest wall) */
+//                new DriveEncoderGyro(0,(pos == Side.RIGHT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction),
+//                /* Drive forward 36 inches */
+//                new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+//                /* Turn π/2 radians (in the opposite direction) */
+//                new DriveEncoderGyro(0,(pos == Side.LEFT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction),
+//                /* Drive forward 36 inches */
+//                new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+//                /* Turn π/2 radians (in the opposite direction) */
+//                new DriveEncoderGyro(0,(pos == Side.LEFT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction),
+//                /* Drive forward 12 inches OR 72 inches, depending on the target */
+//                new DriveEncoderGyro(pos == target ? 12 : 72, POWER, Angle.ZERO, true, PowerUpRobot.correction),
+//                /* Move the lift, turn, and drop the cube into the switch */
+//                new DropCubeSwitch(new Input<Side>(){public Side get(){return pos;}}, PowerUpRobot.correction));
+//                /* Back away from switch
+//    }
+//}

src/main/java/frc/team555/robot/auto/CubeOuttake.java

@@ -5,7 +5,7 @@
 import org.montclairrobotics.sprocket.auto.states.Disable;
 import org.montclairrobotics.sprocket.auto.states.Enable;
 import org.montclairrobotics.sprocket.states.StateMachine;
-@Deprecated
+
 public class CubeOuttake extends StateMachine {
 
     public CubeOuttake(CubeIntake intake, double time){

src/main/java/frc/team555/robot/auto/DropCubeScale.java

@@ -0,0 +1,10 @@
+package frc.team555.robot.auto;
+
+import org.montclairrobotics.sprocket.states.MultiState;
+import org.montclairrobotics.sprocket.states.StateMachine;
+
+public class DropCubeScale extends StateMachine {
+    public DropCubeScale() {
+        super(new MultiState());
+    }
+}

src/main/java/frc/team555/robot/auto/DropCubeSwitch.java

@@ -0,0 +1,29 @@
+package frc.team555.robot.auto;
+
+import frc.team555.robot.components.CubeIntake;
+import frc.team555.robot.components.IntakeLift;
+import frc.team555.robot.core.PowerUpRobot;
+import frc.team555.robot.utils.Side;
+import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro;
+import org.montclairrobotics.sprocket.drive.steps.GyroCorrection;
+import org.montclairrobotics.sprocket.geometry.Angle;
+import org.montclairrobotics.sprocket.states.MultiState;
+import org.montclairrobotics.sprocket.states.State;
+import org.montclairrobotics.sprocket.states.StateMachine;
+import org.montclairrobotics.sprocket.utils.Input;
+
+public class DropCubeSwitch extends StateMachine {
+
+    private CubeIntake intake;
+    private IntakeLift lift;
+
+    public DropCubeSwitch(Input<Side> side, GyroCorrection correction){
+        super(
+                new MultiState(0,
+                        new SideTurn(correction,false, side),
+                        new IntakeLiftMove(10, .75, true)
+                ),
+                new DriveEncoderGyro(3, .5, Angle.ZERO, true, correction),
+                new CubeOuttake(PowerUpRobot.intake, 1));
+    }
+}