Replace all placeholders in auton code with real code

CODEOWNERS

@@ -23,7 +23,7 @@
 /src/org/usfirst/frc/team334/robot/commands/Elevator @samuelpiltch
 
 
-# Patrick Fan - PIDS (& Auton??)
+# Patrick Fan - PIDS & Auton
 /src/org/usfirst/frc/team334/robot/auto @Cyfurion
 /src/org/usfirst/frc/team334/robot/pids @Cyfurion
 /src/org/usfirst/frc/team334/robot/commands/Auton @Cyfurion

src/org/usfirst/frc/team334/robot/Constants.java

@@ -119,11 +119,11 @@ public class Constants {
     public static final double DRIVETRAIN_SPEED_MULTIPLIER = 1;
     public static final double ELEVATOR_SPEED_MULTIPLIER = 0.1;
 
-  //Intake
-    public static final int INTAKE_MOTOR_L = -1;
-    public static final int INTAKE_MOTOR_R = -1;
-    public static final int INTAKE_LIMITSWITCH_1 = -1;
-    public static final int INTAKE_LIMITSWITCH_2 = -1;
+    // Intake
+    public static final int INTAKE_MOTOR_L = 10; // PLACEHOLDER
+    public static final int INTAKE_MOTOR_R = 11; // PLACEHOLDER
+    public static final int INTAKE_LIMITSWITCH_1 = 12; // PLACEHOLDER
+    public static final int INTAKE_LIMITSWITCH_2 = 13; // PLACEHOLDER
     public static final double INTAKE_SPEED = 0.25;
     public static final double INTAKE_SPEED_HIGH = 1;
     public static final double INTAKE_OUT_SPEED = -0.25;

src/org/usfirst/frc/team334/robot/Robot.java

@@ -116,7 +116,6 @@ public void autonomousInit() {
                     break;
                 default:
                     auton_command = new CrossLine();
-                    break;
             }
         } else if (fms.getLocation() == 3) { // RIGHT START
             switch (fieldConfig) {
@@ -150,6 +149,8 @@ public void autonomousInit() {
                     auton_command = new CrossLine();
                     break;
             }
+        } else {
+
         }
         Scheduler.getInstance().add(auton_command);
     }

src/org/usfirst/frc/team334/robot/auto/scenarios/LeftStartLeftSwitch.java

@@ -2,15 +2,18 @@
 
 import org.usfirst.frc.team334.robot.Constants;
 import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class LeftStartLeftSwitch extends CommandGroup {
 
     public LeftStartLeftSwitch() {
+        addParallel(new SetElevatorToSwitchCommand());
         addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SWITCH_PARALLEL));
         addSequential(new TurnCommand(90)); // Turn to switch
-        // Drop cube command
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/LeftStartLeftSwitchLeftScale.java

@@ -2,20 +2,25 @@
 
 import org.usfirst.frc.team334.robot.Constants;
 import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class LeftStartLeftSwitchLeftScale extends CommandGroup {
 
     public LeftStartLeftSwitchLeftScale() {
+        addParallel(new SetElevatorToScaleCommand());
         addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SCALE_DIAGONAL));
         addSequential(new WaitCommand(0.3));
         addSequential(new TurnCommand(45)); // Turn to scale diagonally
-        addSequential(new WaitCommand(0.5)); // Placeholder for drop cube command
+        addSequential(new ReleasePowerCubeCommand());
         addSequential(new TurnCommand(90)); // Turn to box
+        addParallel(new SetElevatorToExchangeCommand());
         addSequential(new DriveForwardCommand(Constants.SWITCH_GAP_DISTANCE_AFTER_TURN));
-        // Intake command
-        // Drop cube command
+        addSequential(new GrabPowerCubeCommand());
+        addSequential(new SetElevatorToSwitchCommand());
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/LeftStartLeftSwitchRightScale.java

@@ -2,27 +2,31 @@
 
 import org.usfirst.frc.team334.robot.Constants;
 import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class LeftStartLeftSwitchRightScale extends CommandGroup {
 
     public LeftStartLeftSwitchRightScale() {
+        addParallel(new SetElevatorToSwitchCommand());
         addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SWITCH_PARALLEL));
