From f827314e09b2e5880b2c51f7cb7ab886ef76716d Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Mon, 13 Jun 2022 20:10:18 -0600
Subject: [PATCH] Did a few things
---
.idea/compiler.xml | 6 +
.idea/encodings.xml | 6 +
.idea/gradle.xml | 17 +
.idea/jarRepositories.xml | 50 ++
.idea/misc.xml | 5 +
.idea/uiDesigner.xml | 124 +++++
.idea/vcs.xml | 6 +
.idea/workspace.xml | 172 ++++++
simgui.json | 43 ++
src/main/java/frc4388/robot/Robot.java | 19 +-
.../java/frc4388/robot/RobotContainer.java | 18 +-
src/main/java/frc4388/robot/RobotMap.java | 3 +-
.../frc4388/robot/subsystems/SwerveDrive.java | 6 +-
.../frc4388/robot/subsystems/TestMotor.java | 59 ++-
.../java/frc4388/utility/DesmosServer.java | 2 +-
src/main/java/frc4388/utility/Gains.java | 2 +-
src/main/java/frc4388/utility/RobotGyro.java | 490 +++++++++---------
.../frc4388/utility/tuner/CANBusReader.java | 97 ++++
.../utility/tuner/TunerCANDeviceFactory.java | 28 +
.../utility/tuner/TunerController.java | 168 ++++++
.../frc4388/utility/tuner/TunerGains.java | 33 ++
.../frc4388/utility/tuner/TunerLogger.java | 49 ++
.../utility/tuner/TunerTablesHandler.java | 114 ++++
.../utility/tuner/annotations/Controller.java | 14 +
.../utility/tuner/annotations/GainsField.java | 13 +
.../tuner/annotations/GainsSetter.java | 13 +
.../utility/tuner/annotations/Loggable.java | 14 +
.../utility/tuner/annotations/Reader.java | 14 +
.../tuner/tunercandevices/TCAN_SparkMax.java | 43 ++
.../tuner/tunercandevices/TCAN_TalonFX.java | 44 ++
.../tuner/tunercandevices/TCAN_TalonSRX.java | 44 ++
.../tuner/tunercandevices/TunerCANDevice.java | 40 ++
32 files changed, 1471 insertions(+), 285 deletions(-)
create mode 100644 .idea/compiler.xml
create mode 100644 .idea/encodings.xml
create mode 100644 .idea/gradle.xml
create mode 100644 .idea/jarRepositories.xml
create mode 100644 .idea/misc.xml
create mode 100644 .idea/uiDesigner.xml
create mode 100644 .idea/vcs.xml
create mode 100644 .idea/workspace.xml
create mode 100644 src/main/java/frc4388/utility/tuner/CANBusReader.java
create mode 100644 src/main/java/frc4388/utility/tuner/TunerCANDeviceFactory.java
create mode 100644 src/main/java/frc4388/utility/tuner/TunerController.java
create mode 100644 src/main/java/frc4388/utility/tuner/TunerGains.java
create mode 100644 src/main/java/frc4388/utility/tuner/TunerLogger.java
create mode 100644 src/main/java/frc4388/utility/tuner/TunerTablesHandler.java
create mode 100644 src/main/java/frc4388/utility/tuner/annotations/Controller.java
create mode 100644 src/main/java/frc4388/utility/tuner/annotations/GainsField.java
create mode 100644 src/main/java/frc4388/utility/tuner/annotations/GainsSetter.java
create mode 100644 src/main/java/frc4388/utility/tuner/annotations/Loggable.java
create mode 100644 src/main/java/frc4388/utility/tuner/annotations/Reader.java
create mode 100644 src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_SparkMax.java
create mode 100644 src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonFX.java
create mode 100644 src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonSRX.java
create mode 100644 src/main/java/frc4388/utility/tuner/tunercandevices/TunerCANDevice.java
diff --git a/.idea/compiler.xml b/.idea/compiler.xml
new file mode 100644
index 0000000..fb7f4a8
--- /dev/null
+++ b/.idea/compiler.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/encodings.xml b/.idea/encodings.xml
new file mode 100644
index 0000000..d079ef8
--- /dev/null
+++ b/.idea/encodings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/gradle.xml b/.idea/gradle.xml
new file mode 100644
index 0000000..611e7c8
--- /dev/null
+++ b/.idea/gradle.xml
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
new file mode 100644
index 0000000..0254f84
--- /dev/null
+++ b/.idea/jarRepositories.xml
@@ -0,0 +1,50 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..cbfe0de
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,5 @@
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/uiDesigner.xml b/.idea/uiDesigner.xml
new file mode 100644
index 0000000..e96534f
--- /dev/null
+++ b/.idea/uiDesigner.xml
@@ -0,0 +1,124 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..94a25f7
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/workspace.xml b/.idea/workspace.xml
new file mode 100644
index 0000000..eee052d
--- /dev/null
+++ b/.idea/workspace.xml
@@ -0,0 +1,172 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ {
+ "keyToString": {
+ "RunOnceActivity.OpenProjectViewOnStart": "true",
+ "RunOnceActivity.ShowReadmeOnStart": "true",
+ "last_opened_file_path": "C:/Users/Owner/Downloads/FRC-2.1.0-2021.3-signed.zip",
+ "project.structure.last.edited": "Project",
+ "project.structure.proportion": "0.0",
+ "project.structure.side.proportion": "0.2",
+ "settings.editor.selected.configurable": "reference.settingsdialog.IDE.editor.colors.Java"
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ true
+ false
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ true
+ false
+
+
+
+
+
+
+
+
+
+
+
+ 1652517563662
+
+
+ 1652517563662
+
+
+
+
+
+
+
+ file://$PROJECT_DIR$/src/main/java/frc4388/utility/tuner/TunerTablesHandler.java
+ 55
+
+
+
+
+
+
\ No newline at end of file
diff --git a/simgui.json b/simgui.json
index 0c4f056..78cbf94 100644
--- a/simgui.json
+++ b/simgui.json
@@ -1,15 +1,58 @@
{
+ "HALProvider": {
+ "Other Devices": {
+ "SPARK MAX [6]": {
+ "header": {
+ "open": true
+ }
+ }
+ }
+ },
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/LiveWindow/LED": "Subsystem",
"/LiveWindow/TestMotor": "Subsystem",
"/LiveWindow/Ungrouped/Scheduler": "Scheduler"
+ },
+ "windows": {
+ "/FMSInfo": {
+ "window": {
+ "visible": true
+ }
+ }
}
},
"NetworkTables": {
"SmartDashboard": {
"open": true
+ },
+ "TunerTables": {
+ "LoggerTable": {
+ "open": true
+ },
+ "TestMotor": {
+ "ControllerTable": {
+ "open": true
+ },
+ "GainsTable": {
+ "open": true
+ },
+ "ReaderTable": {
+ "open": true
+ },
+ "open": true
+ },
+ "open": true
+ },
+ "tunertable": {
+ "TestMotor": {
+ "controllertable": {
+ "open": true
+ },
+ "open": true
+ },
+ "open": true
}
}
}
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index aaef078..b7c87f4 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.DesmosServer;
import frc4388.utility.RobotTime;
+import frc4388.utility.tuner.CANBusReader;
+import frc4388.utility.tuner.TunerTablesHandler;
/**
* The VM is configured to automatically run this class, and to call the
@@ -19,7 +21,9 @@ import frc4388.utility.RobotTime;
*/
public class Robot extends TimedRobot {
Command m_autonomousCommand;
-
+
+ private TunerTablesHandler m_tunerTablesHandler = TunerTablesHandler.getInstance();
+
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
@@ -34,11 +38,13 @@ public class Robot extends TimedRobot {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
-
- desmosServer = new DesmosServer(8000);
- desmosServer.start();
- DesmosServer.putTable("table", "x1", new double[] {1, 2, 3.1, 3.9}, "y1", new double[] {1, 2, 2.9, 4.1});
+ CANBusReader.readCANBus();
+
+// desmosServer = new DesmosServer(8000);
+// desmosServer.start();
+
+// DesmosServer.putTable("table", "x1", new double[] {1, 2, 3.1, 3.9}, "y1", new double[] {1, 2, 2.9, 4.1});
}
/**
@@ -51,6 +57,9 @@ public class Robot extends TimedRobot {
*/
@Override
public void robotPeriodic() {
+ // * Necessary for communication with Tuner application
+ m_tunerTablesHandler.updateReaders();
+
m_robotTime.updateTimes();
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index f76d3aa..0dc9827 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -29,18 +29,6 @@ public class RobotContainer {
/* RobotMap */
private final RobotMap m_robotMap = new RobotMap();
- /* Subsystems */
- // private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
- // m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
- // m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
- // m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
- // m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
- // m_robotMap.leftFrontEncoder,
- // m_robotMap.rightFrontEncoder,
- // m_robotMap.leftBackEncoder,
- // m_robotMap.rightBackEncoder
- // );
-
private final TestMotor m_testMotor = new TestMotor(m_robotMap.testMotor);
private final LED m_robotLED = new LED(m_robotMap.LEDController);
@@ -55,11 +43,7 @@ public class RobotContainer {
configureButtonBindings();
/* Default Commands */
- // // drives the swerve drive with a two-axis input from the driver controller
- // m_robotSwerveDrive.setDefaultCommand(
- // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
- // getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
- m_testMotor.setDefaultCommand(new RunCommand(() -> m_testMotor.testDesmos(), m_testMotor));
+// m_testMotor.setDefaultCommand(new RunCommand(() -> m_testMotor.testDesmos(), m_testMotor));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index c09d5ec..a9c827a 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -5,6 +5,7 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.sensors.CANCoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
@@ -94,6 +95,6 @@ public class RobotMap {
//rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
- public final CANSparkMax testMotor = new CANSparkMax(6, MotorType.kBrushless);
+ public final WPI_TalonSRX testMotor = new WPI_TalonSRX(6);
}
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 396e8e6..8a73f78 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
-import frc4388.utility.RobotGyro;
+//import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase {
SwerveDriveKinematics m_kinematics;
@@ -59,7 +59,7 @@ public class SwerveDrive extends SubsystemBase {
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
- public RobotGyro gyro; //TODO Add Gyro Lol
+// public RobotGyro gyro; //TODO Add Gyro Lol
// public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
@@ -107,7 +107,7 @@ public class SwerveDrive extends SubsystemBase {
SwerveModuleState[] states =
kinematics.toSwerveModuleStates(
fieldRelative
- ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d())
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, new Rotation2d())//, gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < states.length; i++) {
diff --git a/src/main/java/frc4388/robot/subsystems/TestMotor.java b/src/main/java/frc4388/robot/subsystems/TestMotor.java
index 46e073c..bc6e78f 100644
--- a/src/main/java/frc4388/robot/subsystems/TestMotor.java
+++ b/src/main/java/frc4388/robot/subsystems/TestMotor.java
@@ -1,23 +1,58 @@
package frc4388.robot.subsystems;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.RelativeEncoder;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.utility.DesmosServer;
+import frc4388.utility.Gains;
+import frc4388.utility.tuner.TunerLogger;
+import frc4388.utility.tuner.annotations.Controller;
+import frc4388.utility.tuner.annotations.GainsField;
+import frc4388.utility.tuner.annotations.Loggable;
+import frc4388.utility.tuner.annotations.Reader;
+import frc4388.utility.tuner.TunerController;
+import frc4388.utility.tuner.TunerTablesHandler;
+
+import java.util.Map;
public class TestMotor extends SubsystemBase {
- private CANSparkMax m_testMotor;
- private RelativeEncoder m_testEncoder;
+ private final WPI_TalonSRX m_testMotor;
- public TestMotor(CANSparkMax testMotor) {
+ @GainsField(id="TestMotor")
+ public Gains gains = new Gains(1, 1, 1, 1, 1, 1);
+
+ @Loggable(id="test")
+ public double logable = 4;
+ @Loggable(id="test2")
+ public double second = 5;
+// private final RelativeEncoder m_testEncoder;
+
+ public TestMotor(WPI_TalonSRX testMotor) {
m_testMotor = testMotor;
- m_testEncoder = m_testMotor.getEncoder();
+ m_testMotor.configFactoryDefault();
+// m_testEncoder = m_testMotor.getEncoder();
+
+ TunerTablesHandler.getInstance().addControllers(TunerController.createTunerControllers(this));
+ TunerTablesHandler.getInstance().addLoggers(TunerLogger.createTunerLoggers(this));
}
- public void testDesmos() {
- DesmosServer.putDouble("Position", m_testEncoder.getPosition());
- m_testMotor.set(DesmosServer.readDouble("Speed"));
+// public void testDesmos() {
+// DesmosServer.putDouble("Position", m_testEncoder.getPosition());
+// m_testMotor.set(DesmosServer.readDouble("Speed"));
+// }
+
+ @Reader(id="TestMotor", value="velocity")
+ public double velocityReader() {
+ return m_testMotor.getSelectedSensorVelocity();
}
-}
+
+ @Reader(id="TestMotor", value="position")
+ public double positionReader() {
+ return m_testMotor.getSelectedSensorPosition();
+ }
+
+ @Controller(id="TestMotor", value="velocity")
+ public void velocityController(double value) {
+ logable = Math.random() * 10;
+ m_testMotor.set(value);
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/utility/DesmosServer.java b/src/main/java/frc4388/utility/DesmosServer.java
index 2c743cd..f75db63 100644
--- a/src/main/java/frc4388/utility/DesmosServer.java
+++ b/src/main/java/frc4388/utility/DesmosServer.java
@@ -208,7 +208,7 @@ public class DesmosServer extends Thread {
* Adds point to desmos queue
*
* @param name Name of desmos variable
- * @param value Point value
+ * @param point Point value
* */
public static void putPoint(String name, Point point) {
desmosQueue.put(name, new String[] {"point", "(" + point.x + "," + point.y + ")"});
diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java
index 930e34f..1d1a654 100644
--- a/src/main/java/frc4388/utility/Gains.java
+++ b/src/main/java/frc4388/utility/Gains.java
@@ -55,6 +55,6 @@ public class Gains {
m_kIzone = kIzone;
m_kminOutput = kMinOutput;
m_kmaxOutput = kMaxOutput;
- m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
+ m_kPeakOutput = Math.max(Math.abs(m_kminOutput), Math.abs(m_kmaxOutput));
}
}
diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index 56390d4..e09bf7a 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/src/main/java/frc4388/utility/RobotGyro.java
@@ -1,245 +1,245 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.utility;
-
-import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
-import com.kauailabs.navx.frc.AHRS;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-
-/**
- * Gyro class that allows for interchangeable use between a pigeon and a navX
- */
-public class RobotGyro implements Gyro, PIDSource, Sendable {
- private RobotTime m_robotTime = RobotTime.getInstance();
-
- private PigeonIMU m_pigeon = null;
- private AHRS m_navX = null;
- public boolean m_isGyroAPigeon; // true if pigeon, false if navX
-
- private double m_lastPigeonAngle;
- private double m_deltaPigeonAngle;
-
- /**
- * Creates a Gyro based on a pigeon
- *
- * @param gyro the gyroscope to use for Gyro
- */
- public RobotGyro(PigeonIMU gyro) {
- m_pigeon = gyro;
- m_isGyroAPigeon = true;
- }
-
- /**
- * Creates a Gyro based on a navX
- *
- * @param gyro the gyroscope to use for Gyro
- */
- public RobotGyro(AHRS gyro) {
- m_navX = gyro;
- m_isGyroAPigeon = false;
- }
-
- /**
- * Run in periodic if you are using a pigeon. Updates a delta angle so that it
- * can calculate getRate(). Note
- * that the getRate() method for a navX will likely be much more accurate than
- * for a pigeon.
- */
- public void updatePigeonDeltas() {
- double currentPigeonAngle = getAngle();
- m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
- m_lastPigeonAngle = currentPigeonAngle;
- }
-
- /**
- *
- * NavX:
- *
- * Calibrate the gyro by running for a number of samples and computing the
- * center value. Then use
- * the center value as the Accumulator center value for subsequent measurements.
- * It's important to
- * make sure that the robot is not moving while the centering calculations are
- * in progress, this
- * is typically done when the robot is first turned on while it's sitting at
- * rest before the
- * competition starts.
- *
- *
- * Pigeon:
- *
- * Calibrate the gyro by collecting data at a range of tempuratures. Allow
- * pigeon to cool, then boot
- * into calibration mode. For faster calibration, use a heat lamp to heat up the
- * pigeon. Once the pigeon
- * has seen a reasonable range of tempuratures, it will exit calibration mode.
- * It's important to
- * make sure that the robot is not moving while the tempurature calculations are
- * in progress, this
- * is typically done when the robot is first turned on while it's sitting at
- * rest before the
- * competition starts.
- */
- @Override
- public void calibrate() {
- if (m_isGyroAPigeon)
- m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
- else
- m_navX.calibrate();
- }
-
- @Override
- public void reset() {
- if (m_isGyroAPigeon)
- m_pigeon.setYaw(0);
- else
- m_navX.reset();
- }
-
- /**
- * Get Yaw, Pitch, and Roll data.
- *
- * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
- * Yaw is within [-368,640, +368,640] degrees.
- * Pitch is within [-90,+90] degrees.
- * Roll is within [-90,+90] degrees.
- */
- private double[] getPigeonAngles() {
- double[] angles = new double[3];
- m_pigeon.getYawPitchRoll(angles);
- return angles;
- }
-
- @Override
- public double getAngle() {
- if (m_isGyroAPigeon) {
- return getPigeonAngles()[0];
- } else {
- return m_navX.getAngle();
- }
- }
-
- /**
- * Gets an absolute heading of the robot
- *
- * @return heading from -180 to 180 degrees
- */
- public double getHeading() {
- return getHeading(getAngle());
- }
-
- /**
- * Gets an absolute heading of the robot
- *
- * @return heading from -180 to 180 degrees
- */
- public double getHeading(double angle) {
- return Math.IEEEremainder(angle, 360);
- }
-
- /**
- * Returns the current pitch value (in degrees, from -90 to 90)
- * reported by the sensor. Pitch is a measure of rotation around
- * the Y Axis.
- *
- * @return The current pitch value in degrees (-90 to 90).
- */
- public double getPitch() {
- if (m_isGyroAPigeon) {
- return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
- } else {
- return MathUtil.clamp(m_navX.getPitch(), -90, 90);
- }
- }
-
- /**
- * Returns the current roll value (in degrees, from -90 to 90)
- * reported by the sensor. Roll is a measure of rotation around
- * the X Axis.
- *
- * @return The current roll value in degrees (-90 to 90).
- */
- public double getRoll() {
- if (m_isGyroAPigeon) {
- return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
- } else {
- return MathUtil.clamp(m_navX.getRoll(), -90, 90);
- }
- }
-
- @Override
- public double getRate() {
- if (m_isGyroAPigeon) {
- return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
- } else {
- return m_navX.getRate();
- }
- }
-
- public PigeonIMU getPigeon() {
- return m_pigeon;
- }
-
- public AHRS getNavX() {
- return m_navX;
- }
-
- @Override
- public void close() throws Exception {
-
- }
-
- // Begin old GyroBase class
- private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
-
- /**
- * Set which parameter of the gyro you are using as a process control variable.
- * The Gyro class
- * supports the rate and displacement parameters
- *
- * @param pidSource An enum to select the parameter.
- */
- @Override
- public void setPIDSourceType(PIDSourceType pidSource) {
- m_pidSource = pidSource;
- }
-
- @Override
- public PIDSourceType getPIDSourceType() {
- return m_pidSource;
- }
-
- /**
- * Get the output of the gyro for use with PIDControllers. May be the angle or
- * rate depending on
- * the set PIDSourceType
- *
- * @return the output according to the gyro
- */
- @Override
- public double pidGet() {
- switch (m_pidSource) {
- case kRate:
- return getRate();
- case kDisplacement:
- return getAngle();
- default:
- return 0.0;
- }
- }
-
- @Override
- public void initSendable(SendableBuilder builder) {
- builder.setSmartDashboardType("Gyro");
- builder.addDoubleProperty("Value", this::getAngle, null);
- }
-}
+//// Copyright (c) FIRST and other WPILib contributors.
+//// Open Source Software; you can modify and/or share it under the terms of
+//// the WPILib BSD license file in the root directory of this project.
+//
+//package frc4388.utility;
+//
+//import com.ctre.phoenix.sensors.PigeonIMU;
+//import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
+//import com.kauailabs.navx.frc.AHRS;
+//
+//import edu.wpi.first.math.MathUtil;
+//import edu.wpi.first.util.sendable.Sendable;
+//import edu.wpi.first.util.sendable.SendableBuilder;
+//import edu.wpi.first.wpilibj.PIDSource;
+//import edu.wpi.first.wpilibj.PIDSourceType;
+//import edu.wpi.first.wpilibj.interfaces.Gyro;
+//
+///**
+// * Gyro class that allows for interchangeable use between a pigeon and a navX
+// */
+//public class RobotGyro implements Gyro, PIDSource, Sendable {
+// private RobotTime m_robotTime = RobotTime.getInstance();
+//
+// private PigeonIMU m_pigeon = null;
+// private AHRS m_navX = null;
+// public boolean m_isGyroAPigeon; // true if pigeon, false if navX
+//
+// private double m_lastPigeonAngle;
+// private double m_deltaPigeonAngle;
+//
+// /**
+// * Creates a Gyro based on a pigeon
+// *
+// * @param gyro the gyroscope to use for Gyro
+// */
+// public RobotGyro(PigeonIMU gyro) {
+// m_pigeon = gyro;
+// m_isGyroAPigeon = true;
+// }
+//
+// /**
+// * Creates a Gyro based on a navX
+// *
+// * @param gyro the gyroscope to use for Gyro
+// */
+// public RobotGyro(AHRS gyro) {
+// m_navX = gyro;
+// m_isGyroAPigeon = false;
+// }
+//
+// /**
+// * Run in periodic if you are using a pigeon. Updates a delta angle so that it
+// * can calculate getRate(). Note
+// * that the getRate() method for a navX will likely be much more accurate than
+// * for a pigeon.
+// */
+// public void updatePigeonDeltas() {
+// double currentPigeonAngle = getAngle();
+// m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
+// m_lastPigeonAngle = currentPigeonAngle;
+// }
+//
+// /**
+// *
+// * NavX:
+// *
+// * Calibrate the gyro by running for a number of samples and computing the
+// * center value. Then use
+// * the center value as the Accumulator center value for subsequent measurements.
+// * It's important to
+// * make sure that the robot is not moving while the centering calculations are
+// * in progress, this
+// * is typically done when the robot is first turned on while it's sitting at
+// * rest before the
+// * competition starts.
+// *
+// *
+// * Pigeon:
+// *
+// * Calibrate the gyro by collecting data at a range of tempuratures. Allow
+// * pigeon to cool, then boot
+// * into calibration mode. For faster calibration, use a heat lamp to heat up the
+// * pigeon. Once the pigeon
+// * has seen a reasonable range of tempuratures, it will exit calibration mode.
+// * It's important to
+// * make sure that the robot is not moving while the tempurature calculations are
+// * in progress, this
+// * is typically done when the robot is first turned on while it's sitting at
+// * rest before the
+// * competition starts.
+// */
+// @Override
+// public void calibrate() {
+// if (m_isGyroAPigeon)
+// m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+// else
+// m_navX.calibrate();
+// }
+//
+// @Override
+// public void reset() {
+// if (m_isGyroAPigeon)
+// m_pigeon.setYaw(0);
+// else
+// m_navX.reset();
+// }
+//
+// /**
+// * Get Yaw, Pitch, and Roll data.
+// *
+// * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
+// * Yaw is within [-368,640, +368,640] degrees.
+// * Pitch is within [-90,+90] degrees.
+// * Roll is within [-90,+90] degrees.
+// */
+// private double[] getPigeonAngles() {
+// double[] angles = new double[3];
+// m_pigeon.getYawPitchRoll(angles);
+// return angles;
+// }
+//
+// @Override
+// public double getAngle() {
+// if (m_isGyroAPigeon) {
+// return getPigeonAngles()[0];
+// } else {
+// return m_navX.getAngle();
+// }
+// }
+//
+// /**
+// * Gets an absolute heading of the robot
+// *
+// * @return heading from -180 to 180 degrees
+// */
+// public double getHeading() {
+// return getHeading(getAngle());
+// }
+//
+// /**
+// * Gets an absolute heading of the robot
+// *
+// * @return heading from -180 to 180 degrees
+// */
+// public double getHeading(double angle) {
+// return Math.IEEEremainder(angle, 360);
+// }
+//
+// /**
+// * Returns the current pitch value (in degrees, from -90 to 90)
+// * reported by the sensor. Pitch is a measure of rotation around
+// * the Y Axis.
+// *
+// * @return The current pitch value in degrees (-90 to 90).
+// */
+// public double getPitch() {
+// if (m_isGyroAPigeon) {
+// return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
+// } else {
+// return MathUtil.clamp(m_navX.getPitch(), -90, 90);
+// }
+// }
+//
+// /**
+// * Returns the current roll value (in degrees, from -90 to 90)
+// * reported by the sensor. Roll is a measure of rotation around
+// * the X Axis.
+// *
+// * @return The current roll value in degrees (-90 to 90).
+// */
+// public double getRoll() {
+// if (m_isGyroAPigeon) {
+// return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
+// } else {
+// return MathUtil.clamp(m_navX.getRoll(), -90, 90);
+// }
+// }
+//
+// @Override
+// public double getRate() {
+// if (m_isGyroAPigeon) {
+// return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
+// } else {
+// return m_navX.getRate();
+// }
+// }
+//
+// public PigeonIMU getPigeon() {
+// return m_pigeon;
+// }
+//
+// public AHRS getNavX() {
+// return m_navX;
+// }
+//
+// @Override
+// public void close() throws Exception {
+//
+// }
+//
+// // Begin old GyroBase class
+// private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+//
+// /**
+// * Set which parameter of the gyro you are using as a process control variable.
+// * The Gyro class
+// * supports the rate and displacement parameters
+// *
+// * @param pidSource An enum to select the parameter.
+// */
+// @Override
+// public void setPIDSourceType(PIDSourceType pidSource) {
+// m_pidSource = pidSource;
+// }
+//
+// @Override
+// public PIDSourceType getPIDSourceType() {
+// return m_pidSource;
+// }
+//
+// /**
+// * Get the output of the gyro for use with PIDControllers. May be the angle or
+// * rate depending on
+// * the set PIDSourceType
+// *
+// * @return the output according to the gyro
+// */
+// @Override
+// public double pidGet() {
+// switch (m_pidSource) {
+// case kRate:
+// return getRate();
+// case kDisplacement:
+// return getAngle();
+// default:
+// return 0.0;
+// }
+// }
+//
+// @Override
+// public void initSendable(SendableBuilder builder) {
+// builder.setSmartDashboardType("Gyro");
+// builder.addDoubleProperty("Value", this::getAngle, null);
+// }
+//}
diff --git a/src/main/java/frc4388/utility/tuner/CANBusReader.java b/src/main/java/frc4388/utility/tuner/CANBusReader.java
new file mode 100644
index 0000000..4290da7
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/CANBusReader.java
@@ -0,0 +1,97 @@
+package frc4388.utility.tuner;
+
+import edu.wpi.first.hal.can.CANJNI;
+import frc4388.utility.tuner.tunercandevices.TCAN_SparkMax;
+import frc4388.utility.tuner.tunercandevices.TCAN_TalonFX;
+import frc4388.utility.tuner.tunercandevices.TCAN_TalonSRX;
+import frc4388.utility.tuner.tunercandevices.TunerCANDevice;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.ArrayList;
+import java.util.List;
+
+// WARN This class may not be possible to test until I have access to actual hardware
+public class CANBusReader {
+ private static final class DeviceSignatures {
+ // CTRE
+ static final int PDP = 0x08041400; static final int PCM = 0x09041400; static final int TalonSRX = 0x02041400;
+ static final int TalonFX = -1;
+ // REV robotics
+ static final int SparkMax = -1;
+
+ // ? Should this be a method
+ static final int[] asArray = new int[] {PDP, PCM, TalonSRX, TalonFX, SparkMax};
+
+ private DeviceSignatures() {}
+ }
+
+ // * Basically copied from https://www.chiefdelphi.com/t/how-to-detect-missing-can-devices-from-java/147675/10 with
+ // * a few small changes
+ private static long checkMessage(int fullId, int deviceID) {
+ try {
+ ByteBuffer targetID = ByteBuffer.allocateDirect(4).order(ByteOrder.LITTLE_ENDIAN);
+ ByteBuffer timeStamp = ByteBuffer.allocateDirect(4).order(ByteOrder.LITTLE_ENDIAN);
+
+ targetID.asIntBuffer().put(0,fullId | deviceID);
+ timeStamp.asIntBuffer().put(0,0x00000000);
+
+ // Literally magic.
+ // ? this may not work in sim.
+ CANJNI.FRCNetCommCANSessionMuxReceiveMessage(targetID.asIntBuffer(), 0x1fffffff, timeStamp);
+
+ long retval = timeStamp.getInt();
+ retval &= 0xFFFFFFFF; /* undo sign-extension */
+ return retval;
+ } catch (Exception e) {
+ return -1;
+ }
+ }
+
+ // TODO: Make this method work. May need actual hardware for testing
+ // https://www.chiefdelphi.com/t/how-to-detect-missing-can-devices-from-java/147675/10
+ // ^ Really f#cking useful
+ // This is basically copied from the link above except rewritten because I didn't like how the official ctre person did it
+ public static List readCANBus() {
+ List activeDevices = new ArrayList<>();
+
+ long pdpTimestamp = checkMessage(DeviceSignatures.PDP, 0);
+
+ // ! Rather poorly done data structure: ArrayList>[63]
+ // ? But maybe the best option
+
+ // Lots of arrays because I don't want to make a proper class that holds multiple Pair values
+ long[] srxTimestamps = new long[63];
+ long[] fxTimestamps = new long[63];
+ long[] smaxTimestamps = new long[63];
+
+ for(int i = 0; i < 63; i++) {
+ srxTimestamps[i] = checkMessage(DeviceSignatures.TalonSRX, i+1);
+ fxTimestamps[i] = checkMessage(DeviceSignatures.TalonFX, i+1);
+ smaxTimestamps[i] = checkMessage(DeviceSignatures.SparkMax, i+1);
+ }
+
+ // Will cause a loop overrun
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ }
+
+ // Writing my own class would reduce most of this copy/paste
+ for(int i = 0; i < 63; i++) {
+ long srxTimestamp = checkMessage(DeviceSignatures.TalonSRX, i+1);
+ long fxTimestamp = checkMessage(DeviceSignatures.TalonFX, i+1);
+ long smaxTimestamp = checkMessage(DeviceSignatures.SparkMax, i+1);
+
+ if(srxTimestamps[i] >= 0 && srxTimestamp >= 0 && srxTimestamps[i] != srxTimestamp)
+ activeDevices.add(new TCAN_TalonSRX(i));
+ else if(fxTimestamps[i] >= 0 && fxTimestamp >= 0 && fxTimestamps[i] != fxTimestamp)
+ activeDevices.add(new TCAN_TalonFX(i));
+ else if(smaxTimestamps[i] >= 0 && smaxTimestamp >= 0 && smaxTimestamps[i] != smaxTimestamp)
+ activeDevices.add(new TCAN_SparkMax(i));
+ }
+
+ return activeDevices;
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/TunerCANDeviceFactory.java b/src/main/java/frc4388/utility/tuner/TunerCANDeviceFactory.java
new file mode 100644
index 0000000..4db8c4b
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/TunerCANDeviceFactory.java
@@ -0,0 +1,28 @@
+package frc4388.utility.tuner;
+
+import frc4388.utility.tuner.tunercandevices.TunerCANDevice;
+
+/* ! -------------------------------------------------------------------------------------------------------------------
+ ! ------------------------------------- THIS CLASS IS POTENTIALLY UNNECESSARY: --------------------------------------
+ ! --------------- Having a factory class that is independent of the CANBusReader class -----------------
+ ! --------------- is probably too much boilerplate and too much room for something to go wrong. -----------------
+ ! --------------- On the other hand a factory that can take device ids and produce the correct -----------------
+ ! --------------- device controller will make adding autodetected types easier to implement. -----------------
+ ! ------------------------------------------------ Think more later -------------------------------------------------
+ ! -----------------------------------------------------------------------------------------------------------------*/
+public class TunerCANDeviceFactory {
+ public static TunerCANDevice getCANDevice(byte i) {
+ // ? :( Why no switch expressions ?
+ //// return switch(i) {
+ //// case 0 -> null;
+ //// case 1 -> null;
+ //// default -> null;
+ //// }
+
+ switch (i) {
+ case 0: return null;
+ case 1: return null;
+ default: return null;
+ }
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/TunerController.java b/src/main/java/frc4388/utility/tuner/TunerController.java
new file mode 100644
index 0000000..830302b
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/TunerController.java
@@ -0,0 +1,168 @@
+package frc4388.utility.tuner;
+
+import frc4388.utility.Gains;
+import frc4388.utility.tuner.annotations.Controller;
+import frc4388.utility.tuner.annotations.GainsField;
+import frc4388.utility.tuner.annotations.GainsSetter;
+import frc4388.utility.tuner.annotations.Reader;
+
+import java.lang.reflect.Field;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
+import java.lang.reflect.Parameter;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.Map;
+import java.util.Set;
+import java.util.function.BiConsumer;
+
+public class TunerController {
+ private final Object baseObject;
+ private final Map readers;
+ private final Map controllers;
+ private Gains gains;
+ private Method gainsSetter;
+
+ public TunerController(Object baseObject) {
+ this.baseObject = baseObject;
+ this.readers = new HashMap<>();
+ this.controllers = new HashMap<>();
+ }
+
+ public void addReader(String name, Method method) {
+ this.readers.put(name, method);
+ }
+
+ public void addController(String name, Method method) {
+ this.controllers.put(name, method);
+ }
+
+ public void setGainsSetter(Method gainsSetter) {
+ if(this.gainsSetter == null)
+ this.gainsSetter = gainsSetter;
+ else
+ System.err.println("Extraneous GainsSetter");
+ }
+
+ public void setGainsObject(Gains gains) {
+ if(this.gains == null)
+ this.gains = gains;
+ else
+ System.err.println("Extraneous Gains");
+ }
+
+ // TODO: Get separate names for controller/readers
+ public Set getNames() {
+ Set names = new HashSet<>(this.controllers.keySet());
+ names.addAll(this.readers.keySet());
+ return names;
+ }
+
+ public double readValue(String value) {
+ try {
+ if(this.readers.containsKey(value))
+ return (Double) this.readers.get(value).invoke(this.baseObject);
+
+ return 0;
+ } catch (IllegalAccessException | InvocationTargetException e) {
+ throw new RuntimeException(e);
+ }
+ }
+
+ public void setTarget(String value, double input) {
+ try {
+ if(this.controllers.containsKey(value))
+ this.controllers.get(value).invoke(this.baseObject, input);
+ } catch (IllegalAccessException | InvocationTargetException e) {
+ throw new RuntimeException(e);
+ }
+ }
+
+ public void setGains(double kP, double kI, double kD, double kF) {
+ try {
+ if(this.gainsSetter == null) return;
+
+ if(this.gains == null)
+ this.gains = new Gains(0, 0, 0, 0, 0, 0);
+
+ this.gainsSetter.invoke(this.baseObject, this.gains, kP, kI, kD, kF);
+ } catch (IllegalAccessException | InvocationTargetException e) {
+ throw new RuntimeException(e);
+ }
+ }
+
+ public Gains getGains() {
+ return this.gains != null ? this.gains : (this.gains = new Gains(0, 0, 0, 0, 0, 0));
+ }
+
+ /* * --------------------------------------------------------------------------------------------------------------- *
+ * -------------- You should probably move the following static code into a more appropriate file ---------------- *
+ * --------------------------- Either a factory class or the TunerTablesHandler class ---------------------------- *
+ * --------------------------------------------------------------------------------------------------------------- */
+
+ public static Map createTunerControllers(Object object) {
+ Map controllers = new HashMap<>(getAnnotatedMethods(object));
+ addAnnotatedFields(object, controllers);
+
+ return controllers;
+ }
+
+ private static Map getAnnotatedMethods(Object object) {
+ Map controllers = new HashMap<>();
+ Class clazz = object.getClass();
+
+ for(Method method : clazz.getMethods()) {
+ Reader reader = method.getAnnotation(Reader.class);
+ if(reader != null && verifyMethod(method, double.class))
+ addAnnotatedMethod(controllers, reader.id(), object, method, (c, m) -> c.addReader(reader.value(), m));
+
+ Controller controller = method.getAnnotation(Controller.class);
+ if(controller != null && verifyParameters(method, double.class))
+ addAnnotatedMethod(controllers, controller.id(), object, method, (c, m) -> c.addController(controller.value(), m));
+
+ GainsSetter gainsSetter = method.getAnnotation(GainsSetter.class);
+ if(gainsSetter != null && verifyParameters(method, double.class, double.class, double.class, double.class))
+ addAnnotatedMethod(controllers, gainsSetter.id(), object, method, TunerController::setGainsSetter);
+ }
+
+ return controllers;
+ }
+
+ private static boolean verifyMethod(Method method, Class returnType, Class... types) {
+ if(!method.getReturnType().equals(returnType)) return false;
+
+ Parameter[] parameters = method.getParameters();
+ if(parameters.length != types.length) return false;
+
+ for(int i = 0; i < parameters.length; i++)
+ if(!parameters[i].getType().equals(types[i])) return false;
+
+ return true;
+ }
+
+ private static boolean verifyParameters(Method method, Class... types) {
+ return verifyMethod(method, void.class, types);
+ }
+
+ private static void addAnnotatedMethod(Map controllers,String id, Object object,
+ Method method, BiConsumer action)
+ {
+ controllers.putIfAbsent(id, new TunerController(object));
+ action.accept(controllers.get(id), method);
+ }
+
+ private static void addAnnotatedFields(Object object, Map controllers) {
+ Class clazz = object.getClass();
+
+ Field[] fields = clazz.getFields();
+ for(Field field : fields) {
+ GainsField gainsField = field.getAnnotation(GainsField.class);
+ if(gainsField != null && controllers.containsKey(gainsField.id())) try {
+ Gains testGains = (Gains) field.get(object);
+ controllers.getOrDefault(gainsField.id(), new TunerController(object)).setGainsObject(testGains);
+ } catch (IllegalAccessException e) {
+ throw new RuntimeException(e);
+ }
+ }
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/TunerGains.java b/src/main/java/frc4388/utility/tuner/TunerGains.java
new file mode 100644
index 0000000..7a1de93
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/TunerGains.java
@@ -0,0 +1,33 @@
+package frc4388.utility.tuner;
+
+import frc4388.utility.Gains;
+
+public class TunerGains extends Gains {
+ String id;
+
+ public TunerGains(String id) {
+ super(0, 0, 0, 0, 0, 0);
+ this.id = id;
+ }
+
+ public TunerGains(String id, double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput) {
+ super(kP, kI, kD, kF, kIzone, kPeakOutput);
+ this.id = id;
+ }
+
+ public TunerGains(String id, double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput) {
+ super(kP, kI, kD, kF, kIzone, kMinOutput, kMaxOutput);
+ this.id = id;
+ }
+
+ public void setGains(double kP, double kI, double kD, double kF) {
+ m_kP = kP;
+ m_kI = kI;
+ m_kD = kD;
+ m_kF = kF;
+ }
+
+ public void setGains(double kP, double kI, double kD) {
+ setGains(kP, kI, kD, 0);
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/TunerLogger.java b/src/main/java/frc4388/utility/tuner/TunerLogger.java
new file mode 100644
index 0000000..9d9ae1d
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/TunerLogger.java
@@ -0,0 +1,49 @@
+package frc4388.utility.tuner;
+
+import frc4388.utility.tuner.annotations.Loggable;
+
+import java.lang.reflect.Field;
+import java.util.HashMap;
+import java.util.Map;
+
+public class TunerLogger {
+ private final Object baseObject;
+ private final Field field;
+
+ private final boolean alwaysActive;
+
+ public TunerLogger(Object baseObject, Field field, boolean alwaysActive) {
+ this.baseObject = baseObject;
+ this.field = field;
+ this.alwaysActive = alwaysActive;
+ }
+
+ public double getCurrentValue() {
+ try {
+ return (double) this.field.get(this.baseObject);
+ } catch (IllegalAccessException e) {
+ return 0;
+ }
+ }
+
+ /* * --------------------------------------------------------------------------------------------------------------- *
+ * -------------- You should probably move the following static code into a more appropriate file ---------------- *
+ * --------------------------- Either a factory class or the TunerTablesHandler class ---------------------------- *
+ * --------------------------------------------------------------------------------------------------------------- */
+
+ public static Map createTunerLoggers(Object object) {
+ Map loggers = new HashMap<>();
+ Class clazz = object.getClass();
+
+ Field[] fields = clazz.getFields();
+ for(Field field : fields) {
+ Loggable loggable = field.getAnnotation(Loggable.class);
+ if(loggable != null && double.class.isAssignableFrom(field.getType())) {
+ field.setAccessible(true);
+ loggers.put(loggable.id(), new TunerLogger(object, field, loggable.alwaysActive()));
+ }
+ }
+
+ return loggers;
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/TunerTablesHandler.java b/src/main/java/frc4388/utility/tuner/TunerTablesHandler.java
new file mode 100644
index 0000000..4974771
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/TunerTablesHandler.java
@@ -0,0 +1,114 @@
+package frc4388.utility.tuner;
+
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc4388.utility.Gains;
+import frc4388.utility.tuner.tunercandevices.TunerCANDevice;
+
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+
+public class TunerTablesHandler {
+ private final NetworkTable tunerTable;
+ private final NetworkTableEntry activeEntry;
+
+ // ? Should NetworkTable be the key or the value ?
+ private final Map controllerNetworkTableMap;
+ private final Map loggerNetworkEntryMap;
+ private boolean hasNotBeenActivated = true;
+ private List canBus; // * Needs to be a field so that the devices are loaded into memory
+ // * Although, since the object belongs to the TunerController, the garbage collector might not get rid the devices
+
+ private static TunerTablesHandler instance;
+
+ public static TunerTablesHandler getInstance() {
+ return instance != null ? instance : (instance = new TunerTablesHandler());
+ }
+
+ private TunerTablesHandler() {
+ this.tunerTable = NetworkTableInstance.getDefault().getTable("TunerTables");
+ this.activeEntry = this.tunerTable.getEntry("active");
+ this.activeEntry.setBoolean(false);
+
+ this.controllerNetworkTableMap = new HashMap<>();
+ this.loggerNetworkEntryMap = new HashMap<>();
+ }
+
+ public void addControllers(Map controllerMap) {
+ for(String name : controllerMap.keySet()) {
+ TunerController controller = controllerMap.get(name);
+
+ NetworkTable subTable = tunerTable.getSubTable(name);
+ controllerNetworkTableMap.put(subTable, controller);
+
+ NetworkTable controllerTable = subTable.getSubTable("ControllerTable");
+ controllerTable.addEntryListener((table, key, entry, value, flags) -> {
+ if(activeEntry.getBoolean(false))
+ controller.setTarget(key, value.getDouble());
+ }, EntryListenerFlags.kUpdate);
+
+ for(String controllerName : controller.getNames())
+ controllerTable.getEntry(controllerName).setDouble(0);
+
+ NetworkTable gainsTable = subTable.getSubTable("GainsTable");
+ gainsTable.addEntryListener((table, key, entry, value, flags) -> {
+ if(activeEntry.getBoolean(false))
+ controller.setGains(
+ gainsTable.getEntry("kP").getDouble(0),
+ gainsTable.getEntry("kI").getDouble(0),
+ gainsTable.getEntry("kD").getDouble(0),
+ gainsTable.getEntry("kF").getDouble(0)
+ );
+ }, EntryListenerFlags.kUpdate);
+
+ // TODO: make these values reflect default values if gains field annotation is involved
+ Gains defaultGains = controller.getGains();
+
+ gainsTable.getEntry("kP").setDouble(defaultGains.m_kP);
+ gainsTable.getEntry("kI").setDouble(defaultGains.m_kI);
+ gainsTable.getEntry("kD").setDouble(defaultGains.m_kD);
+ gainsTable.getEntry("kF").setDouble(defaultGains.m_kF);
+ }
+ }
+
+ public void addLoggers(Map loggerMap) {
+ NetworkTable loggerTable = tunerTable.getSubTable("LoggerTable");
+
+ for(String name : loggerMap.keySet()) {
+ TunerLogger logger = loggerMap.get(name);
+ NetworkTableEntry loggerEntry = loggerTable.getEntry(name);
+
+ loggerNetworkEntryMap.put(loggerEntry, logger);
+ }
+ }
+
+ public void writeReadersToNetTables(NetworkTable table, TunerController controller) {
+ for(String readerName : controller.getNames())
+ table.getEntry(readerName).setDouble(controller.readValue(readerName));
+ }
+
+ public void updateReaders() {
+ if(!activeEntry.getBoolean(false)) return;
+
+ if(hasNotBeenActivated) {
+ canBus = CANBusReader.readCANBus();
+ for (TunerCANDevice device : canBus)
+ addControllers(device.getTunerController());
+
+ hasNotBeenActivated = false;
+ }
+
+ for(NetworkTable table : controllerNetworkTableMap.keySet()) {
+ TunerController controller = controllerNetworkTableMap.get(table);
+ writeReadersToNetTables(table.getSubTable("ReaderTable"), controller);
+ }
+
+ // TODO: make always active work properly
+ for(NetworkTableEntry entry : loggerNetworkEntryMap.keySet()) {
+ entry.setDouble(loggerNetworkEntryMap.get(entry).getCurrentValue());
+ }
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/annotations/Controller.java b/src/main/java/frc4388/utility/tuner/annotations/Controller.java
new file mode 100644
index 0000000..4a4df69
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/annotations/Controller.java
@@ -0,0 +1,14 @@
+package frc4388.utility.tuner.annotations;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+@Retention(RetentionPolicy.RUNTIME)
+@Target(ElementType.METHOD)
+
+public @interface Controller {
+ String id();
+ String value() default "value";
+}
diff --git a/src/main/java/frc4388/utility/tuner/annotations/GainsField.java b/src/main/java/frc4388/utility/tuner/annotations/GainsField.java
new file mode 100644
index 0000000..7b54f0e
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/annotations/GainsField.java
@@ -0,0 +1,13 @@
+package frc4388.utility.tuner.annotations;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+@Retention(RetentionPolicy.RUNTIME)
+@Target(ElementType.FIELD)
+
+public @interface GainsField {
+ String id();
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/utility/tuner/annotations/GainsSetter.java b/src/main/java/frc4388/utility/tuner/annotations/GainsSetter.java
new file mode 100644
index 0000000..1d71d36
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/annotations/GainsSetter.java
@@ -0,0 +1,13 @@
+package frc4388.utility.tuner.annotations;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+@Retention(RetentionPolicy.RUNTIME)
+@Target(ElementType.METHOD)
+
+public @interface GainsSetter {
+ String id();
+}
diff --git a/src/main/java/frc4388/utility/tuner/annotations/Loggable.java b/src/main/java/frc4388/utility/tuner/annotations/Loggable.java
new file mode 100644
index 0000000..982c99f
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/annotations/Loggable.java
@@ -0,0 +1,14 @@
+package frc4388.utility.tuner.annotations;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+@Retention(RetentionPolicy.RUNTIME)
+@Target(ElementType.FIELD)
+
+public @interface Loggable {
+ String id();
+ boolean alwaysActive() default false; // Run logger when
+}
diff --git a/src/main/java/frc4388/utility/tuner/annotations/Reader.java b/src/main/java/frc4388/utility/tuner/annotations/Reader.java
new file mode 100644
index 0000000..27ceac1
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/annotations/Reader.java
@@ -0,0 +1,14 @@
+package frc4388.utility.tuner.annotations;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+@Retention(RetentionPolicy.RUNTIME)
+@Target(ElementType.METHOD)
+
+public @interface Reader {
+ String id();
+ String value() default "value";
+}
diff --git a/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_SparkMax.java b/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_SparkMax.java
new file mode 100644
index 0000000..47794ac
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_SparkMax.java
@@ -0,0 +1,43 @@
+package frc4388.utility.tuner.tunercandevices;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel;
+
+public class TCAN_SparkMax extends TunerCANDevice {
+ private final CANSparkMax motor;
+
+ public TCAN_SparkMax(int id) {
+ super(id, "SparkMax");
+ this.motor = new CANSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
+ }
+
+ @Override
+ void setPosition(double pos) {
+ this.motor.getPIDController().setReference(pos, CANSparkMax.ControlType.kPosition);
+ }
+
+ @Override
+ void setVelocity(double vel) {
+ this.motor.getPIDController().setReference(vel, CANSparkMax.ControlType.kVelocity);
+ }
+
+ @Override
+ void setOutput(double output) {
+ this.motor.set(output);
+ }
+
+ @Override
+ double getPosition() {
+ return this.motor.getEncoder().getPosition();
+ }
+
+ @Override
+ double getVelocity() {
+ return this.motor.getEncoder().getVelocity();
+ }
+
+ @Override
+ double getOutput() {
+ return this.motor.get();
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonFX.java b/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonFX.java
new file mode 100644
index 0000000..175e601
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonFX.java
@@ -0,0 +1,44 @@
+package frc4388.utility.tuner.tunercandevices;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+public class TCAN_TalonFX extends TunerCANDevice {
+ private final WPI_TalonFX motor;
+
+ public TCAN_TalonFX(int id) {
+ super(id, "FalconFX");
+ this.motor = new WPI_TalonFX(this.id);
+ this.motor.configFactoryDefault();
+ }
+
+ @Override
+ void setPosition(double pos) {
+ this.motor.set(ControlMode.Position, pos);
+ }
+
+ @Override
+ void setVelocity(double vel) {
+ this.motor.set(ControlMode.Velocity, vel);
+ }
+
+ @Override
+ void setOutput(double output) {
+ this.motor.set(ControlMode.PercentOutput, output);
+ }
+
+ @Override
+ double getPosition() {
+ return this.motor.getSelectedSensorPosition();
+ }
+
+ @Override
+ double getVelocity() {
+ return this.motor.getSelectedSensorVelocity();
+ }
+
+ @Override
+ double getOutput() {
+ return this.motor.getMotorOutputPercent();
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonSRX.java b/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonSRX.java
new file mode 100644
index 0000000..2b5be42
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonSRX.java
@@ -0,0 +1,44 @@
+package frc4388.utility.tuner.tunercandevices;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+public class TCAN_TalonSRX extends TunerCANDevice {
+ private final WPI_TalonSRX motor;
+
+ public TCAN_TalonSRX(int id) {
+ super(id, "FalconSRX");
+ motor = new WPI_TalonSRX(this.id);
+ motor.configFactoryDefault();
+ }
+
+ @Override
+ void setPosition(double pos) {
+ motor.set(ControlMode.Position, pos);
+ }
+
+ @Override
+ void setVelocity(double vel) {
+ motor.set(ControlMode.Velocity, vel);
+ }
+
+ @Override
+ void setOutput(double output) {
+ motor.set(ControlMode.PercentOutput, output);
+ }
+
+ @Override
+ double getPosition() {
+ return motor.getSelectedSensorPosition();
+ }
+
+ @Override
+ double getVelocity() {
+ return motor.getSelectedSensorVelocity();
+ }
+
+ @Override
+ double getOutput() {
+ return motor.getMotorOutputPercent();
+ }
+}
diff --git a/src/main/java/frc4388/utility/tuner/tunercandevices/TunerCANDevice.java b/src/main/java/frc4388/utility/tuner/tunercandevices/TunerCANDevice.java
new file mode 100644
index 0000000..43594cc
--- /dev/null
+++ b/src/main/java/frc4388/utility/tuner/tunercandevices/TunerCANDevice.java
@@ -0,0 +1,40 @@
+package frc4388.utility.tuner.tunercandevices;
+
+import frc4388.utility.tuner.annotations.Controller;
+import frc4388.utility.tuner.annotations.Reader;
+import frc4388.utility.tuner.TunerController;
+
+import java.util.Map;
+
+// ! Could be an interface, but that would be boring
+// * serious: the constructor is one bit of boilerplate I would rather not have to write again
+public abstract class TunerCANDevice {
+ protected final int id;
+ protected final String name;
+
+ public TunerCANDevice(int id, String name) {
+ this.id = id;
+ this.name = name;
+ }
+
+ public Map getTunerController() {
+ Map controllers = TunerController.createTunerControllers(this);
+ TunerController controller = controllers.remove("TunerCANDevice");
+ controllers.put(name + ": " + id, controller);
+ return controllers;
+ }
+
+ @Controller(id="TunerCANDevice", value="position")
+ abstract void setPosition(double pos);
+ @Controller(id="TunerCANDevice", value="velocity")
+ abstract void setVelocity(double vel);
+ @Controller(id="TunerCANDevice", value="output")
+ abstract void setOutput(double output);
+
+ @Reader(id="TunerCANDevice", value="position")
+ abstract double getPosition();
+ @Reader(id="TunerCANDevice", value="velocity")
+ abstract double getVelocity();
+ @Reader(id="TunerCANDevice", value="output")
+ abstract double getOutput();
+}