Josh auto switch

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

@@ -1,13 +1,10 @@
 package frc.team555.robot;
 
-import frc.team555.robot.core.PIDLoopTest;
+
 import frc.team555.robot.core.PowerUpRobot;
-import frc.team555.robot.core.PowerUpRobot2;
-import frc.team555.robot.test.EncoderTestRobot;
 import frc.team555.robot.test.TestRobot;
 import frc.team555.robot.test.TestRobot2;
-import frc.team555.robot.test.TestRobot3;
 
-public class Robot extends EncoderTestRobot {
+public class Robot extends PowerUpRobot {
 }
 

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

@@ -0,0 +1,77 @@
+package frc.team555.robot.auto;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.team555.robot.components.Lift;
+import frc.team555.robot.components.MainLift;
+import frc.team555.robot.core.PowerUpRobot;
+import org.montclairrobotics.sprocket.auto.AutoMode;
+import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro;
+import org.montclairrobotics.sprocket.auto.states.DriveTime;
+import org.montclairrobotics.sprocket.geometry.Angle;
+import org.montclairrobotics.sprocket.states.State;
+import org.montclairrobotics.sprocket.states.StateMachine;
+
+import static frc.team555.robot.components.MainLift.TOP;
+
+public class AutoClimbSequence extends StateMachine {
+
+    private Lift lift;
+    private double height;
+    private double power;
+    private boolean up;
+
+    public AutoClimbSequence(MainLift lift){
+        super(
+                new DriveEncoderGyro(10,
+                        0.4,
+                        Angle.ZERO,
+                        true,
+                        PowerUpRobot.correction
+
+                ),
+
+                new MoveLift(lift,
+                        TOP,
+                        1,
+                        true
+                ),
+
+                new DriveTime(3,-0.5),
+                new MoveLift(lift,
+                        TOP*0.3,
+                        1,
+                        false
+                )
+
+        );
+    }
+
+    State MoveLift = new State() {
+        @Override
+        public void start() {
+            power=Math.abs(power);
+            if(!up)
+            {
+                power=-power;
+            }
+        }
+
+        @Override
+        public void stop()
+        {
+            lift.setAuto(0);
+        }
+
+        @Override
+        public void stateUpdate() {
+            lift.setAuto(power);
+        }
+
+        @Override
+        public boolean isDone() {
+            return lift.getEncoder().getInches().get()>height==up;
+        }
+    };
+
+
+}

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/DropCubeSwitch.java

@@ -2,25 +2,28 @@
 
 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(CubeIntake intake, IntakeLift lift, Side side, GyroCorrection correction){
-        super(new MultiState(0,
-                new SideTurn(correction,false, side),
-                new IntakeLiftMove(10, .75, true)
-        ),
+    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(intake, 1));
+                new CubeOuttake(PowerUpRobot.intake, 1));
     }
 }

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

@@ -0,0 +1,22 @@
+package frc.team555.robot.auto;
+
+import frc.team555.robot.components.CubeIntake;
+import frc.team555.robot.utils.Side;
+import org.montclairrobotics.sprocket.auto.AutoState;
+import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro;
+import org.montclairrobotics.sprocket.auto.states.DriveTime;
+import org.montclairrobotics.sprocket.drive.steps.GyroCorrection;
+import org.montclairrobotics.sprocket.geometry.Angle;
+import org.montclairrobotics.sprocket.states.StateMachine;
+import org.montclairrobotics.sprocket.utils.Input;
+
+public class DropCubeTop extends StateMachine {
+    public DropCubeTop(CubeIntake intake, GyroCorrection correction, Input<Side> side){
+        super(
+                new DriveEncoderGyro(140, .5, Angle.ZERO, false, correction),
+                new SideTurn(correction, false, side),
+                // new MainLiftMove(12,.5,true),
+                new DriveTime(2, .75)
+        );
+    }
+}