-        addSequential(new TurnCommand(90)); // Turn left 90 to face scale
-        addSequential(new WaitCommand(0.5)); // Placeholder for drop cube command
-        addSequential(new TurnCommand(-90)); // Turn away from scale
-        addSequential(
-                new DriveForwardCommand(Constants.DISTANCE_TO_SWITCH_REAR - Constants.DISTANCE_TO_SWITCH_PARALLEL));
+        addSequential(new TurnCommand(90)); // Turn left 90 to face switch
+        addSequential(new ReleasePowerCubeCommand());
+        addSequential(new TurnCommand(-90)); // Turn away from switch
+        addParallel(new SetElevatorToExchangeCommand());
+        addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SWITCH_REAR - Constants.DISTANCE_TO_SWITCH_PARALLEL));
         addSequential(new TurnCommand(90)); // Turn to box behind switch
         addSequential(new DriveForwardCommand(10)); // Go up to box
-        addSequential(new WaitCommand(0.5)); // Placeholder for intake command
+        addSequential(new GrabPowerCubeCommand());
         addSequential(new TurnCommand(-90)); // Turn to alleyway
         addSequential(new DriveForwardCommand(15)); // Move to middle of alleyway
         addSequential(new TurnCommand(90)); // Turn to far side of alleyway
-        addSequential(new DriveForwardCommand(Constants.ALLEYWAY_DISTANCE_FROM_ALLIANCE_WALL));
+        addParallel(new SetElevatorToSwitchCommand());
+        addSequential(new DriveForwardCommand(Constants.ALLEYWAY_TOTAL_LENGTH));
         addSequential(new TurnCommand(-90)); // Turn to scale
-        // Drop cube command
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/LeftStartRightSwitch.java

@@ -2,6 +2,8 @@
 
 import org.usfirst.frc.team334.robot.Constants;
 import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
@@ -10,9 +12,10 @@ public class LeftStartRightSwitch extends CommandGroup {
     public LeftStartRightSwitch() {
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_DISTANCE_FROM_ALLIANCE_WALL));
         addSequential(new TurnCommand(90)); // Turn to alleyway
+        addParallel(new SetElevatorToSwitchCommand());
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_TOTAL_LENGTH));
         addSequential(new TurnCommand(90)); // Turn to switch
-        // Drop cube command
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/LeftStartRightSwitchLeftScale.java

@@ -2,23 +2,29 @@
 
 import org.usfirst.frc.team334.robot.Constants;
 import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class LeftStartRightSwitchLeftScale extends CommandGroup {
 
     public LeftStartRightSwitchLeftScale() {
+        addParallel(new SetElevatorToScaleCommand());
         addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SCALE_DIAGONAL));
         addSequential(new WaitCommand(0.3));
         addSequential(new TurnCommand(45)); // Turn to scale diagonally
-        addSequential(new WaitCommand(0.5)); // Placeholder for drop cube command
+        addSequential(new ReleasePowerCubeCommand()); 
         addSequential(new TurnCommand(90)); // Turn to box
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_GAP_DISTANCE_AFTER_TURN));
         addSequential(new TurnCommand(-45)); // Turn to alleyway
+        addParallel(new SetElevatorToExchangeCommand());
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_TOTAL_LENGTH));
         addSequential(new TurnCommand(90)); // Turn to switch box
-        // Intake cube
-        // Drop cube command
+        addSequential(new DriveForwardCommand(10));
+        addSequential(new GrabPowerCubeCommand());
+        addSequential(new SetElevatorToSwitchCommand());
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/LeftStartRightSwitchRightScale.java

@@ -1,8 +1,9 @@
 package org.usfirst.frc.team334.robot.auto.scenarios;
 
 import org.usfirst.frc.team334.robot.Constants;
-import org.usfirst.frc.team334.robot.commands.auton.DriveForwardCommand;
-import org.usfirst.frc.team334.robot.commands.auton.TurnCommand;
+import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
@@ -11,12 +12,15 @@ public class LeftStartRightSwitchRightScale extends CommandGroup {
     public LeftStartRightSwitchRightScale() {
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_DISTANCE_FROM_ALLIANCE_WALL));
         addSequential(new TurnCommand(90)); // Turn to alleyway
