diff --git a/.vscode/settings.json b/.vscode/settings.json index a286ad2..7847686 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -9,5 +9,11 @@ "bin/": true, ".classpath": true, ".project": true - } + }, + "wpilib.deployOffline": false, + + "editor.tabSize": 2, + "editor.insertSpaces": true, + "editor.detectIndentation": false, + "editor.formatOnSave": true } diff --git a/README.md b/README.md deleted file mode 100644 index 4f34467..0000000 --- a/README.md +++ /dev/null @@ -1,4 +0,0 @@ -# 2019-Deep-Space -2019 robot code - -JavaDocs http://first.wpi.edu/FRC/roborio/release/docs/java/edu/wpi/first/wpilibj/Relay.Value.html diff --git a/build.gradle b/build.gradle index ef5913a..68051bb 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.2.1" + id "edu.wpi.first.GradleRIO" version "2019.3.2" } sourceCompatibility = JavaVersion.VERSION_11 @@ -46,12 +46,14 @@ repositories { maven { url "https://raw.githubusercontent.com/momentumfrc/Momentum-Tools/master/mvn-repo" } + maven { url 'https://nexus.otake.pw/repository/maven-public/' } } // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { implementation group:'org.usfirst.frc.team4999', name: 'momentum-utils', version: '1.0' + compile 'pw.otake.pseudoresonance:pixy2-java-api:1.3.1' compile wpi.deps.wpilib() compile wpi.deps.vendor.java() nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) 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 9e8ba4b..ba2976e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,16 +7,29 @@ package frc.robot; +import com.kauailabs.navx.frc.AHRS; +import com.kauailabs.navx.frc.AHRS.SerialDataType; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +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.SpeedControllerGroup; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import frc.robot.commands.DriveCommand; -import frc.robot.commands.HatchPusherCommand; -import frc.robot.commands.WheelArmComand; +import frc.robot.commands.lift.LiftCommandGroup; +import frc.robot.commands.WheelArmCommand; +import frc.robot.commands.arm.ArmAngleCommand; import frc.robot.controls.*; import frc.robot.subsystems.*; +import frc.robot.subsystems.vision.Cameras; /** * The VM is configured to automatically run this class, and to call the @@ -26,43 +39,72 @@ * project. */ public class Robot extends TimedRobot { - public static Drive drive = new Drive(); + public static HatchPusher hatchPusher = new HatchPusher(); public static WheelArm wheelArm = new WheelArm(); public static LiftSystem liftSystem = new LiftSystem(); + public static ArmAngle armAngle = new ArmAngle(); public static DriveController controller = new F310Controller(); + public static Drive drive1 = new Drive(); public static OI m_oi; - Command m_autonomousCommand; - SendableChooser m_chooser = new SendableChooser<>(); + public AHRS ahrs; + + public static SpeedControllerGroup left = new SpeedControllerGroup(RobotMap.leftFront, RobotMap.leftBack); + public static SpeedControllerGroup right = new SpeedControllerGroup(RobotMap.rightFront, RobotMap.rightBack); + + public static DifferentialDrive drive = new DifferentialDrive(left, right); + + 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); + + VideoSource lifeCam = new UsbCamera("Driver Camera", 0); + lifeCam.setResolution(640, 480); + lifeCam.setFPS(15); + CameraServer.getInstance().startAutomaticCapture(lifeCam); + + Cameras.setup(); // Setup and Connection to Pixy2 + try { + 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() { + + if (ahrs != null) { + SmartDashboard.putData(ahrs); + } + + 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() { @@ -75,30 +117,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(); } /** @@ -111,15 +145,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(); + Scheduler.getInstance().removeAll(); + driveCommand.start(); + armCommand.start(); + liftCommand.start(); + armAngleCommand.start(); } /** @@ -128,6 +158,9 @@ public void teleopInit() { @Override public void teleopPeriodic() { Scheduler.getInstance().run(); + double move_request = 0; // controller.get + double turn_request = 0; // controller.getRawAxis(4); + drive.arcadeDrive(move_request, turn_request); } /** diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 1b9cdc8..b7abe8a 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -6,12 +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.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 @@ -19,35 +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(0);// TODO: Set to actual port numbers - public static final Spark rightFront = new Spark(1); - public static final Spark leftBack = new Spark(2); - public static final Spark rightBack = new Spark(3); - - public static final VictorSP wheelArmLeft = new VictorSP(4);;// TODO: Set to actual port numbers - public static final VictorSP wheelArmRight = new VictorSP(5); - public static final VictorSP wheelArmAngle = new VictorSP(6); - public static final VictorSP leftLiftWheel = new VictorSP(7); - public static final VictorSP rightLiftWheel = new VictorSP(8); - - public static final Encoder wheelArmEncoder = new Encoder(0,1,false);//TODO: Fix encoder constructor - - public static final DoubleSolenoid hatchSolenoidTop = new DoubleSolenoid(0,1); // TODO: set to actual solenoid values - public static final DoubleSolenoid hatchSolenoidLeft = new DoubleSolenoid(2,3); - public static final DoubleSolenoid hatchSolenoidRight = new DoubleSolenoid(4,5); //placeholder values - public static final DoubleSolenoid liftSystemBackLeft = new DoubleSolenoid(6,7); // Do we have this many? - public static final DoubleSolenoid liftSystemBackRight = new DoubleSolenoid(8,9); - public static final DoubleSolenoid liftSystemFrontLeft = new DoubleSolenoid(10,11); - public static final DoubleSolenoid liftSystemFrontRight = new DoubleSolenoid(12,13); - - public static final LogitechF310 controller1 = new LogitechF310(0);// TODO: PORT NUMBERS + 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 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 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..26dbd85 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -12,9 +12,12 @@ import frc.robot.controls.DriveController; public class DriveCommand extends Command { + private boolean reverse = false; + public DriveCommand() { - requires(Robot.drive); + super(); + requires(Robot.drive1); } // Called just before this Command runs the first time @@ -27,11 +30,14 @@ 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()); + + // Robot.drive1.arcadeDrive(moveRequest, controller.getTurnRequest()); + Robot.drive1.bad(); } // Make this return true when this Command no longer needs to run execute() @@ -43,11 +49,13 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { + // Robot.drive1.stop(); } // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { + // Robot.drive1.stop(); } } diff --git a/src/main/java/frc/robot/commands/WheelArmCommand.java b/src/main/java/frc/robot/commands/WheelArmCommand.java index 9e714aa..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,19 +30,14 @@ protected void initialize() { @Override protected void execute() { DriveController controller = Robot.controller; - if(Robot.wheelArm.isInwards()||Robot.wheelArm.isOutwards()){ - if(controller.getToggleInwards()||controller.getToggleOutwards()){ - Robot.WheelArm.stopArms(); - } - - } - if(controller.getToggleInwards()){ - Robot.WheelArm.spinInwards(); - } - if(controller.getToggleOutwards()){ - Robot.WheelArm.spinOutwards(); - } + if (controller.getWheelArmInwards()) { + arms.runInwards(); + } else if (controller.getWheelArmOutwards()) { + arms.runOutwards(); + } else { + arms.stopArms(); + } } // Make this return true when this Command no longer needs to run execute() @@ -49,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 86% rename from src/main/java/frc/robot/commands/ArmAngleCommand.java rename to src/main/java/frc/robot/commands/arm/ArmAngleCommand.java index 4884eb2..b1e2553 100644 --- a/src/main/java/frc/robot/commands/ArmAngleCommand.java +++ b/src/main/java/frc/robot/commands/arm/ArmAngleCommand.java @@ -5,14 +5,15 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.arm; import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; public class ArmAngleCommand extends Command { public ArmAngleCommand() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); + super(); + requires(Robot.armAngle); } // Called just before this Command runs the first time @@ -23,6 +24,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + Robot.armAngle.setSpeed(Robot.controller.getArmsSpeed()); } // Make this return true when this Command no longer needs to run execute() @@ -34,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/HatchPusherCommand.java b/src/main/java/frc/robot/commands/lift/EngageLiftSolenoidCommand.java similarity index 74% rename from src/main/java/frc/robot/commands/HatchPusherCommand.java rename to src/main/java/frc/robot/commands/lift/EngageLiftSolenoidCommand.java index c7f2ba4..6ce0de3 100644 --- a/src/main/java/frc/robot/commands/HatchPusherCommand.java +++ b/src/main/java/frc/robot/commands/lift/EngageLiftSolenoidCommand.java @@ -5,44 +5,44 @@ /* 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; 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); +public class EngageLiftSolenoidCommand extends Command { + Timer t = new Timer(); + private static final double SOLENOID_DELAY = 4.0; + + public EngageLiftSolenoidCommand() { + super(); + requires(Robot.liftSystem); } // 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.getToggleHatchPusher()){ - if(Robot.hatchPusher.isOut()){ - Robot.hatchPusher.retract(); - } - else{ - Robot.hatchPusher.extend(); - } - } + if (controller.getStartLift()) { + Robot.liftSystem.engageSolenoids(); + } } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return (t.get() >= SOLENOID_DELAY); } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/lift/LiftCommandGroup.java b/src/main/java/frc/robot/commands/lift/LiftCommandGroup.java new file mode 100644 index 0000000..f381b68 --- /dev/null +++ b/src/main/java/frc/robot/commands/lift/LiftCommandGroup.java @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* 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.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 LiftDriveForwardFront()); + addSequential(new LiftRetractFront()); + addSequential(new LiftDriveForwardBack()); + // addSequential(new LiftSystemEnd()); + } +} diff --git a/src/main/java/frc/robot/commands/lift/LiftDriveForwardBack.java b/src/main/java/frc/robot/commands/lift/LiftDriveForwardBack.java new file mode 100644 index 0000000..0ed8b5e --- /dev/null +++ b/src/main/java/frc/robot/commands/lift/LiftDriveForwardBack.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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.lift; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; +import edu.wpi.first.wpilibj.Timer; + +public class LiftDriveForwardBack extends Command { + Timer t = new Timer(); + private static final double DRIVE_DELAY = 3.0; + + public LiftDriveForwardBack() { + 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 + @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.hasPeriodPassed(DRIVE_DELAY); + } + + // 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/lift/LiftDriveForwardFront.java similarity index 75% rename from src/main/java/frc/robot/commands/LiftSystemCommand.java rename to src/main/java/frc/robot/commands/lift/LiftDriveForwardFront.java index f6fb04a..2e0b685 100644 --- a/src/main/java/frc/robot/commands/LiftSystemCommand.java +++ b/src/main/java/frc/robot/commands/lift/LiftDriveForwardFront.java @@ -5,33 +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 LiftSystemCommand extends Command { - public LiftSystemCommand() { +public class LiftDriveForwardFront extends Command { + Timer t = new Timer(); + 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() { - if(controller.getStartLift()) - Robot.LiftSystem.lift(); + Robot.liftSystem.driveForward(); + } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return t.hasPeriodPassed(DRIVE_DELAY); } // Called once after isFinished returns true @@ -43,5 +49,6 @@ protected void end() { // subsystems is scheduled to run @Override protected void interrupted() { + } } diff --git a/src/main/java/frc/robot/commands/lift/LiftRetractFront.java b/src/main/java/frc/robot/commands/lift/LiftRetractFront.java new file mode 100644 index 0000000..9f86379 --- /dev/null +++ b/src/main/java/frc/robot/commands/lift/LiftRetractFront.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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.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); + } + + // 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(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return t.hasPeriodPassed(RETRACT_DELAY); + } + + // 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/controls/DriveController.java b/src/main/java/frc/robot/controls/DriveController.java index b4cb103..6dd6fcf 100644 --- a/src/main/java/frc/robot/controls/DriveController.java +++ b/src/main/java/frc/robot/controls/DriveController.java @@ -11,12 +11,23 @@ * Add your docs here. */ public interface DriveController { - public double getMoveRequest(); - public double getTurnRequest(); - public double getSpeedLimit(); - public boolean getReverseDirection(); - public boolean getToggleHatchPusher(); - public boolean getToggleInwards(); - public boolean getToggleOutwards(); + + 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 85ae533..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,74 +16,78 @@ /** * Add your docs here. */ -public class F310Controller implements DriveController{ - private LogitechF310 controller1 = RobotMap.controller1; +public class F310Controller implements DriveController { + private LogitechF310 controller1 = RobotMap.controller1; - private static final double CURVE = 2; - private static final double DEADZONE = .03; + 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.getY(Hand.kRight); - speed = curve(speed, CURVE); - speed = deadzone(speed, DEADZONE); - return speed; + 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 getTurnRequest(){ - double speed = controller1.getX(Hand.kLeft); - speed = curve(speed, CURVE); - speed = deadzone(speed, DEADZONE); - return speed; + @Override + public double getMoveRequest() { + double speed = controller1.getRawAxis(1); + speed = curve(speed, CURVE); + speed = deadzone(speed, DEADZONE); + return speed; - } - @Override - public double getSpeedLimit(){ - if(controller1.getBackButtonPressed() && speedLimitIndex>0) - speedLimitIndex--; - if(controller1.getStartButtonPressed() && speedLimitIndex 0) + speedLimitIndex--; + if (controller1.getRawButtonPressed(8) && speedLimitIndex < speedLimits.length - 1) + speedLimitIndex++; + return speedLimits[speedLimitIndex]; + } - @Override - public boolean getToggleArmsUp(){ - return controller1.getYButtonPressed(); - } + @Override + public boolean getReverseDirection() { + return controller1.getRawButtonPressed(3); + } - @Override - public boolean getToggleArmsDown(){ - return controller1.getAButtonPressed(); - } - @Override - public boolean getStartLift(){ - return controller1.getTriggerAxis(hand kLeft)>.75&&controller1.getTriggerAxis(hand kRight)>.75; - + @Override + public boolean getToggleHatchPusher() { + return controller1.getRawButtonPressed(2); + } + + @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 51aaa1f..b7e7ac3 100644 --- a/src/main/java/frc/robot/subsystems/ArmAngle.java +++ b/src/main/java/frc/robot/subsystems/ArmAngle.java @@ -8,19 +8,29 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; - +import edu.wpi.first.wpilibj.PWMVictorSPX; +import frc.robot.RobotMap; /** * Add your docs here. */ public class ArmAngle extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. + 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 3923139..4d4cc52 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,22 +24,37 @@ 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){ - moveRequest = map(moveRequest, -1, 1, -speedLimiter, speedLimiter); - turnRequest = map(turnRequest, -1, 1, -speedLimiter, speedLimiter); - drive.arcadeDrive(moveRequest, turnRequest, false); + + /* + * 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); } + */ + public void arcadeDrive(double moveRequest, double turnRequest) { + + drive.arcadeDrive(moveRequest, turnRequest); } + + public void bad() { + leftMotors.set(.5); + rightMotors.set(.5); + } + + 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/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 7f5e1a4..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,24 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-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.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; - -/** - * An example subsystem. You can replace me with your own Subsystem. - */ -public class ExampleSubsystem extends Subsystem { - // 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()); - } -} diff --git a/src/main/java/frc/robot/subsystems/HatchPusher.java b/src/main/java/frc/robot/subsystems/HatchPusher.java index 453d8a9..d6a4ab9 100644 --- a/src/main/java/frc/robot/subsystems/HatchPusher.java +++ b/src/main/java/frc/robot/subsystems/HatchPusher.java @@ -20,36 +20,22 @@ public class HatchPusher extends Subsystem { // here. Call these from Commands. private DoubleSolenoid top = RobotMap.hatchSolenoidTop; - private DoubleSolenoid left = RobotMap.hatchSolenoidLeft; - private DoubleSolenoid right = RobotMap.hatchSolenoidRight; - private boolean out = false; -} - - public HatchPusher(){ + public HatchPusher() { super("Hatch Pusher"); addChild("Top Solenoid", top); - addChild("Left Solenoid", left); - addChild("Right Solenoid", right); } - public void extend(){ - out = true; + public void extend() { top.set(Value.kForward); - left.set(Value.kForward); - right.set(Value.kForward); - } - public void retract(){ - out = false; + public void retract() { top.set(Value.kReverse); - left.set(Value.kReverse); - right.set(Value.kReverse); } - public boolean isOut(){ - return out; + 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 7a90956..d350809 100644 --- a/src/main/java/frc/robot/subsystems/LiftSystem.java +++ b/src/main/java/frc/robot/subsystems/LiftSystem.java @@ -8,56 +8,51 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; +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 DoubleSolenoid frontLeft = RobotMap.liftSystemFrontRight; - private DoubleSolenoid frontRight = RobotMap.liftSystemFrontRight; - private DoubleSolenoid backLeft = RobotMap.liftSystemFrontRight; - private DoubleSolenoid backRight = RobotMap.liftSystemFrontRight; - private Timer t= new Timer(); + 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 HatchPusher(){ + public LiftSystem() { super("Lift System"); - addChild("Front Left Solenoid", frontLeft); - addChild("Front Right Solenoid", frontRight); - addChild("Back Left Solenoid", backLeft); - addChild("Back Right Solenoid", backRight); + addChild("Front Solenoid", front); + addChild("Back Solenoid", back); addChild("Left Lift Wheel", leftWheel); addChild("Right Lift Wheel", rightWheel); } - public void lift(){ - frontLeft.set(Value.kForward); - frontRight.set(Value.kForward); - backleft.set(Value.kForward); - backRight.set(Value.kForward); - t.delay(3.0); - leftWheel.set(Value.kForward); - rightWheel.set(Value.kForward); - t.delay(1.5); - frontLeft.set(Value.kBackward); - frontRight.set(Value.kBackward); - t.delay(2.5); - leftWheel.set(Value.kStop); - rightWheel.set(Value.kStop); - backLeft.set(Value.kBackward); - backRight.set(Value.kBackward); - - + public void engageSolenoids() { + front.set(Value.kForward); + back.set(Value.kForward); + } + public void driveForward() { + leftWheel.set(driveSpeed); + rightWheel.set(driveSpeed); + } + public void retractFrontSolenoids() { + front.set(Value.kReverse); + } + 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 c35648e..5fca4bb 100644 --- a/src/main/java/frc/robot/subsystems/WheelArm.java +++ b/src/main/java/frc/robot/subsystems/WheelArm.java @@ -7,50 +7,45 @@ package frc.robot.subsystems; +import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.command.Subsystem; import frc.robot.RobotMap; -private VictorSP left = RobotMap.wheelArmLeft; -private VictorSP right = RobotMap.wheelArmRight; - -private boolean inwards = false; -private boolean outwards = false; public class WheelArm extends Subsystem { + 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; -public WheelArm(){ + public WheelArm() { super("Wheel Arm"); addChild("Left Motor", left); addChild("Right Motor", right); - - } - public void spinInwards(){ - inwards = true; - left.set(Value.kForward);//Check if Forwards means clockwise - right.set(Value.kReverse); + left.setInverted(false); + right.setInverted(true); } - public void spinOutwards(){ - outwards = true; - left.set(Value.kReverse); - right.set(Value.kForward); + public void stopArms() { + left.set(0); + right.set(0); } - public void stopArms(){ - inwards = false; - outwards = false; - left.set(Value.kOff); - right.set(Value.kOff) - - - } - - public boolean isInwards(){ - return inwards; + public void runInwards() { + left.set(WHEEL_ARM_INTAKE); + right.set(WHEEL_ARM_INTAKE); } - public boolean isOutwards(){ - return outwards; + + public void runOutwards() { + left.set(WHEEL_ARM_OUTTAKE); + right.set(WHEEL_ARM_OUTTAKE); } + + /* + * 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 new file mode 100644 index 0000000..ecfa49b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Cameras.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.cscore.UsbCamera; +import edu.wpi.first.cameraserver.CameraServer; +import io.github.pseudoresonance.pixy2api.links.SPILink; + +public class Cameras { + + 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/PixyCamera.java b/src/main/java/frc/robot/subsystems/vision/PixyCamera.java new file mode 100644 index 0000000..e4edd17 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/PixyCamera.java @@ -0,0 +1,67 @@ +package frc.robot.subsystems.vision; + + + +import io.github.pseudoresonance.pixy2api.Pixy2; +import io.github.pseudoresonance.pixy2api.Pixy2CCC; +//import io.github.pseudoresonance.pixy2api.Pixy2CCC.Block; +import io.github.pseudoresonance.pixy2api.links.Link; + +public class PixyCamera { + + private final Pixy2 pixy; + private boolean laststate = false; + public final static int PixyResult = 0; + + public PixyCamera(Link link) { + pixy = Pixy2.createInstance(link); + pixy.init(); + } + + public PixyCamera(Link link, int arg) { + pixy = Pixy2.createInstance(link); + pixy.init(arg); + } + + public void run() { + + final int pixystatus = pixy.init(PixyResult); // Checks for Pixy2 Error (If issue, Robot is Not Effected) + if (pixystatus == 0) { + + final int count = pixy.getCCC().getBlocks(false, Pixy2CCC.CCC_SIG1, 25); + + TargetBall.run(count); // Track Orange Ball Code Run + + + // final int countt = pixy.getCCC().getBlocks(false, Pixy2CCC.CCC_SIG3, 25); + + // TargetHatch.run(countt); // Track Orange Ball Code Run + + } + + // pixy.getCCC().getBlocks(false, Pixy2CCC.CCC_SIG1, 25); + // ArrayList blocks = pixy.getCCC().getBlocks(); + // pixy.getCCC().getBlocks(false, Pixy2CCC.CCC_SIG1, 25); + + } + + public void light(boolean state) { + final int pixystatus = pixy.init(PixyResult); // Checks for Pixy2 Error (If issue, Robot is Not Effected) + if (pixystatus == 0) { + + if ((state == true) && (laststate == false)) { // Pixy2 Lamp Turns On/Off w/ Shuffleboard Button + laststate = true; + pixy.setLamp((byte) 1, (byte) 0); + } else if (state == false && laststate == true) { + laststate = false; + pixy.setLamp((byte) 0, (byte) 0); + } + + } + } + + public Pixy2 getPixy() { + 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 new file mode 100644 index 0000000..01319b6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/TargetBall.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.vision; + +import java.util.ArrayList; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.vision.Cameras; +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(); + // 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 new file mode 100644 index 0000000..efe5a7c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/TargetHatch.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.vision; + +import java.util.ArrayList; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.vision.Cameras; +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 + 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(); + } +} diff --git a/src/main/java/org/usfirst/frc/team4999/controllers/LogitechF310.java b/src/main/java/org/usfirst/frc/team4999/controllers/LogitechF310.java index e8f8802..ce4b81d 100644 --- a/src/main/java/org/usfirst/frc/team4999/controllers/LogitechF310.java +++ b/src/main/java/org/usfirst/frc/team4999/controllers/LogitechF310.java @@ -125,6 +125,10 @@ public boolean getBackButtonPressed() { public boolean getBackButtonReleased() { return getRawButtonReleased(BACK); } + + public double getArmsSpeed() { + return 0; + } } diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..2f05720 --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,135 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.13.0", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.13.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.13.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.13.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.13.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.13.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.13.0", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.13.0", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.13.0", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers" + } + ] +} \ No newline at end of file diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json new file mode 100644 index 0000000..49c7d70 --- /dev/null +++ b/vendordeps/navx_frc.json @@ -0,0 +1,33 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "3.1.366", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2019/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "3.1.366" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "3.1.366", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file