();
10 | private int maxSize;
11 |
12 | public MovingAverage(int maxSize) {
13 | this.maxSize = maxSize;
14 | }
15 |
16 | public void add(double newNumber) {
17 | numbers.add(newNumber);
18 | if (numbers.size() > maxSize) {
19 | numbers.remove(0);
20 | }
21 | }
22 |
23 | public double getAverage() {
24 | double total = 0;
25 |
26 | for (double number : numbers) {
27 | total += number;
28 | }
29 |
30 | return total / numbers.size();
31 | }
32 |
33 | public int getSize() {
34 | return numbers.size();
35 | }
36 |
37 | public boolean isUnderMaxSize() {
38 | return getSize() < maxSize;
39 | }
40 |
41 | public void clear() {
42 | numbers.clear();
43 | }
44 |
45 | }
46 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # FRC 2019 Offseason
2 |
3 | Code for an FRC swerve drive with [Backlash's](https://www.team254.com/first/2019/) superstructure.
4 |
5 | ## Getting Started
6 |
7 | These instructions will get you a copy of the project up and running on your local machine for development and testing purposes. See deployment for notes on how to deploy the project on a live system.
8 |
9 | ### Prerequisites
10 |
11 | - Java 11
12 |
13 | ### Installing
14 |
15 | Clone this repository
16 |
17 | ```
18 | $ git clone https://github.com/Team254/FRC-2019-Offseason-Public.git
19 | ```
20 |
21 | Install all necessary tools: Gradle, Third-Party libraries, etc.
22 |
23 | ```
24 | $ ./gradlew
25 | ```
26 |
27 | If you're using IntelliJ, run the script to generate the IDEA project files, and then open in IntelliJ
28 |
29 | ```
30 | $ ./gradlew idea
31 | ```
32 |
33 |
34 | ## Usage
35 |
36 | To build, run
37 |
38 | ```
39 | $ ./gradlew build
40 | ```
41 |
42 | To deploy to a connected roboRIO, run
43 |
44 | ```
45 | $ ./gradlew deploy
46 | ```
47 |
48 | To see other Gradle tasks, run
49 |
50 | ```
51 | $ ./gradlew tasks
52 | ```
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2019 Team 254
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/auto/actions/Action.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.auto.actions;
2 |
3 | /**
4 | * An interface that describes an iterative action. It is run by an autonomous action, called by the
5 | * method runAction in AutoModeBase (or more commonly in autonomous modes that extend AutoModeBase)
6 | *
7 | * @see com.team254.frc2019.auto.AutoModeBase#runAction
8 | */
9 | public interface Action {
10 | /**
11 | * Returns whether or not the code has finished execution. When implementing this interface, this method is used by
12 | * the runAction method every cycle to know when to stop running the action
13 | *
14 | * @return boolean
15 | */
16 | boolean isFinished();
17 |
18 | /**
19 | * Called by runAction in AutoModeBase iteratively until isFinished returns true. Iterative logic lives in this
20 | * method
21 | */
22 | void update();
23 |
24 | /**
25 | * Run code once when the action finishes, usually for clean up
26 | */
27 | void done();
28 |
29 | /**
30 | * Run code once when the action is started, for set up
31 | */
32 | void start();
33 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/Units.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | public class Units {
4 | public static final double kGravityInPerSecSq = 386.09;
5 |
6 | public static double rpm_to_rads_per_sec(double rpm) {
7 | return rpm * 2.0 * Math.PI / 60.0;
8 | }
9 |
10 | public static double rads_per_sec_to_rpm(double rads_per_sec) {
11 | return rads_per_sec * 60.0 / (2.0 * Math.PI);
12 | }
13 |
14 | public static double inches_to_meters(double inches) {
15 | return inches * 0.0254;
16 | }
17 |
18 | public static double meters_to_inches(double meters) {
19 | return meters / 0.0254;
20 | }
21 |
22 | public static double feet_to_meters(double feet) {
23 | return inches_to_meters(feet * 12.0);
24 | }
25 |
26 | public static double meters_to_feet(double meters) {
27 | return meters_to_inches(meters) / 12.0;
28 | }
29 |
30 | public static double degrees_to_radians(double degrees) {
31 | return Math.toRadians(degrees);
32 | }
33 |
34 | public static double radians_to_degrees(double radians) {
35 | return Math.toDegrees(radians);
36 | }
37 | }
38 |
--------------------------------------------------------------------------------
/src/test/java/com/team254/frc2019/KinematicsTest.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019;
2 |
3 | import com.team254.lib.geometry.Twist2d;
4 | import com.team254.lib.util.Util;
5 | import org.junit.Assert;
6 | import org.junit.Test;
7 | import org.junit.runner.RunWith;
8 | import org.junit.runners.JUnit4;
9 |
10 | /**
11 | * Class that tests the system test
12 | */
13 | @RunWith(JUnit4.class)
14 | public class KinematicsTest {
15 | public final double R = Math.hypot(Constants.kDriveWheelbase, Constants.kDriveTrackwidth);
16 |
17 | public void testKinematics(double forwardInput, double strafeInput, double rotationInput) {
18 | Twist2d result = Kinematics.forwardKinematics(Kinematics.inverseKinematics(forwardInput, strafeInput, rotationInput, false, false));
19 |
20 | Assert.assertEquals(forwardInput, result.dx, Util.kEpsilon);
21 | Assert.assertEquals(strafeInput, result.dy, Util.kEpsilon);
22 | Assert.assertEquals(rotationInput, result.dtheta, Util.kEpsilon);
23 | }
24 |
25 | @Test
26 | public void test() {
27 | testKinematics(1, 0, 0);
28 | testKinematics(0, 1, 0);
29 | testKinematics(0, 0, 1);
30 | testKinematics(0.6, 0.4, 0.5);
31 | }
32 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/MultiTrigger.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | /**
4 | * Class to differentiate between tapping and holding a joystick button/trigger
5 | */
6 | public class MultiTrigger {
7 | private final double mTimeout;
8 | private boolean lastPressed = false;
9 | private final LatchedBoolean wasTapped = new LatchedBoolean();
10 | private final LatchedBoolean wasHeld = new LatchedBoolean();
11 | private boolean lastTapped = false;
12 | private final TimeDelayedBoolean isHeld = new TimeDelayedBoolean();
13 |
14 | public MultiTrigger(double timeout) {
15 | mTimeout = timeout;
16 | }
17 |
18 | public void update(boolean pressed) {
19 | lastPressed = pressed;
20 | lastTapped = wasTapped.update(pressed);
21 | isHeld.update(pressed, mTimeout);
22 | }
23 |
24 | public boolean wasTapped() {
25 | return lastTapped;
26 | }
27 |
28 | public boolean isPressed() {
29 | return lastPressed;
30 | }
31 |
32 | public boolean isHeld() {
33 | return isHeld.update(lastPressed, mTimeout);
34 | }
35 |
36 | public boolean holdStarted() {
37 | return wasHeld.update(isHeld());
38 | }
39 | }
40 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/Main.java:
--------------------------------------------------------------------------------
1 | /*----------------------------------------------------------------------------*/
2 | /* Copyright (c) 2018 FIRST. All Rights Reserved. */
3 | /* Open Source Software - may be modified and shared by FRC teams. The code */
4 | /* must be accompanied by the FIRST BSD license file in the root directory of */
5 | /* the project. */
6 | /*----------------------------------------------------------------------------*/
7 |
8 | package com.team254.frc2019;
9 |
10 | import edu.wpi.first.wpilibj.RobotBase;
11 |
12 | /**
13 | * Do NOT add any static variables to this class, or any initialization at all.
14 | * Unless you know what you are doing, do not modify this file except to
15 | * change the parameter class to the startRobot call.
16 | */
17 | public final class Main {
18 | private Main() {}
19 |
20 | /**
21 | * Main initialization function. Do not perform any initialization here.
22 | *
23 | * If you change your main robot class, change the parameter type.
24 | */
25 | public static void main(String... args) {
26 | RobotBase.startRobot(Robot::new);
27 | }
28 | }
29 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/auto/AutoModeExecutor.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.auto;
2 |
3 | import com.team254.frc2019.auto.modes.AutoModeBase;
4 | import com.team254.lib.util.CrashTrackingRunnable;
5 |
6 | /**
7 | * This class selects, runs, and stops (if necessary) a specified autonomous mode.
8 | */
9 | public class AutoModeExecutor {
10 | private AutoModeBase m_auto_mode;
11 | private Thread m_thread = null;
12 |
13 | public void setAutoMode(AutoModeBase new_auto_mode) {
14 | m_auto_mode = new_auto_mode;
15 | m_thread = new Thread(new CrashTrackingRunnable() {
16 | @Override
17 | public void runCrashTracked() {
18 | if (m_auto_mode != null) {
19 | m_auto_mode.run();
20 | }
21 | }
22 | });
23 | }
24 |
25 | public void start() {
26 | if (m_thread != null) {
27 | m_thread.start();
28 | }
29 | }
30 |
31 | public void stop() {
32 | if (m_auto_mode != null) {
33 | m_auto_mode.stop();
34 | }
35 |
36 | m_thread = null;
37 | }
38 |
39 | public AutoModeBase getAutoMode() {
40 | return m_auto_mode;
41 | }
42 | }
43 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/motion/MotionProfileConstraints.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.motion;
2 |
3 | /**
4 | * Constraints for constructing a MotionProfile.
5 | */
6 | public class MotionProfileConstraints {
7 | protected double max_abs_vel = Double.POSITIVE_INFINITY;
8 | protected double max_abs_acc = Double.POSITIVE_INFINITY;
9 |
10 | public MotionProfileConstraints(double max_vel, double max_acc) {
11 | this.max_abs_vel = Math.abs(max_vel);
12 | this.max_abs_acc = Math.abs(max_acc);
13 | }
14 |
15 | /**
16 | * @return The (positive) maximum allowed velocity.
17 | */
18 | public double max_abs_vel() {
19 | return max_abs_vel;
20 | }
21 |
22 | /**
23 | * @return The (positive) maximum allowed acceleration.
24 | */
25 | public double max_abs_acc() {
26 | return max_abs_acc;
27 | }
28 |
29 | @Override
30 | public boolean equals(Object obj) {
31 | if (!(obj instanceof MotionProfileConstraints)) {
32 | return false;
33 | }
34 | final MotionProfileConstraints other = (MotionProfileConstraints) obj;
35 | return (other.max_abs_acc() == max_abs_acc()) && (other.max_abs_vel() == max_abs_vel());
36 | }
37 | }
38 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/MovingAverageTwist2d.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import com.team254.lib.geometry.Twist2d;
4 |
5 | import java.util.ArrayList;
6 |
7 | /**
8 | * Helper class for storing and calculating a moving average of the Twist2d class
9 | */
10 | public class MovingAverageTwist2d {
11 | ArrayList twists = new ArrayList();
12 | private int maxSize;
13 |
14 | public MovingAverageTwist2d(int maxSize) {
15 | this.maxSize = maxSize;
16 | }
17 |
18 | public synchronized void add(Twist2d twist) {
19 | twists.add(twist);
20 | if (twists.size() > maxSize) {
21 | twists.remove(0);
22 | }
23 | }
24 |
25 | public synchronized Twist2d getAverage() {
26 | double x = 0.0, y = 0.0, t = 0.0;
27 |
28 | for (Twist2d twist : twists) {
29 | x += twist.dx;
30 | y += twist.dy;
31 | t += twist.dtheta;
32 | }
33 |
34 | double size = getSize();
35 | return new Twist2d(x / size, y / size, t / size);
36 | }
37 |
38 | public int getSize() {
39 | return twists.size();
40 | }
41 |
42 | public boolean isUnderMaxSize() {
43 | return getSize() < maxSize;
44 | }
45 |
46 | public void clear() {
47 | twists.clear();
48 | }
49 |
50 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/drivers/LazySparkMax.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.drivers;
2 |
3 | import com.revrobotics.CANError;
4 | import com.revrobotics.CANSparkMax;
5 | import com.revrobotics.ControlType;
6 |
7 | /**
8 | * This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU
9 | * overhead by skipping duplicate set commands.
10 | */
11 | public class LazySparkMax extends CANSparkMax {
12 | protected double mLastSet = Double.NaN;
13 | protected ControlType mLastControlType = null;
14 |
15 | // Set if is a follower
16 | protected CANSparkMax mLeader = null;
17 |
18 | public LazySparkMax(int deviceNumber) {
19 | super(deviceNumber, MotorType.kBrushless);
20 | }
21 |
22 | public CANSparkMax getLeader() {
23 | return mLeader;
24 | }
25 |
26 | @Override
27 | public CANError follow(final CANSparkMax leader) {
28 | mLeader = leader;
29 | return super.follow(leader);
30 | }
31 |
32 | /**
33 | * wrapper method to mimic TalonSRX set method
34 | */
35 | public void set(ControlType type, double setpoint) {
36 | if (setpoint != mLastSet || type != mLastControlType) {
37 | mLastSet = setpoint;
38 | mLastControlType = type;
39 | super.getPIDController().setReference(setpoint, type);
40 | }
41 | }
42 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/states/SuperstructureGoal.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.states;
2 |
3 | /**
4 | * Represents a goal for the superstructure
5 | */
6 | public class SuperstructureGoal {
7 | public final SuperstructureState state;
8 |
9 | public SuperstructureGoal(double elevator, double shoulder, double wrist) {
10 | this(new SuperstructureState(elevator, shoulder, wrist));
11 | }
12 |
13 | public SuperstructureGoal(SuperstructureState state) {
14 | this.state = new SuperstructureState(state);
15 | }
16 |
17 | public boolean equals(SuperstructureGoal other) {
18 | return this.state.elevator == other.state.elevator &&
19 | this.state.shoulder == other.state.shoulder &&
20 | this.state.wrist == other.state.wrist;
21 | }
22 |
23 | public boolean isAtDesiredState(SuperstructureState currentState) {
24 | double[] distances = {
25 | currentState.elevator - state.elevator,
26 | currentState.shoulder - state.shoulder,
27 | currentState.wrist - state.wrist
28 | };
29 |
30 | for (int i = 0; i < distances.length; i++) {
31 | if (Math.abs(distances[i]) > SuperstructureConstants.kPadding[i]) {
32 | return false;
33 | }
34 | }
35 |
36 | return true;
37 | }
38 | }
39 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/states/LEDState.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.states;
2 |
3 | public class LEDState {
4 | public static final LEDState kOff = new LEDState(0.0, 0.0, 0.0);
5 |
6 | public static final LEDState kIntakeIntakingDisc = new LEDState(0.0, 1.0, 0.0);
7 | public static final LEDState kIntakeIntakingCargo = new LEDState(1.0, 1.0, 1.0);
8 |
9 | public static final LEDState kIntakeExhuasting = new LEDState(1.0, 0.0, 0.0);
10 |
11 | public static final LEDState kRobotZeroed = new LEDState(0.0, 1.0, 0.0);
12 | public static final LEDState kFault = new LEDState(0.0, 0.0, 1.0);
13 | public static final LEDState kFaultElevator = new LEDState(1.0, 0.0, 1.0);
14 |
15 | public static final LEDState kHanging = new LEDState(0.0, 0.3, 1.0);
16 | public static final LEDState kMinimalPressure = new LEDState(0.0, 1.0, 1.0);
17 | public static final LEDState kOptimalPressure = new LEDState(1.0, 0.0, 0.0);
18 |
19 | public LEDState() {}
20 |
21 | public LEDState(double b, double g, double r) {
22 | blue = b;
23 | green = g;
24 | red = r;
25 | }
26 |
27 | public void copyFrom(LEDState other) {
28 | this.blue = other.blue;
29 | this.green = other.green;
30 | this.red = other.red;
31 | }
32 |
33 | public double blue;
34 | public double green;
35 | public double red;
36 | }
37 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/DriveSignal.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import com.team254.lib.geometry.Rotation2d;
4 |
5 | import java.text.DecimalFormat;
6 |
7 | /**
8 | * A drivetrain signal containing the speed and azimuth for each wheel
9 | */
10 | public class DriveSignal {
11 | private double[] mWheelSpeeds;
12 | private Rotation2d[] mWheelAzimuths; // Radians!
13 |
14 | public DriveSignal() {
15 | this(new double[]{0, 0, 0, 0}, new Rotation2d[]{Rotation2d.identity(), Rotation2d.identity(), Rotation2d.identity(), Rotation2d.identity()});
16 | }
17 |
18 | public DriveSignal(double[] wheelSpeeds, Rotation2d[] wheelAzimuths) {
19 | mWheelSpeeds = wheelSpeeds;
20 | mWheelAzimuths = wheelAzimuths;
21 | }
22 |
23 | public double[] getWheelSpeeds() {
24 | return mWheelSpeeds;
25 | }
26 |
27 | public Rotation2d[] getWheelAzimuths() {
28 | return mWheelAzimuths;
29 | }
30 |
31 | @Override
32 | public String toString() {
33 | String ret_val = "DriveSignal - \n";
34 | final DecimalFormat fmt = new DecimalFormat("#0.000");
35 | for (int i = 0; i < mWheelSpeeds.length; i++) {
36 | ret_val += "\tWheel " + i + ": Speed - " + mWheelSpeeds[i] + ", Azimuth - " + fmt.format(mWheelAzimuths[i].getDegrees()) + " deg\n";
37 | }
38 |
39 | return ret_val;
40 | }
41 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/CircularBuffer.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import java.util.LinkedList;
4 |
5 | /**
6 | * Implements a simple circular buffer.
7 | */
8 | public class CircularBuffer {
9 | final int mWindowSize;
10 | final LinkedList mSamples;
11 | double mSum;
12 |
13 | public CircularBuffer(int window_size) {
14 | mWindowSize = window_size;
15 | mSamples = new LinkedList<>();
16 | mSum = 0.0;
17 | }
18 |
19 | public void clear() {
20 | mSamples.clear();
21 | mSum = 0.0;
22 | }
23 |
24 | public double getAverage() {
25 | if (mSamples.isEmpty())
26 | return 0.0;
27 | return mSum / mSamples.size();
28 | }
29 |
30 | public void recomputeAverage() {
31 | // Reset any accumulation drift.
32 | mSum = 0.0;
33 | if (mSamples.isEmpty())
34 | return;
35 | for (Double val : mSamples) {
36 | mSum += val;
37 | }
38 | mSum /= mWindowSize;
39 | }
40 |
41 | public void addValue(double val) {
42 | mSamples.addLast(val);
43 | mSum += val;
44 | if (mSamples.size() > mWindowSize) {
45 | mSum -= mSamples.removeFirst();
46 | }
47 | }
48 |
49 | public int getNumValues() {
50 | return mSamples.size();
51 | }
52 |
53 | public boolean isFull() {
54 | return mWindowSize == mSamples.size();
55 | }
56 | }
57 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/Subsystem.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.team254.frc2019.loops.ILooper;
4 |
5 | /**
6 | * The Subsystem abstract class, which serves as a basic framework for all robot subsystems. Each subsystem outputs
7 | * commands to SmartDashboard, has a stop routine (for after each match), and a routine to zero all sensors, which helps
8 | * with calibration.
9 | *
10 | * All Subsystems only have one instance (after all, one robot does not have two drivetrains), and functions get the
11 | * instance of the drivetrain and act accordingly. Subsystems are also a state machine with a desired state and actual
12 | * state; the robot code will try to match the two states with actions. Each Subsystem also is responsible for
13 | * instantializing all member components at the start of the match.
14 | */
15 | public abstract class Subsystem {
16 | public void writeToLog() {}
17 |
18 | // Optional design pattern for caching periodic reads to avoid hammering the HAL/CAN.
19 | public void readPeriodicInputs() {}
20 |
21 | // Optional design pattern for caching periodic writes to avoid hammering the HAL/CAN.
22 | public void writePeriodicOutputs() {}
23 |
24 | public void registerEnabledLoops(ILooper mEnabledLooper) {}
25 |
26 | public void zeroSensors() {}
27 |
28 | public abstract void stop();
29 |
30 | public abstract boolean checkSystem();
31 |
32 | public abstract void outputTelemetry();
33 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/InterpolatingLong.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | /**
4 | * A Long that can be interpolated using the InterpolatingTreeMap.
5 | *
6 | * @see InterpolatingTreeMap
7 | */
8 | public class InterpolatingLong implements Interpolable, InverseInterpolable,
9 | Comparable {
10 | public Long value = 0L;
11 |
12 | public InterpolatingLong(Long val) {
13 | value = val;
14 | }
15 |
16 | @Override
17 | public InterpolatingLong interpolate(InterpolatingLong other, double x) {
18 | Long dydx = other.value - value;
19 | Double searchY = dydx * x + value;
20 | return new InterpolatingLong(searchY.longValue());
21 | }
22 |
23 | @Override
24 | public double inverseInterpolate(InterpolatingLong upper, InterpolatingLong query) {
25 | long upper_to_lower = upper.value - value;
26 | if (upper_to_lower <= 0) {
27 | return 0;
28 | }
29 | long query_to_lower = query.value - value;
30 | if (query_to_lower <= 0) {
31 | return 0;
32 | }
33 | return query_to_lower / (double) upper_to_lower;
34 | }
35 |
36 | @Override
37 | public int compareTo(InterpolatingLong other) {
38 | if (other.value < value) {
39 | return 1;
40 | } else if (other.value > value) {
41 | return -1;
42 | } else {
43 | return 0;
44 | }
45 | }
46 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/InterpolatingDouble.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | /**
4 | * A Double that can be interpolated using the InterpolatingTreeMap.
5 | *
6 | * @see InterpolatingTreeMap
7 | */
8 | public class InterpolatingDouble implements Interpolable, InverseInterpolable,
9 | Comparable {
10 | public Double value = 0.0;
11 |
12 | public InterpolatingDouble(Double val) {
13 | value = val;
14 | }
15 |
16 | @Override
17 | public InterpolatingDouble interpolate(InterpolatingDouble other, double x) {
18 | Double dydx = other.value - value;
19 | Double searchY = dydx * x + value;
20 | return new InterpolatingDouble(searchY);
21 | }
22 |
23 | @Override
24 | public double inverseInterpolate(InterpolatingDouble upper, InterpolatingDouble query) {
25 | double upper_to_lower = upper.value - value;
26 | if (upper_to_lower <= 0) {
27 | return 0;
28 | }
29 | double query_to_lower = query.value - value;
30 | if (query_to_lower <= 0) {
31 | return 0;
32 | }
33 | return query_to_lower / upper_to_lower;
34 | }
35 |
36 | @Override
37 | public int compareTo(InterpolatingDouble other) {
38 | if (other.value < value) {
39 | return 1;
40 | } else if (other.value > value) {
41 | return -1;
42 | } else {
43 | return 0;
44 | }
45 | }
46 |
47 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/geometry/Displacement1d.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.geometry;
2 |
3 | import com.team254.lib.util.Util;
4 |
5 | import java.text.DecimalFormat;
6 |
7 | public class Displacement1d implements State {
8 |
9 | protected final double displacement_;
10 |
11 | public Displacement1d() {
12 | displacement_ = 0.0;
13 | }
14 |
15 | public Displacement1d(double displacement) {
16 | displacement_ = displacement;
17 | }
18 |
19 | public double x() {
20 | return displacement_;
21 | }
22 |
23 | @Override
24 | public Displacement1d interpolate(final Displacement1d other, double x) {
25 | return new Displacement1d(Util.interpolate(displacement_, other.displacement_, x));
26 | }
27 |
28 | @Override
29 | public double distance(final Displacement1d other) {
30 | return Math.abs(x() - other.x());
31 | }
32 |
33 | @Override
34 | public boolean equals(final Object other) {
35 | if (!(other instanceof Displacement1d)) {
36 | return false;
37 | }
38 |
39 | return Util.epsilonEquals(x(), ((Displacement1d) other).x());
40 | }
41 |
42 | @Override
43 | public String toString() {
44 | final DecimalFormat fmt = new DecimalFormat("#0.000");
45 | return fmt.format("(" + x() + ")");
46 | }
47 |
48 | @Override
49 | public String toCSV() {
50 | final DecimalFormat fmt = new DecimalFormat("#0.000");
51 | return fmt.format(x());
52 | }
53 | }
54 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/CircularBufferGeneric.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import java.util.LinkedList;
4 |
5 | /**
6 | * Implements a simple circular buffer.
7 | * Can be used for any class.
8 | */
9 | public class CircularBufferGeneric {
10 | final int mWindowSize;
11 | final LinkedList mSamples;
12 | double mSum;
13 |
14 | public CircularBufferGeneric(int window_size) {
15 | mWindowSize = window_size;
16 | mSamples = new LinkedList<>();
17 | mSum = 0.0;
18 | }
19 |
20 |
21 | public void clear() {
22 | mSamples.clear();
23 | mSum = 0.0;
24 | }
25 |
26 | public void addValue(E val) {
27 | mSamples.addLast(val);
28 | if (mSamples.size() > mWindowSize) {
29 | mSamples.removeFirst();
30 | }
31 | }
32 |
33 | public int getNumValues() {
34 | return mSamples.size();
35 | }
36 |
37 | public boolean isFull() {
38 | return mWindowSize == mSamples.size();
39 | }
40 |
41 | public LinkedList getLinkedList() {
42 | /*
43 | * NOTE: To get an Array of the specific class type which the instance is using,
44 | * you have to use this specific code:
45 | * specificCircularBufferGeneric.getLinkedList().toArray(new ClassThatIWant[specificCircularBufferGeneric
46 | * .getLinkedList().size()]);
47 | * The reason is that for some reason an array of a generic class(i.e. E[]) cannot be created because
48 | * of some archaic data flow ambiguities
49 | */
50 |
51 | return mSamples;
52 | }
53 | }
54 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/geometry/Twist2d.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.geometry;
2 |
3 | import com.team254.lib.util.Util;
4 |
5 | import java.text.DecimalFormat;
6 |
7 | /**
8 | * A movement along an arc at constant curvature and velocity. We can use ideas from "differential calculus" to create
9 | * new RigidTransform2d's from a Twist2d and visa versa.
10 | *
11 | * A Twist can be used to represent a difference between two poses, a velocity, an acceleration, etc.
12 | */
13 | public class Twist2d {
14 | protected static final Twist2d kIdentity = new Twist2d(0.0, 0.0, 0.0);
15 |
16 | public static Twist2d identity() {
17 | return kIdentity;
18 | }
19 |
20 | public final double dx;
21 | public final double dy;
22 | public final double dtheta; // Radians!
23 |
24 | public Twist2d(double dx, double dy, double dtheta) {
25 | this.dx = dx;
26 | this.dy = dy;
27 | this.dtheta = dtheta;
28 | }
29 |
30 | public Twist2d scaled(double scale) {
31 | return new Twist2d(dx * scale, dy * scale, dtheta * scale);
32 | }
33 |
34 | public double norm() {
35 | // Common case of dy == 0
36 | if (dy == 0.0)
37 | return Math.abs(dx);
38 | return Math.hypot(dx, dy);
39 | }
40 |
41 | public double curvature() {
42 | if (Math.abs(dtheta) < Util.kEpsilon && norm() < Util.kEpsilon)
43 | return 0.0;
44 | return dtheta / norm();
45 | }
46 |
47 | @Override
48 | public String toString() {
49 | final DecimalFormat fmt = new DecimalFormat("#0.000");
50 | return "(" + fmt.format(dx) + "," + fmt.format(dy) + "," + fmt.format(Math.toDegrees(dtheta)) + " deg)";
51 | }
52 | }
--------------------------------------------------------------------------------
/vendordeps/REVRobotics.json:
--------------------------------------------------------------------------------
1 | {
2 | "fileName": "REVRobotics.json",
3 | "name": "REVRobotics",
4 | "version": "1.1.9",
5 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
6 | "mavenUrls": [
7 | "http://www.revrobotics.com/content/sw/max/sdk/maven/"
8 | ],
9 | "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json",
10 | "javaDependencies": [
11 | {
12 | "groupId": "com.revrobotics.frc",
13 | "artifactId": "SparkMax-java",
14 | "version": "1.1.9"
15 | }
16 | ],
17 | "jniDependencies": [
18 | {
19 | "groupId": "com.revrobotics.frc",
20 | "artifactId": "SparkMax-driver",
21 | "version": "1.1.9",
22 | "skipInvalidPlatforms": true,
23 | "isJar": false,
24 | "validPlatforms": [
25 | "linuxathena"
26 | ]
27 | }
28 | ],
29 | "cppDependencies": [
30 | {
31 | "groupId": "com.revrobotics.frc",
32 | "artifactId": "SparkMax-cpp",
33 | "version": "1.1.9",
34 | "libName": "libSparkMax",
35 | "headerClassifier": "headers",
36 | "sharedLibrary": false,
37 | "skipInvalidPlatforms": true,
38 | "binaryPlatforms": [
39 | "linuxathena"
40 | ]
41 | },
42 | {
43 | "groupId": "com.revrobotics.frc",
44 | "artifactId": "SparkMax-driver",
45 | "version": "1.1.9",
46 | "libName": "libSparkMaxDriver",
47 | "headerClassifier": "headers",
48 | "sharedLibrary": false,
49 | "skipInvalidPlatforms": true,
50 | "binaryPlatforms": [
51 | "linuxathena"
52 | ]
53 | }
54 | ]
55 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/controlboard/XboxController.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.controlboard;
2 |
3 | import com.team254.frc2019.Constants;
4 | import edu.wpi.first.wpilibj.GenericHID.RumbleType;
5 | import edu.wpi.first.wpilibj.Joystick;
6 |
7 | public class XboxController {
8 | private final Joystick mController;
9 |
10 | public enum Side {
11 | LEFT, RIGHT
12 | }
13 |
14 | public enum Axis {
15 | X, Y
16 | }
17 |
18 | public enum Button {
19 | A(1), B(2), X(3), Y(4), LB(5), RB(6), BACK(7), START(8), L_JOYSTICK(9), R_JOYSTICK(10);
20 |
21 | public final int id;
22 |
23 | Button(int id) {
24 | this.id = id;
25 | }
26 | }
27 |
28 | XboxController(int port) {
29 | mController = new Joystick(port);
30 | }
31 |
32 | double getJoystick(Side side, Axis axis) {
33 | double deadband = Constants.kJoystickThreshold;
34 |
35 | boolean left = side == Side.LEFT;
36 | boolean y = axis == Axis.Y;
37 | // multiplies by -1 if y-axis (inverted normally)
38 | return handleDeadband((y ? -1 : 1) * mController.getRawAxis((left ? 0 : 4) + (y ? 1 : 0)), deadband);
39 | }
40 |
41 | boolean getTrigger(Side side) {
42 | return mController.getRawAxis(side == Side.LEFT ? 2 : 3) > Constants.kJoystickThreshold;
43 | }
44 |
45 | boolean getButton(Button button) {
46 | return mController.getRawButton(button.id);
47 | }
48 |
49 | int getDPad() {
50 | return mController.getPOV();
51 | }
52 |
53 | public void setRumble(boolean on) {
54 | mController.setRumble(RumbleType.kRightRumble, on ? 1 : 0);
55 | }
56 |
57 | private double handleDeadband(double value, double deadband) {
58 | return (Math.abs(value) > Math.abs(deadband)) ? value : 0;
59 | }
60 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/auto/modes/AutoModeBase.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.auto.modes;
2 |
3 | import com.team254.frc2019.auto.AutoModeEndedException;
4 | import com.team254.frc2019.auto.actions.Action;
5 | import edu.wpi.first.wpilibj.DriverStation;
6 |
7 | /**
8 | * An abstract class that is the basis of the robot's autonomous routines. This is implemented in auto modes (which are
9 | * routines that do actions).
10 | */
11 | public abstract class AutoModeBase {
12 | protected double mUpdateRate = 1.0 / 50.0;
13 | protected boolean mActive = false;
14 |
15 | protected abstract void routine() throws AutoModeEndedException;
16 |
17 | public void run() {
18 | mActive = true;
19 |
20 | try {
21 | routine();
22 | } catch (AutoModeEndedException e) {
23 | DriverStation.reportError("AUTO MODE DONE!!!! ENDED EARLY!!!!", false);
24 | return;
25 | }
26 |
27 | done();
28 | }
29 |
30 | public void done() {
31 | System.out.println("Auto mode done");
32 | }
33 |
34 | public void stop() {
35 | mActive = false;
36 | }
37 |
38 | public boolean isActive() {
39 | return mActive;
40 | }
41 |
42 | public boolean isActiveWithThrow() throws AutoModeEndedException {
43 | if (!isActive()) {
44 | throw new AutoModeEndedException();
45 | }
46 |
47 | return isActive();
48 | }
49 |
50 | public void runAction(Action action) throws AutoModeEndedException {
51 | isActiveWithThrow();
52 | action.start();
53 |
54 | while (isActiveWithThrow() && !action.isFinished()) {
55 | action.update();
56 | long waitTime = (long) (mUpdateRate * 1000.0);
57 |
58 | try {
59 | Thread.sleep(waitTime);
60 | } catch (InterruptedException e) {
61 | e.printStackTrace();
62 | }
63 | }
64 |
65 | action.done();
66 | }
67 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/CrashTracker.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import java.io.FileWriter;
4 | import java.io.IOException;
5 | import java.io.PrintWriter;
6 | import java.util.Date;
7 | import java.util.UUID;
8 |
9 | /**
10 | * Tracks start-up and caught crash events, logging them to a file which dosn't roll over
11 | */
12 | public class CrashTracker {
13 |
14 | private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID();
15 |
16 | public static void logRobotConstruction() {
17 | logMarker("robot startup");
18 | }
19 |
20 | public static void logRobotInit() {
21 | logMarker("robot init");
22 | }
23 |
24 | public static void logTeleopInit() {
25 | logMarker("teleop init");
26 | }
27 |
28 | public static void logAutoInit() {
29 | logMarker("auto init");
30 | }
31 |
32 | public static void logDisabledInit() {
33 | logMarker("disabled init");
34 | }
35 |
36 | public static void logTestInit() {
37 | logMarker("test init");
38 | }
39 |
40 | public static void logThrowableCrash(Throwable throwable) {
41 | logMarker("Exception", throwable);
42 | }
43 |
44 | private static void logMarker(String mark) {
45 | logMarker(mark, null);
46 | }
47 |
48 | private static void logMarker(String mark, Throwable nullableException) {
49 |
50 | try (PrintWriter writer = new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true))) {
51 | writer.print(RUN_INSTANCE_UUID.toString());
52 | writer.print(", ");
53 | writer.print(mark);
54 | writer.print(", ");
55 | writer.print(new Date().toString());
56 |
57 | if (nullableException != null) {
58 | writer.print(", ");
59 | nullableException.printStackTrace(writer);
60 | }
61 |
62 | writer.println();
63 | } catch (IOException e) {
64 | e.printStackTrace();
65 | }
66 | }
67 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/vision/AimingParameters.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.vision;
2 |
3 | import com.team254.lib.geometry.Pose2d;
4 | import com.team254.lib.geometry.Rotation2d;
5 |
6 | public class AimingParameters {
7 | private final double range;
8 | private final Pose2d robot_to_goal;
9 | private final Pose2d field_to_goal;
10 | private final Rotation2d robot_to_goal_rotation;
11 | private final double last_seen_timestamp;
12 | private final double stability;
13 | private final Rotation2d field_to_vision_target_normal;
14 | private final int track_id;
15 |
16 | public AimingParameters(Pose2d robot_to_goal,
17 | Pose2d field_to_goal,
18 | Rotation2d field_to_vision_target_normal, double last_seen_timestamp,
19 | double stability, int track_id) {
20 | this.robot_to_goal = robot_to_goal;
21 | this.field_to_vision_target_normal = field_to_vision_target_normal;
22 | this.field_to_goal = field_to_goal;
23 | this.range = robot_to_goal.getTranslation().norm();
24 | this.robot_to_goal_rotation = robot_to_goal.getTranslation().direction();
25 | this.last_seen_timestamp = last_seen_timestamp;
26 | this.stability = stability;
27 | this.track_id = track_id;
28 | }
29 |
30 | public Pose2d getRobotToGoal() {
31 | return robot_to_goal;
32 | }
33 |
34 | public Pose2d getFieldToGoal() {
35 | return field_to_goal;
36 | }
37 |
38 | public double getRange() {
39 | return range;
40 | }
41 |
42 | public Rotation2d getRobotToGoalRotation() {
43 | return robot_to_goal_rotation;
44 | }
45 |
46 | public double getLastSeenTimestamp() {
47 | return last_seen_timestamp;
48 | }
49 |
50 | public double getStability() {
51 | return stability;
52 | }
53 |
54 | public Rotation2d getFieldToVisionTargetNormal() {
55 | return field_to_vision_target_normal;
56 | }
57 |
58 | public int getTrackId() {
59 | return track_id;
60 | }
61 | }
62 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/drivers/SparkMaxChecker.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.drivers;
2 |
3 | import com.revrobotics.CANSparkMax;
4 | import com.revrobotics.ControlType;
5 | import com.team254.frc2019.subsystems.Subsystem;
6 |
7 | import java.util.ArrayList;
8 |
9 | public class SparkMaxChecker extends MotorChecker {
10 | private static class StoredSparkConfiguration {
11 | CANSparkMax leader = null;
12 | }
13 |
14 | protected ArrayList mStoredConfigurations = new ArrayList<>();
15 |
16 | public static boolean checkMotors(Subsystem subsystem,
17 | ArrayList> motorsToCheck,
18 | CheckerConfig checkerConfig) {
19 | SparkMaxChecker checker = new SparkMaxChecker();
20 | return checker.checkMotorsImpl(subsystem, motorsToCheck, checkerConfig);
21 | }
22 |
23 | @Override
24 | protected void storeConfiguration() {
25 | // record previous configuration for all talons
26 | for (MotorConfig config : mMotorsToCheck) {
27 | LazySparkMax spark = (LazySparkMax) config.mMotor;
28 |
29 | StoredSparkConfiguration configuration = new StoredSparkConfiguration();
30 | configuration.leader = spark.getLeader();
31 |
32 | mStoredConfigurations.add(configuration);
33 | spark.restoreFactoryDefaults();
34 | }
35 | }
36 |
37 | @Override
38 | protected void restoreConfiguration() {
39 | for (int i = 0; i < mMotorsToCheck.size(); ++i) {
40 | if (mStoredConfigurations.get(i).leader != null) {
41 | mMotorsToCheck.get(i).mMotor.follow(mStoredConfigurations.get(i).leader);
42 | }
43 | }
44 | }
45 |
46 | @Override
47 | protected void setMotorOutput(CANSparkMax motor, double output) {
48 | motor.getPIDController().setReference(output, ControlType.kDutyCycle);
49 | }
50 |
51 | @Override
52 | protected double getMotorCurrent(CANSparkMax motor) {
53 | return motor.getOutputCurrent();
54 | }
55 | }
56 |
57 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/drivers/TalonSRXChecker.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.drivers;
2 |
3 | import com.ctre.phoenix.motorcontrol.ControlMode;
4 | import com.ctre.phoenix.motorcontrol.can.TalonSRX;
5 | import com.team254.frc2019.subsystems.Subsystem;
6 |
7 | import java.util.ArrayList;
8 |
9 | public class TalonSRXChecker extends MotorChecker {
10 | private static class StoredTalonSRXConfiguration {
11 | public ControlMode mMode;
12 | public double mSetValue;
13 | }
14 |
15 | protected ArrayList mStoredConfigurations = new ArrayList<>();
16 |
17 | public static boolean checkMotors(Subsystem subsystem,
18 | ArrayList> motorsToCheck,
19 | CheckerConfig checkerConfig) {
20 | TalonSRXChecker checker = new TalonSRXChecker();
21 | return checker.checkMotorsImpl(subsystem, motorsToCheck, checkerConfig);
22 | }
23 |
24 | @Override
25 | protected void storeConfiguration() {
26 | // record previous configuration for all talons
27 | for (MotorConfig config : mMotorsToCheck) {
28 | LazyTalonSRX talon = (LazyTalonSRX) config.mMotor;
29 |
30 | StoredTalonSRXConfiguration configuration = new StoredTalonSRXConfiguration();
31 | configuration.mMode = talon.getControlMode();
32 | configuration.mSetValue = talon.getLastSet();
33 |
34 | mStoredConfigurations.add(configuration);
35 | }
36 | }
37 |
38 | @Override
39 | protected void restoreConfiguration() {
40 | for (int i = 0; i < mMotorsToCheck.size(); ++i) {
41 | mMotorsToCheck.get(i).mMotor.set(mStoredConfigurations.get(i).mMode,
42 | mStoredConfigurations.get(i).mSetValue);
43 | }
44 | }
45 |
46 | @Override
47 | protected void setMotorOutput(TalonSRX motor, double output) {
48 | motor.set(ControlMode.PercentOutput, output);
49 | }
50 |
51 | @Override
52 | protected double getMotorCurrent(TalonSRX motor) {
53 | return motor.getOutputCurrent();
54 | }
55 | }
56 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/Wrist.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice;
4 | import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
5 | import com.ctre.phoenix.motorcontrol.can.TalonSRX;
6 | import com.team254.frc2019.Constants;
7 | import com.team254.lib.drivers.MotorChecker;
8 | import com.team254.lib.drivers.TalonSRXChecker;
9 | import com.team254.lib.drivers.TalonSRXUtil;
10 |
11 | import java.util.ArrayList;
12 |
13 | public class Wrist extends ServoMotorSubsystem {
14 | private static Wrist mInstance;
15 |
16 | public synchronized static Wrist getInstance() {
17 | if (mInstance == null) {
18 | mInstance = new Wrist(Constants.kWristConstants);
19 | }
20 |
21 | return mInstance;
22 | }
23 |
24 | private Wrist(final ServoMotorSubsystemConstants constants) {
25 | super(constants);
26 |
27 | TalonSRXUtil.checkError(mMaster.configRemoteFeedbackFilter(Constants.kCanifierWristId, RemoteSensorSource.CANifier_Quadrature,
28 | 0, Constants.kLongCANTimeoutMs),
29 | "Could not set wrist encoder!!!: ");
30 |
31 | TalonSRXUtil.checkError(mMaster.configSelectedFeedbackSensor(
32 | RemoteFeedbackDevice.RemoteSensor0, 0, Constants.kLongCANTimeoutMs),
33 | "Could not detect wrist encoder: ");
34 | }
35 |
36 | // Syntactic sugar.
37 | public synchronized double getAngle() {
38 | return getPosition();
39 | }
40 |
41 | @Override
42 | public boolean checkSystem() {
43 | return TalonSRXChecker.checkMotors(this,
44 | new ArrayList>() {
45 | private static final long serialVersionUID = -716113039054569446L;
46 |
47 | {
48 | add(new MotorChecker.MotorConfig<>("master", mMaster));
49 | }
50 | }, new MotorChecker.CheckerConfig() {
51 | {
52 | mRunOutputPercentage = 0.5;
53 | mRunTimeSec = 1.0;
54 | mCurrentFloor = 0.1;
55 | mRPMFloor = 90;
56 | mCurrentEpsilon = 2.0;
57 | mRPMEpsilon = 200;
58 | mRPMSupplier = () -> mMaster.getSelectedSensorVelocity();
59 | }
60 | });
61 | }
62 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/Infrastructure.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.frc2019.loops.ILooper;
5 | import com.team254.frc2019.loops.Loop;
6 | import edu.wpi.first.wpilibj.Compressor;
7 |
8 | /**
9 | * Subsystem to ensure the compressor never runs while the superstructure moves
10 | */
11 | public class Infrastructure extends Subsystem {
12 | private static Infrastructure mInstance;
13 |
14 | private Superstructure mSuperstructure = Superstructure.getInstance();
15 | private Compressor mCompressor = new Compressor(Constants.kPCMId);
16 |
17 | private boolean mIsManualControl = false;
18 |
19 | private Infrastructure() {}
20 |
21 | public static Infrastructure getInstance() {
22 | if (mInstance == null) {
23 | mInstance = new Infrastructure();
24 | }
25 |
26 | return mInstance;
27 | }
28 |
29 | @Override
30 | public void registerEnabledLoops(ILooper mEnabledLooper) {
31 | mEnabledLooper.register(new Loop() {
32 | @Override
33 | public void onStart(double timestamp) {}
34 |
35 | @Override
36 | public void onLoop(double timestamp) {
37 | synchronized (Infrastructure.this) {
38 | boolean superstructureMoving = !mSuperstructure.isAtDesiredState();
39 |
40 | if (superstructureMoving || !mIsManualControl) {
41 | stopCompressor();
42 | } else {
43 | startCompressor();
44 | }
45 | }
46 | }
47 |
48 | @Override
49 | public void onStop(double timestamp) {}
50 | });
51 | }
52 |
53 | public synchronized void setIsManualControl(boolean isManualControl) {
54 | mIsManualControl = isManualControl;
55 |
56 | if (mIsManualControl) {
57 | startCompressor();
58 | }
59 | }
60 |
61 | public synchronized boolean isManualControl() {
62 | return mIsManualControl;
63 | }
64 |
65 | private void startCompressor() {
66 | mCompressor.start();
67 | }
68 |
69 | private void stopCompressor() {
70 | mCompressor.stop();
71 | }
72 |
73 | @Override
74 | public void stop() {}
75 |
76 | @Override
77 | public boolean checkSystem() {
78 | return false;
79 | }
80 |
81 | @Override
82 | public void outputTelemetry() {}
83 | }
84 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/ReflectingCSVWriter.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import java.io.FileNotFoundException;
4 | import java.io.PrintWriter;
5 | import java.lang.reflect.Field;
6 | import java.util.concurrent.ConcurrentLinkedDeque;
7 |
8 | /**
9 | * Writes data to a CSV file
10 | */
11 | public class ReflectingCSVWriter {
12 | private ConcurrentLinkedDeque mLinesToWrite = new ConcurrentLinkedDeque<>();
13 | private PrintWriter mOutput = null;
14 | private Field[] mFields;
15 |
16 | public ReflectingCSVWriter(String fileName, Class typeClass) {
17 | mFields = typeClass.getFields();
18 | try {
19 | mOutput = new PrintWriter(fileName);
20 | } catch (FileNotFoundException e) {
21 | e.printStackTrace();
22 | }
23 | // Write field names.
24 | StringBuilder line = new StringBuilder();
25 | for (Field field : mFields) {
26 | if (line.length() != 0) {
27 | line.append(", ");
28 | }
29 | line.append(field.getName());
30 | }
31 | writeLine(line.toString());
32 | }
33 |
34 | public void add(T value) {
35 | StringBuilder line = new StringBuilder();
36 | for (Field field : mFields) {
37 | if (line.length() != 0) {
38 | line.append(", ");
39 | }
40 | try {
41 | if (CSVWritable.class.isAssignableFrom(field.getType())) {
42 | line.append(((CSVWritable) field.get(value)).toCSV());
43 | } else {
44 | line.append(field.get(value).toString());
45 | }
46 | } catch (IllegalArgumentException | IllegalAccessException e) {
47 | e.printStackTrace();
48 | }
49 | }
50 | mLinesToWrite.add(line.toString());
51 | }
52 |
53 | protected synchronized void writeLine(String line) {
54 | if (mOutput != null) {
55 | mOutput.println(line);
56 | }
57 | }
58 |
59 | // Call this periodically from any thread to write to disk.
60 | public void write() {
61 | while (true) {
62 | String val = mLinesToWrite.pollFirst();
63 | if (val == null) {
64 | break;
65 | }
66 | writeLine(val);
67 | }
68 | }
69 |
70 | public synchronized void flush() {
71 | if (mOutput != null) {
72 | write();
73 | mOutput.flush();
74 | }
75 | }
76 | }
77 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/controlboard/GamepadButtonControlBoard.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.controlboard;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.lib.util.Deadband;
5 |
6 | public class GamepadButtonControlBoard implements IButtonControlBoard {
7 | private final double kDeadband = 0.15;
8 | private static GamepadButtonControlBoard mInstance = null;
9 |
10 | public static GamepadButtonControlBoard getInstance() {
11 | if (mInstance == null) {
12 | mInstance = new GamepadButtonControlBoard();
13 | }
14 |
15 | return mInstance;
16 | }
17 |
18 | private final XboxController mController;
19 |
20 | private GamepadButtonControlBoard() {
21 | mController = new XboxController(Constants.kButtonGamepadPort);
22 | }
23 |
24 | @Override
25 | public boolean getScorePresetLow() {
26 | return mController.getButton(XboxController.Button.A);
27 | }
28 |
29 | @Override
30 | public boolean getScorePresetMiddle() {
31 | return mController.getButton(XboxController.Button.B);
32 | }
33 |
34 | @Override
35 | public boolean getScorePresetHigh() {
36 | return mController.getButton(XboxController.Button.Y);
37 | }
38 |
39 | @Override
40 | public void setRumble(boolean on) {
41 | mController.setRumble(on);
42 | }
43 |
44 | @Override
45 | public boolean getScorePresetCargo() {
46 | return mController.getButton(XboxController.Button.X);
47 | }
48 |
49 | @Override
50 | public boolean getTuck() {
51 | return mController.getButton(XboxController.Button.LB);
52 | }
53 |
54 | @Override
55 | public boolean getPickupDiskWall() {
56 | return mController.getTrigger(XboxController.Side.RIGHT);
57 | }
58 |
59 | @Override
60 | public boolean getPickupBallGround() {
61 | return mController.getButton(XboxController.Button.RB);
62 | }
63 |
64 | @Override
65 | public boolean getToggleHangMode() {
66 | return mController.getButton(XboxController.Button.START);
67 | }
68 |
69 | @Override
70 | public boolean getToggleHangModeLow() {
71 | return mController.getButton(XboxController.Button.BACK);
72 | }
73 |
74 | @Override
75 | public double getJoggingZ() {
76 | double jog = mController.getJoystick(XboxController.Side.RIGHT, XboxController.Axis.Y);
77 | if (Deadband.inDeadband(jog, kDeadband)) {
78 | return 0.0;
79 | }
80 | return (jog - kDeadband * Math.signum(jog));
81 | }
82 | }
--------------------------------------------------------------------------------
/gradlew.bat:
--------------------------------------------------------------------------------
1 | @if "%DEBUG%" == "" @echo off
2 | @rem ##########################################################################
3 | @rem
4 | @rem Gradle startup script for Windows
5 | @rem
6 | @rem ##########################################################################
7 |
8 | @rem Set local scope for the variables with windows NT shell
9 | if "%OS%"=="Windows_NT" setlocal
10 |
11 | set DIRNAME=%~dp0
12 | if "%DIRNAME%" == "" set DIRNAME=.
13 | set APP_BASE_NAME=%~n0
14 | set APP_HOME=%DIRNAME%
15 |
16 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
17 | set DEFAULT_JVM_OPTS="-Xmx64m"
18 |
19 | @rem Find java.exe
20 | if defined JAVA_HOME goto findJavaFromJavaHome
21 |
22 | set JAVA_EXE=java.exe
23 | %JAVA_EXE% -version >NUL 2>&1
24 | if "%ERRORLEVEL%" == "0" goto init
25 |
26 | echo.
27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
28 | echo.
29 | echo Please set the JAVA_HOME variable in your environment to match the
30 | echo location of your Java installation.
31 |
32 | goto fail
33 |
34 | :findJavaFromJavaHome
35 | set JAVA_HOME=%JAVA_HOME:"=%
36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe
37 |
38 | if exist "%JAVA_EXE%" goto init
39 |
40 | echo.
41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
42 | echo.
43 | echo Please set the JAVA_HOME variable in your environment to match the
44 | echo location of your Java installation.
45 |
46 | goto fail
47 |
48 | :init
49 | @rem Get command-line arguments, handling Windows variants
50 |
51 | if not "%OS%" == "Windows_NT" goto win9xME_args
52 |
53 | :win9xME_args
54 | @rem Slurp the command line arguments.
55 | set CMD_LINE_ARGS=
56 | set _SKIP=2
57 |
58 | :win9xME_args_slurp
59 | if "x%~1" == "x" goto execute
60 |
61 | set CMD_LINE_ARGS=%*
62 |
63 | :execute
64 | @rem Setup the command line
65 |
66 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
67 |
68 | @rem Execute Gradle
69 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
70 |
71 | :end
72 | @rem End local scope for the variables with windows NT shell
73 | if "%ERRORLEVEL%"=="0" goto mainEnd
74 |
75 | :fail
76 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
77 | rem the _cmd.exe /c_ return code!
78 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
79 | exit /b 1
80 |
81 | :mainEnd
82 | if "%OS%"=="Windows_NT" endlocal
83 |
84 | :omega
85 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/StatFinder.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import java.util.ArrayList;
4 | import java.util.DoubleSummaryStatistics;
5 | import java.util.List;
6 |
7 | /**
8 | * Finds the stats (mean, standard deviation, etc.) of a list
9 | *
10 | * Example use case: finding out how long a planner takes from the average of 100 tries
11 | */
12 | public class StatFinder {
13 | private final List numbers = new ArrayList<>();
14 | private final int numberToIgnore;
15 | private int numberIgnored = 0;
16 |
17 | private boolean stopped = false;
18 |
19 | /**
20 | * Creates a new StatFinder
21 | *
22 | * @param numberToIgnore the number of entries to ignore before logging (like burning the top card of a deck before dealing)
23 | */
24 | public StatFinder(int numberToIgnore) {
25 | this.numberToIgnore = numberToIgnore;
26 | }
27 |
28 | public boolean add(double number) {
29 | if (stopped) {
30 | return false;
31 | }
32 |
33 | if (numberIgnored < numberToIgnore) {
34 | numberIgnored++;
35 | return false;
36 | }
37 |
38 | numbers.add(number);
39 | return true;
40 | }
41 |
42 | public boolean addAndPrint(double number) {
43 | boolean success = add(number);
44 |
45 | if (success) {
46 | System.out.println("added: " + number);
47 | }
48 |
49 | return success;
50 | }
51 |
52 | public DoubleSummaryStatistics getStats() {
53 | return numbers.stream().mapToDouble(x -> x).summaryStatistics();
54 | }
55 |
56 | public double getMean() {
57 | return getStats().getAverage();
58 | }
59 |
60 | public double getStandardDeviation() {
61 | double mean = getMean();
62 | return Math.sqrt(numbers.stream().mapToDouble(x -> Math.pow(x - mean, 2)).sum() / (getSize() - 1));
63 | }
64 |
65 | public double getSize() {
66 | return numbers.size();
67 | }
68 |
69 | public void printStats() {
70 | System.out.println("mean: " + getMean());
71 | System.out.println("standard deviation: " + getStandardDeviation());
72 | System.out.println("min: " + getStats().getMin());
73 | System.out.println("max: " + getStats().getMax());
74 | System.out.println("size: " + getSize());
75 | }
76 |
77 | public void stop() {
78 | stopped = true;
79 | }
80 |
81 | public void stopAndPrint() {
82 | if (stopped) {
83 | return;
84 | }
85 |
86 | stop();
87 | printStats();
88 | }
89 |
90 | }
91 |
--------------------------------------------------------------------------------
/src/test/java/com/team254/frc2019/subsystems/LimelightTest.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.team254.lib.util.Util;
4 | import org.junit.Assert;
5 | import org.junit.Test;
6 | import org.junit.runner.RunWith;
7 | import org.junit.runners.JUnit4;
8 |
9 | import java.util.List;
10 |
11 | /**
12 | * Class that tests the system test
13 | */
14 | @RunWith(JUnit4.class)
15 | public class LimelightTest {
16 |
17 | @Test
18 | public void testExtractTopCornersFromBoundingBoxes() {
19 | List topCorners1 = Limelight.extractTopCornersFromBoundingBoxes(
20 | new double[]{94.0, 91.0, 98.0, 101.0, 136.0, 140.0, 147.0, 142.0},
21 | new double[]{65.0, 85.0, 87.0, 66.0, 66.0, 86.0, 85.0, 65.0});
22 | Assert.assertArrayEquals(new double[]{94.0, 65.0}, topCorners1.get(0), Util.kEpsilon);
23 | Assert.assertArrayEquals(new double[]{142.0, 65.0}, topCorners1.get(1), Util.kEpsilon);
24 |
25 | List topCorners2 = Limelight.extractTopCornersFromBoundingBoxes(
26 | new double[]{118.0, 108.0, 127.0, 135.0, 208.0, 215.0, 229.0, 223.0},
27 | new double[]{151.0, 193.0, 191.0, 149.0, 140.0, 179.0, 174.0, 136.0});
28 | Assert.assertArrayEquals(new double[]{118.0, 151.0}, topCorners2.get(0), Util.kEpsilon);
29 | Assert.assertArrayEquals(new double[]{223.0, 136.0}, topCorners2.get(1), Util.kEpsilon);
30 |
31 | List topCorners3 = Limelight.extractTopCornersFromBoundingBoxes(
32 | new double[]{118.0, 108.0, 127.0, 135.0, 208.0, 215.0, 229.0, 223.0},
33 | new double[]{151.0, 193.0, 191.0, 149.0, 140.0, 179.0, 174.0, 136.0});
34 | Assert.assertArrayEquals(new double[]{118.0, 151.0}, topCorners3.get(0), Util.kEpsilon);
35 | Assert.assertArrayEquals(new double[]{223.0, 136.0}, topCorners3.get(1), Util.kEpsilon);
36 |
37 | List topCorners4 = Limelight.extractTopCornersFromBoundingBoxes(
38 | new double[]{114.0, 102.0, 108.0, 121.0, 186.0, 200.0, 209.0, 194.0},
39 | new double[]{161.0, 156.0, 123.0, 126.0, 135.0, 134.0, 175.0, 175.0});
40 | Assert.assertArrayEquals(new double[]{108.0, 123.0}, topCorners4.get(0), Util.kEpsilon);
41 | Assert.assertArrayEquals(new double[]{200.0, 134.0}, topCorners4.get(1), Util.kEpsilon);
42 |
43 | List topCorners5 = Limelight.extractTopCornersFromBoundingBoxes(
44 | new double[]{88, 86, 91, 94, 123, 128, 134, 129},
45 | new double[]{42, 61, 62, 44, 46, 67, 67, 46});
46 | Assert.assertArrayEquals(new double[]{88, 42.0}, topCorners5.get(0), Util.kEpsilon);
47 | Assert.assertArrayEquals(new double[]{129, 46.0}, topCorners5.get(1), Util.kEpsilon);
48 | }
49 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/Util.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import java.util.List;
4 |
5 | /**
6 | * Contains basic functions that are used often.
7 | */
8 | public class Util {
9 | public static final double kEpsilon = 1e-12;
10 |
11 | /**
12 | * Prevent this class from being instantiated.
13 | */
14 | private Util() {}
15 |
16 | /**
17 | * Limits the given input to the given magnitude.
18 | */
19 | public static double limit(double v, double maxMagnitude) {
20 | return limit(v, -maxMagnitude, maxMagnitude);
21 | }
22 |
23 | public static double limit(double v, double min, double max) {
24 | return Math.min(max, Math.max(min, v));
25 | }
26 |
27 | public static boolean inRange(double v, double maxMagnitude) {
28 | return inRange(v, -maxMagnitude, maxMagnitude);
29 | }
30 |
31 | /**
32 | * Checks if the given input is within the range (min, max), both exclusive.
33 | */
34 | public static boolean inRange(double v, double min, double max) {
35 | return v > min && v < max;
36 | }
37 |
38 | public static double interpolate(double a, double b, double x) {
39 | x = limit(x, 0.0, 1.0);
40 | return a + (b - a) * x;
41 | }
42 |
43 | public static String joinStrings(final String delim, final List strings) {
44 | StringBuilder sb = new StringBuilder();
45 | for (int i = 0; i < strings.size(); ++i) {
46 | sb.append(strings.get(i).toString());
47 | if (i < strings.size() - 1) {
48 | sb.append(delim);
49 | }
50 | }
51 | return sb.toString();
52 | }
53 |
54 | public static boolean epsilonEquals(double a, double b, double epsilon) {
55 | return (a - epsilon <= b) && (a + epsilon >= b);
56 | }
57 |
58 | public static boolean epsilonEquals(double a, double b) {
59 | return epsilonEquals(a, b, kEpsilon);
60 | }
61 |
62 | public static boolean epsilonEquals(int a, int b, int epsilon) {
63 | return (a - epsilon <= b) && (a + epsilon >= b);
64 | }
65 |
66 | public static boolean allCloseTo(final List list, double value, double epsilon) {
67 | boolean result = true;
68 | for (Double value_in : list) {
69 | result &= epsilonEquals(value_in, value, epsilon);
70 | }
71 | return result;
72 | }
73 |
74 | public static double bound0To2PIRadians(double radians) {
75 | return Math.toRadians(bound0To360Degrees(Math.toDegrees(radians)));
76 | }
77 |
78 | public static double bound0To360Degrees(double degrees) {
79 | degrees %= 360;
80 | if (degrees < 0) {
81 | degrees += 360;
82 | }
83 |
84 | return degrees;
85 | }
86 | }
87 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/InterpolatingTreeMap.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import java.util.Map;
4 | import java.util.TreeMap;
5 |
6 | /**
7 | * Interpolating Tree Maps are used to get values at points that are not defined by making a guess from points that are
8 | * defined. This uses linear interpolation.
9 | *
10 | * @param The type of the key (must implement InverseInterpolable)
11 | * @param The type of the value (must implement Interpolable)
12 | */
13 | public class InterpolatingTreeMap & Comparable, V extends Interpolable>
14 | extends TreeMap {
15 | private static final long serialVersionUID = 8347275262778054124L;
16 |
17 | final int max_;
18 |
19 | public InterpolatingTreeMap(int maximumSize) {
20 | max_ = maximumSize;
21 | }
22 |
23 | public InterpolatingTreeMap() {
24 | this(0);
25 | }
26 |
27 | /**
28 | * Inserts a key value pair, and trims the tree if a max size is specified
29 | *
30 | * @param key Key for inserted data
31 | * @param value Value for inserted data
32 | * @return the value
33 | */
34 | @Override
35 | public V put(K key, V value) {
36 | if (max_ > 0 && max_ <= size()) {
37 | // "Prune" the tree if it is oversize
38 | K first = firstKey();
39 | remove(first);
40 | }
41 |
42 | super.put(key, value);
43 |
44 | return value;
45 | }
46 |
47 | @Override
48 | public void putAll(Map map) {
49 | System.out.println("Unimplemented Method");
50 | }
51 |
52 | /**
53 | * @param key Lookup for a value (does not have to exist)
54 | * @return V or null; V if it is Interpolable or exists, null if it is at a bound and cannot average
55 | */
56 | public V getInterpolated(K key) {
57 | V gotval = get(key);
58 | if (gotval == null) {
59 | // get surrounding keys for interpolation
60 | K topBound = ceilingKey(key);
61 | K bottomBound = floorKey(key);
62 |
63 | // if attempting interpolation at ends of tree, return the nearest data point
64 | if (topBound == null && bottomBound == null) {
65 | return null;
66 | } else if (topBound == null) {
67 | return get(bottomBound);
68 | } else if (bottomBound == null) {
69 | return get(topBound);
70 | }
71 |
72 | // get surrounding values for interpolation
73 | V topElem = get(topBound);
74 | V bottomElem = get(bottomBound);
75 | return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key));
76 | } else {
77 | return gotval;
78 | }
79 | }
80 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/states/SuperstructureState.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.states;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.lib.geometry.Translation2d;
5 |
6 | public class SuperstructureState {
7 | public double elevator; // inches
8 | public double shoulder; // degrees
9 | public double wrist; // degrees
10 |
11 | public SuperstructureState(double elevator, double shoulder, double wrist) {
12 | this.elevator = elevator;
13 | this.shoulder = shoulder;
14 | this.wrist = wrist;
15 | }
16 |
17 | public SuperstructureState(SuperstructureState other) {
18 | this.elevator = other.elevator;
19 | this.shoulder = other.shoulder;
20 | this.wrist = other.wrist;
21 | }
22 |
23 | // default robot position
24 | public SuperstructureState() {
25 | this(0, 0, 0);
26 | }
27 |
28 | public void setFrom(SuperstructureState source) {
29 | elevator = source.elevator;
30 | shoulder = source.shoulder;
31 | wrist = source.wrist;
32 | }
33 |
34 | /**
35 | * @return height of the bottom roller of the end effector; only applicable for SFR and SVR end effectors
36 | */
37 | public double getBottomEndEffectorHeight() {
38 | double z = Constants.kElevatorHeightToFloor; // z will represent the height of the bottom of the end effector to the ground in inches
39 | z += this.elevator;
40 | z += Constants.kArmLength * Math.sin(Math.toRadians(this.shoulder));
41 | z += Constants.kWristToBottomEndEffectorLength * Math.sin(Math.toRadians(this.wrist + Constants.kEndEffectorBottomAngle));
42 | return z;
43 | }
44 |
45 | /**
46 | * @return Translation2d where x maps to x position and y maps to z position
47 | */
48 | public Translation2d getPlanarWristJointLocation() {
49 | double z = this.elevator;
50 | z += Constants.kArmLength * Math.sin(Math.toRadians(this.shoulder));
51 |
52 | double x = Constants.kArmLength * Math.cos(Math.toRadians(this.shoulder));
53 |
54 | return new Translation2d(x, z);
55 | }
56 |
57 | /**
58 | * @return is bottom roller of the end effector above the bumper; only applicable for SFR and SVR end effectors
59 | */
60 | public boolean isOverBumper() {
61 | return getBottomEndEffectorHeight() > Constants.kBumperHeight + SuperstructureConstants.kElevatorPaddingInches;
62 | }
63 |
64 | @Override
65 | public String toString() {
66 | return "SuperstructureState{" +
67 | "elevator=" + elevator +
68 | ", shoulder=" + shoulder +
69 | ", wrist=" + wrist +
70 | '}';
71 | }
72 |
73 | public Double[] asVector() {
74 | return new Double[]{elevator, shoulder, wrist};
75 | }
76 | }
77 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/Kickstand.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.team254.frc2019.Constants;
4 | import edu.wpi.first.wpilibj.DoubleSolenoid;
5 |
6 | /**
7 | * Deploys and retracts the climbing mechanism, through the kickstand solenoid,
8 | * and the rachet system, which makes sure the robot does not fall down while
9 | * climbing
10 | */
11 | public class Kickstand extends Subsystem {
12 | private final DoubleSolenoid.Value kSolenoidForKickstandEngaged = DoubleSolenoid.Value.kForward;
13 | private final DoubleSolenoid.Value kSolenoidForKickstandDisengaged = DoubleSolenoid.Value.kReverse;
14 |
15 | private static Kickstand mInstance;
16 | private DoubleSolenoid mKickstandSolenoid;
17 | private DoubleSolenoid mRachetSolenoid;
18 | private boolean mEngaged;
19 | private boolean mRachetEngaged;
20 |
21 | public synchronized static Kickstand getInstance() {
22 | if (mInstance == null) {
23 | mInstance = new Kickstand();
24 | }
25 |
26 | return mInstance;
27 | }
28 |
29 | private Kickstand() {
30 | mKickstandSolenoid = new DoubleSolenoid(Constants.kPCMId, Constants.kKickstandForwardId,
31 | Constants.kKickstandReverseId);
32 | mRachetSolenoid = new DoubleSolenoid(Constants.kPCMId, Constants.kRatchetForwardId,
33 | Constants.kRatchetReverseId);
34 | mEngaged = true;
35 | mRachetEngaged = true;
36 | setDisengaged();
37 | setRachetDisengaged();
38 | }
39 |
40 | public synchronized void setRachetEngaged() {
41 | if (!mRachetEngaged) {
42 | mRachetEngaged = true;
43 | mRachetSolenoid.set(kSolenoidForKickstandEngaged);
44 | }
45 | }
46 |
47 | public synchronized void setRachetDisengaged() {
48 | if (mRachetEngaged) {
49 | mRachetEngaged = false;
50 | mRachetSolenoid.set(kSolenoidForKickstandDisengaged);
51 | }
52 | }
53 |
54 | /**
55 | * Deploys the climbing mechanism
56 | */
57 | public synchronized void setEngaged() {
58 | if (!mEngaged) {
59 | mEngaged = true;
60 | mKickstandSolenoid.set(kSolenoidForKickstandEngaged);
61 | }
62 | }
63 |
64 | /**
65 | * Retracts the climbing mechanism
66 | */
67 | public synchronized void setDisengaged() {
68 | if (mEngaged) {
69 | mEngaged = false;
70 | mKickstandSolenoid.set(kSolenoidForKickstandDisengaged);
71 | }
72 | }
73 |
74 | public synchronized boolean isEngaged() {
75 | return mEngaged;
76 | }
77 |
78 | @Override
79 | public void stop() {}
80 |
81 | @Override
82 | public boolean checkSystem() {
83 | return false;
84 | }
85 |
86 | @Override
87 | public void outputTelemetry() {}
88 | }
89 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/control/SwerveHeadingController.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.control;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.frc2019.subsystems.Drive;
5 | import com.team254.lib.util.SynchronousPIDF;
6 |
7 | /**
8 | * Controls overall swerve heading of the robot through motion profile.
9 | *
10 | * All units are in degrees (for this class only) for easy integration with DPad
11 | */
12 | public class SwerveHeadingController {
13 | private static SwerveHeadingController mInstance;
14 |
15 | public static SwerveHeadingController getInstance() {
16 | if (mInstance == null) {
17 | mInstance = new SwerveHeadingController();
18 | }
19 |
20 | return mInstance;
21 | }
22 |
23 | public enum HeadingControllerState {
24 | OFF, SNAP, // for dpad snapping to cardinals
25 | MAINTAIN, // maintaining current heading while driving
26 | }
27 |
28 | private final SynchronousPIDF mPIDFController;
29 | private double mSetpoint = 0.0;
30 |
31 | private HeadingControllerState mHeadingControllerState = HeadingControllerState.OFF;
32 |
33 | private SwerveHeadingController() {
34 | mPIDFController = new SynchronousPIDF();
35 | }
36 |
37 | public HeadingControllerState getHeadingControllerState() {
38 | return mHeadingControllerState;
39 | }
40 |
41 | public void setHeadingControllerState(HeadingControllerState state) {
42 | mHeadingControllerState = state;
43 | }
44 |
45 | /**
46 | * @param goal pos in degrees
47 | */
48 | public void setGoal(double goal_pos) {
49 | mSetpoint = goal_pos;
50 | }
51 |
52 | public boolean isAtGoal() {
53 | return mPIDFController.onTarget(Constants.kSwerveHeadingControllerErrorTolerance);
54 | }
55 |
56 | /**
57 | * Should be called from a looper at a constant dt
58 | */
59 | public double update() {
60 | mPIDFController.setSetpoint(mSetpoint);
61 | double current_angle = Drive.getInstance().getHeading().getDegrees();
62 | double current_error = mSetpoint - current_angle;
63 |
64 | if (current_error > 180) {
65 | current_angle += 360;
66 | } else if (current_error < -180) {
67 | current_angle -= 360;
68 | }
69 |
70 | switch (mHeadingControllerState) {
71 | case OFF:
72 | return 0.0;
73 | case SNAP:
74 | mPIDFController.setPID(Constants.kSnapSwerveHeadingKp, Constants.kSnapSwerveHeadingKi, Constants.kSnapSwerveHeadingKd);
75 | break;
76 | case MAINTAIN:
77 | mPIDFController.setPID(Constants.kMaintainSwerveHeadingKp, Constants.kMaintainSwerveHeadingKi, Constants.kMaintainSwerveHeadingKd);
78 | break;
79 | }
80 |
81 | return mPIDFController.calculate(current_angle);
82 | }
83 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/states/TimedLEDState.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.states;
2 |
3 | public interface TimedLEDState {
4 | void getCurrentLEDState(LEDState desiredState, double timestamp);
5 |
6 | class BlinkingLEDState implements TimedLEDState {
7 | public static BlinkingLEDState kZeroingFault = new BlinkingLEDState(
8 | LEDState.kOff, LEDState.kFault, 1.0);
9 | public static BlinkingLEDState kJustZeroed = new BlinkingLEDState(
10 | LEDState.kOff, LEDState.kRobotZeroed, 0.250);
11 | public static BlinkingLEDState kBlinkingIntakeCargo = new BlinkingLEDState(
12 | LEDState.kOff, LEDState.kIntakeIntakingCargo, 0.1);
13 | public static BlinkingLEDState kBlinkingIntakingDisc = new BlinkingLEDState(
14 | LEDState.kOff, LEDState.kIntakeIntakingDisc, 0.1);
15 |
16 | public static BlinkingLEDState kHangNoPressure = new BlinkingLEDState(
17 | LEDState.kOff, LEDState.kHanging, 0.5);
18 |
19 | LEDState mStateOne = new LEDState(0.0, 0.0, 0.0);
20 | LEDState mStateTwo = new LEDState(0.0, 0.0, 0.0);
21 | double mDuration;
22 |
23 | public BlinkingLEDState(LEDState stateOne, LEDState stateTwo, double duration) {
24 | mStateOne.copyFrom(stateOne);
25 | mStateTwo.copyFrom(stateTwo);
26 | mDuration = duration;
27 | }
28 |
29 | @Override
30 | public void getCurrentLEDState(LEDState desiredState, double timestamp) {
31 | if ((int) (timestamp / mDuration) % 2 == 0) {
32 | desiredState.copyFrom(mStateOne);
33 | } else {
34 | desiredState.copyFrom(mStateTwo);
35 | }
36 | }
37 | }
38 |
39 | class StaticLEDState implements TimedLEDState {
40 | public static StaticLEDState kStaticOff = new StaticLEDState(LEDState.kOff);
41 | public static StaticLEDState kIntakingDisc = new StaticLEDState(LEDState.kIntakeIntakingDisc);
42 | public static StaticLEDState kIntakingCargo = new StaticLEDState(LEDState.kIntakeIntakingCargo);
43 | public static StaticLEDState kExhausting = new StaticLEDState(LEDState.kIntakeExhuasting);
44 |
45 | public static StaticLEDState kRobotZeroed = new StaticLEDState(LEDState.kRobotZeroed);
46 |
47 | public static StaticLEDState kHangMinimalPressure = new StaticLEDState(LEDState.kMinimalPressure);
48 | public static StaticLEDState kHangOptimalPressure = new StaticLEDState(LEDState.kOptimalPressure);
49 |
50 | LEDState mStaticState = new LEDState(0.0, 0.0, 0.0);
51 |
52 | public StaticLEDState(LEDState staticState) {
53 | mStaticState.copyFrom(staticState);
54 | }
55 |
56 | @Override
57 | public void getCurrentLEDState(LEDState desiredState, double timestamp) {
58 | desiredState.copyFrom(mStaticState);
59 | }
60 | }
61 | }
62 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/motion/MotionSegment.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.motion;
2 |
3 | import static com.team254.lib.motion.MotionUtil.kEpsilon;
4 | import static com.team254.lib.util.Util.epsilonEquals;
5 |
6 | /**
7 | * A MotionSegment is a movement from a start MotionState to an end MotionState with a constant acceleration.
8 | */
9 | public class MotionSegment {
10 | protected MotionState mStart;
11 | protected MotionState mEnd;
12 |
13 | public MotionSegment(MotionState start, MotionState end) {
14 | mStart = start;
15 | mEnd = end;
16 | }
17 |
18 | /**
19 | * Verifies that:
20 | *
21 | * 1. All segments have a constant acceleration.
22 | *
23 | * 2. All segments have monotonic position (sign of velocity doesn't change).
24 | *
25 | * 3. The time, position, velocity, and acceleration of the profile are consistent.
26 | */
27 | public boolean isValid() {
28 | if (!epsilonEquals(start().acc(), end().acc(), kEpsilon)) {
29 | // Acceleration is not constant within the segment.
30 | System.err.println(
31 | "Segment acceleration not constant! Start acc: " + start().acc() + ", End acc: " + end().acc());
32 | return false;
33 | }
34 | if (Math.signum(start().vel()) * Math.signum(end().vel()) < 0.0 && !epsilonEquals(start().vel(), 0.0, kEpsilon)
35 | && !epsilonEquals(end().vel(), 0.0, kEpsilon)) {
36 | // Velocity direction reverses within the segment.
37 | System.err.println("Segment velocity reverses! Start vel: " + start().vel() + ", End vel: " + end().vel());
38 | return false;
39 | }
40 | if (!start().extrapolate(end().t()).equals(end())) {
41 | // A single segment is not consistent.
42 | if (start().t() == end().t() && Double.isInfinite(start().acc())) {
43 | // One allowed exception: If acc is infinite and dt is zero.
44 | return true;
45 | }
46 | System.err.println("Segment not consistent! Start: " + start() + ", End: " + end());
47 | return false;
48 | }
49 | return true;
50 | }
51 |
52 | public boolean containsTime(double t) {
53 | return t >= start().t() && t <= end().t();
54 | }
55 |
56 | public boolean containsPos(double pos) {
57 | return pos >= start().pos() && pos <= end().pos() || pos <= start().pos() && pos >= end().pos();
58 | }
59 |
60 | public MotionState start() {
61 | return mStart;
62 | }
63 |
64 | public void setStart(MotionState start) {
65 | mStart = start;
66 | }
67 |
68 | public MotionState end() {
69 | return mEnd;
70 | }
71 |
72 | public void setEnd(MotionState end) {
73 | mEnd = end;
74 | }
75 |
76 | @Override
77 | public String toString() {
78 | return "Start: " + start() + ", End: " + end();
79 | }
80 | }
81 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/loops/Looper.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.loops;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.lib.util.CrashTrackingRunnable;
5 | import edu.wpi.first.wpilibj.Notifier;
6 | import edu.wpi.first.wpilibj.Timer;
7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
8 |
9 | import java.util.ArrayList;
10 | import java.util.List;
11 |
12 | /**
13 | * This code runs all of the robot's loops. Loop objects are stored in a List object. They are started when the robot
14 | * powers up and stopped after the match.
15 | */
16 | public class Looper implements ILooper {
17 | public final double kPeriod = Constants.kLooperDt;
18 |
19 | private boolean mRunning;
20 |
21 | private final Notifier mNotifier;
22 | private final List mLoops;
23 | private final Object mTaskRunningLock = new Object();
24 | private double mTimestamp = 0;
25 | private double mDT = 0;
26 |
27 | private final CrashTrackingRunnable runnable_ = new CrashTrackingRunnable() {
28 | @Override
29 | public void runCrashTracked() {
30 | synchronized (mTaskRunningLock) {
31 | if (mRunning) {
32 | double now = Timer.getFPGATimestamp();
33 |
34 | for (Loop loop : mLoops) {
35 | loop.onLoop(now);
36 | }
37 |
38 | mDT = now - mTimestamp;
39 | mTimestamp = now;
40 | }
41 | }
42 | }
43 | };
44 |
45 | public Looper() {
46 | mNotifier = new Notifier(runnable_);
47 | mRunning = false;
48 | mLoops = new ArrayList<>();
49 | }
50 |
51 | @Override
52 | public synchronized void register(Loop loop) {
53 | synchronized (mTaskRunningLock) {
54 | mLoops.add(loop);
55 | }
56 | }
57 |
58 | public synchronized void start() {
59 | if (!mRunning) {
60 | System.out.println("Starting loops");
61 |
62 | synchronized (mTaskRunningLock) {
63 | mTimestamp = Timer.getFPGATimestamp();
64 | for (Loop loop : mLoops) {
65 | loop.onStart(mTimestamp);
66 | }
67 | mRunning = true;
68 | }
69 |
70 | mNotifier.startPeriodic(kPeriod);
71 | }
72 | }
73 |
74 | public synchronized void stop() {
75 | if (mRunning) {
76 | System.out.println("Stopping loops");
77 | mNotifier.stop();
78 |
79 | synchronized (mTaskRunningLock) {
80 | mRunning = false;
81 | mTimestamp = Timer.getFPGATimestamp();
82 | for (Loop loop : mLoops) {
83 | System.out.println("Stopping " + loop);
84 | loop.onStop(mTimestamp);
85 | }
86 | }
87 | }
88 | }
89 |
90 | public void outputToSmartDashboard() {
91 | SmartDashboard.putNumber("looper_dt", mDT);
92 | }
93 | }
94 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/Arm.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice;
4 | import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
5 | import com.ctre.phoenix.motorcontrol.can.TalonSRX;
6 | import com.team254.frc2019.Constants;
7 | import com.team254.lib.drivers.MotorChecker;
8 | import com.team254.lib.drivers.TalonSRXChecker;
9 | import com.team254.lib.drivers.TalonSRXUtil;
10 |
11 | import java.util.ArrayList;
12 |
13 | public class Arm extends ServoMotorSubsystem {
14 | private static Arm mInstance;
15 |
16 | public synchronized static Arm getInstance() {
17 | if (mInstance == null) {
18 | mInstance = new Arm(Constants.kArmConstants);
19 | }
20 |
21 | return mInstance;
22 | }
23 |
24 | private Arm(final ServoMotorSubsystemConstants constants) {
25 | super(constants);
26 |
27 | TalonSRXUtil.checkError(mMaster.configRemoteFeedbackFilter(Constants.kCanifierArmId,
28 | RemoteSensorSource.CANifier_Quadrature,
29 | 0, Constants.kLongCANTimeoutMs),
30 | "Could not set arm encoder!!!: ");
31 |
32 | TalonSRXUtil.checkError(mMaster.configSelectedFeedbackSensor(
33 | RemoteFeedbackDevice.RemoteSensor0, 0, Constants.kLongCANTimeoutMs),
34 | "Could not detect arm encoder: ");
35 | }
36 |
37 | // Syntactic sugar.
38 | public synchronized double getAngle() {
39 | return getPosition();
40 | }
41 |
42 | public synchronized void setSetpointThrust(double units) {
43 | mMaster.configMotionCruiseVelocity(Constants.kArmCruiseVelocityForThrust);
44 | super.setSetpointMotionMagic(units);
45 | }
46 |
47 | @Override
48 | public synchronized void setSetpointMotionMagic(double units) {
49 | mMaster.configMotionCruiseVelocity(Constants.kArmConstants.kCruiseVelocity);
50 | super.setSetpointMotionMagic(units);
51 | }
52 |
53 | @Override
54 | public synchronized void setSetpointMotionMagic(double units, double feedforward_v) {
55 | mMaster.configMotionCruiseVelocity(Constants.kArmConstants.kCruiseVelocity);
56 | super.setSetpointMotionMagic(units, feedforward_v);
57 | }
58 |
59 | @Override
60 | public boolean checkSystem() {
61 | return TalonSRXChecker.checkMotors(this,
62 | new ArrayList>() {
63 | private static final long serialVersionUID = 3069865439600365807L;
64 |
65 | {
66 | add(new MotorChecker.MotorConfig<>("master", mMaster));
67 | }
68 | }, new MotorChecker.CheckerConfig() {
69 | {
70 | mRunOutputPercentage = 0.5;
71 | mRunTimeSec = 0.5;
72 | mCurrentFloor = 0.1;
73 | mRPMFloor = 90;
74 | mCurrentEpsilon = 2.0;
75 | mRPMEpsilon = 200;
76 | mRPMSupplier = () -> mMaster.getSelectedSensorVelocity();
77 | }
78 | });
79 | }
80 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/RobotStateEstimator.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.team254.frc2019.Kinematics;
4 | import com.team254.frc2019.RobotState;
5 | import com.team254.frc2019.loops.ILooper;
6 | import com.team254.frc2019.loops.Loop;
7 | import com.team254.lib.geometry.Pose2d;
8 | import com.team254.lib.geometry.Rotation2d;
9 | import com.team254.lib.geometry.Twist2d;
10 |
11 | public class RobotStateEstimator extends Subsystem {
12 | private static RobotStateEstimator mInstance = null;
13 | private RobotState mRobotState = RobotState.getInstance();
14 | private Drive mDrive = Drive.getInstance();
15 | private double prev_timestamp_ = -1.0;
16 | private Rotation2d prev_heading_ = null;
17 |
18 | public static RobotStateEstimator getInstance() {
19 | if (mInstance == null) {
20 | mInstance = new RobotStateEstimator();
21 | }
22 |
23 | return mInstance;
24 | }
25 |
26 | private RobotStateEstimator() {}
27 |
28 | @Override
29 | public void registerEnabledLoops(ILooper looper) {
30 | looper.register(new EnabledLoop());
31 | }
32 |
33 | private class EnabledLoop implements Loop {
34 | @Override
35 | public synchronized void onStart(double timestamp) {
36 | prev_timestamp_ = timestamp;
37 | }
38 |
39 | @Override
40 | public synchronized void onLoop(double timestamp) {
41 | if (prev_heading_ == null) {
42 | prev_heading_ = mRobotState.getLatestFieldToVehicle().getValue().getRotation();
43 | }
44 | final double dt = timestamp - prev_timestamp_;
45 | final double[] wheel_speeds = mDrive.getModuleVelocities();
46 | final Rotation2d[] wheel_azimuths = mDrive.getModuleAzimuths();
47 | final Rotation2d gyro_angle = mDrive.getHeading();
48 | Twist2d odometry_twist;
49 | synchronized (mRobotState) {
50 | final Pose2d last_measurement = mRobotState.getLatestFieldToVehicle().getValue();
51 |
52 | // this should be used for debugging forward kinematics without gyro (shouldn't be used in actual code)
53 | // odometry_twist = Kinematics.forwardKinematics(wheel_speeds, wheel_azimuths).scaled(dt);
54 |
55 | // this should be used for more accurate measurements for actual code
56 | odometry_twist = Kinematics.forwardKinematics(wheel_speeds,
57 | wheel_azimuths, last_measurement.getRotation(), gyro_angle, dt).scaled(dt);
58 | }
59 | final Twist2d measured_velocity = Kinematics.forwardKinematics(
60 | wheel_speeds, wheel_azimuths, prev_heading_, gyro_angle, dt);
61 | mRobotState.addObservations(timestamp, odometry_twist, measured_velocity);
62 |
63 | prev_heading_ = gyro_angle;
64 | prev_timestamp_ = timestamp;
65 | }
66 |
67 | @Override
68 | public void onStop(double timestamp) {}
69 | }
70 |
71 | @Override
72 | public void stop() {}
73 |
74 | @Override
75 | public boolean checkSystem() {
76 | return true;
77 | }
78 |
79 | @Override
80 | public void outputTelemetry() {
81 | mRobotState.outputToSmartDashboard();
82 | }
83 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/controlboard/ControlBoard.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.controlboard;
2 |
3 | public class ControlBoard implements IControlBoard {
4 | private static ControlBoard mInstance = null;
5 |
6 | public static ControlBoard getInstance() {
7 | if (mInstance == null) {
8 | mInstance = new ControlBoard();
9 | }
10 |
11 | return mInstance;
12 | }
13 |
14 | private final IDriveControlBoard mDriveControlBoard;
15 | private final IButtonControlBoard mButtonControlBoard;
16 |
17 | private ControlBoard() {
18 | mDriveControlBoard = GamepadDriveControlBoard.getInstance();
19 | mButtonControlBoard = GamepadButtonControlBoard.getInstance();
20 | }
21 |
22 | @Override
23 | public double getThrottle() {
24 | return mDriveControlBoard.getThrottle();
25 | }
26 |
27 | @Override
28 | public double getStrafe() {
29 | return mDriveControlBoard.getStrafe();
30 | }
31 |
32 | @Override
33 | public double getRotation() {
34 | return mDriveControlBoard.getRotation();
35 | }
36 |
37 | @Override
38 | public boolean getDriveLowPower() {
39 | return mDriveControlBoard.getDriveLowPower();
40 | }
41 |
42 | @Override
43 | public boolean getFieldRelative() {
44 | return mDriveControlBoard.getFieldRelative();
45 | }
46 |
47 | @Override
48 | public double getDPad() {
49 | return mDriveControlBoard.getDPad();
50 | }
51 |
52 | @Override
53 | public boolean getShoot() {
54 | return mDriveControlBoard.getShoot();
55 | }
56 |
57 | @Override
58 | public boolean getThrust() {
59 | return mDriveControlBoard.getThrust();
60 | }
61 |
62 | @Override
63 | public boolean getScorePresetLow() {
64 | return mButtonControlBoard.getScorePresetLow();
65 | }
66 |
67 | @Override
68 | public boolean getScorePresetMiddle() {
69 | return mButtonControlBoard.getScorePresetMiddle();
70 | }
71 |
72 | @Override
73 | public boolean getScorePresetHigh() {
74 | return mButtonControlBoard.getScorePresetHigh();
75 | }
76 |
77 | @Override
78 | public boolean getScorePresetCargo() {
79 | return mButtonControlBoard.getScorePresetCargo();
80 | }
81 |
82 | @Override
83 | public boolean getTuck() {
84 | return mButtonControlBoard.getTuck();
85 | }
86 |
87 | @Override
88 | public boolean getPickupDiskWall() {
89 | return mButtonControlBoard.getPickupDiskWall();
90 | }
91 |
92 | @Override
93 | public boolean getPickupBallGround() {
94 | return mButtonControlBoard.getPickupBallGround();
95 | }
96 |
97 | @Override
98 | public void setRumble(boolean on) {
99 | mButtonControlBoard.setRumble(on);
100 | }
101 |
102 | @Override
103 | public boolean getToggleHangMode() {
104 | return mButtonControlBoard.getToggleHangMode();
105 | }
106 |
107 | @Override
108 | public boolean getToggleHangModeLow() {
109 | return mButtonControlBoard.getToggleHangModeLow();
110 | }
111 |
112 | @Override
113 | public double getJoggingZ() {
114 | return mButtonControlBoard.getJoggingZ();
115 | }
116 |
117 | @Override
118 | public boolean getHorizontalAlign() {
119 | return mDriveControlBoard.getHorizontalAlign();
120 | }
121 | }
122 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/SubsystemManager.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019;
2 |
3 | import com.team254.frc2019.loops.ILooper;
4 | import com.team254.frc2019.loops.Loop;
5 | import com.team254.frc2019.loops.Looper;
6 | import com.team254.frc2019.subsystems.Subsystem;
7 |
8 | import java.util.ArrayList;
9 | import java.util.List;
10 |
11 | /**
12 | * Used to reset, start, stop, and update all subsystems at once
13 | */
14 | public class SubsystemManager implements ILooper {
15 | public static SubsystemManager mInstance = null;
16 |
17 | private List mAllSubsystems;
18 | private List mLoops = new ArrayList<>();
19 |
20 | private SubsystemManager() {}
21 |
22 | public static SubsystemManager getInstance() {
23 | if (mInstance == null) {
24 | mInstance = new SubsystemManager();
25 | }
26 |
27 | return mInstance;
28 | }
29 |
30 | public void outputToSmartDashboard() {
31 | mAllSubsystems.forEach(Subsystem::outputTelemetry);
32 | }
33 |
34 | public boolean checkSubsystems() {
35 | boolean ret_val = true;
36 |
37 | for (Subsystem s : mAllSubsystems) {
38 | ret_val &= s.checkSystem();
39 | }
40 |
41 | return ret_val;
42 | }
43 |
44 | public void stop() {
45 | mAllSubsystems.forEach(Subsystem::stop);
46 | }
47 |
48 | public List getSubsystems() {
49 | return mAllSubsystems;
50 | }
51 |
52 | public void setSubsystems(Subsystem[]... subsystems) {
53 | List allSubsystems = new ArrayList<>();
54 | for (int i = 0; i < subsystems.length; i++) {
55 | for (int j = 0; j < subsystems[i].length; j++) {
56 | allSubsystems.add(subsystems[i][j]);
57 | }
58 | }
59 |
60 | mAllSubsystems = allSubsystems;
61 | }
62 |
63 | private class EnabledLoop implements Loop {
64 | @Override
65 | public void onStart(double timestamp) {
66 | mLoops.forEach(l -> l.onStart(timestamp));
67 | }
68 |
69 | @Override
70 | public void onLoop(double timestamp) {
71 | mAllSubsystems.forEach(Subsystem::readPeriodicInputs);
72 | mLoops.forEach(l -> l.onLoop(timestamp));
73 | mAllSubsystems.forEach(Subsystem::writePeriodicOutputs);
74 | }
75 |
76 | @Override
77 | public void onStop(double timestamp) {
78 | mLoops.forEach(l -> l.onStop(timestamp));
79 | }
80 | }
81 |
82 | private class DisabledLoop implements Loop {
83 | @Override
84 | public void onStart(double timestamp) {}
85 |
86 | @Override
87 | public void onLoop(double timestamp) {
88 | mAllSubsystems.forEach(Subsystem::readPeriodicInputs);
89 | mAllSubsystems.forEach(Subsystem::writePeriodicOutputs);
90 | }
91 |
92 | @Override
93 | public void onStop(double timestamp) {}
94 | }
95 |
96 | public void registerEnabledLoops(Looper enabledLooper) {
97 | mAllSubsystems.forEach(s -> s.registerEnabledLoops(this));
98 | enabledLooper.register(new EnabledLoop());
99 | }
100 |
101 | public void registerDisabledLoops(Looper disabledLooper) {
102 | disabledLooper.register(new DisabledLoop());
103 | }
104 |
105 | @Override
106 | public void register(Loop loop) {
107 | mLoops.add(loop);
108 | }
109 | }
110 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/wpilib/TimedRobot.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.wpilib;
2 |
3 | /*----------------------------------------------------------------------------*/
4 | /* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
5 | /* Open Source Software - may be modified and shared by FRC teams. The code */
6 | /* must be accompanied by the FIRST BSD license file in the root directory of */
7 | /* the project. */
8 | /*----------------------------------------------------------------------------*/
9 |
10 | import edu.wpi.first.hal.FRCNetComm.tInstances;
11 | import edu.wpi.first.hal.FRCNetComm.tResourceType;
12 | import edu.wpi.first.hal.HAL;
13 | import edu.wpi.first.hal.NotifierJNI;
14 | import edu.wpi.first.wpilibj.RobotController;
15 |
16 | /**
17 | * TimedRobot implements the IterativeRobotBase robot program framework.
18 | *
19 | * The TimedRobot class is intended to be subclassed by a user creating a robot program.
20 | *
21 | *
periodic() functions from the base class are called on an interval by a Notifier instance.
22 | */
23 | public class TimedRobot extends IterativeRobotBase {
24 | public static final double kDefaultPeriod = 0.02;
25 |
26 | // The C pointer to the notifier object. We don't use it directly, it is
27 | // just passed to the JNI bindings.
28 | private final int m_notifier = NotifierJNI.initializeNotifier();
29 |
30 | // The absolute expiration time
31 | private double m_expirationTime;
32 |
33 | /**
34 | * Constructor for TimedRobot.
35 | */
36 | protected TimedRobot() {
37 | this(kDefaultPeriod);
38 | }
39 |
40 | /**
41 | * Constructor for TimedRobot.
42 | *
43 | * @param period Period in seconds.
44 | */
45 | protected TimedRobot(double period) {
46 | super(period);
47 |
48 | HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed);
49 | }
50 |
51 | @Override
52 | @SuppressWarnings("NoFinalizer")
53 | protected void finalize() {
54 | NotifierJNI.stopNotifier(m_notifier);
55 | NotifierJNI.cleanNotifier(m_notifier);
56 | }
57 |
58 | /**
59 | * Provide an alternate "main loop" via startCompetition().
60 | */
61 | @Override
62 | @SuppressWarnings("UnsafeFinalization")
63 | public void startCompetition() {
64 | robotInit();
65 |
66 | // Tell the DS that the robot is ready to be enabled
67 | HAL.observeUserProgramStarting();
68 |
69 | m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period;
70 | updateAlarm();
71 |
72 | // Loop forever, calling the appropriate mode-dependent function
73 | while (true) {
74 | long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
75 | if (curTime == 0) {
76 | break;
77 | }
78 |
79 | m_expirationTime += m_period;
80 | updateAlarm();
81 |
82 | loopFunc();
83 | }
84 | }
85 |
86 | /**
87 | * Get time period between calls to Periodic() functions.
88 | */
89 | public double getPeriod() {
90 | return m_period;
91 | }
92 |
93 | /**
94 | * Update the alarm hardware to reflect the next alarm.
95 | */
96 | @SuppressWarnings("UnsafeFinalization")
97 | private void updateAlarm() {
98 | NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6));
99 | }
100 | }
101 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/AutoModeSelector.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019;
2 |
3 | import com.team254.frc2019.auto.modes.AutoModeBase;
4 | import com.team254.frc2019.auto.modes.DriveByCameraMode;
5 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
6 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
7 |
8 | import java.util.Optional;
9 |
10 | public class AutoModeSelector {
11 |
12 | enum StartingPosition {
13 | LEFT_HAB_2, RIGHT_HAB_2, CENTER_HAB_1, LEFT_HAB_1, RIGHT_HAB_1
14 | }
15 |
16 | enum DesiredMode {
17 | DRIVE_BY_CAMERA
18 | }
19 |
20 | private DesiredMode mCachedDesiredMode = null;
21 | private StartingPosition mCachedStartingPosition = null;
22 |
23 | private SendableChooser mModeChooser;
24 | private SendableChooser mStartPositionChooser;
25 |
26 | private Optional mAutoMode = Optional.empty();
27 |
28 | public AutoModeSelector() {
29 | mStartPositionChooser = new SendableChooser<>();
30 | mStartPositionChooser.setDefaultOption("Left HAB 2", StartingPosition.LEFT_HAB_2);
31 | mStartPositionChooser.addOption("Right HAB 2", StartingPosition.RIGHT_HAB_2);
32 | mStartPositionChooser.addOption("Right HAB 1", StartingPosition.RIGHT_HAB_1);
33 | mStartPositionChooser.addOption("Left HAB 1", StartingPosition.LEFT_HAB_1);
34 | mStartPositionChooser.addOption("Center HAB 1", StartingPosition.CENTER_HAB_1);
35 |
36 | SmartDashboard.putData("Starting Position", mStartPositionChooser);
37 |
38 | mModeChooser = new SendableChooser<>();
39 | mModeChooser.setDefaultOption("Drive By Camera", DesiredMode.DRIVE_BY_CAMERA);
40 | SmartDashboard.putData("Auto mode", mModeChooser);
41 | }
42 |
43 | public void updateModeCreator() {
44 | DesiredMode desiredMode = mModeChooser.getSelected();
45 | StartingPosition staringPosition = mStartPositionChooser.getSelected();
46 | if (mCachedDesiredMode != desiredMode || staringPosition != mCachedStartingPosition) {
47 | System.out.println("Auto selection changed, updating creator: desiredMode->" + desiredMode.name()
48 | + ", starting position->" + staringPosition.name());
49 | mAutoMode = getAutoModeForParams(desiredMode, staringPosition);
50 | }
51 | mCachedDesiredMode = desiredMode;
52 | mCachedStartingPosition = staringPosition;
53 | }
54 |
55 | private boolean startingLeft(StartingPosition position) {
56 | return position == StartingPosition.LEFT_HAB_1 || position == StartingPosition.LEFT_HAB_2;
57 | }
58 |
59 | private boolean startingHab1(StartingPosition position) {
60 | return position == StartingPosition.LEFT_HAB_1 || position == StartingPosition.RIGHT_HAB_1;
61 | }
62 |
63 | private Optional getAutoModeForParams(DesiredMode mode, StartingPosition position) {
64 | switch (mode) {
65 | case DRIVE_BY_CAMERA:
66 | return Optional.of(new DriveByCameraMode());
67 | default:
68 | break;
69 | }
70 |
71 | System.err.println("No valid auto mode found for " + mode);
72 | return Optional.empty();
73 | }
74 |
75 | public void reset() {
76 | mAutoMode = Optional.empty();
77 | mCachedDesiredMode = null;
78 | }
79 |
80 | public void outputToSmartDashboard() {
81 | SmartDashboard.putString("AutoModeSelected", mCachedDesiredMode.name());
82 | SmartDashboard.putString("StartingPositionSelected", mCachedStartingPosition.name());
83 | }
84 |
85 | public Optional getAutoMode() {
86 | return mAutoMode;
87 | }
88 |
89 | public boolean isDriveByCamera() {
90 | return mCachedDesiredMode == DesiredMode.DRIVE_BY_CAMERA;
91 | }
92 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/util/SwerveDriveHelper.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.util;
2 |
3 | import com.team254.frc2019.Kinematics;
4 | import com.team254.lib.geometry.Rotation2d;
5 | import com.team254.lib.geometry.Translation2d;
6 |
7 | /**
8 | * Class based on Team 1323's sendInput method to make driving feel better
9 | */
10 | public class SwerveDriveHelper {
11 | private final static double kHighAdjustmentPower = 1.75 + 0.4375;
12 | private final static double kLowAdjustmentPower = 1.50;
13 | private final static double kMaxSpeed = 1.0;
14 | private final static double kHighPowerRotationScalar = 0.8;
15 | private final static double kLowPowerScalar = 0.5;
16 | private final static double kRotationExponent = 4.0;
17 | private final static double kPoleThreshold = 0.0;
18 | private final static double kRobotRelativePoleThreshold = Math.toRadians(5);
19 | private final static double kDeadband = 0.25;
20 | private final static double kRotationDeadband = 0.15;
21 |
22 | public static DriveSignal calculateDriveSignal(double forwardInput, double strafeInput, double rotationInput,
23 | boolean low_power, boolean field_relative, boolean use_heading_controller) {
24 |
25 | Translation2d translationalInput = new Translation2d(forwardInput, strafeInput);
26 | double inputMagnitude = translationalInput.norm();
27 |
28 | // Snap the translational input to its nearest pole, if it is within a certain
29 | // threshold of it.
30 |
31 | if (field_relative) {
32 | if (Math.abs(translationalInput.direction()
33 | .distance(translationalInput.direction().nearestPole())) < kPoleThreshold) {
34 | translationalInput = translationalInput.direction().nearestPole().toTranslation().scale(inputMagnitude);
35 | }
36 | } else {
37 | if (Math.abs(translationalInput.direction()
38 | .distance(translationalInput.direction().nearestPole())) < kRobotRelativePoleThreshold) {
39 | translationalInput = translationalInput.direction().nearestPole().toTranslation().scale(inputMagnitude);
40 | }
41 | }
42 |
43 |
44 | if (inputMagnitude < kDeadband) {
45 | translationalInput = new Translation2d();
46 | inputMagnitude = 0;
47 | }
48 |
49 | // Scale x and y by applying a power to the magnitude of the vector they create,
50 | // in order to make the controls less sensitive at the lower end.
51 | final double power = (low_power) ? kHighAdjustmentPower : kLowAdjustmentPower;
52 | Rotation2d direction = translationalInput.direction();
53 | double scaledMagnitude = Math.pow(inputMagnitude, power);
54 | translationalInput = new Translation2d(direction.cos() * scaledMagnitude, direction.sin() * scaledMagnitude);
55 |
56 | rotationInput = (Math.abs(rotationInput) < kRotationDeadband) ? 0 : rotationInput;
57 | if (use_heading_controller) { // current constants are tuned to be put to the power of 1.75, and I don't want to retune right now
58 | rotationInput = Math.pow(Math.abs(rotationInput), 1.75) * Math.signum(rotationInput);
59 | } else {
60 | rotationInput = Math.pow(Math.abs(rotationInput), kRotationExponent) * Math.signum(rotationInput);
61 | }
62 |
63 | translationalInput = translationalInput.scale(kMaxSpeed);
64 | rotationInput *= kMaxSpeed;
65 |
66 | if (low_power) {
67 | translationalInput = translationalInput.scale(kLowPowerScalar);
68 | rotationInput *= kLowPowerScalar;
69 | } else {
70 | rotationInput *= kHighPowerRotationScalar;
71 | }
72 |
73 | return Kinematics.inverseKinematics(translationalInput.x(), translationalInput.y(), rotationInput,
74 | field_relative);
75 | }
76 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/drivers/SparkMaxFactory.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.drivers;
2 |
3 | import com.revrobotics.CANError;
4 | import com.revrobotics.CANSparkMax;
5 | import com.revrobotics.CANSparkMax.IdleMode;
6 | import com.revrobotics.CANSparkMaxLowLevel;
7 | import com.revrobotics.ControlType;
8 | import edu.wpi.first.wpilibj.DriverStation;
9 | import edu.wpi.first.wpilibj.Timer;
10 |
11 | /**
12 | * Creates CANTalon objects and configures all the parameters we care about to factory defaults. Closed-loop and sensor
13 | * parameters are not set, as these are expected to be set by the application.
14 | */
15 | public class SparkMaxFactory {
16 | public static class Configuration {
17 | public boolean BURN_FACTORY_DEFAULT_FLASH = false;
18 | public IdleMode NEUTRAL_MODE = IdleMode.kCoast;
19 | public boolean INVERTED = false;
20 |
21 | public int STATUS_FRAME_0_RATE_MS = 10;
22 | public int STATUS_FRAME_1_RATE_MS = 1000;
23 | public int STATUS_FRAME_2_RATE_MS = 1000;
24 |
25 | public double OPEN_LOOP_RAMP_RATE = 0.0;
26 | public double CLOSED_LOOP_RAMP_RATE = 0.0;
27 |
28 | public boolean ENABLE_VOLTAGE_COMPENSATION = false;
29 | public double NOMINAL_VOLTAGE = 12.0;
30 | }
31 |
32 | private static final Configuration kDefaultConfiguration = new Configuration();
33 | private static final Configuration kSlaveConfiguration = new Configuration();
34 |
35 | static {
36 | kSlaveConfiguration.STATUS_FRAME_0_RATE_MS = 1000;
37 | kSlaveConfiguration.STATUS_FRAME_1_RATE_MS = 1000;
38 | kSlaveConfiguration.STATUS_FRAME_2_RATE_MS = 1000;
39 | }
40 |
41 | // Create a CANTalon with the default (out of the box) configuration.
42 | public static LazySparkMax createDefaultSparkMax(int id) {
43 | return createSparkMax(id, kDefaultConfiguration);
44 | }
45 |
46 | private static void handleCANError(int id, CANError error, String message) {
47 | if (error != CANError.kOK) {
48 | DriverStation.reportError(
49 | "Could not configure spark id: " + id + " error: " + error.toString() + " " + message, false);
50 | }
51 | }
52 |
53 | public static LazySparkMax createPermanentSlaveSparkMax(int id, CANSparkMax master) {
54 | final LazySparkMax sparkMax = createSparkMax(id, kSlaveConfiguration);
55 | handleCANError(id, sparkMax.follow(master), "setting follower");
56 | return sparkMax;
57 | }
58 |
59 | public static LazySparkMax createSparkMax(int id, Configuration config) {
60 | // Delay for CAN bus bandwidth to clear up.
61 | Timer.delay(0.25);
62 | LazySparkMax sparkMax = new LazySparkMax(id);
63 | handleCANError(id, sparkMax.setCANTimeout(200), "set timeout");
64 |
65 | //sparkMax.restoreFactoryDefaults(config.BURN_FACTORY_DEFAULT_FLASH);
66 |
67 | sparkMax.set(ControlType.kDutyCycle, 0.0);
68 |
69 | handleCANError(id, sparkMax.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, config.STATUS_FRAME_0_RATE_MS), "set status0 rate");
70 | handleCANError(id, sparkMax.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, config.STATUS_FRAME_1_RATE_MS), "set status1 rate");
71 | handleCANError(id, sparkMax.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, config.STATUS_FRAME_2_RATE_MS), "set status2 rate");
72 |
73 | sparkMax.clearFaults();
74 |
75 | handleCANError(id, sparkMax.setIdleMode(config.NEUTRAL_MODE), "set neutrual");
76 | sparkMax.setInverted(config.INVERTED);
77 | handleCANError(id, sparkMax.setOpenLoopRampRate(config.OPEN_LOOP_RAMP_RATE), "set open loop ramp");
78 | handleCANError(id, sparkMax.setClosedLoopRampRate(config.CLOSED_LOOP_RAMP_RATE), "set closed loop ramp");
79 |
80 | if (config.ENABLE_VOLTAGE_COMPENSATION) {
81 | handleCANError(id, sparkMax.enableVoltageCompensation(config.NOMINAL_VOLTAGE), "voltage compensation");
82 | } else {
83 | handleCANError(id, sparkMax.disableVoltageCompensation(), "voltage compensation");
84 | }
85 |
86 | return sparkMax;
87 | }
88 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/geometry/Pose2dWithCurvature.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.geometry;
2 |
3 | import com.team254.lib.util.Util;
4 |
5 | import java.text.DecimalFormat;
6 |
7 | public class Pose2dWithCurvature implements IPose2d, ICurvature {
8 | protected static final Pose2dWithCurvature kIdentity = new Pose2dWithCurvature();
9 |
10 | public static Pose2dWithCurvature identity() {
11 | return kIdentity;
12 | }
13 |
14 | protected final Pose2d pose_;
15 | protected final double curvature_;
16 | protected final double dcurvature_ds_;
17 |
18 | public Pose2dWithCurvature() {
19 | pose_ = new Pose2d();
20 | curvature_ = 0.0;
21 | dcurvature_ds_ = 0.0;
22 | }
23 |
24 | public Pose2dWithCurvature(final Pose2d pose, double curvature) {
25 | pose_ = pose;
26 | curvature_ = curvature;
27 | dcurvature_ds_ = 0.0;
28 | }
29 |
30 | public Pose2dWithCurvature(final Pose2d pose, double curvature, double dcurvature_ds) {
31 | pose_ = pose;
32 | curvature_ = curvature;
33 | dcurvature_ds_ = dcurvature_ds;
34 | }
35 |
36 | public Pose2dWithCurvature(final Translation2d translation, final Rotation2d rotation, double curvature) {
37 | pose_ = new Pose2d(translation, rotation);
38 | curvature_ = curvature;
39 | dcurvature_ds_ = 0.0;
40 | }
41 |
42 | public Pose2dWithCurvature(final Translation2d translation, final Rotation2d rotation, double curvature, double dcurvature_ds) {
43 | pose_ = new Pose2d(translation, rotation);
44 | curvature_ = curvature;
45 | dcurvature_ds_ = dcurvature_ds;
46 | }
47 |
48 | @Override
49 | public final Pose2d getPose() {
50 | return pose_;
51 | }
52 |
53 | @Override
54 | public Pose2dWithCurvature transformBy(Pose2d transform) {
55 | return new Pose2dWithCurvature(getPose().transformBy(transform), getCurvature(), getDCurvatureDs());
56 | }
57 |
58 | @Override
59 | public Pose2dWithCurvature mirror() {
60 | return new Pose2dWithCurvature(getPose().mirror().getPose(), -getCurvature(), -getDCurvatureDs());
61 | }
62 |
63 | @Override
64 | public double getCurvature() {
65 | return curvature_;
66 | }
67 |
68 | @Override
69 | public double getDCurvatureDs() {
70 | return dcurvature_ds_;
71 | }
72 |
73 | @Override
74 | public final Translation2d getTranslation() {
75 | return getPose().getTranslation();
76 | }
77 |
78 | @Override
79 | public final Rotation2d getRotation() {
80 | return getPose().getRotation();
81 | }
82 |
83 | @Override
84 | public Pose2dWithCurvature interpolate(final Pose2dWithCurvature other, double x) {
85 | return new Pose2dWithCurvature(getPose().interpolate(other.getPose(), x),
86 | Util.interpolate(getCurvature(), other.getCurvature(), x),
87 | Util.interpolate(getDCurvatureDs(), other.getDCurvatureDs(), x));
88 | }
89 |
90 | @Override
91 | public double distance(final Pose2dWithCurvature other) {
92 | return getPose().distance(other.getPose());
93 | }
94 |
95 | @Override
96 | public boolean equals(final Object other) {
97 | if (!(other instanceof Pose2dWithCurvature)) {
98 | return false;
99 | }
100 |
101 | Pose2dWithCurvature p2dwc = (Pose2dWithCurvature) other;
102 | return getPose().equals(p2dwc.getPose()) && Util.epsilonEquals(getCurvature(), p2dwc.getCurvature()) && Util.epsilonEquals(getDCurvatureDs(), p2dwc.getDCurvatureDs());
103 | }
104 |
105 | @Override
106 | public String toString() {
107 | final DecimalFormat fmt = new DecimalFormat("#0.000");
108 | return getPose().toString() + ", curvature: " + fmt.format(getCurvature()) + ", dcurvature_ds: " + fmt.format(getDCurvatureDs());
109 | }
110 |
111 | @Override
112 | public String toCSV() {
113 | final DecimalFormat fmt = new DecimalFormat("#0.000");
114 | return getPose().toCSV() + "," + fmt.format(getCurvature()) + "," + fmt.format(getDCurvatureDs());
115 | }
116 | }
117 |
--------------------------------------------------------------------------------
/vendordeps/Phoenix.json:
--------------------------------------------------------------------------------
1 | {
2 | "fileName": "Phoenix.json",
3 | "name": "CTRE-Phoenix",
4 | "version": "5.14.1",
5 | "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
6 | "mavenUrls": [
7 | "http://devsite.ctr-electronics.com/maven/release/"
8 | ],
9 | "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json",
10 | "javaDependencies": [
11 | {
12 | "groupId": "com.ctre.phoenix",
13 | "artifactId": "api-java",
14 | "version": "5.14.1"
15 | },
16 | {
17 | "groupId": "com.ctre.phoenix",
18 | "artifactId": "wpiapi-java",
19 | "version": "5.14.1"
20 | }
21 | ],
22 | "jniDependencies": [
23 | {
24 | "groupId": "com.ctre.phoenix",
25 | "artifactId": "cci",
26 | "version": "5.14.1",
27 | "isJar": false,
28 | "skipInvalidPlatforms": true,
29 | "validPlatforms": [
30 | "linuxathena",
31 | "windowsx86-64",
32 | "linuxx86-64"
33 | ]
34 | },
35 | {
36 | "groupId": "com.ctre.phoenix",
37 | "artifactId": "canutils",
38 | "version": "5.14.1",
39 | "isJar": false,
40 | "skipInvalidPlatforms": true,
41 | "validPlatforms": [
42 | "windowsx86-64",
43 | "linuxx86-64"
44 | ]
45 | },
46 | {
47 | "groupId": "com.ctre.phoenix",
48 | "artifactId": "platform-stub",
49 | "version": "5.14.1",
50 | "isJar": false,
51 | "skipInvalidPlatforms": true,
52 | "validPlatforms": [
53 | "windowsx86-64",
54 | "linuxx86-64"
55 | ]
56 | }
57 | ],
58 | "cppDependencies": [
59 | {
60 | "groupId": "com.ctre.phoenix",
61 | "artifactId": "wpiapi-cpp",
62 | "version": "5.14.1",
63 | "libName": "CTRE_Phoenix_WPI",
64 | "headerClassifier": "headers",
65 | "sharedLibrary": false,
66 | "skipInvalidPlatforms": true,
67 | "binaryPlatforms": [
68 | "linuxathena",
69 | "windowsx86-64",
70 | "linuxx86-64"
71 | ]
72 | },
73 | {
74 | "groupId": "com.ctre.phoenix",
75 | "artifactId": "api-cpp",
76 | "version": "5.14.1",
77 | "libName": "CTRE_Phoenix",
78 | "headerClassifier": "headers",
79 | "sharedLibrary": false,
80 | "skipInvalidPlatforms": true,
81 | "binaryPlatforms": [
82 | "linuxathena",
83 | "windowsx86-64",
84 | "linuxx86-64"
85 | ]
86 | },
87 | {
88 | "groupId": "com.ctre.phoenix",
89 | "artifactId": "cci",
90 | "version": "5.14.1",
91 | "libName": "CTRE_PhoenixCCI",
92 | "headerClassifier": "headers",
93 | "sharedLibrary": false,
94 | "skipInvalidPlatforms": true,
95 | "binaryPlatforms": [
96 | "linuxathena",
97 | "windowsx86-64",
98 | "linuxx86-64"
99 | ]
100 | },
101 | {
102 | "groupId": "com.ctre.phoenix",
103 | "artifactId": "canutils",
104 | "version": "5.14.1",
105 | "libName": "CTRE_PhoenixCanutils",
106 | "headerClassifier": "headers",
107 | "sharedLibrary": false,
108 | "skipInvalidPlatforms": true,
109 | "binaryPlatforms": [
110 | "windowsx86-64",
111 | "linuxx86-64"
112 | ]
113 | },
114 | {
115 | "groupId": "com.ctre.phoenix",
116 | "artifactId": "platform-stub",
117 | "version": "5.14.1",
118 | "libName": "CTRE_PhoenixPlatform",
119 | "headerClassifier": "headers",
120 | "sharedLibrary": false,
121 | "skipInvalidPlatforms": true,
122 | "binaryPlatforms": [
123 | "windowsx86-64",
124 | "linuxx86-64"
125 | ]
126 | },
127 | {
128 | "groupId": "com.ctre.phoenix",
129 | "artifactId": "core",
130 | "version": "5.14.1",
131 | "libName": "CTRE_PhoenixCore",
132 | "headerClassifier": "headers"
133 | }
134 | ]
135 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/Elevator.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.ctre.phoenix.motorcontrol.ControlMode;
4 | import com.ctre.phoenix.motorcontrol.DemandType;
5 | import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
6 | import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
7 | import com.ctre.phoenix.motorcontrol.can.TalonSRX;
8 | import com.team254.frc2019.Constants;
9 | import com.team254.lib.drivers.MotorChecker;
10 | import com.team254.lib.drivers.TalonSRXChecker;
11 | import com.team254.lib.drivers.TalonSRXUtil;
12 | import com.team254.lib.util.LatchedBoolean;
13 |
14 | import java.util.ArrayList;
15 |
16 | public class Elevator extends ServoMotorSubsystem {
17 | private static Elevator mInstance;
18 | private boolean mHoming = false;
19 | private LatchedBoolean mJustReset = new LatchedBoolean();
20 | private boolean mCanHome = true;
21 |
22 | public synchronized static Elevator getInstance() {
23 | if (mInstance == null) {
24 | mInstance = new Elevator(Constants.kElevatorConstants);
25 | }
26 |
27 | return mInstance;
28 | }
29 |
30 | private Elevator(final ServoMotorSubsystemConstants constants) {
31 | super(constants);
32 | TalonSRXUtil.checkError(
33 | mMaster.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector,
34 | LimitSwitchNormal.NormallyOpen, Constants.kLongCANTimeoutMs),
35 | "Unable to set reverse limit switch for elevator.");
36 |
37 | mMaster.overrideLimitSwitchesEnable(true);
38 | }
39 |
40 | public synchronized void setCanHome(boolean canHome) {
41 | mCanHome = canHome;
42 | }
43 |
44 | @Override
45 | public synchronized boolean atHomingLocation() {
46 | return mMaster.getSensorCollection().isRevLimitSwitchClosed();
47 | }
48 |
49 | @Override
50 | public synchronized void handleMasterReset(boolean reset) {
51 | if (mJustReset.update(reset) && mCanHome) {
52 | System.out.println("Elevator going into home mode!");
53 | mHoming = true;
54 | LED.getInstance().setElevatorFault();
55 | mMaster.overrideSoftLimitsEnable(false);
56 | }
57 | }
58 |
59 | public synchronized boolean isHoming() {
60 | return mHoming;
61 | }
62 |
63 | @Override
64 | public synchronized void writePeriodicOutputs() {
65 | if (mHoming) {
66 | if (atHomingLocation()) {
67 | zeroSensors();
68 | mMaster.overrideSoftLimitsEnable(true);
69 | System.out.println("Homed!!!");
70 | LED.getInstance().clearElevatorFault();
71 | mHoming = false;
72 | }
73 |
74 | if (mControlState == ControlState.OPEN_LOOP) {
75 | mMaster.set(ControlMode.PercentOutput, mPeriodicIO.demand, DemandType.ArbitraryFeedForward,
76 | 0.0);
77 | } else {
78 | mMaster.set(ControlMode.PercentOutput, 0.0, DemandType.ArbitraryFeedForward,
79 | 0.0);
80 | }
81 | } else {
82 | super.writePeriodicOutputs();
83 | }
84 |
85 | }
86 |
87 |
88 | public synchronized void updateSoftLimit(int limit) {
89 | mMaster.configForwardSoftLimitThreshold(limit);
90 | }
91 |
92 | public synchronized void removeCurrentLimits() {
93 | mMaster.enableCurrentLimit(false);
94 | }
95 |
96 | @Override
97 | public boolean checkSystem() {
98 | return TalonSRXChecker.checkMotors(this,
99 | new ArrayList>() {
100 | private static final long serialVersionUID = 2555581143886197844L;
101 |
102 | {
103 | add(new MotorChecker.MotorConfig<>("master", mMaster));
104 | add(new MotorChecker.MotorConfig<>("slave 1", mSlaves[0]));
105 | add(new MotorChecker.MotorConfig<>("slave 2", mSlaves[1]));
106 | }
107 | }, new MotorChecker.CheckerConfig() {
108 | {
109 | mRunOutputPercentage = 0.5;
110 | mRunTimeSec = 1.0;
111 | mCurrentFloor = 0.1;
112 | mRPMFloor = 90;
113 | mCurrentEpsilon = 2.0;
114 | mRPMEpsilon = 200;
115 | mRPMSupplier = () -> mMaster.getSelectedSensorVelocity();
116 | }
117 | });
118 | }
119 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/CarriageCanifier.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.ctre.phoenix.CANifier;
4 | import com.ctre.phoenix.CANifier.PinValues;
5 | import com.ctre.phoenix.CANifierStatusFrame;
6 | import com.team254.frc2019.Constants;
7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
8 |
9 | public class CarriageCanifier extends Subsystem {
10 | private static CarriageCanifier mInstance;
11 | private CANifier mCanifierArm, mCanifierWrist;
12 | private PeriodicInputs mPeriodicInputs;
13 | private PeriodicOutputs mPeriodicOutputs;
14 | private boolean mOutputsChanged;
15 |
16 | private CarriageCanifier() {
17 | mCanifierArm = new CANifier(Constants.kCanifierArmId);
18 | mCanifierWrist = new CANifier(Constants.kCanifierWristId);
19 |
20 | mCanifierArm.setStatusFramePeriod(CANifierStatusFrame.Status_2_General, 10, Constants.kLongCANTimeoutMs);
21 | mCanifierWrist.setStatusFramePeriod(CANifierStatusFrame.Status_2_General, 10, Constants.kLongCANTimeoutMs);
22 |
23 | mPeriodicInputs = new PeriodicInputs();
24 | mPeriodicOutputs = new PeriodicOutputs();
25 |
26 | // Force a first update.
27 | mOutputsChanged = true;
28 | }
29 |
30 | public synchronized static CarriageCanifier getInstance() {
31 | if (mInstance == null) {
32 | mInstance = new CarriageCanifier();
33 | }
34 | return mInstance;
35 | }
36 |
37 | public synchronized void setLEDColor(double red, double green, double blue) {
38 | if (red != mPeriodicOutputs.r_ ||
39 | green != mPeriodicOutputs.g_ ||
40 | blue != mPeriodicOutputs.b_) {
41 | mPeriodicOutputs.r_ = red;
42 | mPeriodicOutputs.g_ = green;
43 | mPeriodicOutputs.b_ = blue;
44 | mOutputsChanged = true;
45 | }
46 | }
47 |
48 | @Override
49 | public synchronized void readPeriodicInputs() {
50 | mPeriodicInputs.has_ball_ = mCanifierArm.getGeneralInput(CANifier.GeneralPin.LIMF);
51 | PinValues pinValues = new PinValues();
52 | mCanifierWrist.getGeneralInputs(pinValues);
53 | mPeriodicInputs.low_pressure_channel_ = pinValues.QUAD_IDX; // black wire, out1
54 | mPeriodicInputs.high_pressure_channel_ = pinValues.SCL; // white wire, out2
55 | }
56 |
57 | @Override
58 | public synchronized void writePeriodicOutputs() {
59 | // A: Green
60 | // B: Red
61 | // C: Blue
62 | if (mOutputsChanged) {
63 | mCanifierWrist.setLEDOutput(mPeriodicOutputs.g_, CANifier.LEDChannel.LEDChannelA);
64 | mCanifierWrist.setLEDOutput(mPeriodicOutputs.r_, CANifier.LEDChannel.LEDChannelB);
65 | mCanifierWrist.setLEDOutput(mPeriodicOutputs.b_, CANifier.LEDChannel.LEDChannelC);
66 | mOutputsChanged = false;
67 | }
68 | }
69 |
70 | @Override
71 | public boolean checkSystem() {
72 | return false;
73 | }
74 |
75 | @Override
76 | public synchronized void outputTelemetry() {
77 | SmartDashboard.putBoolean("HasBall:", mPeriodicInputs.has_ball_);
78 | SmartDashboard.putBoolean("LowPressureSensor:", mPeriodicInputs.low_pressure_channel_);
79 | SmartDashboard.putBoolean("HighPressureSensor:", mPeriodicInputs.high_pressure_channel_);
80 | }
81 |
82 | public synchronized boolean hasBall() {
83 | return mPeriodicInputs.has_ball_;
84 | }
85 |
86 | public synchronized boolean getLowPressureSensor() {
87 | return mPeriodicInputs.low_pressure_channel_;
88 | }
89 |
90 | public synchronized boolean getHighPressureSensor() {
91 | return mPeriodicInputs.high_pressure_channel_;
92 | }
93 |
94 | @Override
95 | public void stop() {
96 | mPeriodicOutputs = new PeriodicOutputs();
97 | mOutputsChanged = true;
98 | writePeriodicOutputs();
99 | }
100 |
101 | @Override
102 | public synchronized void zeroSensors() {
103 | mCanifierWrist.setQuadraturePosition(0, 0);
104 | mCanifierArm.setQuadraturePosition(0, 0);
105 | }
106 |
107 | private static class PeriodicInputs {
108 | public boolean has_ball_; // Q12 Banner photoelectric sensor on the end effector
109 | public boolean low_pressure_channel_; // "lower" (more negative) threshold on the ProSense QPS digital pressure sensor
110 | public boolean high_pressure_channel_; // "higher" (closer to 0) threshold on the ProSense QPS digital pressure sensor
111 | }
112 |
113 | private static class PeriodicOutputs {
114 | public double r_;
115 | public double g_;
116 | public double b_;
117 | }
118 | }
119 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/drivers/MotorChecker.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.drivers;
2 |
3 | import com.team254.frc2019.subsystems.Subsystem;
4 | import com.team254.lib.util.Util;
5 | import edu.wpi.first.wpilibj.Timer;
6 |
7 | import java.util.ArrayList;
8 | import java.util.function.DoubleSupplier;
9 |
10 | public abstract class MotorChecker {
11 | public static class CheckerConfig {
12 | public double mCurrentFloor = 5;
13 | public double mRPMFloor = 2000;
14 |
15 | public double mCurrentEpsilon = 5.0;
16 | public double mRPMEpsilon = 500;
17 | public DoubleSupplier mRPMSupplier = null;
18 |
19 | public double mRunTimeSec = 4.0;
20 | public double mWaitTimeSec = 2.0;
21 | public double mRunOutputPercentage = 0.5;
22 | }
23 |
24 | public static class MotorConfig {
25 | public String mName;
26 | public T mMotor;
27 |
28 | public MotorConfig(String name, T motor) {
29 | mName = name;
30 | mMotor = motor;
31 | }
32 | }
33 |
34 | protected ArrayList> mMotorsToCheck;
35 |
36 | protected abstract void storeConfiguration();
37 |
38 | protected abstract void restoreConfiguration();
39 |
40 | protected abstract void setMotorOutput(T motor, double output);
41 |
42 | protected abstract double getMotorCurrent(T motor);
43 |
44 | protected boolean checkMotorsImpl(Subsystem subsystem,
45 | ArrayList> motorsToCheck,
46 | CheckerConfig checkerConfig) {
47 | boolean failure = false;
48 | System.out.println("////////////////////////////////////////////////");
49 | System.out.println("Checking subsystem " + subsystem.getClass()
50 | + " for " + motorsToCheck.size() + " motors.");
51 |
52 | ArrayList currents = new ArrayList<>();
53 | ArrayList rpms = new ArrayList<>();
54 |
55 | mMotorsToCheck = motorsToCheck;
56 | storeConfiguration();
57 |
58 | for (MotorConfig config : motorsToCheck) {
59 | setMotorOutput(config.mMotor, 0.0);
60 | }
61 |
62 | for (MotorConfig config : motorsToCheck) {
63 | System.out.println("Checking: " + config.mName);
64 |
65 | setMotorOutput(config.mMotor, checkerConfig.mRunOutputPercentage);
66 | Timer.delay(checkerConfig.mRunTimeSec);
67 |
68 | // poll the interesting information
69 | double current = getMotorCurrent(config.mMotor);
70 | currents.add(current);
71 | System.out.print("Current: " + current);
72 |
73 | double rpm = Double.NaN;
74 | if (checkerConfig.mRPMSupplier != null) {
75 | rpm = checkerConfig.mRPMSupplier.getAsDouble();
76 | rpms.add(rpm);
77 | System.out.print(" RPM: " + rpm);
78 | }
79 | System.out.print('\n');
80 |
81 | setMotorOutput(config.mMotor, 0.0);
82 |
83 | // perform checks
84 | if (current < checkerConfig.mCurrentFloor) {
85 | System.out.println(config.mName + " has failed current floor check vs " +
86 | checkerConfig.mCurrentFloor + "!!");
87 | failure = true;
88 | }
89 | if (checkerConfig.mRPMSupplier != null) {
90 | if (rpm < checkerConfig.mRPMFloor) {
91 | System.out.println(config.mName + " has failed rpm floor check vs " +
92 | checkerConfig.mRPMFloor + "!!");
93 | failure = true;
94 | }
95 | }
96 |
97 | Timer.delay(checkerConfig.mWaitTimeSec);
98 | }
99 |
100 | // run aggregate checks
101 |
102 | if (currents.size() > 0) {
103 | double average = currents.stream().mapToDouble(val -> val).average().getAsDouble();
104 |
105 | if (!Util.allCloseTo(currents, average, checkerConfig.mCurrentEpsilon)) {
106 | System.out.println("Currents varied!!!!!!!!!!!");
107 | failure = true;
108 | }
109 | }
110 |
111 | if (rpms.size() > 0) {
112 | double average = rpms.stream().mapToDouble(val -> val).average().getAsDouble();
113 |
114 | if (!Util.allCloseTo(rpms, average, checkerConfig.mRPMEpsilon)) {
115 | System.out.println("RPMs varied!!!!!!!!");
116 | failure = true;
117 | }
118 | }
119 |
120 | // restore talon configurations
121 | restoreConfiguration();
122 |
123 | return !failure;
124 | }
125 | }
126 |
127 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/motion/SetpointGenerator.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.motion;
2 |
3 | import java.util.Optional;
4 |
5 | /**
6 | * A SetpointGenerate does just-in-time motion profile generation to supply a stream of setpoints that obey the given
7 | * constraints to a controller. The profile is regenerated when any of the inputs change, but is cached (and trimmed as
8 | * we go) if the only update is to the current state.
9 | *
10 | * Note that typically for smooth control, a user will feed the last iteration's setpoint as the argument to
11 | * getSetpoint(), and should only use a measured state directly on the first iteration or if a large disturbance is
12 | * detected.
13 | */
14 | public class SetpointGenerator {
15 | /**
16 | * A Setpoint is just a MotionState and an additional flag indicating whether this is setpoint achieves the goal
17 | * (useful for higher-level logic to know that it is now time to do something else).
18 | */
19 | public static class Setpoint {
20 | public MotionState motion_state;
21 | public boolean final_setpoint;
22 |
23 | public Setpoint(MotionState motion_state, boolean final_setpoint) {
24 | this.motion_state = motion_state;
25 | this.final_setpoint = final_setpoint;
26 | }
27 | }
28 |
29 | protected MotionProfile mProfile = null;
30 | protected MotionProfileGoal mGoal = null;
31 | protected MotionProfileConstraints mConstraints = null;
32 |
33 | public SetpointGenerator() {}
34 |
35 | /**
36 | * Force a reset of the profile.
37 | */
38 | public void reset() {
39 | mProfile = null;
40 | mGoal = null;
41 | mConstraints = null;
42 | }
43 |
44 | /**
45 | * Get a new Setpoint (and generate a new MotionProfile if necessary).
46 | *
47 | * @param constraints The constraints to use.
48 | * @param goal The goal to use.
49 | * @param prev_state The previous setpoint (or measured state of the system to do a reset).
50 | * @param t The time to generate a setpoint for.
51 | * @return The new Setpoint at time t.
52 | */
53 | public synchronized Setpoint getSetpoint(MotionProfileConstraints constraints, MotionProfileGoal goal,
54 | MotionState prev_state,
55 | double t) {
56 | boolean regenerate = mConstraints == null || !mConstraints.equals(constraints) || mGoal == null
57 | || !mGoal.equals(goal) || mProfile == null;
58 | if (!regenerate && !mProfile.isEmpty()) {
59 | Optional expected_state = mProfile.stateByTime(prev_state.t());
60 | regenerate = !expected_state.isPresent() || !expected_state.get().equals(prev_state);
61 | }
62 | if (regenerate) {
63 | // Regenerate the profile, as our current profile does not satisfy the inputs.
64 | mConstraints = constraints;
65 | mGoal = goal;
66 | mProfile = MotionProfileGenerator.generateProfile(constraints, goal, prev_state);
67 | // System.out.println("Regenerating profile: " + mProfile);
68 | }
69 |
70 | // Sample the profile at time t.
71 | Setpoint rv = null;
72 | if (!mProfile.isEmpty() && mProfile.isValid()) {
73 | MotionState setpoint;
74 | if (t > mProfile.endTime()) {
75 | setpoint = mProfile.endState();
76 | } else if (t < mProfile.startTime()) {
77 | setpoint = mProfile.startState();
78 | } else {
79 | setpoint = mProfile.stateByTime(t).get();
80 | }
81 | // Shorten the profile and return the new setpoint.
82 | mProfile.trimBeforeTime(t);
83 | rv = new Setpoint(setpoint, mProfile.isEmpty() || mGoal.atGoalState(setpoint));
84 | }
85 |
86 | // Invalid or empty profile - just output the same state again.
87 | if (rv == null) {
88 | rv = new Setpoint(prev_state, true);
89 | }
90 |
91 | if (rv.final_setpoint) {
92 | // Ensure the final setpoint matches the goal exactly.
93 | rv.motion_state = new MotionState(rv.motion_state.t(), mGoal.pos(),
94 | Math.signum(rv.motion_state.vel()) * Math.max(mGoal.max_abs_vel(), Math.abs(rv.motion_state.vel())),
95 | 0.0);
96 | }
97 |
98 | return rv;
99 | }
100 |
101 | /**
102 | * Get the full profile from the latest call to getSetpoint(). Useful to check estimated time or distance to goal.
103 | *
104 | * @return The profile from the latest call to getSetpoint(), or null if there is not yet a profile.
105 | */
106 | public MotionProfile getProfile() {
107 | return mProfile;
108 | }
109 | }
110 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/controlboard/GamepadDriveControlBoard.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.controlboard;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.lib.geometry.Rotation2d;
5 |
6 | public class GamepadDriveControlBoard implements IDriveControlBoard {
7 | private static GamepadDriveControlBoard mInstance = null;
8 |
9 | public static GamepadDriveControlBoard getInstance() {
10 | if (mInstance == null) {
11 | mInstance = new GamepadDriveControlBoard();
12 | }
13 |
14 | return mInstance;
15 | }
16 |
17 | private final XboxController mController;
18 |
19 | private GamepadDriveControlBoard() {
20 | mController = new XboxController(Constants.kDriveGamepadPort);
21 | }
22 |
23 | @Override
24 | public double getThrottle() {
25 | return mController.getJoystick(XboxController.Side.LEFT, XboxController.Axis.Y);
26 | }
27 |
28 | @Override
29 | public double getStrafe() {
30 | return -mController.getJoystick(XboxController.Side.LEFT, XboxController.Axis.X);
31 | }
32 |
33 | @Override
34 | public double getRotation() {
35 | return -mController.getJoystick(XboxController.Side.RIGHT, XboxController.Axis.X);
36 | }
37 |
38 | @Override
39 | public boolean getDriveLowPower() {
40 | return mController.getButton(XboxController.Button.A);
41 | }
42 |
43 | @Override
44 | public boolean getFieldRelative() {
45 | return !mController.getButton(XboxController.Button.LB);
46 | }
47 |
48 | @Override
49 | public double getDPad() {
50 |
51 | if (mController.getDPad() == -1) {
52 | return -1;
53 | }
54 |
55 | if (mController.getTrigger(XboxController.Side.LEFT)) {
56 | double degs = SwerveCardinal.findClosest(Rotation2d.fromDegrees(mController.getDPad()), true).rotation.getDegrees();
57 | if (degs < 0) {
58 | degs += 360;
59 | }
60 | return degs;
61 | } else {
62 | double degs = SwerveCardinal.findClosest(Rotation2d.fromDegrees(mController.getDPad()), false).rotation.getDegrees();
63 | if (degs < 0) {
64 | degs += 360;
65 | }
66 | return degs;
67 | }
68 | }
69 |
70 |
71 | enum SwerveCardinal {
72 | BACK(180),
73 | FRONT(0),
74 | LEFT(90, -90, false),
75 | RIGHT(-90, 90, false),
76 | NONE(0),
77 | FRONT_LEFT(-30, 180, true),
78 | FRONT_RIGHT(-150, 90, true),
79 | BACK_LEFT(150, 0, true),
80 | BACK_RIGHT(30, -90, true);
81 |
82 | public final Rotation2d rotation;
83 | private final Rotation2d inputDirection;
84 | private final boolean isARocketCardinal;
85 |
86 | SwerveCardinal(double degrees) {
87 | this(degrees, degrees, false);
88 | }
89 |
90 | SwerveCardinal(double degrees, double inputDirectionDegrees, boolean isARocketCardinal) {
91 | rotation = Rotation2d.fromDegrees(degrees);
92 | inputDirection = Rotation2d.fromDegrees(inputDirectionDegrees);
93 | this.isARocketCardinal = isARocketCardinal;
94 | }
95 |
96 | public static SwerveCardinal findClosest(double xAxis, double yAxis, boolean isARocketCardinal) {
97 | return findClosest(new Rotation2d(yAxis, -xAxis, true), isARocketCardinal);
98 | }
99 |
100 | public static SwerveCardinal findClosest(Rotation2d stickDirection, boolean isARocketCardinal) {
101 | var values = SwerveCardinal.values();
102 |
103 | SwerveCardinal closest = null;
104 | double closestDistance = Double.MAX_VALUE;
105 | for (int i = 0; i < values.length; i++) {
106 | if (values[i].isARocketCardinal != isARocketCardinal) {
107 | continue;
108 | }
109 | var checkDirection = values[i];
110 | var distance = Math.abs(stickDirection.distance(checkDirection.inputDirection));
111 | if (distance < closestDistance) {
112 | closestDistance = distance;
113 | closest = checkDirection;
114 | }
115 | }
116 | return closest;
117 | }
118 |
119 | public static boolean isDiagonal(SwerveCardinal cardinal) {
120 | return cardinal == FRONT_LEFT || cardinal == FRONT_RIGHT || cardinal == BACK_LEFT || cardinal == BACK_RIGHT;
121 | }
122 | }
123 |
124 | @Override
125 | public boolean getThrust() {
126 | return mController.getButton(XboxController.Button.RB);
127 | }
128 |
129 | @Override
130 | public boolean getShoot() {
131 | return mController.getTrigger(XboxController.Side.RIGHT);
132 | }
133 |
134 | @Override
135 | public boolean getHorizontalAlign() {
136 | return mController.getButton(XboxController.Button.B);
137 | }
138 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/vision/GoalTrack.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.vision;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.lib.geometry.Pose2d;
5 | import com.team254.lib.geometry.Rotation2d;
6 | import edu.wpi.first.wpilibj.Timer;
7 |
8 | import java.util.Map;
9 | import java.util.TreeMap;
10 |
11 | /**
12 | * A class that is used to keep track of all goals detected by the vision system. As goals are detected/not detected
13 | * anymore by the vision system, function calls will be made to create, destroy, or update a goal track.
14 | *
15 | * This helps in the goal ranking process that determines which goal to fire into, and helps to smooth measurements of
16 | * the goal's location over time.
17 | *
18 | * @see GoalTracker
19 | */
20 | public class GoalTrack {
21 | TreeMap mObservedPositions = new TreeMap<>();
22 | Pose2d mSmoothedPosition = null;
23 | int mId;
24 |
25 | private GoalTrack() {}
26 |
27 | /**
28 | * Makes a new track based on the timestamp and the goal's coordinates (from vision)
29 | */
30 | public static synchronized GoalTrack makeNewTrack(double timestamp, Pose2d first_observation, int id) {
31 | GoalTrack rv = new GoalTrack();
32 | rv.mObservedPositions.put(timestamp, first_observation);
33 | rv.mSmoothedPosition = first_observation;
34 | rv.mId = id;
35 | return rv;
36 | }
37 |
38 | public synchronized void emptyUpdate() {
39 | pruneByTime();
40 | }
41 |
42 | /**
43 | * Attempts to update the track with a new observation.
44 | *
45 | * @return True if the track was updated
46 | */
47 | public synchronized boolean tryUpdate(double timestamp, Pose2d new_observation) {
48 | if (!isAlive()) {
49 | return false;
50 | }
51 | double distance = mSmoothedPosition.inverse().transformBy(new_observation).getTranslation().norm();
52 | if (distance < Constants.kMaxTrackerDistance) {
53 | mObservedPositions.put(timestamp, new_observation);
54 | pruneByTime();
55 | return true;
56 | } else {
57 | emptyUpdate();
58 | return false;
59 | }
60 | }
61 |
62 | public synchronized boolean isAlive() {
63 | return mObservedPositions.size() > 0;
64 | }
65 |
66 | /**
67 | * Removes the track if it is older than the set "age" described in the Constants file.
68 | *
69 | * @see Constants
70 | */
71 | synchronized void pruneByTime() {
72 | double delete_before = Timer.getFPGATimestamp() - Constants.kMaxGoalTrackAge;
73 | mObservedPositions.entrySet().removeIf(entry -> entry.getKey() < delete_before);
74 | if (mObservedPositions.isEmpty()) {
75 | mSmoothedPosition = null;
76 | } else {
77 | smooth();
78 | }
79 | }
80 |
81 | /**
82 | * Averages out the observed positions based on an set of observed positions
83 | */
84 | synchronized void smooth() {
85 | if (isAlive()) {
86 | double x = 0;
87 | double y = 0;
88 | double s = 0; // sin of angle
89 | double c = 0; // cos of angle
90 | double t_now = Timer.getFPGATimestamp();
91 | int num_samples = 0;
92 | for (Map.Entry entry : mObservedPositions.entrySet()) {
93 | if (t_now - entry.getKey() > Constants.kMaxGoalTrackSmoothingTime) {
94 | continue;
95 | }
96 | ++num_samples;
97 | x += entry.getValue().getTranslation().x();
98 | y += entry.getValue().getTranslation().y();
99 | c += entry.getValue().getRotation().cos();
100 | s += entry.getValue().getRotation().sin();
101 | }
102 | x /= num_samples;
103 | y /= num_samples;
104 | s /= num_samples;
105 | c /= num_samples;
106 |
107 | if (num_samples == 0) {
108 | // Handle the case that all samples are older than kMaxGoalTrackSmoothingTime.
109 | mSmoothedPosition = mObservedPositions.lastEntry().getValue();
110 | } else {
111 | mSmoothedPosition = new Pose2d(x, y, new Rotation2d(c, s, true));
112 | }
113 | }
114 | }
115 |
116 | public synchronized Pose2d getSmoothedPosition() {
117 | return mSmoothedPosition;
118 | }
119 |
120 | public synchronized Pose2d getLatestPosition() {
121 | return mObservedPositions.lastEntry().getValue();
122 | }
123 |
124 | public synchronized double getLatestTimestamp() {
125 | return mObservedPositions.keySet().stream().max(Double::compareTo).orElse(0.0);
126 | }
127 |
128 | public synchronized double getStability() {
129 | return Math.min(1.0, mObservedPositions.size() / (Constants.kCameraFrameRate * Constants.kMaxGoalTrackAge));
130 | }
131 |
132 | public synchronized int getId() {
133 | return mId;
134 | }
135 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/geometry/Translation2d.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.geometry;
2 |
3 | import com.team254.lib.util.Util;
4 |
5 | import java.text.DecimalFormat;
6 |
7 | /**
8 | * A translation in a 2d coordinate frame. Translations are simply shifts in an (x, y) plane.
9 | */
10 | public class Translation2d implements ITranslation2d {
11 | protected static final Translation2d kIdentity = new Translation2d();
12 |
13 | public static Translation2d identity() {
14 | return kIdentity;
15 | }
16 |
17 | protected final double x_;
18 | protected final double y_;
19 |
20 | public Translation2d() {
21 | x_ = 0;
22 | y_ = 0;
23 | }
24 |
25 | public Translation2d(double x, double y) {
26 | x_ = x;
27 | y_ = y;
28 | }
29 |
30 | public Translation2d(final Translation2d other) {
31 | x_ = other.x_;
32 | y_ = other.y_;
33 | }
34 |
35 | public Translation2d(final Translation2d start, final Translation2d end) {
36 | x_ = end.x_ - start.x_;
37 | y_ = end.y_ - start.y_;
38 | }
39 |
40 | /**
41 | * The "norm" of a transform is the Euclidean distance in x and y.
42 | *
43 | * @return sqrt(x ^ 2 + y ^ 2)
44 | */
45 | public double norm() {
46 | return Math.hypot(x_, y_);
47 | }
48 |
49 | public double norm2() {
50 | return x_ * x_ + y_ * y_;
51 | }
52 |
53 | public double x() {
54 | return x_;
55 | }
56 |
57 | public double y() {
58 | return y_;
59 | }
60 |
61 | /**
62 | * We can compose Translation2d's by adding together the x and y shifts.
63 | *
64 | * @param other The other translation to add.
65 | * @return The combined effect of translating by this object and the other.
66 | */
67 | public Translation2d translateBy(final Translation2d other) {
68 | return new Translation2d(x_ + other.x_, y_ + other.y_);
69 | }
70 |
71 | /**
72 | * We can also rotate Translation2d's. See: https://en.wikipedia.org/wiki/Rotation_matrix
73 | *
74 | * @param rotation The rotation to apply.
75 | * @return This translation rotated by rotation.
76 | */
77 | public Translation2d rotateBy(final Rotation2d rotation) {
78 | return new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos());
79 | }
80 |
81 | public Rotation2d direction() {
82 | return new Rotation2d(x_, y_, true);
83 | }
84 |
85 | /**
86 | * The inverse simply means a Translation2d that "undoes" this object.
87 | *
88 | * @return Translation by -x and -y.
89 | */
90 | public Translation2d inverse() {
91 | return new Translation2d(-x_, -y_);
92 | }
93 |
94 | @Override
95 | public Translation2d interpolate(final Translation2d other, double x) {
96 | if (x <= 0) {
97 | return new Translation2d(this);
98 | } else if (x >= 1) {
99 | return new Translation2d(other);
100 | }
101 | return extrapolate(other, x);
102 | }
103 |
104 | public Translation2d extrapolate(final Translation2d other, double x) {
105 | return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_);
106 | }
107 |
108 | public Translation2d scale(double s) {
109 | return new Translation2d(x_ * s, y_ * s);
110 | }
111 |
112 | public boolean epsilonEquals(final Translation2d other, double epsilon) {
113 | return Util.epsilonEquals(x(), other.x(), epsilon) && Util.epsilonEquals(y(), other.y(), epsilon);
114 | }
115 |
116 | @Override
117 | public String toString() {
118 | final DecimalFormat format = new DecimalFormat("#0.000");
119 | return "(" + format.format(x_) + "," + format.format(y_) + ")";
120 | }
121 |
122 | @Override
123 | public String toCSV() {
124 | final DecimalFormat format = new DecimalFormat("#0.000");
125 | return format.format(x_) + "," + format.format(y_);
126 | }
127 |
128 | public static double dot(final Translation2d a, final Translation2d b) {
129 | return a.x_ * b.x_ + a.y_ * b.y_;
130 | }
131 |
132 | public static Rotation2d getAngle(final Translation2d a, final Translation2d b) {
133 | double cos_angle = dot(a, b) / (a.norm() * b.norm());
134 | if (Double.isNaN(cos_angle)) {
135 | return new Rotation2d();
136 | }
137 | return Rotation2d.fromRadians(Math.acos(Util.limit(cos_angle, 1.0)));
138 | }
139 |
140 | public static double cross(final Translation2d a, final Translation2d b) {
141 | return a.x_ * b.y_ - a.y_ * b.x_;
142 | }
143 |
144 | @Override
145 | public double distance(final Translation2d other) {
146 | return inverse().translateBy(other).norm();
147 | }
148 |
149 | @Override
150 | public boolean equals(final Object other) {
151 | if (!(other instanceof Translation2d)) {
152 | return false;
153 | }
154 |
155 | return distance((Translation2d) other) < Util.kEpsilon;
156 | }
157 |
158 | @Override
159 | public Translation2d getTranslation() {
160 | return this;
161 | }
162 | }
163 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/vision/GoalTracker.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.vision;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.lib.geometry.Pose2d;
5 |
6 | import java.util.ArrayList;
7 | import java.util.Comparator;
8 | import java.util.List;
9 |
10 | /**
11 | * This is used in the event that multiple goals are detected to judge all goals based on timestamp, stability, and
12 | * continuation of previous goals (i.e. if a goal was detected earlier and has changed locations). This allows the robot
13 | * to make consistent decisions about which goal to aim at and to smooth out jitter from vibration of the camera.
14 | *
15 | * @see GoalTrack
16 | */
17 | public class GoalTracker {
18 | /**
19 | * Track reports contain all of the relevant information about a given goal track.
20 | */
21 | public static class TrackReport {
22 | // Transform from the field frame to the vision target.
23 | public Pose2d field_to_target;
24 |
25 | // The timestamp of the latest time that the goal has been observed
26 | public double latest_timestamp;
27 |
28 | // The percentage of the goal tracking time during which this goal has
29 | // been observed (0 to 1)
30 | public double stability;
31 |
32 | // The track id
33 | public int id;
34 |
35 | public TrackReport(GoalTrack track) {
36 | this.field_to_target = track.getSmoothedPosition();
37 | this.latest_timestamp = track.getLatestTimestamp();
38 | this.stability = track.getStability();
39 | this.id = track.getId();
40 | }
41 | }
42 |
43 | /**
44 | * TrackReportComparators are used in the case that multiple tracks are active (e.g. we see or have recently seen
45 | * multiple goals). They contain heuristics used to pick which track we should aim at by calculating a score for
46 | * each track (highest score wins).
47 | */
48 | public static class TrackReportComparator implements Comparator {
49 | // Reward tracks for being more stable (seen in more frames)
50 | double mStabilityWeight;
51 | // Reward tracks for being recently observed
52 | double mAgeWeight;
53 | double mCurrentTimestamp;
54 | // Reward tracks for being continuations of tracks that we are already
55 | // tracking
56 | double mSwitchingWeight;
57 | int mLastTrackId;
58 |
59 | public TrackReportComparator(double stability_weight, double age_weight, double switching_weight,
60 | int last_track_id, double current_timestamp) {
61 | this.mStabilityWeight = stability_weight;
62 | this.mAgeWeight = age_weight;
63 | this.mSwitchingWeight = switching_weight;
64 | this.mLastTrackId = last_track_id;
65 | this.mCurrentTimestamp = current_timestamp;
66 | }
67 |
68 | double score(TrackReport report) {
69 | double stability_score = mStabilityWeight * report.stability;
70 | double age_score = mAgeWeight
71 | * Math.max(0, (Constants.kMaxGoalTrackAge - (mCurrentTimestamp - report.latest_timestamp))
72 | / Constants.kMaxGoalTrackAge);
73 | double switching_score = (report.id == mLastTrackId ? mSwitchingWeight : 0);
74 | return stability_score + age_score + switching_score;
75 | }
76 |
77 | @Override
78 | public int compare(TrackReport o1, TrackReport o2) {
79 | double diff = score(o1) - score(o2);
80 | // Greater than 0 if o1 is better than o2
81 | if (diff < 0) {
82 | return 1;
83 | } else if (diff > 0) {
84 | return -1;
85 | } else {
86 | return 0;
87 | }
88 | }
89 | }
90 |
91 | List mCurrentTracks = new ArrayList<>();
92 | int mNextId = 0;
93 |
94 | public GoalTracker() {}
95 |
96 | public synchronized void reset() {
97 | mCurrentTracks.clear();
98 | }
99 |
100 | public synchronized void update(double timestamp, List field_to_goals) {
101 | // Try to update existing tracks
102 | for (Pose2d target : field_to_goals) {
103 | boolean hasUpdatedTrack = false;
104 | for (GoalTrack track : mCurrentTracks) {
105 | if (!hasUpdatedTrack) {
106 | if (track.tryUpdate(timestamp, target)) {
107 | hasUpdatedTrack = true;
108 | }
109 | } else {
110 | track.emptyUpdate();
111 | }
112 | }
113 | if (!hasUpdatedTrack) {
114 | // Add a new track.
115 | // System.out.println("Created new track");
116 | mCurrentTracks.add(GoalTrack.makeNewTrack(timestamp, target, mNextId));
117 | ++mNextId;
118 | }
119 | }
120 |
121 | mCurrentTracks.removeIf(track -> !track.isAlive());
122 | }
123 |
124 | public synchronized boolean hasTracks() {
125 | return !mCurrentTracks.isEmpty();
126 | }
127 |
128 | public synchronized List getTracks() {
129 | List rv = new ArrayList<>();
130 | for (GoalTrack track : mCurrentTracks) {
131 | rv.add(new TrackReport(track));
132 | }
133 | return rv;
134 | }
135 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/LimelightManager.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.team254.frc2019.Constants;
4 | import com.team254.frc2019.GamePiece;
5 | import com.team254.frc2019.RobotState;
6 | import com.team254.frc2019.loops.ILooper;
7 | import com.team254.frc2019.loops.Loop;
8 |
9 | import java.util.List;
10 |
11 | /**
12 | * Class that manages using multiple Limelight 2's, one at a time
13 | *
14 | * @see Limelight
15 | */
16 | public class LimelightManager extends Subsystem {
17 | private static LimelightManager sInstance = null;
18 | private Limelight mTopLimelight;
19 | private Limelight mBottomLimelight;
20 | private Elevator mElevator;
21 | private List mAllLimelights;
22 |
23 | enum ActiveLimelight {
24 | TOP,
25 | BOTTOM,
26 | }
27 |
28 | private ActiveLimelight mActiveLimelight = ActiveLimelight.TOP;
29 |
30 | private LimelightManager() {
31 | mTopLimelight = new Limelight(Constants.kTopLimelightConstants);
32 | mBottomLimelight = new Limelight(Constants.kBottomLimelightConstants);
33 | mAllLimelights = List.of(mTopLimelight, mBottomLimelight);
34 | mElevator = Elevator.getInstance();
35 | }
36 |
37 | public static LimelightManager getInstance() {
38 | if (sInstance == null) {
39 | sInstance = new LimelightManager();
40 | }
41 | return sInstance;
42 | }
43 |
44 |
45 | @Override
46 | public void registerEnabledLoops(ILooper mEnabledLooper) {
47 | Loop mLoop = new Loop() {
48 | @Override
49 | public void onStart(double timestamp) {
50 | mAllLimelights.forEach(limelight -> limelight.setLed(Limelight.LedMode.OFF));
51 | RobotState.getInstance().resetVision();
52 | }
53 |
54 | @Override
55 | public void onLoop(double timestamp) {
56 | synchronized (LimelightManager.this) {
57 | Limelight limelight;
58 | if (mActiveLimelight == ActiveLimelight.TOP) {
59 | limelight = mTopLimelight;
60 | } else {
61 | limelight = mBottomLimelight;
62 | }
63 | if (mActiveLimelight == ActiveLimelight.TOP &&
64 | mElevator.getPosition() > Constants.kMaxTopLimelightHeight) {
65 | RobotState.getInstance().addVisionUpdate(
66 | timestamp - limelight.getLatency(),
67 | null, getActiveLimelightObject());
68 | } else {
69 | RobotState.getInstance().addVisionUpdate(
70 | timestamp - limelight.getLatency(),
71 | limelight.getTarget(), getActiveLimelightObject());
72 | }
73 | }
74 |
75 | }
76 |
77 | @Override
78 | public void onStop(double timestamp) {
79 | stop();
80 | }
81 | };
82 | mEnabledLooper.register(mLoop);
83 | }
84 |
85 | @Override
86 | public synchronized void readPeriodicInputs() {
87 | mAllLimelights.forEach(limelight -> limelight.readPeriodicInputs());
88 | }
89 |
90 | @Override
91 | public synchronized void writePeriodicOutputs() {
92 | mAllLimelights.forEach(limelight -> limelight.writePeriodicOutputs());
93 | }
94 |
95 | @Override
96 | public synchronized void stop() {
97 | mAllLimelights.forEach(limelight -> limelight.stop());
98 | }
99 |
100 | @Override
101 | public boolean checkSystem() {
102 | return true;
103 | }
104 |
105 | @Override
106 | public void outputTelemetry() {
107 | mAllLimelights.forEach(limelight -> limelight.outputTelemetry());
108 | }
109 |
110 | public synchronized ActiveLimelight getActiveLimelight() {
111 | return mActiveLimelight;
112 | }
113 |
114 | public synchronized void setUseTopLimelight(boolean useTop) {
115 | mActiveLimelight = useTop ? ActiveLimelight.TOP : ActiveLimelight.BOTTOM;
116 | getInactiveLimelightObject().setLed(Limelight.LedMode.OFF);
117 | getActiveLimelightObject().setLed(Limelight.LedMode.PIPELINE);
118 | }
119 |
120 | private synchronized Limelight getActiveLimelightObject() {
121 | if (mActiveLimelight == ActiveLimelight.TOP) {
122 | return mTopLimelight;
123 | } else {
124 | return mBottomLimelight;
125 | }
126 | }
127 |
128 | private synchronized Limelight getInactiveLimelightObject() {
129 | if (mActiveLimelight == ActiveLimelight.TOP) {
130 | return mBottomLimelight;
131 | } else {
132 | return mTopLimelight;
133 | }
134 | }
135 |
136 | public Limelight getTopLimelight() {
137 | return mTopLimelight;
138 | }
139 |
140 | public Limelight getBottomLimelight() {
141 | return mBottomLimelight;
142 | }
143 |
144 | public synchronized void setPipeline(int mode) {
145 | mAllLimelights.forEach(limelight -> limelight.setPipeline(mode));
146 | }
147 |
148 | public synchronized void triggerOutputs() {
149 | mAllLimelights.forEach(limelight -> limelight.triggerOutputs());
150 | }
151 |
152 | public synchronized void setAllLeds(Limelight.LedMode mode) {
153 | mAllLimelights.forEach(limelight -> limelight.setLed(mode));
154 | }
155 |
156 | public synchronized void updatePipeline(GamePiece gamePiece) {
157 | if (gamePiece == GamePiece.BALL) {
158 | setPipeline(Limelight.kSortTopPipeline);
159 | } else {
160 | setPipeline(Limelight.kDefaultPipeline);
161 | }
162 | }
163 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/motion/MotionProfileGoal.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.motion;
2 |
3 | import static com.team254.lib.util.Util.epsilonEquals;
4 |
5 | /**
6 | * A MotionProfileGoal defines a desired position and maximum velocity (at this position), along with the behavior that
7 | * should be used to determine if we are at the goal and what to do if it is infeasible to reach the goal within the
8 | * desired velocity bounds.
9 | */
10 | public class MotionProfileGoal {
11 | /**
12 | * A goal consists of a desired position and specified maximum velocity magnitude. But what should we do if we would
13 | * reach the goal at a velocity greater than the maximum? This enum allows a user to specify a preference on
14 | * behavior in this case.
15 | *
16 | * Example use-cases of each:
17 | *
18 | * OVERSHOOT - Generally used with a goal max_abs_vel of 0.0 to stop at the desired pos without violating any
19 | * constraints.
20 | *
21 | * VIOLATE_MAX_ACCEL - If we absolutely do not want to pass the goal and are unwilling to violate the max_abs_vel
22 | * (for example, there is an obstacle in front of us - slam the brakes harder than we'd like in order to avoid
23 | * hitting it).
24 | *
25 | * VIOLATE_MAX_ABS_VEL - If the max velocity is just a general guideline and not a hard performance limit, it's
26 | * better to slightly exceed it to avoid skidding wheels.
27 | */
28 | public enum CompletionBehavior {
29 | // Overshoot the goal if necessary (at a velocity greater than max_abs_vel) and come back.
30 | // Only valid if the goal velocity is 0.0 (otherwise VIOLATE_MAX_ACCEL will be used).
31 | OVERSHOOT,
32 | // If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the max accel
33 | // constraint.
34 | VIOLATE_MAX_ACCEL,
35 | // If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the goal velocity.
36 | VIOLATE_MAX_ABS_VEL
37 | }
38 |
39 | protected double pos;
40 | protected double max_abs_vel;
41 | protected CompletionBehavior completion_behavior = CompletionBehavior.OVERSHOOT;
42 | protected double pos_tolerance = 1E-3;
43 | protected double vel_tolerance = 1E-2;
44 |
45 | public MotionProfileGoal() {}
46 |
47 | public MotionProfileGoal(double pos) {
48 | this.pos = pos;
49 | this.max_abs_vel = 0.0;
50 | sanityCheck();
51 | }
52 |
53 | public MotionProfileGoal(double pos, double max_abs_vel) {
54 | this.pos = pos;
55 | this.max_abs_vel = max_abs_vel;
56 | sanityCheck();
57 | }
58 |
59 | public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior) {
60 | this.pos = pos;
61 | this.max_abs_vel = max_abs_vel;
62 | this.completion_behavior = completion_behavior;
63 | sanityCheck();
64 | }
65 |
66 | public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior,
67 | double pos_tolerance, double vel_tolerance) {
68 | this.pos = pos;
69 | this.max_abs_vel = max_abs_vel;
70 | this.completion_behavior = completion_behavior;
71 | this.pos_tolerance = pos_tolerance;
72 | this.vel_tolerance = vel_tolerance;
73 | sanityCheck();
74 | }
75 |
76 | public MotionProfileGoal(MotionProfileGoal other) {
77 | this(other.pos, other.max_abs_vel, other.completion_behavior, other.pos_tolerance, other.vel_tolerance);
78 | }
79 |
80 | /**
81 | * @return A flipped MotionProfileGoal (where the position is negated, but all other attributes remain the same).
82 | */
83 | public MotionProfileGoal flipped() {
84 | return new MotionProfileGoal(-pos, max_abs_vel, completion_behavior, pos_tolerance, vel_tolerance);
85 | }
86 |
87 | public double pos() {
88 | return pos;
89 | }
90 |
91 | public double max_abs_vel() {
92 | return max_abs_vel;
93 | }
94 |
95 | public double pos_tolerance() {
96 | return pos_tolerance;
97 | }
98 |
99 | public double vel_tolerance() {
100 | return vel_tolerance;
101 | }
102 |
103 | public CompletionBehavior completion_behavior() {
104 | return completion_behavior;
105 | }
106 |
107 | public boolean atGoalState(MotionState state) {
108 | return atGoalPos(state.pos()) && (Math.abs(state.vel()) < (max_abs_vel + vel_tolerance)
109 | || completion_behavior == CompletionBehavior.VIOLATE_MAX_ABS_VEL);
110 | }
111 |
112 | public boolean atGoalPos(double pos) {
113 | return epsilonEquals(pos, this.pos, pos_tolerance);
114 | }
115 |
116 | /**
117 | * This method makes sure that the completion behavior is compatible with the max goal velocity.
118 | */
119 | protected void sanityCheck() {
120 | if (max_abs_vel > vel_tolerance && completion_behavior == CompletionBehavior.OVERSHOOT) {
121 | completion_behavior = CompletionBehavior.VIOLATE_MAX_ACCEL;
122 | }
123 | }
124 |
125 | @Override
126 | public String toString() {
127 | return "pos: " + pos + " (+/- " + pos_tolerance + "), max_abs_vel: " + max_abs_vel + " (+/- " + vel_tolerance
128 | + "), completion behavior: " + completion_behavior.name();
129 | }
130 |
131 | @Override
132 | public boolean equals(Object obj) {
133 | if (!(obj instanceof MotionProfileGoal)) {
134 | return false;
135 | }
136 | final MotionProfileGoal other = (MotionProfileGoal) obj;
137 | return (other.completion_behavior() == completion_behavior()) && (other.pos() == pos())
138 | && (other.max_abs_vel() == max_abs_vel()) && (other.pos_tolerance() == pos_tolerance())
139 | && (other.vel_tolerance() == vel_tolerance());
140 | }
141 | }
142 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/Kinematics.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019;
2 |
3 | import com.team254.frc2019.subsystems.Drive;
4 | import com.team254.lib.geometry.Pose2d;
5 | import com.team254.lib.geometry.Rotation2d;
6 | import com.team254.lib.geometry.Twist2d;
7 | import com.team254.lib.util.DriveSignal;
8 |
9 | /**
10 | * Provides forward and inverse kinematics equations for the robot modeling the
11 | * wheelbase as a swerve drive.
12 | *
13 | * Equations extrapolated through papers by Ether, then corrected for our frame of reference:
14 | * https://www.chiefdelphi.com/t/paper-4-wheel-independent-drive-independent-steering-swerve/107383
15 | */
16 |
17 | public class Kinematics {
18 | private static final double L = Constants.kDriveWheelbase;
19 | private static final double W = Constants.kDriveTrackwidth;
20 | private static final double R = Math.hypot(L, W);
21 |
22 | /**
23 | * Forward kinematics using only encoders
24 | */
25 | public static Twist2d forwardKinematics(DriveSignal drive_signal) {
26 | return forwardKinematics(drive_signal.getWheelSpeeds(), drive_signal.getWheelAzimuths());
27 | }
28 |
29 | /**
30 | * @param wheel_speeds
31 | * @param wheel_azimuths
32 | * @return Twist2d representing forward, strafe, and angular velocities in real world units
33 | */
34 | public static Twist2d forwardKinematics(double[] wheel_speeds, Rotation2d[] wheel_azimuths) {
35 | double[] vx = new double[4]; // wheel velocities in the x (forward) direction
36 | double[] vy = new double[4]; // wheel velocities in the y (strafe) direction
37 |
38 | for (int i = 0; i < vx.length; i++) {
39 | vx[i] = wheel_azimuths[i].cos() * wheel_speeds[i];
40 | vy[i] = wheel_azimuths[i].sin() * wheel_speeds[i];
41 | }
42 |
43 | // average possible solutions to minimize error
44 | double A = (vy[2] + vy[3]) / 2;
45 | double B = (vy[0] + vy[1]) / 2;
46 | double C = (vx[0] + vx[3]) / 2;
47 | double D = (vx[1] + vx[2]) / 2;
48 |
49 | // average possible solutions to minimize error
50 | double forward = (C + D) / 2;
51 | double strafe = (A + B) / 2;
52 | double rotation = (((strafe - A) * R / L) + ((B - strafe) * R / L) + ((forward - C) * R / W)
53 | + ((D - forward) * R / W)) / 4;
54 |
55 | return new Twist2d(forward, strafe, rotation);
56 | }
57 |
58 | /**
59 | * Use Gyro for dtheta
60 | */
61 | public static Twist2d forwardKinematics(DriveSignal drive_signal, Rotation2d prev_heading,
62 | Rotation2d current_heading, double dt) {
63 | Twist2d ret_val = forwardKinematics(drive_signal);
64 | return new Twist2d(ret_val.dx, ret_val.dy, prev_heading.inverse().rotateBy(current_heading).getRadians() / dt);
65 | }
66 |
67 | public static Twist2d forwardKinematics(double[] wheel_speeds, Rotation2d[] wheel_azimuths, Rotation2d prev_heading,
68 | Rotation2d current_heading, double dt) {
69 | Twist2d ret_val = forwardKinematics(wheel_speeds, wheel_azimuths);
70 | return new Twist2d(ret_val.dx, ret_val.dy, prev_heading.inverse().rotateBy(current_heading).getRadians() / dt);
71 | }
72 |
73 | /**
74 | * For convenience, integrate forward kinematics with a Twist2d and previous
75 | * rotation.
76 | */
77 | public static Pose2d integrateForwardKinematics(Pose2d current_pose, Twist2d forward_kinematics) {
78 | return current_pose.transformBy(new Pose2d(forward_kinematics.dx, forward_kinematics.dy,
79 | Rotation2d.fromRadians(forward_kinematics.dtheta)));
80 | }
81 |
82 | public static DriveSignal inverseKinematics(double forward, double strafe, double rotation,
83 | boolean field_relative) {
84 | return inverseKinematics(forward, strafe, rotation, field_relative, true);
85 | }
86 |
87 | public static DriveSignal inverseKinematics(double forward, double strafe, double rotation, boolean field_relative,
88 | boolean normalize_outputs) {
89 | if (field_relative) {
90 | Rotation2d gyroHeading = Drive.getInstance().getHeading();
91 | double temp = forward * gyroHeading.cos() + strafe * gyroHeading.sin();
92 | strafe = -forward * gyroHeading.sin() + strafe * gyroHeading.cos();
93 | forward = temp;
94 | }
95 |
96 | double A = strafe - rotation * L / R;
97 | double B = strafe + rotation * L / R;
98 | double C = forward - rotation * W / R;
99 | double D = forward + rotation * W / R;
100 |
101 | double[] wheel_speeds = new double[4];
102 | wheel_speeds[0] = Math.hypot(B, C);
103 | wheel_speeds[1] = Math.hypot(B, D);
104 | wheel_speeds[2] = Math.hypot(A, D);
105 | wheel_speeds[3] = Math.hypot(A, C);
106 |
107 | // normalize wheel speeds if above 1
108 | if (normalize_outputs) {
109 | double max_speed = 1;
110 | for (int i = 0; i < wheel_speeds.length; i++) {
111 | if (Math.abs(wheel_speeds[i]) > max_speed) {
112 | max_speed = Math.abs(wheel_speeds[i]);
113 | }
114 | }
115 |
116 | for (var i = 0; i < wheel_speeds.length; i++) {
117 | wheel_speeds[i] /= max_speed;
118 | }
119 | }
120 |
121 | Rotation2d[] wheel_azimuths = new Rotation2d[4];
122 | wheel_azimuths[0] = Rotation2d.fromRadians(Math.atan2(B, C));
123 | wheel_azimuths[1] = Rotation2d.fromRadians(Math.atan2(B, D));
124 | wheel_azimuths[2] = Rotation2d.fromRadians(Math.atan2(A, D));
125 | wheel_azimuths[3] = Rotation2d.fromRadians(Math.atan2(A, C));
126 |
127 | return new DriveSignal(wheel_speeds, wheel_azimuths);
128 | }
129 | }
--------------------------------------------------------------------------------
/src/main/java/com/team254/lib/motion/MotionState.java:
--------------------------------------------------------------------------------
1 | package com.team254.lib.motion;
2 |
3 | import static com.team254.lib.motion.MotionUtil.kEpsilon;
4 | import static com.team254.lib.util.Util.epsilonEquals;
5 |
6 | /**
7 | * A MotionState is a completely specified state of 1D motion through time.
8 | */
9 | public class MotionState {
10 | protected final double t;
11 | protected final double pos;
12 | protected final double vel;
13 | protected final double acc;
14 |
15 | public static MotionState kInvalidState = new MotionState(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
16 |
17 | public MotionState(double t, double pos, double vel, double acc) {
18 | this.t = t;
19 | this.pos = pos;
20 | this.vel = vel;
21 | this.acc = acc;
22 | }
23 |
24 | public MotionState(MotionState state) {
25 | this(state.t, state.pos, state.vel, state.acc);
26 | }
27 |
28 | public double t() {
29 | return t;
30 | }
31 |
32 | public double pos() {
33 | return pos;
34 | }
35 |
36 | public double vel() {
37 | return vel;
38 | }
39 |
40 | public double vel2() {
41 | return vel * vel;
42 | }
43 |
44 | public double acc() {
45 | return acc;
46 | }
47 |
48 | /**
49 | * Extrapolates this MotionState to the specified time by applying this MotionState's acceleration.
50 | *
51 | * @param t The time of the new MotionState.
52 | * @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this state.
53 | */
54 | public MotionState extrapolate(double t) {
55 | return extrapolate(t, acc);
56 | }
57 |
58 | /**
59 | * Extrapolates this MotionState to the specified time by applying a given acceleration to the (t, pos, vel) portion
60 | * of this MotionState.
61 | *
62 | * @param t The time of the new MotionState.
63 | * @param acc The acceleration to apply.
64 | * @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this state (with the
65 | * specified accel).
66 | */
67 | public MotionState extrapolate(double t, double acc) {
68 | final double dt = t - this.t;
69 | return new MotionState(t, pos + vel * dt + .5 * acc * dt * dt, vel + acc * dt, acc);
70 | }
71 |
72 | /**
73 | * Find the next time (first time > MotionState.t()) that this MotionState will be at pos. This is an inverse of the
74 | * extrapolate() method.
75 | *
76 | * @param pos The position to query.
77 | * @return The time when we are next at pos() if we are extrapolating with a positive dt. NaN if we never reach pos.
78 | */
79 | public double nextTimeAtPos(double pos) {
80 | if (epsilonEquals(pos, this.pos, kEpsilon)) {
81 | // Already at pos.
82 | return t;
83 | }
84 | if (epsilonEquals(acc, 0.0, kEpsilon)) {
85 | // Zero acceleration case.
86 | final double delta_pos = pos - this.pos;
87 | if (!epsilonEquals(vel, 0.0, kEpsilon) && Math.signum(delta_pos) == Math.signum(vel)) {
88 | // Constant velocity heading towards pos.
89 | return delta_pos / vel + t;
90 | }
91 | return Double.NaN;
92 | }
93 |
94 | // Solve the quadratic formula.
95 | // ax^2 + bx + c == 0
96 | // x = dt
97 | // a = .5 * acc
98 | // b = vel
99 | // c = this.pos - pos
100 | final double disc = vel * vel - 2.0 * acc * (this.pos - pos);
101 | if (disc < 0.0) {
102 | // Extrapolating this MotionState never reaches the desired pos.
103 | return Double.NaN;
104 | }
105 | final double sqrt_disc = Math.sqrt(disc);
106 | final double max_dt = (-vel + sqrt_disc) / acc;
107 | final double min_dt = (-vel - sqrt_disc) / acc;
108 | if (min_dt >= 0.0 && (max_dt < 0.0 || min_dt < max_dt)) {
109 | return t + min_dt;
110 | }
111 | if (max_dt >= 0.0) {
112 | return t + max_dt;
113 | }
114 | // We only reach the desired pos in the past.
115 | return Double.NaN;
116 | }
117 |
118 | @Override
119 | public String toString() {
120 | return "(t=" + t + ", pos=" + pos + ", vel=" + vel + ", acc=" + acc + ")";
121 | }
122 |
123 | /**
124 | * Checks if two MotionStates are epsilon-equals (all fields are equal within a nominal tolerance).
125 | */
126 | @Override
127 | public boolean equals(Object other) {
128 | return (other instanceof MotionState) && equals((MotionState) other, kEpsilon);
129 | }
130 |
131 | /**
132 | * Checks if two MotionStates are epsilon-equals (all fields are equal within a specified tolerance).
133 | */
134 | public boolean equals(MotionState other, double epsilon) {
135 | return coincident(other, epsilon) && epsilonEquals(acc, other.acc, epsilon);
136 | }
137 |
138 | /**
139 | * Checks if two MotionStates are coincident (t, pos, and vel are equal within a nominal tolerance, but acceleration
140 | * may be different).
141 | */
142 | public boolean coincident(MotionState other) {
143 | return coincident(other, kEpsilon);
144 | }
145 |
146 | /**
147 | * Checks if two MotionStates are coincident (t, pos, and vel are equal within a specified tolerance, but
148 | * acceleration may be different).
149 | */
150 | public boolean coincident(MotionState other, double epsilon) {
151 | return epsilonEquals(t, other.t, epsilon) && epsilonEquals(pos, other.pos, epsilon)
152 | && epsilonEquals(vel, other.vel, epsilon);
153 | }
154 |
155 | /**
156 | * Returns a MotionState that is the mirror image of this one. Pos, vel, and acc are all negated, but time is not.
157 | */
158 | public MotionState flipped() {
159 | return new MotionState(t, -pos, -vel, -acc);
160 | }
161 | }
162 |
--------------------------------------------------------------------------------
/gradlew:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env sh
2 |
3 | ##############################################################################
4 | ##
5 | ## Gradle start up script for UN*X
6 | ##
7 | ##############################################################################
8 |
9 | # Attempt to set APP_HOME
10 | # Resolve links: $0 may be a link
11 | PRG="$0"
12 | # Need this for relative symlinks.
13 | while [ -h "$PRG" ] ; do
14 | ls=`ls -ld "$PRG"`
15 | link=`expr "$ls" : '.*-> \(.*\)$'`
16 | if expr "$link" : '/.*' > /dev/null; then
17 | PRG="$link"
18 | else
19 | PRG=`dirname "$PRG"`"/$link"
20 | fi
21 | done
22 | SAVED="`pwd`"
23 | cd "`dirname \"$PRG\"`/" >/dev/null
24 | APP_HOME="`pwd -P`"
25 | cd "$SAVED" >/dev/null
26 |
27 | APP_NAME="Gradle"
28 | APP_BASE_NAME=`basename "$0"`
29 |
30 | # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
31 | DEFAULT_JVM_OPTS='"-Xmx64m"'
32 |
33 | # Use the maximum available, or set MAX_FD != -1 to use that value.
34 | MAX_FD="maximum"
35 |
36 | warn () {
37 | echo "$*"
38 | }
39 |
40 | die () {
41 | echo
42 | echo "$*"
43 | echo
44 | exit 1
45 | }
46 |
47 | # OS specific support (must be 'true' or 'false').
48 | cygwin=false
49 | msys=false
50 | darwin=false
51 | nonstop=false
52 | case "`uname`" in
53 | CYGWIN* )
54 | cygwin=true
55 | ;;
56 | Darwin* )
57 | darwin=true
58 | ;;
59 | MINGW* )
60 | msys=true
61 | ;;
62 | NONSTOP* )
63 | nonstop=true
64 | ;;
65 | esac
66 |
67 | CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
68 |
69 | # Determine the Java command to use to start the JVM.
70 | if [ -n "$JAVA_HOME" ] ; then
71 | if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
72 | # IBM's JDK on AIX uses strange locations for the executables
73 | JAVACMD="$JAVA_HOME/jre/sh/java"
74 | else
75 | JAVACMD="$JAVA_HOME/bin/java"
76 | fi
77 | if [ ! -x "$JAVACMD" ] ; then
78 | die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
79 |
80 | Please set the JAVA_HOME variable in your environment to match the
81 | location of your Java installation."
82 | fi
83 | else
84 | JAVACMD="java"
85 | which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
86 |
87 | Please set the JAVA_HOME variable in your environment to match the
88 | location of your Java installation."
89 | fi
90 |
91 | # Increase the maximum file descriptors if we can.
92 | if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then
93 | MAX_FD_LIMIT=`ulimit -H -n`
94 | if [ $? -eq 0 ] ; then
95 | if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
96 | MAX_FD="$MAX_FD_LIMIT"
97 | fi
98 | ulimit -n $MAX_FD
99 | if [ $? -ne 0 ] ; then
100 | warn "Could not set maximum file descriptor limit: $MAX_FD"
101 | fi
102 | else
103 | warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
104 | fi
105 | fi
106 |
107 | # For Darwin, add options to specify how the application appears in the dock
108 | if $darwin; then
109 | GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
110 | fi
111 |
112 | # For Cygwin, switch paths to Windows format before running java
113 | if $cygwin ; then
114 | APP_HOME=`cygpath --path --mixed "$APP_HOME"`
115 | CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
116 | JAVACMD=`cygpath --unix "$JAVACMD"`
117 |
118 | # We build the pattern for arguments to be converted via cygpath
119 | ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
120 | SEP=""
121 | for dir in $ROOTDIRSRAW ; do
122 | ROOTDIRS="$ROOTDIRS$SEP$dir"
123 | SEP="|"
124 | done
125 | OURCYGPATTERN="(^($ROOTDIRS))"
126 | # Add a user-defined pattern to the cygpath arguments
127 | if [ "$GRADLE_CYGPATTERN" != "" ] ; then
128 | OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
129 | fi
130 | # Now convert the arguments - kludge to limit ourselves to /bin/sh
131 | i=0
132 | for arg in "$@" ; do
133 | CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
134 | CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option
135 |
136 | if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition
137 | eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
138 | else
139 | eval `echo args$i`="\"$arg\""
140 | fi
141 | i=$((i+1))
142 | done
143 | case $i in
144 | (0) set -- ;;
145 | (1) set -- "$args0" ;;
146 | (2) set -- "$args0" "$args1" ;;
147 | (3) set -- "$args0" "$args1" "$args2" ;;
148 | (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
149 | (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
150 | (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
151 | (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
152 | (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
153 | (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
154 | esac
155 | fi
156 |
157 | # Escape application args
158 | save () {
159 | for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
160 | echo " "
161 | }
162 | APP_ARGS=$(save "$@")
163 |
164 | # Collect all arguments for the java command, following the shell quoting and substitution rules
165 | eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
166 |
167 | # by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong
168 | if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then
169 | cd "$(dirname "$0")"
170 | fi
171 |
172 | exec "$JAVACMD" "$@"
173 |
--------------------------------------------------------------------------------
/src/main/java/com/team254/frc2019/subsystems/Vacuum.java:
--------------------------------------------------------------------------------
1 | package com.team254.frc2019.subsystems;
2 |
3 | import com.ctre.phoenix.motorcontrol.ControlMode;
4 | import com.ctre.phoenix.motorcontrol.can.TalonSRX;
5 | import com.team254.frc2019.Constants;
6 | import com.team254.frc2019.loops.ILooper;
7 | import com.team254.frc2019.loops.Loop;
8 | import com.team254.lib.drivers.TalonSRXFactory;
9 | import com.team254.lib.util.ReflectingCSVWriter;
10 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
11 |
12 | /**
13 | * Controls a 775pro which drives a GM UP28 vacuum pump
14 | *
15 | * Feedback on the pressure build up is given through a ProSense QPS digital
16 | * pressure switch/transmitter, set on hysteresis mode
17 | */
18 | public class Vacuum extends Subsystem {
19 | private static Vacuum mInstance;
20 |
21 | public synchronized static Vacuum getInstance() {
22 | if (mInstance == null) {
23 | mInstance = new Vacuum();
24 | }
25 |
26 | return mInstance;
27 | }
28 |
29 | private final TalonSRX mMaster;
30 | private boolean mOn = false;
31 |
32 | private CarriageCanifier mCanifier = CarriageCanifier.getInstance();
33 |
34 | enum ThresholdState {
35 | ABOVE, BETWEEN, BELOW
36 | }
37 |
38 | public static class PeriodicIO {
39 | // INPUTS
40 | public boolean lowPressureSensor;
41 | public boolean highPressureSensor;
42 | public ThresholdState thresholdState = ThresholdState.BELOW;
43 | }
44 |
45 | private PeriodicIO mPeriodicIO = new PeriodicIO();
46 | private ReflectingCSVWriter mCSVWriter = null;
47 |
48 | private final double kFullSetpoint = -1.0;
49 | private final double kSteadySetpoint = -0.8;
50 |
51 | private Vacuum() {
52 | mMaster = TalonSRXFactory.createDefaultTalon(Constants.kVacuumMasterId);
53 | mMaster.overrideSoftLimitsEnable(false);
54 | mMaster.configPeakCurrentLimit(30);
55 | }
56 |
57 | @Override
58 | public void registerEnabledLoops(ILooper mEnabledLooper) {
59 | mEnabledLooper.register(new Loop() {
60 | @Override
61 | public void onStart(double timestamp) {
62 | if (mCSVWriter == null) {
63 | // mCSVWriter = new ReflectingCSVWriter<>("/home/lvuser/VACUUM-LOGS.csv", PeriodicIO.class);
64 | }
65 | }
66 |
67 | @Override
68 | public void onLoop(double timestamp) {}
69 |
70 | @Override
71 | public void onStop(double timestamp) {
72 | if (mCSVWriter != null) {
73 | mCSVWriter.flush();
74 | mCSVWriter = null;
75 | }
76 |
77 | stop();
78 | }
79 | });
80 | }
81 |
82 | public synchronized void setOn(boolean on) {
83 | mOn = on;
84 | }
85 |
86 | /**
87 | * @return whether we are actually at climbing pressure to auto climb
88 | */
89 | public synchronized boolean isAtClimbingPressure() {
90 | return mPeriodicIO.thresholdState == ThresholdState.ABOVE;
91 | }
92 |
93 | /**
94 | * @return whether we are within the climable range to allow drivers to trigger climb
95 | */
96 | public synchronized boolean isAlmostAtClimbingPressure() {
97 | return mPeriodicIO.thresholdState == ThresholdState.BETWEEN
98 | || mPeriodicIO.thresholdState == ThresholdState.ABOVE;
99 | }
100 |
101 | private boolean getLowerPressureSensor() {
102 | return !mCanifier.getLowPressureSensor(); // NPN sensor
103 | }
104 |
105 | private boolean getHigherPressureSensor() {
106 | return !mCanifier.getHighPressureSensor(); // NPN sensor
107 | }
108 |
109 | @Override
110 | public synchronized void readPeriodicInputs() {
111 | mPeriodicIO.lowPressureSensor = getLowerPressureSensor();
112 | mPeriodicIO.highPressureSensor = getHigherPressureSensor();
113 |
114 | if (mPeriodicIO.highPressureSensor) {
115 | mPeriodicIO.thresholdState = ThresholdState.ABOVE;
116 | } else if (mPeriodicIO.lowPressureSensor && !mPeriodicIO.highPressureSensor) {
117 | mPeriodicIO.thresholdState = ThresholdState.BETWEEN;
118 | } else {
119 | mPeriodicIO.thresholdState = ThresholdState.BELOW;
120 | }
121 |
122 | if (mCSVWriter != null) {
123 | mCSVWriter.add(mPeriodicIO);
124 | }
125 | }
126 |
127 | @Override
128 | public synchronized void writePeriodicOutputs() {
129 | double output = 0.0;
130 | if (mOn) {
131 | if (mPeriodicIO.thresholdState == ThresholdState.BELOW || mPeriodicIO.thresholdState == ThresholdState.BETWEEN) {
132 | output = kFullSetpoint;
133 | } else { // ABOVE
134 | output = kSteadySetpoint;
135 | }
136 | }
137 | mMaster.set(ControlMode.PercentOutput, output);
138 | }
139 |
140 | @Override
141 | public synchronized void stop() {
142 | mOn = false;
143 |
144 | // Make sure the vacuum is off
145 | mMaster.set(ControlMode.PercentOutput, 0.0);
146 | }
147 |
148 | @Override
149 | public boolean checkSystem() {
150 | return true;
151 | }
152 |
153 | @Override
154 | public void outputTelemetry() {
155 | SmartDashboard.putBoolean("Vacuum: Low sensor", getLowerPressureSensor());
156 | SmartDashboard.putBoolean("Vacuum: High sensor", getHigherPressureSensor());
157 |
158 | String state = "default";
159 | if (mPeriodicIO.thresholdState == ThresholdState.ABOVE) {
160 | state = "Above";
161 | } else if (mPeriodicIO.thresholdState == ThresholdState.BETWEEN) {
162 | state = "Between";
163 | } else if (mPeriodicIO.thresholdState == ThresholdState.BELOW) {
164 | state = "Below";
165 | }
166 | SmartDashboard.putString("Vacuum: Threshold state", state);
167 |
168 | if (mCSVWriter != null) {
169 | mCSVWriter.write();
170 | }
171 | }
172 |
173 | public ThresholdState getVacuumState() {
174 | return mPeriodicIO.thresholdState;
175 | }
176 | }
177 |
--------------------------------------------------------------------------------