+        addParallel(new SetElevatorToSwitchCommand());
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_TOTAL_LENGTH));
         addSequential(new TurnCommand(90)); // Turn to switch
-        // Drop cube command
-        // Pickup cube command
+        addSequential(new ReleasePowerCubeCommand());
+        addSequential(new SetElevatorToExchangeCommand());
+        addSequential(new GrabPowerCubeCommand());
+        addParallel(new SetElevatorToScaleCommand());
         addSequential(new TurnCommand(180)); // Turn to scale
-        // Drop cube command
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/RightStartLeftSwitch.java

@@ -1,8 +1,9 @@
 package org.usfirst.frc.team334.robot.auto.scenarios;
 
 import org.usfirst.frc.team334.robot.Constants;
-import org.usfirst.frc.team334.robot.commands.auton.DriveForwardCommand;
-import org.usfirst.frc.team334.robot.commands.auton.TurnCommand;
+import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
@@ -11,9 +12,10 @@ public class RightStartLeftSwitch extends CommandGroup {
     public RightStartLeftSwitch() {
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_DISTANCE_FROM_ALLIANCE_WALL));
         addSequential(new TurnCommand(-90)); // Turn to alleyway
+        addParallel(new SetElevatorToSwitchCommand());
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_TOTAL_LENGTH));
         addSequential(new TurnCommand(-90)); // Turn to switch
-        // Drop cube command
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/RightStartLeftSwitchLeftScale.java

@@ -1,8 +1,9 @@
 package org.usfirst.frc.team334.robot.auto.scenarios;
 
 import org.usfirst.frc.team334.robot.Constants;
-import org.usfirst.frc.team334.robot.commands.auton.DriveForwardCommand;
-import org.usfirst.frc.team334.robot.commands.auton.TurnCommand;
+import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
@@ -11,12 +12,15 @@ public class RightStartLeftSwitchLeftScale extends CommandGroup {
     public RightStartLeftSwitchLeftScale() {
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_DISTANCE_FROM_ALLIANCE_WALL));
         addSequential(new TurnCommand(-90)); // Turn to alleyway
+        addParallel(new SetElevatorToSwitchCommand());
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_TOTAL_LENGTH));
         addSequential(new TurnCommand(-90)); // Turn to switch
-        // Drop cube command
-        // Pickup cube command
+        addSequential(new ReleasePowerCubeCommand());
+        addSequential(new SetElevatorToExchangeCommand());
+        addSequential(new GrabPowerCubeCommand());
+        addParallel(new SetElevatorToScaleCommand());
         addSequential(new TurnCommand(180)); // Turn to scale
-        // Drop cube command
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/RightStartLeftSwitchRightScale.java

@@ -2,23 +2,28 @@
 
 import org.usfirst.frc.team334.robot.Constants;
 import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class RightStartLeftSwitchRightScale extends CommandGroup {
 
     public RightStartLeftSwitchRightScale() {
+        addParallel(new SetElevatorToScaleCommand());
         addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SCALE_DIAGONAL));
         addSequential(new WaitCommand(0.3));
         addSequential(new TurnCommand(-45)); // Turn to scale diagonally
-        addSequential(new WaitCommand(0.5)); // Placeholder for drop cube command
+        addSequential(new ReleasePowerCubeCommand());
         addSequential(new TurnCommand(-90)); // Turn to box
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_GAP_DISTANCE_AFTER_TURN));
         addSequential(new TurnCommand(45)); // Turn to alleyway
+        addParallel(new SetElevatorToExchangeCommand());
         addSequential(new DriveForwardCommand(Constants.ALLEYWAY_TOTAL_LENGTH));
         addSequential(new TurnCommand(-90)); // Turn to switch box
-        // Intake cube
-        // Drop cube command
+        addSequential(new GrabPowerCubeCommand());
+        addSequential(new SetElevatorToSwitchCommand());
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/RightStartRightSwitch.java

@@ -1,17 +1,19 @@
 package org.usfirst.frc.team334.robot.auto.scenarios;
 
 import org.usfirst.frc.team334.robot.Constants;
