diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..e866bb8 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,8 @@ +language: java +script: + - cd Robot2019/ + - ./gradlew check +jdk: + - oraclejdk11 +env: + - TERM=dumb \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..7e0a6e0 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,27 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "type": "java", + "name": "Debug (Launch) - Current File", + "request": "launch", + "mainClass": "${file}" + }, + { + "type": "java", + "name": "Debug (Launch)-Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "Robot2019" + }, + { + "type": "java", + "name": "Debug (Launch)-DrivetrainCharacterization", + "request": "launch", + "mainClass": "DrivetrainCharacterization" + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..c5f3f6b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "interactive" +} \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..f0ee648 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "currentLanguage": "none", + "enableCppIntellisense": false, + "projectYear": "none", + "teamNumber": 199 +} \ No newline at end of file diff --git a/README.md b/README.md index 7fd03a2..6f1a754 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,3 @@ -# RobotCode2019 +# RobotCode2019 [![Build Status](https://travis-ci.com/DeepBlueRobotics/RobotCode2019.svg?branch=dev)](https://travis-ci.com/DeepBlueRobotics/RobotCode2019) + Our code for 2019 FRC Build Season. diff --git a/Robot2019/.wpilib/wpilib_preferences.json b/Robot2019/.wpilib/wpilib_preferences.json index 8290745..a7431db 100644 --- a/Robot2019/.wpilib/wpilib_preferences.json +++ b/Robot2019/.wpilib/wpilib_preferences.json @@ -1,4 +1,6 @@ { + "enableCppIntellisense": false, "currentLanguage": "java", + "projectYear": "2019", "teamNumber": 199 } \ No newline at end of file diff --git a/Robot2019/build.gradle b/Robot2019/build.gradle index d1082e4..a40819e 100644 --- a/Robot2019/build.gradle +++ b/Robot2019/build.gradle @@ -1,54 +1,64 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.0.0-alpha-3" + id "edu.wpi.first.GradleRIO" version "2019.3.2" } -def ROBOT_CLASS = "frc.robot.Robot" +def ROBOT_MAIN_CLASS = "frc.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project EmbeddedTools. deploy { targets { - target("roborio", edu.wpi.first.gradlerio.frc.RoboRIO) { + roboRIO("roborio") { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = getTeamNumber() + team = frc.getTeamNumber() } } artifacts { - artifact('frcJava', edu.wpi.first.gradlerio.frc.FRCJavaArtifact) { + frcJavaArtifact('frcJava') { targets << "roborio" // Debug can be overridden by command line, for use with VSCode - debug = getDebugOrDefault(false) + debug = frc.getDebugOrDefault(false) + } + // Built in artifact to deploy arbitrary files to the roboRIO. + fileTreeArtifact('frcStaticFileDeploy') { + // The directory below is the local directory to deploy + files = fileTree(dir: 'src/main/deploy') + // Deploy to RoboRIO target, into /home/lvuser/deploy + targets << "roborio" + directory = '/home/lvuser/deploy' } } } -// Defining my dependencies. In this case, WPILib (+ friends), CTRE Toolsuite (Talon SRX) -// and NavX. +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +// Maven central needed for JUnit +repositories { + mavenCentral() +} + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. dependencies { - compile wpilib() - compile ctre() - compile navx() + compile wpi.deps.wpilib() + compile wpi.deps.vendor.java() + nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) + testCompile 'junit:junit:4.12' + compile group: 'org.apache.commons', name: 'commons-math3', version: '3.6.1' + compile group: 'org.apache.commons', name: 'commons-lang3', version: '3.8.1' + compile group: 'org.apache.commons', name: 'commons-csv', version: '1.6' } // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_CLASS) -} - -// Force Java 8 Compatibility mode for deployed code, in case the develoment -// system is using Java 10. -compileJava { - sourceCompatibility = 1.8 - targetCompatibility = 1.8 -} - -wrapper { - gradleVersion = '4.9' + from { configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) } diff --git a/Robot2019/gradle/wrapper/gradle-wrapper.jar b/Robot2019/gradle/wrapper/gradle-wrapper.jar index 0d4a951..457aad0 100644 Binary files a/Robot2019/gradle/wrapper/gradle-wrapper.jar and b/Robot2019/gradle/wrapper/gradle-wrapper.jar differ diff --git a/Robot2019/gradle/wrapper/gradle-wrapper.properties b/Robot2019/gradle/wrapper/gradle-wrapper.properties index a95009c..d08253c 100644 --- a/Robot2019/gradle/wrapper/gradle-wrapper.properties +++ b/Robot2019/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME -distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-4.9-bin.zip +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-5.0-bin.zip zipStoreBase=GRADLE_USER_HOME -zipStorePath=wrapper/dists +zipStorePath=permwrapper/dists diff --git a/Robot2019/gradlew b/Robot2019/gradlew old mode 100644 new mode 100755 index cccdd3d..af6708f --- a/Robot2019/gradlew +++ b/Robot2019/gradlew @@ -28,7 +28,7 @@ APP_NAME="Gradle" APP_BASE_NAME=`basename "$0"` # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS="" +DEFAULT_JVM_OPTS='"-Xmx64m"' # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD="maximum" diff --git a/Robot2019/gradlew.bat b/Robot2019/gradlew.bat index f955316..6d57edc 100644 --- a/Robot2019/gradlew.bat +++ b/Robot2019/gradlew.bat @@ -14,7 +14,7 @@ set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -set DEFAULT_JVM_OPTS= +set DEFAULT_JVM_OPTS="-Xmx64m" @rem Find java.exe if defined JAVA_HOME goto findJavaFromJavaHome diff --git a/Robot2019/settings.gradle b/Robot2019/settings.gradle index 3b58236..b0f4d48 100644 --- a/Robot2019/settings.gradle +++ b/Robot2019/settings.gradle @@ -1,6 +1,25 @@ +import org.gradle.internal.os.OperatingSystem + pluginManagement { repositories { mavenLocal() gradlePluginPortal() + String frcYear = '2019' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + frcHome = new File(publicFolder, "frc${frcYear}") + } else { + def userFolder = System.getProperty("user.home") + frcHome = new File(userFolder, "frc${frcYear}") + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } } } diff --git a/Robot2019/src/main/deploy/example.txt b/Robot2019/src/main/deploy/example.txt new file mode 100644 index 0000000..70c79b6 --- /dev/null +++ b/Robot2019/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/DrivetrainCharAnalysis.java b/Robot2019/src/main/java/frc/robot/DrivetrainCharAnalysis.java new file mode 100644 index 0000000..f9116e7 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/DrivetrainCharAnalysis.java @@ -0,0 +1,222 @@ +package frc.robot; + +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.io.FileWriter; +import java.io.IOException; +import java.io.Reader; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import org.apache.commons.math3.stat.regression.OLSMultipleLinearRegression; +import org.apache.commons.math3.linear.SingularMatrixException; +import org.apache.commons.lang3.ArrayUtils; +import org.apache.commons.csv.CSVRecord; +import org.apache.commons.csv.CSVFormat; +import org.apache.commons.csv.CSVParser; + +public class DrivetrainCharAnalysis { + // Ordinary Least Squares approach (multi-variable approach) + public static void ordinaryLeastSquares(String file1, String file2, String outfile) { + double leftKv, rightKv; + double leftKa, rightKa; + double leftVoltageIntercept, rightVoltageIntercept; + int spread = 30; + + double[][] returns; + double[] params, leftVelocities, rightVelocities, voltages, leftAccelerations, rightAccelerations; + + returns = parseCSV(file1, spread); + leftVelocities = returns[0]; + rightVelocities = returns[1]; + voltages = returns[2]; + leftAccelerations = returns[3]; + rightAccelerations = returns[4]; + + returns = parseCSV(file2, spread); + leftVelocities = ArrayUtils.addAll(leftVelocities, returns[0]); + rightVelocities = ArrayUtils.addAll(rightVelocities, returns[1]); + voltages = ArrayUtils.addAll(voltages, returns[2]); + leftAccelerations = ArrayUtils.addAll(leftAccelerations, returns[3]); + rightAccelerations = ArrayUtils.addAll(rightAccelerations, returns[4]); + + double[][] xs = new double[voltages.length][2]; + double[] ys = new double[voltages.length]; + for (int i = 0; i < voltages.length; i++) { + xs[i][0] = leftVelocities[i]; + xs[i][1] = leftAccelerations[i]; + ys[i] = voltages[i]; + } + + OLSMultipleLinearRegression algorithm = new OLSMultipleLinearRegression(); + algorithm.newSampleData(ys, xs); + params = algorithm.estimateRegressionParameters(); + // System.out.println(params.length); + leftKv = params[1]; + leftKa = params[2]; + leftVoltageIntercept = params[0]; + + xs = new double[voltages.length][2]; + ys = new double[voltages.length]; + for (int i = 0; i < voltages.length; i++) { + xs[i][0] = rightVelocities[i]; + xs[i][1] = rightAccelerations[i]; + ys[i] = voltages[i]; + } + + try { + algorithm = new OLSMultipleLinearRegression(); + algorithm.newSampleData(ys, xs); + params = algorithm.estimateRegressionParameters(); + } catch (SingularMatrixException e) { + System.out.println(Arrays.deepToString(xs)); + System.out.println(Arrays.toString(ys)); + } + // System.out.println(params.length); + rightKv = params[1]; + rightKa = params[2]; + rightVoltageIntercept = params[0]; + + FileWriter f; + try { + f = new FileWriter(new File(outfile), true); + f.append(leftKv + "," + leftKa + "," + leftVoltageIntercept + "\r\n"); + f.append(rightKv + "," + rightKa + "," + rightVoltageIntercept + "\r\n"); + f.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + // OUTDATED + // Simplistic approach to the problem. + public static void simpleRegression(String file1, String file2, String outfile) { + // Argument 1 should be the filepath of the linear increase CSV file. + // Argument 2 should be the filepath of the stepwise increase CSV file. + double kv = 0.0; // The parameter for velocity in the drivetrain characterization formula + double ka = 0.0; // The parameter for acceleration in the drivetrain characterization formula + double voltageIntercept = 0.0; + int spread = 18; // How much data should our difference quotient cover? + + double[][] returns; + double[] params, velocities, voltages, accelerations, stepwise_x; + + returns = parseCSV(file1, spread); + velocities = returns[0]; + voltages = returns[1]; + accelerations = returns[2]; + + // System.out.println(linearVelocities); + params = simpleRegressionFormula(voltages, velocities); + kv = 1 / params[1]; // Voltseconds / inches + voltageIntercept = -params[0] / params[1]; // Think of this as -(b/m) in y = mx + b + + returns = parseCSV(file2, spread); + velocities = returns[0]; + voltages = returns[1]; + accelerations = returns[2]; + stepwise_x = velocities; // To define the size of stepwise_x + + for (int i = 0; i < velocities.length; i++) { + stepwise_x[i] = voltages[i] - (kv * velocities[i] + voltageIntercept); + } + + params = simpleRegressionFormula(stepwise_x, accelerations); + ka = 1 / params[1]; // Volt * (seconds^2) / inches. + + System.out.print("Velocity Constant is " + Double.toString(12 * kv) /* Change inches to feet in printout */); + System.out.print(" and the Acceleration Constant is " + Double.toString(12 * ka)); + System.out.print(" with a voltage intercept of " + Double.toString(voltageIntercept) + "\n"); + + FileWriter f; + try { + f = new FileWriter(new File(outfile)); + f.append(kv + "," + ka + "," + voltageIntercept); + f.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + public static double[] simpleRegressionFormula(double[] xs, double[] ys) { + double sx, sxx, sy, sxy, a, b; + sx = sxx = sy = sxy = a = b = 0.0; + int n = xs.length; + + for (int i = 0; i < n; i++) { + sx += xs[i]; + sxx += xs[i] * xs[i]; + sxy += xs[i] * ys[i]; + sy += ys[i]; + } + + b = (n * sxy - sx * sy) / (n * sxx - sx * sx); + a = (sy - b * sx) / n; + + double[] params = { a, b }; + return params; + } + + public static double[][] parseCSV(String filename, int spread) { + try { + Reader in = new FileReader(filename); + CSVParser csvParser = new CSVParser(in, CSVFormat.DEFAULT); + + List records = csvParser.getRecords(); + int num_values = records.size(); + List leftVelocities, rightVelocities, voltages, leftAccelerations, rightAccelerations; + leftVelocities = new ArrayList(); + rightVelocities = new ArrayList(); + voltages = new ArrayList(); + leftAccelerations = new ArrayList(); + rightAccelerations = new ArrayList(); + leftAccelerations.add(0.0); + rightAccelerations.add(0.0); + + for (int n = 0; n < num_values; n++) { + CSVRecord record = records.get(n); + if (!record.get(0).equals("Timestamp (s)")) { + double v1 = Math.abs(Double.valueOf(record.get(2))); + double v2 = Math.abs(Double.valueOf(record.get(3))); + double v = Double.valueOf(record.get(1)); + + if (leftVelocities.size() >= spread) { + double a1 = (v1 - leftVelocities.get(leftVelocities.size() - spread)) / (spread * 0.02); + double a2 = (v2 - rightVelocities.get(rightVelocities.size() - spread)) / (spread * 0.02); + leftAccelerations.add(Math.abs(a1)); + rightAccelerations.add(Math.abs(a2)); + } + + if (leftVelocities.size() < num_values - spread) { + leftVelocities.add(v1); + rightVelocities.add(v2); + voltages.add(v); + } + } + } + + Double[][] Returns = { leftVelocities.toArray(new Double[leftVelocities.size()]), + rightVelocities.toArray(new Double[rightVelocities.size()]), + voltages.toArray(new Double[voltages.size()]), + leftAccelerations.toArray(new Double[leftAccelerations.size()]), + rightAccelerations.toArray(new Double[rightAccelerations.size()]) }; + + double[][] returns = new double[5][voltages.size()]; + for (int i = 0; i < 5; i++) { + for (int j = 0; j < voltages.size(); j++) { + returns[i][j] = Returns[i][j]; + } + } + + return returns; + } catch (FileNotFoundException fnf) { + fnf.printStackTrace(); + return null; + } catch (IOException io) { + io.printStackTrace(); + return null; + } + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/Main.java b/Robot2019/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..a64a8d5 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/Main.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to change + * the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

