From d53a7da8aed68878efbd22bc09c9e5391ee14c90 Mon Sep 17 00:00:00 2001 From: Jordan Powers Date: Sun, 24 Feb 2019 14:03:26 -0800 Subject: [PATCH] Made the changes which weren't being made --- .vscode/settings.json | 7 +- src/main/java/frc/robot/Dashboard.java | 54 ----- src/main/java/frc/robot/OI.java | 11 + src/main/java/frc/robot/Robot.java | 226 ++++-------------- src/main/java/frc/robot/RobotMap.java | 34 +-- .../java/frc/robot/commands/DriveCommand.java | 13 +- .../robot/commands/HatchPusherCommand.java | 58 ----- .../frc/robot/commands/LiftDriveForward.java | 54 ----- .../frc/robot/commands/LiftSystemCommand.java | 49 ---- .../frc/robot/commands/WheelArmCommand.java | 39 ++- .../commands/{ => arm}/ArmAngleCommand.java | 5 +- .../hatchpusher/ToggleHatchPusher.java | 35 +++ .../{ => lift}/EngageLiftSolenoidCommand.java | 18 +- .../commands/{ => lift}/LiftCommandGroup.java | 27 +-- .../{ => lift}/LiftDriveForwardBack.java | 18 +- .../{ => lift}/LiftDriveForwardFront.java | 15 +- .../commands/{ => lift}/LiftRetractFront.java | 15 +- .../frc/robot/controls/DriveController.java | 29 ++- .../frc/robot/controls/F310Controller.java | 132 +++++----- .../java/frc/robot/subsystems/ArmAngle.java | 17 +- src/main/java/frc/robot/subsystems/Drive.java | 20 +- .../frc/robot/subsystems/HatchPusher.java | 18 +- .../java/frc/robot/subsystems/LiftSystem.java | 28 +-- .../java/frc/robot/subsystems/WheelArm.java | 65 ++--- .../frc/robot/subsystems/vision/Cameras.java | 49 ++-- .../robot/subsystems/vision/TargetBall.java | 61 +++-- .../robot/subsystems/vision/TargetHatch.java | 60 +++-- .../frc/robot/triggers/BooleanTrigger.java | 27 +++ 28 files changed, 431 insertions(+), 753 deletions(-) delete mode 100644 src/main/java/frc/robot/Dashboard.java delete mode 100644 src/main/java/frc/robot/commands/HatchPusherCommand.java delete mode 100644 src/main/java/frc/robot/commands/LiftDriveForward.java delete mode 100644 src/main/java/frc/robot/commands/LiftSystemCommand.java rename src/main/java/frc/robot/commands/{ => arm}/ArmAngleCommand.java (94%) create mode 100644 src/main/java/frc/robot/commands/hatchpusher/ToggleHatchPusher.java rename src/main/java/frc/robot/commands/{ => lift}/EngageLiftSolenoidCommand.java (88%) rename src/main/java/frc/robot/commands/{ => lift}/LiftCommandGroup.java (52%) rename src/main/java/frc/robot/commands/{ => lift}/LiftDriveForwardBack.java (82%) rename src/main/java/frc/robot/commands/{ => lift}/LiftDriveForwardFront.java (86%) rename src/main/java/frc/robot/commands/{ => lift}/LiftRetractFront.java (81%) create mode 100644 src/main/java/frc/robot/triggers/BooleanTrigger.java diff --git a/.vscode/settings.json b/.vscode/settings.json index cfe9f88..7847686 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -10,5 +10,10 @@ ".classpath": true, ".project": true }, - "wpilib.deployOffline": false + "wpilib.deployOffline": false, + + "editor.tabSize": 2, + "editor.insertSpaces": true, + "editor.detectIndentation": false, + "editor.formatOnSave": true } diff --git a/src/main/java/frc/robot/Dashboard.java b/src/main/java/frc/robot/Dashboard.java deleted file mode 100644 index e9a213c..0000000 --- a/src/main/java/frc/robot/Dashboard.java +++ /dev/null @@ -1,54 +0,0 @@ -package frc.robot; - -//import java.util.HashMap; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.Sendable; -//import edu.wpi.first.wpilibj.command.Command; - -public class Dashboard { - - private static Dashboard instance; - - private boolean shouldDisplayField(String key) { - return true; - } - - public boolean showDiagnostics; - - public void init(boolean showDiagonistics) { - this.showDiagnostics = showDiagonistics; - } - - public void putString(boolean isCompetition, String key, String value) { - if (shouldDisplayField(key)) { - SmartDashboard.putString(key, value); - } - } - - public void putNumber(boolean isCompetition, String key, double value) { - if (shouldDisplayField(key)) - - { - SmartDashboard.putNumber(key, value); - } - } - - public void putBoolean(boolean isCompetition, String key, boolean value) { - if (shouldDisplayField(key)) { - SmartDashboard.putBoolean(key, value); - } - } - - public void putData(boolean isCompetition, String key, Sendable value) { - if (shouldDisplayField(key)) { - SmartDashboard.putData(key, value); - } - } - - public static Dashboard getInstance() { - if (instance == null) - instance = new Dashboard(); - return instance; - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 5d57329..efbfa92 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -7,6 +7,10 @@ package frc.robot; +import edu.wpi.first.wpilibj.buttons.Trigger; +import frc.robot.triggers.*; +import frc.robot.commands.hatchpusher.ToggleHatchPusher; + /** * This class is the glue that binds the controls on the physical operator * interface to the commands and command groups that allow control of the robot. @@ -39,4 +43,11 @@ public class OI { // Start the command when the button is released and let it run the command // until it is finished as determined by it's isFinished method. // button.whenReleased(new ExampleCommand()); + + private Trigger toggleHatchPusher = new BooleanTrigger(Robot.controller::getToggleHatchPusher); + + public OI() { + toggleHatchPusher.whenActive(new ToggleHatchPusher()); + } + } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1f1b1cc..9413996 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,24 +13,18 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.cscore.VideoSource; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -/*import org.opencv.core.Mat; -import org.opencv.imgproc.Imgproc; - -import edu.wpi.cscore.CvSink; -import edu.wpi.cscore.CvSource; -import edu.wpi.cscore.UsbCamera; -import edu.wpi.first.cameraserver.CameraServer; -*/ import frc.robot.commands.DriveCommand; -import frc.robot.commands.HatchPusherCommand; -import frc.robot.commands.LiftCommandGroup; +import frc.robot.commands.lift.LiftCommandGroup; import frc.robot.commands.WheelArmCommand; -import frc.robot.commands.ArmAngleCommand; +import frc.robot.commands.arm.ArmAngleCommand; import frc.robot.controls.*; import frc.robot.subsystems.*; import frc.robot.subsystems.vision.Cameras; @@ -50,161 +44,59 @@ public class Robot extends TimedRobot { public static ArmAngle armAngle = new ArmAngle(); public static DriveController controller = new F310Controller(); public static OI m_oi; - public int myCounter = 0; - AHRS ahrs; - Command m_autonomousCommand; - SendableChooser m_chooser = new SendableChooser<>(); + public AHRS ahrs; + + private Command driveCommand = new DriveCommand(); + private Command armCommand = new WheelArmCommand(); + private Command liftCommand = new LiftCommandGroup(); + private Command armAngleCommand = new ArmAngleCommand(); /** - * This function is run when the robot is first started up and should be - * used for any initialization code. + * This function is run when the robot is first started up and should be used + * for any initialization code. */ @Override public void robotInit() { m_oi = new OI(); - // chooser.addOption("My Auto", new MyAutoCommand()); - //SmartDashboard.putData("Auto mode", m_chooser); - /* - // hatchCamera = CameraServer.getInstance().startAutomaticCapture("hatch",0); - // hatchCamera.setResolution(640, 480); - portCamera = CameraServer.getInstance().startAutomaticCapture("port",1); - portCamera.setResolution(1280, 960); + VideoSource lifeCam = new UsbCamera("Driver Camera", 0); + lifeCam.setResolution(640, 480); + lifeCam.setFPS(15); + CameraServer.getInstance().startAutomaticCapture(lifeCam); - new Thread(()->{ - UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(0); - camera.setResolution(640, 480); - CvSink cvSink = CameraServer.getInstance().getVideo(); - CvSource outputStream = CameraServer.getInstance().putVideo("7042", 640, 480); - Mat source = new Mat(); - Mat output = new Mat(); - while(!Thread.interrupted()){ - cvSink.grabFrame(source); - Imgproc.cvtColor(source, output, Imgproc.COLOR_BGR2GRAY); - outputStream.putFrame(output); - } - }).start(); - */ - Cameras.setup(); // Setup and Connection to Pixy2 and Microsoft Camera - SmartDashboard.putNumber("Iteration", myCounter); + Cameras.setup(); // Setup and Connection to Pixy2 try { - /*********************************************************************** - * navX-MXP: - * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB. - * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface. - * - * navX-Micro: - * - Communication via I2C (RoboRIO MXP or Onboard) and USB. - * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface. - * - * Multiple navX-model devices on a single robot are supported. - ************************************************************************/ - //ahrs = new AHRS(SerialPort.Port.kUSB1); - ahrs = new AHRS(SerialPort.Port.kMXP, SerialDataType.kProcessedData, (byte)50); - ahrs.enableLogging(true); - } catch (RuntimeException ex ) { - DriverStation.reportError("Error instantiating navX MXP: " + ex.getMessage(), true); - } + ahrs = new AHRS(SerialPort.Port.kMXP); + } catch (RuntimeException ex) { + DriverStation.reportError("Error instantiating navX MXP: " + ex.getMessage(), false); + } } /** - * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. + * This function is called every robot packet, no matter the mode. Use this for + * items like diagnostics that you want ran during disabled, autonomous, + * teleoperated and test. * - *

