From f8bfcc62a43e76e355b581152ef9710c2fb8cd58 Mon Sep 17 00:00:00 2001 From: Harel Dor Date: Sat, 5 Mar 2016 12:44:40 -0800 Subject: [PATCH] Purge code of all unused fields/classes. Only testing code. --- .../usfirst/frc/team3501/robot/Constants.java | 132 +-- src/org/usfirst/frc/team3501/robot/OI.java | 2 - src/org/usfirst/frc/team3501/robot/Robot.java | 99 +-- .../robot/commands/auton/AimAndAlign.java | 30 - .../robot/commands/auton/AlignToScore.java | 173 ---- .../robot/commands/auton/CompactRobot.java | 14 - .../commands/auton/DefaultAutonStrategy.java | 53 -- .../robot/commands/auton/LiftPortcullis.java | 50 -- .../robot/commands/auton/LiftRobot.java | 25 - .../commands/auton/PassChevalDeFrise.java | 44 - .../robot/commands/auton/PassDrawBridge.java | 41 - .../robot/commands/auton/PassLowBar.java | 38 - .../robot/commands/auton/PassMoat.java | 41 - .../robot/commands/auton/PassPortcullis.java | 19 - .../robot/commands/auton/PassRampart.java | 41 - .../robot/commands/auton/PassRockWall.java | 39 - .../commands/auton/PassRoughTerrain.java | 40 - .../robot/commands/auton/PassSallyPort.java | 46 - .../defensearm/LowerDefenseArmContinuous.java | 46 - .../LowerDefenseWristContinuous.java | 46 - .../defensearm/RaiseDefenseArmContinuous.java | 45 - .../RaiseDefenseWristContinuous.java | 46 - .../defensearm/RetractDefenseArm.java | 24 - .../commands/defensearm/SetArmToAngle.java | 67 -- .../commands/defensearm/SetHandToAngle.java | 67 -- .../robot/commands/driving/DriveDistance.java | 64 -- .../robot/commands/driving/DriveForTime.java | 63 -- .../robot/commands/driving/JoystickDrive.java | 5 +- .../team3501/robot/commands/driving/Stop.java | 34 - .../robot/commands/driving/Turn180.java | 16 - .../robot/commands/driving/TurnForAngle.java | 62 -- .../robot/commands/driving/TurnForTime.java | 73 -- .../intakearm/BallRollerExpelContinuous.java | 48 -- .../intakearm/BallRollerIntakeContinuous.java | 49 -- .../robot/commands/intakearm/ExpelBall.java | 54 -- .../robot/commands/intakearm/IntakeBall.java | 57 -- .../robot/commands/scaler/ClampBar.java | 39 - .../robot/commands/scaler/ExtendLift.java | 34 - .../robot/commands/scaler/RetractLift.java | 34 - .../commands/scaler/RunWinchContinuous.java | 53 -- .../robot/commands/scaler/StopWinch.java | 34 - .../robot/commands/scaler/ToggleScaling.java | 14 - .../robot/commands/shooter/Shoot.java | 15 +- .../frc/team3501/robot/sensors/GyroLib.java | 799 ------------------ .../frc/team3501/robot/sensors/Lidar.java | 85 -- .../frc/team3501/robot/sensors/Photogate.java | 49 -- .../robot/sensors/RotationTracker.java | 74 -- .../team3501/robot/subsystems/DefenseArm.java | 160 ---- .../team3501/robot/subsystems/DriveTrain.java | 134 +-- .../frc/team3501/robot/subsystems/Scaler.java | 49 -- .../team3501/robot/subsystems/Shooter.java | 27 - 51 files changed, 20 insertions(+), 3373 deletions(-) delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/AimAndAlign.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/auton/CompactRobot.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/DefaultAutonStrategy.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/LiftPortcullis.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/LiftRobot.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/PassChevalDeFrise.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/PassDrawBridge.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/auton/PassPortcullis.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/PassSallyPort.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/defensearm/LowerDefenseArmContinuous.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/defensearm/LowerDefenseWristContinuous.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/defensearm/RaiseDefenseArmContinuous.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/defensearm/RaiseDefenseWristContinuous.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/defensearm/RetractDefenseArm.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/defensearm/SetArmToAngle.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/defensearm/SetHandToAngle.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/driving/DriveForTime.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/BallRollerExpelContinuous.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/BallRollerIntakeContinuous.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/ExpelBall.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/IntakeBall.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/scaler/ClampBar.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/scaler/ExtendLift.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/scaler/RetractLift.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/scaler/RunWinchContinuous.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/scaler/StopWinch.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/scaler/ToggleScaling.java delete mode 100644 src/org/usfirst/frc/team3501/robot/sensors/GyroLib.java delete mode 100644 src/org/usfirst/frc/team3501/robot/sensors/Lidar.java delete mode 100644 src/org/usfirst/frc/team3501/robot/sensors/Photogate.java delete mode 100644 src/org/usfirst/frc/team3501/robot/sensors/RotationTracker.java delete mode 100755 src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java delete mode 100755 src/org/usfirst/frc/team3501/robot/subsystems/Scaler.java diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index f57595ed..f8c56b76 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -47,18 +47,12 @@ public class Constants { public static final double INCHES_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256; public static double kp = 0.013, ki = 0.000015, kd = -0.002; - public static double gp = 0.018, gi = 0.000015, gd = 0; - public static double encoderTolerance = 8.0, gyroTolerance = 5.0; - - public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3; - public static double time = 0; + public static double encoderTolerance = 8.0; // Gearing constants public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward; public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse; - public static boolean inverted = false; - public static final double PASS_DEFENSE_TIMEOUT = 10; // find this } @@ -113,128 +107,4 @@ public class Constants { public static final double INTAKE_SPEED = 0.5; public static final double OUTPUT_SPEED = -0.5; } - - public static class DefenseArm { - // Potentiometer related ports - public static final int ARM_CHANNEL = 0; - public static final int ARM_PORT = 0; - public static final int HAND_PORT = 1; - public static final int HAND_CHANNEL = 1; - public final static double FULL_RANGE = 270.0; // in degrees - public final static double OFFSET = -135.0; // in degrees - - public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3 - // level - public final static double ARM_LENGTH = 0; // TODO: find actual length - public final static double HAND_LENGTH = 0; // TODO: find actual length - public final static double ARM_MOUNTED_HEIGHT = 0; // TODO: find actual - // height - } - - public static class Auton { - /* - * Distance dead reckoning constants - */ - public static final double POS1_DIST1 = 109; - public static final double POS1_TURN1 = 60; - public static final double POS1_DIST2 = 0; - - // constants for position 2 - public static final double POS2_DIST1 = 140; - public static final double POS2_TURN1 = 60; - public static final double POS2_DIST2 = 0; - - // constants for position 3 - public static final double POS3_DIST1 = 0; - public static final double POS3_TURN1 = 90; - public static final double POS3_DIST2 = 35.5; - public static final double POS3_TURN2 = -90; - public static final double POS3_DIST3 = 0; - - // constants for position 4 - public static final double POS4_DIST1 = 0; - public static final double POS4_TURN1 = -90; - public static final double POS4_DIST2 = 18.5; - public static final double POS4_TURN2 = 90; - public static final double POS4_DIST3 = 0; - - // constants for position 5 - public static final double POS5_DIST1 = 0; - public static final double POS5_TURN1 = -90; - public static final double POS5_DIST2 = 72.5; - public static final double POS5_TURN2 = 90; - public static final double POS5_DIST3 = 0; - public static final double DRIVE_MAX_TIMEOUT = 3.0; - public static final double TURN_MAX_TIMEOUT = 5.0; - - /* - * Time dead Reckoning constants - */ - public static final double POS1_DIST1_TIME = 109; - public static final double POS1_DRIVE_MAXSPEED = 0.5; - public static final double POS1_TURN1_TIME = 60; - public static final double POS1_TURN_MAXSPEED = 0.5; - public static final double POS1_DIST2_TIME = 0; - - // constants for position 2 - - public static final double POS2_DIST1_TIME = 109; - public static final double POS2_DRIVE_MAXSPEED = 0.5; - public static final double POS2_TURN1_TIME = 60; - public static final double POS2_TURN_MAXSPEED = 0.5; - public static final double POS2_DIST2_TIME = 0; - - // constants for position 3 - - public static final double POS3_DIST1_TIME = 109; - public static final double POS3_DRIVE_MAXSPEED = 0.5; - public static final double POS3_TURN1_TIME = 60; - public static final double POS3_TURN_MAXSPEED = 0.5; - public static final double POS3_DIST2_TIME = 0; - // constants for position 4 - - public static final double POS4_DIST1_TIME = 109; - public static final double POS4_DRIVE_MAXSPEED = 0.5; - public static final double POS4_TURN1_TIME = 60; - public static final double POS4_TURN_MAXSPEED = 0.5; - public static final double POS4_DIST2_TIME = 0; - // constants for position 5 - - public static final double POS5_DIST1_TIME = 109; - public static final double POS5_DRIVE_MAXSPEED = 0.5; - public static final double POS5_TURN1_TIME = 60; - public static final double POS5_TURN_MAXSPEED = 0.5; - public static final double POS5_DIST2_TIME = 0; - - // Passing Defenses Constants - - public static final double DEFAULT_SPEED = 0.5; - public static final boolean IS_USING_TIME = true; - - // dead reckoning time and speed constants for driving through defenses - // TODO: find the times it takes to pass each defense - public static final double PASS_ROCK_WALL_TIME = 0; - public static final double PASS_ROCK_WALL_SPEED = 0; - public static final double PASS_ROCK_WALL_DIST = 0; - public static final double PASS_LOW_BAR_TIME = 0; - public static final double PASS_LOW_BAR_SPEED = 0; - public static final double PASS_LOW_BAR_DIST = 0; - public static final double PASS_MOAT_TIME = 0; - public static final double PASS_MOAT_SPEED = 0; - public static final double PASS_MOAT_DIST = 0; - public static final double PASS_RAMPART_TIME = 0; - public static final double PASS_RAMPART_SPEED = 0; - public static final double PASS_RAMPART_DIST = 0; - public static final double PASS_ROUGH_TERRAIN_TIME = 0; - public static final double PASS_ROUGH_TERRAIN_SPEED = 0; - public static final double PASS_ROUGH_TERRAIN_DIST = 0; - } - - public enum Direction { - UP, DOWN, RIGHT, LEFT, FORWARD, BACKWARD; - } - - public enum Defense { - PORTCULLIS, SALLY_PORT, ROUGH_TERRAIN, LOW_BAR, CHEVAL_DE_FRISE, DRAWBRIDGE, MOAT, ROCK_WALL, RAMPART; - } } diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index b59fe490..fdb91263 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -6,8 +6,6 @@ import org.usfirst.frc.team3501.robot.commands.driving.ToggleFront; import org.usfirst.frc.team3501.robot.commands.intakearm.RunIntake; import org.usfirst.frc.team3501.robot.commands.intakearm.StopIntake; import org.usfirst.frc.team3501.robot.commands.shooter.Shoot; -import org.usfirst.frc.team3501.robot.subsystems.IntakeArm; - import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 1111fea6..d10b2c5f 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,33 +1,20 @@ package org.usfirst.frc.team3501.robot; -import org.usfirst.frc.team3501.robot.Constants.Defense; import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear; +import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArm; import org.usfirst.frc.team3501.robot.commands.shooter.ResetCatapult; -import org.usfirst.frc.team3501.robot.subsystems.DefenseArm; 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.IterativeRobot; import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Robot extends IterativeRobot { public static OI oi; public static DriveTrain driveTrain; public static Shooter shooter; - - public static Scaler scaler; - 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; @Override public void robotInit() { @@ -35,93 +22,12 @@ public class Robot extends IterativeRobot { oi = new OI(); shooter = new Shooter(); - scaler = new Scaler(); intakeArm = new IntakeArm(); - - initializeSendableChoosers(); - addPositionChooserOptions(); - addDefensesToAllDefenseSendableChoosers(); - sendSendableChoosersToSmartDashboard(); - - } - - private void initializeSendableChoosers() { - positionChooser = new SendableChooser(); - positionOneDefense = new SendableChooser(); - positionTwoDefense = new SendableChooser(); - positionThreeDefense = new SendableChooser(); - positionFourDefense = new SendableChooser(); - positionFiveDefense = new SendableChooser(); - } - - private void addPositionChooserOptions() { - positionChooser.addDefault("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 addDefensesToAllDefenseSendableChoosers() { - addDefenseOptions(positionOneDefense); - addDefenseOptions(positionTwoDefense); - addDefenseOptions(positionThreeDefense); - addDefenseOptions(positionFourDefense); - addDefenseOptions(positionFiveDefense); - } - - 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("Drawbridge", Defense.DRAWBRIDGE); - chooser.addObject("Moat", Defense.MOAT); - chooser.addObject("Rock Wall", 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(); - } @Override public void autonomousInit() { Scheduler.getInstance().run(); - - // get options chosen from drop down menu - Integer chosenPosition = (Integer) positionChooser.getSelected(); - Integer chosenDefense = 0; - - if (chosenPosition == 1) - chosenDefense = (Integer) positionOneDefense.getSelected(); - else if (chosenPosition == 2) - chosenDefense = (Integer) positionTwoDefense.getSelected(); - else if (chosenPosition == 3) - chosenDefense = (Integer) positionThreeDefense.getSelected(); - else if (chosenPosition == 4) - chosenDefense = (Integer) positionFourDefense.getSelected(); - else if (chosenPosition == 5) - chosenDefense = (Integer) positionFiveDefense.getSelected(); - - System.out.println("Chosen Position: " + chosenPosition); - System.out.println("Chosen Defense: " + chosenDefense); } @Override @@ -135,6 +41,9 @@ public class Robot extends IterativeRobot { // gear Scheduler.getInstance().add(new ResetCatapult()); // Reset catapult at start // of each match. + + Scheduler.getInstance().add(new MoveIntakeArm(Constants.IntakeArm.EXTEND)); + // Start testing with intake arm extended TODO remove this } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AimAndAlign.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AimAndAlign.java deleted file mode 100755 index d304013f..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/AimAndAlign.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import edu.wpi.first.wpilibj.command.Command; - -public class AimAndAlign extends Command { - - public AimAndAlign() { - } - - @Override - protected void initialize() { - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return false; - } - - @Override - protected void end() { - } - - @Override - protected void interrupted() { - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java deleted file mode 100644 index e6fdf157..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java +++ /dev/null @@ -1,173 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; -import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; -import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** - * This command group will be used in autonomous. Based on what position the - * robot is in, the robot will align with the goal. In the Software 2015-2016 - * Google folder is a picture explaining each of the cases. - * - * dependency on sensors: lidars, encoders, gyro - * - * dependency on subsystems: drivetrain - * - * dependency on other commands: TurnForAngle(), DriveForDistance() - * - * pre-condition: robot is flush against a defense at the specified position in - * the opponent's courtyard - * - * post-condition: the robot is parallel to one of the three goals and the - * shooter is facing that goal - * - */ -public class AlignToScore extends CommandGroup { - - private final double DEFAULT_SPEED = 0.5; - private final double maxTimeout = 5; - - // in inches - // assuming that positive angle means turning right - // and negative angle means turning left - - // constants for position 1: low bar - - public double horizontalDistToGoal; - - public AlignToScore(int position) { - if (Constants.Auton.IS_USING_TIME) { - if (position == 1) { - addSequential(new DriveForTime(Constants.Auton.POS1_DIST1_TIME, - Constants.Auton.POS1_DRIVE_MAXSPEED)); - addSequential(new TurnForAngle(Constants.Auton.POS1_TURN1, - Constants.Auton.POS1_TURN_MAXSPEED)); - addSequential(new DriveDistance(Constants.Auton.POS1_DIST2_TIME, - Constants.Auton.POS1_DRIVE_MAXSPEED)); - horizontalDistToGoal = 0; - } - else if (position == 2) { - addSequential(new DriveForTime(Constants.Auton.POS2_DIST1_TIME, - Constants.Auton.POS2_DRIVE_MAXSPEED)); - addSequential(new TurnForAngle(Constants.Auton.POS2_TURN1, - Constants.Auton.POS2_TURN_MAXSPEED)); - addSequential(new DriveDistance(Constants.Auton.POS2_DIST2_TIME, - Constants.Auton.POS2_DRIVE_MAXSPEED)); - horizontalDistToGoal = 0; - } - else if (position == 3) { - addSequential(new DriveForTime(Constants.Auton.POS3_DIST1_TIME, - Constants.Auton.POS3_DRIVE_MAXSPEED)); - addSequential(new TurnForAngle(Constants.Auton.POS3_TURN1, - Constants.Auton.POS3_TURN_MAXSPEED)); - addSequential(new DriveDistance(Constants.Auton.POS3_DIST2_TIME, - Constants.Auton.POS3_DRIVE_MAXSPEED)); - horizontalDistToGoal = 0; - } - else if (position == 4) { - addSequential(new DriveForTime(Constants.Auton.POS4_DIST1_TIME, - Constants.Auton.POS4_DRIVE_MAXSPEED)); - addSequential(new TurnForAngle(Constants.Auton.POS4_TURN1, - Constants.Auton.POS4_TURN_MAXSPEED)); - addSequential(new DriveDistance(Constants.Auton.POS4_DIST2_TIME, - Constants.Auton.POS4_DRIVE_MAXSPEED)); - horizontalDistToGoal = 0; - } - else if (position == 5) { - addSequential(new DriveForTime(Constants.Auton.POS5_DIST1_TIME, - Constants.Auton.POS5_DRIVE_MAXSPEED)); - addSequential(new TurnForAngle(Constants.Auton.POS5_TURN1, - Constants.Auton.POS5_TURN_MAXSPEED)); - addSequential(new DriveDistance(Constants.Auton.POS5_DIST2_TIME, - Constants.Auton.POS5_DRIVE_MAXSPEED)); - horizontalDistToGoal = 0; - } - } - else { - // position 1 is always the low bar - if (position == 1) { - addSequential(new DriveDistance(Constants.Auton.POS1_DIST1, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - addSequential(new TurnForAngle(Constants.Auton.POS1_TURN1, - Constants.Auton.TURN_MAX_TIMEOUT)); - addSequential(new DriveDistance(Constants.Auton.POS1_DIST2, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - } else if (position == 2) { - - addSequential(new DriveDistance(Constants.Auton.POS2_DIST1, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - addSequential(new TurnForAngle(Constants.Auton.POS2_TURN1, - Constants.Auton.TURN_MAX_TIMEOUT)); - addSequential(new DriveDistance(Constants.Auton.POS2_DIST2, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - - } else if (position == 3) { - - addSequential(new DriveDistance(Constants.Auton.POS3_DIST1, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - addSequential(new TurnForAngle(Constants.Auton.POS3_TURN1, maxTimeout)); - addSequential(new DriveDistance(Constants.Auton.POS3_DIST2, - Constants.Auton.TURN_MAX_TIMEOUT)); - addSequential(new TurnForAngle(Constants.Auton.POS3_TURN2, maxTimeout)); - addSequential(new DriveDistance(Constants.Auton.POS3_DIST3, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - - } else if (position == 4) { - - addSequential(new DriveDistance(Constants.Auton.POS4_DIST1, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - addSequential(new TurnForAngle(Constants.Auton.POS4_TURN1, maxTimeout)); - addSequential(new DriveDistance(Constants.Auton.POS4_DIST2, - Constants.Auton.TURN_MAX_TIMEOUT)); - addSequential(new TurnForAngle(Constants.Auton.POS4_TURN2, maxTimeout)); - addSequential(new DriveDistance(Constants.Auton.POS4_DIST3, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - - } else if (position == 5) { - - addSequential(new DriveDistance(Constants.Auton.POS5_DIST1, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - addSequential(new TurnForAngle(Constants.Auton.POS5_TURN1, maxTimeout)); - addSequential(new DriveDistance(Constants.Auton.POS5_DIST2, - Constants.Auton.TURN_MAX_TIMEOUT)); - addSequential(new TurnForAngle(Constants.Auton.POS5_TURN2, maxTimeout)); - addSequential(new DriveDistance(Constants.Auton.POS5_DIST3, - Constants.Auton.DRIVE_MAX_TIMEOUT)); - } - } - } - // following commented out method is calculations for path of robot in auton - // after passing through defense using two lidars - /* - * public static double lidarCalculateAngleToTurn(int position, - * double horizontalDistToGoal) { - * double leftDist = Robot.driveTrain.getLeftLidarDistance(); - * double rightDist = Robot.driveTrain.getRightLidarDistance(); - * - * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2); - * double distToTower; - * // TODO: figure out if we do want to shoot into the side goal if we are - * // in position 1 or 2, or if we want to change that - * if (position == 1 || position == 2) { - * distToTower = Math - * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) - * - DIST_CASTLE_WALL_TO_SIDE_GOAL; - * } - * - * // TODO: figure out if we do want to shoot into the font goal if we are - * // in position 3, 4, 5, or if we want to change that - * else { - * distToTower = Math - * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) - * - DIST_CASTLE_WALL_TO_SIDE_GOAL; - * } - * - * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal); - * - * return angleToTurn; - * } - */ -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/CompactRobot.java b/src/org/usfirst/frc/team3501/robot/commands/auton/CompactRobot.java deleted file mode 100644 index 8596cd3b..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/CompactRobot.java +++ /dev/null @@ -1,14 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** - * - */ -public class CompactRobot extends CommandGroup { - - // TODO: implement CompactRobot with updated Robot capabilities - public CompactRobot() { - - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/DefaultAutonStrategy.java b/src/org/usfirst/frc/team3501/robot/commands/auton/DefaultAutonStrategy.java deleted file mode 100755 index 6ccef7c5..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/DefaultAutonStrategy.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Constants.Defense; -import org.usfirst.frc.team3501.robot.commands.shooter.Shoot; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.command.WaitCommand; - -/** - * The default autonomous strategy involves passing the defense that is in front - * of it, aiming the robot/ shooter towards the goal, and shooting. - */ - -public class DefaultAutonStrategy extends CommandGroup { - - public DefaultAutonStrategy(int position, Defense defense) { - - if (defense == Constants.Defense.PORTCULLIS) - addSequential(new LiftPortcullis()); - - else if (defense == Constants.Defense.SALLY_PORT) - addSequential(new PassSallyPort()); - - else if (defense == Constants.Defense.ROUGH_TERRAIN) - addSequential(new PassRoughTerrain()); - - else if (defense == Constants.Defense.LOW_BAR) - addSequential(new PassLowBar()); - - else if (defense == Constants.Defense.CHEVAL_DE_FRISE) - addSequential(new PassChevalDeFrise()); - - else if (defense == Constants.Defense.DRAWBRIDGE) - addSequential(new PassDrawBridge()); - - else if (defense == Constants.Defense.MOAT) - addSequential(new PassMoat()); - - else if (defense == Constants.Defense.ROCK_WALL) - addSequential(new PassRockWall()); - - else if (defense == Constants.Defense.RAMPART) - addSequential(new PassRampart()); - - addSequential(new AlignToScore(position)); - // TODO: test for how long robot should wait - addSequential(new WaitCommand(5.0)); - addSequential(new Shoot()); - - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/LiftPortcullis.java b/src/org/usfirst/frc/team3501/robot/commands/auton/LiftPortcullis.java deleted file mode 100755 index e81f5c1f..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/LiftPortcullis.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will move the robot forward while lifting the defense arm up. - * - * pre-condition: robot is flush against the ramp of the outerworks in front of - * the portcullis - * - * This command has to drive forward at the same time because that's how the - * robot opens the portcullis - * - * post-condition: the robot has passed the portcullis and is in the next zone - * - * @author Meryem and Avi - * - */ - -public class LiftPortcullis extends Command { - - public LiftPortcullis() { - } - - @Override - protected void end() { - - } - - @Override - protected void execute() { - - } - - @Override - protected void initialize() { - - } - - @Override - protected void interrupted() { - - } - - @Override - protected boolean isFinished() { - return false; - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/LiftRobot.java b/src/org/usfirst/frc/team3501/robot/commands/auton/LiftRobot.java deleted file mode 100755 index a2f1f262..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/LiftRobot.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.commands.scaler.RunWinchContinuous; -import org.usfirst.frc.team3501.robot.commands.scaler.StopWinch; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/*** - * This command group runs the winch to lift robot. - * Requires Scaler - * - * @author shaina - * - */ -public class LiftRobot extends CommandGroup { - - public LiftRobot() { - requires(Robot.scaler); - - addSequential(new RunWinchContinuous(Constants.Scaler.SCALE_SPEED, Constants.Scaler.SECONDS_TO_SCALE)); - addSequential(new StopWinch()); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassChevalDeFrise.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassChevalDeFrise.java deleted file mode 100755 index 0762a907..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassChevalDeFrise.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will move the intake arm downwards using a motor to lower the - * cheval de frise and drive over it. - * - * pre-condition: robot is flush against the ramp of the outerworks in front of - * the cheval de frise - * - * post-condition: the robot has passed the cheval de frise and is in the next - * zone - * - * @author Meryem and Avi - * - */ -public class PassChevalDeFrise extends Command { - - public PassChevalDeFrise() { - } - - @Override - protected void end() { - } - - @Override - protected void execute() { - } - - @Override - protected void initialize() { - } - - @Override - protected void interrupted() { - } - - @Override - protected boolean isFinished() { - return false; - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassDrawBridge.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassDrawBridge.java deleted file mode 100755 index ca5c425d..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassDrawBridge.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will lower the draw bridge and go through it - * - * pre-condition: robot is flush against the ramp of the outerworks in front of - * the draw bridge - * - * post-condition: the robot has passed the draw bridge and is in the next zone - * - * @author Meryem and Avi - * - */ -public class PassDrawBridge extends Command { - - public PassDrawBridge() { - } - - @Override - protected void end() { - } - - @Override - protected void execute() { - } - - @Override - protected void initialize() { - } - - @Override - protected void interrupted() { - } - - @Override - protected boolean isFinished() { - return false; - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java deleted file mode 100644 index be002264..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; -import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/*** - * This command will drive the robot through the low bar. - * - * dependency on sensors: encoders - * dependency on subsystems: drivetrain - * dependency on other commands: DriveForDist - * - * pre-condition: robot is flush against the ramp of the outerworks in front of - * the low bar - * - * post-condition: the robot has passed the low bar and is in the next zone - * - * @author Meryem and Avi - * - */ - -public class PassLowBar extends CommandGroup { - - public PassLowBar() { - if (Constants.Auton.IS_USING_TIME) { - addSequential(new DriveForTime(Constants.Auton.PASS_LOW_BAR_TIME, - Constants.Auton.PASS_LOW_BAR_SPEED)); - } - else { - addSequential(new DriveDistance( - Constants.Auton.PASS_LOW_BAR_DIST, - Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); - } - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java deleted file mode 100755 index 7afc1782..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; -import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/*** - * This command will drive the robot through the moat. - * - * The code drives the robot for a specific time at a specific speed up the ramp - * to the defense then drive over the defense at a different speed and time. - * - * dependency on subsystem: drivetrain - * - * dependency on other commands: DriveForTime - * - * pre-condition: robot is flush against the ramp of the outerworks in front of - * the moat - * - * post-condition: the robot has passed the moat and is in the next zone - * - * @author Meryem and Avi - * - */ - -public class PassMoat extends CommandGroup { - - public PassMoat() { - if (Constants.Auton.IS_USING_TIME) { - addSequential(new DriveForTime(Constants.Auton.PASS_MOAT_TIME, - Constants.Auton.PASS_MOAT_SPEED)); - } - else { - addSequential(new DriveDistance(Constants.Auton.PASS_MOAT_DIST, - Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); - } - - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassPortcullis.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassPortcullis.java deleted file mode 100644 index 4371c61d..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassPortcullis.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** - * - */ -public class PassPortcullis extends CommandGroup { - - public PassPortcullis() { - // TODO: in theory, these two commands should not be running in parallel all - // the time, because of specifics of the series of actions needed to - // successfully pass the portcullis, so edit these to reflect that - addParallel(new LiftPortcullis()); - addParallel(new DriveDistance(0, 0)); // TODO: figure out distance to travel - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java deleted file mode 100755 index ec467836..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; -import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/*** - * This command will drive the robot through the rampart. - * - * dependency on subsystems: drivetrain - * - * dependency on other commands: DriveForTime - * - * pre-condition: robot is flush against the ramp of the outerworks in front of - * the rampart - * - * post-condition: the robot has passed the rampart and is in the next zone - * - * @author Meryem and Avi - * - */ - -public class PassRampart extends CommandGroup { - - public PassRampart() { - - if (Constants.Auton.IS_USING_TIME) { - addSequential(new DriveForTime(Constants.Auton.PASS_RAMPART_TIME, - Constants.Auton.PASS_RAMPART_SPEED)); - } - else { - addSequential(new DriveDistance( - Constants.Auton.PASS_RAMPART_DIST, - Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); - } - - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java deleted file mode 100755 index 1b74da6c..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; -import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/*** - * This command will drive the robot through the rock wall. - * - * dependency on subsystems: drivetrain - * - * dependency on other commands: DriveForTime - * - * pre-condition: robot is flush against the ramp of the outerworks in front of - * the rock wall - * - * post-condition: the robot has passed the rock wall and is in the next zone - * - * @author Meryem and Avi - * - */ - -public class PassRockWall extends CommandGroup { - - public PassRockWall() { - if (Constants.Auton.IS_USING_TIME) { - addSequential(new DriveForTime(Constants.Auton.PASS_ROCK_WALL_TIME, - Constants.Auton.PASS_ROCK_WALL_SPEED)); - } - else { - addSequential(new DriveDistance( - Constants.Auton.PASS_ROCK_WALL_DIST, - Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); - } - - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java deleted file mode 100644 index 632974a1..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; -import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/*** - * This command will drive the robot through the rough terrain. - * - * dependency on subsytems: drivetrain - * - * dependency on other commands: DriveForTime - * - * pre-condition: robot is flush against the ramp of the outerworks in front of - * the rough terrain - * - * post-condition: the robot has passed the rough terrain and is in the next - * zone - * - * @author Meryem and Avi - * - */ -public class PassRoughTerrain extends CommandGroup { - - public PassRoughTerrain() { - - if (Constants.Auton.IS_USING_TIME) { - addSequential(new DriveForTime(Constants.Auton.PASS_ROUGH_TERRAIN_TIME, - Constants.Auton.PASS_ROUGH_TERRAIN_SPEED)); - } - else { - addSequential(new DriveDistance( - Constants.Auton.PASS_ROUGH_TERRAIN_DIST, - Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); - } - - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassSallyPort.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassSallyPort.java deleted file mode 100755 index 8d5db25f..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassSallyPort.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.auton; - -import edu.wpi.first.wpilibj.command.Command; - -public class PassSallyPort extends Command { - /*** - * This command will only open the sally port pass through it. It will do this - * by hooking onto the port and driving backwards while rotating then driving - * backwards through the sally port. - * - * pre-condition: robot is in the neutral zone, flush against the ramp of the - * outerworks in front of the portcullis - * - * post-condition: the robot has passed the sally port and is in a courtyard - * - * note: to go from the courtyard to neutral zone, the driver just has to - * drive through the port - * - * @author Meryem and Avi - * - */ - - public PassSallyPort() { - } - - @Override - protected void initialize() { - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return false; - } - - @Override - protected void end() { - } - - @Override - protected void interrupted() { - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/defensearm/LowerDefenseArmContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/defensearm/LowerDefenseArmContinuous.java deleted file mode 100755 index 208accbd..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/defensearm/LowerDefenseArmContinuous.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.defensearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command is intended to be run from OI using button.whileHeld(...). - * It lowers the defenseArm continually while the button is being held down. - * - * @author shaina - * - */ -public class LowerDefenseArmContinuous extends Command { - - private double speed; - - public LowerDefenseArmContinuous(double speed) { - requires(Robot.defenseArm); - this.speed = -speed; - } - - @Override - protected void initialize() { - Robot.defenseArm.setArmSpeed(speed); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - Robot.defenseArm.setArmSpeed(0); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/defensearm/LowerDefenseWristContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/defensearm/LowerDefenseWristContinuous.java deleted file mode 100755 index 204bdf27..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/defensearm/LowerDefenseWristContinuous.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.defensearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command is intended to be run from OI using button.whileHeld(...). - * It lowers the defenseWrist continually while the button is being held down. - * - * @author shaina - * - */ -public class LowerDefenseWristContinuous extends Command { - - private double speed; - - public LowerDefenseWristContinuous(double speed) { - requires(Robot.defenseArm); - this.speed = -speed; - } - - @Override - protected void initialize() { - Robot.defenseArm.setHandSpeed(speed); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - Robot.defenseArm.setHandSpeed(0); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/defensearm/RaiseDefenseArmContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/defensearm/RaiseDefenseArmContinuous.java deleted file mode 100755 index 53839142..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/defensearm/RaiseDefenseArmContinuous.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.defensearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command is intended to be run from OI using button.whileHeld(...). - * It raises the defenseArm continually while the button is being held down. - * - * @author shaina - */ -public class RaiseDefenseArmContinuous extends Command { - - private double speed; - - public RaiseDefenseArmContinuous(double speed) { - requires(Robot.defenseArm); - this.speed = speed; - } - - @Override - protected void initialize() { - Robot.defenseArm.setArmSpeed(speed); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - Robot.defenseArm.setArmSpeed(0); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/defensearm/RaiseDefenseWristContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/defensearm/RaiseDefenseWristContinuous.java deleted file mode 100755 index a670fac1..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/defensearm/RaiseDefenseWristContinuous.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.defensearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command is intended to be run from OI using button.whileHeld(...). - * It raises the defenseWrist continually while the button is being held down. - * - * @author shaina - * - */ -public class RaiseDefenseWristContinuous extends Command { - - private double speed; - - public RaiseDefenseWristContinuous(double speed) { - requires(Robot.defenseArm); - this.speed = speed; - } - - @Override - protected void initialize() { - Robot.defenseArm.setHandSpeed(speed); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - Robot.defenseArm.setHandSpeed(0); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/defensearm/RetractDefenseArm.java b/src/org/usfirst/frc/team3501/robot/commands/defensearm/RetractDefenseArm.java deleted file mode 100755 index 9a4c7880..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/defensearm/RetractDefenseArm.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.defensearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/*** - * This command group simultaneously move arm and hand to retracted position. - * - * @author shaina - * - */ -public class RetractDefenseArm extends CommandGroup { - static final double ARM_FULLY_RETRACTED_ANGLE = 0; // change value once tested - static final double HAND_FULLY_RETRACTED_ANGLE = 0; // change value once - // tested - - public RetractDefenseArm(double speed) { - requires(Robot.defenseArm); - - addParallel(new SetArmToAngle(speed, ARM_FULLY_RETRACTED_ANGLE)); - addParallel(new SetHandToAngle(speed, HAND_FULLY_RETRACTED_ANGLE)); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/defensearm/SetArmToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/defensearm/SetArmToAngle.java deleted file mode 100755 index 709757de..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/defensearm/SetArmToAngle.java +++ /dev/null @@ -1,67 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.defensearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command moves the defense arm to a input angle position. - * Requires input of a targetPosition and a speed. - * - * @author shaina - * - */ -public class SetArmToAngle extends Command { - private static final double THRESHOLD = 0.1; - private double speed; - private double targetPosition; - private double currentPosition; - private boolean isDecreasing = false; - - public SetArmToAngle(double speed, double targetPosition) { - requires(Robot.defenseArm); - - this.speed = speed; - this.targetPosition = targetPosition; - } - - @Override - public void initialize() { - currentPosition = Robot.defenseArm.getArmPotAngle(); - - if (currentPosition > targetPosition) { - Robot.defenseArm.setArmSpeed(-speed); - isDecreasing = true; - } else { - Robot.defenseArm.setArmSpeed(speed); - isDecreasing = false; - } - } - - @Override - public void execute() { - - } - - @Override - public boolean isFinished() { - currentPosition = Robot.defenseArm.getArmPotAngle(); - - if (isDecreasing == true) { - return (currentPosition <= targetPosition + THRESHOLD); - } else { - return (currentPosition >= targetPosition - THRESHOLD); - } - } - - @Override - public void end() { - Robot.defenseArm.setArmSpeed(0); - } - - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/defensearm/SetHandToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/defensearm/SetHandToAngle.java deleted file mode 100755 index 60618d56..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/defensearm/SetHandToAngle.java +++ /dev/null @@ -1,67 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.defensearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command moves the defense arm to a input angle position. - * Requires input of a targetPosition and a speed. - * - * @author shaina - * - */ -public class SetHandToAngle extends Command { - private static final double THRESHOLD = 0.1; - private double speed; - private double targetPosition; - private double currentPosition; - private boolean isDecreasing = false; - - public SetHandToAngle(double speed, double targetPosition) { - requires(Robot.defenseArm); - - this.speed = speed; - this.targetPosition = targetPosition; - } - - @Override - public void initialize() { - currentPosition = Robot.defenseArm.getHandPotAngle(); - - if (currentPosition > targetPosition) { - Robot.defenseArm.setArmSpeed(-speed); - isDecreasing = true; - } else { - Robot.defenseArm.setArmSpeed(speed); - isDecreasing = false; - } - } - - @Override - public void execute() { - - } - - @Override - public boolean isFinished() { - currentPosition = Robot.defenseArm.getHandPotAngle(); - - if (isDecreasing == true) { - return (currentPosition <= targetPosition + THRESHOLD); - } else { - return (currentPosition >= targetPosition - THRESHOLD); - } - } - - @Override - public void end() { - Robot.defenseArm.setArmSpeed(0); - } - - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java deleted file mode 100644 index 81b6c5c1..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java +++ /dev/null @@ -1,64 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.driving; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will drive the drivetrain a certain distance in inches - * - * @param distance - * is the distance we want to drive - * maxTimeOut is a catch just in case the robot malfunctions and never - * gets to the setpoint - * - * @code - * Repeatedly updates the driveTrain setpoint - * Finishes when the time goes over maxTimeOut or the driveTrain hits the - * setpoint - * end() disables the PID driveTrain - */ -public class DriveDistance extends Command { - private double maxTimeOut; - double distance; - int count = 0; - - public DriveDistance(double distance, double maxTimeOut) { - requires(Robot.driveTrain); - this.maxTimeOut = maxTimeOut; - this.distance = distance; - } - - @Override - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - - @Override - protected void execute() { - Robot.driveTrain.driveDistance(distance, maxTimeOut); - - } - - @Override - protected boolean isFinished() { - if (timeSinceInitialized() >= maxTimeOut - || Robot.driveTrain - .reachedTarget() || Robot.driveTrain.getError() < 1) { - System.out.println("time: " + timeSinceInitialized()); - Constants.DriveTrain.time = timeSinceInitialized(); - return true; - } - return false; - } - - @Override - protected void end() { - Robot.driveTrain.disable(); - } - - @Override - protected void interrupted() { - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveForTime.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveForTime.java deleted file mode 100755 index eafc0408..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveForTime.java +++ /dev/null @@ -1,63 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.driving; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; - -/** - * This command drives the robot for the specified time and specified speed. (If - * a speed is not specified, a default speed is used - * - * - * dependency on subsystems: drivetrain - * - * pre-condition: robot is on - * - * post condition: robot has driven for the specified amount of time - */ -public class DriveForTime extends Command { - - private final static double DEFAULT_SPEED = 0.5; - private double speed; - private double seconds; - - private Timer timer; - - public DriveForTime(double seconds, double speed) { - this.seconds = seconds; - this.speed = speed; - } - - public DriveForTime(double seconds) { - this(seconds, DEFAULT_SPEED); - } - - @Override - protected void initialize() { - timer = new Timer(); - timer.start(); - - Robot.driveTrain.setMotorSpeeds(speed, speed); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - if (timer.get() >= seconds) - return true; - return false; - } - - @Override - protected void end() { - Robot.driveTrain.setMotorSpeeds(0, 0); - } - - @Override - protected void interrupted() { - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java index 861432f5..3db43880 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java @@ -1,5 +1,6 @@ package org.usfirst.frc.team3501.robot.commands.driving; +import org.usfirst.frc.team3501.robot.OI; import org.usfirst.frc.team3501.robot.Robot; import edu.wpi.first.wpilibj.command.Command; @@ -22,8 +23,8 @@ public class JoystickDrive extends Command { protected void execute() { double k = (Robot.driveTrain.isFlipped() ? -1 : 1); // IDK why but the joystick gives positive values for pulling backwards - double left = -Robot.oi.leftJoystick.getY(); - double right = -Robot.oi.rightJoystick.getY(); + double left = -OI.leftJoystick.getY(); + double right = -OI.rightJoystick.getY(); Robot.driveTrain.drive(left * k, right * k); } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java b/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java deleted file mode 100755 index 675f9a90..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.driving; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class Stop extends Command { - - public Stop() { - } - - @Override - protected void initialize() { - Robot.driveTrain.stop(); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java b/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java deleted file mode 100644 index 863d39bd..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java +++ /dev/null @@ -1,16 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.driving; - -import org.usfirst.frc.team3501.robot.Constants; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** - * - */ -public class Turn180 extends CommandGroup { - - public Turn180() { - addSequential(new TurnForAngle(180, 5)); - Constants.DriveTrain.inverted = !Constants.DriveTrain.inverted; - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java deleted file mode 100755 index 2362b89b..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.driving; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * @param angle - * is the setpoint we want to turn for - * maxTimeOut catch just in case robot malfunctions and never reaches - * setpoint - * @initialize resets the Gyro and prints the current mode - * @code repeatedly sets a new setpoint angle to the motor - * @end ends when the setpoint is reached. - */ -public class TurnForAngle extends Command { - private double maxTimeOut; - double angle; - - public TurnForAngle(double angle, double maxTimeOut) { - requires(Robot.driveTrain); - this.maxTimeOut = maxTimeOut; - this.angle = angle; - } - - @Override - protected void initialize() { - Robot.driveTrain.resetGyro(); - System.out.println(Robot.driveTrain.getMode()); - } - - @Override - protected void execute() { - Robot.driveTrain.turnAngle(angle); - Robot.driveTrain.printGyroOutput(); - Robot.driveTrain.printOutput(); - - } - - @Override - protected boolean isFinished() { - if (timeSinceInitialized() >= maxTimeOut - || Robot.driveTrain - .reachedTarget() || Robot.driveTrain.getError() < 8) { - System.out.println("time: " + timeSinceInitialized()); - Constants.DriveTrain.time = timeSinceInitialized(); - return true; - } - return false; - } - - @Override - protected void end() { - Robot.driveTrain.disable(); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java deleted file mode 100755 index 368d1045..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java +++ /dev/null @@ -1,73 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.driving; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Constants.Direction; -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command turns the robot in a specified direction for a specified - * duration in seconds. - * - * pre-condition: robot is on a flat surface - * - * post-condition: robot has turned in the specified direction for the specified - * time - * - * TODO: test for speed/ time constants for specific angles (ex. 30 degrees, 60 - * degrees, 90 degrees) - * - * @author Meryem, Avi, and Sarvesh - * - */ - -public class TurnForTime extends Command { - private Direction direction; - private double seconds; - private Timer timer; - private double speed; - - public TurnForTime(double seconds, Direction direction, double speed) { - this.seconds = seconds; - this.direction = direction; - this.speed = speed; - } - - public TurnForTime(double seconds, Direction direction) { - this(seconds, direction, Constants.Auton.DEFAULT_SPEED); - } - - @Override - protected void initialize() { - timer = new Timer(); - timer.start(); - - if (direction == Direction.RIGHT) { - Robot.driveTrain.drive(speed, -speed); - } else if (direction == Direction.LEFT) { - Robot.driveTrain.drive(-speed, speed); - } - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return (timer.get() >= seconds); - } - - @Override - protected void end() { - Robot.driveTrain.drive(0, 0); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/BallRollerExpelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/BallRollerExpelContinuous.java deleted file mode 100644 index 638716cf..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/BallRollerExpelContinuous.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.intakearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will continually roll the rollers of the intake arm in the - * outwards direction and stop the rollers once the command is canceled. - * - * pre-condition: This command must be called by a button with the method - * .whileHeld() in OI - * - * @author Lauren and Niyati - * - */ - -public class BallRollerExpelContinuous extends Command { - - public BallRollerExpelContinuous() { - requires(Robot.intakeArm); - } - - @Override - protected void initialize() { - Robot.intakeArm.outputBall(); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - Robot.intakeArm.stopRollers(); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/BallRollerIntakeContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/BallRollerIntakeContinuous.java deleted file mode 100644 index d3a01b4b..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/BallRollerIntakeContinuous.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.intakearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will continually roll the rollers of the intake arm in the - * inwards direction and stop the rollers once the command is canceled. - * - * pre-condition: This command must be called by a button with the method - * .whileHeld() in OI - * - * @author Lauren and Niyati - * - */ - -public class BallRollerIntakeContinuous extends Command { - - public BallRollerIntakeContinuous() { - requires(Robot.intakeArm); - } - - @Override - protected void initialize() { - Robot.intakeArm.intakeBall(); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - Robot.intakeArm.stopRollers(); - } - - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/ExpelBall.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/ExpelBall.java deleted file mode 100644 index 04fa1268..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/ExpelBall.java +++ /dev/null @@ -1,54 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.intakearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will expel a boulder from the robot, if it is even present to - * begin with. - * - * pre-condition: Intake arm is at correct height and a boulder is present - * inside the robot. - * - * post-condition: A boulder is expelled from inside the robot to the field - * outside of the robot. - * - * @author Lauren and Niyati - * - */ - -public class ExpelBall extends Command { - private final int TIMEOUT_AMOUNT = 5; - - public ExpelBall() { - requires(Robot.intakeArm); - } - - @Override - protected void initialize() { - this.setTimeout(TIMEOUT_AMOUNT); - if (Robot.shooter.isBallInside()) - Robot.intakeArm.outputBall(); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return (this.isTimedOut() || !Robot.shooter.isBallInside()); - } - - @Override - protected void end() { - Robot.intakeArm.stopRollers(); - } - - @Override - protected void interrupted() { - - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/IntakeBall.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/IntakeBall.java deleted file mode 100644 index 7966c614..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/IntakeBall.java +++ /dev/null @@ -1,57 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.intakearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will take a boulder into the robot if there is not a boulder - * present inside already. - * - * pre-condition: Intake arm must be at correct height and a boulder is not - * present inside the robot. - * - * post-condition: A boulder is taken in from the field outside of the robot - * into the robot. - * - * @author Lauren and Niyati - * - */ - -public class IntakeBall extends Command { - private final int TIMEOUT_AMOUNT = 5; - - public IntakeBall() { - requires(Robot.intakeArm); - } - - @Override - protected void initialize() { - this.setTimeout(TIMEOUT_AMOUNT); - if (!Robot.shooter.isBallInside()) - Robot.intakeArm.intakeBall(); - - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return (this.isTimedOut() || Robot.shooter.isBallInside()); - } - - @Override - protected void end() { - Robot.intakeArm.stopRollers(); - - } - - @Override - protected void interrupted() { - - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/scaler/ClampBar.java b/src/org/usfirst/frc/team3501/robot/commands/scaler/ClampBar.java deleted file mode 100755 index 23b010f1..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/scaler/ClampBar.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.scaler; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class ClampBar extends Command { - private double speed = 0; - - public ClampBar(double speed) { - this.speed = speed; - } - - @Override - protected void initialize() { - setTimeout(Constants.Scaler.SECONDS_TO_CLAMP); - Robot.scaler.runWinch(this.speed); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return isTimedOut(); - } - - @Override - protected void end() { - Robot.scaler.runWinch(0); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/scaler/ExtendLift.java b/src/org/usfirst/frc/team3501/robot/commands/scaler/ExtendLift.java deleted file mode 100755 index bead3866..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/scaler/ExtendLift.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.scaler; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class ExtendLift extends Command { - - public ExtendLift() { - } - - @Override - protected void initialize() { - Robot.scaler.liftScissorLift(); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/scaler/RetractLift.java b/src/org/usfirst/frc/team3501/robot/commands/scaler/RetractLift.java deleted file mode 100755 index 880ff503..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/scaler/RetractLift.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.scaler; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class RetractLift extends Command { - - public RetractLift() { - } - - @Override - protected void initialize() { - Robot.scaler.lowerScissorLift(); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/scaler/RunWinchContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/scaler/RunWinchContinuous.java deleted file mode 100644 index 5b45d0b4..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/scaler/RunWinchContinuous.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.scaler; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/*** - * This command will run the winch motor continuously until the button - * tirggering it is released. - * - * pre-condition: This command must be run by a button in OI with method - * whileHeld(). The robot must be attached to the tower rung. - * - * post-condition: winch motor set to a specified speed. - * - * @author Lauren - * - */ - -public class RunWinchContinuous extends Command { - private double winchUpSpeed; - private int timeoutAmount; - - public RunWinchContinuous(double speed, int timeout) { - requires(Robot.scaler); - winchUpSpeed = speed; - timeoutAmount = timeout; - } - - @Override - protected void initialize() { - this.setTimeout(timeoutAmount); - Robot.scaler.runWinch(winchUpSpeed); - } - - @Override - protected void execute() { - } - - @Override - protected boolean isFinished() { - return this.isTimedOut(); - } - - @Override - protected void end() { - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/scaler/StopWinch.java b/src/org/usfirst/frc/team3501/robot/commands/scaler/StopWinch.java deleted file mode 100644 index d6bfef09..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/scaler/StopWinch.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.scaler; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class StopWinch extends Command { - - @Override - protected void initialize() { - Robot.scaler.stopWinch(); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - - } - - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/scaler/ToggleScaling.java b/src/org/usfirst/frc/team3501/robot/commands/scaler/ToggleScaling.java deleted file mode 100644 index 5f5a64cc..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/scaler/ToggleScaling.java +++ /dev/null @@ -1,14 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.scaler; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.commands.auton.CompactRobot; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -public class ToggleScaling extends CommandGroup { - public ToggleScaling() { - Constants.Scaler.SCALING = !Constants.Scaler.SCALING; - if (Constants.Scaler.SCALING) - addSequential(new CompactRobot()); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/Shoot.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/Shoot.java index 0b80c1d9..f0416b4e 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/Shoot.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/Shoot.java @@ -1,7 +1,6 @@ package org.usfirst.frc.team3501.robot.commands.shooter; import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Robot; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.wpilibj.command.WaitCommand; @@ -18,16 +17,8 @@ public class Shoot extends CommandGroup { * catapult. */ public Shoot() { - if (Robot.shooter.usePhotogate()) { - if (Robot.shooter.isBallInside()) { - addSequential(new FireCatapult()); - addSequential(new WaitCommand(Constants.Shooter.WAIT_TIME)); - addSequential(new ResetCatapult()); - } - } else { - addSequential(new FireCatapult()); - addSequential(new WaitCommand(Constants.Shooter.WAIT_TIME)); - addSequential(new ResetCatapult()); - } + addSequential(new FireCatapult()); + addSequential(new WaitCommand(Constants.Shooter.WAIT_TIME)); + addSequential(new ResetCatapult()); } } diff --git a/src/org/usfirst/frc/team3501/robot/sensors/GyroLib.java b/src/org/usfirst/frc/team3501/robot/sensors/GyroLib.java deleted file mode 100644 index c683b516..00000000 --- a/src/org/usfirst/frc/team3501/robot/sensors/GyroLib.java +++ /dev/null @@ -1,799 +0,0 @@ -package org.usfirst.frc.team3501.robot.sensors; -/** - * Copyright (c) 2015, www.techhounds.com - * All rights reserved. - * - *

- * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - *

- * - * - *

- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *

- */ - -import java.io.File; -import java.io.IOException; -import java.io.PrintStream; - -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * A Java wrapper around the ITC based ITG-3200 triple axis gryo. - * - *

- * Typical usage: - *

- * - *

- * Be aware of the following: - *

- * - * - *

Suggested Usage

- *

- * You should be able to use this class to aid your robot in making relative - * turns. For example, if you want to create a command which rotates your robot - * 90 degrees. - *

- * - */ -public final class GyroLib { - - /** - * Object used to monitor robot rotation. - * - * - */ - public final class Rotation implements RotationTracker { - /** Raw axis accumulator on gyro associated with this rotation tracker. */ - private Accumulator m_axis; - /** - * The degrees reported by the accumulator the last time this tracker was - * zeroed. - */ - private double m_zeroDeg; - /** - * The number of readings reported by the accumulator the last time this - * tracker was zeroed. - */ - private int m_zeroCnt; - - /** - * Constructor is protected, instances are created through the - * {@link GyroLib} methods. - * - * @param axis - * An accumulator from the gyro for the axis to be tracked. - */ - private Rotation(Accumulator axis) { - m_axis = axis; - m_zeroDeg = 0; - m_zeroCnt = 0; - } - - /** - * Zero the tracker (sets the current heading/direction as the zero point). - */ - public void zero() { - m_zeroDeg = m_axis.getDegrees(); - m_zeroCnt = m_axis.getReadings(); - } - - /** - * Get the number of degrees rotated since last zeroed. - * - * @return A signed number of degrees [-INF, +INF]. - */ - public double getAngle() { - double angle = m_axis.getDegrees() - m_zeroDeg; - return angle; - } - - /** - * Get the total number of times the raw values from the gyro have been read - * since zeroed. - * - * @return A diagnostic count that can be used to make sure the angle is - * still being updated. - */ - public int getReadings() { - return m_axis.getReadings() - m_zeroCnt; - } - - /** - * Returns the last raw (integer) value read from the gyro for the axis. - * - * @return An integer value from the ITG-3200 for the associated axis. - */ - public int getAngleRateRaw() { - return m_axis.getRaw(); - } - - /** - * Returns the current rotation rate in degrees/second from the last - * reading. - * - * @return How quickly the system is rotating about the axis in - * degrees/second. - */ - public double getAngleRate() { - return getAngleRateRaw() * COUNT_TO_DEGSEC; - } - - /** - * Returns the angle value from {@link #getAngle()} so object can be used as - * a source to a PID controller. - * - * @return See {@link #getAngle()}. - * - * @see edu.wpi.first.wpilibj.PIDSource#pidGet() - */ - public double pidGet() { - return getAngle(); - } - - public void setPIDSourceType(PIDSourceType pidSource) { - // TODO Auto-generated method stub - - } - - public PIDSourceType getPIDSourceType() { - // TODO Auto-generated method stub - return null; - } - } - - // - // List of I2C registers which the ITG-3200 uses from the datasheet - // - private static final byte WHO_AM_I = 0x00; - private static final byte SMPLRT_DIV = 0x15; - private static final byte DLPF_FS = 0x16; - // private static final byte INT_CFG = 0x17; - // private static final byte INT_STATUS = 0x1A; - // private static final byte TEMP_OUT_H = 0x1B; - // private static final byte TEMP_OUT_L = 0x1C; - private static final byte GYRO_XOUT_H = 0x1D; - // private static final byte GYRO_XOUT_L = 0x1E; - // private static final byte GYRO_YOUT_H = 0x1F; - // private static final byte GYRO_YOUT_L = 0x20; - // private static final byte GYRO_ZOUT_H = 0x21; - // private static final byte GYRO_ZOUT_L = 0x22; - - // - // Bit flags used for interrupt operation - // - - /* - * Set this bit in INT_CFG register for logic level of INT output pin to be - * low when interrupt is active (leave 0 if you want it high). - */ - // private static final byte INT_CFG_ACTL_LOW = (byte) (1 << 7); - - /* - * Set drive type for interrupt to open drain mode, omit if you want push-pull - * mode (what does this mean?). - */ - // private static final byte INT_CFG_OPEN_DRAIN = 1 << 6; - - /* - * Interrupt latch mode (remains set until you clear it), omit this flag to - * get a 0-50us interrupt pulse. - */ - // private static final byte INT_CFG_LATCH_INT_EN = 1 << 5; - - /* - * Allow any read operation of data to clear the interrupt flag (otherwise it - * is only cleared after reading status register). - */ - // private static final byte INT_CFG_ANYRD_2CLEAR = 1 << 4; - - /* - * Enable interrup when device is ready (PLL ready after changing clock - * source). Hmmm? - */ - // private static final byte INT_CFG_RDY_EN = 1 << 3; - - /* Enable interrupt when new data is available. */ - // private static final byte INT_CFG_RAW_RDY_EN = 1 << 1; - - /* Guess at mode to use if we want to try enabling interrupts. */ - // private static final byte INT_CFG_SETTING = (INT_CFG_LATCH_INT_EN | - // INT_CFG_ANYRD_2CLEAR | INT_CFG_RAW_RDY_EN); - - // - // The bit flags that can be set in the DLPF register on the ITG-3200 - // as specified in the ITG-3200 data sheet. - // - - // - // The low pass filter bandwidth settings - // - - // private static final byte DLPF_LOWPASS_256HZ = 0 << 0; - private static final byte DLPF_LOWPASS_188HZ = 1 << 0; - // private static final byte DLPF_LOWPASS_98HZ = 2 << 0; - // private static final byte DLPF_LOWPASS_42HZ = 3 << 0; - // private static final byte DLPF_LOWPASS_20HZ = 4 << 0; - // private static final byte DLPF_LOWPASS_10HZ = 5 << 0; - // private static final byte DLPF_LOWPASS_5HZ = 6 << 0; - - /** Select range of +/-2000 deg/sec. (only range supported). */ - private static final byte DLPF_FS_SEL_2000 = 3 << 3; - - /** - * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic high. - */ - private static final byte itgAddressJumper = 0x69; - - /** - * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic low. - */ - private static final byte itgAddressNoJumper = 0x68; - - /** Multiplier to convert raw integer values returned to degrees/sec. */ - private static final float COUNT_TO_DEGSEC = (float) (1.0 / 14.375); - - /** Set this to true for lots of diagnostic output. */ - private static final boolean DEBUG = false; - - /** - * How many sample readings to make to determine the bias value for each axis. - */ - private static final int MIN_READINGS_TO_SET_BIAS = 50; - - /** I2C Address to use to communicate with the ITG-3200. */ - private byte m_addr; - - /** I2C object used to communicate with Gyro. */ - private I2C m_i2c; - - /** - * Background thread responsible for accumulating angle data from the sensor. - */ - private Thread m_thread; - - /** Flag used to signal background thread that the gyro should be reset. */ - private boolean m_need_reset; - - /** Flag used to signal background thread that it's time to stop. */ - private boolean m_run_thread; - - /** Accumulator for rotation around the x-axis. */ - private Accumulator m_x; - - /** Accumulator for rotation around the y-axis. */ - private Accumulator m_y; - - /** Accumulator for rotation around the z-axis. */ - private Accumulator m_z; - private int[] m_xBuffer; - private int[] m_yBuffer; - private int[] m_zBuffer; - private int m_cntBuffer; - private int m_sizeBuffer; - private double[] m_timeBuffer; - - /** - * Construct a new instance of the ITG-3200 gryo class. - * - *

- * IMPORTANT - * - * @param port - * This should be {@link I2C.Port#kOnboard} if the ITG-3200 is - * connected to the main I2C bus on the roboRIO. This should be - * {@link I2C.Port#kMXP} if it is connected to the I2C bus on the MXP - * port on the roboRIO. - * @param jumper - * This should be true if the ITG-3200 has the AD0 jumpered to logic - * level high and false if not. - */ - public GyroLib(I2C.Port port, boolean jumper) { - m_addr = (jumper ? itgAddressJumper : itgAddressNoJumper); - m_i2c = new I2C(port, m_addr); - m_thread = new Thread(new Runnable() { - @Override - public void run() { - accumulateData(); - } - }); - if (DEBUG) { - check(); - } - m_x = new Accumulator(); - m_y = new Accumulator(); - m_z = new Accumulator(); - m_need_reset = true; - - start(); - } - - /** - * Construct a {@link Rotation} object used to monitor rotation about the - * Z-axis. - * - * @return A rotation object that very useful for checking the direction your - * robot is facing. - */ - public Rotation getRotationZ() { - return new Rotation(m_z); - } - - /** - * Construct a {@link Rotation} object used to monitor rotation about the - * X-axis. - * - * @return A rotation object that is probably only useful for checking if your - * robot is starting to tip over. - */ - public Rotation getRotationX() { - return new Rotation(m_x); - } - - /** - * Construct a {@link Rotation} object used to monitor rotation about the - * Y-axis. - * - * @return A rotation object that is probably only useful for checking if your - * robot is starting to tip over. - */ - public Rotation getRotationY() { - return new Rotation(m_y); - } - - /** - * Returns string representation of the object for debug purposes. - */ - public String toString() { - return "Gyro[0x" + Integer.toHexString(m_addr & 0xff) + "]"; - } - - /** - * Dumps information about the state of the Gyro to the smart dashboard. - * - * @param tag - * Short name like "Gyro" to prefix each label with on the dashboard. - * @param debug - * Pass true if you want a whole lot of details dumped onto the - * dashboard, pass false if you just want the direction of each axis - * and the temperature. - */ - public void updateDashboard(String tag, boolean debug) { - SmartDashboard.putNumber(tag + " x-axis degrees", m_x.getDegrees()); - SmartDashboard.putNumber(tag + " y-axis degrees", m_y.getDegrees()); - SmartDashboard.putNumber(tag + " z-axis degrees", m_z.getDegrees()); - - if (debug) { - SmartDashboard.putNumber(tag + " x-axis raw", m_x.getRaw()); - SmartDashboard.putNumber(tag + " y-axis raw", m_y.getRaw()); - SmartDashboard.putNumber(tag + " z-axis raw", m_z.getRaw()); - - SmartDashboard.putNumber(tag + " x-axis count", m_x.getReadings()); - SmartDashboard.putNumber(tag + " y-axis count", m_y.getReadings()); - SmartDashboard.putNumber(tag + " z-axis count", m_z.getReadings()); - - SmartDashboard.putString(tag + " I2C Address", - "0x" + Integer.toHexString(m_addr)); - } - } - - /** - * Internal method that runs in the background thread to accumulate data from - * the Gyro. - */ - private void accumulateData() { - m_run_thread = true; - int resetCnt = 0; - - while (m_run_thread) { - if (m_need_reset) { - // Set gyro to the proper mode - performResetSequence(); - - // Reset accumulators and set the number of readings to take to - // compute bias values - resetCnt = MIN_READINGS_TO_SET_BIAS; - m_x.reset(); - m_y.reset(); - m_z.reset(); - m_need_reset = false; - } else { - // Go read raw values from ITG-3200 and update our accumulators - readRawAngleBytes(); - - if (resetCnt > 0) { - // If we were recently reset, and have made enough initial - // readings, - // then go compute and set our new bias (correction) values - // for each accumulator - resetCnt--; - if (resetCnt == 0) { - m_x.setBiasByAccumulatedValues(); - m_y.setBiasByAccumulatedValues(); - m_z.setBiasByAccumulatedValues(); - } - } - } - - // Short delay between each reading - if (m_run_thread) { - Timer.delay(.01); - } - } - } - - /** - * Singles the gyro's background thread that we want to reset the gyro. - * - *

- * You don't typically need to call this during a match. If you do call it, - * you should only do so when the robot is stationary and will remain - * stationary for a short time. - *

- */ - public void reset() { - m_need_reset = true; - } - - /** - * Starts up the background thread that accumulates gyro statistics. - * - *

- * You never need to call this method unless you have stopped the gyro and now - * want to start it up again. If you do call this method, you should probably - * also call the {@link #reset} method. - *

- */ - public void start() { - if (!m_thread.isAlive()) { - m_thread.start(); - } - } - - /** - * Stops the background thread from accumulating angle information (turns OFF - * gyro!). - * - *

- * This method is not typically called as it stops the gyro from accumulating - * statistics essentially turning it off. The only time you might want to do - * this is if you are done using the gyro for the rest of the match and want - * to save some CPU cyles (for example, if you only needed the gyro during the - * autonomous period). - *

- */ - public void stop() { - m_run_thread = false; - } - - /** - * Sends commands to configure the ITG-3200 the way we need it to run. - */ - private void performResetSequence() { - // Configure the gyroscope - // Set the gyroscope scale for the outputs to +/-2000 degrees per second - m_i2c.write(DLPF_FS, (DLPF_FS_SEL_2000 | DLPF_LOWPASS_188HZ)); - // Set the sample rate to 100 hz - m_i2c.write(SMPLRT_DIV, 9); - } - - /** - * Enables the buffering of the next "n" data samples (which can then be saved - * for analysis). - * - * @param samples - * Maximum number of samples to read. - */ - public void enableBuffer(int samples) { - double[] timeBuffer = new double[samples]; - int[] xBuffer = new int[samples]; - int[] yBuffer = new int[samples]; - int[] zBuffer = new int[samples]; - synchronized (this) { - m_timeBuffer = timeBuffer; - m_xBuffer = xBuffer; - m_yBuffer = yBuffer; - m_zBuffer = zBuffer; - m_cntBuffer = 0; - m_sizeBuffer = samples; - } - } - - /** - * Check to see if the buffer is full. - * - * @return true if buffer is at capacity. - */ - public boolean isBufferFull() { - boolean isFull; - synchronized (this) { - isFull = (m_cntBuffer == m_sizeBuffer); - } - return isFull; - } - - /** - * Writes any raw buffered data to the file "/tmp/gyro-data.csv" for - * inspection via Excel. - */ - public void saveBuffer() { - double[] timeBuffer; - int[] xBuffer; - int[] yBuffer; - int[] zBuffer; - int size; - - // Transfer buffer info to local variables and turn off buffering in a - // thread safe way. - synchronized (this) { - timeBuffer = m_timeBuffer; - xBuffer = m_xBuffer; - yBuffer = m_yBuffer; - zBuffer = m_zBuffer; - size = m_cntBuffer; - m_sizeBuffer = 0; - m_cntBuffer = 0; - } - - if (size > 0) { - try { - PrintStream out = new PrintStream(new File("/tmp/gryo-data.csv")); - out.println("\"FPGA Time\",\"x-axis\",\"y-axis\",\"z-axis\""); - for (int i = 0; i < size; i++) { - out.println(timeBuffer[i] + "," + xBuffer[i] + "," + yBuffer[i] + "," - + zBuffer[i]); - } - out.close(); - SmartDashboard.putBoolean("Gyro Save OK", true); - } catch (IOException ignore) { - SmartDashboard.putBoolean("Gyro Save OK", false); - } - } - } - - /** - * Internal method run in the background thread that reads values from the - * ITG-3200 and updates the accumulators. - */ - private void readRawAngleBytes() { - double now = Timer.getFPGATimestamp(); - - byte[] buffer = new byte[6]; - boolean rc = m_i2c.read(GYRO_XOUT_H, buffer.length, buffer); - - if (rc) { - // Got a good read, get 16 bit integer values for each axis and - // update accumulated values - int x = (buffer[0] << 8) | (buffer[1] & 0xff); - int y = (buffer[2] << 8) | (buffer[3] & 0xff); - int z = (buffer[4] << 8) | (buffer[5] & 0xff); - - m_x.update(x, now); - m_y.update(y, now); - m_z.update(z, now); - - // If buffered enabled, then save values in a thread safe way - if (m_sizeBuffer > 0) { - synchronized (this) { - int i = m_cntBuffer; - if (i < m_sizeBuffer) { - m_timeBuffer[i] = now; - m_xBuffer[i] = x; - m_yBuffer[i] = y; - m_zBuffer[i] = z; - m_cntBuffer++; - } - } - } - } - - if (DEBUG) { - String name = toString(); - String[] labels = { "XOUT_H", "XOUT_L", "YOUT_H", "YOUT_L", "ZOUT_H", - "ZOUT_L" }; - for (int i = 0; i < labels.length; i++) { - SmartDashboard.putString(name + " " + labels[i], - "0x" + Integer.toHexString(0xff & buffer[i])); - } - } - } - - /** - * Helper method to check that we can communicate with the gyro. - */ - private void check() { - byte[] buffer = new byte[1]; - boolean rc = m_i2c.read(WHO_AM_I, buffer.length, buffer); - if (DEBUG) { - String name = toString(); - SmartDashboard.putBoolean(name + " Check OK?", rc); - SmartDashboard.putNumber(name + " WHO_AM_I", buffer[0]); - } - } - - /** - * Private helper class to accumulate values read from the gryo and convert - * degs/sec into degrees. - */ - private class Accumulator { - /** Accumulated degrees since zero. */ - private double m_accumulatedDegs; - /** - * 2 times the computed bias value that is used when getting average of - * readings. - */ - private double m_bias2; - /** The prior raw value read from the gyro. */ - private int m_lastRaw; - /** The prior time stamp the last raw value was read. */ - private double m_lastTime; - /** The total count of time the gyro value has been read. */ - private int m_cnt; - /** The sum of all of the raw values read. */ - private long m_sum; - - /** Multipler to covert 2*Count to degrees/sec (optimization). */ - private static final double COUNT2_TO_DEGSEC = (COUNT_TO_DEGSEC / 2.0); - - /** - * Returns the accumulated degrees. - * - * @return Accumulated signed degrees since last zeroed. - */ - public synchronized double getDegrees() { - return m_accumulatedDegs; - } - - /** - * @return The raw integer reading from the ITG-3200 associated with the - * axis. - */ - public int getRaw() { - return m_lastRaw; - } - - /** - * Returns the number or readings that went into the accumulated degrees. - * - * @return Count of readings since last zeroed. - */ - public synchronized int getReadings() { - return m_cnt; - } - - /** - * Constructs a new instance. - */ - private Accumulator() { - m_bias2 = 0; - zero(); - } - - /** - * Zeros out accumulated information. - */ - private void zero() { - m_lastRaw = 0; - m_lastTime = 0; - m_sum = 0; - synchronized (this) { - m_cnt = 0; - m_accumulatedDegs = 0; - } - } - - /** - * Zeros out accumulated information and clears (zeros) the internal bias - * value. - */ - private void reset() { - zero(); - m_bias2 = 0; - } - - /** - * Computes new bias value from accumulated values and then zeros. - */ - private void setBiasByAccumulatedValues() { - m_bias2 = 2.0 * ((double) m_sum) / ((double) m_cnt); - zero(); - } - - /** - * Updates (accumulates) new value read from axis. - * - * @param raw - * Raw signed 16 bit value read from gyro for axis. - * @param time - * The time stamp when the value was read. - */ - private void update(int raw, double time) { - double degs = 0; - - if (m_cnt != 0) { - // Get average of degrees per second over the time span - double degPerSec = (m_lastRaw + raw - m_bias2) * COUNT2_TO_DEGSEC; - // Get time span this rate occurred for - double secs = (m_lastTime - time); - // Get number of degrees rotated for time period - degs = degPerSec * secs; - } - - // Update our thread shared values - synchronized (this) { - m_accumulatedDegs += degs; - m_sum += raw; - m_cnt++; - m_lastRaw = raw; - m_lastTime = time; - } - } - } -} diff --git a/src/org/usfirst/frc/team3501/robot/sensors/Lidar.java b/src/org/usfirst/frc/team3501/robot/sensors/Lidar.java deleted file mode 100644 index 8ba641e3..00000000 --- a/src/org/usfirst/frc/team3501/robot/sensors/Lidar.java +++ /dev/null @@ -1,85 +0,0 @@ -package org.usfirst.frc.team3501.robot.sensors; - -import java.util.TimerTask; - -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.I2C.Port; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.Timer; - -public class Lidar implements PIDSource { - private I2C i2c; - private byte[] distance; - private java.util.Timer updater; - - private final int LIDAR_ADDR = 0x62; - private final int LIDAR_CONFIG_REGISTER = 0x00; - private final int LIDAR_DISTANCE_REGISTER = 0x8f; - - public Lidar(Port port) { - i2c = new I2C(port, LIDAR_ADDR); - - distance = new byte[2]; - - updater = new java.util.Timer(); - } - - // Distance in cm - public int getDistance() { - return (int) Integer.toUnsignedLong(distance[0] << 8) - + Byte.toUnsignedInt(distance[1]); - } - - @Override - public double pidGet() { - return getDistance(); - } - - // Start 10Hz polling - public void start() { - updater.scheduleAtFixedRate(new LIDARUpdater(), 0, 100); - } - - // Start polling for period in milliseconds - public void start(int period) { - updater.scheduleAtFixedRate(new LIDARUpdater(), 0, period); - } - - public void stop() { - updater.cancel(); - updater = new java.util.Timer(); - } - - // Update distance variable - public void update() { - i2c.write(LIDAR_CONFIG_REGISTER, 0x04); // Initiate measurement - Timer.delay(0.04); // Delay for measurement to be taken - i2c.read(LIDAR_DISTANCE_REGISTER, 2, distance); // Read in measurement - Timer.delay(0.005); // Delay to prevent over polling - } - - // Timer task to keep distance updated - private class LIDARUpdater extends TimerTask { - @Override - public void run() { - while (true) { - update(); - try { - Thread.sleep(10); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - } - } - - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - } - - @Override - public PIDSourceType getPIDSourceType() { - return null; - } -} diff --git a/src/org/usfirst/frc/team3501/robot/sensors/Photogate.java b/src/org/usfirst/frc/team3501/robot/sensors/Photogate.java deleted file mode 100644 index c9f3d642..00000000 --- a/src/org/usfirst/frc/team3501/robot/sensors/Photogate.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.usfirst.frc.team3501.robot.sensors; - -import edu.wpi.first.wpilibj.AnalogInput; - -/*** - * The photogate is a pair of IR LED and phototransistor sensor that uses a - * reflective method to sense the presence of the boulder within the robot's - * shooting chamber. This class specifically checks for the ball's presence - * using a threshold of voltages outputted from the phototransistor. - * - * @author niyatisriram - */ -public class Photogate extends AnalogInput { - - private double threshold = 1.8; - - /*** - * The constructor inputs the channel of the transistor and the threshold - * value. - * The threshold is a specific value, representing the outputted voltage of - * the phototransistor. This value will be somewhere within the range [0, - * 4095] Find the value by testing and finding an average value for which the - * ball is present when the output is greater, and absent when the output is - * less. - */ - public Photogate(int channel, int threshold) { - super(channel); - this.threshold = threshold; - } - - /*** - * @return whether the ball is present or not - */ - public boolean isBallPresent() { - if (this.getVoltage() > threshold) - return true; - else - return false; - - } - - /*** - * @param threshold - * (range [0, 4095]) - */ - public void setThreshold(int threshold) { - this.threshold = threshold; - } -} diff --git a/src/org/usfirst/frc/team3501/robot/sensors/RotationTracker.java b/src/org/usfirst/frc/team3501/robot/sensors/RotationTracker.java deleted file mode 100644 index 4c0d6df8..00000000 --- a/src/org/usfirst/frc/team3501/robot/sensors/RotationTracker.java +++ /dev/null @@ -1,74 +0,0 @@ -/** - * Copyright (c) 2015, www.techhounds.com - * All rights reserved. - * - *

- * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - *

- * - * - *

- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *

- */ - -package org.usfirst.frc.team3501.robot.sensors; - -import edu.wpi.first.wpilibj.PIDSource; - -/** - * A rotation tracker allows you to keep track of how far the robot has rotated - * since the object was constructed or zeroed. - * - *

- * This object is most useful when you want to turn your robot a particular - * number of degrees, or when you want to detect whether your robot is tipping. - *

- * - *

- * You will typically use the {@link #getRotationTracker} method associated with - * the gyro class associated with your hardware. For example - * {@link GyroItg3200.getRotationTrackerZ()}. - *

- */ -public interface RotationTracker extends PIDSource { - - /** - * Returns the angle in signed decimal degrees since construction or zeroing. - * - *

- * This value is used as the PID sensor value. - *

- * - * @return Number of degrees the robot has rotated in the range of [-INF, - * +INF]. Positive values indicate clockwise rotation (720 means it - * has spun around twice and is facing the same direction). - */ - public double getAngle(); - - /** - * Zeros the rotation tracker so the current direction we are pointing now - * becomes the zero point. - */ - public void zero(); - -} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java deleted file mode 100755 index 24037413..00000000 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java +++ /dev/null @@ -1,160 +0,0 @@ -package org.usfirst.frc.team3501.robot.subsystems; - -import org.usfirst.frc.team3501.robot.Constants; - -import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.CANTalon; -import edu.wpi.first.wpilibj.command.Subsystem; - -public class DefenseArm extends Subsystem { - private AnalogPotentiometer defenseArmPotentiometer; - private AnalogPotentiometer defenseHandPotentiometer; - private CANTalon defenseArm; - private CANTalon defenseHand; - private double hookHeight; - private double footHeight; - - private double[] potHandAngles; - private double[] potArmAngles; - private double[] potAngles; - - // angles corresponding to pre-determined heights we will need - - public DefenseArm() { - defenseArmPotentiometer = new AnalogPotentiometer( - Constants.DefenseArm.ARM_CHANNEL, Constants.DefenseArm.FULL_RANGE, - Constants.DefenseArm.OFFSET); - defenseHandPotentiometer = new AnalogPotentiometer( - Constants.DefenseArm.HAND_CHANNEL, Constants.DefenseArm.FULL_RANGE, - Constants.DefenseArm.OFFSET); - - defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT); - defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT); - potHandAngles = createHandPotArray(); - potArmAngles = createArmPotArray(); - } - - public double getArmPotAngle() { - return defenseArmPotentiometer.get(); - } - - public double getHandPotAngle() { - return defenseHandPotentiometer.get(); - } - - /*** - * This method takes an arm location as input (range of [0,2]) Returns the - * angle of the arm corresponding to that arm location - * - * @param desiredArmLocation - * takes an arm location ranging from [0,2] 0 is the lowest position - * of arm 2 is the highest position of arm - * @return the angle of the arm corresponding to that arm location - */ - public double getAngleForHandLocation(int desiredArmLocation) { - return potHandAngles[desiredArmLocation]; - } - - public double getAngleForArmLocation(int desiredArmLocation) { - return potArmAngles[desiredArmLocation]; - } - - public double[] createHandPotArray() { - double[] arr = new double[3]; - - for (int i = 0; i < 3; i++) { - arr[i] = 45 * i; - } - return arr; - } - - public double[] createArmPotArray() { - double[] arr = new double[3]; - - for (int i = 0; i < 3; i++) { - arr[i] = 45 * i; - } - return arr; - } - - /*** - * This method sets the voltage of the arm motor. The range is from [-1,1]. A - * negative voltage makes the direction of the motor go backwards. - * - * @param speed - * The voltage that you set the motor at. The range of the voltage of - * the arm motor is from [-1,1]. A negative voltage makes the - * direction of the motor go backwards. - */ - - public void setArmSpeed(double speed) { - if (speed > 1) - speed = 1; - else if (speed < -1) - speed = -1; - - defenseArm.set(speed); - } - - /*** - * This method sets the voltage of the hand motor. The range is from [-1,1]. A - * negative voltage makes the direction of the motor go backwards. - * - * @param speed - * The voltage that you set the motor at. The range of the voltage of - * the hand motor is from [-1,1]. A negative voltage makes the - * direction of the motor go backwards. - */ - - public void setHandSpeed(double speed) { - if (speed > 1) - speed = 1; - else if (speed < -1) - speed = -1; - - defenseHand.set(speed); - } - - // TODO: figure out if measurements are all in inches - public double getArmHorizontalDisplacement() { - double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH - * Math.cos(getArmPotAngle()); - double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH - * Math.cos(getHandPotAngle()); - return (armHorizontalDisplacement + handHorizontalDisplacement); - } - - public double getArmVerticalDisplacement() { - double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT; - double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH - * Math.sin(getArmPotAngle()); - double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH - * Math.sin(getHandPotAngle()); - return (armMounted + armVerticalDisplacement + handVerticalDisplacement); - } - - public double getArmHorizontalDist() { - double arm = Constants.DefenseArm.ARM_LENGTH * Math.cos(getArmPotAngle()); - double hand = Constants.DefenseArm.HAND_LENGTH - * Math.cos(getHandPotAngle()); - return (arm + hand); - } - - public double getArmHeight() { - double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT; - double arm = Constants.DefenseArm.ARM_LENGTH * Math.sin(getArmPotAngle()); - double hand = Constants.DefenseArm.HAND_LENGTH - * Math.sin(getHandPotAngle()); - return (armMounted + arm + hand); - } - - public boolean isOutsideRange() { - if (getArmHorizontalDist() < 15) - return false; - return true; - } - - @Override - protected void initDefaultCommand() { - } -} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index dec42e9d..5f370e98 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -1,34 +1,25 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.MathLib; import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive; -import org.usfirst.frc.team3501.robot.sensors.GyroLib; -import org.usfirst.frc.team3501.robot.sensors.Lidar; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CounterBase.EncodingType; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.PIDSubsystem; public class DriveTrain extends PIDSubsystem { - // Current Drive Mode Default Drive Mode is Manual - private int DRIVE_MODE = 1; private boolean outputFlipped = false; private static double pidOutput = 0; private Encoder leftEncoder, rightEncoder; - public static Lidar lidar; - private CANTalon frontLeft, frontRight, rearLeft, rearRight; private RobotDrive robotDrive; - private GyroLib gyro; private DoubleSolenoid leftGearPiston, rightGearPiston; // Drivetrain specific constants that relate to the inches per pulse value for @@ -45,7 +36,6 @@ public class DriveTrain extends PIDSubsystem { robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight); - lidar = new Lidar(I2C.Port.kMXP); leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A, Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X); rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A, @@ -56,18 +46,12 @@ public class DriveTrain extends PIDSubsystem { leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); - gyro = new GyroLib(I2C.Port.kOnboard, false); - - DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE; - setEncoderPID(); this.disable(); - gyro.start(); leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_MODULE, Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE); rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_MODULE, Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE); - Constants.DriveTrain.inverted = false; } @Override @@ -104,10 +88,6 @@ public class DriveTrain extends PIDSubsystem { rightEncoder.reset(); } - public double getLidarDistance() { - return lidar.pidGet(); - } - public double getRightSpeed() { return rightEncoder.getRate(); // in inches per second } @@ -131,18 +111,7 @@ public class DriveTrain extends PIDSubsystem { // Get error between the setpoint of PID Controller and the current state of // the robot public double getError() { - if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) - return Math.abs(this.getSetpoint() - getAvgEncoderDistance()); - else - return Math.abs(this.getSetpoint() + getGyroAngle()); - } - - public double getGyroAngle() { - return gyro.getRotationZ().getAngle(); - } - - public void resetGyro() { - gyro.reset(); + return Math.abs(this.getSetpoint() - getAvgEncoderDistance()); } public void printEncoder(int i, int n) { @@ -153,10 +122,6 @@ public class DriveTrain extends PIDSubsystem { } } - public void printGyroOutput() { - System.out.println("Gyro Angle" + -this.getGyroAngle()); - } - /* * returns the PID output that is returned by the PID Controller */ @@ -166,12 +131,8 @@ public class DriveTrain extends PIDSubsystem { // Updates the PID constants based on which control mode is being used public void updatePID() { - if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) - this.getPIDController().setPID(Constants.DriveTrain.kp, - Constants.DriveTrain.ki, Constants.DriveTrain.kd); - else - this.getPIDController().setPID(Constants.DriveTrain.gp, - Constants.DriveTrain.gd, Constants.DriveTrain.gi); + this.getPIDController().setPID(Constants.DriveTrain.kp, + Constants.DriveTrain.ki, Constants.DriveTrain.kd); } public CANTalon getFrontLeft() { @@ -190,10 +151,6 @@ public class DriveTrain extends PIDSubsystem { return rearRight; } - public int getMode() { - return DRIVE_MODE; - } - /* * Method is a required method that the PID Subsystem uses to return the * calculated PID value to the driver @@ -207,16 +164,11 @@ public class DriveTrain extends PIDSubsystem { protected void usePIDOutput(double output) { double left = 0; double right = 0; - if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) { - double drift = this.getLeftDistance() - this.getRightDistance(); - if (Math.abs(output) > 0 && Math.abs(output) < 0.3) - output = Math.signum(output) * 0.3; - left = output; - right = output + drift * Constants.DriveTrain.kp / 10; - } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { - left = output; - right = -output; - } + double drift = this.getLeftDistance() - this.getRightDistance(); + if (Math.abs(output) > 0 && Math.abs(output) < 0.3) + output = Math.signum(output) * 0.3; + left = output; + right = output + drift * Constants.DriveTrain.kp / 10; drive(left, right); pidOutput = output; } @@ -233,13 +185,7 @@ public class DriveTrain extends PIDSubsystem { * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE */ private double sensorFeedback() { - if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) - return getAvgEncoderDistance(); - else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) - return -this.getGyroAngle(); - // counterclockwise is positive on joystick but we want it to be negative - else - return 0; + return getAvgEncoderDistance(); } /* @@ -250,68 +196,6 @@ public class DriveTrain extends PIDSubsystem { */ public void drive(double left, double right) { robotDrive.tankDrive(-left, -right); - // dunno why but inverted drive (- values is forward) - if (!Constants.DriveTrain.inverted) - robotDrive.tankDrive(-left, -right); - else - robotDrive.tankDrive(right, left); - } - - /* - * constrains the distance to within -100 and 100 since we aren't going to - * drive more than 100 inches - * - * Configure Encoder PID - * - * Sets the setpoint to the PID subsystem - */ - public void driveDistance(double dist, double maxTimeOut) { - dist = MathLib.constrain(dist, -100, 100); - setEncoderPID(); - setSetpoint(dist); - } - - /* - * Sets the encoder mode Updates the PID constants sets the tolerance and sets - * output/input ranges Enables the PID controllers - */ - public void setEncoderPID() { - DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE; - this.updatePID(); - this.setAbsoluteTolerance(Constants.DriveTrain.encoderTolerance); - this.setOutputRange(-1.0, 1.0); - this.setInputRange(-200.0, 200.0); - this.enable(); - } - - /* - * Sets the Gyro Mode Updates the PID constants, sets the tolerance and sets - * output/input ranges Enables the PID controllers - */ - private void setGyroPID() { - DRIVE_MODE = Constants.DriveTrain.GYRO_MODE; - this.updatePID(); - this.getPIDController().setPID(Constants.DriveTrain.gp, - Constants.DriveTrain.gi, Constants.DriveTrain.gd); - - this.setAbsoluteTolerance(Constants.DriveTrain.gyroTolerance); - this.setOutputRange(-1.0, 1.0); - this.setInputRange(-360.0, 360.0); - this.enable(); - } - - /* - * Turning method that should be used repeatedly in a command - * - * First constrains the angle to within -360 and 360 since that is as much as - * we need to turn - * - * Configures Gyro PID and sets the setpoint as an angle - */ - public void turnAngle(double angle) { - angle = MathLib.constrain(angle, -360, 360); - setGyroPID(); - setSetpoint(angle); } public void setMotorSpeeds(double left, double right) { diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Scaler.java b/src/org/usfirst/frc/team3501/robot/subsystems/Scaler.java deleted file mode 100755 index e56e4a81..00000000 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Scaler.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.usfirst.frc.team3501.robot.subsystems; - -import org.usfirst.frc.team3501.robot.Constants; - -import edu.wpi.first.wpilibj.CANTalon; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.DoubleSolenoid.Value; -import edu.wpi.first.wpilibj.command.Subsystem; - -public class Scaler extends Subsystem { - private DoubleSolenoid scaler; - private CANTalon winch; - - public Scaler() { - scaler = new DoubleSolenoid(Constants.Scaler.FORWARD_CHANNEL, - Constants.Scaler.REVERSE_CHANNEL); - winch = new CANTalon(Constants.Scaler.WINCH_MOTOR); - } - - public Value getSolenoidValue() { - return scaler.get(); - } - - public void liftScissorLift() { - scaler.set(DoubleSolenoid.Value.kReverse); - } - - public void lowerScissorLift() { - scaler.set(DoubleSolenoid.Value.kForward); - } - - public void runWinch(double speed) { - if (speed > 1) - speed = 1; - if (speed < -1) - speed = -1; - - winch.set(speed); - } - - public void stopWinch() { - runWinch(Constants.Scaler.WINCH_STOP_SPEED); - } - - @Override - protected void initDefaultCommand() { - - } -} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index 8649045a..c613d7bb 100755 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -1,7 +1,6 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.sensors.Photogate; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.command.Subsystem; @@ -18,8 +17,6 @@ import edu.wpi.first.wpilibj.command.Subsystem; public class Shooter extends Subsystem { private DoubleSolenoid catapult1, catapult2; - private Photogate photogate; - private boolean usePhotoGate; public Shooter() { catapult1 = new DoubleSolenoid(Constants.Shooter.CATAPULT1_MODULE, @@ -28,22 +25,6 @@ public class Shooter extends Subsystem { catapult2 = new DoubleSolenoid(Constants.Shooter.CATAPULT2_MODULE, Constants.Shooter.CATAPULT2_FORWARD, Constants.Shooter.CATAPULT2_REVERSE); - usePhotoGate = false; - } - - /*** - * This method checks to see if the ball has successfully passed through the - * intake rollers and is inside. - * - * @return whether the presence of the ball is true or false and returns the - * state of the condition (true or false). - */ - - public boolean isBallInside() { - if (usePhotogate()) - return photogate.isBallPresent(); - else - return true; } // Catapult Commands @@ -57,14 +38,6 @@ public class Shooter extends Subsystem { catapult2.set(Constants.Shooter.reset); } - public boolean usePhotogate() { - return this.usePhotoGate; - } - - public void togglePhotoGate() { - this.usePhotoGate = !this.usePhotoGate; - } - @Override protected void initDefaultCommand() { } -- 2.30.2