Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Did a few things

Browse Source
This commit is contained in:
66945
2022-06-13 20:10:18 -06:00
parent 0b2f1871ef
commit f827314e09
32 changed files with 1471 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+14 -5
View File
@@ -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,
src/main/java/frc4388/robot/RobotContainer.java
+1 -17
View File
@@ -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));
src/main/java/frc4388/robot/RobotMap.java
+2 -1
View File
@@ -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);
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+3 -3
View File
@@ -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++) {
src/main/java/frc4388/robot/subsystems/TestMotor.java
+47 -12
View File
@@ -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);
}
}
src/main/java/frc4388/utility/DesmosServer.java
+1 -1
View File
@@ -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 + ")"});
src/main/java/frc4388/utility/Gains.java
+1 -1
View File
@@ -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));
}
}
src/main/java/frc4388/utility/RobotGyro.java
+245 -245
View File
@@ -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;
}
/**
* <p>
* NavX:
* <p>
* 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.
*
* <p>
* Pigeon:
* <p>
* 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;
// }
//
// /**
// * <p>
// * NavX:
// * <p>
// * 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.
// *
// * <p>
// * Pigeon:
// * <p>
// * 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);
// }
//}
src/main/java/frc4388/utility/tuner/CANBusReader.java
+97
View File
@@ -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<TunerCANDevice> readCANBus() {
List<TunerCANDevice> activeDevices = new ArrayList<>();
long pdpTimestamp = checkMessage(DeviceSignatures.PDP, 0);
// ! Rather poorly done data structure: ArrayList<Pair<Integer, Long>>[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;
}
}
src/main/java/frc4388/utility/tuner/TunerCANDeviceFactory.java
+28
View File
@@ -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;
}
}
}
src/main/java/frc4388/utility/tuner/TunerController.java
+168
View File
@@ -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<String, Method> readers;
private final Map<String, Method> 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<String> getNames() {
Set<String> 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<String, TunerController> createTunerControllers(Object object) {
Map<String, TunerController> controllers = new HashMap<>(getAnnotatedMethods(object));
addAnnotatedFields(object, controllers);
return controllers;
}
private static Map<String, TunerController> getAnnotatedMethods(Object object) {
Map<String, TunerController> 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<String, TunerController> controllers,String id, Object object,
Method method, BiConsumer<TunerController, Method> action)
{
controllers.putIfAbsent(id, new TunerController(object));
action.accept(controllers.get(id), method);
}
private static void addAnnotatedFields(Object object, Map<String, TunerController> 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);
}
}
}
}
src/main/java/frc4388/utility/tuner/TunerGains.java
+33
View File
@@ -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);
}
}
src/main/java/frc4388/utility/tuner/TunerLogger.java
+49
View File
@@ -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<String, TunerLogger> createTunerLoggers(Object object) {
Map<String, TunerLogger> 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;
}
}
src/main/java/frc4388/utility/tuner/TunerTablesHandler.java
+114
View File
@@ -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<NetworkTable, TunerController> controllerNetworkTableMap;
private final Map<NetworkTableEntry, TunerLogger> loggerNetworkEntryMap;
private boolean hasNotBeenActivated = true;
private List<TunerCANDevice> 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<String, TunerController> 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<String, TunerLogger> 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());
}
}
}
src/main/java/frc4388/utility/tuner/annotations/Controller.java
+14
View File
@@ -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";
}
src/main/java/frc4388/utility/tuner/annotations/GainsField.java
+13
View File
@@ -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();
}
src/main/java/frc4388/utility/tuner/annotations/GainsSetter.java
+13
View File
@@ -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();
}
src/main/java/frc4388/utility/tuner/annotations/Loggable.java
+14
View File
@@ -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
}
src/main/java/frc4388/utility/tuner/annotations/Reader.java
+14
View File
@@ -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";
}
src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_SparkMax.java
+43
View File
@@ -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();
}
}
src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonFX.java
+44
View File
@@ -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();
}
}
src/main/java/frc4388/utility/tuner/tunercandevices/TCAN_TalonSRX.java
+44
View File
@@ -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();
}
}
src/main/java/frc4388/utility/tuner/tunercandevices/TunerCANDevice.java
+40
View File
@@ -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<String, TunerController> getTunerController() {
Map<String, TunerController> 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();
}