It is advised to statically import this class (or one of its inner classes) wherever the + *
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
@@ -33,7 +37,7 @@ public static final class CanConstants {
public static final int kRightRearMotorPort = 5;
public static final double kRampRate = 0.1;
- public static double maxSpeed = 0.8;
+ public static double maxSpeed = 1;
public static final double maxSpeedIncrement = 0.05;
public static final String kBaseType = "tank";
@@ -41,7 +45,7 @@ public static final class CanConstants {
public static final class OperatorConstants {
public static final int kDriverControllerPort = 0;
- public static final double ProportionalDivisor = 17;
+ public static final double Pconstant = 0.00635;
public static final String m_driveControllerType = "accurateTank";
}
@@ -51,4 +55,7 @@ public static final class VisionConstants {
public static final double kTargetHeightMeters = 1.524;
public static final double kCameraPitchRadians = 0;
}
+
+ public static final String OperatorConstast = null;
+public boolean isDriveCommandFinished = false;
}
diff --git a/src/main/java/frc/robot/Interfaces/CANSparkMax.java b/src/main/java/frc/robot/Interfaces/CANSparkMax.java
index 0e23cce..cac9efb 100644
--- a/src/main/java/frc/robot/Interfaces/CANSparkMax.java
+++ b/src/main/java/frc/robot/Interfaces/CANSparkMax.java
@@ -1,4 +1,7 @@
-package frc.robot.Interfaces;
+package frc.robot.interfaces;
+
+import com.revrobotics.REVLibError;
+import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
@@ -10,116 +13,169 @@ public CANSparkMax(int deviceId, MotorType type) {
super(deviceId, type);
}
- // for reference, here's all the methods that have a getter and, optionally, a setter, in the
- // REVLib CANSparkMax class (brackets indicate a note, preceeding percent signs indicate a method
- // that returns an object for--you guessed it-- more interfacing, a dollar sign indicates a method
- // we won't be using, exclamation points indicate a method that returns an object that we will be
+ // for reference, here's all the methods that have a getter and, optionally, a
+ // setter, in the
+ // REVLib CANSparkMax class (brackets indicate a note, preceeding percent signs
+ // indicate a method
+ // that returns an object for--you guessed it-- more interfacing, a dollar sign
+ // indicates a method
+ // we won't be using, exclamation points indicate a method that returns an
+ // object that we will be
// using or that we need to pay attention to)
/*
- * get() and set() -- these are the main methods for getting and setting the output of the motor controller
- * % getAbsoluteEncoder() [no setter] -- returns an object for interfacing with a connected absolute encoder
- * %$ getAlternateEncoder() [no setter] -- returns an object for interfacing with a quadrature encoder connected to the alternate encoder pins
- * %$ getAnalog() [no setter] -- returns an object for interfacing with a connected analog sensor
- * getAppliedOutput() [no setter] -- returns the output of the motor controller, in the range [-1, 1]
- * getBusVoltage() [no setter] -- returns the bus voltage of the motor controller
- * getClosedLoopRampRate() and setClosedLoopRampRate() -- gets and sets the closed loop ramp rate of the motor controller
- * ! getEncoder() [no setter] -- returns an object for interfacing with a connected encoder
- * getFaults() [no setter] -- returns an object for interfacing with the faults of the motor controller
- * an extension of this is getStickyFaults(), which returns an object for interfacing with the sticky faults of the motor controller
- * as well as getFault() and getStickyFault(), which return a boolean for whether or not a specific fault is present
- * getFeedbackDeviceID() [no setter] -- returns the ID of the feedback device set on the motor controller
- * % getForwardLimitSwitch() [no setter] -- returns an object for interfacing with the forward limit switch
- * ! getIdleMode() and setIdleMode() -- gets and sets the idle mode of the motor controller
- * getInverted() and setInverted() -- gets and sets the inversion of the motor controller
+ * get() and set() -- these are the main methods for getting and setting the
+ * output of the motor controller
+ * % getAbsoluteEncoder() [no setter] -- returns an object for interfacing with
+ * a connected absolute encoder
+ * %$ getAlternateEncoder() [no setter] -- returns an object for interfacing
+ * with a quadrature encoder connected to the alternate encoder pins
+ * %$ getAnalog() [no setter] -- returns an object for interfacing with a
+ * connected analog sensor
+ * getAppliedOutput() [no setter] -- returns the output of the motor controller,
+ * in the range [-1, 1]
+ * getBusVoltage() [no setter] -- returns the bus voltage of the motor
+ * controller
+ * getClosedLoopRampRate() and setClosedLoopRampRate() -- gets and sets the
+ * closed loop ramp rate of the motor controller
+ * ! getEncoder() [no setter] -- returns an object for interfacing with a
+ * connected encoder
+ * getFaults() [no setter] -- returns an object for interfacing with the faults
+ * of the motor controller
+ * an extension of this is getStickyFaults(), which returns an object for
+ * interfacing with the sticky faults of the motor controller
+ * as well as getFault() and getStickyFault(), which return a boolean for
+ * whether or not a specific fault is present
+ * getFeedbackDeviceID() [no setter] -- returns the ID of the feedback device
+ * set on the motor controller
+ * % getForwardLimitSwitch() [no setter] -- returns an object for interfacing
+ * with the forward limit switch
+ * ! getIdleMode() and setIdleMode() -- gets and sets the idle mode of the motor
+ * controller
+ * getInverted() and setInverted() -- gets and sets the inversion of the motor
+ * controller
* getLastError() [no setter] -- returns the last error of the motor controller
- * getMotorTemperature() [no setter] -- returns the temperature of the motor controller (at least, that's what we assume, the documentation is unclear)
- * getOutputCurrent() [no setter] -- returns the output current of the motor controller
- * % getPIDController() [no setter] -- returns an object for interfacing with the PID controller of the motor controller
- * %$ getReverseLimitSwitch() [no setter] -- returns an object for interfacing with the reverse limit switch
- * getSoftLimit() and setSoftLimit() -- gets and sets the soft limit of the motor controller
- * getSmartCurrentLimit() and setSmartCurrentLimit() -- gets and sets the smart current limit of the motor controller
- * getVoltageCompensationNomininalVoltage() and setVoltageCompensationNomininalVoltage() -- gets and sets the voltage compensation nominal voltage of the motor controller
+ * getMotorTemperature() [no setter] -- returns the temperature of the motor
+ * controller (at least, that's what we assume, the documentation is unclear)
+ * getOutputCurrent() [no setter] -- returns the output current of the motor
+ * controller
+ * % getPIDController() [no setter] -- returns an object for interfacing with
+ * the PID controller of the motor controller
+ * %$ getReverseLimitSwitch() [no setter] -- returns an object for interfacing
+ * with the reverse limit switch
+ * getSoftLimit() and setSoftLimit() -- gets and sets the soft limit of the
+ * motor controller
+ * getSmartCurrentLimit() and setSmartCurrentLimit() -- gets and sets the smart
+ * current limit of the motor controller
+ * getVoltageCompensationNomininalVoltage() and
+ * setVoltageCompensationNomininalVoltage() -- gets and sets the voltage
+ * compensation nominal voltage of the motor controller
*
- * is() statements -- these are all boolean methods that return whether or not a specific condition is true
+ * is() statements -- these are all boolean methods that return whether or not a
+ * specific condition is true
*
- * isFollower() [no setter] -- returns a boolean for whether or not the motor controller is a follower of another motor controller
- * isSoftLimitEnabled() [no setter] -- returns a boolean for whether or not the soft limit is enabled
+ * isFollower() [no setter] -- returns a boolean for whether or not the motor
+ * controller is a follower of another motor controller
+ * isSoftLimitEnabled() [no setter] -- returns a boolean for whether or not the
+ * soft limit is enabled
*
* set() statements -- these are all methods that set a specific property
*
* setCANTimeout() -- sets the CAN timeout of the motor controller
- * setVoltage() -- sets the voltage of the motor controller (This seems really dangerous, but it's in the documentation, so we'll include it)
+ * setVoltage() -- sets the voltage of the motor controller (This seems really
+ * dangerous, but it's in the documentation, so we'll include it)
*
- * inherited methods -- these are all methods that are inherited from various other things
+ * inherited methods -- these are all methods that are inherited from various
+ * other things
*
* getDeviceId() [no setter] -- returns the device ID of the motor controller
- * getFirmwareString() [no setter] -- returns the firmware string of the motor controller
- * getFirmwareVersion() [no setter] -- returns the firmware version of the motor controller
- * getInitialMotorType() [no setter] -- returns the initial motor type of the motor controller
+ * getFirmwareString() [no setter] -- returns the firmware string of the motor
+ * controller
+ * getFirmwareVersion() [no setter] -- returns the firmware version of the motor
+ * controller
+ * getInitialMotorType() [no setter] -- returns the initial motor type of the
+ * motor controller
* getMotorType() [no setter] -- returns the motor type of the motor controller
- * getSafeFloat() [no setter] -- returns the safe float of the motor controller (??)
- * getSerialNumber() [no setter] -- returns the serial number of the motor controller
- * restoreFactoryDefaults() -- restores the factory defaults of the motor controller
- * setControlFramePeriodMs() -- sets the control frame period of the motor controller
- * setPeriodicFramePeriod() -- sets the periodic frame period of the motor controller
- * throwIfClosed() [no setter] -- throws an exception if the motor controller is closed
+ * getSafeFloat() [no setter] -- returns the safe float of the motor controller
+ * (??)
+ * getSerialNumber() [no setter] -- returns the serial number of the motor
+ * controller
+ * restoreFactoryDefaults() -- restores the factory defaults of the motor
+ * controller
+ * setControlFramePeriodMs() -- sets the control frame period of the motor
+ * controller
+ * setPeriodicFramePeriod() -- sets the periodic frame period of the motor
+ * controller
+ * throwIfClosed() [no setter] -- throws an exception if the motor controller is
+ * closed
*/
- // IGNORE EVERYTHING ABOVE THIS! I rewrote the entire thing, and it's much better now!
+ // IGNORE EVERYTHING ABOVE THIS! I rewrote the entire thing, and it's much
+ // better now!
- // here's all the methods found in CANSparkMaxLowLevel using the magic of regex (public [a-zA-Z]*
+ // here's all the methods found in CANSparkMaxLowLevel using the magic of regex
+ // (public [a-zA-Z]*
// get[a-zA-z_]*())
/*
- * getFirmwareVersion
- * getFirmwareString
- * getDeviceId
- * getInitialMotorType
- * getMotorType
- * getSafeFloat
- * getRaw
- *
- * and all the set methods (public [a-zA-Z]* set[a-zA-z_]*())
- *
- * setControlFramePeriodMs
- * setPeriodicFramePeriod
- *
- * and all the public variables that we can *technically* access, and we probably shouldn't, but we can, so safety be damned (safety? i hardly know her!)
- *
- * public class PeriodicStatus0 {
- public double appliedOutput;
- public short faults;
- public short stickyFaults;
- public byte lock;
- public MotorType motorType;
- public boolean isFollower;
- public boolean isInverted;
- public boolean roboRIO;
- }
-
- public class PeriodicStatus1 {
- public double sensorVelocity;
- public byte motorTemperature;
- public double busVoltage;
- public double outputCurrent;
- }
-
- public class PeriodicStatus2 {
- public double sensorPosition;
- public double iAccum;
- }
- *
- * now, naturally, there's some things we can't access, like the CAN ID, because that's private, and the firmware version, because that's protected, but we can access the rest of it, so we're going to. see above for the list of methods we can access
- *
- * (water break! if you're actually reading this, you're a trooper, and i salute you. even the best of us need water sometimes, so go get some, and then come back and finish reading this. i'll wait.)
- */
-
- // here's all the methods found in CANSparkMax using the magic of regex (public [a-zA-Z]*
+ * getFirmwareVersion
+ * getFirmwareString
+ * getDeviceId
+ * getInitialMotorType
+ * getMotorType
+ * getSafeFloat
+ * getRaw
+ *
+ * and all the set methods (public [a-zA-Z]* set[a-zA-z_]*())
+ *
+ * setControlFramePeriodMs
+ * setPeriodicFramePeriod
+ *
+ * and all the public variables that we can *technically* access, and we
+ * probably shouldn't, but we can, so safety be damned (safety? i hardly know
+ * her!)
+ *
+ * public class PeriodicStatus0 {
+ * public double appliedOutput;
+ * public short faults;
+ * public short stickyFaults;
+ * public byte lock;
+ * public MotorType motorType;
+ * public boolean isFollower;
+ * public boolean isInverted;
+ * public boolean roboRIO;
+ * }
+ *
+ * public class PeriodicStatus1 {
+ * public double sensorVelocity;
+ * public byte motorTemperature;
+ * public double busVoltage;
+ * public double outputCurrent;
+ * }
+ *
+ * public class PeriodicStatus2 {
+ * public double sensorPosition;
+ * public double iAccum;
+ * }
+ *
+ * now, naturally, there's some things we can't access, like the CAN ID, because
+ * that's private, and the firmware version, because that's protected, but we
+ * can access the rest of it, so we're going to. see above for the list of
+ * methods we can access
+ *
+ * (water break! if you're actually reading this, you're a trooper, and i salute
+ * you. even the best of us need water sometimes, so go get some, and then come
+ * back and finish reading this. i'll wait.)
+ */
+
+ // here's all the methods found in CANSparkMax using the magic of regex (public
+ // [a-zA-Z]*
// get[a-zA-z_]*())
/*
* get()
* getInverted()
- * getEncoder() [optionally, it takes 2 paramaters, encoderType and countsPerRev]
- * getAlternateEncoder() [optionally, it takes 2 paramaters, encoderType and countsPerRev]
+ * getEncoder() [optionally, it takes 2 paramaters, encoderType and
+ * countsPerRev]
+ * getAlternateEncoder() [optionally, it takes 2 paramaters, encoderType and
+ * countsPerRev]
* getAnalog(mode) [no docs on what mode is, so that's cool]
* getAbsoluteEncoder()
* getPIDController()
@@ -143,9 +199,11 @@ public class PeriodicStatus2 {
* and all the set methods (public [a-zA-Z]* set[a-zA-z_]*())
*
* set()
- * setVoltage() [also seems unsafe, but it's in the documentation, so we'll include it]
+ * setVoltage() [also seems unsafe, but it's in the documentation, so we'll
+ * include it]
* setInverted()
- * setSmartCurrentLimit() [the library actually instantiated this a bunch of times, so we'll just include the first one]
+ * setSmartCurrentLimit() [the library actually instantiated this a bunch of
+ * times, so we'll just include the first one]
* setSecondaryCurrentLimit()
* setIdleMode()
* setOpenLoopRampRate()
@@ -179,7 +237,8 @@ public class PeriodicStatus2 {
* setMeasurementPeriod()
* setInverted()
*
- * Copilot, you seem to know what you're doing. I'll let you fill in the blanks for what I'm missing.
+ * Copilot, you seem to know what you're doing. I'll let you fill in the blanks
+ * for what I'm missing.
*
*
*/
@@ -247,12 +306,36 @@ public void initSendable(SendableBuilder builder) {
"Reverse Hard Limit?*", () -> getStickyFault(FaultID.kHardLimitRev), null);
// brake mode
- // this one is a bit weird. we have to create a switch statement to get the right value, because
+ // this one is a bit weird. we have to create a switch statement to get the
+ // right value, because
// it's either gonna be kCoast or kBrake
- // we do that outside the initSendable() method, because it's a bit too long to put in there,
- // and I don't think it'll work. If I put outside, I'll be able to call it in other places too.
+ // we do that outside the initSendable() method, because it's a bit too long to
+ // put in there,
+ // and I don't think it'll work. If I put outside, I'll be able to call it in
+ // other places too.
builder.addBooleanProperty("Brake Mode", this::isBraking, this::setBrakeMode);
builder.addBooleanProperty("Coast Mode", this::isCoasting, this::setCoastMode);
+
+ // current limits
+ builder.addIntegerProperty("Current Limit", null, this::setSmartCurrentLimit);
+
+
+ // pid stuff
+ builder.addDoubleProperty("P", this::getP, this::setP);
+ builder.addDoubleProperty("I", this::getI, this::setI);
+ builder.addDoubleProperty("D", this::getD, this::setD);
+ builder.addDoubleProperty("F", this::getF, this::setF);
+ builder.addDoubleProperty("IZone", this::getIZone, this::setIZone);
+ builder.addDoubleProperty("Min Output Range", this::getMinOutputRange, this::setMinOutputRange);
+ builder.addDoubleProperty("Max Output Range", this::getMaxOutputRange, this::setMaxOutputRange);
+
+ // encoder stuff
+ builder.addDoubleProperty("Encoder Position", this::getAbsoluteEncoderPosition, null);
+ builder.addDoubleProperty("Encoder Velocity", this::getAbsoluteEncoderVelocity, null);
+
+ builder.addDoubleProperty("Conversion Factor", this::getPositionConversionFactor, this::setPositionConversionFactor);
+
+
}
/**
@@ -270,6 +353,7 @@ public boolean isBraking() {
return false;
}
}
+
/**
* Returns true if the motor controller is coasting for its idle mode.
*
@@ -311,4 +395,94 @@ public void setCoastMode(boolean coast) {
setIdleMode(IdleMode.kBrake);
}
}
+
+ public REVLibError setSmartCurrentLimit(Long limit) {
+ throwIfClosed();
+ return setSmartCurrentLimit(limit.intValue(), 0, 20000);
+ }
+
+
+ // pid stuff
+ public void setP(double p) {
+ throwIfClosed();
+ getPIDController().setP(p);
+ }
+ public void setI(double i) {
+ throwIfClosed();
+ getPIDController().setI(i);
+ }
+ public void setD(double d) {
+ throwIfClosed();
+ getPIDController().setD(d);
+ }
+ public void setF(double f) {
+ throwIfClosed();
+ getPIDController().setFF(f);
+ }
+ public void setIZone(double iZone) {
+ throwIfClosed();
+ getPIDController().setIZone(iZone);
+ }
+ public void setMinOutputRange(double minOutputRange) {
+ throwIfClosed();
+ getPIDController().setOutputRange(minOutputRange, getMaxOutputRange());
+ }
+ public void setMaxOutputRange(double maxOutputRange) {
+ throwIfClosed();
+ getPIDController().setOutputRange(getMinOutputRange(), maxOutputRange);
+ }
+
+ public double getP() {
+ throwIfClosed();
+ return getPIDController().getP();
+ }
+ public double getI() {
+ throwIfClosed();
+ return getPIDController().getI();
+ }
+ public double getD() {
+ throwIfClosed();
+ return getPIDController().getD();
+ }
+ public double getF() {
+ throwIfClosed();
+ return getPIDController().getFF();
+ }
+ public double getIZone() {
+ throwIfClosed();
+ return getPIDController().getIZone();
+ }
+ public double getMinOutputRange() {
+ throwIfClosed();
+ return getPIDController().getOutputMin();
+ }
+ public double getMaxOutputRange() {
+ throwIfClosed();
+ return getPIDController().getOutputMax();
+ }
+
+
+ // absolute encoder stuff
+ public double getAbsoluteEncoderPosition() {
+ throwIfClosed();
+ return getAbsoluteEncoder(Type.kDutyCycle).getPosition();
+ }
+ public double getAbsoluteEncoderVelocity() {
+ throwIfClosed();
+ return getAbsoluteEncoder(Type.kDutyCycle).getVelocity();
+ }
+
+
+ public double getPositionConversionFactor() {
+ throwIfClosed();
+ return getAbsoluteEncoder(Type.kDutyCycle).getPositionConversionFactor();
+ }
+
+ public void setPositionConversionFactor(double a) {
+ throwIfClosed();
+ getAbsoluteEncoder(Type.kDutyCycle).setPositionConversionFactor(a);
+ }
+
+
+
}
diff --git a/src/main/java/frc/robot/Interfaces/Idriveable.java b/src/main/java/frc/robot/Interfaces/Idriveable.java
index 072d6c3..1b2980f 100644
--- a/src/main/java/frc/robot/Interfaces/Idriveable.java
+++ b/src/main/java/frc/robot/Interfaces/Idriveable.java
@@ -1,4 +1,4 @@
-package frc.robot.Interfaces;
+package frc.robot.interfaces;
public interface Idriveable {
void driveForward();
diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java
new file mode 100644
index 0000000..f51e57f
--- /dev/null
+++ b/src/main/java/frc/robot/LimelightHelpers.java
@@ -0,0 +1,776 @@
+//LimelightHelpers v1.2.1 (March 1, 2023)
+
+package frc.robot;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+import java.io.IOException;
+import java.net.HttpURLConnection;
+import java.net.MalformedURLException;
+import java.net.URL;
+import java.util.concurrent.CompletableFuture;
+
+import com.fasterxml.jackson.annotation.JsonFormat;
+import com.fasterxml.jackson.annotation.JsonFormat.Shape;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationFeature;
+import com.fasterxml.jackson.databind.ObjectMapper;
+
+public class LimelightHelpers {
+
+ public static class LimelightTarget_Retro {
+
+ @JsonProperty("t6c_ts")
+ private double[] cameraPose_TargetSpace;
+
+ @JsonProperty("t6r_fs")
+ private double[] robotPose_FieldSpace;
+
+ @JsonProperty("t6r_ts")
+ private double[] robotPose_TargetSpace;
+
+ @JsonProperty("t6t_cs")
+ private double[] targetPose_CameraSpace;
+
+ @JsonProperty("t6t_rs")
+ private double[] targetPose_RobotSpace;
+
+ public Pose3d getCameraPose_TargetSpace() {
+ return toPose3D(cameraPose_TargetSpace);
+ }
+
+ public Pose3d getRobotPose_FieldSpace() {
+ return toPose3D(robotPose_FieldSpace);
+ }
+
+ public Pose3d getRobotPose_TargetSpace() {
+ return toPose3D(robotPose_TargetSpace);
+ }
+
+ public Pose3d getTargetPose_CameraSpace() {
+ return toPose3D(targetPose_CameraSpace);
+ }
+
+ public Pose3d getTargetPose_RobotSpace() {
+ return toPose3D(targetPose_RobotSpace);
+ }
+
+ public Pose2d getCameraPose_TargetSpace2D() {
+ return toPose2D(cameraPose_TargetSpace);
+ }
+
+ public Pose2d getRobotPose_FieldSpace2D() {
+ return toPose2D(robotPose_FieldSpace);
+ }
+
+ public Pose2d getRobotPose_TargetSpace2D() {
+ return toPose2D(robotPose_TargetSpace);
+ }
+
+ public Pose2d getTargetPose_CameraSpace2D() {
+ return toPose2D(targetPose_CameraSpace);
+ }
+
+ public Pose2d getTargetPose_RobotSpace2D() {
+ return toPose2D(targetPose_RobotSpace);
+ }
+
+ @JsonProperty("ta")
+ public double ta;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ @JsonProperty("ts")
+ public double ts;
+
+ public LimelightTarget_Retro() {
+ cameraPose_TargetSpace = new double[6];
+ robotPose_FieldSpace = new double[6];
+ robotPose_TargetSpace = new double[6];
+ targetPose_CameraSpace = new double[6];
+ targetPose_RobotSpace = new double[6];
+ }
+
+ }
+
+ public static class LimelightTarget_Fiducial {
+
+ @JsonProperty("fID")
+ public double fiducialID;
+
+ @JsonProperty("fam")
+ public String fiducialFamily;
+
+ @JsonProperty("t6c_ts")
+ private double[] cameraPose_TargetSpace;
+
+ @JsonProperty("t6r_fs")
+ private double[] robotPose_FieldSpace;
+
+ @JsonProperty("t6r_ts")
+ private double[] robotPose_TargetSpace;
+
+ @JsonProperty("t6t_cs")
+ private double[] targetPose_CameraSpace;
+
+ @JsonProperty("t6t_rs")
+ private double[] targetPose_RobotSpace;
+
+ public Pose3d getCameraPose_TargetSpace() {
+ return toPose3D(cameraPose_TargetSpace);
+ }
+
+ public Pose3d getRobotPose_FieldSpace() {
+ return toPose3D(robotPose_FieldSpace);
+ }
+
+ public Pose3d getRobotPose_TargetSpace() {
+ return toPose3D(robotPose_TargetSpace);
+ }
+
+ public Pose3d getTargetPose_CameraSpace() {
+ return toPose3D(targetPose_CameraSpace);
+ }
+
+ public Pose3d getTargetPose_RobotSpace() {
+ return toPose3D(targetPose_RobotSpace);
+ }
+
+ public Pose2d getCameraPose_TargetSpace2D() {
+ return toPose2D(cameraPose_TargetSpace);
+ }
+
+ public Pose2d getRobotPose_FieldSpace2D() {
+ return toPose2D(robotPose_FieldSpace);
+ }
+
+ public Pose2d getRobotPose_TargetSpace2D() {
+ return toPose2D(robotPose_TargetSpace);
+ }
+
+ public Pose2d getTargetPose_CameraSpace2D() {
+ return toPose2D(targetPose_CameraSpace);
+ }
+
+ public Pose2d getTargetPose_RobotSpace2D() {
+ return toPose2D(targetPose_RobotSpace);
+ }
+
+ @JsonProperty("ta")
+ public double ta;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ @JsonProperty("ts")
+ public double ts;
+
+ public LimelightTarget_Fiducial() {
+ cameraPose_TargetSpace = new double[6];
+ robotPose_FieldSpace = new double[6];
+ robotPose_TargetSpace = new double[6];
+ targetPose_CameraSpace = new double[6];
+ targetPose_RobotSpace = new double[6];
+ }
+ }
+
+ public static class LimelightTarget_Barcode {
+
+ }
+
+ public static class LimelightTarget_Classifier {
+
+ @JsonProperty("class")
+ public String className;
+
+ @JsonProperty("classID")
+ public double classID;
+
+ @JsonProperty("conf")
+ public double confidence;
+
+ @JsonProperty("zone")
+ public double zone;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ public LimelightTarget_Classifier() {
+ }
+ }
+
+ public static class LimelightTarget_Detector {
+
+ @JsonProperty("class")
+ public String className;
+
+ @JsonProperty("classID")
+ public double classID;
+
+ @JsonProperty("conf")
+ public double confidence;
+
+ @JsonProperty("ta")
+ public double ta;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ public LimelightTarget_Detector() {
+ }
+ }
+
+ public static class Results {
+
+ @JsonProperty("pID")
+ public double pipelineID;
+
+ @JsonProperty("tl")
+ public double latency_pipeline;
+
+ @JsonProperty("cl")
+ public double latency_capture;
+
+ public double latency_jsonParse;
+
+ @JsonProperty("ts")
+ public double timestamp_LIMELIGHT_publish;
+
+ @JsonProperty("ts_rio")
+ public double timestamp_RIOFPGA_capture;
+
+ @JsonProperty("v")
+ @JsonFormat(shape = Shape.NUMBER)
+ public boolean valid;
+
+ @JsonProperty("botpose")
+ public double[] botpose;
+
+ @JsonProperty("botpose_wpired")
+ public double[] botpose_wpired;
+
+ @JsonProperty("botpose_wpiblue")
+ public double[] botpose_wpiblue;
+
+ @JsonProperty("t6c_rs")
+ public double[] camerapose_robotspace;
+
+ public Pose3d getBotPose3d() {
+ return toPose3D(botpose);
+ }
+
+ public Pose3d getBotPose3d_wpiRed() {
+ return toPose3D(botpose_wpired);
+ }
+
+ public Pose3d getBotPose3d_wpiBlue() {
+ return toPose3D(botpose_wpiblue);
+ }
+
+ public Pose2d getBotPose2d() {
+ return toPose2D(botpose);
+ }
+
+ public Pose2d getBotPose2d_wpiRed() {
+ return toPose2D(botpose_wpired);
+ }
+
+ public Pose2d getBotPose2d_wpiBlue() {
+ return toPose2D(botpose_wpiblue);
+ }
+
+ @JsonProperty("Retro")
+ public LimelightTarget_Retro[] targets_Retro;
+
+ @JsonProperty("Fiducial")
+ public LimelightTarget_Fiducial[] targets_Fiducials;
+
+ @JsonProperty("Classifier")
+ public LimelightTarget_Classifier[] targets_Classifier;
+
+ @JsonProperty("Detector")
+ public LimelightTarget_Detector[] targets_Detector;
+
+ @JsonProperty("Barcode")
+ public LimelightTarget_Barcode[] targets_Barcode;
+
+ public Results() {
+ botpose = new double[6];
+ botpose_wpired = new double[6];
+ botpose_wpiblue = new double[6];
+ camerapose_robotspace = new double[6];
+ targets_Retro = new LimelightTarget_Retro[0];
+ targets_Fiducials = new LimelightTarget_Fiducial[0];
+ targets_Classifier = new LimelightTarget_Classifier[0];
+ targets_Detector = new LimelightTarget_Detector[0];
+ targets_Barcode = new LimelightTarget_Barcode[0];
+
+ }
+ }
+
+ public static class LimelightResults {
+ @JsonProperty("Results")
+ public Results targetingResults;
+
+ public LimelightResults() {
+ targetingResults = new Results();
+ }
+ }
+
+ private static ObjectMapper mapper;
+
+ /**
+ * Print JSON Parse time to the console in milliseconds
+ */
+ static boolean profileJSON = false;
+
+ static final String sanitizeName(String name) {
+ if (name == "" || name == null) {
+ return "limelight";
+ }
+ return name;
+ }
+
+ private static Pose3d toPose3D(double[] inData) {
+ if (inData.length < 6) {
+ System.err.println("Bad LL 3D Pose Data!");
+ return new Pose3d();
+ }
+ return new Pose3d(
+ new Translation3d(inData[0], inData[1], inData[2]),
+ new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]),
+ Units.degreesToRadians(inData[5])));
+ }
+
+ private static Pose2d toPose2D(double[] inData) {
+ if (inData.length < 6) {
+ System.err.println("Bad LL 2D Pose Data!");
+ return new Pose2d();
+ }
+ Translation2d tran2d = new Translation2d(inData[0], inData[1]);
+ Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5]));
+ return new Pose2d(tran2d, r2d);
+ }
+
+ public static NetworkTable getLimelightNTTable(String tableName) {
+ return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
+ }
+
+ public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
+ return getLimelightNTTable(tableName).getEntry(entryName);
+ }
+
+ public static double getLimelightNTDouble(String tableName, String entryName) {
+ return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0);
+ }
+
+ public static void setLimelightNTDouble(String tableName, String entryName, double val) {
+ getLimelightNTTableEntry(tableName, entryName).setDouble(val);
+ }
+
+ public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) {
+ getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val);
+ }
+
+ public static double[] getLimelightNTDoubleArray(String tableName, String entryName) {
+ return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]);
+ }
+
+ public static String getLimelightNTString(String tableName, String entryName) {
+ return getLimelightNTTableEntry(tableName, entryName).getString("");
+ }
+
+ public static URL getLimelightURLString(String tableName, String request) {
+ String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request;
+ URL url;
+ try {
+ url = new URL(urlString);
+ return url;
+ } catch (MalformedURLException e) {
+ System.err.println("bad LL URL");
+ }
+ return null;
+ }
+ /////
+ /////
+
+ public static double getTX(String limelightName) {
+ return getLimelightNTDouble(limelightName, "tx");
+ }
+
+ public static double getTY(String limelightName) {
+ return getLimelightNTDouble(limelightName, "ty");
+ }
+
+ public static double getTA(String limelightName) {
+ return getLimelightNTDouble(limelightName, "ta");
+ }
+
+ public static double getLatency_Pipeline(String limelightName) {
+ return getLimelightNTDouble(limelightName, "tl");
+ }
+
+ public static double getLatency_Capture(String limelightName) {
+ return getLimelightNTDouble(limelightName, "cl");
+ }
+
+ public static double getCurrentPipelineIndex(String limelightName) {
+ return getLimelightNTDouble(limelightName, "getpipe");
+ }
+
+ public static String getJSONDump(String limelightName) {
+ return getLimelightNTString(limelightName, "json");
+ }
+
+ /**
+ * Switch to getBotPose
+ *
+ * @param limelightName
+ * @return
+ */
+ @Deprecated
+ public static double[] getBotpose(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose");
+ }
+
+ /**
+ * Switch to getBotPose_wpiRed
+ *
+ * @param limelightName
+ * @return
+ */
+ @Deprecated
+ public static double[] getBotpose_wpiRed(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+ }
+
+ /**
+ * Switch to getBotPose_wpiBlue
+ *
+ * @param limelightName
+ * @return
+ */
+ @Deprecated
+ public static double[] getBotpose_wpiBlue(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+ }
+
+ public static double[] getBotPose(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose");
+ }
+
+ public static double[] getBotPose_wpiRed(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+ }
+
+ public static double[] getBotPose_wpiBlue(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+ }
+
+ public static double[] getBotPose_TargetSpace(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+ }
+
+ public static double[] getCameraPose_TargetSpace(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+ }
+
+ public static double[] getTargetPose_CameraSpace(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+ }
+
+ public static double[] getTargetPose_RobotSpace(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+ }
+
+ public static double[] getTargetColor(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "tc");
+ }
+
+ public static double getFiducialID(String limelightName) {
+ return getLimelightNTDouble(limelightName, "tid");
+ }
+
+ public static double getNeuralClassID(String limelightName) {
+ return getLimelightNTDouble(limelightName, "tclass");
+ }
+
+ /////
+ /////
+
+ public static Pose3d getBotPose3d(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose");
+ return toPose3D(poseArray);
+ }
+
+ public static Pose3d getBotPose3d_wpiRed(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+ return toPose3D(poseArray);
+ }
+
+ public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+ return toPose3D(poseArray);
+ }
+
+ public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+ return toPose3D(poseArray);
+ }
+
+ public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+ return toPose3D(poseArray);
+ }
+
+ public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+ return toPose3D(poseArray);
+ }
+
+ public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+ return toPose3D(poseArray);
+ }
+
+ public static Pose3d getCameraPose3d_RobotSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * Gets the Pose2d for easy use with Odometry vision pose estimator
+ * (addVisionMeasurement)
+ *
+ * @param limelightName
+ * @return
+ */
+ public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
+
+ double[] result = getBotPose_wpiBlue(limelightName);
+ return toPose2D(result);
+ }
+
+ /**
+ * Gets the Pose2d for easy use with Odometry vision pose estimator
+ * (addVisionMeasurement)
+ *
+ * @param limelightName
+ * @return
+ */
+ public static Pose2d getBotPose2d_wpiRed(String limelightName) {
+
+ double[] result = getBotPose_wpiRed(limelightName);
+ return toPose2D(result);
+
+ }
+
+ /**
+ * Gets the Pose2d for easy use with Odometry vision pose estimator
+ * (addVisionMeasurement)
+ *
+ * @param limelightName
+ * @return
+ */
+ public static Pose2d getBotPose2d(String limelightName) {
+
+ double[] result = getBotPose(limelightName);
+ return toPose2D(result);
+
+ }
+
+ public static boolean getTV(String limelightName) {
+ return 1.0 == getLimelightNTDouble(limelightName, "tv");
+ }
+
+ /////
+ /////
+
+ public static void setPipelineIndex(String limelightName, int pipelineIndex) {
+ setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
+ }
+
+ /**
+ * The LEDs will be controlled by Limelight pipeline settings, and not by robot
+ * code.
+ */
+ public static void setLEDMode_PipelineControl(String limelightName) {
+ setLimelightNTDouble(limelightName, "ledMode", 0);
+ }
+
+ public static void setLEDMode_ForceOff(String limelightName) {
+ setLimelightNTDouble(limelightName, "ledMode", 1);
+ }
+
+ public static void setLEDMode_ForceBlink(String limelightName) {
+ setLimelightNTDouble(limelightName, "ledMode", 2);
+ }
+
+ public static void setLEDMode_ForceOn(String limelightName) {
+ setLimelightNTDouble(limelightName, "ledMode", 3);
+ }
+
+ public static void setStreamMode_Standard(String limelightName) {
+ setLimelightNTDouble(limelightName, "stream", 0);
+ }
+
+ public static void setStreamMode_PiPMain(String limelightName) {
+ setLimelightNTDouble(limelightName, "stream", 1);
+ }
+
+ public static void setStreamMode_PiPSecondary(String limelightName) {
+ setLimelightNTDouble(limelightName, "stream", 2);
+ }
+
+ public static void setCameraMode_Processor(String limelightName) {
+ setLimelightNTDouble(limelightName, "camMode", 0);
+ }
+
+ public static void setCameraMode_Driver(String limelightName) {
+ setLimelightNTDouble(limelightName, "camMode", 1);
+ }
+
+ /**
+ * Sets the crop window. The crop window in the UI must be completely open for
+ * dynamic cropping to work.
+ */
+ public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin,
+ double cropYMax) {
+ double[] entries = new double[4];
+ entries[0] = cropXMin;
+ entries[1] = cropXMax;
+ entries[2] = cropYMin;
+ entries[3] = cropYMax;
+ setLimelightNTDoubleArray(limelightName, "crop", entries);
+ }
+
+ public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up,
+ double roll, double pitch, double yaw) {
+ double[] entries = new double[6];
+ entries[0] = forward;
+ entries[1] = side;
+ entries[2] = up;
+ entries[3] = roll;
+ entries[4] = pitch;
+ entries[5] = yaw;
+ setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries);
+ }
+
+ /////
+ /////
+
+ public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) {
+ setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData);
+ }
+
+ public static double[] getPythonScriptData(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "llpython");
+ }
+
+ /////
+ /////
+
+ /**
+ * Asynchronously take snapshot.
+ */
+ public static CompletableFuture If you change your main robot class, change the parameter type.
+ *
+ * If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
diff --git a/src/main/java/frc/robot/RebindHat.java b/src/main/java/frc/robot/RebindHat.java
index 2868c6d..593f331 100644
--- a/src/main/java/frc/robot/RebindHat.java
+++ b/src/main/java/frc/robot/RebindHat.java
@@ -1,5 +1,6 @@
package frc.robot;
+
public class RebindHat {
public static double JoystickToYAxis() {
if (RobotContainer.m_armController.getPOV() == -1
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8c88eb4..be67fa4 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,15 +4,20 @@
package frc.robot;
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
- * 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
+ * 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 {
@@ -21,30 +26,42 @@ public class Robot extends TimedRobot {
private RobotContainer m_robotContainer;
/**
- * This function is run when the robot is first started up and should be used for any
+ * This function is run when the robot is first started up and should be used
+ * for any
* initialization code.
*/
@Override
public void robotInit() {
- // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // Instantiate our RobotContainer. This will perform all our button bindings,
+ // and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
- // Test values for the servo on startup (This is just a test that should be removed if it
+ // Test values for the servo on startup (This is just a test that should be
+ // removed if it
// actually works)
+
+ DataLogManager.start();
+ DriverStation.startDataLog(DataLogManager.getLog());
}
/**
- * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+ * This function is called every 20 ms, 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
+ *
+ * This runs after the mode specific periodic functions, but before LiveWindow
+ * and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
- // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
- // commands, running already-scheduled commands, removing finished or interrupted commands,
- // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // Runs the Scheduler. This is responsible for polling buttons, adding
+ // newly-scheduled
+ // commands, running already-scheduled commands, removing finished or
+ // interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the
+ // robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
SmartDashboard.putData(CommandScheduler.getInstance());
@@ -52,13 +69,17 @@ public void robotPeriodic() {
/** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {}
+ public void disabledInit() {
+ }
@Override
- public void disabledPeriodic() {}
+ public void disabledPeriodic() {
+ }
- /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
-
+ /**
+ * This autonomous runs the autonomous command selected by your
+ * {@link RobotContainer} class.
+ */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -71,7 +92,8 @@ public void autonomousInit() {
/** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {}
+ public void autonomousPeriodic() {
+ }
@Override
public void teleopInit() {
@@ -88,7 +110,8 @@ public void teleopInit() {
/** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {}
+ public void teleopPeriodic() {
+ }
@Override
public void testInit() {
@@ -98,13 +121,16 @@ public void testInit() {
/** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {}
+ public void testPeriodic() {
+ }
/** This function is called once when the robot is first started up. */
@Override
- public void simulationInit() {}
+ public void simulationInit() {
+ }
/** This function is called periodically whilst in simulation. */
@Override
- public void simulationPeriodic() {}
+ public void simulationPeriodic() {
+ }
}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index bff66d3..d8d2ae8 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,65 +4,103 @@
package frc.robot;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.commands.*;
import frc.robot.subsystems.*;
+import io.github.oblarg.oblog.Logger;
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in
+ * the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of
+ * the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
+ public static boolean inverted = false;
+
+ // INSTANTIATES ALL JOYSTICKS
+
+ /** @deprecated The drivers are much more comfortable with an xbox controller */
+ @Deprecated
public static final Joystick m_armController = new Joystick(0);
+
public static final XboxController m_armController2 = new XboxController(1);
public static final XboxController m_driverController = new XboxController(2);
// INSTANTIATES ALL SUBSYSTEMS
private final Arm m_Arm = new Arm();
- private final Claw m_claw = new Claw();
+ private final Claw m_Claw = new Claw();
private final Tank m_Tank = new Tank();
- private final Turret m_Turret = new Turret();
- private final PID m_PID = new PID();
+ private final AutoBalence m_Balance = new AutoBalence();
private final PDP m_PDP = new PDP();
+ private final AutoPiecePickUp m_Vision = new AutoPiecePickUp(m_Tank, m_Claw);
+ private final PoleTracking m_PoleTracking = new PoleTracking(m_Tank);
// INSTANTIATES ALL COMMANDS
- private final OpenClawCommand m_OpenClawCommand = new OpenClawCommand(m_claw);
- private final CloseClawCommand m_CloseClawCommand = new CloseClawCommand(m_claw);
- private final MoveArmXCommand m_MoveArmXCommand = new MoveArmXCommand(m_Turret);
+ private final MoveClawCommand m_MoveClawCommand = new MoveClawCommand(m_Claw);
private final MoveArmYCommand m_MoveArmYCommand = new MoveArmYCommand(m_Arm);
private final DriveCommand m_DriveCommand = new DriveCommand(m_Tank);
- private final BalanceCommand m_BalanceCommand = new BalanceCommand(m_PID, m_Tank);
- private final PlaceGamePieceCommand m_PlaceGamePieceCommand =
- new PlaceGamePieceCommand(m_Tank, m_claw, m_Arm);
- private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank);
- private final IncreaseMaxSpeedCommand m_IncreaseMaxSpeedCommand =
- new IncreaseMaxSpeedCommand(m_Tank);
- private final DecreaseMaxSpeedCommand m_DecreaseMaxSpeedCommand =
- new DecreaseMaxSpeedCommand(m_Tank);
- private final SwitchIdleModeCommmand m_SwitchIdleModeCommmand =
- new SwitchIdleModeCommmand(m_Tank);
+ private final BalanceCommand m_BalanceCommand = new BalanceCommand(m_Balance, m_Tank);
+ private final AutoCommand m_AutoCommand = new AutoCommand(m_Balance, m_Tank, m_Arm, m_Claw);
+ private final InvertDriveCommand m_InvertDriveCommand = new InvertDriveCommand(m_Tank, this);
+ private final PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand = new PlaceConeSecondLevelCommand(m_Tank,
+ m_Arm, m_Claw);
+ private final IncreaseMaxSpeedCommand m_IncreaseMaxSpeedCommand = new IncreaseMaxSpeedCommand(m_Tank);
+ private final DecreaseMaxSpeedCommand m_DecreaseMaxSpeedCommand = new DecreaseMaxSpeedCommand(m_Tank);
private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank);
+ private final PlaceCubeSecondLevelCommand m_PlaceCubeSecondLevelCommand = new PlaceCubeSecondLevelCommand(m_Tank,
+ m_Arm, m_Claw);
+ private final TrackPiece m_TrackPiece = new TrackPiece(m_Vision, m_Tank, m_Claw);
+ private final TrackPole m_TrackPole = new TrackPole(m_PoleTracking, m_Tank);
+
+ // private final XTracking m_XTracking = new XTracking(m_Tank, null, m_Target);
+ // private final YTracking m_YTracking = new YTr
- /** The container for the robot. Contains subsystems, OI devices, and commands. */
+ private GenericEntry timeWidget = Shuffleboard.getTab("stuff").add("time", 0).withWidget("Match Time").getEntry();
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
// add a few things to the shuffleboard
// joysticks
- Shuffleboard.getTab("Driver").add("Left Stick Y", m_driverController.getRawAxis(1));
- Shuffleboard.getTab("Driver").add("Right Stick X", m_driverController.getRawAxis(4));
- Shuffleboard.getTab("Driver")
- .add("Left Stick Y Adjusted", getDriverControllerLeftStickYAdjusted());
- Shuffleboard.getTab("Driver")
- .add("Right Stick X Adjusted", getDriverControllerRightStickXAdjusted());
+ var subsystemsTab = Shuffleboard.getTab("Subsystems");
+ var mainTab = Shuffleboard.getTab("Main");
+ subsystemsTab.add(m_Arm);
+ subsystemsTab.add(m_AutoCommand);
+ subsystemsTab.add(m_BalanceCommand);
+ subsystemsTab.add(m_DecreaseMaxSpeedCommand);
+ subsystemsTab.add(m_DriveCommand);
+ subsystemsTab.add(m_FineDriveCommand);
+ subsystemsTab.add(m_IncreaseMaxSpeedCommand);
+ subsystemsTab.add(m_MoveArmYCommand);
+ subsystemsTab.add(m_PDP);
+ subsystemsTab.add(m_PlaceCubeSecondLevelCommand);
+ subsystemsTab.add(m_Vision);
+
+ subsystemsTab.add(m_Balance);
+ subsystemsTab.add(m_PlaceConeSecondLevelCommand);
+ subsystemsTab.add(m_Tank);
+ subsystemsTab.add(m_Claw);
+
+ Logger.configureLoggingAndConfig(this, false);
+
}
/**
@@ -73,26 +111,34 @@ public RobotContainer() {
private void configureButtonBindings() {
new JoystickButton(m_driverController, XboxController.Button.kY.value)
.whileTrue(m_BalanceCommand);
- new JoystickButton(m_driverController, XboxController.Button.kA.value)
- .whileTrue(m_PlaceGamePieceCommand);
-
new JoystickButton(m_driverController, XboxController.Button.kStart.value)
.whileTrue(m_IncreaseMaxSpeedCommand);
new JoystickButton(m_driverController, XboxController.Button.kBack.value)
.whileTrue(m_DecreaseMaxSpeedCommand);
new JoystickButton(m_driverController, XboxController.Button.kX.value)
- .whileTrue(m_SwitchIdleModeCommmand);
+ .toggleOnTrue(m_InvertDriveCommand);
+ // new JoystickButton(m_armController2, XboxController.Button.kX.value)
+ // .whileTrue(m_CloseClawCommand);
+ // new JoystickButton(m_armController2, XboxController.Button.kY.value)
+ // .whileTrue(m_OpenClawCommand);
new JoystickButton(m_armController2, XboxController.Button.kLeftBumper.value)
- .whileTrue(m_CloseClawCommand);
- new JoystickButton(m_armController2, XboxController.Button.kRightBumper.value)
- .whileTrue(m_OpenClawCommand);
-
- new JoystickButton(m_armController2, XboxController.Button.kA.value)
.whileTrue(m_FineDriveCommand);
+ new JoystickButton(m_armController2, XboxController.Button.kY.value).whileTrue(m_PlaceConeSecondLevelCommand);
+
+ new JoystickButton(m_armController2, XboxController.Button.kX.value).whileTrue(m_TrackPiece);
+ new JoystickButton(m_armController2, XboxController.Button.kB.value).whileTrue(m_TrackPole);
+
+ // new JoystickButton(m_armController2, XboxController.Button.kB.value)
+ // .whileTrue(m_PlaceConeSecondLevelCommand);
}
- public static double getDriverControllerLeftStickY() {
+ /**
+ * gets the value of the left stick on the driver controller to be used for
+ * turning
+ */
+ public static double getTurningStickInput() {
+ // uses the left stick on the driver controller
double axis = m_driverController.getRawAxis(1);
// 0.60 is the minimum amount of power we need
if (Math.abs(axis) < 0.01) {
@@ -101,18 +147,25 @@ public static double getDriverControllerLeftStickY() {
return axis * -1;
}
- public static double getDriverControllerLeftStickYAdjusted() {
- double val = getDriverControllerLeftStickY();
+ /**
+ * returns value used for turning
+ */
+ public static double getAdjustedTurningStickInput() {
+ double val = getTurningStickInput();
if (val > 0) {
- return (val * val + 0.6) / 1.6;
+ return (val * val + 0.3) / 1.3;
} else if (val < 0) {
- return -(val * val + 0.6) / 1.6;
+ return -(val * val + 0.3) / 1.3;
} else {
return 0;
}
}
- public static double getDriverControllerRightStickX() {
+ /**
+ * gets the value of the right stick on the driver controller to be used for
+ * forward and backward movement
+ */
+ public static double getForwardStickInput() {
double axis = m_driverController.getRawAxis(4);
if (Math.abs(axis) < 0.01) {
axis = 0;
@@ -120,17 +173,27 @@ public static double getDriverControllerRightStickX() {
return axis;
}
- public static double getDriverControllerRightStickXAdjusted() {
- double val = getDriverControllerRightStickX();
+ /** returns value used for forward and backward movement */
+ public static double getAdjustedForwardStickInput() {
+ double val = getForwardStickInput();
if (val > 0) {
- return (val * val + 0.6) / 1.6;
+ return (val * val + 0.1) / 1.1;
} else if (val < 0) {
- return -(val * val + 0.6) / 1.6;
+ return -(val * val + 0.1) / 1.1;
} else {
return 0;
}
}
+ /**
+ * This function gets the joystick value of the left stick on the arm controller
+ * The left stick is used to move the arm up and down
+ * The function returns the value of the left stick
+ * The value of the left stick is put on the smart dashboard
+ *
+ * @deprecated This controller is no longer used
+ */
+ @Deprecated
public static double getJoystickArmControllerLeftStickY() {
double axis = m_armController.getRawAxis(1);
SmartDashboard.putNumber("arm left stick y", axis);
@@ -141,7 +204,19 @@ public static double getJoystickArmControllerLeftStickY() {
return axis;
}
- public static double getJoystickArmControllerRightStickX() {
+ /**
+ * This function gets the joystick x-value of the right stick on the arm
+ * controller
+ * The right stick is used to move the arm left and right
+ * The function returns the value of the right stick
+ * The value of the right stick is put on the smart dashboard
+ *
+ * @deprecated The turret is no longer on the arm, and this controller is no
+ * longer used
+ */
+ @Deprecated
+ public static double getJoystickArmControllerRightStickX() { // "do not forget to remove this deprecated code someday"
+ // lmao never
double axis = m_armController.getRawAxis(4);
SmartDashboard.putNumber("arm right stick x", axis);
@@ -151,6 +226,11 @@ public static double getJoystickArmControllerRightStickX() {
return axis;
}
+ /**
+ * This function gets the y-value of the left stick on the arm controller
+ * The left stick is used to move the arm up and down
+ * The function returns the value of the left stick
+ */
public static double getControllerLeftStickY() {
double axis = m_armController2.getRawAxis(1);
if (Math.abs(axis) < 0.1) {
@@ -167,17 +247,39 @@ public static double getControllerRightStickX() {
return axis;
}
- public static double getJoystickArmY() {
- double axis = m_armController.getRawAxis(1);
- if (Math.abs(axis) < 0.15) {
+ public static double getControllerRightStickY() {
+ double axis = m_armController2.getRawAxis(5);
+ if (Math.abs(axis) < 0.4) {
+ axis = 0;
+ }
+ return -axis;
+ }
+
+ public static void rumbleGabeController(double rumble) {
+ XboxController controller = m_driverController;
+ controller.setRumble(RumbleType.kBothRumble, rumble);
+ }
+
+ /**
+ * gets value from the right trigger axis (yes, it's an axis) for grabbing up
+ * the game piece
+ */
+
+ public static double getClawInValue() {
+ double axis = m_armController2.getRightTriggerAxis();
+ if (Math.abs(axis) < 0.01) {
axis = 0;
}
return axis;
}
- public static double getJoystickArmTwist() {
- double axis = m_armController.getRawAxis(2);
- if (Math.abs(axis) < 0.15) {
+ /**
+ * gets value from the left trigger axis (yes, it's an axis) for ejecting the
+ * game piece
+ */
+ public static double getClawOutValue() {
+ double axis = m_armController2.getLeftTriggerAxis();
+ if (Math.abs(axis) < 0.01) {
axis = 0;
}
return axis;
@@ -189,9 +291,23 @@ public Command getAutonomousCommand() {
}
public void initTeleop() {
- // Set the default tank command to DriveCommand
- m_Tank.setDefaultCommand(m_DriveCommand);
- m_Turret.setDefaultCommand(m_MoveArmXCommand);
+ if (inverted) {
+ m_Tank.setDefaultCommand(m_InvertDriveCommand);
+ } else {
+ m_Tank.setDefaultCommand(m_DriveCommand);
+ }
m_Arm.setDefaultCommand(m_MoveArmYCommand);
+ m_Claw.setDefaultCommand(m_MoveClawCommand);
+ }
+
+ public void initTest() {
+ // Set the default tank command to DriveCommand
+
}
+
+ public void robotPeriodic() {
+ Logger.updateEntries();
+ timeWidget.setDouble(DriverStation.getMatchTime());
+ }
+
}
diff --git a/src/main/java/frc/robot/TrackingType.java b/src/main/java/frc/robot/TrackingType.java
deleted file mode 100644
index a7e1d2d..0000000
--- a/src/main/java/frc/robot/TrackingType.java
+++ /dev/null
@@ -1,10 +0,0 @@
-package frc.robot;
-
-public enum TrackingType {
- /**
- * Literally just an enum I made because I'm too lazy to do anything else. Besides, it's good
- * training.
- */
- tape,
- tag
-}
diff --git a/src/main/java/frc/robot/commands/AutoCommand.java b/src/main/java/frc/robot/commands/AutoCommand.java
index c8826ad..6811e70 100644
--- a/src/main/java/frc/robot/commands/AutoCommand.java
+++ b/src/main/java/frc/robot/commands/AutoCommand.java
@@ -7,9 +7,35 @@
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.*;
-
+/**
+ * This command will run the autonomous routine. In it, you can add commands to
+ * be run in sequence.
+ */
public class AutoCommand extends SequentialCommandGroup {
- public AutoCommand(PID m_pid, Tank m_driveBase) {
- addCommands(new BalanceCommand(m_pid, m_driveBase));
+ public AutoCommand(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) {
+
+ // pick one
+ // sideAutoWCone(m_pid,m_driveBase,m_arm,m_claw);
+ // sideAutoWCube(m_pid,m_driveBase,m_arm,m_claw);
+ // middleAutoWCone(m_pid,m_driveBase,m_arm,m_claw);
+ // middleAutoWCube(m_pid,m_driveBase,m_arm,m_claw);
+
+ }
+
+ // TODO document this
+ private void sideAutoWCone(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) {
+ addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw));
+ }
+
+ private void sideAutoWCube(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) {
+ addCommands(new PlaceCubeSecondLevelCommand(m_driveBase, m_arm, m_claw));
+ }
+
+ private void middleAutoWCone(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) {
+ addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw), new BalanceCommand(m_pid, m_driveBase));
+ }
+
+ private void middleAutoWCube(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) {
+ addCommands(new PlaceCubeSecondLevelCommand(m_driveBase, m_arm, m_claw), new BalanceCommand(m_pid, m_driveBase));
}
}
diff --git a/src/main/java/frc/robot/commands/BalanceCommand.java b/src/main/java/frc/robot/commands/BalanceCommand.java
index f54da94..0bcd8be 100644
--- a/src/main/java/frc/robot/commands/BalanceCommand.java
+++ b/src/main/java/frc/robot/commands/BalanceCommand.java
@@ -8,22 +8,26 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants;
-import frc.robot.subsystems.PID;
+import frc.robot.subsystems.AutoBalence;
+import frc.robot.subsystems.AutoPiecePickUp;
import frc.robot.subsystems.Tank;
-/** An example command that uses an example subsystem. */
+/**
+ * This command balances the robot on the charge station. Default auto command.
+ */
public class BalanceCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
- private final PID pid;
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+ private final AutoBalence pidLoop;
private final Tank driveBase;
- private double initPitch, max;
+ private double initPitch, maxPitch;
private boolean onRamp;
- public BalanceCommand(PID m_pid, Tank m_driveBase) {
- pid = m_pid;
+ /** Create a new {@link BalanceCommand}. */
+ public BalanceCommand(AutoBalence m_pid, Tank m_driveBase) {
+ pidLoop = m_pid;
driveBase = m_driveBase;
- addRequirements(pid);
+ addRequirements(pidLoop);
addRequirements(driveBase);
}
@@ -34,46 +38,50 @@ public void initialize() {
driveBase.rightFront.setIdleMode(IdleMode.kBrake);
driveBase.rightRear.setIdleMode(IdleMode.kBrake);
- pid.resetPitch();
- initPitch = pid.gyro.getPitch();
- SmartDashboard.putNumber("init pitch", initPitch);
- max = 0;
+ pidLoop.resetPitch();
+ initPitch = pidLoop.gyro.getPitch();
+ SmartDashboard.putNumber("inital pitch", initPitch);
+ maxPitch = 0;
onRamp = false;
+ // start driving forward
+ driveBase.setAllMotors(0.5);
}
// avoid while loops inside execute
@Override
public void execute() {
- double pitchOffset = pid.gyro.getPitch() - initPitch;
+ double pitchOffset = initPitch - pidLoop.gyro.getPitch();
SmartDashboard.putNumber("pitch offset", pitchOffset);
+ SmartDashboard.putBoolean("onRamp", onRamp);
+ SmartDashboard.putNumber("max offset", maxPitch);
+ SmartDashboard.putNumber(
+ "motor speed", Constants.OperatorConstants.Pconstant * (pitchOffset));
+ /*
+ * if (pitchOffset > maxPitch) {
+ * maxPitch = pitchOffset;
+ * }
+ */
- if (pitchOffset > max) {
- max = pitchOffset;
- }
- SmartDashboard.putNumber("max offset", max);
-
- if (pitchOffset < 15.7 && !onRamp) {
- driveBase.setAllMotors(0.5);
- SmartDashboard.putBoolean("onRamp", onRamp);
-
- } else {
+ // set boolean to true when robot is on ramp to run p loop
+ if (!onRamp && pitchOffset >= 16) {
onRamp = true;
- SmartDashboard.putBoolean("onRamp", onRamp);
+ }
+ if (onRamp) {
+ // dead zone of tilt
if (Math.abs(pitchOffset) < 2) {
driveBase.setAllMotors(0);
} else {
- driveBase.setAllMotors(
- 0.15 * (pitchOffset / Constants.OperatorConstants.ProportionalDivisor));
-
- SmartDashboard.putNumber(
- "motor speed", 0.15 * (pitchOffset / Constants.OperatorConstants.ProportionalDivisor));
+ // run p loop
+ driveBase.setAllMotors(Constants.OperatorConstants.Pconstant * (pitchOffset));
}
}
}
@Override
- public void end(boolean interrupted) {}
+ public void end(boolean interrupted) {
+ // not used
+ }
@Override
public boolean isFinished() {
diff --git a/src/main/java/frc/robot/commands/CloseClawCommand.java b/src/main/java/frc/robot/commands/CloseClawCommand.java
deleted file mode 100644
index a5ad783..0000000
--- a/src/main/java/frc/robot/commands/CloseClawCommand.java
+++ /dev/null
@@ -1,38 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystems.Claw;
-
-public class CloseClawCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
- private final Claw m_subsystem;
-
- public CloseClawCommand(Claw subsystem) {
- m_subsystem = subsystem;
- addRequirements(subsystem);
- }
-
- @Override
- public void initialize() {}
-
- @Override
- public void execute() {
- // check if the power draw for the motor is over 2 amps -- if so, end the command
- // if (m_subsystem.get)
- m_subsystem.closeClaw();
- }
-
- @Override
- public void end(boolean interrupted) {
- m_subsystem.stopClaw();
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-}
diff --git a/src/main/java/frc/robot/commands/DecreaseMaxSpeedCommand.java b/src/main/java/frc/robot/commands/DecreaseMaxSpeedCommand.java
index 61aacc8..26c03c0 100644
--- a/src/main/java/frc/robot/commands/DecreaseMaxSpeedCommand.java
+++ b/src/main/java/frc/robot/commands/DecreaseMaxSpeedCommand.java
@@ -7,8 +7,9 @@
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Tank;
+/** Decrease the maximum speed of the drivebase. */
public class DecreaseMaxSpeedCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final Tank m_subsystem;
public DecreaseMaxSpeedCommand(Tank subsystem) {
@@ -27,7 +28,8 @@ public void execute() {
}
@Override
- public void end(boolean interrupted) {}
+ public void end(boolean interrupted) {
+ }
@Override
public boolean isFinished() {
diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java
index a645116..c8852a7 100644
--- a/src/main/java/frc/robot/commands/DriveCommand.java
+++ b/src/main/java/frc/robot/commands/DriveCommand.java
@@ -11,23 +11,31 @@
import frc.robot.subsystems.Tank;
public class DriveCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final Tank m_drivebase;
+ private boolean isFinishedVar = false;
+
+
+
+
public DriveCommand(Tank subsystem) {
m_drivebase = subsystem;
addRequirements(m_drivebase);
}
@Override
- public void initialize() {}
+ public void initialize() {
+ // nothing to do here
+ }
@Override
public void execute() {
// double check getMaxSpeed(), might be wrong
+
m_drivebase.arcadeDrive(
- RobotContainer.getDriverControllerLeftStickYAdjusted() * Constants.CanConstants.maxSpeed,
- RobotContainer.getDriverControllerRightStickXAdjusted() * 0.65);
+ /* rotation */RobotContainer.getAdjustedTurningStickInput() * Constants.CanConstants.maxSpeed,
+ /* speed */RobotContainer.getAdjustedForwardStickInput() * 0.7);
SmartDashboard.putNumber("maxSpeed", Constants.CanConstants.maxSpeed);
}
@@ -39,7 +47,11 @@ public void end(boolean interrupted) {
@Override
public boolean isFinished() {
- return false;
+ return isFinishedVar;
+ }
+
+ public void setFinished(boolean x) {
+ isFinishedVar = x;
}
@Override
diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java
new file mode 100644
index 0000000..c932756
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DriveForward.java
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Tank;
+
+public class DriveForward extends CommandBase {
+ /** Creates a new DriveForward. */
+ private Tank m_tank;
+
+ public DriveForward(Tank tank) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ m_tank = tank;
+ addRequirements(m_tank);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+
+ m_tank.arcadeDrive(0.5, 0);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ m_tank.arcadeDrive(0, 0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/DriveForwardAndPickUpPieceCommand.java b/src/main/java/frc/robot/commands/DriveForwardAndPickUpPieceCommand.java
new file mode 100644
index 0000000..ce07679
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DriveForwardAndPickUpPieceCommand.java
@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class DriveForwardAndPickUpPieceCommand extends ParallelDeadlineGroup {
+ /** Creates a new DriveForwardAndPickUpPieceCommand. */
+ public DriveForwardAndPickUpPieceCommand() {
+ // Add the deadline command in the super() call. Add other commands using
+ // addCommands().
+ super(new InstantCommand());
+ // addCommands(new FooCommand(), new BarCommand());
+ }
+}
diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java
index 63992c9..dfcf11d 100644
--- a/src/main/java/frc/robot/commands/ExampleCommand.java
+++ b/src/main/java/frc/robot/commands/ExampleCommand.java
@@ -8,7 +8,7 @@
import frc.robot.subsystems.ExampleSubsystem;
public class ExampleCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField", "PMD.UnusedImport"})
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField", "PMD.UnusedImport" })
private final ExampleSubsystem m_subsystem; // NOPMD
public ExampleCommand(ExampleSubsystem subsystem) {
diff --git a/src/main/java/frc/robot/commands/FineDriveCommand.java b/src/main/java/frc/robot/commands/FineDriveCommand.java
index 29e9eb4..f7f2e1b 100644
--- a/src/main/java/frc/robot/commands/FineDriveCommand.java
+++ b/src/main/java/frc/robot/commands/FineDriveCommand.java
@@ -4,32 +4,43 @@
package frc.robot.commands;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.RebindHat;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
import frc.robot.subsystems.Tank;
public class FineDriveCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final Tank m_drivebase;
+ private double speedMult = 0.43;
+ private GenericEntry multWidget;
public FineDriveCommand(Tank subsystem) {
m_drivebase = subsystem;
addRequirements(m_drivebase);
+ multWidget = Shuffleboard.getTab("Fine Drive").add("Speed", speedMult).getEntry();
}
@Override
- public void initialize() {}
+ public void initialize() {
+
+ }
@Override
public void execute() {
// double check getMaxSpeed(), might be wrong
+ RobotContainer.rumbleGabeController(1);
m_drivebase.arcadeDrive(
- RebindHat.ControllerToYAxis() * 0.43, RebindHat.ControllerToXAxis() * 0.43);
+ RebindHat.ControllerToYAxis() * multWidget.getDouble(0), RebindHat.ControllerToXAxis() * multWidget.getDouble(0));
}
@Override
public void end(boolean interrupted) {
m_drivebase.arcadeDrive(0, 0);
+ RobotContainer.rumbleGabeController(0);
}
@Override
diff --git a/src/main/java/frc/robot/commands/IncreaseMaxSpeedCommand.java b/src/main/java/frc/robot/commands/IncreaseMaxSpeedCommand.java
index f9be623..eb5287c 100644
--- a/src/main/java/frc/robot/commands/IncreaseMaxSpeedCommand.java
+++ b/src/main/java/frc/robot/commands/IncreaseMaxSpeedCommand.java
@@ -8,7 +8,7 @@
import frc.robot.subsystems.Tank;
public class IncreaseMaxSpeedCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final Tank m_subsystem;
public IncreaseMaxSpeedCommand(Tank subsystem) {
@@ -26,7 +26,8 @@ public void execute() {
}
@Override
- public void end(boolean interrupted) {}
+ public void end(boolean interrupted) {
+ }
@Override
public boolean isFinished() {
diff --git a/src/main/java/frc/robot/commands/InvertDriveCommand.java b/src/main/java/frc/robot/commands/InvertDriveCommand.java
new file mode 100644
index 0000000..b4540fd
--- /dev/null
+++ b/src/main/java/frc/robot/commands/InvertDriveCommand.java
@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Tank;
+
+public class InvertDriveCommand extends CommandBase {
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+ private final Tank m_drivebase;
+ private final RobotContainer robot;
+
+ public InvertDriveCommand(Tank subsystem, RobotContainer robot) {
+ m_drivebase = subsystem;
+ this.robot = robot;
+
+ addRequirements(m_drivebase);
+ }
+
+ @Override
+ public void initialize() {
+ RobotContainer.inverted = true;
+ robot.initTeleop();
+ }
+
+ @Override
+ public void execute() {
+ // double check getMaxSpeed(), might be wrong
+ m_drivebase.arcadeDrive(
+ -RobotContainer.getAdjustedTurningStickInput() * Constants.CanConstants.maxSpeed,
+ RobotContainer.getAdjustedForwardStickInput() * 0.75);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_drivebase.arcadeDrive(0, 0);
+ RobotContainer.inverted = false;
+ robot.initTeleop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return false;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/MoveArmXCommand.java b/src/main/java/frc/robot/commands/MoveArmXCommand.java
index 75d0714..3098b2f 100644
--- a/src/main/java/frc/robot/commands/MoveArmXCommand.java
+++ b/src/main/java/frc/robot/commands/MoveArmXCommand.java
@@ -9,7 +9,7 @@
import frc.robot.subsystems.Turret;
public class MoveArmXCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final Turret m_subsystem;
public MoveArmXCommand(Turret subsystem) {
@@ -18,15 +18,17 @@ public MoveArmXCommand(Turret subsystem) {
}
@Override
- public void initialize() {}
+ public void initialize() {
+ }
@Override
public void execute() {
- m_subsystem.rotateTurret(RobotContainer.getControllerRightStickX());
+ m_subsystem.rotateTurret(RobotContainer.getControllerRightStickX() / 1.5);
}
@Override
- public void end(boolean interrupted) {}
+ public void end(boolean interrupted) {
+ }
@Override
public boolean isFinished() {
diff --git a/src/main/java/frc/robot/commands/MoveArmYCommand.java b/src/main/java/frc/robot/commands/MoveArmYCommand.java
index cf5c3a4..8559d65 100644
--- a/src/main/java/frc/robot/commands/MoveArmYCommand.java
+++ b/src/main/java/frc/robot/commands/MoveArmYCommand.java
@@ -9,7 +9,7 @@
import frc.robot.subsystems.Arm;
public class MoveArmYCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final Arm m_subsystem;
public MoveArmYCommand(Arm subsystem) {
@@ -18,7 +18,8 @@ public MoveArmYCommand(Arm subsystem) {
}
@Override
- public void initialize() {}
+ public void initialize() {
+ }
@Override
public void execute() {
@@ -26,7 +27,8 @@ public void execute() {
}
@Override
- public void end(boolean interrupted) {}
+ public void end(boolean interrupted) {
+ }
@Override
public boolean isFinished() {
diff --git a/src/main/java/frc/robot/commands/OpenClawCommand.java b/src/main/java/frc/robot/commands/MoveClawCommand.java
similarity index 60%
rename from src/main/java/frc/robot/commands/OpenClawCommand.java
rename to src/main/java/frc/robot/commands/MoveClawCommand.java
index 05f177d..5b8c433 100644
--- a/src/main/java/frc/robot/commands/OpenClawCommand.java
+++ b/src/main/java/frc/robot/commands/MoveClawCommand.java
@@ -5,25 +5,29 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.RobotContainer;
import frc.robot.subsystems.Claw;
-public class OpenClawCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+public class MoveClawCommand extends CommandBase {
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final Claw m_subsystem;
- public OpenClawCommand(Claw subsystem) {
+ public MoveClawCommand(Claw subsystem) {
m_subsystem = subsystem;
addRequirements(subsystem);
}
@Override
public void initialize() {
- // no initialization needed
}
@Override
public void execute() {
- m_subsystem.openClaw();
+ if (Math.abs(RobotContainer.getClawOutValue()) < 0.2) {
+ m_subsystem.clawMotor.set(RobotContainer.getClawInValue() * 0.7);
+ } else {
+ m_subsystem.clawMotor.set(-RobotContainer.getClawOutValue() * 0.7);
+ }
}
@Override
diff --git a/src/main/java/frc/robot/commands/PickUpPiece.java b/src/main/java/frc/robot/commands/PickUpPiece.java
new file mode 100644
index 0000000..7df5c5e
--- /dev/null
+++ b/src/main/java/frc/robot/commands/PickUpPiece.java
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class PickUpPiece extends SequentialCommandGroup {
+ /** Creates a new PickUpPiece. */
+ public PickUpPiece() {
+ // Add your commands in the addCommands() call, e.g.
+ // addCommands(new FooCommand(), new BarCommand());
+ addCommands();
+ }
+}
diff --git a/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java b/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java
new file mode 100644
index 0000000..3ca2cde
--- /dev/null
+++ b/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java
@@ -0,0 +1,85 @@
+// Copyright (c) Elliot Snyder, Parker Brownlowe, and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WTFPL license file in the internet. Any questions about the mental
+// health of the authors can be shouted into the endless void of the internet.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.*;
+
+public class PlaceConeSecondLevelCommand extends CommandBase {
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+ private final Tank drive;
+ private final Claw claw;
+ private final Arm arm;
+ private boolean coneOut;
+ private boolean clawDown;
+ private boolean firstStep = true;
+ private boolean armBack;
+
+ public PlaceConeSecondLevelCommand(Tank drive, Arm arm, Claw claw) {
+ this.drive = drive;
+ this.arm = arm;
+ this.claw = claw;
+ addRequirements(drive);
+ addRequirements(arm);
+ }
+
+ @Override
+ public void initialize() {
+ drive.leftFront.getEncoder().setPosition(0);
+ drive.leftRear.getEncoder().setPosition(0);
+ drive.rightFront.getEncoder().setPosition(0);
+ drive.rightRear.getEncoder().setPosition(0);
+
+ arm.armMotor.getEncoder().setPosition(0);
+ // starting movement
+ claw.closeClaw();
+ arm.armMotor.set(0.5);
+ }
+
+ // double check constants
+ @Override
+ public void execute() {
+
+ if (!clawDown && Math.abs(arm.armMotor.getEncoder().getPosition()) >= 115) {
+ claw.stopClaw();
+ arm.armMotor.set(0);
+ drive.setAllMotors(-0.3); // move back so that cone falls in
+ clawDown = true;
+ }
+ if (!coneOut && Math.abs(drive.getAverageDistance()) >= 1.00) {
+ coneOut = true;
+ arm.armMotor.set(-0.5);
+ }
+ if (coneOut && !armBack && arm.armMotor.getEncoder().getPosition() < 10) {
+ armBack = true;
+ arm.armMotor.set(0);
+ }
+ // stops after reaching a certian point
+ /*
+ * if (Math.abs(drive.getAverageDistance()) >= 4.95) {
+ * drive.setAllMotors(0);
+ * }
+ */
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ drive.leftFront.getEncoder().setPosition(0);
+ drive.leftRear.getEncoder().setPosition(0);
+ drive.rightFront.getEncoder().setPosition(0);
+ drive.rightRear.getEncoder().setPosition(0);
+ arm.armMotor.getEncoder().setPosition(0);
+
+ drive.setAllMotors(0);
+ arm.armMotor.set(0); // stop the arm motor
+ claw.stopClaw();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return Math.abs(drive.getAverageDistance()) >= 4.95;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/PlaceCubeSecondLevelCommand.java b/src/main/java/frc/robot/commands/PlaceCubeSecondLevelCommand.java
new file mode 100644
index 0000000..1da8e44
--- /dev/null
+++ b/src/main/java/frc/robot/commands/PlaceCubeSecondLevelCommand.java
@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.*;
+
+public class PlaceCubeSecondLevelCommand extends CommandBase {
+ @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+ private final Tank drive;
+ private final Claw claw;
+ private final Arm arm;
+ private GenericEntry widget;
+ private int counter;
+ private boolean finalStep;
+ private boolean firstStep = true;
+ private boolean done;
+
+ public PlaceCubeSecondLevelCommand(Tank drive, Arm arm, Claw claw) {
+ this.drive = drive;
+ this.arm = arm;
+ this.claw = claw;
+ addRequirements(drive);
+ addRequirements(arm);
+
+ }
+
+ @Override
+ public void initialize() {
+ /*
+ * drive.leftFront.getEncoder().setPosition(0);
+ * drive.leftRear.getEncoder().setPosition(0);
+ * drive.rightFront.getEncoder().setPosition(0);
+ * drive.rightRear.getEncoder().setPosition(0);
+ */
+ counter = 0;
+ finalStep = false;
+ done = false;
+ arm.armMotor.getEncoder().setPosition(0);
+ widget = Shuffleboard.getTab("stuff").add("arm thing", arm.armMotor.getEncoder().getPosition()).getEntry();
+ claw.closeClaw();
+ arm.armMotor.set(0.25);
+ }
+
+ // double check constants
+ @Override
+ public void execute() {
+ widget.setDouble(arm.armMotor.getEncoder().getPosition());
+ counter++;
+
+ if (arm.armMotor.getEncoder().getPosition() >= 60 && firstStep) {
+ firstStep = false;
+ arm.armMotor.set(0);
+ claw.openClaw();
+ int counter = 0;
+ }
+
+ // go over charge station
+
+ if (counter > 10 && !firstStep && !finalStep) {
+ claw.stopClaw();
+ drive.setAllMotors(-0.3);
+ arm.armMotor.set(-0.5);
+ finalStep = true;
+ }
+
+ if (Math.abs(drive.getAverageDistance()) >= 4.95 && finalStep) {
+ drive.setAllMotors(0);
+ done = true;
+ }
+
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ /*
+ * drive.leftFront.getEncoder().setPosition(0);
+ * drive.leftRear.getEncoder().setPosition(0);
+ * drive.rightFront.getEncoder().setPosition(0);
+ * drive.rightRear.getEncoder().setPosition(0);
+ */
+ arm.armMotor.getEncoder().setPosition(0);
+
+ // drive.setAllMotors(0);
+ arm.armMotor.set(0); // stop the arm motor
+
+ }
+
+ @Override
+ public boolean isFinished() {
+ /* return Math.abs(drive.getAverageDistance()) >= 4.95; */
+ return false;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/PlaceGamePieceCommand.java b/src/main/java/frc/robot/commands/PlaceGamePieceCommand.java
deleted file mode 100644
index 828c7e9..0000000
--- a/src/main/java/frc/robot/commands/PlaceGamePieceCommand.java
+++ /dev/null
@@ -1,57 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystems.*;
-
-public class PlaceGamePieceCommand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
- private final Tank drive;
-
- private final Claw claw; // NOPMD
- private final Arm arm;
-
- public PlaceGamePieceCommand(Tank drive, Claw claw, Arm arm) {
- this.drive = drive;
- this.claw = claw;
- this.arm = arm;
- addRequirements(drive);
- addRequirements(claw);
- addRequirements(arm);
- }
-
- @Override
- public void initialize() {
- drive.leftFront.getEncoder().setPosition(0);
- drive.leftRear.getEncoder().setPosition(0);
- drive.rightFront.getEncoder().setPosition(0);
- drive.rightRear.getEncoder().setPosition(0);
-
- arm.armMotor.getEncoder().setPosition(0);
- }
-
- @Override
- public void execute() {
- if (arm.allTheWayDownRear
- .get()) { // the '== true' is implied, because the if statement is looking for the
- // expression to be true. If it is false, it will not run the code inside the if
- // statement, so we don't need to write it.
- arm.armMotor.set(0.4); // move the arm up
- } else {
- // claw.openClaw();
- }
- }
-
- @Override
- public void end(boolean interrupted) {
- arm.armMotor.set(0); // stop the arm motor
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-}
diff --git a/src/main/java/frc/robot/commands/SpinIntake.java b/src/main/java/frc/robot/commands/SpinIntake.java
new file mode 100644
index 0000000..c9f87ab
--- /dev/null
+++ b/src/main/java/frc/robot/commands/SpinIntake.java
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Claw;
+
+public class SpinIntake extends CommandBase {
+ private Claw m_claw;
+ /** Creates a new SpinIntake. */
+ public SpinIntake(Claw claw) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ m_claw = claw;
+ addRequirements(m_claw);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {}
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {}
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {}
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/SwitchIdleModeCommmand.java b/src/main/java/frc/robot/commands/SwitchIdleModeCommmand.java
deleted file mode 100644
index e201201..0000000
--- a/src/main/java/frc/robot/commands/SwitchIdleModeCommmand.java
+++ /dev/null
@@ -1,34 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystems.Tank;
-
-public class SwitchIdleModeCommmand extends CommandBase {
- @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
- private final Tank m_subsystem;
-
- public SwitchIdleModeCommmand(Tank subsystem) {
- m_subsystem = subsystem;
- addRequirements(subsystem);
- }
-
- @Override
- public void initialize() {
- m_subsystem.switchIdleMode();
- }
-
- @Override
- public void execute() {}
-
- @Override
- public void end(boolean interrupted) {}
-
- @Override
- public boolean isFinished() {
- return false;
- }
-}
diff --git a/src/main/java/frc/robot/commands/TrackPiece.java b/src/main/java/frc/robot/commands/TrackPiece.java
new file mode 100644
index 0000000..413147f
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TrackPiece.java
@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Claw;
+import frc.robot.subsystems.Tank;
+import frc.robot.subsystems.AutoPiecePickUp;
+
+public class TrackPiece extends CommandBase {
+ /** Creates a new TrackPiece. */
+ private AutoPiecePickUp m_Vision;
+ private Tank m_Tank;
+ private Claw m_Claw;
+ public TrackPiece(AutoPiecePickUp vision, Tank tank, Claw claw) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ m_Vision = vision;
+ m_Tank = tank;
+ m_Claw = claw;
+ addRequirements(m_Vision, m_Tank, m_Claw);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ //m_Tank.arcadeDrive(0.05, m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint()));
+ m_Vision.enable();
+ SmartDashboard.putString("a", "pid on");
+ m_Claw.closeClaw();
+
+
+ // set pipeline to 2
+ NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(2);
+
+ }
+
+ @Override
+ public void execute() {
+ //m_Tank.setAllMotors(-0.2);
+ RobotContainer.rumbleGabeController(1);
+
+ SmartDashboard.putNumber(getName(), m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint()));
+ }
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ RobotContainer.rumbleGabeController(0);
+ m_Vision.disable();
+ m_Tank.arcadeDrive(0,0);
+ SmartDashboard.putString("a", "pid off");
+ m_Claw.stopClaw();
+
+ NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/TrackPole.java b/src/main/java/frc/robot/commands/TrackPole.java
new file mode 100644
index 0000000..c38f662
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TrackPole.java
@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Claw;
+import frc.robot.subsystems.PoleTracking;
+import frc.robot.subsystems.Tank;
+import frc.robot.subsystems.AutoPiecePickUp;
+
+public class TrackPole extends CommandBase {
+ /** Creates a new TrackPiece. */
+ private PoleTracking m_PoleTracking;
+ private Tank m_Tank;
+ public TrackPole(PoleTracking poleTracking, Tank tank) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ m_PoleTracking = poleTracking;
+ m_Tank = tank;
+ addRequirements(m_PoleTracking, m_Tank);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ //m_Tank.arcadeDrive(0.05, m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint()));
+ m_PoleTracking.enable();
+ SmartDashboard.putString("a", "pid on");
+
+ NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(1);
+
+ }
+
+ @Override
+ public void execute() {
+ //m_Tank.setAllMotors(-0.2);
+ NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(1);
+ RobotContainer.rumbleGabeController(1);
+
+ SmartDashboard.putNumber(getName(), m_PoleTracking.getController().calculate(m_PoleTracking.getMeasurement(), m_PoleTracking.getSetpoint()));
+ }
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ RobotContainer.rumbleGabeController(0);
+ m_PoleTracking.disable();
+ m_Tank.arcadeDrive(0,0);
+ SmartDashboard.putString("a", "pid off");
+
+ NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return m_PoleTracking.atSetpoint();
+ }
+}
diff --git a/src/main/java/frc/robot/commands/tests/FullTestCommand.java b/src/main/java/frc/robot/commands/tests/FullTestCommand.java
deleted file mode 100644
index 6b9b2b8..0000000
--- a/src/main/java/frc/robot/commands/tests/FullTestCommand.java
+++ /dev/null
@@ -1,31 +0,0 @@
-package frc.robot.commands.tests;
-
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-
-public class FullTestCommand extends InstantCommand {
- private final TestArmCommand m_arm;
- private final TestClawCommand m_claw;
- private final TestDrivebaseCommand m_drive;
- private final TestTurretCommand m_turret;
-
- public FullTestCommand(
- TestArmCommand arm,
- TestClawCommand claw,
- TestDrivebaseCommand drive,
- TestTurretCommand turret) {
- m_arm = arm;
- m_claw = claw;
- m_drive = drive;
- m_turret = turret;
- }
-
- @Override
- public void execute() {
- m_arm.andThen(m_claw.andThen(m_drive.andThen(m_turret))).schedule();
- }
-
- @Override
- public boolean runsWhenDisabled() {
- return false;
- }
-}
diff --git a/src/main/java/frc/robot/commands/tests/TestArmCommand.java b/src/main/java/frc/robot/commands/tests/TestArmCommand.java
deleted file mode 100644
index 5fdb275..0000000
--- a/src/main/java/frc/robot/commands/tests/TestArmCommand.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package frc.robot.commands.tests;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.subsystems.Arm;
-
-public class TestArmCommand extends InstantCommand {
- private final Arm m_arm;
-
- public TestArmCommand(Arm subsystem) {
- m_arm = subsystem;
- addRequirements(m_arm);
- }
-
- @Override
- public void execute() {
- m_arm.moveArm(1);
- Timer.delay(1);
- m_arm.moveArm(0);
- Timer.delay(1);
- m_arm.moveArm(-1);
- Timer.delay(1);
- m_arm.moveArmToZeroDeg();
- }
-
- @Override
- public boolean runsWhenDisabled() {
- return false;
- }
-}
diff --git a/src/main/java/frc/robot/commands/tests/TestClawCommand.java b/src/main/java/frc/robot/commands/tests/TestClawCommand.java
deleted file mode 100644
index 6339fab..0000000
--- a/src/main/java/frc/robot/commands/tests/TestClawCommand.java
+++ /dev/null
@@ -1,27 +0,0 @@
-package frc.robot.commands.tests;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.subsystems.Claw;
-
-public class TestClawCommand extends InstantCommand {
- private final Claw m_claw;
-
- public TestClawCommand(Claw subsystem) {
- m_claw = subsystem;
- addRequirements(m_claw);
- }
-
- @Override
- public void execute() {
- m_claw.openClaw();
- Timer.delay(0.5);
- m_claw.closeClaw();
- Timer.delay(0.5);
- }
-
- @Override
- public boolean runsWhenDisabled() {
- return false;
- }
-}
diff --git a/src/main/java/frc/robot/commands/tests/TestDrivebaseCommand.java b/src/main/java/frc/robot/commands/tests/TestDrivebaseCommand.java
deleted file mode 100644
index 785746d..0000000
--- a/src/main/java/frc/robot/commands/tests/TestDrivebaseCommand.java
+++ /dev/null
@@ -1,44 +0,0 @@
-package frc.robot.commands.tests;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.subsystems.Tank;
-
-public class TestDrivebaseCommand extends InstantCommand {
- private final Tank m_drivebase;
-
- public TestDrivebaseCommand(Tank subsystem) {
- m_drivebase = subsystem;
- addRequirements(m_drivebase);
- }
-
- @Override
- public void execute() {
- m_drivebase.arcadeDrive(1, 0);
- Timer.delay(1);
- m_drivebase.arcadeDrive(0, 0);
- Timer.delay(1);
- m_drivebase.arcadeDrive(-1, 0);
- Timer.delay(1);
- m_drivebase.arcadeDrive(0, 0);
- Timer.delay(1);
- m_drivebase.arcadeDrive(0, 1);
- Timer.delay(1);
- m_drivebase.arcadeDrive(0, 0);
- Timer.delay(1);
- m_drivebase.arcadeDrive(0, -1);
- Timer.delay(1);
- m_drivebase.arcadeDrive(0, 0);
- Timer.delay(1);
- }
-
- @Override
- public void end(boolean interrupted) {
- m_drivebase.arcadeDrive(0, 0);
- }
-
- @Override
- public boolean runsWhenDisabled() {
- return false;
- }
-}
diff --git a/src/main/java/frc/robot/commands/tests/TestTurretCommand.java b/src/main/java/frc/robot/commands/tests/TestTurretCommand.java
deleted file mode 100644
index a0e2af9..0000000
--- a/src/main/java/frc/robot/commands/tests/TestTurretCommand.java
+++ /dev/null
@@ -1,36 +0,0 @@
-package frc.robot.commands.tests;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.subsystems.Turret;
-
-public class TestTurretCommand extends InstantCommand {
- private final Turret m_turret;
-
- public TestTurretCommand(Turret subsystem) {
- m_turret = subsystem;
- addRequirements(m_turret);
- }
-
- @Override
- public void execute() {
- m_turret.rotateTurret(1);
- Timer.delay(1);
- m_turret.rotateTurret(0);
- Timer.delay(1);
- m_turret.rotateTurret(-1);
- Timer.delay(1);
- m_turret.rotateTurret(0);
- Timer.delay(1);
- }
-
- @Override
- public void end(boolean interrupted) {
- m_turret.rotateTurret(0);
- }
-
- @Override
- public boolean runsWhenDisabled() {
- return false;
- }
-}
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 0456a8e..37992d1 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -4,58 +4,138 @@
package frc.robot.subsystems;
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.REVPhysicsSim;
+import com.revrobotics.SparkMaxAlternateEncoder;
+import com.revrobotics.SparkMaxPIDController;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
+
+import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Interfaces.CANSparkMax;
+import frc.robot.interfaces.CANSparkMax;
public class Arm extends SubsystemBase {
+ public double armToFrontAngle = 3;
+ public double armToBackAngle = -3;
+
public CANSparkMax armMotor;
public DigitalInput allTheWayDownRear = new DigitalInput(1);
- public double desiredArmAngle;
- public double currentArmAngle;
+ //public double desiredArmAngle;
+ //public double currentArmAngle;
public double gearRatio = 87;
+ public GenericEntry encoderWidget;
+
+ public REVPhysicsSim physicsSim;
+
+ public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, kSetpoint;
+
+ //public SparkMaxPIDController armPID;
+
+ public AbsoluteEncoder encoder;
+
+ double lastEncoderValue;
+ int rotations;
+
public Arm() {
+
+ // kP = 0.0001;
+ // kI = 0;
+ // kD = 0;
+ // kIz = 0;
+ // kFF = 0;
+ // kMaxOutput = 1;
+ // kMinOutput = -1;
+
+ // kSetpoint = 0;
+
armMotor = new CANSparkMax(7, MotorType.kBrushless);
armMotor.setIdleMode(IdleMode.kBrake);
- armMotor.setSmartCurrentLimit(20, 30);
+ armMotor.setSmartCurrentLimit(30, 40);
armMotor.burnFlash();
+ // set
+ addChild("Arm Motor", armMotor);
+
+ // armPID = armMotor.getPIDController();
+
+ // armPID.setP(kP);
+ // armPID.setI(kI);
+ // armPID.setD(kD);
+ // armPID.setIZone(kIz);
+ // armPID.setFF(kFF);
+ // armPID.setOutputRange(kMinOutput, kMaxOutput);
+
+ // set the spark max to alternate encoder mode
+
+ // configure the data port on top to be used with the REV Through Bore Encoder
+ // using the absolute encoder adapter
+ encoder = armMotor.getAbsoluteEncoder(Type.kDutyCycle);
+
+ encoderWidget = Shuffleboard.getTab("Motors").add("Arm Encoder", armMotor.getEncoder().getPosition()).getEntry();
Shuffleboard.getTab("Motors").add("Arm", armMotor);
+
+ lastEncoderValue = armMotor.getEncoder().getPosition();
+
+ // configure the PID loop to use the alternate encoder
+ // armPID.setFeedbackDevice(encoder);
+
+ // add simulation data
+ if (RobotBase.isSimulation()) {
+ physicsSim = new REVPhysicsSim();
+ physicsSim.addSparkMax(armMotor, 5000, 20);
+ }
+
}
@Override
public void periodic() {
- SmartDashboard.putBoolean("zeroDeg", allTheWayDownRear.get());
+ // SmartDashboard.putBoolean("zeroDeg", allTheWayDownRear.get());
+ double encoderValue = armMotor.getEncoder().getPosition();
+ double change = encoderValue - lastEncoderValue;
+ if (Math.abs(change) > 0.5) {
+ if (change < 0) {
+ rotations++;
+ } else {
+ rotations--;
+ }
+ }
+ lastEncoderValue = encoderValue;
+
+ encoderWidget.setDouble(encoderValue);
}
- public void moveArm(double leftStickYaxis) {
+ /**
+ * uses input double value to set the motor speed of the arm
+ */
+ public void moveArm(double value) {
double speed = 0.8;
- double motorDrive = leftStickYaxis * speed;
+ double motorDrive = value * speed;
armMotor.set(motorDrive);
}
- public void moveArmToZeroDeg() {
- double speedY = 0.2;
- while (allTheWayDownRear
- .get()) { // the '== true' is implied, because the if statement is looking for the
- // expression to be true. If it is false, it will not run the code inside the if
- // statement, so we don't need to write it.
- armMotor.set(-1 * speedY);
- }
- armMotor.set(0);
- armMotor.getEncoder().setPosition(0);
- currentArmAngle = 0;
- }
-
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
+ public double getAngle(){
+ return armMotor.getEncoder().getPosition() + rotations;
+ }
+
+ // public void setArmAngle(double angle) {
+ // //desiredArmAngle = angle;
+ // //armPID.setReference(angle, CANSparkMax.ControlType.kPosition);
+ // }
+
+ // public void putArmAtFront() {
+ // setArmAngle(armToFrontAngle);
+ // }
}
diff --git a/src/main/java/frc/robot/subsystems/AutoArmPostioning.java b/src/main/java/frc/robot/subsystems/AutoArmPostioning.java
new file mode 100644
index 0000000..0e9aaa0
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/AutoArmPostioning.java
@@ -0,0 +1,73 @@
+package frc.robot.subsystems;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class AutoArmPostioning extends PIDSubsystem {
+ private static double kP = 0.18;
+ private static double kI = 0;
+ private static double kD = 0;
+ public PIDController m_controller;
+ public double target;
+ public Arm m_arm;
+ public boolean going;
+
+ public AutoArmPostioning(Arm arm) {
+ super(
+ // PIDController
+ new PIDController(kP, kI, kD));
+ m_arm = arm;
+ m_controller.setTolerance(0.1);
+ m_controller.setSetpoint(0);
+ going = false;
+ }
+
+ public void setTarget(double target) {
+ this.target = target;
+ m_controller.setSetpoint(target);
+ }
+
+ @Override
+ protected void useOutput(double output, double setpoint) {
+ m_arm.moveArm(output);
+ }
+
+ @Override
+ protected double getMeasurement() {
+ return m_arm.getAngle();
+ }
+
+ public boolean atSetpoint() {
+ return m_controller.atSetpoint();
+ }
+
+ @Override
+ public void setSetpoint(double setpoint) {
+ m_controller.setSetpoint(setpoint);
+ }
+
+ public void setPID(double p, double i, double d) {
+ m_controller.setPID(p, i, d);
+ }
+
+ public void setTolerance(double tolerance) {
+ m_controller.setTolerance(tolerance);
+ }
+
+ @Override
+ public void periodic() {
+ if (going) {
+ super.periodic();
+ SmartDashboard.putNumber("caluclated power to arm form PID",
+ m_controller.calculate(getMeasurement(), m_controller.getSetpoint()));
+ }
+ }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/PID.java b/src/main/java/frc/robot/subsystems/AutoBalence.java
similarity index 78%
rename from src/main/java/frc/robot/subsystems/PID.java
rename to src/main/java/frc/robot/subsystems/AutoBalence.java
index e45ae1c..26df6d5 100644
--- a/src/main/java/frc/robot/subsystems/PID.java
+++ b/src/main/java/frc/robot/subsystems/AutoBalence.java
@@ -4,16 +4,13 @@
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-// TODO@totbas1 - Have PID require either mecanum OR tank -- update in the initializer for each
-// command to the base to correspond to the selected one
-// In the meantime, I'll try not to touch this file :-)
+public class AutoBalence extends SubsystemBase {
-public class PID extends SubsystemBase {
- public AHRS gyro;
private final double kP = 0.18;
private final double kI = 0.025;
private final double kD = 0.3;
@@ -24,17 +21,16 @@ public class PID extends SubsystemBase {
public ShuffleboardTab tab;
public ShuffleboardTab dash;
- private float initPitch;
+ public AHRS gyro = Tank.gyro;
- public PIDController pidController;
+ private float initPitch;
- public PID() {
+ public AutoBalence() {
toleranceDeg = 0.5;
iLimit = 2.0;
- gyro = new AHRS(SPI.Port.kMXP);
initPitch = gyro.getPitch();
- pidController = new PIDController(P, I, D);
+
}
@Override
@@ -69,10 +65,6 @@ public double PIDoutput() {
I = kI * errorSum;
D = kD * errorRate;
- pidController.setP(P);
- pidController.setI(I);
- pidController.setD(D);
-
double outputSpeed = P + I + D;
return outputSpeed;
@@ -87,5 +79,6 @@ private float getPitchOffset() {
}
@Override
- public void simulationPeriodic() {}
+ public void simulationPeriodic() {
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/AutoPiecePickUp.java b/src/main/java/frc/robot/subsystems/AutoPiecePickUp.java
new file mode 100644
index 0000000..48c8c99
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/AutoPiecePickUp.java
@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+import frc.robot.RobotContainer;
+
+public class AutoPiecePickUp extends PIDSubsystem {
+ /** Creates a new Vision. */
+ private static double kP = 0.1;
+ private static double kI = 0;
+ private static double kD = 0;
+
+ public PIDController pidController;
+ public Tank m_tank;
+ public Claw m_claw;
+
+ public AutoPiecePickUp(Tank tank, Claw claw) {
+ super(
+ // The PIDController used by the subsystem
+ new PIDController(kP, kI, kD));
+
+ addChild(getName(), m_controller);
+ m_tank = tank;
+ m_claw = claw;
+ m_controller.setSetpoint(0);
+ m_controller.setTolerance(1);
+ m_controller.setIntegratorRange(-0.43, 0.43);
+
+ Shuffleboard.getTab(getName()).add(m_controller);
+
+
+ }
+
+ @Override
+ public void useOutput(double output, double setpoint) {
+ NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(2);
+
+ double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0.0);
+
+ // Use the output here
+ double forward = 1 - Math.abs(output) / 30;
+ if (forward < .1) {
+ forward = .1;
+ }
+ forward *= 0.6;
+ if (ty < 0) {
+ forward = 0.5;
+ }
+ double stick = RobotContainer.getControllerRightStickY();
+ if (stick > .5) {
+ forward += (stick / 2) - .25;
+ }
+
+ if (0.50 < output) {
+ output = 0.50;
+ } else if (-0.50 > output) {
+ output = -0.50;
+ } else if (0.1 < output && output < 0.40) {
+ output = 0.40;
+ } else if (-0.40 < output && output < -0.1) {
+ output = -0.40;
+ }
+
+ m_tank.arcadeDrive(-forward, -output * 1);
+ SmartDashboard.putNumber("will this work?", output);
+ SmartDashboard.putNumber("setpoint", setpoint);
+
+ }
+
+ @Override
+ public double getMeasurement() {
+ // Return the process variable measurement here
+ return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0.0);
+ }
+
+ public boolean atSetpoint() {
+ return m_controller.atSetpoint();
+ }
+
+ @Override
+ public void setSetpoint(double setpoint) {
+ m_controller.setSetpoint(setpoint);
+ }
+
+ public void setPID(double p, double i, double d) {
+ m_controller.setPID(p, i, d);
+ }
+
+ public void setTolerance(double tolerance) {
+ m_controller.setTolerance(tolerance);
+ }
+
+ @Override
+ public void periodic() {
+ super.periodic();
+ SmartDashboard.putNumber("calculated turning output from vision",
+ m_controller.calculate(getMeasurement(), m_controller.getSetpoint()));
+ }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index e6044b7..23604e6 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -7,55 +7,35 @@
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Interfaces.*;
-import java.util.Map;
+import frc.robot.RobotContainer;
+import frc.robot.interfaces.*;
public class Claw extends SubsystemBase {
- private CANSparkMax clawMotor;
-
- private GenericEntry stallWidget;
- private GenericEntry freeWidget;
- private ShuffleboardLayout layout;
+ public CANSparkMax clawMotor;
// gear ratio is 100:1
public Claw() {
clawMotor = new CANSparkMax(8, MotorType.kBrushless);
clawMotor.setIdleMode(IdleMode.kBrake);
- clawMotor.setSmartCurrentLimit(14, 11);
+ clawMotor.setSmartCurrentLimit(20);
clawMotor.burnFlash();
+ addChild("Claw Motor", clawMotor);
Shuffleboard.getTab("Motors").add("Claw", clawMotor);
- layout = Shuffleboard.getTab("Config").getLayout("Claw", BuiltInLayouts.kList);
-
- stallWidget =
- layout
- .add("Stall Limit", 10)
- .withWidget(BuiltInWidgets.kNumberSlider)
- .withProperties(Map.of("min", 0, "max", 20))
- .getEntry();
- freeWidget =
- layout
- .add("Free Limit", 11)
- .withWidget(BuiltInWidgets.kNumberSlider)
- .withProperties(Map.of("min", 0, "max", 20))
- .getEntry();
}
@Override
public void periodic() {
- double freeLimit;
- double stallLimit;
- stallLimit = stallWidget.getDouble(10);
- freeLimit = freeWidget.getDouble(11);
-
- clawMotor.setSmartCurrentLimit((int) stallLimit, (int) freeLimit);
+ SmartDashboard.putNumber("rightTrigger", RobotContainer.getClawInValue());
}
public void closeClaw() {
@@ -63,7 +43,7 @@ public void closeClaw() {
}
public void openClaw() {
- clawMotor.set(-0.5);
+ clawMotor.set(-0.75);
}
public void stopClaw() {
diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
index caaef81..210f136 100644
--- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
@@ -8,7 +8,8 @@
public class ExampleSubsystem extends SubsystemBase {
/** Creates a new ExampleSubsystem. */
- public ExampleSubsystem() {}
+ public ExampleSubsystem() {
+ }
/**
* Example command factory method.
@@ -17,7 +18,8 @@ public ExampleSubsystem() {}
*/
/**
- * An example method querying a boolean state of the subsystem (for example, a digital sensor).
+ * An example method querying a boolean state of the subsystem (for example, a
+ * digital sensor).
*
* @return value of some boolean subsystem state, such as a digital sensor.
*/
diff --git a/src/main/java/frc/robot/subsystems/FieldConstants.java b/src/main/java/frc/robot/subsystems/FieldConstants.java
new file mode 100644
index 0000000..b78eab5
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/FieldConstants.java
@@ -0,0 +1,373 @@
+// Copyright (c) 2023 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+
+/**
+ * Contains various field dimensions and useful reference points. Dimensions are
+ * in meters, and sets
+ * of corners start in the lower left moving clockwise.
+ *
+ *
+ * All translations and poses are stored with the origin at the rightmost point
+ * on the BLUE
+ * ALLIANCE wall. Use the {@link #allianceFlip(Translation2d)} and
+ * {@link #allianceFlip(Pose2d)}
+ * methods to flip these values based on the current alliance color.
+ */
+public final class FieldConstants {
+ public static final boolean isWPIField = false; // Red alliance
+
+ public static final double fieldLength = Units.inchesToMeters(651.25);
+ public static final double fieldWidth = Units.inchesToMeters(315.5)
+ + (isWPIField ? Units.inchesToMeters(3.0) : 0.0);
+ public static final double tapeWidth = Units.inchesToMeters(2.0);
+
+ // Dimensions for community and charging station, including the tape.
+ public static final class Community {
+ // Region dimensions
+ public static final double innerX = 0.0;
+ public static final double midX = Units.inchesToMeters(132.375); // Tape to the left of charging station
+ public static final double outerX = Units.inchesToMeters(193.25); // Tape to the right of charging
+ // station
+ public static final double leftY = Units.feetToMeters(18.0);
+ public static final double midY = leftY - Units.inchesToMeters(59.39) + tapeWidth;
+ public static final double rightY = 0.0;
+ public static final Translation2d[] regionCorners = new Translation2d[] {
+ new Translation2d(innerX, rightY),
+ new Translation2d(innerX, leftY),
+ new Translation2d(midX, leftY),
+ new Translation2d(midX, midY),
+ new Translation2d(outerX, midY),
+ new Translation2d(outerX, rightY),
+ };
+
+ // Charging station dimensions
+ public static final double chargingStationLength = Units.inchesToMeters(76.125);
+ public static final double chargingStationWidth = Units.inchesToMeters(97.25);
+ public static final double chargingStationOuterX = outerX - tapeWidth;
+ public static final double chargingStationInnerX = chargingStationOuterX - chargingStationLength;
+ public static final double chargingStationLeftY = midY - tapeWidth;
+ public static final double chargingStationRightY = chargingStationLeftY - chargingStationWidth;
+ public static final Translation2d[] chargingStationCorners = new Translation2d[] {
+ new Translation2d(chargingStationInnerX, chargingStationRightY),
+ new Translation2d(chargingStationInnerX, chargingStationLeftY),
+ new Translation2d(chargingStationOuterX, chargingStationRightY),
+ new Translation2d(chargingStationOuterX, chargingStationLeftY)
+ };
+
+ // Cable bump
+ public static final double cableBumpInnerX = innerX + Grids.outerX + Units.inchesToMeters(95.25);
+ public static final double cableBumpOuterX = cableBumpInnerX + Units.inchesToMeters(7);
+ public static final Translation2d[] cableBumpCorners = new Translation2d[] {
+ new Translation2d(cableBumpInnerX, 0.0),
+ new Translation2d(cableBumpInnerX, chargingStationRightY),
+ new Translation2d(cableBumpOuterX, 0.0),
+ new Translation2d(cableBumpOuterX, chargingStationRightY)
+ };
+ }
+
+ // Dimensions for grids and nodes
+ public static final class Grids {
+ // X layout
+ public static final double outerX = Units.inchesToMeters(54.25);
+ public static final double lowX = outerX - (Units.inchesToMeters(14.25) / 2.0); // Centered when under
+ // cube
+ // nodes
+ public static final double midX = outerX - Units.inchesToMeters(22.75);
+ public static final double highX = outerX - Units.inchesToMeters(39.75);
+
+ // Y layout
+ public static final int nodeRowCount = 9;
+ public static final double[] nodeY = isWPIField
+ ? new double[] {
+ Units.inchesToMeters(20.19 + 22.0 * 0),
+ Units.inchesToMeters(20.19 + 22.0 * 1),
+ Units.inchesToMeters(20.19 + 22.0 * 2),
+ Units.inchesToMeters(20.19 + 22.0 * 3),
+ Units.inchesToMeters(20.19 + 22.0 * 4),
+ Units.inchesToMeters(20.19 + 22.0 * 5),
+ Units.inchesToMeters(20.19 + 22.0 * 6),
+ Units.inchesToMeters(20.19 + 22.0 * 7),
+ Units.inchesToMeters(20.19 + 22.0 * 8 + 2.5)
+ }
+ : new double[] {
+ Units.inchesToMeters(20.19 + 22.0 * 0),
+ Units.inchesToMeters(20.19 + 22.0 * 1),
+ Units.inchesToMeters(20.19 + 22.0 * 2),
+ Units.inchesToMeters(20.19 + 22.0 * 3),
+ Units.inchesToMeters(20.19 + 22.0 * 4),
+ Units.inchesToMeters(20.19 + 22.0 * 5),
+ Units.inchesToMeters(20.19 + 22.0 * 6),
+ Units.inchesToMeters(20.19 + 22.0 * 7),
+ Units.inchesToMeters(20.19 + 22.0 * 8)
+ };
+
+ // Z layout
+ public static final double cubeEdgeHigh = Units.inchesToMeters(3.0);
+ public static final double highCubeZ = Units.inchesToMeters(35.5) - cubeEdgeHigh;
+ public static final double midCubeZ = Units.inchesToMeters(23.5) - cubeEdgeHigh;
+ public static final double highConeZ = Units.inchesToMeters(46.0);
+ public static final double midConeZ = Units.inchesToMeters(34.0);
+
+ // Translations (all nodes in the same column/row have the same X/Y coordinate)
+ public static final Translation2d[] lowTranslations = new Translation2d[nodeRowCount];
+ public static final Translation3d[] low3dTranslations = new Translation3d[nodeRowCount];
+ public static final Translation2d[] midTranslations = new Translation2d[nodeRowCount];
+ public static final Translation3d[] mid3dTranslations = new Translation3d[nodeRowCount];
+ public static final Translation2d[] highTranslations = new Translation2d[nodeRowCount];
+ public static final Translation3d[] high3dTranslations = new Translation3d[nodeRowCount];
+
+ static {
+ for (int i = 0; i < nodeRowCount; i++) {
+ boolean isCube = i == 1 || i == 4 || i == 7;
+ lowTranslations[i] = new Translation2d(lowX, nodeY[i]);
+ low3dTranslations[i] = new Translation3d(lowX, nodeY[i], 0.0);
+ midTranslations[i] = new Translation2d(midX, nodeY[i]);
+ mid3dTranslations[i] = new Translation3d(midX, nodeY[i], isCube ? midCubeZ : midConeZ);
+ highTranslations[i] = new Translation2d(highX, nodeY[i]);
+ high3dTranslations[i] = new Translation3d(highX, nodeY[i],
+ isCube ? highCubeZ : highConeZ);
+ }
+ }
+
+ // Complex low layout (shifted to account for cube vs cone rows and wide edge
+ // nodes)
+ public static final double complexLowXCones = outerX - Units.inchesToMeters(16.0) / 2.0; // Centered X
+ // under
+ // cone nodes
+ public static final double complexLowXCubes = lowX; // Centered X under cube nodes
+ public static final double complexLowOuterYOffset = nodeY[0]
+ - (Units.inchesToMeters(3.0) + (Units.inchesToMeters(25.75) / 2.0));
+
+ public static final Translation2d[] complexLowTranslations = new Translation2d[] {
+ new Translation2d(complexLowXCones, nodeY[0] - complexLowOuterYOffset),
+ new Translation2d(complexLowXCubes, nodeY[1]),
+ new Translation2d(complexLowXCones, nodeY[2]),
+ new Translation2d(complexLowXCones, nodeY[3]),
+ new Translation2d(complexLowXCubes, nodeY[4]),
+ new Translation2d(complexLowXCones, nodeY[5]),
+ new Translation2d(complexLowXCones, nodeY[6]),
+ new Translation2d(complexLowXCubes, nodeY[7]),
+ new Translation2d(complexLowXCones, nodeY[8] + complexLowOuterYOffset),
+ };
+
+ public static final Translation3d[] complexLow3dTranslations = new Translation3d[] {
+ new Translation3d(complexLowXCones, nodeY[0] - complexLowOuterYOffset, 0.0),
+ new Translation3d(complexLowXCubes, nodeY[1], 0.0),
+ new Translation3d(complexLowXCones, nodeY[2], 0.0),
+ new Translation3d(complexLowXCones, nodeY[3], 0.0),
+ new Translation3d(complexLowXCubes, nodeY[4], 0.0),
+ new Translation3d(complexLowXCones, nodeY[5], 0.0),
+ new Translation3d(complexLowXCones, nodeY[6], 0.0),
+ new Translation3d(complexLowXCubes, nodeY[7], 0.0),
+ new Translation3d(complexLowXCones, nodeY[8] + complexLowOuterYOffset, 0.0),
+ };
+ }
+
+ // Dimensions for loading zone and substations, including the tape
+ public static final class LoadingZone {
+ // Region dimensions
+ public static final double width = Units.inchesToMeters(99.0);
+ public static final double innerX = FieldConstants.fieldLength;
+ public static final double midX = fieldLength - Units.inchesToMeters(132.25);
+ public static final double outerX = fieldLength - Units.inchesToMeters(264.25);
+ public static final double leftY = FieldConstants.fieldWidth;
+ public static final double midY = leftY - Units.inchesToMeters(50.5);
+ public static final double rightY = leftY - width;
+ public static final Translation2d[] regionCorners = new Translation2d[] {
+ new Translation2d(
+ midX, rightY), // Start at lower left next to border with opponent
+ // community
+ new Translation2d(midX, midY),
+ new Translation2d(outerX, midY),
+ new Translation2d(outerX, leftY),
+ new Translation2d(innerX, leftY),
+ new Translation2d(innerX, rightY),
+ };
+
+ // Double substation dimensions
+ public static final double doubleSubstationLength = Units.inchesToMeters(14.0);
+ public static final double doubleSubstationX = innerX - doubleSubstationLength;
+ public static final double doubleSubstationShelfZ = Units.inchesToMeters(37.375);
+ public static final double doubleSubstationCenterY = fieldWidth - Units.inchesToMeters(49.76);
+
+ // Single substation dimensions
+ public static final double singleSubstationWidth = Units.inchesToMeters(22.75);
+ public static final double singleSubstationLeftX = FieldConstants.fieldLength - doubleSubstationLength
+ - Units.inchesToMeters(88.77);
+ public static final double singleSubstationCenterX = singleSubstationLeftX
+ + (singleSubstationWidth / 2.0);
+ public static final double singleSubstationRightX = singleSubstationLeftX + singleSubstationWidth;
+ public static final Translation2d singleSubstationTranslation = new Translation2d(
+ singleSubstationCenterX,
+ leftY);
+
+ public static final double singleSubstationHeight = Units.inchesToMeters(18.0);
+ public static final double singleSubstationLowZ = Units.inchesToMeters(27.125);
+ public static final double singleSubstationCenterZ = singleSubstationLowZ
+ + (singleSubstationHeight / 2.0);
+ public static final double singleSubstationHighZ = singleSubstationLowZ + singleSubstationHeight;
+ }
+
+ // Locations of staged game pieces
+ public static final class StagingLocations {
+ public static final double centerOffsetX = Units.inchesToMeters(47.36);
+ public static final double positionX = fieldLength / 2.0 - Units.inchesToMeters(47.36);
+ public static final double firstY = Units.inchesToMeters(36.19);
+ public static final double separationY = Units.inchesToMeters(48.0);
+ public static final Translation2d[] translations = new Translation2d[4];
+
+ static {
+ for (int i = 0; i < translations.length; i++) {
+ translations[i] = new Translation2d(positionX, firstY + (i * separationY));
+ }
+ }
+ }
+
+ // AprilTag constants
+ public static final double aprilTagWidth = Units.inchesToMeters(6.0);
+ public static final AprilTagFieldLayout aprilTags = isWPIField
+ ? new AprilTagFieldLayout(
+ List.of(
+ new AprilTag(
+ 1,
+ new Pose3d(
+ Units.inchesToMeters(610.125),
+ Units.inchesToMeters(43.5),
+ Units.inchesToMeters(19.25),
+ new Rotation3d(0.0, 0.0,
+ Math.PI))),
+ new AprilTag(
+ 2,
+ new Pose3d(
+ Units.inchesToMeters(610.375),
+ Units.inchesToMeters(109.5),
+ Units.inchesToMeters(19.25),
+ new Rotation3d(0.0, 0.0,
+ Math.PI))),
+ new AprilTag(
+ 3,
+ new Pose3d(
+ Units.inchesToMeters(610.0),
+ Units.inchesToMeters(176.0),
+ Units.inchesToMeters(19.25),
+ new Rotation3d(0.0, 0.0,
+ Math.PI))),
+ new AprilTag(
+ 4,
+ new Pose3d(
+ Units.inchesToMeters(635.375),
+ Units.inchesToMeters(272.0),
+ Units.inchesToMeters(27.25),
+ new Rotation3d(0.0, 0.0,
+ Math.PI))),
+ new AprilTag(
+ 5,
+ new Pose3d(
+ Units.inchesToMeters(14.25),
+ LoadingZone.doubleSubstationCenterY,
+ Units.inchesToMeters(27.38),
+ new Rotation3d())),
+ new AprilTag(
+ 6,
+ new Pose3d(
+ Units.inchesToMeters(40.45),
+ Grids.nodeY[7],
+ Units.inchesToMeters(18.22),
+ new Rotation3d())),
+ new AprilTag(
+ 7,
+ new Pose3d(
+ Units.inchesToMeters(40.45),
+ Grids.nodeY[4],
+ Units.inchesToMeters(18.22),
+ new Rotation3d())),
+ new AprilTag(
+ 8,
+ new Pose3d(
+ Units.inchesToMeters(40.45),
+ Grids.nodeY[1],
+ Units.inchesToMeters(18.22),
+ new Rotation3d()))),
+ fieldLength,
+ fieldWidth)
+ : new AprilTagFieldLayout(
+ List.of(
+ new AprilTag(
+ 1,
+ new Pose3d(
+ Units.inchesToMeters(610.77),
+ Grids.nodeY[1],
+ Units.inchesToMeters(18.22),
+ new Rotation3d(0.0, 0.0,
+ Math.PI))),
+ new AprilTag(
+ 2,
+ new Pose3d(
+ Units.inchesToMeters(610.77),
+ Grids.nodeY[4],
+ Units.inchesToMeters(18.22),
+ new Rotation3d(0.0, 0.0,
+ Math.PI))),
+ new AprilTag(
+ 3,
+ new Pose3d(
+ Units.inchesToMeters(610.77),
+ Grids.nodeY[7],
+ Units.inchesToMeters(18.22),
+ new Rotation3d(0.0, 0.0,
+ Math.PI))),
+ new AprilTag(
+ 4,
+ new Pose3d(
+ Units.inchesToMeters(636.96),
+ LoadingZone.doubleSubstationCenterY,
+ Units.inchesToMeters(27.38),
+ new Rotation3d(0.0, 0.0,
+ Math.PI))),
+ new AprilTag(
+ 5,
+ new Pose3d(
+ Units.inchesToMeters(14.25),
+ LoadingZone.doubleSubstationCenterY,
+ Units.inchesToMeters(27.38),
+ new Rotation3d())),
+ new AprilTag(
+ 6,
+ new Pose3d(
+ Units.inchesToMeters(40.45),
+ Grids.nodeY[7],
+ Units.inchesToMeters(18.22),
+ new Rotation3d())),
+ new AprilTag(
+ 7,
+ new Pose3d(
+ Units.inchesToMeters(40.45),
+ Grids.nodeY[4],
+ Units.inchesToMeters(18.22),
+ new Rotation3d())),
+ new AprilTag(
+ 8,
+ new Pose3d(
+ Units.inchesToMeters(40.45),
+ Grids.nodeY[1],
+ Units.inchesToMeters(18.22),
+ new Rotation3d()))),
+ fieldLength,
+ fieldWidth);
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/PDP.java b/src/main/java/frc/robot/subsystems/PDP.java
index 8de6204..f356923 100644
--- a/src/main/java/frc/robot/subsystems/PDP.java
+++ b/src/main/java/frc/robot/subsystems/PDP.java
@@ -1,11 +1,14 @@
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.PDPSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -13,6 +16,7 @@ public class PDP extends SubsystemBase {
public PowerDistribution pdp;
ShuffleboardTab tab;
ComplexWidget widget;
+ private PDPSim pdpSim;
public PDP() {
pdp = new PowerDistribution(0, ModuleType.kCTRE);
@@ -21,40 +25,55 @@ public PDP() {
tab = Shuffleboard.getTab("PDP");
addChild("PDP", pdp);
widget = tab.add("PDP", pdp).withWidget(BuiltInWidgets.kPowerDistribution);
+ if (!RobotBase.isReal()) {
+ // this is a simulation, so add a simulated PDP
+ pdpSim = new PDPSim(pdp);
+
+ }
}
@Override
public void periodic() {
// This method will be called once per scheduler run
- SmartDashboard.putNumber("PDP Voltage", pdp.getVoltage());
+
}
+
/**
* @return the voltage of the PDP in Volts
*/
public double getVoltage() {
return pdp.getVoltage();
}
+
/**
* @return the temperature of the PDP in degrees Celsius
*/
public double getTemperature() {
return pdp.getTemperature();
}
+
/**
- * @param param the channel to get the current from, or any string to get the total current
- * @return the current drawn from the specified channel, or all channels, in Amps
+ * @param param the channel to get the current from, or any string to get the
+ * total current
+ * @return the current drawn from the specified channel, or all channels, in
+ * Amps
*/
public double getCurrent(Object param) {
- if (param instanceof Integer) return pdp.getCurrent((int) param);
- else if (param instanceof String) return pdp.getTotalCurrent();
- else return 0;
+ if (param instanceof Integer)
+ return pdp.getCurrent((int) param);
+ else if (param instanceof String)
+ return pdp.getTotalCurrent();
+ else
+ return 0;
}
+
/**
* @return the total current drawn from the PDP in Amps
*/
public double getTotalCurrent() {
return pdp.getTotalCurrent();
}
+
/**
* @return the total power drawn from the PDP in Watts
*/
diff --git a/src/main/java/frc/robot/subsystems/PoleTracking.java b/src/main/java/frc/robot/subsystems/PoleTracking.java
new file mode 100644
index 0000000..847e62d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/PoleTracking.java
@@ -0,0 +1,86 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+import frc.robot.RobotContainer;
+
+public class PoleTracking extends PIDSubsystem {
+ /** Creates a new Vision. */
+ private static double kP = 0.1;
+ private static double kI = 0;
+ private static double kD = 0;
+
+ public PIDController pidController;
+ public Tank m_tank;
+
+ public PoleTracking(Tank tank) {
+ super(
+ // The PIDController used by the subsystem
+ new PIDController(kP, kI, kD));
+
+ addChild(getName(), m_controller);
+ m_tank = tank;
+ m_controller.setSetpoint(0);
+ m_controller.setTolerance(1);
+ m_controller.setIntegratorRange(-0.43, 0.43);
+
+ Shuffleboard.getTab(getName()).add(m_controller);
+
+ }
+
+ @Override
+ public void useOutput(double output, double setpoint) {
+ NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(1);
+
+ if (0.50 < output) {
+ output = 0.50;
+ } else if (-0.50 > output) {
+ output = -0.50;
+ } else if (0.1 < output && output < 0.40) {
+ output = 0.40;
+ } else if (-0.40 < output && output < -0.1) {
+ output = -0.40;
+ }
+
+ m_tank.arcadeDrive(0, -output * 1);
+
+ }
+
+ @Override
+ public double getMeasurement() {
+ // Return the process variable measurement here
+ return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0.0);
+ }
+
+ public boolean atSetpoint() {
+ return m_controller.atSetpoint();
+ }
+
+ @Override
+ public void setSetpoint(double setpoint) {
+ m_controller.setSetpoint(setpoint);
+ }
+
+ public void setPID(double p, double i, double d) {
+ m_controller.setPID(p, i, d);
+ }
+
+ public void setTolerance(double tolerance) {
+ m_controller.setTolerance(tolerance);
+ }
+
+ @Override
+ public void periodic() {
+ super.periodic();
+ SmartDashboard.putNumber("calculated turning output from vision (pole)",
+ m_controller.calculate(getMeasurement(), m_controller.getSetpoint()));
+ }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/Tank.java b/src/main/java/frc/robot/subsystems/Tank.java
index a96d529..d75a92e 100644
--- a/src/main/java/frc/robot/subsystems/Tank.java
+++ b/src/main/java/frc/robot/subsystems/Tank.java
@@ -1,16 +1,25 @@
package frc.robot.subsystems;
+import com.kauailabs.navx.frc.AHRS;
+import com.revrobotics.REVPhysicsSim;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.shuffleboard.*;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
-import frc.robot.Interfaces.*;
import frc.robot.RobotContainer;
+import frc.robot.interfaces.*;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.SPI;
public class Tank extends SubsystemBase {
public CANSparkMax leftFront;
@@ -22,7 +31,7 @@ public class Tank extends SubsystemBase {
public MotorControllerGroup right;
public MotorControllerGroup all;
- public DifferentialDrive drive;
+ public static DifferentialDrive drive;
public boolean brakeMode;
private GenericEntry stallWidget;
@@ -44,26 +53,31 @@ public class Tank extends SubsystemBase {
private ShuffleboardTab motorsTab;
- private GenericEntry leftFrontWidget;
- private GenericEntry leftRearWidget;
- private GenericEntry rightFrontWidget;
- private GenericEntry rightRearWidget;
+ private DifferentialDriveOdometry odometry;
+
+ public static AHRS gyro;
+
+ private Field2d field2d;
+
+ private int initialCurrentLimit = 30; // TODO tune this
- private GenericEntry leftWidget;
- private GenericEntry rightWidget;
- private GenericEntry allWidget;
+ private REVPhysicsSim physicsSim;
- private GenericEntry driveWidget;
/** */
public Tank() {
+
+ gyro = new AHRS(SPI.Port.kMXP);
+ addChild("Gyro", gyro);
+ Shuffleboard.getTab("Driver").add("Gyro", gyro);
+
motorsTab = Shuffleboard.getTab("Motors");
leftFront = new CANSparkMax(Constants.CanConstants.kLeftFrontMotorPort, MotorType.kBrushless);
leftFront.restoreFactoryDefaults();
leftFront.setInverted(true);
leftFront.setIdleMode(IdleMode.kBrake);
- leftFront.setSmartCurrentLimit(40);
- // leftFront.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
+ leftFront.setSmartCurrentLimit(initialCurrentLimit);
+ leftFront.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
leftFront.burnFlash();
motorsTab.add("Left Front", leftFront);
addChild("Left Front", leftFront);
@@ -72,8 +86,8 @@ public Tank() {
leftRear.restoreFactoryDefaults();
leftRear.setInverted(true);
leftRear.setIdleMode(IdleMode.kBrake);
- leftRear.setSmartCurrentLimit(40);
- // leftRear.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
+ leftRear.setSmartCurrentLimit(initialCurrentLimit);
+ leftRear.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
leftRear.burnFlash();
motorsTab.add("Left Rear", leftRear);
addChild("Left Rear", leftRear);
@@ -87,8 +101,8 @@ public Tank() {
rightFront.setInverted(false);
rightFront.setIdleMode(IdleMode.kBrake);
- rightFront.setSmartCurrentLimit(40);
- // rightFront.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
+ rightFront.setSmartCurrentLimit(initialCurrentLimit);
+ rightFront.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
rightFront.burnFlash();
motorsTab.add("Right Front", rightFront);
addChild("Right Front", rightFront);
@@ -97,8 +111,8 @@ public Tank() {
rightRear.restoreFactoryDefaults();
rightRear.setInverted(false);
rightRear.setIdleMode(IdleMode.kBrake);
- rightRear.setSmartCurrentLimit(40);
- // rightRear.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
+ rightRear.setSmartCurrentLimit(initialCurrentLimit);
+ rightRear.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
rightRear.burnFlash();
motorsTab.add("Right Rear", rightRear);
@@ -120,7 +134,7 @@ public Tank() {
drive.setSafetyEnabled(false);
drive.setExpiration(0.1);
- drive.setMaxOutput(1.0);
+ drive.setMaxOutput(0.8);
brakeMode = true;
SmartDashboard.putBoolean("brakeMode", brakeMode);
@@ -129,77 +143,53 @@ public Tank() {
maxSpeed = Constants.CanConstants.maxSpeed;
// set the current limits for all four motors
- leftFront.setSmartCurrentLimit(40, 60);
- leftRear.setSmartCurrentLimit(40, 60);
- rightFront.setSmartCurrentLimit(40, 60);
- rightRear.setSmartCurrentLimit(40, 60);
-
- tankLayout =
- Shuffleboard.getTab("Config")
- .getLayout("Tank", BuiltInLayouts.kList)
- .withSize(1, 4)
- .withPosition(0, 0);
-
- // create a new slider widget for the current limits
- stallWidget =
- tankLayout.add("Stall Limit", 40).withWidget(BuiltInWidgets.kNumberSlider).getEntry();
- freeWidget =
- tankLayout.add("Free Limit", 40).withWidget(BuiltInWidgets.kNumberSlider).getEntry();
-
- // create a new boolean box widget for the idle mode
- idleModeWidget = tankLayout.add("Braking Mode", false).withWidget(BuiltInWidgets.kBooleanBox);
-
- // create a new button widget for the idle mode switch
- idleModeSwitch =
- tankLayout
- .add("Switch Idle Mode", false)
- .withWidget(BuiltInWidgets.kToggleButton)
- .getEntry();
-
- // create a new slider widget for the ramp rate
- rampRateWidget =
- tankLayout
- .add("Ramp Rate", Constants.CanConstants.kRampRate)
- .withWidget(BuiltInWidgets.kNumberSlider)
- .getEntry();
-
- // create a new slider widget for the max speed (don't call getEntry() yet, it can be changed
- // by button inputs)
- maxSpeedWidget = tankLayout.add("Max Speed", maxSpeed).withWidget(BuiltInWidgets.kNumberSlider);
- maxSpeedEntry = maxSpeedWidget.getEntry();
+ leftFront.setSmartCurrentLimit(40, 50);
+ leftRear.setSmartCurrentLimit(40, 50);
+ rightFront.setSmartCurrentLimit(40, 50);
+ rightRear.setSmartCurrentLimit(40, 50);
+
+ // implement odometry
+ odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(gyro.getAngle()),
+ Units.inchesToMeters(
+ (leftFront.getEncoder().getPosition() / 10) * (leftFront.getEncoder().getPosition() / 10)
+ * Math.PI),
+ Units.inchesToMeters(
+ (rightFront.getEncoder().getPosition() / 10) * (rightFront.getEncoder().getPosition() / 10)
+ * Math.PI));
+
+ field2d = new Field2d();
+ SmartDashboard.putData(field2d);
+
+ // create a physics simulation for the robot
+ if (RobotBase.isSimulation()) {
+ physicsSim = new REVPhysicsSim();
+ // add all the spark maxes to the simulation
+ physicsSim.addSparkMax(leftFront, (float) 2.6, 445);
+ physicsSim.addSparkMax(leftRear, (float) 2.6, 445);
+ physicsSim.addSparkMax(rightFront, (float) 2.6, 445);
+ physicsSim.addSparkMax(rightRear, (float) 2.6, 445);
+ // run the simulation
+ physicsSim.run();
+ }
+
}
@Override
public void periodic() {
- SmartDashboard.putNumber("leftStickY", RobotContainer.getDriverControllerLeftStickY());
- SmartDashboard.putNumber("rightStickX", RobotContainer.getDriverControllerRightStickX());
+ SmartDashboard.putNumber("leftStickY", RobotContainer.getTurningStickInput());
+ SmartDashboard.putNumber("rightStickX", RobotContainer.getForwardStickInput());
- // set all four motors to the stall and free limit widget values
- stallLimit = (int) stallWidget.getDouble(10);
- freeLimit = (int) freeWidget.getDouble(40);
-
- leftFront.setSmartCurrentLimit(stallLimit, freeLimit);
- leftRear.setSmartCurrentLimit(stallLimit, freeLimit);
- rightFront.setSmartCurrentLimit(stallLimit, freeLimit);
- rightRear.setSmartCurrentLimit(stallLimit, freeLimit);
-
- // set the ramp rate to the ramp rate widget value
- rampRate = rampRateWidget.getDouble(Constants.CanConstants.kRampRate);
-
- leftFront.setOpenLoopRampRate(rampRate);
- leftRear.setOpenLoopRampRate(rampRate);
- rightFront.setOpenLoopRampRate(rampRate);
- rightRear.setOpenLoopRampRate(rampRate);
+ // i hate my life
- // check if the max speed widget value has changed
- if (maxSpeed != maxSpeedEntry.getDouble(maxSpeed)) {
- // set the max speed to the new value
- maxSpeed = maxSpeedEntry.getDouble(maxSpeed);
- }
- // set the widget value to the current max speed
- maxSpeedEntry.setDouble(maxSpeed);
+ odometry.update(Rotation2d.fromDegrees(gyro.getAngle()),
+ Units.inchesToMeters(
+ (leftFront.getEncoder().getPosition() / 10) * (leftFront.getEncoder().getPosition() / 10)
+ * Math.PI),
+ Units.inchesToMeters(
+ (rightFront.getEncoder().getPosition() / 10) * (rightFront.getEncoder().getPosition() / 10)
+ * Math.PI));
- // i hate my life
+ field2d.setRobotPose(odometry.getPoseMeters());
}
public double getMaxSpeed() {
@@ -209,16 +199,14 @@ public double getMaxSpeed() {
/**
* Drives the robot using arcade drive.
*
- * @param speed The forward/backward speed.
+ * @param speed The forward/backward speed.
* @param rotation The rotation speed.
*/
public void arcadeDrive(double speed, double rotation) {
- try {
- drive.arcadeDrive(
- -speed * Math.pow(Math.abs(speed), 0.5), rotation * Math.pow(Math.abs(rotation), 0.5));
- } catch (Exception e) {
- throw e;
- }
+
+ drive.arcadeDrive(
+ -speed * Math.pow(Math.abs(speed), 0.5), rotation * Math.pow(Math.abs(rotation), 0.5));
+
}
public void increaseMaxSpeed() {
@@ -270,38 +258,31 @@ public void simulationPeriodic() {
/** Change the ramp rate of the motor controllers. */
public void setRampRate(double rampRate) {
- try {
- leftFront.setOpenLoopRampRate(rampRate);
- leftRear.setOpenLoopRampRate(rampRate);
- rightFront.setOpenLoopRampRate(rampRate);
- rightRear.setOpenLoopRampRate(rampRate);
- } catch (Exception e) {
- throw e;
- }
+ leftFront.setOpenLoopRampRate(rampRate);
+ leftRear.setOpenLoopRampRate(rampRate);
+ rightFront.setOpenLoopRampRate(rampRate);
+ rightRear.setOpenLoopRampRate(rampRate);
+
}
// for some reason -speed is forward. dont ask me why
public void setAllMotors(double speed) {
- try {
- leftFront.set(-speed);
- leftRear.set(-speed);
- rightFront.set(-speed);
- rightRear.set(-speed);
- } catch (Exception e) {
- throw e;
- }
+
+ leftFront.set(-speed);
+ leftRear.set(-speed);
+ rightFront.set(-speed);
+ rightRear.set(-speed);
+
}
public double getLeftDistance() {
- double numRotations =
- (leftFront.getEncoder().getPosition() + leftRear.getEncoder().getPosition()) / 2;
+ double numRotations = (leftFront.getEncoder().getPosition() + leftRear.getEncoder().getPosition()) / 2;
return -numRotations
* Constants.AutoConstants.encoderFactor; // This is flipped to make forward positive
}
public double getRightDistance() {
- double numRotations =
- (rightFront.getEncoder().getPosition() + rightRear.getEncoder().getPosition()) / 2;
+ double numRotations = (rightFront.getEncoder().getPosition() + rightRear.getEncoder().getPosition()) / 2;
return -numRotations
* Constants.AutoConstants.encoderFactor; // This is flipped to make forward positive
}
diff --git a/src/main/java/frc/robot/subsystems/Turret.java b/src/main/java/frc/robot/subsystems/Turret.java
index 2c868c4..4d3810c 100644
--- a/src/main/java/frc/robot/subsystems/Turret.java
+++ b/src/main/java/frc/robot/subsystems/Turret.java
@@ -9,7 +9,7 @@
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Interfaces.CANSparkMax;
+import frc.robot.interfaces.CANSparkMax;
public class Turret extends SubsystemBase {
private CANSparkMax xMotor;
@@ -21,6 +21,7 @@ public Turret() {
xMotor.setIdleMode(IdleMode.kBrake);
xMotor.setSmartCurrentLimit(5, 10);
xMotor.burnFlash();
+ addChild("Turret Motor", xMotor);
Shuffleboard.getTab("Motors").add("Turret", xMotor);
}
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
deleted file mode 100644
index aaeb93f..0000000
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ /dev/null
@@ -1,53 +0,0 @@
-package frc.robot.subsystems;
-
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class Vision extends SubsystemBase {
- NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
- // Pipeline mode.
-
- public Vision() {
- // Stuff goes here
- }
-
- // display camera on shuffleboard
-
- @Override
- public void periodic() {
- // This method will be called once per scheduler run
- }
-
- @Override
- public void simulationPeriodic() {
- // This method will be called once per scheduler run during simulation
- }
-
- /**
- * Get the selected property from the NetworkTable.
- *
- * @param property The property to fetch.
- * @return The fetched property.
- */
- public double[]
- getProperties() { // we don't know what type we return until we process it in the if statement
- //
- var tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
- var ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
- var tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
- var ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0);
- var tl = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tl").getDouble(0);
- var cl = NetworkTableInstance.getDefault().getTable("limelight").getEntry("cl").getDouble(0);
- var tshort =
- NetworkTableInstance.getDefault().getTable("limelight").getEntry("tshort").getDouble(0);
- var tlong =
- NetworkTableInstance.getDefault().getTable("limelight").getEntry("tlong").getDouble(0);
- var thor =
- NetworkTableInstance.getDefault().getTable("limelight").getEntry("thor").getDouble(0);
- var tvert =
- NetworkTableInstance.getDefault().getTable("limelight").getEntry("tvert").getDouble(0);
-
- return new double[] {tx, ty, tv, ta, tl, cl, tshort, tlong, thor, tvert};
- }
-}
diff --git a/src/main/java/frc/robot/util/limelight/LEDMode.java b/src/main/java/frc/robot/util/limelight/LEDMode.java
new file mode 100644
index 0000000..4b760f1
--- /dev/null
+++ b/src/main/java/frc/robot/util/limelight/LEDMode.java
@@ -0,0 +1,28 @@
+package frc.robot.util.limelight;
+
+import java.util.Objects;
+
+public enum LEDMode {
+ PIPELINE(0),
+ OFF(1),
+ BLINK(2),
+ ON(3);
+ private int value;
+
+ LEDMode(int value) {
+ this.value = value;
+ }
+
+ public int getValue() {
+ return value;
+ }
+
+ public static LEDMode fromValue(int value) {
+ for (LEDMode ledMode : LEDMode.values()) {
+ if (Objects.equals(ledMode.value, value)) {
+ return ledMode;
+ }
+ }
+ return null;
+ }
+}
diff --git a/src/main/java/frc/robot/util/limelight/Limelight.java b/src/main/java/frc/robot/util/limelight/Limelight.java
new file mode 100644
index 0000000..ce8974c
--- /dev/null
+++ b/src/main/java/frc/robot/util/limelight/Limelight.java
@@ -0,0 +1,95 @@
+package frc.robot.util.limelight;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import java.util.Arrays;
+import java.util.List;
+import java.util.Objects;
+import java.util.Optional;
+
+public class Limelight {
+ private NetworkTable table;
+ private NetworkTableEntry tv;
+ private NetworkTableEntry tx;
+ private NetworkTableEntry ty;
+ private NetworkTableEntry ta;
+ private NetworkTableEntry ts;
+ private NetworkTableEntry tl;
+ private NetworkTableEntry ledMode;
+ private NetworkTableEntry pipeline;
+ private NetworkTableEntry stream;
+ private List