+ * If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index 5d57329..06eec8e 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/Robot2019/src/main/java/frc/robot/OI.java @@ -7,36 +7,101 @@ package frc.robot; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.cscore.VideoSink; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.JoystickButton; +import frc.robot.commands.ToggleClimberRails; +import frc.robot.commands.Climb; +import frc.robot.commands.EjectCargo; +import frc.robot.commands.ToggleHatchEject; +import frc.robot.commands.IntakeCargo; +import frc.robot.commands.ToggleHatchIntake; +import frc.robot.commands.ManualClimb; +import frc.robot.commands.NormalDrive; +import frc.robot.commands.ResetWobble; +import frc.robot.commands.SetArcadeOrTank; +import frc.robot.commands.SlowDrive; +import frc.robot.commands.ToggleCamera; +import frc.robot.commands.ToggleLight; +import frc.robot.commands.WobbleDrive; +import frc.robot.subsystems.Cargo; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.HatchPanel; +import frc.robot.subsystems.Lights; + /** * 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. */ public class OI { - //// CREATING BUTTONS - // One type of button is a joystick button which is any button on a - //// joystick. - // You create one by telling it which joystick it's on and which button - // number it is. - // Joystick stick = new Joystick(port); - // Button button = new JoystickButton(stick, buttonNumber); - - // There are a few additional built in buttons you can use. Additionally, - // by subclassing Button you can create custom triggers and bind those to - // commands the same as any other Button. - - //// TRIGGERING COMMANDS WITH BUTTONS - // Once you have a button, it's trivial to bind it to a button in one of - // three ways: - - // Start the command when the button is pressed and let it run the command - // until it is finished as determined by it's isFinished method. - // button.whenPressed(new ExampleCommand()); - - // Run the command while the button is being held down and interrupt it once - // the button is released. - // button.whileHeld(new ExampleCommand()); - - // 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()); + Joystick leftJoy, rightJoy, manipulator; + + JoystickButton leftSlowBtn, rightSlowBtn; + JoystickButton arcadeOrTankBtn; + JoystickButton normDriveBtn; + JoystickButton hatchIntakeBtn, hatchEjectBtn; + JoystickButton cargoIntakeBtn, cargoEjectBtn; + JoystickButton climberRailBtn; + JoystickButton autoClimbBtn; + JoystickButton manualClimbBtn; + JoystickButton toggleCameraBtn; + JoystickButton wobbleDriveBtn; + JoystickButton cycleLightBtn; + + OI(Drivetrain dt, HatchPanel hp, Cargo cargo, Climber climber, Lights lights, UsbCamera driveCamera, + UsbCamera hatchCamera, VideoSink cameraServer) { + leftJoy = new Joystick(0); + rightJoy = new Joystick(1); + manipulator = new Joystick(2); + + leftSlowBtn = new JoystickButton(leftJoy, 1); + leftSlowBtn.whileHeld(new SlowDrive(SlowDrive.Side.LEFT)); + rightSlowBtn = new JoystickButton(rightJoy, 1); + rightSlowBtn.whileHeld(new SlowDrive(SlowDrive.Side.RIGHT)); + wobbleDriveBtn = new JoystickButton(rightJoy, 4); // TODO: confirm button with drivers + wobbleDriveBtn.whenPressed(new WobbleDrive(dt)); + wobbleDriveBtn.whenReleased(new ResetWobble(dt)); + + arcadeOrTankBtn = new JoystickButton(leftJoy, 4); + arcadeOrTankBtn.whenPressed(new SetArcadeOrTank()); + normDriveBtn = new JoystickButton(leftJoy, 3); + normDriveBtn.whileHeld(new NormalDrive()); + + hatchIntakeBtn = new JoystickButton(manipulator, Manip.X); + hatchIntakeBtn.whenPressed(new ToggleHatchIntake(hp)); + hatchEjectBtn = new JoystickButton(manipulator, Manip.Y); + hatchEjectBtn.whenPressed(new ToggleHatchEject(hp)); + + cargoIntakeBtn = new JoystickButton(manipulator, Manip.A); // TODO: set ports to correct values + cargoIntakeBtn.whenPressed(new IntakeCargo(cargo)); + cargoEjectBtn = new JoystickButton(manipulator, Manip.B); // TODO: set ports to correct values + cargoEjectBtn.whenPressed(new EjectCargo(cargo)); + + climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder); + climberRailBtn.whenPressed(new ToggleClimberRails(climber)); + + autoClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger); + autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy)); + + manualClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger); + manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator)); + + toggleCameraBtn = new JoystickButton(leftJoy, 2); + toggleCameraBtn.whenPressed(new ToggleCamera(driveCamera, hatchCamera, cameraServer)); + + cycleLightBtn = new JoystickButton(manipulator, Manip.START); + cycleLightBtn.whenPressed(new ToggleLight(lights)); + } + + private class Manip { + static final int X = 1, A = 2, B = 3, Y = 4, LB_lShoulder = 5, RB_rShoulder = 6, LT_lTrigger = 7, RT_rTrigger = 8, + BACK = 9, START = 10; + + // Front four buttons look like: + // Y + // X B + // A + } } diff --git a/Robot2019/src/main/java/frc/robot/Robot.java b/Robot2019/src/main/java/frc/robot/Robot.java index 89aa744..7481b38 100644 --- a/Robot2019/src/main/java/frc/robot/Robot.java +++ b/Robot2019/src/main/java/frc/robot/Robot.java @@ -8,58 +8,72 @@ package frc.robot; 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.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.commands.DrivetrainAnalysis; +import frc.robot.commands.IncreaseVoltageLinear; +import frc.robot.commands.IncreaseVoltageStepwise; +import frc.robot.commands.TeleopDrive; +import frc.robot.subsystems.Cargo; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.HatchPanel; +import frc.robot.subsystems.Lights; -/** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to each mode, as described in the TimedRobot - * documentation. If you change the name of this class or the package after - * creating this project, you must also update the build.gradle file in the - * project. - */ public class Robot extends TimedRobot { - public static ExampleSubsystem m_subsystem = new ExampleSubsystem(); - public static OI m_oi; + private static Drivetrain dt; + private static HatchPanel hp; + private static OI oi; + private static Cargo cargo; + private static Climber climber; + private static Lights lights; + private static String fname1, fname2, fname3, fname4; - Command m_autonomousCommand; - SendableChooser m_chooser = new SendableChooser<>(); - - /** - * 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(); - m_chooser.addDefault("Default Auto", new ExampleCommand()); - // chooser.addObject("My Auto", new MyAutoCommand()); - SmartDashboard.putData("Auto mode", m_chooser); + dt = new Drivetrain(RobotMap.leftMaster, RobotMap.leftSlave1, RobotMap.leftSlave2, RobotMap.rightMaster, + RobotMap.rightSlave1, RobotMap.rightSlave2, RobotMap.leftEnc, RobotMap.rightEnc, RobotMap.ahrs); + hp = new HatchPanel(RobotMap.hatchGrabberPiston, RobotMap.hatchEjectPistons); + cargo = new Cargo(RobotMap.cargoRoller, RobotMap.pdp, RobotMap.cargoPDPPort); + climber = new Climber(RobotMap.climberMotor, RobotMap.climberEncoder, RobotMap.ahrs, RobotMap.climberPistons); + lights = new Lights(RobotMap.lights); + oi = new OI(dt, hp, cargo, climber, lights, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer); + + fname1 = "/home/lvuser/drive_char_linear_for.csv"; + fname2 = "/home/lvuser/drive_char_stepwise_for.csv"; + fname3 = "/home/lvuser/drive_char_linear_back.csv"; + fname4 = "/home/lvuser/drive_char_stepwise_back.csv"; + IncreaseVoltageLinear ivlf = new IncreaseVoltageLinear(dt, 0.25 / 50, 6.0, fname1, "forward"); + IncreaseVoltageStepwise ivsf = new IncreaseVoltageStepwise(dt, 0.25 / 50, 6.0, fname2, "forward"); + IncreaseVoltageLinear ivlb = new IncreaseVoltageLinear(dt, 0.25 / 50, 6.0, fname3, "backward"); + IncreaseVoltageStepwise ivsb = new IncreaseVoltageStepwise(dt, 0.25 / 50, 6.0, fname4, "backward"); + DrivetrainAnalysis dca = new DrivetrainAnalysis(dt); + SmartDashboard.putData("Increase Voltage Linearly Forward", ivlf); + SmartDashboard.putData("Increase Voltage Stepwise Forward", ivsf); + SmartDashboard.putData("Increase Voltage Linearly Backward", ivlb); + SmartDashboard.putData("Increase Voltage Stepwise Backward", ivsb); + SmartDashboard.putData("Drivetrain Characterization Analysis", dca); + + dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy)); + SmartDashboard.putNumber("Max Acceleration", dt.getMaxSpeed() / 1.0); } /** - * 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() { } - /** - * 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() { + cargo.stopIntake(); } @Override @@ -67,37 +81,10 @@ public void disabledPeriodic() { Scheduler.getInstance().run(); } - /** - * 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 - * - *

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(); - } } - /** - * This function is called periodically during autonomous. - */ @Override public void autonomousPeriodic() { Scheduler.getInstance().run(); @@ -109,22 +96,13 @@ public void teleopInit() { // 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(); - } } - /** - * This function is called periodically during operator control. - */ @Override public void teleopPeriodic() { Scheduler.getInstance().run(); } - /** - * This function is called periodically during test mode. - */ @Override public void testPeriodic() { } diff --git a/Robot2019/src/main/java/frc/robot/RobotMap.java b/Robot2019/src/main/java/frc/robot/RobotMap.java index 45360e4..bea8cbf 100644 --- a/Robot2019/src/main/java/frc/robot/RobotMap.java +++ b/Robot2019/src/main/java/frc/robot/RobotMap.java @@ -7,6 +7,24 @@ package frc.robot; +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.cscore.UsbCamera; +import edu.wpi.cscore.VideoSink; +import edu.wpi.cscore.VideoSource; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PowerDistributionPanel; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.VictorSP; + /** * 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 @@ -14,13 +32,132 @@ * 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; + static WPI_TalonSRX leftMaster, rightMaster; + static BaseMotorController leftSlave1, leftSlave2, rightSlave1, rightSlave2; + static VictorSP climberMotor; + static Encoder climberEncoder; + static DoubleSolenoid climberPistons; + static DoubleSolenoid hatchGrabberPiston, hatchEjectPistons; + static VictorSP cargoRoller; + static Encoder leftEnc, rightEnc; + static String driveMode; + static VictorSP lights; + static AHRS ahrs; + static PowerDistributionPanel pdp; + static UsbCamera driveCamera, hatchCamera; + static VideoSink cameraServer; + + final static int cargoPDPPort; + final static int climberPDPPort; + + static { + // Initialize motors on the left side of the drivetrain. + leftMaster = createConfiguredTalon(8); + leftSlave1 = createConfiguredMotorController(9); + leftSlave2 = createConfiguredMotorController(10); + + // Initialize motors on the right side of the drivetrain. + rightMaster = createConfiguredTalon(5); + rightSlave1 = createConfiguredMotorController(6); + rightSlave2 = createConfiguredMotorController(7); + + // Initialize motors on the climbing mech + climberMotor = new VictorSP(1); + climberEncoder = new Encoder(new DigitalInput(4), new DigitalInput(5)); + climberPistons = new DoubleSolenoid(5, 2); + + // Initialize motors on the cargo mech + cargoRoller = new VictorSP(0); + + // Initialize solenoid on hatch panel mech + hatchGrabberPiston = new DoubleSolenoid(7, 0); // 7 is A/Forward, 0 is B/Reverse + hatchEjectPistons = new DoubleSolenoid(6, 1); + + leftEnc = new Encoder(new DigitalInput(0), new DigitalInput(1)); + rightEnc = new Encoder(new DigitalInput(2), new DigitalInput(3)); + + // Initialize lights + lights = new VictorSP(3); // TODO: Set this to correct port + + ahrs = new AHRS(SPI.Port.kMXP); + pdp = new PowerDistributionPanel(); + + // Initialize cameras + driveCamera = configureCamera(0); + hatchCamera = configureCamera(1); + cameraServer = CameraServer.getInstance().getServer(); + cameraServer.setSource(driveCamera); + + cargoPDPPort = 11; + climberPDPPort = 3; + } + + private static BaseMotorController createConfiguredMotorController(int port) { + BaseMotorController mc = new WPI_VictorSPX(port); + + // Put all configurations for the talon motor controllers in here. + // All values are from last year's code. + ErrorCode e = mc.configNominalOutputForward(0, 10); + if (e == ErrorCode.OK) { + mc = createConfiguredVictor(port); + } else { + mc = createConfiguredTalon(port); + } + + return mc; + } + + private static WPI_TalonSRX createConfiguredTalon(int port) { + WPI_TalonSRX tsrx = new WPI_TalonSRX(port); + ErrorCode ecDeadband; + + // Put all configurations for the talon motor controllers in here. + // All values are from last year's code. + catchError(tsrx.configNominalOutputForward(0, 10)); + catchError(tsrx.configNominalOutputReverse(0, 10)); + catchError(tsrx.configPeakOutputForward(1, 10)); + catchError(tsrx.configPeakOutputReverse(-1, 10)); + catchError(tsrx.configPeakCurrentLimit(0, 0)); + catchError(tsrx.configPeakCurrentDuration(0, 0)); + // 40 Amps is the amp limit of a CIM. lThe PDP has 40 amp circuit breakers, + catchError(tsrx.configContinuousCurrentLimit(40, 0)); + tsrx.enableCurrentLimit(true); + catchError(tsrx.configNeutralDeadband(0.001, 10)); + tsrx.setNeutralMode(NeutralMode.Brake); + + return tsrx; + } + + private static WPI_VictorSPX createConfiguredVictor(int port) { + WPI_VictorSPX vspx = new WPI_VictorSPX(port); + ErrorCode ecDeadband; + + // Put all configurations for the victor motor controllers in here. + catchError(vspx.configNominalOutputForward(0, 10)); + catchError(vspx.configNominalOutputReverse(0, 10)); + catchError(vspx.configPeakOutputForward(1, 10)); + catchError(vspx.configPeakOutputReverse(-1, 10)); + catchError(vspx.configNeutralDeadband(0.001, 10)); + vspx.setNeutralMode(NeutralMode.Brake); + + return vspx; + } + + private static UsbCamera configureCamera(int port) { + UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(port); + camera.setConnectionStrategy(VideoSource.ConnectionStrategy.kKeepOpen); + return camera; + } + + /** + * Checks an error code and prints it to standard out if it is not ok + * + * @param ec The error code to check + */ + public static void catchError(ErrorCode ec) { + if(ec != ErrorCode.OK) { + System.out.println("Error configuring in RobotMap.java at line: " + new Throwable().getStackTrace()[1].getLineNumber()); + System.out.println(ec.toString()); + } + } } diff --git a/Robot2019/src/main/java/frc/robot/commands/Climb.java b/Robot2019/src/main/java/frc/robot/commands/Climb.java new file mode 100644 index 0000000..e02bf97 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/Climb.java @@ -0,0 +1,115 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Drivetrain; +import edu.wpi.first.wpilibj.Joystick; + +public class Climb extends Command { + private Climber climber; + private Drivetrain dt; + private Joystick dtJoy; + private State state; + + private final double backDrive = -0.5; // TODO: Set this to reasonable/tested value; + private final double climbUp = 1; + private final double climbDown = -1; + private final double retract = 0.25; + private final double overrideThreshold = 0.1; // TODO: Set this to reasonable/tested value; + private final double retractGoal = 15; // This only needs to be off the ground. Climb is 19 inches. + private final double offGroundHeight = 10; + + public Climb(Climber climber, Drivetrain dt, Joystick joy) { + // Use requires() here to declare subsystem dependencies + this.climber = climber; + this.dt = dt; + dtJoy = joy; + requires(climber); + requires(dt); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + state = State.CLIMBING; + climber.resetEncoder(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + switch (state) { + case CLIMBING: + dt.drive(backDrive, backDrive); + climber.runClimber(climbUp); + if (!climber.needToClimb()) { + state = State.LEVELING; + } + if (robotOnPlatform()) + state = State.RETRACTING; + break; + case LEVELING: + dt.drive(backDrive, backDrive); + climber.runClimber(climbDown); + if (!climber.canDrop()) { + state = State.CLIMBING; + } + if (robotOnPlatform()) + state = State.RETRACTING; + break; + case RETRACTING: + climber.runClimber(retract); + if (climber.getEncDistance() <= retractGoal) { + state = State.FINISHED; + } + break; + case FINISHED: + end(); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return state == State.FINISHED; + } + + // Called once after isFinished returns true + @Override + protected void end() { + climber.stopClimber(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + + /** + * This method checks whether 1) The driver is attempting to override the + * climbing sequence; or 2) The drivetrain current draw is above average, + * implying that the robot is on the platform. + * + * @return Returns whether the robot is on the plaform, or the driver is + * overriding the climb + */ + private boolean robotOnPlatform() { + return (dt.isStalled() && climber.getEncDistance() > offGroundHeight) || Math.abs(dtJoy.getY()) > overrideThreshold; + } + + private enum State { + CLIMBING, // going up + LEVELING, // going down (needed to level robot) + RETRACTING, // retract foot once on ledge + FINISHED + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/DrivetrainAnalysis.java b/Robot2019/src/main/java/frc/robot/commands/DrivetrainAnalysis.java new file mode 100644 index 0000000..654a69c --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/DrivetrainAnalysis.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* 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 java.io.FileNotFoundException; +import java.io.PrintWriter; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.DrivetrainCharAnalysis; +import frc.robot.subsystems.Drivetrain; + +public class DrivetrainAnalysis extends Command { + Drivetrain dt; + String file1, file2, file3, file4, outfile; + /** + * Handles all the teleoperated driving functionality + * + * @param dt the Drivetrain object to use, passing it in is useful for testing + * purposes + */ + public DrivetrainAnalysis(Drivetrain dt) { + this.dt = dt; + file1 = "/home/lvuser/drive_char_linear_for.csv"; + file2 = "/home/lvuser/drive_char_stepwise_for.csv"; + file3 = "/home/lvuser/drive_char_linear_back.csv"; + file4 = "/home/lvuser/drive_char_stepwise_back.csv"; + outfile = "/home/lvuser/drive_char_params.csv"; + } + + @Override + protected void execute() { + try { + PrintWriter pw = new PrintWriter(outfile); + pw.close(); + } catch (FileNotFoundException f) { + f.printStackTrace(); + } + DrivetrainCharAnalysis.ordinaryLeastSquares(file1, file2, outfile); + DrivetrainCharAnalysis.ordinaryLeastSquares(file3, file4, outfile); + System.out.println("Drivetrain Characterization Analysis Successful. All data has been dumped into " + outfile); + dt.updateDrivetrainParameters(); + } + + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end() { + } + + @Override + protected void interrupted() { + end(); + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/DrivetrainCharacterization.java b/Robot2019/src/main/java/frc/robot/commands/DrivetrainCharacterization.java new file mode 100644 index 0000000..b9ffb60 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/DrivetrainCharacterization.java @@ -0,0 +1,96 @@ +package frc.robot.commands; + +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.subsystems.Drivetrain; + +/* Command which supplies the motors with a desired voltage that increases linearly. +*/ +public class DrivetrainCharacterization extends Command { + public Drivetrain dt; + public FileWriter fw; + private String filename; + public double suppliedVoltage, voltStep, stepwiseVoltage, voltageRuntime, maxVoltage; + + public DrivetrainCharacterization(Drivetrain dt, double voltStep, double stepwiseVoltage, String filename) { + requires(dt); + this.dt = dt; + this.voltStep = voltStep; + this.stepwiseVoltage = stepwiseVoltage; + this.filename = filename; + suppliedVoltage = 0.0; + voltageRuntime = 0.0; + maxVoltage = 9.0; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + File f = new File(filename); + try { + f.createNewFile(); + fw = new FileWriter(f); + fw.write("Timestamp (s),"); + fw.write("Voltage (V),"); + fw.write("LeftMotorVelocity (inches / s),"); + fw.write("RightMotorVelocity (inches / s)\r\n"); + } catch (IOException e) { + System.out.println("Error caught creating FileWriter object: " + e); + } + + dt.setVoltageCompensation(9.0); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // 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() { + try { + fw.close(); + } catch (IOException e) { + System.out.println("Cannot close FileWriter"); + } + + dt.disableVoltageCompensation(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + + public void writeMeasuredVelocity(FileWriter fw) { + double leftMotorVelocity, rightMotorVelocity; + StringBuilder sb = new StringBuilder(); + + leftMotorVelocity = dt.getEncRate(Drivetrain.Side.LEFT); + rightMotorVelocity = dt.getEncRate(Drivetrain.Side.RIGHT); + + voltageRuntime += 0.02; // IncreaseVoltage occurs every 1/50 of a second + sb.append(String.valueOf(voltageRuntime) + ","); + sb.append(String.valueOf(suppliedVoltage) + ","); + sb.append(String.valueOf(leftMotorVelocity) + ","); + sb.append(String.valueOf(rightMotorVelocity) + "\r\n"); + + try { + fw.write(sb.toString()); + } catch (IOException e) { + System.out.println("FileWriter object cannot write StringBuilder object: " + e); + } + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/EjectCargo.java b/Robot2019/src/main/java/frc/robot/commands/EjectCargo.java new file mode 100644 index 0000000..62b5c2e --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/EjectCargo.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Cargo; + +public class EjectCargo extends Command { + + final double ejectDuration = 0.5; // seconds + + Timer timer; + Cargo cargo; + + public EjectCargo(Cargo cargo) { + requires(cargo); + this.cargo = cargo; + timer = new Timer(); + } + + @Override + protected void initialize() { + timer.reset(); + timer.start(); + } + + @Override + protected void execute() { + cargo.runEject(); + } + + @Override + protected boolean isFinished() { + return (timer.get() > ejectDuration); + } + + @Override + protected void end() { + cargo.stopIntake(); + if (isFinished()) { + SmartDashboard.putBoolean("Has cargo", false); + } + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageLinear.java b/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageLinear.java new file mode 100644 index 0000000..570eae0 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageLinear.java @@ -0,0 +1,40 @@ +package frc.robot.commands; + +import frc.robot.commands.DrivetrainCharacterization; +import frc.robot.subsystems.Drivetrain; + +/* Command which supplies the motors with a desired voltage that increases linearly. +*/ +public class IncreaseVoltageLinear extends DrivetrainCharacterization { + String direction; + + public IncreaseVoltageLinear(Drivetrain dt, double voltStep, double stepwiseVoltage, String filename, String dir) { + super(dt, voltStep, stepwiseVoltage, filename); + direction = dir; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double throttle = suppliedVoltage / maxVoltage; + if (direction.equals("backward")) { + throttle *= -1; + } + dt.drive(throttle, throttle); + writeMeasuredVelocity(fw); + + if (suppliedVoltage + voltStep <= maxVoltage) { + suppliedVoltage += voltStep; + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + if (suppliedVoltage == maxVoltage) { + return true; + } else { + return false; + } + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageStepwise.java b/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageStepwise.java new file mode 100644 index 0000000..9244cb8 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageStepwise.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import frc.robot.commands.DrivetrainCharacterization; +import frc.robot.subsystems.Drivetrain; + +/* Command which supplies the motors with a desired voltage that increases linearly. +*/ +public class IncreaseVoltageStepwise extends DrivetrainCharacterization { + String direction; + + public IncreaseVoltageStepwise(Drivetrain dt, double voltStep, double stepwiseVoltage, String filename, String dir) { + super(dt, voltStep, stepwiseVoltage, filename); + direction = dir; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double throttle = suppliedVoltage / maxVoltage; + if (direction.equals("backward")) { + throttle *= -1; + } + dt.drive(throttle, throttle); + writeMeasuredVelocity(fw); + suppliedVoltage = stepwiseVoltage; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + if (dt.getEncRate(Drivetrain.Side.LEFT) >= 0.75 * dt.getMaxSpeed() || dt.getEncRate(Drivetrain.Side.RIGHT) >= 0.75 * dt.getMaxSpeed()) { + return true; + } else { + return false; + } + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/IntakeCargo.java b/Robot2019/src/main/java/frc/robot/commands/IntakeCargo.java new file mode 100644 index 0000000..0bd5aac --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/IntakeCargo.java @@ -0,0 +1,66 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Cargo; + +public class IntakeCargo extends Command { + Timer timer; + Cargo cargo; + boolean overdraw; + + public IntakeCargo(Cargo cargo) { + requires(cargo); + this.cargo = cargo; + timer = new Timer(); + overdraw = false; + } + + @Override + protected void initialize() { + timer.reset(); + } + + @Override + protected void execute() { + cargo.runIntake(); + if (cargo.hasCargo()) { + if (!overdraw) { + overdraw = true; + timer.start(); + } + } else { + overdraw = false; + timer.stop(); + timer.reset(); + } + } + + @Override + protected boolean isFinished() { + return (timer.get() > 0.5); + } + + @Override + protected void end() { + if (isFinished()) { + cargo.keepIntake(); + SmartDashboard.putBoolean("Has cargo", true); + } else { + cargo.stopIntake(); + } + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java b/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java new file mode 100644 index 0000000..4255ef7 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems.Climber; + +public class KeepClimber extends Command { + private Climber climber; + + private final double retractSpeed = -1; + + public KeepClimber(Climber climber) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + this.climber = climber; + requires(climber); + } + + // 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 (climber.slipping()) { + climber.runClimber(retractSpeed); + } else { + climber.stopClimber(); + } + } + + // 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() { + climber.stopClimber(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java b/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java new file mode 100644 index 0000000..93a095b --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java @@ -0,0 +1,80 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Joystick; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Climber; + +public class ManualClimb extends Command { + private Climber climber; + private Joystick manip; + + final private double retractDist = 0; + final private double climbDist = 24.46; // In inches + + final private int climbJoyAxis = 1; + + public ManualClimb(Climber climber, Joystick manip) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(climber); + + this.climber = climber; + this.manip = manip; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + SmartDashboard.putNumber("Current Angle", 0); + SmartDashboard.putNumber("Max Angle", 0); + SmartDashboard.putNumber("Min Angle", 0); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double climbSpeed = manip.getRawAxis(climbJoyAxis); + + // if (climbSpeed > 0 && climber.getEncDistance() >= climbDist) { + // climbSpeed = 0; + // } else if (climbSpeed < 0 && climber.getEncDistance() <= retractDist) { + // climbSpeed = 0; + // } + climber.runClimber(climbSpeed); + + double angle = climber.getAngle(); + SmartDashboard.putNumber("Current Angle", angle); + if (angle > SmartDashboard.getNumber("Max Angle", 0)) { + SmartDashboard.putNumber("Max Angle", angle); + } + if (angle < SmartDashboard.getNumber("Min Angle", 0)) { + SmartDashboard.putNumber("Min Angle", angle); + } + } + + // 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() { + climber.stopClimber(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java b/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java new file mode 100644 index 0000000..f30bb46 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class NormalDrive extends Command { + protected void initialize() { + SmartDashboard.putBoolean("Characterized Drive", false); + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + SmartDashboard.putBoolean("Characterized Drive", true); + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/ResetWobble.java b/Robot2019/src/main/java/frc/robot/commands/ResetWobble.java new file mode 100644 index 0000000..b7d52a6 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ResetWobble.java @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* 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.InstantCommand; +import frc.robot.subsystems.Drivetrain; + +/** + * Add your docs here. + */ +public class ResetWobble extends InstantCommand { + /** + * Add your docs here. + */ + Drivetrain dt; + public ResetWobble(Drivetrain dt) { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + this.dt = dt; + } + + // Called once when the command executes + @Override + protected void initialize() { + dt.setWobbleDone(true); + } + +} diff --git a/Robot2019/src/main/java/frc/robot/commands/SetArcadeOrTank.java b/Robot2019/src/main/java/frc/robot/commands/SetArcadeOrTank.java new file mode 100644 index 0000000..6b065a6 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/SetArcadeOrTank.java @@ -0,0 +1,20 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SetArcadeOrTank extends InstantCommand { + public SetArcadeOrTank() { + + } + + @Override + protected void initialize() { + if (SmartDashboard.getBoolean("Arcade Drive", true)) { + SmartDashboard.putBoolean("Arcade Drive", false); + } else { + SmartDashboard.putBoolean("Arcade Drive", true); + } + + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/ExampleCommand.java b/Robot2019/src/main/java/frc/robot/commands/SetLight.java similarity index 78% rename from Robot2019/src/main/java/frc/robot/commands/ExampleCommand.java rename to Robot2019/src/main/java/frc/robot/commands/SetLight.java index 83a3fcc..89a0d9a 100644 --- a/Robot2019/src/main/java/frc/robot/commands/ExampleCommand.java +++ b/Robot2019/src/main/java/frc/robot/commands/SetLight.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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. */ @@ -8,15 +8,16 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Robot; +import frc.robot.subsystems.Lights; -/** - * An example command. You can replace me with your own command. - */ -public class ExampleCommand extends Command { - public ExampleCommand() { +public class SetLight extends Command { + private Lights lights; + + public SetLight(Lights lights) { // Use requires() here to declare subsystem dependencies - requires(Robot.m_subsystem); + // eg. requires(chassis); + this.lights = lights; + requires(lights); } // Called just before this Command runs the first time @@ -27,6 +28,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + lights.setLights(); } // Make this return true when this Command no longer needs to run execute() @@ -44,5 +46,6 @@ protected void end() { // subsystems is scheduled to run @Override protected void interrupted() { + end(); } } diff --git a/Robot2019/src/main/java/frc/robot/commands/SlowDrive.java b/Robot2019/src/main/java/frc/robot/commands/SlowDrive.java new file mode 100644 index 0000000..2237f10 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/SlowDrive.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SlowDrive extends Command { + public enum Side { + LEFT, RIGHT + } + + String sdValue; + + /** + * Reduces the joystick input values while this command is running + * + * @param side the joystick to reduce + */ + public SlowDrive(Side side) { + sdValue = (side == Side.LEFT ? "Slow Left" : "Slow Right"); + } + + @Override + protected void initialize() { + SmartDashboard.putBoolean(sdValue, true); + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + SmartDashboard.putBoolean(sdValue, false); + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java b/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java new file mode 100644 index 0000000..7d4979c --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java @@ -0,0 +1,195 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Joystick; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Drivetrain; + +public class TeleopDrive extends Command { + Drivetrain dt; + Joystick leftJoy, rightJoy; + + /** + * Handles all the teleoperated driving functionality + * + * @param dt the Drivetrain object to use, passing it in is useful for testing + * purposes + */ + public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) { + requires(dt); + this.dt = dt; + this.leftJoy = leftJoy; + this.rightJoy = rightJoy; + } + + @Override + protected void execute() { + if (SmartDashboard.getBoolean("Arcade Drive", true)) { + arcadeDrive(); + } else { + tankDrive(); + } + } + + private void arcadeDrive() { + double speed = -leftJoy.getY(); + double rot = rightJoy.getX(); + + // System.out.println("Speed: " + speed + ", Rotation: " + rot); + + if (SmartDashboard.getBoolean("Square Joysticks", true)) { + speed = Math.copySign(speed * speed, speed); + rot = Math.copySign(rot * rot, rot); + } + + if (SmartDashboard.getBoolean("Slow Left", false)) { + speed *= SmartDashboard.getNumber("Speed Slow Ratio", 0.5); + } + if (SmartDashboard.getBoolean("Slow Right", false)) { + rot *= SmartDashboard.getNumber("Rotation Slow Ratio", 0.35); + } + + double left, right; + + // double maxInput = Math.copySign(Math.max(Math.abs(speed), Math.abs(rot)), + // speed); + // copySign is returning incorrect signs in operation but not tests + double maxInput = Math.max(Math.abs(speed), Math.abs(rot)); + + if (speed >= 0.0) { + // First quadrant, else second quadrant + if (rot >= 0.0) { + left = maxInput; + right = speed - rot; + } else { + left = speed + rot; + right = maxInput; + } + } else { + // Third quadrant, else fourth quadrant + maxInput *= -1; + if (rot >= 0.0) { + left = speed + rot; + right = maxInput; + } else { + left = maxInput; + right = speed - rot; + } + } + + if (SmartDashboard.getBoolean("Characterized Drive", true)) { + charDrive(left, right); + } else { + dt.drive(left, right); + } + } + + private void tankDrive() { + double left = -leftJoy.getY(); + double right = -rightJoy.getY(); + + if (SmartDashboard.getBoolean("Square Joysticks", true)) { + left = Math.copySign(left * left, left); + right = Math.copySign(right * right, right); + } + + if (SmartDashboard.getBoolean("Slow Left", false)) { + left *= SmartDashboard.getNumber("Speed Slow Ratio", 0.5); + } + if (SmartDashboard.getBoolean("Slow Right", false)) { + right *= SmartDashboard.getNumber("Speed Slow Ratio", 0.5); + } + + if (SmartDashboard.getBoolean("Characterized Drive", true)) { + charDrive(left, right); + } else { + dt.drive(left, right); + } + } + + private void charDrive(double left, double right) { + double leftV, rightV; + dt.setVoltageCompensation(12.0); + double maxV = dt.getMaxVoltage(); + + double DT = 1 / 4.0; + double actualLeftVel = dt.getEncRate(Drivetrain.Side.LEFT); + double actualRightVel = dt.getEncRate(Drivetrain.Side.RIGHT); + double actualAvgVel = 0.5 * (actualLeftVel + actualRightVel); + + double desiredLeftVel = left * dt.getMaxSpeed(); + double desiredRightVel = right * dt.getMaxSpeed(); + double desiredAvgVel = 0.5 * (desiredLeftVel + desiredRightVel); + + double leftDV = desiredLeftVel - actualLeftVel; + double rightDV = desiredRightVel - actualRightVel; + double avgDV = 0.5 * (desiredAvgVel - actualAvgVel); + + double leftA = leftDV / DT; + double rightA = rightDV / DT; + double avgA = avgDV / DT; + + SmartDashboard.putNumber("Left Speed", actualLeftVel); + SmartDashboard.putNumber("Right Speed", actualRightVel); + //System.out.println("Left Speed: " + actualLeftVel + ", Right Speed: " + actualRightVel); + double maxAccel = SmartDashboard.getNumber("Max Acceleration", dt.getMaxSpeed() / 1.0); + + if (Math.abs(avgA) >= maxAccel) { // dt.getMaxSpeed() is a temporary value. The actual value will be determined + // through experimentation + leftA = Math.signum(leftA) * Math.abs(leftA / avgA) * maxAccel; + } + if (Math.abs(avgA) >= maxAccel) { + rightA = Math.signum(rightA) * Math.abs(rightA / avgA) * maxAccel; + } + + //System.out.println("Left Accel: " + leftA + ", Right Accel: " + rightA); + + if (left >= 0.0) { + leftV = dt.calculateVoltage(Drivetrain.Direction.FL, actualLeftVel, leftA); + } else { + leftV = dt.calculateVoltage(Drivetrain.Direction.BL, actualLeftVel, leftA); + } + + if (right >= 0.0) { + rightV = dt.calculateVoltage(Drivetrain.Direction.FR, actualRightVel, rightA); + } else { + rightV = dt.calculateVoltage(Drivetrain.Direction.BR, actualRightVel, rightA); + } + + if (Math.abs(leftV) >= maxV) { + leftV = maxV * Math.signum(leftV); + } + if (Math.abs(rightV) >= maxV) { + rightV = maxV * Math.signum(rightV); + } + + SmartDashboard.putNumber("Left Volts", leftV); + SmartDashboard.putNumber("Right Volts", rightV); + //System.out.println("LeftV: " + leftV + ", RightV: " + rightV); + + dt.drive(leftV / maxV, rightV / maxV); + dt.disableVoltageCompensation(); + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + dt.stop(); + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/ToggleCamera.java b/Robot2019/src/main/java/frc/robot/commands/ToggleCamera.java new file mode 100644 index 0000000..0d1e1b0 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleCamera.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.cscore.UsbCamera; +import edu.wpi.cscore.VideoSink; +import edu.wpi.first.wpilibj.command.InstantCommand; + +public class ToggleCamera extends InstantCommand { + boolean toggle = false; + + UsbCamera driveCamera, hatchCamera; + VideoSink cameraServer; + + /** + * Toggles between the two cameras. + */ + public ToggleCamera(UsbCamera driveCamera, UsbCamera hatchCamera, VideoSink cameraServer) { + super(); + this.driveCamera = driveCamera; + this.hatchCamera = hatchCamera; + this.cameraServer = cameraServer; + } + + // Called once when the command executes + @Override + protected void initialize() { + if (toggle) { + cameraServer.setSource(driveCamera); + } else { + cameraServer.setSource(hatchCamera); + } + toggle = !toggle; + } + +} diff --git a/Robot2019/src/main/java/frc/robot/commands/ToggleClimberRails.java b/Robot2019/src/main/java/frc/robot/commands/ToggleClimberRails.java new file mode 100644 index 0000000..7c5df3c --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleClimberRails.java @@ -0,0 +1,20 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import frc.robot.subsystems.Climber; + +public class ToggleClimberRails extends InstantCommand { + private Climber climber; + + public ToggleClimberRails(Climber cl) { + super(); + requires(cl); + climber = cl; + } + + // Called once when the command executes + @Override + protected void initialize() { + climber.toggleRails(); + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/ToggleHatchEject.java b/Robot2019/src/main/java/frc/robot/commands/ToggleHatchEject.java new file mode 100644 index 0000000..2acbeef --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleHatchEject.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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.InstantCommand; +import frc.robot.subsystems.HatchPanel; + +public class ToggleHatchEject extends InstantCommand { + private HatchPanel hp; + + /** + * Toggles the eject pistons of the hatch panel mechanism, also closes the + * grabbing piston when ejecting + */ + public ToggleHatchEject(HatchPanel hp) { + requires(hp); + this.hp = hp; + } + + @Override + protected void initialize() { + // If the pistons are currently extend them, retract them back, otherwise + // release and then eject + if (hp.state == HatchPanel.State.EJECTING) { + hp.reset(); + } else { + hp.reset(); + hp.eject(); + } + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/ToggleHatchIntake.java b/Robot2019/src/main/java/frc/robot/commands/ToggleHatchIntake.java new file mode 100644 index 0000000..67194b2 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleHatchIntake.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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.InstantCommand; +import frc.robot.subsystems.HatchPanel; + +public class ToggleHatchIntake extends InstantCommand { + private HatchPanel hp; + + /** + * Toggles the grabbing piston of the hatch mechanism + */ + public ToggleHatchIntake(HatchPanel hp) { + requires(hp); + this.hp = hp; + } + + @Override + protected void initialize() { + switch(hp.state) { + case EJECTING: + case DEFAULT: + hp.grab(); + break; + case GRABBING: + hp.reset(); + break; + } + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/ToggleLight.java b/Robot2019/src/main/java/frc/robot/commands/ToggleLight.java new file mode 100644 index 0000000..b36ac72 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleLight.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; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import frc.robot.subsystems.Lights; + +/** + * Add your docs here. + */ +public class ToggleLight extends InstantCommand { + /** + * Add your docs here. + */ + private Lights lights; + + public ToggleLight(Lights lights) { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + this.lights = lights; + } + + // Called once when the command executes + @Override + protected void initialize() { + lights.toggleLights(); + } + +} diff --git a/Robot2019/src/main/java/frc/robot/commands/WaitMove.java b/Robot2019/src/main/java/frc/robot/commands/WaitMove.java new file mode 100644 index 0000000..1c8241a --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/WaitMove.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems.Drivetrain; +import frc.robot.subsystems.Drivetrain.Side; + +public class WaitMove extends Command { + Drivetrain dt; + double startl; + double startr; + + private final double reqDist = 2; // TODO: Test value + + public WaitMove(Drivetrain dt) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + this.dt = dt; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + startr = dt.getEncDist(Side.RIGHT); + startl = dt.getEncDist(Side.LEFT); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return startr - dt.getEncDist(Side.RIGHT) > reqDist && startl - dt.getEncDist(Side.LEFT) > reqDist; + } + + // 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() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/WobbleDrive.java b/Robot2019/src/main/java/frc/robot/commands/WobbleDrive.java new file mode 100644 index 0000000..39cf240 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/WobbleDrive.java @@ -0,0 +1,101 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Drivetrain.Side; + +public class WobbleDrive extends Command { + Drivetrain dt; + Side side = Side.LEFT; // We start with left side. TODO: confirm this + Timer tim; + boolean leftSideDone; + boolean rightSideDone; + private final double minTime = 0.2; + private final double wobbleTime = 0.3; // TODO: Set to actual number + private final double driveSpeed = 0.3; // TODO: Set to actual number + private final double encRateTolerance = 1; // TODO: Set to actual number + + public WobbleDrive(Drivetrain dt) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(dt); + this.dt = dt; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + tim = new Timer(); + tim.reset(); + tim.start(); + dt.setWobbleDone(false); + leftSideDone = false; + rightSideDone = false; + SmartDashboard.putBoolean("Wobble drive done", false); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + boolean done = dt.getEncRate(Side.LEFT) < encRateTolerance && dt.getEncRate(Side.RIGHT) < encRateTolerance; + if (tim.get() > minTime) { + if (side == Side.LEFT) { + leftSideDone = done; + } else { + rightSideDone = done; + } + } + if (leftSideDone) { + tim.reset(); + tim.start(); + side = Side.RIGHT; + } + if (rightSideDone) { + tim.reset(); + tim.start(); + side = Side.LEFT; + } + + if (tim.get() > wobbleTime && (!leftSideDone && !rightSideDone)) { + if (side == Side.LEFT) { + side = Side.RIGHT; + } else { + side = Side.LEFT; + } + tim.reset(); + tim.start(); + dt.drive(0, 0); + } else { + dt.drive(side == Side.LEFT ? driveSpeed : 0, side == Side.RIGHT ? driveSpeed : 0); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return dt.wobbleDone() || (leftSideDone && rightSideDone); + } + + // Called once after isFinished returns true + @Override + protected void end() { + dt.stop(); + SmartDashboard.putBoolean("Wobble drive done", true); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Cargo.java b/Robot2019/src/main/java/frc/robot/subsystems/Cargo.java new file mode 100644 index 0000000..c5123f2 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/subsystems/Cargo.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* 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.PowerDistributionPanel; +import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.command.Subsystem; + +public class Cargo extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + + private final double intakeCurrentThreshold = 8.0; // Amps + private final double rollerIntakeSpeed = 4.0 / 12; + private final double rollerStallSpeed = 3.0 / 12; + private final double rollerEjectSpeed = 12.0 / 12; + + private VictorSP roller; + private PowerDistributionPanel pdp; + private int rollerPort; // The port for the VictorSP on the PDP, not the RoboRIO. + + public Cargo(VictorSP roller, PowerDistributionPanel pdp, int rollerPort) { + this.roller = roller; + this.pdp = pdp; + this.rollerPort = rollerPort; + } + + public void stopIntake() { + roller.stopMotor(); + } + + /** + * keeps the cargo in the mechanism by running the rollers at a slower speed + */ + public void keepIntake() { + roller.set(rollerStallSpeed); + } + + public void runIntake() { + roller.set(rollerIntakeSpeed); + } + + public void runEject() { + roller.set(-1 * rollerEjectSpeed); + } + + public boolean hasCargo() { + return pdp.getCurrent(rollerPort) > intakeCurrentThreshold; + } + + @Override + public void initDefaultCommand() { + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..5278c50 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,89 @@ +/*----------------------------------------------------------------------------*/ +/* 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 com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.commands.KeepClimber; + +public class Climber extends Subsystem { + private VictorSP motor; // Mini-CIM + private Encoder enc; + private AHRS ahrs; + private DoubleSolenoid pistons; + + final private double minTilt = 0; // In degrees // TODO: Update wtih actual number + final private double minDist = 20; // In inches // TODO: Update with actual number + final private double maxTilt = 30; // In degrees // TODO: Update with actual number + final private double maxDist = 24; // In inches // TODO: Update with actual number + final private double slipTolerance = 0.5; // In inches // TODO: Update with actual number; + + public Climber(VictorSP motor, Encoder enc, AHRS ahrs, DoubleSolenoid pistons) { + this.motor = motor; + this.enc = enc; + this.ahrs = ahrs; + this.pistons = pistons; + + double pulseFraction = 1.0 / 256; + double pitchDiameter = 1.790; // https://www.vexrobotics.com/35-sprockets.html#Drawing + enc.setDistancePerPulse(pulseFraction * Math.PI * pitchDiameter); + enc.setReverseDirection(true); + enc.reset(); + } + + public void toggleRails() { + if (pistons.get() == DoubleSolenoid.Value.kForward) { + pistons.set(DoubleSolenoid.Value.kReverse); + } else { + pistons.set(DoubleSolenoid.Value.kForward); + } + } + + public void runClimber(double speed) { + motor.set(speed); + } + + public void stopClimber() { + motor.stopMotor(); + } + + public double getAngle() { + return ahrs.getPitch(); + } + + //We are erring on the side of changing directions too much + public boolean needToClimb() { + return getAngle() < maxTilt && enc.getDistance() < maxDist; + } + + public boolean canDrop() { + return getAngle() > minTilt && enc.getDistance() > minDist; + } + + public double getEncDistance() { + return enc.getDistance(); + } + + public void resetEncoder() { + enc.reset(); + } + + public boolean slipping() { + return enc.getDistance() > slipTolerance; + } + + @Override + public void initDefaultCommand() { + setDefaultCommand(new KeepClimber(this)); + } +} diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java b/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java new file mode 100644 index 0000000..bde135d --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -0,0 +1,256 @@ +/*----------------------------------------------------------------------------*/ +/* 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 java.io.File; +import java.io.FileNotFoundException; +import java.util.Scanner; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotMap; + +public class Drivetrain extends Subsystem { + public enum Side { + LEFT, RIGHT + } + + public enum Direction { + FL, FR, BL, BR // Forward-Left, Forward-Right, Backward-Left, Backward-Right + } + + private WPI_TalonSRX leftMaster, rightMaster; + private Encoder leftEnc, rightEnc; + private AHRS ahrs; + + private double maxVoltage; + + private double flKV, flKA, flVI; + private double frKV, frKA, frVI; + private double blKV, blKA, blVI; + private double brKV, brKA, brVI; + + private boolean wobbleDone; + + public Drivetrain(WPI_TalonSRX leftMaster, BaseMotorController leftSlave1, BaseMotorController leftSlave2, + WPI_TalonSRX rightMaster, BaseMotorController rightSlave1, BaseMotorController rightSlave2, Encoder leftEnc, + Encoder rightEnc, AHRS ahrs) { + + leftSlave1.follow(leftMaster); + leftSlave2.follow(leftMaster); + this.leftMaster = leftMaster; + + rightSlave1.follow(rightMaster); + rightSlave2.follow(rightMaster); + this.rightMaster = rightMaster; + + rightMaster.setInverted(true); + rightSlave1.setInverted(true); + rightSlave2.setInverted(true); + + this.leftEnc = leftEnc; + this.rightEnc = rightEnc; + + double pulseFraction = 1.0 / 256; + double wheelDiameter = 5; + leftEnc.setDistancePerPulse(pulseFraction * Math.PI * wheelDiameter); + rightEnc.setDistancePerPulse(pulseFraction * Math.PI * wheelDiameter); + leftEnc.reset(); + rightEnc.reset(); + rightEnc.setReverseDirection(true); + + this.ahrs = ahrs; + updateDrivetrainParameters(); + + maxVoltage = 12.0; + wobbleDone = false; + } + + /** + * teleop drive initialized in Robot.robotInit() to avoid dependency loops + * between dt and oi + */ + @Override + public void initDefaultCommand() { + } + + public void drive(double left, double right) { + leftMaster.set(left); + rightMaster.set(right); + SmartDashboard.putNumber("Encoder Distance Left:", leftEnc.getDistance()); + SmartDashboard.putNumber("Encoder Distance Right:", rightEnc.getDistance()); + } + + public void stop() { + leftMaster.stopMotor(); + rightMaster.stopMotor(); + } + + public boolean isStalled() { + return leftMaster.getOutputCurrent() >= 30 || rightMaster.getOutputCurrent() >= 30; + } + + public double getEncDist(Side type) { + if (type == Side.LEFT) { + return leftEnc.getDistance(); + } else { + return rightEnc.getDistance(); + } + } + + public double getEncRate(Side type) { + if (type == Side.LEFT) { + return leftEnc.getRate(); + } else { + return rightEnc.getRate(); + } + } + + public void resetGyro() { + ahrs.reset(); + } + + public double getGyroRate() { + return ahrs.getRate(); + } + + public double getGyroAngle() { + return ahrs.getYaw(); + } + + public void setWobbleDone(boolean set) { + wobbleDone = set; + } + + public boolean wobbleDone() { + return wobbleDone; + } + + public double getMaxSpeed() { // Return must be adjusted in the future; + return 13 * 12; + } + + public void setMaxVoltage(double volts) { + maxVoltage = volts; + } + + public double getMaxVoltage() { + return maxVoltage; + } + + public void setVoltageCompensation(double volts) { + ErrorCode ecVoltSat = leftMaster.configVoltageCompSaturation(volts, 10); + RobotMap.catchError(ecVoltSat); + + ecVoltSat = rightMaster.configVoltageCompSaturation(volts, 10); + RobotMap.catchError(ecVoltSat); + + leftMaster.enableVoltageCompensation(true); + rightMaster.enableVoltageCompensation(true); + } + + public void disableVoltageCompensation() { + leftMaster.enableVoltageCompensation(false); + rightMaster.enableVoltageCompensation(false); + } + + public void updateDrivetrainParameters() { + try { + Scanner filereader = new Scanner(new File("/home/lvuser/drive_char_params.csv")); + String line = filereader.next(); + // Forward-Left + flKV = Double.valueOf(line.split(",")[0]); + flKA = Double.valueOf(line.split(",")[1]); + flVI = Double.valueOf(line.split(",")[2]); + System.out.println(flKV + "," + flKA + "," + flVI); + + line = filereader.next(); + // Forward-right + frKV = Double.valueOf(line.split(",")[0]); + frKA = Double.valueOf(line.split(",")[1]); + frVI = Double.valueOf(line.split(",")[2]); + System.out.println(frKV + "," + frKA + "," + frVI); + + line = filereader.next(); + // Backward-Left + blKV = Double.valueOf(line.split(",")[0]); + blKA = Double.valueOf(line.split(",")[1]); + blVI = Double.valueOf(line.split(",")[2]); + System.out.println(blKV + "," + blKA + "," + blVI); + + line = filereader.next(); + // Backward-Right + brKV = Double.valueOf(line.split(",")[0]); + brKA = Double.valueOf(line.split(",")[1]); + brVI = Double.valueOf(line.split(",")[2]); + System.out.println(brKV + "," + brKA + "," + brVI); + filereader.close(); + + // Averaging numbers because they vary so much + double avg = (flKA + frKA + blKA + brKA) / 4; + flKA = avg; + frKA = avg; + blKA = avg; + brKA = avg; + } catch (FileNotFoundException e) { + flKV = 0.06369046755507658; + flKA = 0.0215894793277297; + flVI = 0.8403701236277824; + + frKV = 0.0619423013628032; + frKA = 0.04044703465602449; + frVI = 0.810212379284332; + + blKV = 0.06388520699977113; + blKA = 0.025492804438184545; + blVI = 0.8071078220643216; + + brKV = 0.06140765089854154; + brKA = 0.042046502553651215; + brVI = 0.7929289166816246; + + e.printStackTrace(); + } + } + + public double calculateVoltage(Direction dir, double velocity, double acceleration) { + /*System.out.println("Velocity: " + velocity + ", Acceleration: " + acceleration); + System.out.println(flKV + "," + flKA + "," + flVI); + System.out.println(frKV + "," + frKA + "," + frVI); + System.out.println(blKV + "," + blKA + "," + blVI); + System.out.println(brKV + "," + brKA + "," + brVI);*/ + + if (dir == Direction.FL) { + return flKV * velocity + flKA * acceleration + flVI; + } else if (dir == Direction.FR) { + return frKV * velocity + frKA * acceleration + frVI; + } else if (dir == Direction.BL) { + return blKV * velocity + blKA * acceleration - blVI; + } else { + return brKV * velocity + brKA * acceleration - brVI; + } + } + + public double calculateAcceleration(Direction dir, double velocity, double voltage) { + if (dir == Direction.FL) { + return (voltage - flKV * velocity - flVI) / flKA; + } else if (dir == Direction.FR) { + return (voltage - frKV * velocity - frVI) / frKA; + } else if (dir == Direction.BL) { + return (voltage - blKV * velocity - blVI) / blKA; + } else { + return (voltage - brKV * velocity + brVI) / brKA; + } + } +} diff --git a/Robot2019/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/Robot2019/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 7f5e1a4..0000000 --- a/Robot2019/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/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java b/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java new file mode 100644 index 0000000..2bfcfc0 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class HatchPanel extends Subsystem { + private DoubleSolenoid grabPiston, ejectPistons; + public State state; + + /** + * Subsystem for controlling the hatch panel mechanism + * + * @param grabPiston the center piston that is used to grab the hatch panel through the hole + * @param ejectPistons the two side pistons that facilitate ejecting the hatch panel + */ + public HatchPanel(DoubleSolenoid grabPiston, DoubleSolenoid ejectPistons) { + this.grabPiston = grabPiston; + this.ejectPistons = ejectPistons; + + reset(); + + SmartDashboard.putString("Hatch Piston State", state.name()); + } + + public void reset() { + grabPiston.set(DoubleSolenoid.Value.kReverse); + ejectPistons.set(DoubleSolenoid.Value.kReverse); + state = State.DEFAULT; + SmartDashboard.putString("Hatch Piston State", state.name()); + } + + public void grab() { + grabPiston.set(DoubleSolenoid.Value.kForward); + ejectPistons.set(DoubleSolenoid.Value.kReverse); + state = State.GRABBING; + SmartDashboard.putString("Hatch Piston State", state.name()); + } + + public void eject() { + grabPiston.set(DoubleSolenoid.Value.kReverse); + ejectPistons.set(DoubleSolenoid.Value.kForward); + state = State.EJECTING; + SmartDashboard.putString("Hatch Piston State", state.name()); + } + + @Override + public void initDefaultCommand() {} + + public enum State { + DEFAULT, + GRABBING, + EJECTING + } +} diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Lights.java b/Robot2019/src/main/java/frc/robot/subsystems/Lights.java new file mode 100644 index 0000000..ff4188e --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/subsystems/Lights.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.commands.SetLight; + +/** + * Add your docs here. + */ +public class Lights extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + private VictorSP lights; + private double lightsValue = 0; + + /** + * 0 is off 0.5 is orange 1 is yellow + */ + + public Lights(VictorSP lights) { + this.lights = lights; + } + + public void setLights() { + lights.set(lightsValue); + String color; + if (lightsValue == 0) { + color = "None"; + } else if (lightsValue == 0.5) { + color = "Yellow"; + } else { + color = "Orange"; + } + SmartDashboard.putString("Lights Current Color", color); + } + + public void toggleLights() { + if (lightsValue == 0) { + lightsValue = 0.5; + } else if (lightsValue == 0.5) { + lightsValue = 1; + } else { + lightsValue = 0; + } + } + + @Override + public void initDefaultCommand() { + setDefaultCommand(new SetLight(this)); + } +} diff --git a/Robot2019/vendordeps/Phoenix.json b/Robot2019/vendordeps/Phoenix.json new file mode 100644 index 0000000..a1654ec --- /dev/null +++ b/Robot2019/vendordeps/Phoenix.json @@ -0,0 +1,87 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.12.1", + "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.12.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.12.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.12.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.12.1", + "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.12.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.12.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.12.1", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers" + } + ] +} \ No newline at end of file diff --git a/Robot2019/vendordeps/navx_frc.json b/Robot2019/vendordeps/navx_frc.json new file mode 100644 index 0000000..80defba --- /dev/null +++ b/Robot2019/vendordeps/navx_frc.json @@ -0,0 +1,33 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "3.1.344", + "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.344" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "3.1.344", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file