public AutoState getState(IRobot robot) { int chooser = (int) SmartDashboard.getNumber("Autonomous", 1); boolean backwards = SmartDashboard.getBoolean("Autonomous Backwards", false); AutoState chain = new NothingAutonomous(); switch (chooser) { // TODO: Refine and test the state chains. case 1: if (backwards) { chain .addNext(new ForwardAutonomous(1000, -speed)) .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro())); } else { chain.addNext(new ForwardAutonomous(1000, speed)); } // FieldPosition.Left chain .addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro())) // TODO: Determine if positive means clockwise. .addNext(null); // TODO: Replace with an Autonomous that drives forward until it reaches the goal. case 2: if (backwards) { chain .addNext(new ForwardAutonomous(1000, -speed)) .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro())); } else { chain.addNext(new ForwardAutonomous(1000, speed)); } // FieldPosition.Left chain .addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro())) .addNext(null); case 3: if (backwards) { chain .addNext(new ForwardAutonomous(1000, -speed)) .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro())); } else { chain.addNext(new ForwardAutonomous(1000, speed)); } // FieldPosition.Left chain .addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro())) .addNext(null); case 4: // This is right in front of the tower. What is our procedure for this? if (backwards) { chain .addNext(new ForwardAutonomous(1000, -speed)) .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro())); } else { chain.addNext(new ForwardAutonomous(1000, speed)); } // FieldPosition.Right chain .addNext(new AngleAutonomous(-90.0, 0.0, robot.getSensorData().getGyro())) .addNext(null); case 5: if (backwards) { chain .addNext(new ForwardAutonomous(1000, -speed)) .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro())); } else { chain.addNext(new ForwardAutonomous(1000, speed)); } // FieldPosition.Right chain .addNext(new AngleAutonomous(-90.0, 0.0, robot.getSensorData().getGyro())) .addNext(null); default: // Just nothing } return chain; }