X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2FRobot.java;h=30926c7af8a9e6b0168b2fa1181e4b59bda65ba0;hb=e2c4d3a5b10fa513a4e4e654a756f2e84eda02d0;hp=425e6704ae5e2170bcefe31c57d7899047c86ea4;hpb=1e039ebdf71f746dd2fb21f7bcf811d4d80ac3fa;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 425e6704..30926c7a 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,14 +1,14 @@ 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.commands.intakearm.Photogate; 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 +18,27 @@ 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; + public static Photogate photogate; // Sendable Choosers send a drop down menu to the Smart Dashboard. SendableChooser positionChooser; SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense, positionFourDefense, positionFiveDefense; - // Gyro stuff - short rawValue; - public GyroClass gyro; - - double now; - double degreesIncreased; - double degrees; - - Rotation rotation; - @Override public void robotInit() { - // driveTrain = new DriveTrain(); - gyro = new GyroClass(I2C.Port.kOnboard, gyro.ITG3200_ADDRESS_AD0_LOW); - // 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(); + photogate = new Photogate(); + initializeSendableChoosers(); + addPositionChooserOptions(); + addDefensesToAllDefenseSendableChoosers(); + sendSendableChoosersToSmartDashboard(); } private void initializeSendableChoosers() { @@ -72,14 +51,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 +70,49 @@ 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" + " Will probably work...", 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,44 +122,12 @@ public class Robot extends IterativeRobot { @Override public void teleopInit() { + Scheduler.getInstance().add(new SetLowGear()); } @Override public void teleopPeriodic() { Scheduler.getInstance().run(); - - } - - public double getZAxisDegreesPerSeconds() { - double rawValue = (double) gyro.getRotationZ() / 14.375; - return rawValue; - } - - public void initializeGyro() { - degrees = 0; - then = System.nanoTime() / 1000000000.0; - gyro.reset(); - gyro.initialize(); - System.out.println("Testing Gyro Init"); - } - - public void addZAxisDegrees() { - double degreesRead = getZAxisDegreesPerSeconds(); - now = System.nanoTime(); - now = now / (1000000000.0); - double differenceInTime = now - then; - then = now; - degreesIncreased = differenceInTime * degreesRead; - - // 0.0 = register - // + 1.0 is the formula constant - // - degrees += degreesIncreased; - - } - - public double getDegrees() { - return degrees; } }