X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2FRobot.java;h=2f97c9ad36f58f9cf91af5a6c21cda66fe624088;hb=b378dfef28369af926b17f5e4bbe8ea4b26d9769;hp=5e916ac9559e5869d5c76ac4c6489316c98313b6;hpb=62b4062044e924447b6c52521b47497a21b8f236;p=3501%2Fstronghold-2016 diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 5e916ac9..2f97c9ad 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,14 +1,13 @@ package org.usfirst.frc.team3501.robot; -import org.usfirst.frc.team3501.robot.AnotherGyroClass.Rotation; +import org.usfirst.frc.team3501.robot.Constants.Auton; import org.usfirst.frc.team3501.robot.Constants.Defense; -import org.usfirst.frc.team3501.robot.subsystems.DefenseArm; +import org.usfirst.frc.team3501.robot.commands.auton.ChooseStrategy; +import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear; import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; import org.usfirst.frc.team3501.robot.subsystems.IntakeArm; -import org.usfirst.frc.team3501.robot.subsystems.Scaler; import org.usfirst.frc.team3501.robot.subsystems.Shooter; -import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -18,48 +17,25 @@ public class Robot extends IterativeRobot { public static OI oi; public static DriveTrain driveTrain; public static Shooter shooter; - - public static Scaler scaler; - double then; - public static IntakeArm intakeArm; - public static DefenseArm defenseArm; // Sendable Choosers send a drop down menu to the Smart Dashboard. SendableChooser positionChooser; SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense, positionFourDefense, positionFiveDefense; - // Gyro stuff - short rawValue; - public AnotherGyroClass gyro; - - double now; - double degreesIncreased; - double degrees; - - Rotation rotation; - @Override public void robotInit() { - // driveTrain = new DriveTrain(); - gyro = new AnotherGyroClass(I2C.Port.kOnboard, false); - // oi = new OI(); - + driveTrain = new DriveTrain(); shooter = new Shooter(); - scaler = new Scaler(); intakeArm = new IntakeArm(); - // Sendable Choosers allows the driver to select the position of the - // robot - // and the positions of the defenses from a drop-down menu on the Smart - // Dashboard - // make the Sendable Choosers - // initializeSendableChoosers(); - // addPositionChooserOptions(); - // addDefensesToAllDefenseSendableChooosers(); - // sendSendableChoosersToSmartDashboard(); + oi = new OI(); + initializeSendableChoosers(); + addPositionChooserOptions(); + addDefensesToAllDefenseSendableChoosers(); + sendSendableChoosersToSmartDashboard(); } private void initializeSendableChoosers() { @@ -72,14 +48,15 @@ public class Robot extends IterativeRobot { } private void addPositionChooserOptions() { - positionChooser.addDefault("Position 1", 1); + positionChooser.addDefault("None", 0); + positionChooser.addObject("Position 1", 1); positionChooser.addObject("Position 2", 2); positionChooser.addObject("Position 3", 3); positionChooser.addObject("Position 4", 4); positionChooser.addObject("Position 5", 5); } - private void addDefensesToAllDefenseSendableChooosers() { + private void addDefensesToAllDefenseSendableChoosers() { addDefenseOptions(positionOneDefense); addDefenseOptions(positionTwoDefense); addDefenseOptions(positionThreeDefense); @@ -90,57 +67,50 @@ public class Robot extends IterativeRobot { private void addDefenseOptions(SendableChooser chooser) { chooser.addDefault("Portcullis", Defense.PORTCULLIS); chooser.addObject("Sally Port", Defense.SALLY_PORT); - chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN); - chooser.addObject("Low Bar", Defense.LOW_BAR); - chooser.addObject("Cheval De Frise", Defense.CHEVAL_DE_FRISE); + chooser.addObject("Rough Terrain" + Auton.ROUGH_TERRAIN_SPEED + " " + + Auton.ROUGH_TERRAIN_TIME, Defense.ROUGH_TERRAIN); + chooser.addObject("Low Bar" + Auton.LOW_BAR_SPEED + " " + + Auton.LOW_BAR_TIME, Defense.LOW_BAR); + chooser.addObject("Chival De Frise", Defense.CHIVAL_DE_FRISE); chooser.addObject("Drawbridge", Defense.DRAWBRIDGE); - chooser.addObject("Moat", Defense.MOAT); - chooser.addObject("Rock Wall", Defense.ROCK_WALL); + chooser.addObject("Moat" + Auton.MOAT_SPEED + " " + Auton.MOAT_TIME, + Defense.MOAT); + chooser.addObject("Rock Wall" + Auton.ROCK_WALL_SPEED + " " + + Auton.ROCK_WALL_TIME, Defense.ROCK_WALL); } private void sendSendableChoosersToSmartDashboard() { SmartDashboard.putData("PositionChooser", positionChooser); - SmartDashboard.putData("Position One Defense Chooser", positionOneDefense); - SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense); - SmartDashboard.putData("Position Three Defense Chooser", - positionThreeDefense); - SmartDashboard.putData("Position Four Defense Chooser", - positionFourDefense); - SmartDashboard.putData("Position Five Defense Chooser", - positionFiveDefense); - - SmartDashboard.putData("Position Four Defense Chooser", - positionFourDefense); - SmartDashboard.putData("Position Five Defense Chooser", - positionFiveDefense); - - shooter = new Shooter(); + SmartDashboard.putData("Position 1 Defense Chooser", positionOneDefense); + SmartDashboard.putData("Position 2 Defense Chooser", positionTwoDefense); + SmartDashboard.putData("Position 3 Defense Chooser", positionThreeDefense); + SmartDashboard.putData("Position 4 Defense Chooser", positionFourDefense); + SmartDashboard.putData("Position 5 Defense Chooser", positionFiveDefense); } @Override public void autonomousInit() { - Scheduler.getInstance().run(); - // get options chosen from drop down menu Integer chosenPosition = (Integer) positionChooser.getSelected(); - Integer chosenDefense = 0; - - switch (chosenPosition) { - case 1: - chosenDefense = (Integer) positionOneDefense.getSelected(); - case 2: - chosenDefense = (Integer) positionTwoDefense.getSelected(); - case 3: - chosenDefense = (Integer) positionThreeDefense.getSelected(); - case 4: - chosenDefense = (Integer) positionFourDefense.getSelected(); - case 5: - chosenDefense = (Integer) positionFiveDefense.getSelected(); - } - - System.out.println("Chosen Position: " + chosenPosition); - System.out.println("Chosen Defense: " + chosenDefense); + Defense chosenDefense = null; + + if (chosenPosition == 1) + chosenDefense = (Defense) positionOneDefense.getSelected(); + else if (chosenPosition == 2) + chosenDefense = (Defense) positionTwoDefense.getSelected(); + else if (chosenPosition == 3) + chosenDefense = (Defense) positionThreeDefense.getSelected(); + else if (chosenPosition == 4) + chosenDefense = (Defense) positionFourDefense.getSelected(); + else if (chosenPosition == 5) + chosenDefense = (Defense) positionFiveDefense.getSelected(); + + if (chosenPosition != 0) + Scheduler.getInstance().add( + new ChooseStrategy(chosenPosition, chosenDefense)); + + // Scheduler.getInstance().add(new TimeDrive(.6, 4)); } @Override @@ -150,12 +120,12 @@ public class Robot extends IterativeRobot { @Override public void teleopInit() { + Scheduler.getInstance().add(new SetLowGear()); } @Override public void teleopPeriodic() { Scheduler.getInstance().run(); - } }