diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/AutoGenerator.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/AutoGenerator.java index 6e67432..171a0e5 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/AutoGenerator.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/AutoGenerator.java @@ -2,9 +2,11 @@ import java.util.ArrayList; -import org.usfirst.frc.team3494.robot.commands.auto.AngleTurn; import org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive; +import org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive; +import org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive; import org.usfirst.frc.team3494.robot.commands.auto.XYDrive; +import org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp; import edu.wpi.first.wpilibj.command.Command; @@ -41,16 +43,45 @@ public static ArrayList autoOne() { */ public static ArrayList crossBaseLine() { ArrayList list = new ArrayList(); - list.add(new DistanceDrive(100, UnitTypes.INCHES)); + list.add(new DistanceDrive(-72, UnitTypes.INCHES)); + return list; + } + + public static ArrayList placeCenterGear() { + ArrayList list = new ArrayList(); + list.add(new PIDFullDrive(110.5)); return list; } public static ArrayList gearPlaceAttempt() { ArrayList list = new ArrayList(); - list.add(new AngleTurn(180)); - list.add(new AngleTurn(-10)); - list.add(new DistanceDrive(-5, UnitTypes.INCHES)); - // list.add(new XYDrive()); + list.add(new PIDFullDrive(87.5)); + list.add(new PIDAngleDrive(65)); + list.add(new PIDFullDrive(54)); + // list.add(new DistanceDrive(-60, UnitTypes.INCHES)); + return list; + } + + public static ArrayList gearPlaceAttemptLeft() { + ArrayList list = new ArrayList(); + list.add(new PIDFullDrive(87.5)); + list.add(new PIDAngleDrive(-65)); + list.add(new PIDFullDrive(54)); + // list.add(new DistanceDrive(-60, UnitTypes.INCHES)); + return list; + } + + public static ArrayList activeLeftGear() { + ArrayList list = AutoGenerator.gearPlaceAttemptLeft(); + list.add(new ToggleGearRamp()); + list.add(new PIDFullDrive(10)); + return list; + } + + public static ArrayList activeGearRight() { + ArrayList list = AutoGenerator.gearPlaceAttempt(); + list.add(new ToggleGearRamp()); + list.add(new PIDFullDrive(-10)); return list; } } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/OI.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/OI.java index 61a7ce6..7ab7abc 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/OI.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/OI.java @@ -1,14 +1,16 @@ package org.usfirst.frc.team3494.robot; +import org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive; import org.usfirst.frc.team3494.robot.commands.climb.Climb; import org.usfirst.frc.team3494.robot.commands.climb.ClimberToggle; import org.usfirst.frc.team3494.robot.commands.climb.StopClimber; import org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain; -import org.usfirst.frc.team3494.robot.commands.gears.HoldInState; +import org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward; +import org.usfirst.frc.team3494.robot.commands.gears.SetReverse; import org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp; -import org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition; import org.usfirst.frc.team3494.robot.commands.turret.Shoot; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.buttons.JoystickButton; @@ -26,6 +28,8 @@ public class OI { // Button button = new JoystickButton(stick, buttonNumber); public final XboxController xbox = new XboxController(RobotMap.XBOX_ONE); public final XboxController xbox_2 = new XboxController(RobotMap.XBOX_TWO); + public final Joystick stick_l = new Joystick(3); + public final Joystick stick_r = new Joystick(4); // There are a few additional built in buttons you can use. Additionally, // by subclassing Button you can create custom triggers and bind those to // commands the same as any other Button. @@ -46,6 +50,7 @@ public class OI { // until it is finished as determined by it's isFinished method. // button.whenReleased(new ExampleCommand()); public JoystickButton xbox_a = new JoystickButton(xbox, 1); + public JoystickButton xbox_a_2 = new JoystickButton(xbox_2, 1); public JoystickButton xbox_lt = new JoystickButton(xbox, 5); @@ -59,20 +64,28 @@ public class OI { public JoystickButton xbox_y_2 = new JoystickButton(xbox_2, 4); public JoystickButton xbox_x = new JoystickButton(xbox, 3); + public JoystickButton xbox_x_2 = new JoystickButton(xbox_2, 3); public JoystickButton xbox_select_2 = new JoystickButton(xbox_2, 7); public JoystickButton xbox_start_2 = new JoystickButton(xbox_2, 8); public OI() { // Ready Player One - xbox_b.whileHeld(new Climb(DriveDirections.UP)); - xbox_b.whenReleased(new StopClimber()); - xbox_x.whileHeld(new HoldInState()); + xbox_a.whenPressed(new DistanceDrive(-12, UnitTypes.INCHES)); // Ready Player Two - xbox_b_2.whenPressed(new SwitchPosition()); + // Climb controls + xbox_a_2.whileActive(new Climb(DriveDirections.UP)); + xbox_a_2.whenReleased(new StopClimber()); + + xbox_b_2.whenPressed(new SetReverse()); + + xbox_x_2.whileHeld(new HoldInState_Forward()); + xbox_y_2.whenPressed(new ToggleGearRamp()); + xbox_rt_2.whenPressed(new Shoot()); xbox_rt_2.whenReleased(new Shoot(0)); + xbox_select_2.whenPressed(new ClimberToggle()); xbox_start_2.whileHeld(new HoldDriveTrain()); } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/Robot.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/Robot.java index 2dffbdf..892a28f 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/Robot.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/Robot.java @@ -1,17 +1,28 @@ package org.usfirst.frc.team3494.robot; +import java.util.ArrayList; + +import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.imgproc.Imgproc; import org.usfirst.frc.team3494.robot.commands.auto.ConstructedAuto; +import org.usfirst.frc.team3494.robot.commands.auto.NullAuto; +import org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive; +import org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive; import org.usfirst.frc.team3494.robot.subsystems.Climber; import org.usfirst.frc.team3494.robot.subsystems.Drivetrain; -import org.usfirst.frc.team3494.robot.subsystems.GearTake; -import org.usfirst.frc.team3494.robot.subsystems.Intake; +import org.usfirst.frc.team3494.robot.subsystems.GearTake_2; import org.usfirst.frc.team3494.robot.subsystems.Kompressor; import org.usfirst.frc.team3494.robot.subsystems.Turret; +import org.usfirst.frc.team3494.robot.vision.GripPipeline; +import com.ctre.CANTalon; import com.kauailabs.navx.frc.AHRS; import edu.wpi.cscore.UsbCamera; import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.Preferences; @@ -21,6 +32,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.vision.VisionThread; /** * The VM is configured to automatically run this class, and to call the @@ -35,9 +47,8 @@ public class Robot extends IterativeRobot { public static Drivetrain driveTrain; public static Climber climber; public static Turret turret; - public static Intake intake; public static Kompressor kompressor; - public static GearTake gearTake; + public static GearTake_2 gearTake; /** * The gyro board mounted to the RoboRIO. * @@ -50,30 +61,93 @@ public class Robot extends IterativeRobot { public static SendableChooser chooser; public static Preferences prefs; + public static UsbCamera camera_0; + public static UsbCamera camera_1; + // Vision items + private static final int IMG_WIDTH = 320; + @SuppressWarnings("unused") + private static final int IMG_HEIGHT = 240; + VisionThread visionThread; + public double centerX = 0.0; + public double absolutelyAverage = 0.0; + @SuppressWarnings("unused") + private ArrayList filteredContours; + private ArrayList averages = new ArrayList(); + + private final Object imgLock = new Object(); + /** * This function is run when the robot is first started up and should be * used for any initialization code. */ @Override public void robotInit() { + System.out.println("Hello FTAs, how are you doing?"); + System.out.println("Because I'm a QUADRANGLE."); chooser = new SendableChooser(); driveTrain = new Drivetrain(); climber = new Climber(); + climber.disengagePTO(); turret = new Turret(); kompressor = new Kompressor(); - intake = new Intake(); - gearTake = new GearTake(); + gearTake = new GearTake_2(); oi = new OI(); ahrs = new AHRS(SerialPort.Port.kMXP); + Robot.climber.disengagePTO(); // Auto programs come after all subsystems are created chooser.addDefault("To the baseline!", new ConstructedAuto(AutoGenerator.crossBaseLine())); - chooser.addObject("Other command", new ConstructedAuto(AutoGenerator.crossBaseLine())); - @SuppressWarnings("unused") - UsbCamera camera_0 = CameraServer.getInstance().startAutomaticCapture(0); + chooser.addObject("Center Gear Placer", new ConstructedAuto(AutoGenerator.placeCenterGear())); + chooser.addObject("[beta] Right Gear Attempt", new ConstructedAuto(AutoGenerator.gearPlaceAttempt())); + chooser.addObject("[beta] Left Gear Attempt", new ConstructedAuto(AutoGenerator.gearPlaceAttemptLeft())); + chooser.addObject("Follow the shiny", null); + chooser.addObject("Do nothing", new NullAuto()); + chooser.addObject("PID Test - turn 90 degrees", new PIDAngleDrive(90)); + chooser.addObject("PID Test - drive straight", new PIDFullDrive(36)); + chooser.addObject("Active Gear placer - Robot turn left", new ConstructedAuto(AutoGenerator.activeLeftGear())); + chooser.addObject("Active Gear placer - Robot turn right", new ConstructedAuto(AutoGenerator.activeGearRight())); // put chooser on DS - SmartDashboard.putData("Auto mode", chooser); + SmartDashboard.putData("AUTO CHOOSER", chooser); // get preferences prefs = Preferences.getInstance(); + camera_0 = CameraServer.getInstance().startAutomaticCapture("Gear View", 0); + camera_0.setExposureManual(20); + // camera_1 = CameraServer.getInstance().startAutomaticCapture("Intake + // View", 1); + // Create and start vision thread + visionThread = new VisionThread(camera_0, new GripPipeline(), pipeline -> { + if (!pipeline.filterContoursOutput().isEmpty()) { + if (pipeline.filterContoursOutput().size() >= 2) { + MatOfPoint firstCont = pipeline.filterContoursOutput().get(0); + MatOfPoint secondCont = pipeline.filterContoursOutput().get(1); + double average_y_one = 0; + for (Point p : firstCont.toList()) { + average_y_one += p.y; + } + double average_y_two = 0; + for (Point p : secondCont.toList()) { + average_y_two += p.y; + } + // divide by number of points to give actual average + average_y_two = average_y_two / secondCont.toList().size(); + average_y_one = average_y_one / firstCont.toList().size(); + Rect r = Imgproc.boundingRect(pipeline.findContoursOutput().get(0)); + synchronized (imgLock) { + centerX = r.x + (r.width / 2); + filteredContours = pipeline.filterContoursOutput(); + // add averages to list + averages.add(average_y_one); + averages.add(average_y_two); + } + } else { + Rect r = Imgproc.boundingRect(pipeline.findContoursOutput().get(0)); + synchronized (imgLock) { + centerX = r.x + (r.width / 2); + filteredContours = pipeline.filterContoursOutput(); + } + } + } + }); + visionThread.start(); } /** @@ -107,7 +181,19 @@ public void autonomousInit() { autonomousCommand = chooser.getSelected(); // schedule the autonomous command (example) if (autonomousCommand != null) { + System.out.println("Selected command " + chooser.getSelected().getName()); autonomousCommand.start(); + } else { + System.out.println("Defaulting to track the shiny"); + } + // set ramps + for (CANTalon t : Robot.driveTrain.leftSide) { + t.setVoltageRampRate(0); + t.enableBrakeMode(true); + } + for (CANTalon t : Robot.driveTrain.rightSide) { + t.setVoltageRampRate(0); + t.enableBrakeMode(true); } } @@ -116,11 +202,41 @@ public void autonomousInit() { */ @Override public void autonomousPeriodic() { - Scheduler.getInstance().run(); + SmartDashboard.putData(Scheduler.getInstance()); + if (autonomousCommand != null) { + Scheduler.getInstance().run(); + } else { + double centerX; + synchronized (imgLock) { + centerX = this.centerX; + System.out.println("CenterX: " + this.centerX); + } + double turn = centerX - (Robot.IMG_WIDTH / 2); + // drive with turn + System.out.println("Turn: " + turn); + Robot.driveTrain.wpiDrive.arcadeDrive(0.5, (turn * 0.005) * -1); + } + SmartDashboard.putNumber("[left] distance", Robot.driveTrain.getLeftDistance(UnitTypes.RAWCOUNT)); + SmartDashboard.putNumber("[left] distance inches", Robot.driveTrain.getLeftDistance(UnitTypes.INCHES)); + + SmartDashboard.putNumber("[right] distance", Robot.driveTrain.getRightDistance(UnitTypes.RAWCOUNT)); + SmartDashboard.putNumber("[right] distance inches", Robot.driveTrain.getRightDistance(UnitTypes.INCHES)); + + SmartDashboard.putNumber("Motor 0", Robot.pdp.getCurrent(0)); + SmartDashboard.putNumber("Motor 1", Robot.pdp.getCurrent(1)); + SmartDashboard.putNumber("Motor 2", Robot.pdp.getCurrent(2)); + + SmartDashboard.putNumber("Talon Distance Right", Robot.driveTrain.rightSide[0].getPosition()); + SmartDashboard.putNumber("Talon Distance Left", Robot.driveTrain.leftSide[0].getPosition()); + + SmartDashboard.putNumber("Motor 13", Robot.pdp.getCurrent(13)); + SmartDashboard.putNumber("Motor 14", Robot.pdp.getCurrent(14)); + SmartDashboard.putNumber("Motor 15", Robot.pdp.getCurrent(15)); } @Override public void teleopInit() { + Robot.gearTake.setGrasp(Value.kOff); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -128,6 +244,17 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + camera_0.setExposureAuto(); + camera_0.setWhiteBalanceAuto(); + // set ramps + for (CANTalon t : Robot.driveTrain.leftSide) { + t.setVoltageRampRate(Drivetrain.RAMP); + t.enableBrakeMode(true); + } + for (CANTalon t : Robot.driveTrain.rightSide) { + t.setVoltageRampRate(Drivetrain.RAMP); + t.enableBrakeMode(true); + } } /** @@ -146,6 +273,9 @@ public void teleopPeriodic() { SmartDashboard.putNumber("[right] distance", Robot.driveTrain.getRightDistance(UnitTypes.RAWCOUNT)); SmartDashboard.putNumber("[right] distance inches", Robot.driveTrain.getRightDistance(UnitTypes.INCHES)); + SmartDashboard.putNumber("Talon Distance Right", Robot.driveTrain.rightSide[0].getPosition()); + SmartDashboard.putNumber("Talon Distance Left", Robot.driveTrain.leftSide[0].getPosition()); + SmartDashboard.putNumber("Motor 0", Robot.pdp.getCurrent(0)); SmartDashboard.putNumber("Motor 1", Robot.pdp.getCurrent(1)); SmartDashboard.putNumber("Motor 2", Robot.pdp.getCurrent(2)); @@ -153,6 +283,9 @@ public void teleopPeriodic() { SmartDashboard.putNumber("Motor 13", Robot.pdp.getCurrent(13)); SmartDashboard.putNumber("Motor 14", Robot.pdp.getCurrent(14)); SmartDashboard.putNumber("Motor 15", Robot.pdp.getCurrent(15)); + + SmartDashboard.putNumber("Climber Motor", Robot.pdp.getCurrent(RobotMap.CLIMBER_MOTOR_PDP)); + SmartDashboard.putBoolean("line break", Robot.gearTake.lb.getBroken()); } /** diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/RobotMap.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/RobotMap.java index f79871b..4a80078 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/RobotMap.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/RobotMap.java @@ -41,7 +41,7 @@ public class RobotMap { public static final int UP_MOTOR = 4; // climber public static final int CLIMBER_MOTOR = 1; - public static final int CLIMBER_MOTOR_PDP = 10; + public static final int CLIMBER_MOTOR_PDP = 11; public static final int CLIMBER_PTO_FORWARD = 6; public static final int CLIMBER_PTO_BACKARD = 7; // turret @@ -50,14 +50,20 @@ public class RobotMap { public static final int TURRET_UPPER = 64; public static final int COMPRESSOR = 0; - // intake pistons - public static final int INTAKE_PISTON_CHONE = 0; - public static final int INTAKE_PISTON_CHTWO = 1; // gear holder pistons public static final int GEAR_RAMP_CHONE = 2; public static final int GEAR_RAMP_CHTWO = 3; public static final int GEAR_GRASP_CHONE = 4; public static final int GEAR_GRASP_CHTWO = 5; + + public static final int GEAR_GRASP_S2_FORWARD = 1; + public static final int GEAR_GRASP_S2_BACKWARD = 0; + // conveyer public static final int CONVEYER_M = 14; + // single piston gear holder + public static final int MONO_GEAR_FORWARD = 10; + public static final int MONO_GEAR_REVERSE = 11; + + public static final long TALON_RESET_DELAY = 100; } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/DelayCommand.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/DelayCommand.java new file mode 100644 index 0000000..a57b29f --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/DelayCommand.java @@ -0,0 +1,50 @@ +package org.usfirst.frc.team3494.robot.commands; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * Command to cause delays in autonomous. WIP. + */ +public class DelayCommand extends Command { + + double time; + + public DelayCommand(double t) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + this.time = t; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + try { + Thread.sleep((long) time); + } catch (InterruptedException e) { + System.out.println("HOW LONG DID YOU WAIT?!"); + e.printStackTrace(); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.java index a596a4b..ddf7167 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3494.robot.commands.auto; import org.usfirst.frc.team3494.robot.Robot; +import org.usfirst.frc.team3494.robot.RobotMap; import org.usfirst.frc.team3494.robot.UnitTypes; import edu.wpi.first.wpilibj.command.Command; @@ -39,26 +40,32 @@ public DistanceDrive(double distance, UnitTypes unitType) { protected void initialize() { Robot.driveTrain.resetRight(); Robot.driveTrain.resetLeft(); + try { + Thread.sleep(RobotMap.TALON_RESET_DELAY); + } catch (InterruptedException e) { + System.out.println("ah crap"); + e.printStackTrace(); + } + System.out.println("Driving " + this.dist + " " + this.unit.toString() + "(s)"); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (this.dist > Robot.driveTrain.getAvgDistance(this.unit)) { - Robot.driveTrain.adjustedTankDrive(0.4, 0.4); - } else if (this.dist < Robot.driveTrain.getAvgDistance(this.unit)) { - Robot.driveTrain.adjustedTankDrive(-0.4, -0.4); + if (this.dist > 0) { + Robot.driveTrain.adjustedTankDrive(0.185, 0.2); + } else if (this.dist < 0) { + Robot.driveTrain.adjustedTankDrive(-0.185, -0.2); } else { return; } - System.out.println(Robot.driveTrain.getAvgDistance(this.unit)); + System.out.println("Average distance: " + Robot.driveTrain.getAvgDistance(this.unit)); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return (Robot.driveTrain.getAvgDistance(this.unit) >= this.dist - 1 - && Robot.driveTrain.getAvgDistance(this.unit) <= this.dist + 1); + return Math.abs(Robot.driveTrain.getAvgDistance(unit)) >= Math.abs(this.dist); } // Called once after isFinished returns true diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/NullAuto.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/NullAuto.java new file mode 100644 index 0000000..beedcbb --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/NullAuto.java @@ -0,0 +1,42 @@ +package org.usfirst.frc.team3494.robot.commands.auto; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * an auto program that does nothing + */ +public class NullAuto extends Command { + + public NullAuto() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + System.out.println("i am doing nothing"); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/PIDAngleDrive.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/PIDAngleDrive.java new file mode 100644 index 0000000..b588bfc --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/PIDAngleDrive.java @@ -0,0 +1,63 @@ +package org.usfirst.frc.team3494.robot.commands.auto; + +import org.usfirst.frc.team3494.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Turns the robot to an angle using PID. Hello, PID. + */ +public class PIDAngleDrive extends Command { + + private double angle; + + /** + * Constructor. + * + * @param angle + * The angle to turn, in degrees. + */ + public PIDAngleDrive(double angle) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + this.angle = angle; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.ahrs.reset(); + Robot.driveTrain.enable(); + Robot.driveTrain.setSetpoint(this.angle); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + SmartDashboard.putNumber("angle", Robot.ahrs.getAngle()); + System.out.println(Robot.ahrs.getAngle()); + Robot.driveTrain.ArcadeDrive(0, -Robot.driveTrain.PIDTune, true); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return Robot.driveTrain.onTarget(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.driveTrain.disable(); + Robot.driveTrain.stopAll(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/PIDFullDrive.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/PIDFullDrive.java new file mode 100644 index 0000000..dd4f862 --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/PIDFullDrive.java @@ -0,0 +1,94 @@ +package org.usfirst.frc.team3494.robot.commands.auto; + +import org.usfirst.frc.team3494.robot.Robot; +import org.usfirst.frc.team3494.robot.RobotMap; +import org.usfirst.frc.team3494.robot.UnitTypes; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Drives straight/forward with an angle (WIP) using the drivetrain's PID loop. + * Only works in inches. + */ +public class PIDFullDrive extends Command { + + private double distance; + private double angle; + + /** + * Constructor. + * + * @param dist + * The distance to drive, in inches. + * @param angle + * The angle to turn to. + */ + public PIDFullDrive(double dist, double angle) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + this.distance = dist; + } + + /** + * Other constructor. Uses default angle of 0 degrees. + * + * @param dist + * The distance to drive in inches. + */ + public PIDFullDrive(double dist) { + this(dist, 0); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.driveTrain.resetLeft(); + Robot.driveTrain.resetRight(); + try { + Thread.sleep(RobotMap.TALON_RESET_DELAY); + } catch (InterruptedException e) { + System.out.println("ah crap"); + e.printStackTrace(); + } + Robot.ahrs.reset(); + Robot.driveTrain.enable(); + Robot.driveTrain.setSetpoint(this.angle); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + SmartDashboard.putNumber("angle", Robot.ahrs.getAngle()); + System.out.println(Robot.ahrs.getYaw()); + // System.out.println(Robot.driveTrain.PIDTune); + if (this.distance < 0) { + Robot.driveTrain.ArcadeDrive(0.5, -Robot.driveTrain.PIDTune, true); + } else { + Robot.driveTrain.ArcadeDrive(-0.5, -Robot.driveTrain.PIDTune, true); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + // return false; + return Math.abs(Robot.driveTrain.getAvgDistance(UnitTypes.INCHES)) >= Math.abs(this.distance); + } + + // Called once after isFinished returns true + @Override + protected void end() { + System.out.println("done PID driving"); + Robot.driveTrain.disable(); + Robot.driveTrain.stopAll(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + this.end(); + } +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/RunIntake.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/SetGearGrasp.java similarity index 63% rename from 3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/RunIntake.java rename to 3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/SetGearGrasp.java index feff20b..976dc0e 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/RunIntake.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/SetGearGrasp.java @@ -1,21 +1,22 @@ -package org.usfirst.frc.team3494.robot.commands.intake; +package org.usfirst.frc.team3494.robot.commands.auto; import org.usfirst.frc.team3494.robot.Robot; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.command.Command; /** - * Runs the intake. Default command for Intake subsystem. + * */ -public class RunIntake extends Command { +public class SetGearGrasp extends Command { - boolean running = false; - double speed; + private Value v; - public RunIntake() { + public SetGearGrasp(Value val) { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); - requires(Robot.intake); + this.requires(Robot.gearTake); + this.v = val; } // Called just before this Command runs the first time @@ -26,19 +27,13 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (Robot.oi.xbox_rt.get()) { - Robot.intake.runIntake(0.75); - } else if (Robot.oi.xbox_lt.get()) { - Robot.intake.runIntake(-0.75); - } else { - Robot.intake.stopAll(); - } + Robot.gearTake.setGrasp(v); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/SpeedTest.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/SpeedTest.java new file mode 100644 index 0000000..f59cafc --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/SpeedTest.java @@ -0,0 +1,75 @@ +package org.usfirst.frc.team3494.robot.commands.auto.tests; + +import org.usfirst.frc.team3494.robot.Robot; + +import com.ctre.CANTalon; +import com.ctre.CANTalon.TalonControlMode; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Test for CAN Talon speed mode. + */ +public class SpeedTest extends Command { + public SpeedTest() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.driveTrain.leftSide[0].changeControlMode(TalonControlMode.Speed); + Robot.driveTrain.leftSide[0].setProfile(0); + Robot.driveTrain.leftSide[0].setF(0); + Robot.driveTrain.leftSide[0].setP(1); + Robot.driveTrain.leftSide[0].setI(0); + Robot.driveTrain.leftSide[0].setD(0); + + Robot.driveTrain.rightSide[0].changeControlMode(TalonControlMode.Speed); + Robot.driveTrain.rightSide[0].setProfile(0); + Robot.driveTrain.rightSide[0].setF(0); + Robot.driveTrain.rightSide[0].setP(1); + Robot.driveTrain.rightSide[0].setI(0); + Robot.driveTrain.rightSide[0].setD(0); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + SmartDashboard.putNumber("left: ", Robot.driveTrain.leftSide[0].getSpeed()); + SmartDashboard.putNumber("right:", Robot.driveTrain.rightSide[0].getSpeed()); + Robot.driveTrain.leftSide[0].set(2000); + Robot.driveTrain.leftSide[1].set(0.4); + Robot.driveTrain.leftSide[2].set(0.4); + Robot.driveTrain.rightSide[0].set(-2500); + Robot.driveTrain.rightSide[1].set(-0.4); + Robot.driveTrain.rightSide[2].set(-0.4); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + for (CANTalon t : Robot.driveTrain.leftSide) { + t.changeControlMode(TalonControlMode.PercentVbus); + } + for (CANTalon t : Robot.driveTrain.rightSide) { + t.changeControlMode(TalonControlMode.PercentVbus); + } + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/StageTest.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/StageTest.java new file mode 100644 index 0000000..2958819 --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/StageTest.java @@ -0,0 +1,50 @@ +package org.usfirst.frc.team3494.robot.commands.auto.tests; + +import org.usfirst.frc.team3494.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * Chris made me do it + */ +public class StageTest extends Command { + private int counter; + + public StageTest() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.driveTrain.initStaging(); + Robot.driveTrain.stagedTankDrive(-0.4, 0.4); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + this.counter++; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return counter > 100; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.driveTrain.snapBackToReality(); + Robot.driveTrain.setAll(0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/package-info.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/package-info.java new file mode 100644 index 0000000..50e52d7 --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/tests/package-info.java @@ -0,0 +1,5 @@ +/** + * Package for holding auto codes that exist solely as tests (i.e. not for + * competition purposes.) + */ +package org.usfirst.frc.team3494.robot.commands.auto.tests; \ No newline at end of file diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/climb/ClimberToggle.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/climb/ClimberToggle.java index 0bdc86a..eb532b0 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/climb/ClimberToggle.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/climb/ClimberToggle.java @@ -2,6 +2,7 @@ import org.usfirst.frc.team3494.robot.Robot; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.command.Command; /** @@ -13,6 +14,8 @@ public ClimberToggle() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); requires(Robot.climber); + requires(Robot.kompressor); + requires(Robot.gearTake); } // Called just before this Command runs the first time @@ -25,8 +28,13 @@ protected void initialize() { protected void execute() { if (!Robot.climber.getState()) { Robot.climber.engagePTO(); + Robot.kompressor.compress.stop(); + Robot.gearTake.setRamp(Value.kForward); + Robot.gearTake.setGrasp(Value.kForward); } else { Robot.climber.disengagePTO(); + Robot.kompressor.compress.start(); + Robot.gearTake.setGrasp(Value.kReverse); } } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/drive/Drive.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/drive/Drive.java index cdcf84e..d2ca781 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/drive/Drive.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/drive/Drive.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.GenericHID.Hand; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * Command to run drivetrain. Only takes input in the form of joysticks. @@ -31,27 +32,46 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - int dpad = Robot.oi.xbox.getPOV(); - if (dpad == 0) { + int dpad = Robot.oi.stick_l.getPOV(); + if (dpad == 0 || Robot.oi.stick_l.getRawButton(7)) { Robot.driveTrain.inverter = 1; - } else if (dpad == 180) { + } else if (dpad == 180 || Robot.oi.stick_l.getRawButton(8)) { Robot.driveTrain.inverter = -1; + } else if (dpad == 270 || Robot.oi.stick_l.getRawButton(9)) { + Robot.driveTrain.scaleDown = 0.5D; + } else if (dpad == 90 || Robot.oi.stick_l.getRawButton(10)) { + Robot.driveTrain.scaleDown = 1.0D; } - if (Robot.prefs.getBoolean("arcade", true)) { - if (!Robot.driveTrain.getInverted()) { - Robot.driveTrain.wpiDrive.arcadeDrive(Robot.oi.xbox.getY(Hand.kLeft) * Robot.driveTrain.inverter, - -Robot.oi.xbox.getX(Hand.kLeft) * Robot.driveTrain.inverter); + SmartDashboard.putNumber("inverter", Robot.driveTrain.inverter); + SmartDashboard.putNumber("scale down", Robot.driveTrain.scaleDown); + boolean useX = Robot.prefs.getBoolean("usexbox", true); + if (useX) { + if (Robot.prefs.getBoolean("arcade", true)) { + if (Robot.driveTrain.getInverted()) { + Robot.driveTrain.ArcadeDrive( + Robot.oi.xbox.getY(Hand.kLeft) * Robot.driveTrain.inverter * Robot.driveTrain.scaleDown, + Robot.oi.xbox.getX(Hand.kLeft) * Robot.driveTrain.inverter * Robot.driveTrain.scaleDown, + true); + } else { + Robot.driveTrain.ArcadeDrive( + Robot.oi.xbox.getY(Hand.kLeft) * Robot.driveTrain.inverter * Robot.driveTrain.scaleDown, + -Robot.oi.xbox.getX(Hand.kLeft) * Robot.driveTrain.inverter * Robot.driveTrain.scaleDown, + true); + } } else { - Robot.driveTrain.wpiDrive.arcadeDrive(Robot.oi.xbox.getY(Hand.kLeft) * Robot.driveTrain.inverter, - -Robot.oi.xbox.getX(Hand.kLeft) * Robot.driveTrain.inverter); + if (!Robot.driveTrain.getInverted()) { + Robot.driveTrain.adjustedTankDrive(-Robot.oi.xbox.getY(Hand.kRight) * Robot.driveTrain.inverter, + -Robot.oi.xbox.getY(Hand.kLeft) * Robot.driveTrain.inverter); + } else { + Robot.driveTrain.adjustedTankDrive(-Robot.oi.xbox.getY(Hand.kLeft) * Robot.driveTrain.inverter, + -Robot.oi.xbox.getY(Hand.kRight) * Robot.driveTrain.inverter); + } } } else { - if (!Robot.driveTrain.getInverted()) { - Robot.driveTrain.adjustedTankDrive(-Robot.oi.xbox.getY(Hand.kRight) * Robot.driveTrain.inverter, - -Robot.oi.xbox.getY(Hand.kLeft) * Robot.driveTrain.inverter); + if (Robot.driveTrain.getInverted()) { + Robot.driveTrain.TankDrive(-Robot.oi.stick_l.getY(), Robot.oi.stick_r.getY()); } else { - Robot.driveTrain.adjustedTankDrive(-Robot.oi.xbox.getY(Hand.kLeft) * Robot.driveTrain.inverter, - -Robot.oi.xbox.getY(Hand.kRight) * Robot.driveTrain.inverter); + Robot.driveTrain.TankDrive(Robot.oi.stick_r.getY(), -Robot.oi.stick_l.getY()); } } } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/drive/HoldDriveTrain.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/drive/HoldDriveTrain.java index 4638d6a..7b60870 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/drive/HoldDriveTrain.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/drive/HoldDriveTrain.java @@ -16,25 +16,30 @@ public HoldDriveTrain() { } // Called just before this Command runs the first time + @Override protected void initialize() { } // Called repeatedly when this Command is scheduled to run + @Override protected void execute() { - Robot.driveTrain.adjustedTankDrive(0.2, 0.2); + Robot.driveTrain.adjustedTankDrive(-0.2, -0.2); } // Make this return true when this Command no longer needs to run execute() + @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true + @Override protected void end() { } // Called when another command which requires one or more of the same // subsystems is scheduled to run + @Override protected void interrupted() { } } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/HoldInState.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/HoldInState_Forward.java similarity index 84% rename from 3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/HoldInState.java rename to 3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/HoldInState_Forward.java index 008b531..700c1e2 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/HoldInState.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/HoldInState_Forward.java @@ -8,35 +8,40 @@ /** * Holds the gear holder in the given position until canceled. */ -public class HoldInState extends Command { +public class HoldInState_Forward extends Command { - public HoldInState() { + public HoldInState_Forward() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); requires(Robot.gearTake); } // Called just before this Command runs the first time + @Override protected void initialize() { } // Called repeatedly when this Command is scheduled to run + @Override protected void execute() { Robot.gearTake.setGrasp(Value.kForward); } // Make this return true when this Command no longer needs to run execute() + @Override protected boolean isFinished() { return false; } // Called once after isFinished returns true + @Override protected void end() { - Robot.gearTake.setGrasp(Value.kReverse); + Robot.gearTake.setGrasp(Value.kOff); } // Called when another command which requires one or more of the same // subsystems is scheduled to run + @Override protected void interrupted() { this.end(); } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/SwitchPosition.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/SetReverse.java similarity index 60% rename from 3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/SwitchPosition.java rename to 3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/SetReverse.java index 2b5c6ef..a59a4f8 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/SwitchPosition.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/SetReverse.java @@ -1,21 +1,19 @@ -package org.usfirst.frc.team3494.robot.commands.intake; +package org.usfirst.frc.team3494.robot.commands.gears; import org.usfirst.frc.team3494.robot.Robot; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * Deploys the intake for the start of teleop. - * - * @see org.usfirst.frc.team3494.robot.subsystems.Intake + * */ -public class SwitchPosition extends Command { +public class SetReverse extends Command { - public SwitchPosition() { + public SetReverse() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); - requires(Robot.intake); + requires(Robot.gearTake); } // Called just before this Command runs the first time @@ -26,12 +24,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (!Robot.intake.isDeployed) { - Robot.intake.pushForward(); - } else { - Robot.intake.retract(); - } - SmartDashboard.putBoolean("Intake Deployed", Robot.intake.isDeployed); + Robot.gearTake.setGrasp(Value.kReverse); } // Make this return true when this Command no longer needs to run execute() diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/SetGearPosition.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/ToggleGearPosition.java similarity index 93% rename from 3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/SetGearPosition.java rename to 3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/ToggleGearPosition.java index eda6f3c..e28577a 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/SetGearPosition.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/gears/ToggleGearPosition.java @@ -8,8 +8,8 @@ /** * Sets the gear holder position to what it is not. */ -public class SetGearPosition extends Command { - public SetGearPosition() { +public class ToggleGearPosition extends Command { + public ToggleGearPosition() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); requires(Robot.gearTake); diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/package-info.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/package-info.java deleted file mode 100644 index e9ecfbd..0000000 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/intake/package-info.java +++ /dev/null @@ -1,7 +0,0 @@ -/** - * Package for intake-related commands. Commands for use in auto belong in - * {@link org.usfirst.frc.team3494.robot.commands.auto}. - * - * @see org.usfirst.frc.team3494.robot.subsystems.Intake - */ -package org.usfirst.frc.team3494.robot.commands.intake; \ No newline at end of file diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/sensors/LineBreak.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/sensors/LineBreak.java new file mode 100644 index 0000000..cf6c18a --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/sensors/LineBreak.java @@ -0,0 +1,38 @@ +package org.usfirst.frc.team3494.robot.sensors; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.AnalogTrigger; + +public class LineBreak { + /** + * Actual analog channel. + */ + private AnalogInput ai1; + /** + * Analog trigger object. + */ + private AnalogTrigger trigger1; + + public LineBreak(int channel) { + this.ai1 = new AnalogInput(channel); + this.trigger1 = new AnalogTrigger(ai1); + this.trigger1.setLimitsVoltage(0.777, 3.703); + } + + public LineBreak() { + this(1); + } + + /** + * Returns true if the trigger is broken. + * + * @return Whether or not the line is broken. + */ + public boolean getBroken() { + if (!this.trigger1.getInWindow()) { + return trigger1.getTriggerState(); + } else { + return false; + } + } +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/sensors/package-info.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/sensors/package-info.java new file mode 100644 index 0000000..5ec6a82 --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/sensors/package-info.java @@ -0,0 +1,4 @@ +/** + * Package for sensor type classes. + */ +package org.usfirst.frc.team3494.robot.sensors; \ No newline at end of file diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/Drivetrain.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/Drivetrain.java index e56402f..c957820 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/Drivetrain.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/Drivetrain.java @@ -1,14 +1,16 @@ package org.usfirst.frc.team3494.robot.subsystems; +import org.usfirst.frc.team3494.robot.Robot; import org.usfirst.frc.team3494.robot.RobotMap; import org.usfirst.frc.team3494.robot.UnitTypes; import org.usfirst.frc.team3494.robot.commands.drive.Drive; import com.ctre.CANTalon; +import com.ctre.CANTalon.FeedbackDevice; +import com.ctre.CANTalon.TalonControlMode; -import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.RobotDrive; -import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.command.PIDSubsystem; /** * Drivetrain subsystem. Contains all methods for controlling the robot's @@ -17,7 +19,7 @@ * * @since 0.0.0 */ -public class Drivetrain extends Subsystem implements IMotorizedSubsystem { +public class Drivetrain extends PIDSubsystem implements IMotorizedSubsystem { /** * Master drive talon, left side. Setting this should set all the talons on * the left side of the drive train. @@ -63,17 +65,25 @@ public class Drivetrain extends Subsystem implements IMotorizedSubsystem { * @since 0.0.0 */ public RobotDrive wpiDrive; - private Encoder encRight; - private Encoder encLeft; - private static double RAMP = 1.1730125; // lowest possible ramp + public static final double RAMP = 48; - public int inverter = 1; + public int inverter = -1; + public double scaleDown = 1; - public Drivetrain() { - super("Drivetrain"); + public CANTalon[] leftSide; + public CANTalon[] rightSide; + + public double PIDTune; + public Drivetrain() { + super("Drivetrain", 0.035, 0, 0); + // int maxAmps = 50; + // create left talons this.driveLeftMaster = new CANTalon(RobotMap.leftTalonOne); + this.driveLeftMaster.setFeedbackDevice(FeedbackDevice.QuadEncoder); + this.driveLeftMaster.configEncoderCodesPerRev(360); + this.driveLeftMaster.setEncPosition(0); this.driveLeftMaster.enableBrakeMode(true); this.driveLeftMaster.setVoltageRampRate(RAMP); this.driveLeftFollower_One = new CANTalon(RobotMap.leftTalonTwo); @@ -83,12 +93,18 @@ public Drivetrain() { this.driveLeftFollower_Two.enableBrakeMode(true); this.driveLeftFollower_Two.setVoltageRampRate(RAMP); // master follower - this.driveLeftFollower_One.changeControlMode(CANTalon.TalonControlMode.Follower); - this.driveLeftFollower_One.set(driveLeftMaster.getDeviceID()); - this.driveLeftFollower_Two.changeControlMode(CANTalon.TalonControlMode.Follower); - this.driveLeftFollower_Two.set(driveLeftMaster.getDeviceID()); + // this.driveLeftFollower_One.changeControlMode(CANTalon.TalonControlMode.Follower); + // this.driveLeftFollower_One.set(driveLeftMaster.getDeviceID()); + // this.driveLeftFollower_Two.changeControlMode(CANTalon.TalonControlMode.Follower); + // this.driveLeftFollower_Two.set(driveLeftMaster.getDeviceID()); + // create list + this.leftSide = new CANTalon[] { this.driveLeftMaster, this.driveLeftFollower_One, this.driveLeftFollower_Two }; this.driveRightMaster = new CANTalon(RobotMap.rightTalonOne); + this.driveRightMaster.setFeedbackDevice(FeedbackDevice.QuadEncoder); + this.driveRightMaster.reverseSensor(true); + this.driveRightMaster.setEncPosition(0); + this.driveRightMaster.configEncoderCodesPerRev(360); this.driveRightMaster.enableBrakeMode(true); this.driveRightMaster.setVoltageRampRate(RAMP); this.driveRightFollower_One = new CANTalon(RobotMap.rightTalonTwo); @@ -98,21 +114,36 @@ public Drivetrain() { this.driveRightFollower_Two.enableBrakeMode(true); this.driveRightFollower_Two.setVoltageRampRate(RAMP); // master follower - this.driveRightFollower_One.changeControlMode(CANTalon.TalonControlMode.Follower); - this.driveRightFollower_One.set(driveRightMaster.getDeviceID()); - this.driveRightFollower_Two.changeControlMode(CANTalon.TalonControlMode.Follower); - this.driveRightFollower_Two.set(driveRightMaster.getDeviceID()); + // this.driveRightFollower_One.changeControlMode(TalonControlMode.Follower); + // this.driveRightFollower_One.set(driveRightMaster.getDeviceID()); + // this.driveRightFollower_Two.changeControlMode(TalonControlMode.Follower); + // this.driveRightFollower_Two.set(driveRightMaster.getDeviceID()); + // list time! + this.rightSide = new CANTalon[] { this.driveRightMaster, this.driveRightFollower_One, + this.driveLeftFollower_Two }; this.wpiDrive = new RobotDrive(driveLeftMaster, driveRightMaster); + this.wpiDrive.setExpiration(Integer.MAX_VALUE); + this.wpiDrive.setSafetyEnabled(false); - this.encRight = new Encoder(RobotMap.ENCODER_RIGHT_A, RobotMap.ENCODER_RIGHT_B); - this.encRight.setDistancePerPulse(1 / 360); - this.encRight.reset(); + /* + * this.encRight = new Encoder(RobotMap.ENCODER_RIGHT_A, + * RobotMap.ENCODER_RIGHT_B); this.encRight.setDistancePerPulse(1 / + * 360); this.encRight.reset(); + */ - this.encLeft = new Encoder(RobotMap.ENCODER_LEFT_A, RobotMap.ENCODER_LEFT_B); - this.encLeft.setDistancePerPulse(1 / 360); - this.encLeft.setReverseDirection(true); - this.encLeft.reset(); + /* + * this.encLeft = new Encoder(RobotMap.ENCODER_LEFT_A, + * RobotMap.ENCODER_LEFT_B); this.encLeft.setDistancePerPulse(1 / 360); + * this.encLeft.setReverseDirection(true); this.encLeft.reset(); + */ + // PID control + this.PIDTune = 0; + double outRange = 0.5; + this.getPIDController().setInputRange(-180, 180); + this.getPIDController().setOutputRange(-outRange, outRange); + this.getPIDController().setContinuous(false); + this.getPIDController().setPercentTolerance(2.5); } // Put methods for controlling this subsystem // here. Call these from Commands. @@ -135,8 +166,18 @@ public void initDefaultCommand() { * between 0 and 1. */ public void TankDrive(double left, double right) { - driveLeftMaster.set(left); - driveRightMaster.set(right); + double leftScale = Robot.prefs.getDouble("left side multiplier", 1.0D); + double rightScale = Robot.prefs.getDouble("right side multiplier", 1.0D); + this.driveLeftMaster.set(left * this.scaleDown * leftScale); + this.driveRightMaster.set(right * this.scaleDown * rightScale); + this.driveLeftFollower_One.set(left * this.scaleDown * leftScale); + this.driveRightFollower_One.set(right * this.scaleDown * rightScale); + this.driveLeftFollower_Two.set(left * this.scaleDown * leftScale); + this.driveRightFollower_Two.set(right * this.scaleDown * rightScale); + } + + public void TeleopTank(double left, double right) { + this.TankDrive(left * this.inverter, right * this.inverter); } /** @@ -152,8 +193,62 @@ public void TankDrive(double left, double right) { * Talons. */ public void adjustedTankDrive(double left, double right) { - driveLeftMaster.set(-left); - driveRightMaster.set(right); + this.TankDrive(-left, right); + } + + /** + * Arcade drive implements single stick driving. This function lets you + * directly provide joystick values from any source. + * + * @param moveValue + * The value to use for forwards/backwards + * @param rotateValue + * The value to use for the rotate right/left + * @param squaredInputs + * If set, decreases the sensitivity at low speeds + * @author Worcester Polytechnic Institute + */ + public void ArcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) { + double leftMotorSpeed; + double rightMotorSpeed; + + moveValue = limit(moveValue); + rotateValue = limit(rotateValue); + + if (squaredInputs) { + // square the inputs (while preserving the sign) to increase fine + // control + // while permitting full power + if (moveValue >= 0.0) { + moveValue = moveValue * moveValue; + } else { + moveValue = -(moveValue * moveValue); + } + if (rotateValue >= 0.0) { + rotateValue = rotateValue * rotateValue; + } else { + rotateValue = -(rotateValue * rotateValue); + } + } + + if (moveValue > 0.0) { + if (rotateValue > 0.0) { + leftMotorSpeed = moveValue - rotateValue; + rightMotorSpeed = Math.max(moveValue, rotateValue); + } else { + leftMotorSpeed = Math.max(moveValue, -rotateValue); + rightMotorSpeed = moveValue + rotateValue; + } + } else { + if (rotateValue > 0.0) { + leftMotorSpeed = -Math.max(-moveValue, rotateValue); + rightMotorSpeed = moveValue + rotateValue; + } else { + leftMotorSpeed = moveValue - rotateValue; + rightMotorSpeed = -Math.max(-moveValue, -rotateValue); + } + } + this.TankDrive(leftMotorSpeed, -rightMotorSpeed); } /** @@ -165,7 +260,8 @@ public void adjustedTankDrive(double left, double right) { * unit. */ public double getRightDistance(UnitTypes unit) { - double inches = (Math.PI * 4) * (this.encRight.get() / 360); + double encCountsTalon = this.driveRightMaster.getPosition(); + double inches = (Math.PI * 4) * (encCountsTalon); if (unit.equals(UnitTypes.INCHES)) { return inches; } else if (unit.equals(UnitTypes.FEET)) { @@ -175,7 +271,7 @@ public double getRightDistance(UnitTypes unit) { } else if (unit.equals(UnitTypes.CENTIMETERS)) { return inches * 2.540; } else { - return this.encRight.get(); + return this.driveRightMaster.getPosition(); } } @@ -187,7 +283,8 @@ public double getRightDistance(UnitTypes unit) { * @return The distance the left encoder has counted, in the specified unit. */ public double getLeftDistance(UnitTypes unit) { - double inches = (Math.PI * 4) * (this.encLeft.get() / 360.0D); + double talonEncDist = this.driveLeftMaster.getPosition(); + double inches = (Math.PI * 4) * (talonEncDist); if (unit.equals(UnitTypes.INCHES)) { return inches; } else if (unit.equals(UnitTypes.FEET)) { @@ -197,23 +294,28 @@ public double getLeftDistance(UnitTypes unit) { } else if (unit.equals(UnitTypes.CENTIMETERS)) { return inches * 2.540; } else { - return this.encRight.get(); + return this.driveLeftMaster.getPosition(); } } public double getAvgDistance(UnitTypes unit) { - return ((this.getLeftDistance(unit) + this.getRightDistance(unit)) / 2); + return (this.getLeftDistance(unit) + this.getRightDistance(unit)) / 2; } /** * Resets the encoder on the right side of the drivetrain. */ public void resetRight() { - this.encRight.reset(); + // this.encRight.reset(); + this.driveRightMaster.setEncPosition(0); } + /** + * Resets the encoder on the left side of the drivetrain. + */ public void resetLeft() { - this.encRight.reset(); + // this.encLeft.reset(); + this.driveLeftMaster.setEncPosition(0); } @Override @@ -235,4 +337,75 @@ public void setAll(double speed) { public boolean getInverted() { return this.inverter == -1; } + + /** + * Stage-sets the drivetrain. Please, for the love of all that is holy call + * {@link Drivetrain#snapBackToReality()} after this. + * + * @param left + * The left power + * @param right + * The right power + */ + public void stagedTankDrive(double left, double right) { + this.driveLeftMaster.set(left); + this.driveRightMaster.set(right); + try { + Thread.sleep(10); + } catch (InterruptedException e) { + e.printStackTrace(); + } + this.driveLeftFollower_One.set(left); + this.driveRightFollower_One.set(right); + try { + Thread.sleep(10); + } catch (InterruptedException e) { + e.printStackTrace(); + } + this.driveLeftFollower_Two.set(left); + this.driveRightFollower_Two.set(right); + } + + public void initStaging() { + // change all to be percent Vbus + for (CANTalon t : this.leftSide) { + t.changeControlMode(TalonControlMode.PercentVbus); + } + for (CANTalon t : this.rightSide) { + t.changeControlMode(TalonControlMode.PercentVbus); + } + } + + public void snapBackToReality() { + // left reset + this.driveLeftFollower_One.changeControlMode(TalonControlMode.Follower); + this.driveLeftFollower_Two.changeControlMode(TalonControlMode.Follower); + this.driveLeftFollower_One.set(this.driveLeftMaster.getDeviceID()); + this.driveLeftFollower_Two.set(this.driveLeftMaster.getDeviceID()); + // right reset + this.driveRightFollower_One.changeControlMode(TalonControlMode.Follower); + this.driveRightFollower_Two.changeControlMode(TalonControlMode.Follower); + this.driveRightFollower_One.set(this.driveRightMaster.getDeviceID()); + this.driveRightFollower_Two.set(this.driveRightMaster.getDeviceID()); + } + + @Override + protected double returnPIDInput() { + return Robot.ahrs.getYaw(); + } + + @Override + protected void usePIDOutput(double output) { + this.PIDTune = output; + } + + private static double limit(double num) { + if (num > 1.0) { + return 1.0; + } + if (num < -1.0) { + return -1.0; + } + return num; + } } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/GearTake.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/GearTake_2.java similarity index 51% rename from 3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/GearTake.java rename to 3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/GearTake_2.java index 218333d..fbf83e7 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/GearTake.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/GearTake_2.java @@ -1,15 +1,17 @@ package org.usfirst.frc.team3494.robot.subsystems; import org.usfirst.frc.team3494.robot.RobotMap; +import org.usfirst.frc.team3494.robot.sensors.LineBreak; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.command.Subsystem; /** - * + * Gear holder subsystem. Contains all methods for controlling the robot's gear + * holder. */ -public class GearTake extends Subsystem { +public class GearTake_2 extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. @@ -22,13 +24,24 @@ public class GearTake extends Subsystem { /** * The solenoid that holds the gear or drops it. */ - private DoubleSolenoid openandclose; + private DoubleSolenoid openandclose_forward; + private DoubleSolenoid openandclose_backward; - public GearTake() { + public LineBreak lb; + + public GearTake_2() { super(); this.rampenoid = new DoubleSolenoid(RobotMap.GEAR_RAMP_CHONE, RobotMap.GEAR_RAMP_CHTWO); - this.openandclose = new DoubleSolenoid(RobotMap.GEAR_GRASP_CHONE, RobotMap.GEAR_GRASP_CHTWO); - this.openandclose.set(Value.kReverse); + this.openandclose_forward = new DoubleSolenoid(RobotMap.GEAR_GRASP_CHONE, RobotMap.GEAR_GRASP_CHTWO); + this.openandclose_backward = new DoubleSolenoid(RobotMap.GEAR_GRASP_S2_FORWARD, + RobotMap.GEAR_GRASP_S2_BACKWARD); + this.openandclose_forward.set(Value.kForward); + this.openandclose_forward.set(Value.kReverse); + + this.openandclose_backward.set(Value.kReverse); + this.openandclose_backward.set(Value.kForward); + + this.lb = new LineBreak(0); } @Override @@ -54,18 +67,29 @@ public void setRamp(Value value) { * The position to set the holder to. */ public void setGrasp(Value value) { - this.openandclose.set(value); + if (value.equals(Value.kForward)) { + this.openandclose_forward.set(Value.kForward); + this.openandclose_backward.set(Value.kForward); + } else if (value.equals(Value.kReverse)) { + this.openandclose_backward.set(Value.kReverse); + this.openandclose_forward.set(Value.kReverse); + } else { + this.openandclose_forward.set(Value.kForward); + this.openandclose_forward.set(Value.kReverse); + this.openandclose_backward.set(Value.kReverse); + this.openandclose_backward.set(Value.kForward); + } } /** - * Releases the gear with a call to {@link GearTake#setGrasp}. + * Releases the gear with a call to {@link GearTake_2#setGrasp}. */ public void releaseGear() { this.setGrasp(Value.kForward); } /** - * Closes the gear holder with a call to {@link GearTake#setGrasp}. + * Closes the gear holder with a call to {@link GearTake_2#setGrasp}. */ public void closeHolder() { this.setGrasp(Value.kReverse); @@ -73,7 +97,8 @@ public void closeHolder() { /** * Gets the state of the intake ramp solenoid. Equivalent to - * {@code this.rampenoid.get()}, but {@link GearTake#rampenoid} is private. + * {@code this.rampenoid.get()}, but {@link GearTake_2#rampenoid} is + * private. * * @return The value of {@code this.rampenoid.get()}. */ @@ -83,12 +108,12 @@ public Value getRampState() { /** * Gets the state of the gear holder. Equivalent to - * {@code this.openandclose.get()}, but {@link GearTake#openandclose} is - * private. + * {@code this.openandclose.get()}, but + * {@link GearTake_2#openandclose_forward} is private. * * @return The value of {@code this.openandclose.get()}. */ public Value getGearState() { - return this.openandclose.get(); + return this.openandclose_forward.get(); } } diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/Intake.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/Intake.java deleted file mode 100644 index f372443..0000000 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/Intake.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.usfirst.frc.team3494.robot.subsystems; - -import org.usfirst.frc.team3494.robot.RobotMap; -import org.usfirst.frc.team3494.robot.commands.intake.RunIntake; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.Talon; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * Intake subsystem. Contains methods for controlling the ball intake. - * - * @since 0.0.0 - */ -public class Intake extends Subsystem implements IMotorizedSubsystem { - /** - * Talon that controls the intake. Thankfully there's only one. - * - * @see RobotMap - */ - private Talon inMotor; - private Talon upMotor; - // pistons push - private DoubleSolenoid piston; - public boolean isDeployed; - - public Intake() { - super("Intake"); - this.inMotor = new Talon(RobotMap.INTAKE_MOTOR); - this.upMotor = new Talon(RobotMap.UP_MOTOR); - this.upMotor.setInverted(true); - this.piston = new DoubleSolenoid(RobotMap.INTAKE_PISTON_CHONE, RobotMap.INTAKE_PISTON_CHTWO); - this.isDeployed = false; - } - // Put methods for controlling this subsystem - // here. Call these from Commands. - - @Override - public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); - this.setDefaultCommand(new RunIntake()); - } - - public void runIntake(double speed) { - this.inMotor.set(speed); - this.upMotor.set(speed); - } - - @Override - public void stopAll() { - this.inMotor.set(0); - this.upMotor.set(0); - } - - @Override - public void setAll(double speed) { - this.inMotor.set(speed); - this.upMotor.set(speed); - } - - public void pushForward() { - this.piston.set(DoubleSolenoid.Value.kForward); - this.isDeployed = true; - } - - public void retract() { - this.piston.set(DoubleSolenoid.Value.kReverse); - this.isDeployed = false; - } - - public void setPiston(DoubleSolenoid.Value position) { - this.piston.set(position); - if (position.equals(DoubleSolenoid.Value.kForward)) { - this.isDeployed = true; - } else { - this.isDeployed = true; - } - SmartDashboard.putBoolean("Intake Deployed", this.isDeployed); - } -} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/MonoGearTake.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/MonoGearTake.java new file mode 100644 index 0000000..53e8805 --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/subsystems/MonoGearTake.java @@ -0,0 +1,32 @@ +package org.usfirst.frc.team3494.robot.subsystems; + +import org.usfirst.frc.team3494.robot.RobotMap; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.command.Subsystem; + +/** + * Single piston gear holder. + */ +public class MonoGearTake extends Subsystem { + + // Put methods for controlling this subsystem + // here. Call these from Commands. + private DoubleSolenoid piston; + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + this.piston = new DoubleSolenoid(RobotMap.MONO_GEAR_FORWARD, RobotMap.MONO_GEAR_REVERSE); + } + + public void setPos(Value v) { + this.piston.set(v); + } + + public Value getPos() { + return this.piston.get(); + } +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/GearPipeline.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/GearPipeline.java new file mode 100644 index 0000000..36e8f3d --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/GearPipeline.java @@ -0,0 +1,215 @@ +package org.usfirst.frc.team3494.robot.vision; + +import java.util.ArrayList; +import java.util.List; + +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfInt; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +import edu.wpi.first.wpilibj.vision.VisionPipeline; + +/** + * GearPipeline class. + * + *

+ * An OpenCV pipeline generated by GRIP. + * + * @author GRIP + */ +public class GearPipeline implements VisionPipeline { + + // Outputs + private Mat rgbThresholdOutput = new Mat(); + private ArrayList findContoursOutput = new ArrayList(); + private ArrayList filterContoursOutput = new ArrayList(); + + static { + System.loadLibrary(Core.NATIVE_LIBRARY_NAME); + } + + /** + * This is the primary method that runs the entire pipeline and updates the + * outputs. + */ + @Override + public void process(Mat source0) { + // Step RGB_Threshold0: + Mat rgbThresholdInput = source0; + double[] rgbThresholdRed = { 0.0, 255.0 }; + double[] rgbThresholdGreen = { 130.71043165467623, 255.0 }; + double[] rgbThresholdBlue = { 0.0, 255.0 }; + rgbThreshold(rgbThresholdInput, rgbThresholdRed, rgbThresholdGreen, rgbThresholdBlue, rgbThresholdOutput); + + // Step Find_Contours0: + Mat findContoursInput = rgbThresholdOutput; + boolean findContoursExternalOnly = false; + findContours(findContoursInput, findContoursExternalOnly, findContoursOutput); + + // Step Filter_Contours0: + ArrayList filterContoursContours = findContoursOutput; + double filterContoursMinArea = 500.0; + double filterContoursMinPerimeter = 100.0; + double filterContoursMinWidth = 0.0; + double filterContoursMaxWidth = 1000.0; + double filterContoursMinHeight = 0.0; + double filterContoursMaxHeight = 1000.0; + double[] filterContoursSolidity = { 0, 100 }; + double filterContoursMaxVertices = 1000000.0; + double filterContoursMinVertices = 0.0; + double filterContoursMinRatio = 0.0; + double filterContoursMaxRatio = 1000.0; + filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, + filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, + filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, + filterContoursMaxRatio, filterContoursOutput); + + } + + /** + * This method is a generated getter for the output of a RGB_Threshold. + * + * @return Mat output from RGB_Threshold. + */ + public Mat rgbThresholdOutput() { + return rgbThresholdOutput; + } + + /** + * This method is a generated getter for the output of a Find_Contours. + * + * @return ArrayList<MatOfPoint> output from Find_Contours. + */ + public ArrayList findContoursOutput() { + return findContoursOutput; + } + + /** + * This method is a generated getter for the output of a Filter_Contours. + * + * @return ArrayList<MatOfPoint> output from Filter_Contours. + */ + public ArrayList filterContoursOutput() { + return filterContoursOutput; + } + + /** + * Segment an image based on color ranges. + * + * @param input + * The image on which to perform the RGB threshold. + * @param red + * The min and max red. + * @param green + * The min and max green. + * @param blue + * The min and max blue. + * @param out + * The image in which to store the output. + */ + private void rgbThreshold(Mat input, double[] red, double[] green, double[] blue, Mat out) { + Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2RGB); + Core.inRange(out, new Scalar(red[0], green[0], blue[0]), new Scalar(red[1], green[1], blue[1]), out); + } + + /** + * Sets the values of pixels in a binary image to their distance to the + * nearest black pixel. + * + * @param input + * The image on which to perform the Distance Transform. + * @param type + * The Transform. + * @param maskSize + * the size of the mask. + * @param output + * The image in which to store the output. + */ + private void findContours(Mat input, boolean externalOnly, List contours) { + Mat hierarchy = new Mat(); + contours.clear(); + int mode; + if (externalOnly) { + mode = Imgproc.RETR_EXTERNAL; + } else { + mode = Imgproc.RETR_LIST; + } + int method = Imgproc.CHAIN_APPROX_SIMPLE; + Imgproc.findContours(input, contours, hierarchy, mode, method); + } + + /** + * Filters out contours that do not meet certain criteria. + * + * @param inputContours + * is the input list of contours + * @param output + * is the the output list of contours + * @param minArea + * is the minimum area of a contour that will be kept + * @param minPerimeter + * is the minimum perimeter of a contour that will be kept + * @param minWidth + * minimum width of a contour + * @param maxWidth + * maximum width + * @param minHeight + * minimum height + * @param maxHeight + * maximimum height + * @param solidity + * the minimum and maximum solidity of a contour + * @param minVertexCount + * minimum vertex Count of the contours + * @param maxVertexCount + * maximum vertex Count + * @param minRatio + * minimum ratio of width to height + * @param maxRatio + * maximum ratio of width to height + */ + private void filterContours(List inputContours, double minArea, double minPerimeter, double minWidth, + double maxWidth, double minHeight, double maxHeight, double[] solidity, double maxVertexCount, + double minVertexCount, double minRatio, double maxRatio, List output) { + final MatOfInt hull = new MatOfInt(); + output.clear(); + // operation + for (int i = 0; i < inputContours.size(); i++) { + final MatOfPoint contour = inputContours.get(i); + final Rect bb = Imgproc.boundingRect(contour); + if (bb.width < minWidth || bb.width > maxWidth) + continue; + if (bb.height < minHeight || bb.height > maxHeight) + continue; + final double area = Imgproc.contourArea(contour); + if (area < minArea) + continue; + if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) + continue; + Imgproc.convexHull(contour, hull); + MatOfPoint mopHull = new MatOfPoint(); + mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2); + for (int j = 0; j < hull.size().height; j++) { + int index = (int) hull.get(j, 0)[0]; + double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1] }; + mopHull.put(j, 0, point); + } + final double solid = 100 * area / Imgproc.contourArea(mopHull); + if (solid < solidity[0] || solid > solidity[1]) + continue; + if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) + continue; + final double ratio = bb.width / (double) bb.height; + if (ratio < minRatio || ratio > maxRatio) + continue; + output.add(contour); + } + } + +} diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/GripPipeline.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/GripPipeline.java index 9c23c2f..87e732e 100644 --- a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/GripPipeline.java +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/GripPipeline.java @@ -19,8 +19,7 @@ * GripPipeline class. * *

- * An OpenCV pipeline generated by GRIP. Docs are poor at best. - *

+ * An OpenCV pipeline generated by GRIP. * * @author GRIP */ @@ -86,7 +85,7 @@ public Mat hslThresholdOutput() { /** * This method is a generated getter for the output of a Find_Contours. * - * @return ArrayList<MatOfPoint> output from Find_Contours. + * @return {@literal ArrayList} output from Find_Contours. */ public ArrayList findContoursOutput() { return findContoursOutput; @@ -95,7 +94,7 @@ public ArrayList findContoursOutput() { /** * This method is a generated getter for the output of a Filter_Contours. * - * @return ArrayList<MatOfPoint> output from Filter_Contours. + * @return {@literal ArrayList} output from Filter_Contours. */ public ArrayList filterContoursOutput() { return filterContoursOutput; diff --git a/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/VisionUtils.java b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/VisionUtils.java new file mode 100644 index 0000000..d48b41f --- /dev/null +++ b/3494_2017_repo/src/org/usfirst/frc/team3494/robot/vision/VisionUtils.java @@ -0,0 +1,16 @@ +package org.usfirst.frc.team3494.robot.vision; + +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.imgproc.Imgproc; + +public class VisionUtils { + public static Mat rotate(Mat source, double angle) { + // Step rotate_image + Point src_center = new Point(source.cols() / 2.0F, source.rows() / 2.0F); + Mat rot_mat = Imgproc.getRotationMatrix2D(src_center, 180, 1.0); + Mat dst = new Mat(); + Imgproc.warpAffine(source, dst, rot_mat, source.size()); + return dst; + } +} diff --git a/docs/allclasses-frame.html b/docs/allclasses-frame.html index 73b5f7a..27f3d7b 100644 --- a/docs/allclasses-frame.html +++ b/docs/allclasses-frame.html @@ -2,9 +2,9 @@ - + All Classes - + @@ -19,30 +19,39 @@

All Classes

  • ClimberToggle
  • ConstructedAuto
  • Conveyer
  • +
  • DelayCommand
  • DistanceDrive
  • Drive
  • DriveDirections
  • Drivetrain
  • -
  • GearTake
  • +
  • GearPipeline
  • +
  • GearTake_2
  • GripPipeline
  • HoldDriveTrain
  • -
  • HoldInState
  • +
  • HoldInState_Forward
  • IMotorizedSubsystem
  • -
  • Intake
  • Kompressor
  • +
  • LineBreak
  • +
  • MonoGearTake
  • +
  • NullAuto
  • OI
  • +
  • PIDAngleDrive
  • +
  • PIDFullDrive
  • Robot
  • RobotMap
  • -
  • RunIntake
  • -
  • SetGearPosition
  • +
  • SetGearGrasp
  • +
  • SetReverse
  • Shoot
  • +
  • SpeedTest
  • +
  • StageTest
  • StopClimber
  • -
  • SwitchPosition
  • +
  • ToggleGearPosition
  • ToggleGearRamp
  • Turret
  • TurretCon
  • TurretEncoders
  • UnitTypes
  • +
  • VisionUtils
  • XYDrive
  • diff --git a/docs/allclasses-noframe.html b/docs/allclasses-noframe.html index d05e3bd..e7a084e 100644 --- a/docs/allclasses-noframe.html +++ b/docs/allclasses-noframe.html @@ -2,9 +2,9 @@ - + All Classes - + @@ -19,30 +19,39 @@

    All Classes

  • ClimberToggle
  • ConstructedAuto
  • Conveyer
  • +
  • DelayCommand
  • DistanceDrive
  • Drive
  • DriveDirections
  • Drivetrain
  • -
  • GearTake
  • +
  • GearPipeline
  • +
  • GearTake_2
  • GripPipeline
  • HoldDriveTrain
  • -
  • HoldInState
  • +
  • HoldInState_Forward
  • IMotorizedSubsystem
  • -
  • Intake
  • Kompressor
  • +
  • LineBreak
  • +
  • MonoGearTake
  • +
  • NullAuto
  • OI
  • +
  • PIDAngleDrive
  • +
  • PIDFullDrive
  • Robot
  • RobotMap
  • -
  • RunIntake
  • -
  • SetGearPosition
  • +
  • SetGearGrasp
  • +
  • SetReverse
  • Shoot
  • +
  • SpeedTest
  • +
  • StageTest
  • StopClimber
  • -
  • SwitchPosition
  • +
  • ToggleGearPosition
  • ToggleGearRamp
  • Turret
  • TurretCon
  • TurretEncoders
  • UnitTypes
  • +
  • VisionUtils
  • XYDrive
  • diff --git a/docs/constant-values.html b/docs/constant-values.html index 4a703be..89492a4 100644 --- a/docs/constant-values.html +++ b/docs/constant-values.html @@ -2,9 +2,9 @@ - + Constant Field Values - + @@ -83,6 +83,32 @@

    org.usfirst.*

    +
      +
    • + + + + + + + + + + + + + + +
      org.usfirst.frc.team3494.robot.subsystems.Drivetrain 
      Modifier and TypeConstant FieldValue
      + +public static final doubleRAMP48.0
      +
    • +
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    A

    +
    absolutelyAverage - Variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    +
    activeGearRight() - Static method in class org.usfirst.frc.team3494.robot.AutoGenerator
    +
     
    +
    activeLeftGear() - Static method in class org.usfirst.frc.team3494.robot.AutoGenerator
    +
     
    adjustedTankDrive(double, double) - Method in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
    Drives the drivetrain, with the value passed in for left inverted.
    @@ -82,8 +88,16 @@

    A

    The gyro board mounted to the RoboRIO.
    +
    ai1 - Variable in class org.usfirst.frc.team3494.robot.sensors.LineBreak
    +
    +
    Actual analog channel.
    +
    angle - Variable in class org.usfirst.frc.team3494.robot.commands.auto.AngleTurn
     
    +
    angle - Variable in class org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive
    +
     
    +
    angle - Variable in class org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive
    +
     
    angle - Variable in class org.usfirst.frc.team3494.robot.commands.auto.XYDrive
     
    AngleTurn - Class in org.usfirst.frc.team3494.robot.commands.auto
    @@ -94,6 +108,10 @@

    A

    Constructor.
    +
    ArcadeDrive(double, double, boolean) - Method in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
    +
    +
    Arcade drive implements single stick driving.
    +
    AutoGenerator - Class in org.usfirst.frc.team3494.robot
    Class containing methods that return valid lists to pass to @@ -116,8 +134,10 @@

    A

    Test method.
    +
    averages - Variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    L

    +
    lb - Variable in class org.usfirst.frc.team3494.robot.subsystems.GearTake_2
    +
     
    +
    leftSide - Variable in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
    +
     
    leftTalonOne - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    leftTalonThree - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    leftTalonTwo - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    +
    limit(double) - Static method in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
    +
     
    +
    LineBreak - Class in org.usfirst.frc.team3494.robot.sensors
    +
     
    +
    LineBreak(int) - Constructor for class org.usfirst.frc.team3494.robot.sensors.LineBreak
    +
     
    +
    LineBreak() - Constructor for class org.usfirst.frc.team3494.robot.sensors.LineBreak
    +
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    M

    +
    MONO_GEAR_FORWARD - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    +
     
    +
    MONO_GEAR_REVERSE - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    +
     
    +
    MonoGearTake - Class in org.usfirst.frc.team3494.robot.subsystems
    +
    +
    Single piston gear holder.
    +
    +
    MonoGearTake() - Constructor for class org.usfirst.frc.team3494.robot.subsystems.MonoGearTake
    +
     
    motor - Variable in class org.usfirst.frc.team3494.robot.subsystems.Climber
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    diff --git a/docs/index-files/index-12.html b/docs/index-files/index-12.html index 122bf7b..01c4864 100644 --- a/docs/index-files/index-12.html +++ b/docs/index-files/index-12.html @@ -2,9 +2,9 @@ - -O-Index - + +N-Index + @@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@ @@ -69,11 +69,17 @@
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    C

    +
    camera_0 - Static variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    +
    camera_1 - Static variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    +
    centerX - Variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    chooser - Static variable in class org.usfirst.frc.team3494.robot.Robot
     
    Climb - Class in org.usfirst.frc.team3494.robot.commands.climb
    @@ -110,9 +116,9 @@

    C

    ClimberToggle() - Constructor for class org.usfirst.frc.team3494.robot.commands.climb.ClimberToggle
     
    -
    closeHolder() - Method in class org.usfirst.frc.team3494.robot.subsystems.GearTake
    +
    closeHolder() - Method in class org.usfirst.frc.team3494.robot.subsystems.GearTake_2
    - +
    compress - Variable in class org.usfirst.frc.team3494.robot.subsystems.Kompressor
     
    @@ -136,12 +142,14 @@

    C

     
    CONVEYER_M - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    +
    counter - Variable in class org.usfirst.frc.team3494.robot.commands.auto.tests.StageTest
    +
     
    crossBaseLine() - Static method in class org.usfirst.frc.team3494.robot.AutoGenerator
    Drives to the baseline and earns us five points (hopefully.)
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    diff --git a/docs/index-files/index-20.html b/docs/index-files/index-20.html index da22431..6c5932e 100644 --- a/docs/index-files/index-20.html +++ b/docs/index-files/index-20.html @@ -2,9 +2,9 @@ - -X-Index - + +W-Index + @@ -12,7 +12,7 @@ + + + + + +
    + + + + + + + +
    + + +
    A C D E F G H I K L M N O P R S T U V W X  + + +

    X

    +
    +
    xbox - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_2 - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_a - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_a_2 - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_b - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_b_2 - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_lt - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    XBOX_ONE - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    +
     
    +
    xbox_rt - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_rt_2 - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_select_2 - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_start_2 - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    XBOX_TWO - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    +
     
    +
    xbox_x - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_x_2 - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_y - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    xbox_y_2 - Variable in class org.usfirst.frc.team3494.robot.OI
    +
     
    +
    XYDrive - Class in org.usfirst.frc.team3494.robot.commands.auto
    +
    +
    Drives to a point relative to the robot.
    +
    +
    XYDrive(double, double) - Constructor for class org.usfirst.frc.team3494.robot.commands.auto.XYDrive
    +
    +
    Constructor.
    +
    +
    +A C D E F G H I K L M N O P R S T U V W X 
    + +
    + + + + + + + +
    + + + + diff --git a/docs/index-files/index-3.html b/docs/index-files/index-3.html index c399ed3..bf4a7d9 100644 --- a/docs/index-files/index-3.html +++ b/docs/index-files/index-3.html @@ -2,9 +2,9 @@ - + D-Index - + @@ -69,11 +69,17 @@
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    D

    +
    DelayCommand - Class in org.usfirst.frc.team3494.robot.commands
    +
    +
    Command to cause delays in autonomous.
    +
    +
    DelayCommand(double) - Constructor for class org.usfirst.frc.team3494.robot.commands.DelayCommand
    +
     
    direction - Variable in class org.usfirst.frc.team3494.robot.commands.climb.Climb
    The direction to climb in.
    @@ -88,6 +94,8 @@

    D

     
    dist - Variable in class org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive
     
    +
    distance - Variable in class org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive
    +
     
    DistanceDrive - Class in org.usfirst.frc.team3494.robot.commands.auto
    Drives a given distance.
    @@ -147,7 +155,7 @@

    D

    driveTrainMode - Variable in class org.usfirst.frc.team3494.robot.subsystems.Climber
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    E

    -
    encLeft - Variable in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
    -
     
    ENCODER_LEFT_A - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    ENCODER_LEFT_B - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    @@ -84,31 +82,41 @@

    E

     
    ENCODER_RIGHT_B - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    -
    encRight - Variable in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
    -
     
    end() - Method in class org.usfirst.frc.team3494.robot.commands.auto.AngleTurn
     
    end() - Method in class org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive
     
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.auto.NullAuto
    +
     
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive
    +
     
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive
    +
     
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.auto.SetGearGrasp
    +
     
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.SpeedTest
    +
     
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.StageTest
    +
     
    end() - Method in class org.usfirst.frc.team3494.robot.commands.climb.Climb
     
    end() - Method in class org.usfirst.frc.team3494.robot.commands.climb.ClimberToggle
     
    end() - Method in class org.usfirst.frc.team3494.robot.commands.climb.StopClimber
     
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.DelayCommand
    +
     
    end() - Method in class org.usfirst.frc.team3494.robot.commands.drive.Drive
     
    end() - Method in class org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain
     
    -
    end() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward
     
    -
    end() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetGearPosition
    -
     
    -
    end() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetReverse
     
    -
    end() - Method in class org.usfirst.frc.team3494.robot.commands.intake.RunIntake
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearPosition
     
    -
    end() - Method in class org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition
    +
    end() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
     
    end() - Method in class org.usfirst.frc.team3494.robot.commands.turret.Shoot
     
    @@ -120,32 +128,44 @@

    E

     
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive
     
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.auto.NullAuto
    +
     
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive
    +
     
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive
    +
     
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.auto.SetGearGrasp
    +
     
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.SpeedTest
    +
     
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.StageTest
    +
     
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.climb.Climb
     
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.climb.ClimberToggle
     
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.climb.StopClimber
     
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.DelayCommand
    +
     
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.drive.Drive
     
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain
     
    -
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward
     
    -
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetGearPosition
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetReverse
     
    -
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearPosition
     
    -
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.intake.RunIntake
    -
     
    -
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition
    +
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
     
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.turret.Shoot
     
    execute() - Method in class org.usfirst.frc.team3494.robot.commands.turret.TurretCon
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    F

    +
    filterContours(List<MatOfPoint>, double, double, double, double, double, double, double[], double, double, double, double, List<MatOfPoint>) - Method in class org.usfirst.frc.team3494.robot.vision.GearPipeline
    +
    +
    Filters out contours that do not meet certain criteria.
    +
    filterContours(List<MatOfPoint>, double, double, double, double, double, double, double[], double, double, double, double, List<MatOfPoint>) - Method in class org.usfirst.frc.team3494.robot.vision.GripPipeline
    Filters out contours that do not meet certain criteria.
    +
    filterContoursOutput - Variable in class org.usfirst.frc.team3494.robot.vision.GearPipeline
    +
     
    +
    filterContoursOutput() - Method in class org.usfirst.frc.team3494.robot.vision.GearPipeline
    +
    +
    This method is a generated getter for the output of a Filter_Contours.
    +
    filterContoursOutput - Variable in class org.usfirst.frc.team3494.robot.vision.GripPipeline
     
    filterContoursOutput() - Method in class org.usfirst.frc.team3494.robot.vision.GripPipeline
    This method is a generated getter for the output of a Filter_Contours.
    +
    filteredContours - Variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    +
    findContours(Mat, boolean, List<MatOfPoint>) - Method in class org.usfirst.frc.team3494.robot.vision.GearPipeline
    +
    +
    Sets the values of pixels in a binary image to their distance to the + nearest black pixel.
    +
    findContours(Mat, boolean, List<MatOfPoint>) - Method in class org.usfirst.frc.team3494.robot.vision.GripPipeline
    Sets the values of pixels in a binary image to their distance to the nearest black pixel.
    +
    findContoursOutput - Variable in class org.usfirst.frc.team3494.robot.vision.GearPipeline
    +
     
    +
    findContoursOutput() - Method in class org.usfirst.frc.team3494.robot.vision.GearPipeline
    +
    +
    This method is a generated getter for the output of a Find_Contours.
    +
    findContoursOutput - Variable in class org.usfirst.frc.team3494.robot.vision.GripPipeline
     
    findContoursOutput() - Method in class org.usfirst.frc.team3494.robot.vision.GripPipeline
    @@ -96,7 +119,7 @@

    F

    This method is a generated getter for the output of a Find_Contours.
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    G

    @@ -78,23 +78,41 @@

    G

     
    GEAR_GRASP_CHTWO - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    +
    GEAR_GRASP_S2_BACKWARD - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    +
     
    +
    GEAR_GRASP_S2_FORWARD - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    +
     
    GEAR_RAMP_CHONE - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    GEAR_RAMP_CHTWO - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    +
    GearPipeline - Class in org.usfirst.frc.team3494.robot.vision
    +
    +
    GearPipeline class.
    +
    +
    GearPipeline() - Constructor for class org.usfirst.frc.team3494.robot.vision.GearPipeline
    +
     
    gearPlaceAttempt() - Static method in class org.usfirst.frc.team3494.robot.AutoGenerator
     
    -
    gearTake - Static variable in class org.usfirst.frc.team3494.robot.Robot
    +
    gearPlaceAttemptLeft() - Static method in class org.usfirst.frc.team3494.robot.AutoGenerator
     
    -
    GearTake - Class in org.usfirst.frc.team3494.robot.subsystems
    +
    gearTake - Static variable in class org.usfirst.frc.team3494.robot.Robot
     
    -
    GearTake() - Constructor for class org.usfirst.frc.team3494.robot.subsystems.GearTake
    +
    GearTake_2 - Class in org.usfirst.frc.team3494.robot.subsystems
    +
    +
    Gear holder subsystem.
    +
    +
    GearTake_2() - Constructor for class org.usfirst.frc.team3494.robot.subsystems.GearTake_2
     
    getAvgDistance(UnitTypes) - Method in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
     
    +
    getBroken() - Method in class org.usfirst.frc.team3494.robot.sensors.LineBreak
    +
    +
    Returns true if the trigger is broken.
    +
    getDistance(TurretEncoders) - Method in class org.usfirst.frc.team3494.robot.subsystems.Turret
     
    -
    getGearState() - Method in class org.usfirst.frc.team3494.robot.subsystems.GearTake
    +
    getGearState() - Method in class org.usfirst.frc.team3494.robot.subsystems.GearTake_2
    Gets the state of the gear holder.
    @@ -109,7 +127,9 @@

    G

    getMotorCurrent() - Method in class org.usfirst.frc.team3494.robot.subsystems.Climber
     
    -
    getRampState() - Method in class org.usfirst.frc.team3494.robot.subsystems.GearTake
    +
    getPos() - Method in class org.usfirst.frc.team3494.robot.subsystems.MonoGearTake
    +
     
    +
    getRampState() - Method in class org.usfirst.frc.team3494.robot.subsystems.GearTake_2
    Gets the state of the intake ramp solenoid.
    @@ -126,7 +146,7 @@

    G

    GripPipeline() - Constructor for class org.usfirst.frc.team3494.robot.vision.GripPipeline
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    H

    @@ -80,11 +80,11 @@

    H

    HoldDriveTrain() - Constructor for class org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain
     
    -
    HoldInState - Class in org.usfirst.frc.team3494.robot.commands.gears
    +
    HoldInState_Forward - Class in org.usfirst.frc.team3494.robot.commands.gears
    Holds the gear holder in the given position until canceled.
    -
    HoldInState() - Constructor for class org.usfirst.frc.team3494.robot.commands.gears.HoldInState
    +
    HoldInState_Forward() - Constructor for class org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward
     
    hslThreshold(Mat, double[], double[], double[], Mat) - Method in class org.usfirst.frc.team3494.robot.vision.GripPipeline
    @@ -99,7 +99,7 @@

    H

    hypot - Variable in class org.usfirst.frc.team3494.robot.commands.auto.XYDrive
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    I

    +
    IMG_HEIGHT - Static variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    +
    IMG_WIDTH - Static variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    +
    imgLock - Variable in class org.usfirst.frc.team3494.robot.Robot
    +
     
    IMotorizedSubsystem - Interface in org.usfirst.frc.team3494.robot.subsystems
    Interface to apply to subsystems with motors.
    @@ -84,83 +90,93 @@

    I

     
    initDefaultCommand() - Method in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
     
    -
    initDefaultCommand() - Method in class org.usfirst.frc.team3494.robot.subsystems.GearTake
    -
     
    -
    initDefaultCommand() - Method in class org.usfirst.frc.team3494.robot.subsystems.Intake
    +
    initDefaultCommand() - Method in class org.usfirst.frc.team3494.robot.subsystems.GearTake_2
     
    initDefaultCommand() - Method in class org.usfirst.frc.team3494.robot.subsystems.Kompressor
     
    +
    initDefaultCommand() - Method in class org.usfirst.frc.team3494.robot.subsystems.MonoGearTake
    +
     
    initDefaultCommand() - Method in class org.usfirst.frc.team3494.robot.subsystems.Turret
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.auto.AngleTurn
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive
     
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.auto.NullAuto
    +
     
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive
    +
     
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive
    +
     
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.auto.SetGearGrasp
    +
     
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.SpeedTest
    +
     
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.StageTest
    +
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.climb.Climb
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.climb.ClimberToggle
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.climb.StopClimber
     
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.DelayCommand
    +
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.drive.Drive
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain
     
    -
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState
    -
     
    -
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetGearPosition
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward
     
    -
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetReverse
     
    -
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.intake.RunIntake
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearPosition
     
    -
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition
    +
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.turret.Shoot
     
    initialize() - Method in class org.usfirst.frc.team3494.robot.commands.turret.TurretCon
     
    -
    inMotor - Variable in class org.usfirst.frc.team3494.robot.subsystems.Intake
    -
    -
    Talon that controls the intake.
    -
    -
    intake - Static variable in class org.usfirst.frc.team3494.robot.Robot
    -
     
    -
    Intake - Class in org.usfirst.frc.team3494.robot.subsystems
    -
    -
    Intake subsystem.
    -
    -
    Intake() - Constructor for class org.usfirst.frc.team3494.robot.subsystems.Intake
    +
    initStaging() - Method in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
     
    INTAKE_MOTOR - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
     
    -
    INTAKE_PISTON_CHONE - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    -
     
    -
    INTAKE_PISTON_CHTWO - Static variable in class org.usfirst.frc.team3494.robot.RobotMap
    -
     
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.auto.AngleTurn
     
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive
     
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.auto.NullAuto
    +
     
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive
    +
     
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive
    +
     
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.auto.SetGearGrasp
    +
     
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.SpeedTest
    +
     
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.StageTest
    +
     
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.climb.Climb
     
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.climb.ClimberToggle
     
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.climb.StopClimber
     
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.DelayCommand
    +
     
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.drive.Drive
     
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain
     
    -
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState
    -
     
    -
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetGearPosition
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward
     
    -
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetReverse
     
    -
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.intake.RunIntake
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearPosition
     
    -
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition
    +
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
     
    interrupted() - Method in class org.usfirst.frc.team3494.robot.commands.turret.Shoot
     
    @@ -168,38 +184,48 @@

    I

     
    inverter - Variable in class org.usfirst.frc.team3494.robot.subsystems.Drivetrain
     
    -
    isDeployed - Variable in class org.usfirst.frc.team3494.robot.subsystems.Intake
    -
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.auto.AngleTurn
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive
     
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.auto.NullAuto
    +
     
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive
    +
     
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive
    +
     
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.auto.SetGearGrasp
    +
     
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.SpeedTest
    +
     
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.auto.tests.StageTest
    +
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.climb.Climb
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.climb.ClimberToggle
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.climb.StopClimber
     
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.DelayCommand
    +
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.drive.Drive
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain
     
    -
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward
     
    -
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetGearPosition
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.gears.SetReverse
     
    -
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearPosition
     
    -
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.intake.RunIntake
    -
     
    -
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition
    +
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.turret.Shoot
     
    isFinished() - Method in class org.usfirst.frc.team3494.robot.commands.turret.TurretCon
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    -
    A C D E F G H I K L M O P R S T U V W X  +
    A C D E F G H I K L M N O P R S T U V W X 

    K

    @@ -83,7 +83,7 @@

    K

    Kompressor() - Constructor for class org.usfirst.frc.team3494.robot.subsystems.Kompressor
     
    -A C D E F G H I K L M O P R S T U V W X 
    +A C D E F G H I K L M N O P R S T U V W X 
    diff --git a/docs/index.html b/docs/index.html index f86140e..9699dbd 100644 --- a/docs/index.html +++ b/docs/index.html @@ -2,7 +2,7 @@ - + Generated Documentation (Untitled) @@ -18,7 +18,7 @@ catch(err) { } //--> -var methods = {"i0":9,"i1":9,"i2":9}; +var methods = {"i0":9,"i1":9,"i2":9,"i3":9,"i4":9,"i5":9,"i6":9}; var tabs = {65535:["t0","All Methods"],1:["t1","Static Methods"],8:["t4","Concrete Methods"]}; var altColor = "altColor"; var rowColor = "rowColor"; @@ -156,20 +156,36 @@

    Method Summary

    static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> +
    activeGearRight()  + + +static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> +activeLeftGear()  + + +static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> autoOne()
    Test method.
    - + static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> crossBaseLine()
    Drives to the baseline and earns us five points (hopefully.)
    - + static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> gearPlaceAttempt()  + +static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> +gearPlaceAttemptLeft()  + + +static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> +placeCenterGear()  + + + + +
      +
    • +

      placeCenterGear

      +
      public static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> placeCenterGear()
      +
    • +
    -
      +
      • gearPlaceAttempt

        public static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> gearPlaceAttempt()
      + + + +
        +
      • +

        gearPlaceAttemptLeft

        +
        public static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> gearPlaceAttemptLeft()
        +
      • +
      + + + +
        +
      • +

        activeLeftGear

        +
        public static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> activeLeftGear()
        +
      • +
      + + + +
        +
      • +

        activeGearRight

        +
        public static java.util.ArrayList<edu.wpi.first.wpilibj.command.Command> activeGearRight()
        +
      • +
    diff --git a/docs/org/usfirst/frc/team3494/robot/DriveDirections.html b/docs/org/usfirst/frc/team3494/robot/DriveDirections.html index 8e55233..c95604d 100644 --- a/docs/org/usfirst/frc/team3494/robot/DriveDirections.html +++ b/docs/org/usfirst/frc/team3494/robot/DriveDirections.html @@ -2,9 +2,9 @@ - + DriveDirections - + diff --git a/docs/org/usfirst/frc/team3494/robot/OI.html b/docs/org/usfirst/frc/team3494/robot/OI.html index 5bad135..d4f0b6e 100644 --- a/docs/org/usfirst/frc/team3494/robot/OI.html +++ b/docs/org/usfirst/frc/team3494/robot/OI.html @@ -2,9 +2,9 @@ - + OI - + @@ -126,6 +126,14 @@

    Field Summary

    Field and Description +edu.wpi.first.wpilibj.Joystick +stick_l  + + +edu.wpi.first.wpilibj.Joystick +stick_r  + + edu.wpi.first.wpilibj.XboxController xbox  @@ -139,36 +147,44 @@

    Field Summary

    edu.wpi.first.wpilibj.buttons.JoystickButton -xbox_b  +xbox_a_2  edu.wpi.first.wpilibj.buttons.JoystickButton -xbox_b_2  +xbox_b  edu.wpi.first.wpilibj.buttons.JoystickButton -xbox_lt  +xbox_b_2  edu.wpi.first.wpilibj.buttons.JoystickButton -xbox_rt  +xbox_lt  edu.wpi.first.wpilibj.buttons.JoystickButton -xbox_rt_2  +xbox_rt  edu.wpi.first.wpilibj.buttons.JoystickButton -xbox_select_2  +xbox_rt_2  edu.wpi.first.wpilibj.buttons.JoystickButton -xbox_start_2  +xbox_select_2  edu.wpi.first.wpilibj.buttons.JoystickButton +xbox_start_2  + + +edu.wpi.first.wpilibj.buttons.JoystickButton xbox_x  + +edu.wpi.first.wpilibj.buttons.JoystickButton +xbox_x_2  + edu.wpi.first.wpilibj.buttons.JoystickButton xbox_y  @@ -242,6 +258,24 @@

    xbox_2

    public final edu.wpi.first.wpilibj.XboxController xbox_2
    + + + +
      +
    • +

      stick_l

      +
      public final edu.wpi.first.wpilibj.Joystick stick_l
      +
    • +
    + + + +
      +
    • +

      stick_r

      +
      public final edu.wpi.first.wpilibj.Joystick stick_r
      +
    • +
    @@ -251,6 +285,15 @@

    xbox_a

    public edu.wpi.first.wpilibj.buttons.JoystickButton xbox_a
    + + + +
      +
    • +

      xbox_a_2

      +
      public edu.wpi.first.wpilibj.buttons.JoystickButton xbox_a_2
      +
    • +
    @@ -323,6 +366,15 @@

    xbox_x

    public edu.wpi.first.wpilibj.buttons.JoystickButton xbox_x
    + + + +
      +
    • +

      xbox_x_2

      +
      public edu.wpi.first.wpilibj.buttons.JoystickButton xbox_x_2
      +
    • +
    diff --git a/docs/org/usfirst/frc/team3494/robot/Robot.html b/docs/org/usfirst/frc/team3494/robot/Robot.html index eeee29d..b2630ba 100644 --- a/docs/org/usfirst/frc/team3494/robot/Robot.html +++ b/docs/org/usfirst/frc/team3494/robot/Robot.html @@ -2,9 +2,9 @@ - + Robot - + @@ -145,34 +145,66 @@

    Field Summary

    Field and Description +double +absolutelyAverage  + + static com.kauailabs.navx.frc.AHRS ahrs
    The gyro board mounted to the RoboRIO.
    - + (package private) edu.wpi.first.wpilibj.command.Command autonomousCommand  + +private java.util.ArrayList<java.lang.Double> +averages  + +static edu.wpi.cscore.UsbCamera +camera_0  + + +static edu.wpi.cscore.UsbCamera +camera_1  + + +double +centerX  + + static edu.wpi.first.wpilibj.smartdashboard.SendableChooser<edu.wpi.first.wpilibj.command.Command> chooser  - + static Climber climber  - + static Drivetrain driveTrain  + +private java.util.ArrayList<org.opencv.core.MatOfPoint> +filteredContours  + -static GearTake +static GearTake_2 gearTake  -static Intake -intake  +private static int +IMG_HEIGHT  + + +private static int +IMG_WIDTH  + + +private java.lang.Object +imgLock  static Kompressor @@ -194,6 +226,10 @@

    Field Summary

    static Turret turret  + +(package private) edu.wpi.first.wpilibj.vision.VisionThread +visionThread  + - - - -
      -
    • -

      intake

      -
      public static Intake intake
      -
    • -
    @@ -375,7 +402,7 @@

    kompressor

    @@ -422,12 +449,110 @@

    chooser

    -
      +
      • prefs

        public static edu.wpi.first.wpilibj.Preferences prefs
      + + + +
        +
      • +

        camera_0

        +
        public static edu.wpi.cscore.UsbCamera camera_0
        +
      • +
      + + + +
        +
      • +

        camera_1

        +
        public static edu.wpi.cscore.UsbCamera camera_1
        +
      • +
      + + + + + + + + + + + +
        +
      • +

        visionThread

        +
        edu.wpi.first.wpilibj.vision.VisionThread visionThread
        +
      • +
      + + + +
        +
      • +

        centerX

        +
        public double centerX
        +
      • +
      + + + +
        +
      • +

        absolutelyAverage

        +
        public double absolutelyAverage
        +
      • +
      + + + +
        +
      • +

        filteredContours

        +
        private java.util.ArrayList<org.opencv.core.MatOfPoint> filteredContours
        +
      • +
      + + + +
        +
      • +

        averages

        +
        private java.util.ArrayList<java.lang.Double> averages
        +
      • +
      + + + +
        +
      • +

        imgLock

        +
        private final java.lang.Object imgLock
        +
      • +
    diff --git a/docs/org/usfirst/frc/team3494/robot/RobotMap.html b/docs/org/usfirst/frc/team3494/robot/RobotMap.html index 0710358..9c0dbe9 100644 --- a/docs/org/usfirst/frc/team3494/robot/RobotMap.html +++ b/docs/org/usfirst/frc/team3494/robot/RobotMap.html @@ -2,9 +2,9 @@ - + RobotMap - + @@ -183,23 +183,23 @@

    Field Summary

    static int -GEAR_RAMP_CHONE  +GEAR_GRASP_S2_BACKWARD  static int -GEAR_RAMP_CHTWO  +GEAR_GRASP_S2_FORWARD  static int -INTAKE_MOTOR  +GEAR_RAMP_CHONE  static int -INTAKE_PISTON_CHONE  +GEAR_RAMP_CHTWO  static int -INTAKE_PISTON_CHTWO  +INTAKE_MOTOR  static int @@ -215,6 +215,14 @@

    Field Summary

    static int +MONO_GEAR_FORWARD  + + +static int +MONO_GEAR_REVERSE  + + +static int rightTalonOne  @@ -226,26 +234,30 @@

    Field Summary

    rightTalonTwo  +static long +TALON_RESET_DELAY  + + static int TURRET_LOWER  - + static int TURRET_RING  - + static int TURRET_UPPER  - + static int UP_MOTOR  - + static int XBOX_ONE  - + static int XBOX_TWO  @@ -596,88 +608,88 @@

    COMPRESSOR

    - + - + - + - + - + - + -
      +
      • CONVEYER_M

        public static final int CONVEYER_M
        @@ -687,6 +699,45 @@

        CONVEYER_M

      + + + +
        +
      • +

        MONO_GEAR_FORWARD

        +
        public static final int MONO_GEAR_FORWARD
        +
        +
        See Also:
        +
        Constant Field Values
        +
        +
      • +
      + + + +
        +
      • +

        MONO_GEAR_REVERSE

        +
        public static final int MONO_GEAR_REVERSE
        +
        +
        See Also:
        +
        Constant Field Values
        +
        +
      • +
      + + + +
        +
      • +

        TALON_RESET_DELAY

        +
        public static final long TALON_RESET_DELAY
        +
        +
        See Also:
        +
        Constant Field Values
        +
        +
      • +
    diff --git a/docs/org/usfirst/frc/team3494/robot/UnitTypes.html b/docs/org/usfirst/frc/team3494/robot/UnitTypes.html index 78253fd..facd93f 100644 --- a/docs/org/usfirst/frc/team3494/robot/UnitTypes.html +++ b/docs/org/usfirst/frc/team3494/robot/UnitTypes.html @@ -2,9 +2,9 @@ - + UnitTypes - + diff --git a/docs/org/usfirst/frc/team3494/robot/class-use/AutoGenerator.html b/docs/org/usfirst/frc/team3494/robot/class-use/AutoGenerator.html index 8a15c0a..965ff65 100644 --- a/docs/org/usfirst/frc/team3494/robot/class-use/AutoGenerator.html +++ b/docs/org/usfirst/frc/team3494/robot/class-use/AutoGenerator.html @@ -2,9 +2,9 @@ - + Uses of Class org.usfirst.frc.team3494.robot.AutoGenerator - + diff --git a/docs/org/usfirst/frc/team3494/robot/class-use/DriveDirections.html b/docs/org/usfirst/frc/team3494/robot/class-use/DriveDirections.html index aa6927f..8ec0add 100644 --- a/docs/org/usfirst/frc/team3494/robot/class-use/DriveDirections.html +++ b/docs/org/usfirst/frc/team3494/robot/class-use/DriveDirections.html @@ -2,9 +2,9 @@ - + Uses of Class org.usfirst.frc.team3494.robot.DriveDirections - + diff --git a/docs/org/usfirst/frc/team3494/robot/class-use/OI.html b/docs/org/usfirst/frc/team3494/robot/class-use/OI.html index 5c629ee..bf6b94b 100644 --- a/docs/org/usfirst/frc/team3494/robot/class-use/OI.html +++ b/docs/org/usfirst/frc/team3494/robot/class-use/OI.html @@ -2,9 +2,9 @@ - + Uses of Class org.usfirst.frc.team3494.robot.OI - + diff --git a/docs/org/usfirst/frc/team3494/robot/class-use/Robot.html b/docs/org/usfirst/frc/team3494/robot/class-use/Robot.html index 82a2bc1..cc4ab50 100644 --- a/docs/org/usfirst/frc/team3494/robot/class-use/Robot.html +++ b/docs/org/usfirst/frc/team3494/robot/class-use/Robot.html @@ -2,9 +2,9 @@ - + Uses of Class org.usfirst.frc.team3494.robot.Robot - + diff --git a/docs/org/usfirst/frc/team3494/robot/class-use/RobotMap.html b/docs/org/usfirst/frc/team3494/robot/class-use/RobotMap.html index 48c514c..076c3c6 100644 --- a/docs/org/usfirst/frc/team3494/robot/class-use/RobotMap.html +++ b/docs/org/usfirst/frc/team3494/robot/class-use/RobotMap.html @@ -2,9 +2,9 @@ - + Uses of Class org.usfirst.frc.team3494.robot.RobotMap - + diff --git a/docs/org/usfirst/frc/team3494/robot/class-use/UnitTypes.html b/docs/org/usfirst/frc/team3494/robot/class-use/UnitTypes.html index 20ef993..aa6f558 100644 --- a/docs/org/usfirst/frc/team3494/robot/class-use/UnitTypes.html +++ b/docs/org/usfirst/frc/team3494/robot/class-use/UnitTypes.html @@ -2,9 +2,9 @@ - + Uses of Class org.usfirst.frc.team3494.robot.UnitTypes - + diff --git a/docs/org/usfirst/frc/team3494/robot/commands/DelayCommand.html b/docs/org/usfirst/frc/team3494/robot/commands/DelayCommand.html new file mode 100644 index 0000000..71731ed --- /dev/null +++ b/docs/org/usfirst/frc/team3494/robot/commands/DelayCommand.html @@ -0,0 +1,396 @@ + + + + + +DelayCommand + + + + + + + + + + + + +
    +
    org.usfirst.frc.team3494.robot.commands
    +

    Class DelayCommand

    +
    +
    +
      +
    • java.lang.Object
    • +
    • +
        +
      • edu.wpi.first.wpilibj.command.Command
      • +
      • +
          +
        • org.usfirst.frc.team3494.robot.commands.DelayCommand
        • +
        +
      • +
      +
    • +
    +
    +
      +
    • +
      +
      All Implemented Interfaces:
      +
      edu.wpi.first.wpilibj.NamedSendable, edu.wpi.first.wpilibj.Sendable
      +
      +
      +
      +
      public class DelayCommand
      +extends edu.wpi.first.wpilibj.command.Command
      +
      Command to cause delays in autonomous. WIP.
      +
    • +
    +
    +
    +
      +
    • + +
        +
      • + + +

        Field Summary

        + + + + + + + + + + +
        Fields 
        Modifier and TypeField and Description
        (package private) doubletime 
        +
      • +
      + +
        +
      • + + +

        Constructor Summary

        + + + + + + + + +
        Constructors 
        Constructor and Description
        DelayCommand(double t) 
        +
      • +
      + +
        +
      • + + +

        Method Summary

        + + + + + + + + + + + + + + + + + + + + + + + + + + +
        All Methods Instance Methods Concrete Methods 
        Modifier and TypeMethod and Description
        protected voidend() 
        protected voidexecute() 
        protected voidinitialize() 
        protected voidinterrupted() 
        protected booleanisFinished() 
        +
          +
        • + + +

          Methods inherited from class edu.wpi.first.wpilibj.command.Command

          +cancel, clearRequirements, doesRequire, getGroup, getName, getSmartDashboardType, getTable, initTable, isCanceled, isInterruptible, isRunning, isTimedOut, requires, setInterruptible, setRunWhenDisabled, setTimeout, start, timeSinceInitialized, toString, willRunWhenDisabled
        • +
        +
          +
        • + + +

          Methods inherited from class java.lang.Object

          +clone, equals, finalize, getClass, hashCode, notify, notifyAll, wait, wait, wait
        • +
        +
      • +
      +
    • +
    +
    +
    +
      +
    • + +
        +
      • + + +

        Field Detail

        + + + +
          +
        • +

          time

          +
          double time
          +
        • +
        +
      • +
      + +
        +
      • + + +

        Constructor Detail

        + + + +
          +
        • +

          DelayCommand

          +
          public DelayCommand(double t)
          +
        • +
        +
      • +
      + +
        +
      • + + +

        Method Detail

        + + + +
          +
        • +

          initialize

          +
          protected void initialize()
          +
          +
          Overrides:
          +
          initialize in class edu.wpi.first.wpilibj.command.Command
          +
          +
        • +
        + + + +
          +
        • +

          execute

          +
          protected void execute()
          +
          +
          Overrides:
          +
          execute in class edu.wpi.first.wpilibj.command.Command
          +
          +
        • +
        + + + +
          +
        • +

          isFinished

          +
          protected boolean isFinished()
          +
          +
          Specified by:
          +
          isFinished in class edu.wpi.first.wpilibj.command.Command
          +
          +
        • +
        + + + +
          +
        • +

          end

          +
          protected void end()
          +
          +
          Overrides:
          +
          end in class edu.wpi.first.wpilibj.command.Command
          +
          +
        • +
        + + + +
          +
        • +

          interrupted

          +
          protected void interrupted()
          +
          +
          Overrides:
          +
          interrupted in class edu.wpi.first.wpilibj.command.Command
          +
          +
        • +
        +
      • +
      +
    • +
    +
    +
    + + + + + + + diff --git a/docs/org/usfirst/frc/team3494/robot/commands/auto/AngleTurn.html b/docs/org/usfirst/frc/team3494/robot/commands/auto/AngleTurn.html index 0446618..cbd6f67 100644 --- a/docs/org/usfirst/frc/team3494/robot/commands/auto/AngleTurn.html +++ b/docs/org/usfirst/frc/team3494/robot/commands/auto/AngleTurn.html @@ -2,9 +2,9 @@ - + AngleTurn - + diff --git a/docs/org/usfirst/frc/team3494/robot/commands/auto/ConstructedAuto.html b/docs/org/usfirst/frc/team3494/robot/commands/auto/ConstructedAuto.html index ee1b7a4..5db21a2 100644 --- a/docs/org/usfirst/frc/team3494/robot/commands/auto/ConstructedAuto.html +++ b/docs/org/usfirst/frc/team3494/robot/commands/auto/ConstructedAuto.html @@ -2,9 +2,9 @@ - + ConstructedAuto - + diff --git a/docs/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.html b/docs/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.html index eccbc7a..9e2b26a 100644 --- a/docs/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.html +++ b/docs/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.html @@ -2,9 +2,9 @@ - + DistanceDrive - + @@ -50,7 +50,7 @@
    Author:
    GRIP
    @@ -488,8 +487,8 @@

    filterContours

    diff --git a/docs/org/usfirst/frc/team3494/robot/vision/package-summary.html b/docs/org/usfirst/frc/team3494/robot/vision/package-summary.html index 6377a0d..53fd8a0 100644 --- a/docs/org/usfirst/frc/team3494/robot/vision/package-summary.html +++ b/docs/org/usfirst/frc/team3494/robot/vision/package-summary.html @@ -2,9 +2,9 @@ - + org.usfirst.frc.team3494.robot.vision - + @@ -83,11 +83,21 @@

    Package org.usfirst.frc.team3494.robot.vi +GearPipeline + +
    GearPipeline class.
    + + + GripPipeline
    GripPipeline class.
    + +VisionUtils +  + diff --git a/docs/org/usfirst/frc/team3494/robot/vision/package-tree.html b/docs/org/usfirst/frc/team3494/robot/vision/package-tree.html index 7d39b09..aac80e7 100644 --- a/docs/org/usfirst/frc/team3494/robot/vision/package-tree.html +++ b/docs/org/usfirst/frc/team3494/robot/vision/package-tree.html @@ -2,9 +2,9 @@ - + org.usfirst.frc.team3494.robot.vision Class Hierarchy - + @@ -81,7 +81,9 @@

    Class Hierarchy

    • java.lang.Object
        +
      • org.usfirst.frc.team3494.robot.vision.GearPipeline (implements edu.wpi.first.wpilibj.vision.VisionPipeline)
      • org.usfirst.frc.team3494.robot.vision.GripPipeline (implements edu.wpi.first.wpilibj.vision.VisionPipeline)
      • +
      • org.usfirst.frc.team3494.robot.vision.VisionUtils
    diff --git a/docs/org/usfirst/frc/team3494/robot/vision/package-use.html b/docs/org/usfirst/frc/team3494/robot/vision/package-use.html index c8318fb..eb32497 100644 --- a/docs/org/usfirst/frc/team3494/robot/vision/package-use.html +++ b/docs/org/usfirst/frc/team3494/robot/vision/package-use.html @@ -2,9 +2,9 @@ - + Uses of Package org.usfirst.frc.team3494.robot.vision - + diff --git a/docs/overview-frame.html b/docs/overview-frame.html index a804a52..6ad559a 100644 --- a/docs/overview-frame.html +++ b/docs/overview-frame.html @@ -2,9 +2,9 @@ - + Overview List - + @@ -16,11 +16,12 @@

    Packages

  • org.usfirst.frc.team3494.robot
  • org.usfirst.frc.team3494.robot.commands
  • org.usfirst.frc.team3494.robot.commands.auto
  • +
  • org.usfirst.frc.team3494.robot.commands.auto.tests
  • org.usfirst.frc.team3494.robot.commands.climb
  • org.usfirst.frc.team3494.robot.commands.drive
  • org.usfirst.frc.team3494.robot.commands.gears
  • -
  • org.usfirst.frc.team3494.robot.commands.intake
  • org.usfirst.frc.team3494.robot.commands.turret
  • +
  • org.usfirst.frc.team3494.robot.sensors
  • org.usfirst.frc.team3494.robot.subsystems
  • org.usfirst.frc.team3494.robot.vision
  • diff --git a/docs/overview-summary.html b/docs/overview-summary.html index 8457b33..ac3044f 100644 --- a/docs/overview-summary.html +++ b/docs/overview-summary.html @@ -2,9 +2,9 @@ - + Overview - + @@ -92,28 +92,28 @@   +org.usfirst.frc.team3494.robot.commands.auto.tests + +
    Package for holding auto codes that exist solely as tests (i.e.
    + + + org.usfirst.frc.team3494.robot.commands.climb   - + org.usfirst.frc.team3494.robot.commands.drive
    Package for storing all drive-related commands (aside from autonomous.)
    - + org.usfirst.frc.team3494.robot.commands.gears
    Package for all things gear holder that aren't auto-specific (put those in the auto package.)
    - -org.usfirst.frc.team3494.robot.commands.intake - -
    Package for intake-related commands.
    - - org.usfirst.frc.team3494.robot.commands.turret @@ -121,10 +121,16 @@ +org.usfirst.frc.team3494.robot.sensors + +
    Package for sensor type classes.
    + + + org.usfirst.frc.team3494.robot.subsystems   - + org.usfirst.frc.team3494.robot.vision   diff --git a/docs/overview-tree.html b/docs/overview-tree.html index c7d77a0..d98b5fc 100644 --- a/docs/overview-tree.html +++ b/docs/overview-tree.html @@ -2,9 +2,9 @@ - + Class Hierarchy - + @@ -76,11 +76,12 @@

    Hierarchy For All Packages

  • org.usfirst.frc.team3494.robot,
  • org.usfirst.frc.team3494.robot.commands,
  • org.usfirst.frc.team3494.robot.commands.auto,
  • +
  • org.usfirst.frc.team3494.robot.commands.auto.tests,
  • org.usfirst.frc.team3494.robot.commands.climb,
  • org.usfirst.frc.team3494.robot.commands.drive,
  • org.usfirst.frc.team3494.robot.commands.gears,
  • -
  • org.usfirst.frc.team3494.robot.commands.intake,
  • org.usfirst.frc.team3494.robot.commands.turret,
  • +
  • org.usfirst.frc.team3494.robot.sensors,
  • org.usfirst.frc.team3494.robot.subsystems,
  • org.usfirst.frc.team3494.robot.vision
  • @@ -102,20 +103,28 @@

    Class Hierarchy

  • org.usfirst.frc.team3494.robot.commands.auto.XYDrive
  • +
  • org.usfirst.frc.team3494.robot.commands.DelayCommand
  • org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive
  • org.usfirst.frc.team3494.robot.commands.drive.Drive
  • org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain
  • -
  • org.usfirst.frc.team3494.robot.commands.gears.HoldInState
  • -
  • org.usfirst.frc.team3494.robot.commands.intake.RunIntake
  • -
  • org.usfirst.frc.team3494.robot.commands.gears.SetGearPosition
  • +
  • org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward
  • +
  • org.usfirst.frc.team3494.robot.commands.auto.NullAuto
  • +
  • org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive
  • +
  • org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive
  • +
  • org.usfirst.frc.team3494.robot.commands.auto.SetGearGrasp
  • +
  • org.usfirst.frc.team3494.robot.commands.gears.SetReverse
  • org.usfirst.frc.team3494.robot.commands.turret.Shoot
  • +
  • org.usfirst.frc.team3494.robot.commands.auto.tests.SpeedTest
  • +
  • org.usfirst.frc.team3494.robot.commands.auto.tests.StageTest
  • org.usfirst.frc.team3494.robot.commands.climb.StopClimber
  • -
  • org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition
  • +
  • org.usfirst.frc.team3494.robot.commands.gears.ToggleGearPosition
  • org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp
  • org.usfirst.frc.team3494.robot.commands.turret.TurretCon
  • +
  • org.usfirst.frc.team3494.robot.vision.GearPipeline (implements edu.wpi.first.wpilibj.vision.VisionPipeline)
  • org.usfirst.frc.team3494.robot.vision.GripPipeline (implements edu.wpi.first.wpilibj.vision.VisionPipeline)
  • +
  • org.usfirst.frc.team3494.robot.sensors.LineBreak
  • org.usfirst.frc.team3494.robot.OI
  • edu.wpi.first.wpilibj.RobotBase
      @@ -131,13 +140,18 @@

      Class Hierarchy

      • org.usfirst.frc.team3494.robot.subsystems.Climber (implements org.usfirst.frc.team3494.robot.subsystems.IMotorizedSubsystem)
      • org.usfirst.frc.team3494.robot.subsystems.Conveyer (implements org.usfirst.frc.team3494.robot.subsystems.IMotorizedSubsystem)
      • -
      • org.usfirst.frc.team3494.robot.subsystems.Drivetrain (implements org.usfirst.frc.team3494.robot.subsystems.IMotorizedSubsystem)
      • -
      • org.usfirst.frc.team3494.robot.subsystems.GearTake
      • -
      • org.usfirst.frc.team3494.robot.subsystems.Intake (implements org.usfirst.frc.team3494.robot.subsystems.IMotorizedSubsystem)
      • +
      • org.usfirst.frc.team3494.robot.subsystems.GearTake_2
      • org.usfirst.frc.team3494.robot.subsystems.Kompressor
      • +
      • org.usfirst.frc.team3494.robot.subsystems.MonoGearTake
      • +
      • edu.wpi.first.wpilibj.command.PIDSubsystem (implements edu.wpi.first.wpilibj.Sendable) + +
      • org.usfirst.frc.team3494.robot.subsystems.Turret (implements org.usfirst.frc.team3494.robot.subsystems.IMotorizedSubsystem)
      +
    • org.usfirst.frc.team3494.robot.vision.VisionUtils
  • diff --git a/docs/package-list b/docs/package-list index 6543d1e..42fdb1a 100644 --- a/docs/package-list +++ b/docs/package-list @@ -1,10 +1,11 @@ org.usfirst.frc.team3494.robot org.usfirst.frc.team3494.robot.commands org.usfirst.frc.team3494.robot.commands.auto +org.usfirst.frc.team3494.robot.commands.auto.tests org.usfirst.frc.team3494.robot.commands.climb org.usfirst.frc.team3494.robot.commands.drive org.usfirst.frc.team3494.robot.commands.gears -org.usfirst.frc.team3494.robot.commands.intake org.usfirst.frc.team3494.robot.commands.turret +org.usfirst.frc.team3494.robot.sensors org.usfirst.frc.team3494.robot.subsystems org.usfirst.frc.team3494.robot.vision