This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. + *

+ * This runs after the mode specific periodic functions, but before LiveWindow + * and SmartDashboard integrated updating. */ @Override public void robotPeriodic() { - SmartDashboard.putNumber("Iteration", myCounter); - /* Display 6-axis Processed Angle Data */ - SmartDashboard.putBoolean( "IMU_Connected", ahrs.isConnected()); - SmartDashboard.putBoolean( "IMU_IsCalibrating", ahrs.isCalibrating()); - SmartDashboard.putNumber( "IMU_Yaw", ahrs.getYaw()); - SmartDashboard.putNumber( "IMU_Pitch", ahrs.getPitch()); - SmartDashboard.putNumber( "IMU_Roll", ahrs.getRoll()); - - /* Display tilt-corrected, Magnetometer-based heading (requires */ - /* magnetometer calibration to be useful) */ - - SmartDashboard.putNumber( "IMU_CompassHeading", ahrs.getCompassHeading()); - - /* Display 9-axis Heading (requires magnetometer calibration to be useful) */ - SmartDashboard.putNumber( "IMU_FusedHeading", ahrs.getFusedHeading()); - - /* These functions are compatible w/the WPI Gyro Class, providing a simple */ - /* path for upgrading from the Kit-of-Parts gyro to the navx MXP */ - - SmartDashboard.putNumber( "IMU_TotalYaw", ahrs.getAngle()); - SmartDashboard.putNumber( "IMU_YawRateDPS", ahrs.getRate()); - /* Display Processed Acceleration Data (Linear Acceleration, Motion Detect) */ - - SmartDashboard.putNumber( "IMU_Accel_X", ahrs.getWorldLinearAccelX()); - SmartDashboard.putNumber( "IMU_Accel_Y", ahrs.getWorldLinearAccelY()); - SmartDashboard.putBoolean( "IMU_IsMoving", ahrs.isMoving()); - SmartDashboard.putBoolean( "IMU_IsRotating", ahrs.isRotating()); + if (ahrs != null) { + SmartDashboard.putData(ahrs); + } - /* Display estimates of velocity/displacement. Note that these values are */ - /* not expected to be accurate enough for estimating robot position on a */ - /* FIRST FRC Robotics Field, due to accelerometer noise and the compounding */ - /* of these errors due to single (velocity) integration and especially */ - /* double (displacement) integration. */ - - SmartDashboard.putNumber( "Velocity_X", ahrs.getVelocityX()); - SmartDashboard.putNumber( "Velocity_Y", ahrs.getVelocityY()); - SmartDashboard.putNumber( "Displacement_X", ahrs.getDisplacementX()); - SmartDashboard.putNumber( "Displacement_Y", ahrs.getDisplacementY()); - - /* Display Raw Gyro/Accelerometer/Magnetometer Values */ - /* NOTE: These values are not normally necessary, but are made available */ - /* for advanced users. Before using this data, please consider whether */ - /* the processed data (see above) will suit your needs. */ - - SmartDashboard.putNumber( "RawGyro_X", ahrs.getRawGyroX()); - SmartDashboard.putNumber( "RawGyro_Y", ahrs.getRawGyroY()); - SmartDashboard.putNumber( "RawGyro_Z", ahrs.getRawGyroZ()); - SmartDashboard.putNumber( "RawAccel_X", ahrs.getRawAccelX()); - SmartDashboard.putNumber( "RawAccel_Y", ahrs.getRawAccelY()); - SmartDashboard.putNumber( "RawAccel_Z", ahrs.getRawAccelZ()); - SmartDashboard.putNumber( "RawMag_X", ahrs.getRawMagX()); - SmartDashboard.putNumber( "RawMag_Y", ahrs.getRawMagY()); - SmartDashboard.putNumber( "RawMag_Z", ahrs.getRawMagZ()); - SmartDashboard.putNumber( "IMU_Temp_C", ahrs.getTempC()); - SmartDashboard.putNumber( "IMU_Timestamp", ahrs.getLastSensorTimestamp()); - - /* Omnimount Yaw Axis Information */ - /* For more info, see http://navx-mxp.kauailabs.com/installation/omnimount */ - AHRS.BoardYawAxis yaw_axis = ahrs.getBoardYawAxis(); - SmartDashboard.putString( "YawAxisDirection", yaw_axis.up ? "Up" : "Down" ); - SmartDashboard.putNumber( "YawAxis", yaw_axis.board_axis.getValue() ); - - /* Sensor Board Information */ - SmartDashboard.putString( "FirmwareVersion", ahrs.getFirmwareVersion()); - - /* Quaternion Data */ - /* Quaternions are fascinating, and are the most compact representation of */ - /* orientation data. All of the Yaw, Pitch and Roll Values can be derived */ - /* from the Quaternions. If interested in motion processing, knowledge of */ - /* Quaternions is highly recommended. */ - SmartDashboard.putNumber( "QuaternionW", ahrs.getQuaternionW()); - SmartDashboard.putNumber( "QuaternionX", ahrs.getQuaternionX()); - SmartDashboard.putNumber( "QuaternionY", ahrs.getQuaternionY()); - SmartDashboard.putNumber( "QuaternionZ", ahrs.getQuaternionZ()); - - /* Connectivity Debugging Support */ - SmartDashboard.putNumber( "IMU_Byte_Count", ahrs.getByteCount()); - SmartDashboard.putNumber( "IMU_Update_Count", ahrs.getUpdateCount()); - Cameras.run(); + Cameras.run(); } /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. */ @Override public void disabledInit() { @@ -217,30 +109,22 @@ public void disabledPeriodic() { /** * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable - * chooser code works with the Java SmartDashboard. If you prefer the - * LabVIEW Dashboard, remove all of the chooser code and uncomment the - * getString code to get the auto name from the text box below the Gyro + * between different autonomous modes using the dashboard. The sendable chooser + * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard, + * remove all of the chooser code and uncomment the getString code to get the + * auto name from the text box below the Gyro * - *

You can add additional auto modes by adding additional commands to the - * chooser code above (like the commented example) or additional comparisons - * to the switch structure below with additional strings & commands. + *

+ * You can add additional auto modes by adding additional commands to the + * chooser code above (like the commented example) or additional comparisons to + * the switch structure below with additional strings & commands. */ @Override public void autonomousInit() { - m_autonomousCommand = m_chooser.getSelected(); - - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } - */ - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.start(); - } + driveCommand.start(); + armCommand.start(); + liftCommand.start(); + armAngleCommand.start(); } /** @@ -253,17 +137,11 @@ public void autonomousPeriodic() { @Override public void teleopInit() { - // 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 - // this line or comment it out. - if (m_autonomousCommand != null) - m_autonomousCommand.cancel(); - new DriveCommand().start(); - new HatchPusherCommand().start(); - new WheelArmCommand().start(); - new LiftCommandGroup().start(); - new ArmAngleCommand().start(); + Scheduler.getInstance().removeAll(); + driveCommand.start(); + armCommand.start(); + liftCommand.start(); + armAngleCommand.start(); } /** diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index f0a187f..b7abe8a 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -6,13 +6,14 @@ /*----------------------------------------------------------------------------*/ package frc.robot; + import org.usfirst.frc.team4999.controllers.LogitechF310; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Spark; -import edu.wpi.first.wpilibj.Talon; -import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.Encoder; + /** * The RobotMap is a mapping from the ports sensors and actuators are wired into * to a variable name. This provides flexibility changing wiring, makes checking @@ -20,31 +21,22 @@ * floating around. */ public class RobotMap { - // For example to map the left and right motors, you could define the - // following variables to use with your drivetrain subsystem. - // public static int leftMotor = 1; - // public static int rightMotor = 2; - - // If you are using multiple modules, make sure to define both the port - // number and the module. For example you with a rangefinder: - // public static int rangefinderPort = 1; - // public static int rangefinderModule = 1; public static final Spark leftFront = new Spark(8);// TODO: Set to actual port numbers public static final Spark leftBack = new Spark(7); public static final Spark rightFront = new Spark(6); public static final Spark rightBack = new Spark(5); - public static final VictorSP wheelArmLeft = new VictorSP(4);// TODO: Set to actual port numbers - public static final VictorSP wheelArmRight = new VictorSP(1); - public static final VictorSP wheelArmAngle = new VictorSP(0); - public static final VictorSP leftLiftWheel = new VictorSP(3); - public static final VictorSP rightLiftWheel = new VictorSP(2); + public static final PWMVictorSPX wheelArmLeft = new PWMVictorSPX(4);// TODO: Set to actual port numbers + public static final PWMVictorSPX wheelArmRight = new PWMVictorSPX(1); + public static final PWMVictorSPX wheelArmAngle = new PWMVictorSPX(0); + public static final PWMVictorSPX leftLiftWheel = new PWMVictorSPX(3); + public static final PWMVictorSPX rightLiftWheel = new PWMVictorSPX(2); + + public static final Encoder wheelArmEncoder = new Encoder(0, 1, false);// TODO: Fix encoder constructor - public static final Encoder wheelArmEncoder = new Encoder(0,1,false);//TODO: Fix encoder constructor + public static final DoubleSolenoid hatchSolenoidTop = new DoubleSolenoid(0, 3); // TODO: set to actual solenoid values + public static final DoubleSolenoid liftSystemBack = new DoubleSolenoid(1, 5); + public static final DoubleSolenoid liftSystemFront = new DoubleSolenoid(2, 4); - public static final DoubleSolenoid hatchSolenoidTop = new DoubleSolenoid(0,3); // TODO: set to actual solenoid values - public static final DoubleSolenoid liftSystemBack = new DoubleSolenoid(1,5); - public static final DoubleSolenoid liftSystemFront = new DoubleSolenoid(2,4); - public static final LogitechF310 controller1 = new LogitechF310(1); } diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 1eca620..cd61848 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -12,8 +12,11 @@ import frc.robot.controls.DriveController; public class DriveCommand extends Command { + private boolean reverse = false; + public DriveCommand() { + super(); requires(Robot.drive); } @@ -27,10 +30,12 @@ protected void initialize() { protected void execute() { DriveController controller = Robot.controller; double moveRequest = controller.getMoveRequest(); - if(controller.getReverseDirection()) + + moveRequest = reverse ? -moveRequest : moveRequest; + + if (controller.getReverseDirection()) reverse = !reverse; - if(reverse) - moveRequest=-moveRequest; + Robot.drive.arcadeDrive(moveRequest, controller.getTurnRequest(), controller.getSpeedLimit()); } @@ -43,11 +48,13 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { + Robot.drive.stop(); } // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { + Robot.drive.stop(); } } diff --git a/src/main/java/frc/robot/commands/HatchPusherCommand.java b/src/main/java/frc/robot/commands/HatchPusherCommand.java deleted file mode 100644 index c7f2ba4..0000000 --- a/src/main/java/frc/robot/commands/HatchPusherCommand.java +++ /dev/null @@ -1,58 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Robot; -import frc.robot.controls.DriveController; - -public class HatchPusherCommand extends Command { - public HatchPusherCommand() { - requires(Robot.hatchPusher); - // 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() { - DriveController controller = Robot.controller; - if(controller.getToggleHatchPusher()){ - if(Robot.hatchPusher.isOut()){ - Robot.hatchPusher.retract(); - } - else{ - Robot.hatchPusher.extend(); - } - } - - - } - - // 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/src/main/java/frc/robot/commands/LiftDriveForward.java b/src/main/java/frc/robot/commands/LiftDriveForward.java deleted file mode 100644 index 265760d..0000000 --- a/src/main/java/frc/robot/commands/LiftDriveForward.java +++ /dev/null @@ -1,54 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Robot; - - -public class LiftDriveForwardFront extends Command { - Timer t = new Timer(); - private static final double DRIVE_DELAY = 2.0; - public LiftDriveForwardFront() { - requires(Robot.liftSystem); - // 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() { - Robot.liftSystem.driveForward(); - - } - - // 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/src/main/java/frc/robot/commands/LiftSystemCommand.java b/src/main/java/frc/robot/commands/LiftSystemCommand.java deleted file mode 100644 index 6dab45b..0000000 --- a/src/main/java/frc/robot/commands/LiftSystemCommand.java +++ /dev/null @@ -1,49 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; - -public class LiftSystemCommand extends Command { - /*public LiftSystemCommand() { - requires(Robot.liftSystem); - // 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() { - if(controller1.getStartLift()) - Robot.LiftSystem.lift(); - } - - // 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/src/main/java/frc/robot/commands/WheelArmCommand.java b/src/main/java/frc/robot/commands/WheelArmCommand.java index c5b7c30..6ac8d32 100644 --- a/src/main/java/frc/robot/commands/WheelArmCommand.java +++ b/src/main/java/frc/robot/commands/WheelArmCommand.java @@ -10,10 +10,15 @@ import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; import frc.robot.controls.DriveController; +import frc.robot.subsystems.WheelArm; public class WheelArmCommand extends Command { + + private WheelArm arms = Robot.wheelArm; + public WheelArmCommand() { - requires(Robot.wheelArm); + super(); + requires(arms); } // Called just before this Command runs the first time @@ -25,32 +30,14 @@ protected void initialize() { @Override protected void execute() { DriveController controller = Robot.controller; - if(controller.getToggleInwards()){ - Robot.wheelArm.toggleInwards(); - } - if(controller.getToggleOutwards()){ - Robot.wheelArm.toggleOutwards(); - } - Robot.wheelArm.setSpeed(); - /*if(Robot.wheelArm.isInwards()||Robot.wheelArm.isOutwards()){ - if(controller.getToggleInwards()||controller.getToggleOutwards()){ - Robot.wheelArm.stopArms(); - } - - }*/ - /* - if(Robot.wheelArm.isInwards()){ - Robot.wheelArm.spinInwards(); + if (controller.getWheelArmInwards()) { + arms.runInwards(); + } else if (controller.getWheelArmOutwards()) { + arms.runOutwards(); + } else { + arms.stopArms(); } - else if(Robot.wheelArm.isOutwards()){ - Robot.wheelArm.spinOutwards(); - } - else{ - Robot.wheelArm.stopArms(); - } -*/ - } // Make this return true when this Command no longer needs to run execute() @@ -62,11 +49,13 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { + arms.stopArms(); } // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { + arms.stopArms(); } } diff --git a/src/main/java/frc/robot/commands/ArmAngleCommand.java b/src/main/java/frc/robot/commands/arm/ArmAngleCommand.java similarity index 94% rename from src/main/java/frc/robot/commands/ArmAngleCommand.java rename to src/main/java/frc/robot/commands/arm/ArmAngleCommand.java index ccdf9b4..b1e2553 100644 --- a/src/main/java/frc/robot/commands/ArmAngleCommand.java +++ b/src/main/java/frc/robot/commands/arm/ArmAngleCommand.java @@ -5,14 +5,14 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.arm; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; -import frc.robot.controls.DriveController; public class ArmAngleCommand extends Command { public ArmAngleCommand() { + super(); requires(Robot.armAngle); } @@ -36,6 +36,7 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { + Robot.armAngle.stop(); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/hatchpusher/ToggleHatchPusher.java b/src/main/java/frc/robot/commands/hatchpusher/ToggleHatchPusher.java new file mode 100644 index 0000000..c2d4842 --- /dev/null +++ b/src/main/java/frc/robot/commands/hatchpusher/ToggleHatchPusher.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.hatchpusher; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import frc.robot.Robot; +import frc.robot.subsystems.HatchPusher; + +/** + * Add your docs here. + */ +public class ToggleHatchPusher extends InstantCommand { + + private HatchPusher pusher = Robot.hatchPusher; + + public ToggleHatchPusher() { + super(); + requires(pusher); + } + + // Called once when the command executes + @Override + protected void initialize() { + if (pusher.isOut()) + pusher.retract(); + else + pusher.extend(); + } + +} diff --git a/src/main/java/frc/robot/commands/EngageLiftSolenoidCommand.java b/src/main/java/frc/robot/commands/lift/EngageLiftSolenoidCommand.java similarity index 88% rename from src/main/java/frc/robot/commands/EngageLiftSolenoidCommand.java rename to src/main/java/frc/robot/commands/lift/EngageLiftSolenoidCommand.java index 46ed681..6ce0de3 100644 --- a/src/main/java/frc/robot/commands/EngageLiftSolenoidCommand.java +++ b/src/main/java/frc/robot/commands/lift/EngageLiftSolenoidCommand.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.lift; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; @@ -15,36 +15,34 @@ public class EngageLiftSolenoidCommand extends Command { Timer t = new Timer(); private static final double SOLENOID_DELAY = 4.0; + public EngageLiftSolenoidCommand() { + super(); requires(Robot.liftSystem); - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); } - // Called just before this Command runs the first time @Override protected void initialize() { t.start(); - + } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { DriveController controller = Robot.controller; - if(controller.getStartLift()){ + if (controller.getStartLift()) { Robot.liftSystem.engageSolenoids(); - + } - - + } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return (t.get()>=SOLENOID_DELAY); + return (t.get() >= SOLENOID_DELAY); } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/LiftCommandGroup.java b/src/main/java/frc/robot/commands/lift/LiftCommandGroup.java similarity index 52% rename from src/main/java/frc/robot/commands/LiftCommandGroup.java rename to src/main/java/frc/robot/commands/lift/LiftCommandGroup.java index 87b1b4a..f381b68 100644 --- a/src/main/java/frc/robot/commands/LiftCommandGroup.java +++ b/src/main/java/frc/robot/commands/lift/LiftCommandGroup.java @@ -5,37 +5,18 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.lift; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.CommandGroup; public class LiftCommandGroup extends CommandGroup { - public LiftCommandGroup() { - /*addSequential(new EngageLiftSolenoidCommand()); + addSequential(new EngageLiftSolenoidCommand()); addSequential(new LiftDriveForwardFront()); addSequential(new LiftRetractFront()); addSequential(new LiftDriveForwardBack()); - addSequential(new LiftSystemEnd()); - */ + // addSequential(new LiftSystemEnd()); } - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } - +} diff --git a/src/main/java/frc/robot/commands/LiftDriveForwardBack.java b/src/main/java/frc/robot/commands/lift/LiftDriveForwardBack.java similarity index 82% rename from src/main/java/frc/robot/commands/LiftDriveForwardBack.java rename to src/main/java/frc/robot/commands/lift/LiftDriveForwardBack.java index b3b8406..0ed8b5e 100644 --- a/src/main/java/frc/robot/commands/LiftDriveForwardBack.java +++ b/src/main/java/frc/robot/commands/lift/LiftDriveForwardBack.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.lift; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; @@ -13,17 +13,18 @@ public class LiftDriveForwardBack extends Command { Timer t = new Timer(); - private static final double DRIVE_DELAY = 3.0; + private static final double DRIVE_DELAY = 3.0; + public LiftDriveForwardBack() { - - requires(Robot.liftSystem); - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } + super(); + requires(Robot.liftSystem); + } + // Called just before this Command runs the first time @Override protected void initialize() { t.start(); + t.reset(); } // Called repeatedly when this Command is scheduled to run @@ -35,8 +36,9 @@ protected void execute() { // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return (t.get()>=DRIVE_DELAY); + return t.hasPeriodPassed(DRIVE_DELAY); } + // subsystems is scheduled to run @Override protected void interrupted() { diff --git a/src/main/java/frc/robot/commands/LiftDriveForwardFront.java b/src/main/java/frc/robot/commands/lift/LiftDriveForwardFront.java similarity index 86% rename from src/main/java/frc/robot/commands/LiftDriveForwardFront.java rename to src/main/java/frc/robot/commands/lift/LiftDriveForwardFront.java index 8a0fc95..2e0b685 100644 --- a/src/main/java/frc/robot/commands/LiftDriveForwardFront.java +++ b/src/main/java/frc/robot/commands/lift/LiftDriveForwardFront.java @@ -5,39 +5,39 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.lift; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; - public class LiftDriveForwardFront extends Command { Timer t = new Timer(); - private static final double DRIVE_DELAY = 2.0; + private static final double DRIVE_DELAY = 2.0; + public LiftDriveForwardFront() { + super(); requires(Robot.liftSystem); - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); } // Called just before this Command runs the first time @Override protected void initialize() { t.start(); + t.reset(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { Robot.liftSystem.driveForward(); - + } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return (t.get()>=DRIVE_DELAY); + return t.hasPeriodPassed(DRIVE_DELAY); } // Called once after isFinished returns true @@ -52,4 +52,3 @@ protected void interrupted() { } } - diff --git a/src/main/java/frc/robot/commands/LiftRetractFront.java b/src/main/java/frc/robot/commands/lift/LiftRetractFront.java similarity index 81% rename from src/main/java/frc/robot/commands/LiftRetractFront.java rename to src/main/java/frc/robot/commands/lift/LiftRetractFront.java index c5dfc27..9f86379 100644 --- a/src/main/java/frc/robot/commands/LiftRetractFront.java +++ b/src/main/java/frc/robot/commands/lift/LiftRetractFront.java @@ -5,41 +5,38 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.lift; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; import edu.wpi.first.wpilibj.Timer; - public class LiftRetractFront extends Command { Timer t = new Timer(); public static final double RETRACT_DELAY = 3.0; + public LiftRetractFront() { + super(); requires(Robot.liftSystem); - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); } - //private static final Timer t2 = new Timer(); - //private static final double stall2 = 2.0; // Called just before this Command runs the first time @Override protected void initialize() { t.start(); + t.reset(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.liftSystem.retractFrontSolenoids(); - //t2.delay(stall2); + Robot.liftSystem.retractFrontSolenoids(); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return (t.get()>=RETRACT_DELAY); + return t.hasPeriodPassed(RETRACT_DELAY); } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/controls/DriveController.java b/src/main/java/frc/robot/controls/DriveController.java index 33d1be8..efe8419 100644 --- a/src/main/java/frc/robot/controls/DriveController.java +++ b/src/main/java/frc/robot/controls/DriveController.java @@ -11,15 +11,24 @@ * Add your docs here. */ public interface DriveController { - Object getArmsSpeed = null; - public double getMoveRequest(); - public double getTurnRequest(); - public double getSpeedLimit(); - public boolean getReverseDirection(); - public boolean getToggleHatchPusher(); - public boolean getToggleInwards(); - public boolean getToggleOutwards(); - public boolean getStartLift(); - public double getArmsSpeed(); + Object getArmsSpeed = null; + + public double getMoveRequest(); + + public double getTurnRequest(); + + public double getSpeedLimit(); + + public boolean getReverseDirection(); + + public boolean getToggleHatchPusher(); + + public boolean getWheelArmInwards(); + + public boolean getWheelArmOutwards(); + + public boolean getStartLift(); + + public double getArmsSpeed(); } diff --git a/src/main/java/frc/robot/controls/F310Controller.java b/src/main/java/frc/robot/controls/F310Controller.java index b299731..fb8f4ef 100644 --- a/src/main/java/frc/robot/controls/F310Controller.java +++ b/src/main/java/frc/robot/controls/F310Controller.java @@ -6,6 +6,7 @@ /*----------------------------------------------------------------------------*/ package frc.robot.controls; + import org.usfirst.frc.team4999.controllers.LogitechF310; import edu.wpi.first.wpilibj.GenericHID.Hand; @@ -15,79 +16,78 @@ /** * Add your docs here. */ -public class F310Controller implements DriveController{ - private LogitechF310 controller1 = RobotMap.controller1; - - private static final double CURVE = 2; - private static final double DEADZONE = .01; - private static final double startLift = .75; - private static final double ARM_UP_SPEED = 0.5; - private static final double ARM_DOWN_SPEED = -0.5; - - private double[] speedLimits = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1.0}; - private int speedLimitIndex = speedLimits.length-1; - @Override - public double getMoveRequest(){ - double speed = controller1.getRawAxis(1); - speed = curve(speed, CURVE); - speed = deadzone(speed, DEADZONE); - return speed; +public class F310Controller implements DriveController { + private LogitechF310 controller1 = RobotMap.controller1; - } - @Override - public double getTurnRequest(){ - double speed = controller1.getRawAxis(4); - speed = curve(speed, CURVE); - speed = deadzone(speed, DEADZONE); - return speed; + private static final double CURVE = 2; + private static final double DEADZONE = .01; + private static final double startLift = .75; + private static final double ARM_UP_SPEED = 0.5; + private static final double ARM_DOWN_SPEED = -0.5; - } - @Override - public double getSpeedLimit(){ - if(controller1.getRawButtonPressed(7) && speedLimitIndex>0) - speedLimitIndex--; - if(controller1.getRawButtonPressed(8) && speedLimitIndex 0) + speedLimitIndex--; + if (controller1.getRawButtonPressed(8) && speedLimitIndex < speedLimits.length - 1) + speedLimitIndex++; + return speedLimits[speedLimitIndex]; + } + + @Override + public boolean getReverseDirection() { + return controller1.getRawButtonPressed(3); + } + + @Override + public boolean getToggleHatchPusher() { + return controller1.getRawButtonPressed(2); + } - public boolean getStartLift(){ - return controller1.getRawAxis(2)>startLift; - + @Override + public boolean getWheelArmInwards() { + return controller1.getRawButton(5); + } + + @Override + public boolean getWheelArmOutwards() { + return controller1.getRawButton(6); + } + + @Override + public double getArmsSpeed() { + if (controller1.getRawButton(4)) { + return ARM_UP_SPEED; + } else if (controller1.getRawButton(1)) { + return ARM_DOWN_SPEED; + } else { + return 0; } -} + } - - + public boolean getStartLift() { + return controller1.getRawAxis(2) > startLift; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmAngle.java b/src/main/java/frc/robot/subsystems/ArmAngle.java index 1602c1b..b7e7ac3 100644 --- a/src/main/java/frc/robot/subsystems/ArmAngle.java +++ b/src/main/java/frc/robot/subsystems/ArmAngle.java @@ -8,28 +8,29 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.Talon; //Are we just using Victor SPX ? Why are we not using PWMVictorSPX class? -import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.PWMVictorSPX; import frc.robot.RobotMap; -import frc.robot.controls.DriveController; /** * Add your docs here. */ public class ArmAngle extends Subsystem { - private double armAngleSpeed = RobotMap.controller1.getArmsSpeed(); - private VictorSP angle = RobotMap.wheelArmAngle; + + private PWMVictorSPX angle = RobotMap.wheelArmAngle; + public ArmAngle(){ super("Arm Angle"); addChild("Angle Motor", angle); } + public void setSpeed(double armAngleSpeed){ angle.set(armAngleSpeed); } + + public void stop() { + angle.set(0); + } @Override public void initDefaultCommand() { - - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); } } diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index da9e226..2a52317 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import frc.robot.RobotMap; +import frc.robot.commands.DriveCommand; + import static org.usfirst.frc.team4999.utils.Utils.map; /** @@ -22,23 +24,27 @@ public class Drive extends Subsystem { private DifferentialDrive drive = new DifferentialDrive(leftMotors, rightMotors); - - public Drive(){ + public Drive() { super("Drive"); addChild("Left Motors", leftMotors); addChild("Right Motors", rightMotors); drive.setDeadband(0); - + } - public void arcadeDrive(double moveRequest, double turnRequest, double speedLimiter){ + + public void arcadeDrive(double moveRequest, double turnRequest, double speedLimiter) { moveRequest = map(moveRequest, -1, 1, -speedLimiter, speedLimiter); turnRequest = map(turnRequest, -1, 1, -speedLimiter, speedLimiter); drive.arcadeDrive(moveRequest, turnRequest, false); } + + public void stop() { + drive.stopMotor(); + } + @Override public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); + setDefaultCommand(new DriveCommand()); } - + } diff --git a/src/main/java/frc/robot/subsystems/HatchPusher.java b/src/main/java/frc/robot/subsystems/HatchPusher.java index 66efb76..d6a4ab9 100644 --- a/src/main/java/frc/robot/subsystems/HatchPusher.java +++ b/src/main/java/frc/robot/subsystems/HatchPusher.java @@ -20,30 +20,22 @@ public class HatchPusher extends Subsystem { // here. Call these from Commands. private DoubleSolenoid top = RobotMap.hatchSolenoidTop; - - private boolean out = false; - - - public HatchPusher(){ + public HatchPusher() { super("Hatch Pusher"); addChild("Top Solenoid", top); - } - public void extend(){ + public void extend() { top.set(Value.kForward); - - } - public void retract(){ + public void retract() { top.set(Value.kReverse); - } - public boolean isOut(){ - return top.get()==Value.kForward; + public boolean isOut() { + return top.get() == Value.kForward; } @Override diff --git a/src/main/java/frc/robot/subsystems/LiftSystem.java b/src/main/java/frc/robot/subsystems/LiftSystem.java index 95af545..d350809 100644 --- a/src/main/java/frc/robot/subsystems/LiftSystem.java +++ b/src/main/java/frc/robot/subsystems/LiftSystem.java @@ -8,27 +8,26 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.command.Subsystem; import frc.robot.RobotMap; + /* * Add your docs here. */ public class LiftSystem extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. - private double driveSpeed = .5; + private double driveSpeed = .5; private DoubleSolenoid front = RobotMap.liftSystemFront; private DoubleSolenoid back = RobotMap.liftSystemBack; - - - private VictorSP leftWheel = RobotMap.leftLiftWheel; - private VictorSP rightWheel = RobotMap.rightLiftWheel; + private PWMVictorSPX leftWheel = RobotMap.leftLiftWheel; + private PWMVictorSPX rightWheel = RobotMap.rightLiftWheel; - public LiftSystem(){ + public LiftSystem() { super("Lift System"); addChild("Front Solenoid", front); addChild("Back Solenoid", back); @@ -36,25 +35,24 @@ public LiftSystem(){ addChild("Right Lift Wheel", rightWheel); } - public void engageSolenoids(){ + public void engageSolenoids() { front.set(Value.kForward); - back.set(Value.kForward); - } - public void driveForward(){ + + public void driveForward() { leftWheel.set(driveSpeed); rightWheel.set(driveSpeed); } - public void retractFrontSolenoids(){ + + public void retractFrontSolenoids() { front.set(Value.kReverse); - } - public void endLiftSequence(){ + + public void endLiftSequence() { leftWheel.set(0); rightWheel.set(0); back.set(Value.kReverse); - } diff --git a/src/main/java/frc/robot/subsystems/WheelArm.java b/src/main/java/frc/robot/subsystems/WheelArm.java index 9f0c3cc..5fca4bb 100644 --- a/src/main/java/frc/robot/subsystems/WheelArm.java +++ b/src/main/java/frc/robot/subsystems/WheelArm.java @@ -7,64 +7,45 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.Talon; -import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.command.Subsystem; import frc.robot.RobotMap; - public class WheelArm extends Subsystem { -private VictorSP left = RobotMap.wheelArmLeft; -private VictorSP right = RobotMap.wheelArmRight; //Did we change this to a PWMVictorSPX ? + private PWMVictorSPX left = RobotMap.wheelArmLeft; + private PWMVictorSPX right = RobotMap.wheelArmRight; // Did we change this to a PWMPWMVictorSPXX ? + + private static double WHEEL_ARM_OUTTAKE = .40; // TODO: Add real values + private static double WHEEL_ARM_INTAKE = .90; -private boolean inwards = false; -private boolean outwards = false; -private static double wheelArmOuttake = .40;//TODO: Add real value -private static double wheelArmIntake = .90; -public WheelArm(){ + public WheelArm() { super("Wheel Arm"); addChild("Left Motor", left); addChild("Right Motor", right); left.setInverted(false); right.setInverted(true); - - } - public void toggleInwards(){ - inwards = !inwards; - if (inwards) outwards = false; - } - public void toggleOutwards(){ - outwards = !outwards; - if (outwards) inwards = false; - } - public void setStopped(){ - inwards = false; - outwards = false; + } - public void stopArms(){ + + public void stopArms() { left.set(0); right.set(0); } - public void setSpeed(){ - if (inwards){ - left.set(wheelArmIntake); - right.set(-wheelArmIntake); - } - else if (outwards){ - left.set(-wheelArmOuttake); - right.set(wheelArmOuttake); - } - else { - stopArms(); - } + + public void runInwards() { + left.set(WHEEL_ARM_INTAKE); + right.set(WHEEL_ARM_INTAKE); } - /*public boolean isInwards(){ - return inwards; + + public void runOutwards() { + left.set(WHEEL_ARM_OUTTAKE); + right.set(WHEEL_ARM_OUTTAKE); } - public boolean isOutwards(){ - return outwards; - }*/ + + /* + * public boolean isInwards(){ return inwards; } public boolean isOutwards(){ + * return outwards; } + */ @Override public void initDefaultCommand() { // Set the default command for a subsystem here. diff --git a/src/main/java/frc/robot/subsystems/vision/Cameras.java b/src/main/java/frc/robot/subsystems/vision/Cameras.java index 491d58e..ecfa49b 100644 --- a/src/main/java/frc/robot/subsystems/vision/Cameras.java +++ b/src/main/java/frc/robot/subsystems/vision/Cameras.java @@ -6,36 +6,23 @@ public class Cameras { - private static UsbCamera drive = null; - private static PixyCamera pixy = null; - - public static void setup() { - initDrive(); - //pixy = new PixyCamera(new SPILink()); - pixy = new PixyCamera(new SPILink(),0); - - } - - public static void light(boolean state) { - pixy.light(state); - } - - public static void run() { - pixy.run(); - } - - public static PixyCamera getPixyCamera() { - return pixy; - } - - public static void initDrive() { - drive = CameraServer.getInstance().startAutomaticCapture(); - if (drive != null) { - // drive.setVideoMode(PixelFormat.kBGR, 320, 240, 30); - drive.setResolution(320, 240); - drive.setFPS(60); - - } - } + private static PixyCamera pixy = null; + + public static void setup() { + pixy = new PixyCamera(new SPILink(), 0); + + } + + public static void light(boolean state) { + pixy.light(state); + } + + public static void run() { + pixy.run(); + } + + public static PixyCamera getPixyCamera() { + return pixy; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/TargetBall.java b/src/main/java/frc/robot/subsystems/vision/TargetBall.java index c5c8181..01319b6 100644 --- a/src/main/java/frc/robot/subsystems/vision/TargetBall.java +++ b/src/main/java/frc/robot/subsystems/vision/TargetBall.java @@ -1,44 +1,43 @@ package frc.robot.subsystems.vision; import java.util.ArrayList; -//import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Dashboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.vision.Cameras; -//import io.github.pseudoresonance.pixy2api.Pixy2CCC; import io.github.pseudoresonance.pixy2api.Pixy2CCC.Block; public class TargetBall { - private static ArrayList blocks = Cameras.getPixyCamera().getPixy().getCCC().getBlocks(); - private static final int blockSignature = 1; - - public static void run(int count) { - if (count > 0) { - Block largestBlock = null; - // Checks for Biggest Block that is of the Orange Color Signature - for (Block block : blocks) { - if (block.getSignature() == blockSignature) { - if (largestBlock == null) { - largestBlock = block; - } else if (block.getWidth() > largestBlock.getWidth()) { - largestBlock = block; - } - } - } - - int ballX = largestBlock.getX(); - double yaw = ((ballX - 157.5) * 0.1904761905); - - // Information about the Big Orange Block is sent to NetworkTables for - // Shuffleboard - Dashboard.getInstance().putNumber(false, "Ball Angle", yaw); - Dashboard.getInstance().putNumber(false, "Ball X", largestBlock.getX()); - Dashboard.getInstance().putNumber(false, "Ball Y", largestBlock.getY()); - Dashboard.getInstance().putNumber(false, "Ball Box Width", largestBlock.getWidth()); - Dashboard.getInstance().putNumber(false, "Ball Box Height", largestBlock.getHeight()); + private static ArrayList blocks = Cameras.getPixyCamera().getPixy().getCCC().getBlocks(); + private static final int blockSignature = 1; + + public static void run(int count) { + if (count > 0) { + Block largestBlock = null; + // Checks for Biggest Block that is of the Orange Color Signature + for (Block block : blocks) { + if (block.getSignature() == blockSignature) { + if (largestBlock == null) { + largestBlock = block; + } else if (block.getWidth() > largestBlock.getWidth()) { + largestBlock = block; + } } - + } + + int ballX = largestBlock.getX(); + // TODO: What is this doing? It needs a comment + double yaw = ((ballX - 157.5) * 0.1904761905); + + // Information about the Big Orange Block is sent to NetworkTables for + // Shuffleboard + SmartDashboard.putNumber("Ball Angle", yaw); + SmartDashboard.putNumber("Ball X", largestBlock.getX()); + SmartDashboard.putNumber("Ball Y", largestBlock.getY()); + SmartDashboard.putNumber("Ball Box Width", largestBlock.getWidth()); + SmartDashboard.putNumber("Ball Box Height", largestBlock.getHeight()); } + } + } diff --git a/src/main/java/frc/robot/subsystems/vision/TargetHatch.java b/src/main/java/frc/robot/subsystems/vision/TargetHatch.java index 519a330..efe5a7c 100644 --- a/src/main/java/frc/robot/subsystems/vision/TargetHatch.java +++ b/src/main/java/frc/robot/subsystems/vision/TargetHatch.java @@ -1,44 +1,42 @@ package frc.robot.subsystems.vision; import java.util.ArrayList; -//import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Dashboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.vision.Cameras; -//import io.github.pseudoresonance.pixy2api.Pixy2CCC; import io.github.pseudoresonance.pixy2api.Pixy2CCC.Block; public class TargetHatch { - private static ArrayList blocks = Cameras.getPixyCamera().getPixy().getCCC().getBlocks(); - private static final int blockSignature = 1; - - public static void run(int count) { - if (count > 0) { - Block largestBlock = null; - // Checks for Biggest Block that is of the Orange Color Signature - for (Block block : blocks) { - if (block.getSignature() == blockSignature) { - if (largestBlock == null) { - largestBlock = block; - } else if (block.getWidth() > largestBlock.getWidth()) { - largestBlock = block; - } - } - } - - int ballX = largestBlock.getX(); - double yaw = ((ballX - 157.5) * 0.1904761905); - - // Information about the Big Orange Block is sent to NetworkTables for - // Shuffleboard - Dashboard.getInstance().putNumber(false, "Ball Angle", yaw); - Dashboard.getInstance().putNumber(false, "Ball X", largestBlock.getX()); - Dashboard.getInstance().putNumber(false, "Ball Y", largestBlock.getY()); - Dashboard.getInstance().putNumber(false, "Ball Box Width", largestBlock.getWidth()); - Dashboard.getInstance().putNumber(false, "Ball Box Height", largestBlock.getHeight()); + private static ArrayList blocks = Cameras.getPixyCamera().getPixy().getCCC().getBlocks(); + private static final int blockSignature = 1; + + public static void run(int count) { + if (count > 0) { + Block largestBlock = null; + // Checks for Biggest Block that is of the Orange Color Signature + for (Block block : blocks) { + if (block.getSignature() == blockSignature) { + if (largestBlock == null) { + largestBlock = block; + } else if (block.getWidth() > largestBlock.getWidth()) { + largestBlock = block; + } } - + } + + int ballX = largestBlock.getX(); + double yaw = ((ballX - 157.5) * 0.1904761905); + + // Information about the Big Orange Block is sent to NetworkTables for + // Shuffleboard + SmartDashboard.putNumber("Ball Angle", yaw); + SmartDashboard.putNumber("Ball X", largestBlock.getX()); + SmartDashboard.putNumber("Ball Y", largestBlock.getY()); + SmartDashboard.putNumber("Ball Box Width", largestBlock.getWidth()); + SmartDashboard.putNumber("Ball Box Height", largestBlock.getHeight()); } + } + } diff --git a/src/main/java/frc/robot/triggers/BooleanTrigger.java b/src/main/java/frc/robot/triggers/BooleanTrigger.java new file mode 100644 index 0000000..c657e5e --- /dev/null +++ b/src/main/java/frc/robot/triggers/BooleanTrigger.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.triggers; + +import edu.wpi.first.wpilibj.buttons.Trigger; +import java.util.function.BooleanSupplier;; + +/** + * Add your docs here. + */ +public class BooleanTrigger extends Trigger { + private BooleanSupplier condition; + + public BooleanTrigger(BooleanSupplier cond) { + condition = cond; + } + + @Override + public boolean get() { + return condition.getAsBoolean(); + } +}