-import org.usfirst.frc.team334.robot.commands.auton.DriveForwardCommand;
-import org.usfirst.frc.team334.robot.commands.auton.TurnCommand;
+import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class RightStartRightSwitch extends CommandGroup {
 
     public RightStartRightSwitch() {
+        addParallel(new SetElevatorToSwitchCommand());
         addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SWITCH_PARALLEL));
         addSequential(new TurnCommand(-90)); // Turn to switch
-        // Drop cube command
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/RightStartRightSwitchLeftScale.java

@@ -1,30 +1,32 @@
 package org.usfirst.frc.team334.robot.auto.scenarios;
 
 import org.usfirst.frc.team334.robot.Constants;
-import org.usfirst.frc.team334.robot.commands.auton.DriveForwardCommand;
-import org.usfirst.frc.team334.robot.commands.auton.TurnCommand;
-import org.usfirst.frc.team334.robot.commands.auton.WaitCommand;
+import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class RightStartRightSwitchLeftScale extends CommandGroup {
 
     public RightStartRightSwitchLeftScale() {
+        addParallel(new SetElevatorToSwitchCommand());
         addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SWITCH_PARALLEL));
-        addSequential(new TurnCommand(-90)); // Turn left 90 to face scale
-        addSequential(new WaitCommand(0.5)); // Placeholder for drop cube command
-        addSequential(new TurnCommand(90)); // Turn away from scale
-        addSequential(
-                new DriveForwardCommand(Constants.DISTANCE_TO_SWITCH_REAR - Constants.DISTANCE_TO_SWITCH_PARALLEL));
+        addSequential(new TurnCommand(-90)); // Turn left 90 to face switch
+        addSequential(new ReleasePowerCubeCommand());
+        addSequential(new TurnCommand(90)); // Turn away from switch
+        addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SWITCH_REAR - Constants.DISTANCE_TO_SWITCH_PARALLEL));
+        addParallel(new SetElevatorToExchangeCommand());
         addSequential(new TurnCommand(-90)); // Turn to box behind switch
         addSequential(new DriveForwardCommand(10)); // Go up to box
-        addSequential(new WaitCommand(0.5)); // Placeholder for intake command
+        addSequential(new GrabPowerCubeCommand());
         addSequential(new TurnCommand(90)); // Turn to alleyway
         addSequential(new DriveForwardCommand(15)); // Move to middle of alleyway
         addSequential(new TurnCommand(-90)); // Turn to far side of alleyway
-        addSequential(new DriveForwardCommand(Constants.ALLEYWAY_DISTANCE_FROM_ALLIANCE_WALL));
+        addParallel(new SetElevatorToScaleCommand());
+        addSequential(new DriveForwardCommand(Constants.ALLEYWAY_TOTAL_LENGTH));
         addSequential(new TurnCommand(90)); // Turn to scale
-        // Drop cube command
+        addSequential(new ReleasePowerCubeCommand());
     }
 
 }

src/org/usfirst/frc/team334/robot/auto/scenarios/RightStartRightSwitchRightScale.java

@@ -2,20 +2,25 @@
 
 import org.usfirst.frc.team334.robot.Constants;
 import org.usfirst.frc.team334.robot.commands.auton.*;
+import org.usfirst.frc.team334.robot.commands.elevator.*;
+import org.usfirst.frc.team334.robot.commands.intake.*;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class RightStartRightSwitchRightScale extends CommandGroup {
 
     public RightStartRightSwitchRightScale() {
+        addParallel(new SetElevatorToScaleCommand());
         addSequential(new DriveForwardCommand(Constants.DISTANCE_TO_SCALE_DIAGONAL));
         addSequential(new WaitCommand(0.3));
         addSequential(new TurnCommand(-45)); // Turn to scale diagonally
-        addSequential(new WaitCommand(0.5)); // Placeholder for drop box command
+        addSequential(new ReleasePowerCubeCommand());
         addSequential(new TurnCommand(-90)); // Turn to box
+        addParallel(new SetElevatorToExchangeCommand());
         addSequential(new DriveForwardCommand(Constants.SWITCH_GAP_DISTANCE_AFTER_TURN));
-        // Intake command
-        // Drop cube command
+        addSequential(new GrabPowerCubeCommand());
+        addSequential(new SetElevatorToSwitchCommand());
+        addSequential(new ReleasePowerCubeCommand()); // du yu knoe de wey
     }
 
 }