mirror of
https://github.com/Team4388/Robot-Essentials.git
synced 2026-06-08 16:28:02 -06:00
Merge pull request #51 from Team4388/2025-Changes
2025 Robot Essentials
This commit is contained in:
Michael Mikovsky
Vendored
+9
-3
@@ -4,18 +4,24 @@
|
||||
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
|
||||
"version": "0.2.0",
|
||||
"configurations": [
|
||||
|
||||
{
|
||||
"type": "java",
|
||||
"name": "Main",
|
||||
"request": "launch",
|
||||
"mainClass": "frc4388.robot.Main",
|
||||
"projectName": "2025RidgeScape"
|
||||
},
|
||||
{
|
||||
"type": "wpilib",
|
||||
"name": "WPILib Desktop Debug",
|
||||
"request": "launch",
|
||||
"desktop": true,
|
||||
"desktop": true
|
||||
},
|
||||
{
|
||||
"type": "wpilib",
|
||||
"name": "WPILib roboRIO Debug",
|
||||
"request": "launch",
|
||||
"desktop": false,
|
||||
"desktop": false
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
Vendored
+3
-1
@@ -56,5 +56,7 @@
|
||||
"edu.wpi.first.math.proto.*",
|
||||
"edu.wpi.first.math.**.proto.*",
|
||||
"edu.wpi.first.math.**.struct.*",
|
||||
]
|
||||
],
|
||||
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable",
|
||||
"wpilib.autoStartRioLog": false
|
||||
}
|
||||
|
||||
+21
-1
@@ -1,6 +1,7 @@
|
||||
plugins {
|
||||
id "java"
|
||||
id "edu.wpi.first.GradleRIO" version "2025.1.1"
|
||||
id "edu.wpi.first.GradleRIO" version "2025.3.2"
|
||||
id "com.peterabeles.gversion" version "1.10"
|
||||
}
|
||||
|
||||
java {
|
||||
@@ -72,6 +73,9 @@ dependencies {
|
||||
|
||||
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
|
||||
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
|
||||
|
||||
def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
|
||||
annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
|
||||
}
|
||||
|
||||
test {
|
||||
@@ -102,3 +106,19 @@ wpi.java.configureTestTasks(test)
|
||||
tasks.withType(JavaCompile) {
|
||||
options.compilerArgs.add '-XDstringConcat=inline'
|
||||
}
|
||||
|
||||
task(replayWatch, type: JavaExec) {
|
||||
mainClass = "org.littletonrobotics.junction.ReplayWatch"
|
||||
classpath = sourceSets.main.runtimeClasspath
|
||||
}
|
||||
|
||||
// Create version file
|
||||
project.compileJava.dependsOn(createVersionFile)
|
||||
gversion {
|
||||
srcDir = "src/main/java/"
|
||||
classPackage = "frc4388.robot.constants"
|
||||
className = "BuildConstants"
|
||||
dateFormat = "yyyy-MM-dd HH:mm:ss z"
|
||||
timeZone = "America/Denver"
|
||||
indent = " "
|
||||
}
|
||||
|
||||
@@ -1,4 +1,9 @@
|
||||
{
|
||||
"System Joysticks": {
|
||||
"window": {
|
||||
"enabled": false
|
||||
}
|
||||
},
|
||||
"keyboardJoysticks": [
|
||||
{
|
||||
"axisConfig": [
|
||||
@@ -88,5 +93,20 @@
|
||||
"buttonCount": 0,
|
||||
"povCount": 0
|
||||
}
|
||||
],
|
||||
"robotJoysticks": [
|
||||
{
|
||||
"guid": "030000005e040000ea0200000b050000",
|
||||
"useGamepad": true
|
||||
},
|
||||
{
|
||||
"useGamepad": true
|
||||
},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{
|
||||
"useGamepad": true
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -1,156 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import frc4388.utility.CanDevice;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
* constants. This class should not be used for any other purpose. All constants should be
|
||||
* declared globally (i.e. public static). Do not put anything functional in this class.
|
||||
*
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final String CANBUS_NAME = "rio";
|
||||
|
||||
public static final class SwerveDriveConstants {
|
||||
|
||||
public static final double MAX_ROT_SPEED = 3.5;
|
||||
public static final double AUTO_MAX_ROT_SPEED = 1.5;
|
||||
public static final double MIN_ROT_SPEED = 1.0;
|
||||
public static double ROTATION_SPEED = MAX_ROT_SPEED;
|
||||
public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
|
||||
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
|
||||
|
||||
public static final double CORRECTION_MIN = 10;
|
||||
public static final double CORRECTION_MAX = 50;
|
||||
|
||||
public static final double[] GEARS = {0.25, 0.5, 1.0};
|
||||
|
||||
public static final double SLOW_SPEED = 0.25;
|
||||
public static final double FAST_SPEED = 0.5;
|
||||
public static final double TURBO_SPEED = 1.0;
|
||||
|
||||
// public static List<CanDevice> CAN_DEVICES = new ArrayList<>();
|
||||
|
||||
public static final class DefaultSwerveRotOffsets {
|
||||
public static final double FRONT_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double BACK_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double BACK_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
}
|
||||
|
||||
public static final class IDs {
|
||||
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 2);
|
||||
public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 3);
|
||||
public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 10);
|
||||
|
||||
public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 4);
|
||||
public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 5);
|
||||
public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 11);
|
||||
|
||||
public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6);
|
||||
public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7);
|
||||
public static final CanDevice LEFT_BACK_ENCODER = new CanDevice("LEFT_BACK_ENCODER", 12);
|
||||
|
||||
public static final CanDevice RIGHT_BACK_WHEEL = new CanDevice("RIGHT_BACK_WHEEL", 8);
|
||||
public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9);
|
||||
public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13);
|
||||
|
||||
public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 4);
|
||||
public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50);
|
||||
}
|
||||
|
||||
public static final class PIDConstants {
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0);
|
||||
|
||||
public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
|
||||
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
|
||||
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
|
||||
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
|
||||
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
|
||||
|
||||
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
|
||||
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
|
||||
}
|
||||
|
||||
public static final class Conversions {
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5;
|
||||
|
||||
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
|
||||
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
|
||||
|
||||
public static final double TICKS_PER_MOTOR_REV = 0.5;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 3.9;
|
||||
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
|
||||
|
||||
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
|
||||
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
|
||||
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
|
||||
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
|
||||
|
||||
public static final double TICK_TIME_TO_SECONDS = 10;
|
||||
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
|
||||
public static final double CLOSED_LOOP_RAMP_RATE = 0.2;
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
}
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = 20.4;
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
|
||||
|
||||
// dimensions
|
||||
public static final double WIDTH = 18.5;
|
||||
public static final double HEIGHT = 18.5;
|
||||
public static final double HALF_WIDTH = WIDTH / 2.d;
|
||||
public static final double HALF_HEIGHT = HEIGHT / 2.d;
|
||||
|
||||
// misc
|
||||
public static final int TIMEOUT_MS = 30;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
|
||||
public static final class VisionConstants {
|
||||
}
|
||||
|
||||
public static final class DriveConstants {
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
|
||||
public static final class LEDConstants {
|
||||
public static final int LED_SPARK_ID = 0;
|
||||
|
||||
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final int XBOX_PROGRAMMER_ID = 2;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
|
||||
}
|
||||
}
|
||||
@@ -7,26 +7,24 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Base64;
|
||||
import java.util.List;
|
||||
import java.util.logging.Level;
|
||||
// Advantagekit
|
||||
import org.littletonrobotics.junction.LogFileUtil;
|
||||
import org.littletonrobotics.junction.LoggedRobot;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
import org.littletonrobotics.junction.networktables.NT4Publisher;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
import com.ctre.phoenix6.CANBus.CANBusStatus;
|
||||
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.utility.CanDevice;
|
||||
import frc4388.robot.constants.BuildConstants;
|
||||
import frc4388.robot.constants.Constants.SimConstants;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.RobotTime;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.Report;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
import frc4388.utility.compute.RobotTime;
|
||||
import frc4388.utility.compute.Trim;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
|
||||
//import frc4388.robot.subsystems.LED;
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
@@ -35,7 +33,7 @@ import frc4388.utility.Status.ReportLevel;
|
||||
* creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
public class Robot extends LoggedRobot {
|
||||
Command m_autonomousCommand;
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
@@ -47,32 +45,20 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// Start logging with AdvantageKit
|
||||
startLogging();
|
||||
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
|
||||
|
||||
|
||||
new Thread() {
|
||||
public void run() {
|
||||
try{
|
||||
while(!this.isInterrupted() && this.isAlive()){
|
||||
Thread.sleep(500);
|
||||
for(int i=0;i<Subsystem.subsystems.size(); i++){
|
||||
Subsystem.subsystems.get(i).queryStatus();
|
||||
}
|
||||
|
||||
System.out.println("Updated statuses!");
|
||||
|
||||
}
|
||||
}catch(Exception e){
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}.start();
|
||||
// // Create a shuffleboard update thread, that will periodically update the values on shuffleboard
|
||||
// FaultReporter.startThread();
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* This function is called every robot packet, no matter the mode. Use
|
||||
* this for items like diagnostics that you want ran during disabled,
|
||||
@@ -84,15 +70,15 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
m_robotTime.updateTimes();
|
||||
//System.out.println(m_robotContainer.limelight.isNearSpeaker());
|
||||
//mled.updateLED();
|
||||
// SmartDashboard.putNumber("Time", System.currentTimeMillis());
|
||||
|
||||
m_robotContainer.m_robotLED.update();
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode.
|
||||
* You can use it to reset any subsystem information you want to clear when
|
||||
@@ -140,14 +126,20 @@ public class Robot extends TimedRobot {
|
||||
public void autonomousPeriodic() {
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.stop();
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
|
||||
if (m_autonomousCommand != null) {
|
||||
CommandScheduler.getInstance().cancel(m_autonomousCommand);
|
||||
m_autonomousCommand.cancel();
|
||||
m_autonomousCommand.end(true);
|
||||
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
}
|
||||
@@ -160,72 +152,113 @@ public class Robot extends TimedRobot {
|
||||
// m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
@Override
|
||||
public void teleopExit() { // the only OTHER mode that teleop can enter into is disabled.
|
||||
Trim.dumpAll();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
|
||||
List<String> errors = new ArrayList<>();
|
||||
|
||||
// Subsystems header
|
||||
System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY")));
|
||||
|
||||
for(int i=0;i< Subsystem.subsystems.size();i++){
|
||||
|
||||
Subsystem subsystem = Subsystem.subsystems.get(i);
|
||||
System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":");
|
||||
Status status = subsystem.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(a);
|
||||
if(r.reportLevel == ReportLevel.ERROR)
|
||||
errors.add(subsystem.getName() + " - " + r.toString());
|
||||
subsystem.Log(r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// CAN header
|
||||
System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWliDilpfiloTilpYg4paX4paWICDilpfilpYK4paQ4paMICAg4paQ4paMIOKWkOKWjOKWkOKWm+KWmuKWluKWkOKWjArilpDilowgICDilpDilpviloDilpzilozilpDilowg4pad4pac4paMCuKWneKWmuKWhOKWhOKWluKWkOKWjCDilpDilozilpDilowgIOKWkOKWjCh0KQ==")));
|
||||
|
||||
CANBus canBus = new CANBus(Constants.CANBUS_NAME);
|
||||
|
||||
CANBusStatus canInfo = canBus.getStatus();
|
||||
|
||||
System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount);
|
||||
System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization);
|
||||
System.out.println("CANInfo RX Errors count - " + canInfo.REC);
|
||||
System.out.println("CANInfo TX Errors count - " + canInfo.TEC);
|
||||
System.out.println("CANInfo Transmit buffer full count - " + canInfo.TxFullCount);
|
||||
// Broken turniary operator
|
||||
ReportLevel canReportLevel = canInfo.Status.isOK() ? (canInfo.Status.isWarning() ? ReportLevel.WARNING : ReportLevel.ERROR) : ReportLevel.INFO;
|
||||
String canStatus = "CAN " + canReportLevel.name() + " - " + canInfo.Status.getName() + " (" + canInfo.Status.getDescription() + ")";
|
||||
if(canReportLevel == ReportLevel.ERROR) {
|
||||
errors.add(canStatus);
|
||||
}
|
||||
System.out.println(canStatus);
|
||||
|
||||
for(int i=0;i<CanDevice.devices.size();i++){
|
||||
|
||||
CanDevice device = CanDevice.devices.get(i);
|
||||
System.out.println("** CAN diagnostic report for " + device.name + ":");
|
||||
Status status = device.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(a);
|
||||
if(r.reportLevel == ReportLevel.ERROR)
|
||||
errors.add(device.getName() + " - " + r.toString());
|
||||
device.Log(r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
// System.out.println("Found CAN devices: " + new DeviceFinder().Find());
|
||||
|
||||
if(errors.size() > 0) {
|
||||
// Errors header
|
||||
System.out.println(new String(Base64.getDecoder().decode("4paX4paE4paE4paE4paW4paX4paE4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilpDilowgICAK4paQ4pab4paA4paA4paY4paQ4pab4paA4paa4paW4paQ4pab4paA4paa4paW4paQ4paMIOKWkOKWjOKWkOKWm+KWgOKWmuKWliDilp3iloDilprilpYK4paQ4paZ4paE4paE4paW4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilp3ilpriloTilp7ilpjilpDilowg4paQ4paM4paX4paE4paE4pae4paY")));
|
||||
for(int i=0;i<errors.size(); i++){
|
||||
System.out.println(errors.get(i));
|
||||
}
|
||||
}
|
||||
|
||||
FaultReporter.printReport();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// VisionSystemSim visionSim;
|
||||
// RobotMapSim robotMapSim;
|
||||
|
||||
// @Override
|
||||
// public void simulationInit() {
|
||||
// visionSim = new VisionSystemSim("main");
|
||||
// robotMapSim = m_robotContainer.m_robotMap.configureSim();
|
||||
|
||||
|
||||
// // A 0.5 x 0.25 meter rectangular target
|
||||
// TargetModel targetModel = new TargetModel(0.5, 0.25);
|
||||
// // The pose of where the target is on the field.
|
||||
// // Its rotation determines where "forward" or the target x-axis points.
|
||||
// // Let's say this target is flat against the far wall center, facing the blue driver stations.
|
||||
// Pose3d targetPose = new Pose3d(16, 4, 2, new Rotation3d(0, 0, Math.PI));
|
||||
// // The given target model at the given pose
|
||||
// VisionTargetSim visionTarget = new VisionTargetSim(targetPose, targetModel);
|
||||
|
||||
// // Add this vision target to the vision system simulation to make it visible
|
||||
// visionSim.addVisionTargets(visionTarget);
|
||||
|
||||
// // The layout of AprilTags which we want to add to the vision system
|
||||
// AprilTagFieldLayout tagLayout = Constants.FieldConstants.kTagLayout;
|
||||
|
||||
// visionSim.addAprilTags(tagLayout);
|
||||
|
||||
|
||||
// visionSim.addCamera(robotMapSim.leftCamera, Constants.VisionConstants.LEFT_CAMERA_POS);
|
||||
// visionSim.addCamera(robotMapSim.rightCamera, Constants.VisionConstants.RIGHT_CAMERA_POS);
|
||||
|
||||
// SmartDashboard.putData(visionSim.getDebugField());
|
||||
// }
|
||||
|
||||
|
||||
// @Override
|
||||
// public void simulationPeriodic() {
|
||||
// m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
|
||||
// // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
|
||||
|
||||
// // m_robotContainer.m_robotSwerveDrive.
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// Start AdvantageKit logging / replay and record metadata
|
||||
// Refrence: https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/template_projects/sources/skeleton/src/main/java/frc/robot/Robot.java
|
||||
public void startLogging() {
|
||||
// Record metadata
|
||||
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
|
||||
Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
|
||||
Logger.recordMetadata("Git+SHA", BuildConstants.GIT_SHA);
|
||||
Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
|
||||
Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
|
||||
switch (BuildConstants.DIRTY) {
|
||||
case 0:
|
||||
Logger.recordMetadata("GitDirty", "All changes committed");
|
||||
break;
|
||||
case 1:
|
||||
Logger.recordMetadata("GitDirty", "Uncomitted changes");
|
||||
break;
|
||||
default:
|
||||
Logger.recordMetadata("GitDirty", "Unknown");
|
||||
break;
|
||||
}
|
||||
|
||||
// Set up data receivers & replay source
|
||||
switch (SimConstants.currentMode) {
|
||||
case REAL:
|
||||
// Running on a real robot, log to a USB stick ("/U/logs")
|
||||
Logger.addDataReceiver(new WPILOGWriter());
|
||||
Logger.addDataReceiver(new NT4Publisher());
|
||||
break;
|
||||
|
||||
case SIM:
|
||||
// Running a physics simulator, log to NT
|
||||
Logger.addDataReceiver(new NT4Publisher());
|
||||
break;
|
||||
|
||||
case REPLAY:
|
||||
// Replaying a log, set up replay source
|
||||
setUseTiming(false); // Run as fast as possible
|
||||
String logPath = LogFileUtil.findReplayLog();
|
||||
Logger.setReplaySource(new WPILOGReader(logPath));
|
||||
Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
|
||||
break;
|
||||
}
|
||||
|
||||
// Start AdvantageKit logger
|
||||
Logger.start();
|
||||
}
|
||||
}
|
||||
@@ -10,177 +10,102 @@ package frc4388.robot;
|
||||
// Drive Systems
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import java.io.File;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
|
||||
// Commands
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
|
||||
// Autos
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
|
||||
import frc4388.robot.commands.MoveForTimeCommand;
|
||||
import frc4388.robot.commands.MoveUntilSuply;
|
||||
import frc4388.robot.commands.alignment.DriveToReef;
|
||||
import frc4388.robot.commands.alignment.DriveUntilLiDAR;
|
||||
import frc4388.robot.commands.alignment.LidarAlign;
|
||||
import frc4388.robot.commands.wait.waitElevatorRefrence;
|
||||
import frc4388.robot.commands.wait.waitEndefectorRefrence;
|
||||
import frc4388.robot.commands.wait.waitFeedCoral;
|
||||
import frc4388.robot.commands.wait.waitSupplier;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.robot.constants.Constants.OIConstants;
|
||||
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
||||
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
|
||||
// Subsystems
|
||||
// import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
// Utilites
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.configurable.ConfigurableString;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.compute.ReefPositionHelper.Side;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* scheduler calls). Instead, the structure of the robot (2including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
/* RobotMap */
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
|
||||
|
||||
/* Subsystems */
|
||||
// private final LED m_robotLED = new LED();
|
||||
public final LED m_robotLED = new LED();
|
||||
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
|
||||
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
||||
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
|
||||
|
||||
public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
|
||||
public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
|
||||
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
|
||||
m_robotMap.rightFront,
|
||||
m_robotMap.leftBack,
|
||||
m_robotMap.rightBack,
|
||||
|
||||
m_robotMap.gyro);
|
||||
|
||||
/* Controllers */
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
|
||||
|
||||
/* Virtual Controllers */
|
||||
private final VirtualController m_virtualDriver = new VirtualController(0);
|
||||
private final VirtualController m_virtualOperator = new VirtualController(1);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
|
||||
|
||||
// public List<Subsystem> subsystems = new ArrayList<>();
|
||||
|
||||
// ! Teleop Commands
|
||||
public void stop() {
|
||||
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
||||
m_robotSwerveDrive.stopModules();
|
||||
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
||||
}
|
||||
|
||||
// ! /* Autos */
|
||||
private String lastAutoName = "defualt.auto";
|
||||
private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
|
||||
private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
() -> autoplaybackName.get(), // lastAutoName
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false);
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
configureVirtualButtonBindings();
|
||||
new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
// CameraServer.startAutomaticCapture();
|
||||
private SendableChooser<String> autoChooser;
|
||||
private Command autoCommand;
|
||||
|
||||
/* Default Commands */
|
||||
// ! Swerve Drive Default Command (Regular Rotation)
|
||||
// drives the robot with a two-axis input from the driver controller
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),
|
||||
true);
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
|
||||
// this.subsystems.add(m_robotSwerveDrive);
|
||||
// this.subsystems.add(m_robotMap.leftFront);
|
||||
// this.subsystems.add(m_robotMap.rightFront);
|
||||
// this.subsystems.add(m_robotMap.rightBack);
|
||||
// this.subsystems.add(m_robotMap.leftBack);
|
||||
|
||||
// ! Swerve Drive One Module Test
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());
|
||||
// }
|
||||
|
||||
// ! Swerve Drive Default Command (Orientation Rotation)
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(),
|
||||
// getDeadbandedDriverController().getRightX(),
|
||||
// getDeadbandedDriverController().getRightY(),
|
||||
// true);
|
||||
// }, m_robotSwerveDrive))
|
||||
// .withName("SwerveDrive OrientationCommand"));
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotSwerveDrive.driveWithInput(
|
||||
// getDeadbandedDriverController().getLeft(),
|
||||
// getDeadbandedDriverController().getRight(),
|
||||
// true);
|
||||
// }, m_robotSwerveDrive));
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
|
||||
// ? /* Driver Buttons */
|
||||
|
||||
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip()));
|
||||
|
||||
// ! /* Speed */
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
// ? /* Programer Buttons (Controller 3)*/
|
||||
|
||||
// * /* Auto Recording */
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
|
||||
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
|
||||
() -> autoplaybackName.get()))
|
||||
.onFalse(new InstantCommand());
|
||||
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
() -> autoplaybackName.get(),
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false))
|
||||
.onFalse(new InstantCommand());
|
||||
}
|
||||
|
||||
/**
|
||||
* This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}. <p/>
|
||||
@@ -214,7 +139,62 @@ public class RobotContainer {
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
return autoPlayback;
|
||||
|
||||
|
||||
//return autoPlayback;
|
||||
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
|
||||
//return autoChooser.getSelected();
|
||||
// try{
|
||||
// // // Load the path you want to follow using its name in the GUI
|
||||
// return autoCommand;
|
||||
// } catch (Exception e) {
|
||||
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
|
||||
return autoCommand;
|
||||
// }
|
||||
// return new PathPlannerAuto("Line-up-no-arm");
|
||||
// zach told me to do the below comment
|
||||
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
|
||||
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
|
||||
}
|
||||
|
||||
public boolean autoChooserUpdated = false;
|
||||
public void makeAutoChooser() {
|
||||
autoChooser = new SendableChooser<String>();
|
||||
|
||||
File dir;
|
||||
|
||||
if(RobotBase.isReal()) {
|
||||
dir = new File("/home/lvuser/deploy/pathplanner/autos/");
|
||||
} else {
|
||||
// dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
|
||||
dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos");
|
||||
}
|
||||
|
||||
String[] autos = dir.list();
|
||||
|
||||
if(autos == null) return;
|
||||
|
||||
for (String auto : autos) {
|
||||
if (auto.endsWith(".auto"))
|
||||
autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
|
||||
// System.out.println(auto);
|
||||
}
|
||||
|
||||
autoChooser.onChange((filename) -> {
|
||||
autoChooserUpdated = true;
|
||||
if (filename.equals("Taxi")) {
|
||||
autoCommand = new SequentialCommandGroup(
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0, -1),
|
||||
new Translation2d(), 1000, true
|
||||
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
|
||||
} else {
|
||||
autoCommand = new PathPlannerAuto(filename);
|
||||
}
|
||||
System.out.println("Robot Auto Changed " + filename);
|
||||
});
|
||||
// SmartDashboard.putData(autoChooser);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -235,11 +215,7 @@ public class RobotContainer {
|
||||
return this.m_operatorXbox;
|
||||
}
|
||||
|
||||
public VirtualController getVirtualDriverController() {
|
||||
return m_virtualDriver;
|
||||
}
|
||||
|
||||
public VirtualController getVirtualOperatorController() {
|
||||
return m_virtualOperator;
|
||||
public ButtonBox getButtonBox() {
|
||||
return this.m_buttonBox;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -8,58 +8,168 @@
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
|
||||
// import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
// import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveModule;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.simulation.PhotonCameraSim;
|
||||
import org.photonvision.simulation.SimCameraProperties;
|
||||
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import frc4388.robot.constants.Constants.ElevatorConstants;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.robot.constants.Constants.SimConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
import frc4388.robot.subsystems.elevator.ElevatorIO;
|
||||
import frc4388.robot.subsystems.elevator.ElevatorReal;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
import frc4388.robot.subsystems.lidar.LidarIO;
|
||||
import frc4388.robot.subsystems.lidar.LidarReal;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.swerve.SwerveIO;
|
||||
import frc4388.robot.subsystems.swerve.SwerveReal;
|
||||
import frc4388.robot.subsystems.vision.VisionIO;
|
||||
import frc4388.robot.subsystems.vision.VisionReal;
|
||||
import frc4388.utility.status.FaultCANCoder;
|
||||
import frc4388.utility.status.FaultPhotonCamera;
|
||||
import frc4388.utility.status.FaultPidgeon2;
|
||||
import frc4388.utility.status.FaultTalonFX;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
* testing and modularization.
|
||||
*/
|
||||
public class RobotMap {
|
||||
private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
|
||||
public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule rightFront;
|
||||
public SwerveModule leftBack;
|
||||
public SwerveModule rightBack;
|
||||
// private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
|
||||
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
public final VisionIO leftCamera;
|
||||
public final VisionIO rightCamera;
|
||||
|
||||
// public final LiDAR lidar = new
|
||||
|
||||
public final LidarIO reefLidar;
|
||||
public final LidarIO reverseLidar;
|
||||
|
||||
public RobotMap() {
|
||||
configureDriveMotorControllers();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
|
||||
/* Swreve Drive Subsystem */
|
||||
public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL.id);
|
||||
public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER.id);
|
||||
public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER.id);
|
||||
public final SwerveIO swerveDrivetrain;
|
||||
|
||||
/* Elevator Subsystem */
|
||||
public final ElevatorIO elevatorIO;
|
||||
|
||||
public RobotMap(SimConstants.Mode mode) {
|
||||
switch (mode) {
|
||||
case REAL:
|
||||
// Configure cameras
|
||||
PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
|
||||
PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
|
||||
|
||||
leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
|
||||
rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
|
||||
|
||||
FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
|
||||
FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
|
||||
|
||||
// Configure LiDAR
|
||||
reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
|
||||
reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
|
||||
|
||||
// Configure swerve drive train
|
||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
|
||||
SwerveDriveConstants.DrivetrainConstants,
|
||||
SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
|
||||
SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
|
||||
);
|
||||
|
||||
swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
|
||||
|
||||
// Configure elevator
|
||||
|
||||
TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
|
||||
TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
|
||||
|
||||
|
||||
DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
|
||||
DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
|
||||
DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
|
||||
|
||||
elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
|
||||
|
||||
|
||||
public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL.id);
|
||||
public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER.id);
|
||||
public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER.id);
|
||||
|
||||
public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL.id);
|
||||
public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER.id);
|
||||
public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER.id);
|
||||
|
||||
public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL.id);
|
||||
public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER.id);
|
||||
public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER.id);
|
||||
|
||||
void configureDriveMotorControllers() {
|
||||
// initialize SwerveModules
|
||||
this.rightFront = new SwerveModule("Right Front Swerve Module", rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
|
||||
this.leftFront = new SwerveModule("Left Front Swerve Module", leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
|
||||
this.leftBack = new SwerveModule("Left Back Swerve Module", leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
|
||||
this.rightBack = new SwerveModule("Right Back Swerve Module", rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
|
||||
// Fault
|
||||
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
|
||||
|
||||
FaultTalonFX.addDevice(elevator, "Elevator");
|
||||
FaultTalonFX.addDevice(endeffector, "Endeffector");
|
||||
|
||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
|
||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
|
||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
|
||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
|
||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
|
||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
|
||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
|
||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
|
||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
|
||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
|
||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
|
||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
|
||||
|
||||
break;
|
||||
// case SIM:
|
||||
// break;
|
||||
default:
|
||||
leftCamera = new VisionIO() {};
|
||||
rightCamera = new VisionIO() {};
|
||||
reefLidar = new LidarIO() {};
|
||||
reverseLidar = new LidarIO() {};
|
||||
swerveDrivetrain = new SwerveIO() {};
|
||||
elevatorIO = new ElevatorIO() {};
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// public class RobotMapSim {
|
||||
// public PhotonCameraSim leftCamera;
|
||||
// public PhotonCameraSim rightCamera;
|
||||
// }
|
||||
|
||||
// public RobotMapSim configureSim() {
|
||||
// RobotMapSim sim = new RobotMapSim();
|
||||
|
||||
// // The simulated camera properties
|
||||
// SimCameraProperties cameraProp = new SimCameraProperties();
|
||||
// // A 640 x 480 camera with a 100 degree diagonal FOV.
|
||||
// cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100));
|
||||
// // Approximate detection noise with average and standard deviation error in pixels.
|
||||
// cameraProp.setCalibError(0.25, 0.08);
|
||||
// // Set the camera image capture framerate (Note: this is limited by robot loop rate).
|
||||
// cameraProp.setFPS(20);
|
||||
// // The average and standard deviation in milliseconds of image data latency.
|
||||
// cameraProp.setAvgLatencyMs(35);
|
||||
// cameraProp.setLatencyStdDevMs(5);
|
||||
|
||||
// // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
|
||||
// // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
|
||||
|
||||
|
||||
// sim.leftCamera.enableRawStream(true);
|
||||
// sim.leftCamera.enableProcessedStream(true);
|
||||
// sim.leftCamera.enableDrawWireframe(true);
|
||||
|
||||
|
||||
// sim.rightCamera.enableRawStream(true);
|
||||
// sim.rightCamera.enableProcessedStream(true);
|
||||
// sim.rightCamera.enableDrawWireframe(true);
|
||||
|
||||
// return sim;
|
||||
|
||||
// }
|
||||
|
||||
}
|
||||
@@ -1,225 +0,0 @@
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,12
|
||||
0.0,0.0,0.0,0.0,26
|
||||
0.0,0.0,0.0,0.0,37
|
||||
0.0,0.0,0.0,0.0,50
|
||||
0.0,0.0,0.0,0.0,62
|
||||
0.0,0.0,0.0,0.0,73
|
||||
0.0,0.0,0.0,0.0,88
|
||||
0.0,0.0,0.0,0.0,103
|
||||
0.0,0.0,0.0,0.0,116
|
||||
0.0,0.0,0.0,0.0,160
|
||||
0.0,0.0,0.0,0.0,173
|
||||
0.0,0.0,0.0,0.0,185
|
||||
0.0,0.0,0.0,0.0,200
|
||||
0.0,0.0,0.0,0.0,211
|
||||
0.0,0.0,0.0,0.0,223
|
||||
0.0,0.0,0.0,0.0,235
|
||||
0.0,0.0,0.0,0.0,247
|
||||
0.0,0.0,0.0,0.0,263
|
||||
0.0,0.0,0.0,0.0,283
|
||||
0.0,0.0,0.0,0.0,303
|
||||
0.0,-0.109375,0.0,0.0,323
|
||||
0.0,-0.1484375,0.0,0.0,343
|
||||
0.0,-0.2109375,0.0,0.0,363
|
||||
0.0,-0.3671875,0.0,0.0,398
|
||||
0.0,-0.4140625,0.0,0.0,411
|
||||
0.0,-0.4765625,0.0,0.0,425
|
||||
0.0,-0.5078125,0.0,0.0,443
|
||||
0.0,-0.5078125,0.0,0.0,463
|
||||
0.0,-0.53125,0.0,0.0,483
|
||||
0.0,-0.5546875,0.0,0.0,504
|
||||
0.0,-0.5625,0.0,0.0,523
|
||||
0.0,-0.5625,0.0,0.0,544
|
||||
0.0,-0.5703125,0.0,0.0,563
|
||||
0.0,-0.5859375,0.0,0.0,584
|
||||
0.0,-0.5859375,0.0,0.0,603
|
||||
0.0,-0.5859375,0.0,0.0,640
|
||||
0.0,-0.59375,0.0,0.0,657
|
||||
0.0,-0.6015625,0.0,0.0,672
|
||||
0.0,-0.6015625,0.0,0.0,685
|
||||
0.0,-0.6015625,0.0,0.0,703
|
||||
0.0,-0.6015625,0.0,0.0,723
|
||||
0.0,-0.6015625,0.0,0.0,743
|
||||
0.0,-0.6015625,0.0,0.0,763
|
||||
0.0,-0.6015625,0.0,0.0,783
|
||||
0.0,-0.6015625,0.0,0.0,803
|
||||
0.0,-0.6015625,0.0,0.0,823
|
||||
0.0,-0.6015625,0.0,0.0,844
|
||||
0.0,-0.6015625,0.0,0.0,878
|
||||
0.0,-0.6015625,0.0,0.0,893
|
||||
0.0,-0.6015625,0.0,0.0,907
|
||||
0.0,-0.6015625,0.0,0.0,924
|
||||
0.0,-0.609375,0.0,0.0,943
|
||||
0.0,-0.609375,0.0,0.0,963
|
||||
0.0,-0.609375,0.0,0.0,983
|
||||
0.0,-0.609375,0.0,0.0,1004
|
||||
0.0,-0.609375,0.0,0.0,1023
|
||||
0.0,-0.609375,0.0,0.0,1043
|
||||
0.0,-0.609375,0.0,0.0,1064
|
||||
0.0,-0.609375,0.0,0.0,1083
|
||||
0.0,-0.609375,0.0,0.0,1156
|
||||
0.0,-0.609375,0.0,0.0,1172
|
||||
0.0,-0.609375,0.0,0.0,1185
|
||||
0.0,-0.609375,0.0,0.0,1200
|
||||
0.0,-0.609375,0.0,0.0,1215
|
||||
0.0,-0.609375,0.0,0.0,1225
|
||||
0.0,-0.609375,0.0,0.0,1236
|
||||
0.0,-0.609375,0.0,0.0,1249
|
||||
0.0,-0.609375,0.0,0.0,1263
|
||||
0.0,-0.609375,0.0,0.0,1283
|
||||
0.0,-0.609375,0.0,0.0,1303
|
||||
0.0,-0.609375,0.0,0.0,1323
|
||||
0.0,-0.609375,0.0,0.0,1363
|
||||
0.0,-0.6015625,0.0,0.0,1376
|
||||
0.0,-0.6015625,0.0,0.0,1394
|
||||
0.0,-0.6015625,0.0,0.0,1405
|
||||
0.0,-0.6015625,0.0,0.0,1423
|
||||
0.0,-0.6015625,0.0,0.0,1443
|
||||
0.0,-0.6015625,0.0,0.0,1463
|
||||
0.0,-0.6015625,0.0,0.0,1483
|
||||
0.0,-0.6015625,0.0,0.0,1503
|
||||
0.0,-0.6015625,0.0,0.0,1523
|
||||
0.0,-0.6015625,0.0,0.0,1543
|
||||
0.0,-0.6015625,0.0,0.0,1563
|
||||
0.0,-0.6015625,0.0,0.0,1597
|
||||
0.0,-0.6015625,0.0,0.0,1608
|
||||
0.0,-0.6015625,0.0,0.0,1624
|
||||
0.0,-0.6015625,0.0,0.0,1643
|
||||
0.0,-0.6015625,0.0,0.0,1664
|
||||
0.0,-0.5859375,0.0,0.0,1683
|
||||
0.0,-0.5859375,0.0,0.0,1703
|
||||
0.0,-0.5625,0.0,0.0,1723
|
||||
0.0,-0.5625,0.0,0.0,1743
|
||||
0.0,-0.5625,0.0,0.0,1763
|
||||
0.0,-0.5625,0.0,0.0,1783
|
||||
0.0,-0.5625,0.0,0.0,1803
|
||||
0.0,-0.5625,0.0,0.0,1843
|
||||
0.0,-0.5625,0.0,0.0,1855
|
||||
0.0,-0.5625,0.0,0.0,1868
|
||||
0.0,-0.5625,0.0,0.0,1883
|
||||
0.0,-0.5625,0.0,0.0,1903
|
||||
0.0,-0.5625,0.0,0.0,1923
|
||||
0.0,-0.5625,0.0,0.0,1943
|
||||
0.0,-0.5625,0.0,0.0,1963
|
||||
0.0,-0.5625,0.0,0.0,1983
|
||||
0.0,-0.5625,0.0,0.0,2003
|
||||
0.0,-0.5625,0.0,0.0,2024
|
||||
0.0,-0.5625,0.0,0.0,2043
|
||||
0.0,-0.5625,0.0,0.0,2081
|
||||
0.0,-0.5625,0.0,0.0,2093
|
||||
0.0,-0.5625,0.0,0.0,2105
|
||||
0.0,-0.5625,0.0,0.0,2123
|
||||
0.0,-0.5625,0.0,0.0,2143
|
||||
0.0,-0.5625,0.0,0.0,2163
|
||||
0.0,-0.5625,0.0,0.0,2183
|
||||
0.0,-0.5625,0.0,0.0,2203
|
||||
0.0,-0.5625,0.0,0.0,2223
|
||||
0.0,-0.5625,0.0,0.0,2243
|
||||
0.0,-0.5625,0.0,0.0,2263
|
||||
0.0,-0.5625,0.0,0.0,2283
|
||||
0.0,-0.5625,0.0,0.0,2366
|
||||
0.0,-0.5625,0.0,0.0,2377
|
||||
0.0,-0.5625,0.0,0.0,2394
|
||||
0.0,-0.5703125,0.0,0.0,2405
|
||||
0.0,-0.5703125,0.0,0.0,2418
|
||||
0.0,-0.5703125,0.0,0.0,2431
|
||||
0.0,-0.5703125,0.0,0.0,2444
|
||||
0.0,-0.5703125,0.0,0.0,2458
|
||||
0.0,-0.5703125,0.0,0.0,2470
|
||||
0.0,-0.5703125,0.0,0.0,2485
|
||||
0.0,-0.5703125,0.0,0.0,2503
|
||||
0.0,-0.5703125,0.0,0.0,2523
|
||||
0.0,-0.5703125,0.0,0.0,2563
|
||||
0.0,-0.5703125,0.0,0.0,2577
|
||||
0.0,-0.5703125,0.0,0.0,2591
|
||||
0.0,-0.5703125,0.0,0.0,2608
|
||||
0.0,-0.5703125,0.0,0.0,2624
|
||||
0.0,-0.5703125,0.0,0.0,2643
|
||||
0.0,-0.5703125,0.0,0.0,2677
|
||||
0.0,-0.5703125,0.0,0.0,2698
|
||||
0.0,-0.5703125,0.0,0.0,2711
|
||||
0.0,-0.5703125,0.0,0.0,2725
|
||||
0.0,-0.5703125,0.0,0.0,2743
|
||||
0.0,-0.5703125,0.0,0.0,2764
|
||||
0.0,-0.5703125,0.0,0.0,2810
|
||||
0.0,-0.5703125,0.0,0.0,2820
|
||||
0.0,-0.5703125,0.0,0.0,2833
|
||||
0.0,-0.5703125,0.0,0.0,2845
|
||||
0.0,-0.5703125,0.0,0.0,2863
|
||||
0.0,-0.5703125,0.0,0.0,2883
|
||||
0.0,-0.5703125,0.0,0.0,2904
|
||||
0.0,-0.5703125,0.0,0.0,2924
|
||||
0.0,-0.5703125,0.0,0.0,2943
|
||||
0.0,-0.5703125,0.0,0.0,2963
|
||||
0.0,-0.5703125,0.0,0.0,2983
|
||||
0.0,-0.5703125,0.0,0.0,3003
|
||||
0.0,-0.5703125,0.0,0.0,3033
|
||||
0.0,-0.5703125,0.0,0.0,3050
|
||||
0.0,-0.5703125,0.0,0.0,3065
|
||||
0.0,-0.5703125,0.0,0.0,3083
|
||||
0.0,-0.5703125,0.0,0.0,3103
|
||||
0.0,-0.5703125,0.0,0.0,3123
|
||||
0.0,-0.5703125,0.0,0.0,3144
|
||||
0.0,-0.5703125,0.0,0.0,3164
|
||||
0.0,-0.5703125,0.0,0.0,3184
|
||||
0.0,-0.5703125,0.0,0.0,3203
|
||||
0.0,-0.5703125,0.0,0.0,3223
|
||||
0.0,-0.5703125,0.0,0.0,3243
|
||||
0.0,-0.5703125,0.0,0.0,3272
|
||||
0.0,-0.5703125,0.0,0.0,3289
|
||||
0.0,-0.5703125,0.0,0.0,3303
|
||||
0.0,-0.5703125,0.0,0.0,3323
|
||||
0.0,-0.5703125,0.0,0.0,3343
|
||||
0.0,-0.5703125,0.0,0.0,3363
|
||||
0.0,-0.5703125,0.0,0.0,3383
|
||||
0.0,-0.5703125,0.0,0.0,3403
|
||||
0.0,-0.5703125,0.0,0.0,3423
|
||||
0.0,-0.5703125,0.0,0.0,3443
|
||||
0.0,-0.5703125,0.0,0.0,3463
|
||||
0.0,-0.5703125,0.0,0.0,3483
|
||||
0.0,-0.5703125,0.0,0.0,3566
|
||||
0.0,-0.5703125,0.0,0.0,3578
|
||||
0.0,-0.5703125,0.0,0.0,3596
|
||||
0.0,-0.5703125,0.0,0.0,3610
|
||||
0.0,-0.5703125,0.0,0.0,3623
|
||||
0.0,-0.5703125,0.0,0.0,3640
|
||||
0.0,-0.5703125,0.0,0.0,3651
|
||||
0.0,-0.5703125,0.0,0.0,3663
|
||||
0.0,-0.5703125,0.0,0.0,3678
|
||||
0.0,-0.5703125,0.0,0.0,3691
|
||||
0.0,-0.5703125,0.0,0.0,3706
|
||||
0.0,-0.5703125,0.0,0.0,3723
|
||||
0.0,-0.5703125,0.0,0.0,3766
|
||||
0.0,-0.5703125,0.0,0.0,3778
|
||||
0.0,-0.5703125,0.0,0.0,3792
|
||||
0.0,-0.5703125,0.0,0.0,3807
|
||||
0.0,-0.5703125,0.0,0.0,3823
|
||||
0.0,-0.5703125,0.0,0.0,3843
|
||||
0.0,-0.53125,0.0,0.0,3863
|
||||
0.0,-0.53125,0.0,0.0,3884
|
||||
0.0,-0.421875,0.0,0.0,3904
|
||||
0.0,0.0,0.0,0.0,3924
|
||||
0.0,0.0,0.0,0.0,3944
|
||||
0.0,0.0,0.0,0.0,3963
|
||||
0.0,0.0,0.0,0.0,3999
|
||||
0.0,0.0,0.0,0.0,4010
|
||||
0.0,0.0,0.0,0.0,4025
|
||||
0.0,0.0,0.0,0.0,4043
|
||||
0.0,0.0,0.0,0.0,4063
|
||||
0.0,0.0,0.0,0.0,4083
|
||||
0.0,0.0,0.0,0.0,4103
|
||||
0.0,0.0,0.0,0.0,4123
|
||||
0.0,0.0,0.0,0.0,4143
|
||||
0.0,0.0,0.0,0.0,4163
|
||||
0.0,0.0,0.0,0.0,4183
|
||||
0.0,0.0,0.0,0.0,4203
|
||||
0.0,0.0,0.0,0.0,4236
|
||||
0.0,0.0,0.0,0.0,4247
|
||||
0.0,0.0,0.0,0.0,4264
|
||||
0.0,0.0,0.0,0.0,4284
|
||||
0.0,0.0,0.0,0.0,4304
|
||||
0.0,0.0,0.0,0.0,4324
|
||||
0.0,0.0,0.0,0.0,4343
|
||||
0.0,0.0,0.0,0.0,4363
|
||||
@@ -1,3 +1,4 @@
|
||||
package frc4388.robot.commands.autos;
|
||||
// package frc4388.robot.commands.Autos;
|
||||
|
||||
// import java.io.File;
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.time.Instant;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class MoveForTimeCommand extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final long duration;
|
||||
private final boolean robotRelative;
|
||||
|
||||
private Instant startTime;
|
||||
|
||||
public MoveForTimeCommand(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
long millis,
|
||||
boolean robotRelative) {
|
||||
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.duration = millis;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
startTime = Instant.now();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class MoveUntilSuply extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final Supplier<Boolean> truth;
|
||||
private final boolean robotRelative;
|
||||
|
||||
public MoveUntilSuply(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
Supplier<Boolean> truth,
|
||||
boolean robotRelative) {
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.truth = truth;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return truth.get();
|
||||
}
|
||||
}
|
||||
@@ -5,7 +5,7 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.structs.Gains;
|
||||
|
||||
public abstract class PID extends Command {
|
||||
protected Gains gains;
|
||||
|
||||
@@ -2,11 +2,11 @@
|
||||
// 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.robot.commands.Swerve;
|
||||
package frc4388.robot.commands.swerve;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PID;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
public class RotateToAngle extends PID {
|
||||
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
package frc4388.robot.commands.swerve;
|
||||
|
||||
import java.io.FileInputStream;
|
||||
import java.util.ArrayList;
|
||||
@@ -6,11 +6,11 @@ import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.DataUtils;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.DataUtils;
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
|
||||
|
||||
|
||||
/**
|
||||
@@ -24,8 +24,8 @@ public class neoJoystickPlayback extends Command {
|
||||
private final Supplier<String> filenameGetter;
|
||||
private String filename;
|
||||
private int frame_index = 0;
|
||||
private long startTime = 0;
|
||||
private long playbackTime = 0;
|
||||
// private long startTime = 0;
|
||||
// private long playbackTime = 0;
|
||||
private boolean m_finished = false; // ! There is no better way.
|
||||
private boolean m_shouldfree = false; // should free memory on ending
|
||||
|
||||
@@ -150,8 +150,8 @@ public class neoJoystickPlayback extends Command {
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
startTime = System.currentTimeMillis();
|
||||
playbackTime = 0;
|
||||
// startTime = System.currentTimeMillis();
|
||||
// playbackTime = 0;
|
||||
frame_index = 0;
|
||||
|
||||
m_finished = !loadAuto();
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
package frc4388.robot.commands.swerve;
|
||||
|
||||
import java.io.FileOutputStream;
|
||||
import java.util.ArrayList;
|
||||
@@ -7,11 +7,11 @@ import java.util.function.Supplier;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.DataUtils;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.DataUtils;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
|
||||
|
||||
/**
|
||||
* The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s
|
||||
|
||||
@@ -0,0 +1,104 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
// 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.
|
||||
|
||||
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
/**
|
||||
* A command composition that runs one of two commands, depending on the value of the given
|
||||
* condition when this command is initialized.
|
||||
*
|
||||
* <p>The rules for command compositions apply: command instances that are passed to it cannot be
|
||||
* added to any other composition or scheduled individually, and the composition requires all
|
||||
* subsystems its components require.
|
||||
*
|
||||
* <p>This class is provided by the NewCommands VendorDep
|
||||
*/
|
||||
public class WhileTrueCommand extends Command {
|
||||
private final Command m_whileTrue;
|
||||
private final BooleanSupplier m_condition;
|
||||
|
||||
/**
|
||||
* Creates a new WhileTrueCommand.
|
||||
*
|
||||
* @param whileTrue the command to run while the condition is true
|
||||
* @param condition the condition to determine which command to run
|
||||
*/
|
||||
@SuppressWarnings("this-escape")
|
||||
public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
|
||||
m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
|
||||
m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
|
||||
|
||||
//CommandScheduler.getInstance().registerComposedCommands(whileTrue);
|
||||
|
||||
// addRequirements(whileTrue.getRequirements());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
if(m_condition.getAsBoolean())
|
||||
m_whileTrue.initialize();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_whileTrue.execute();
|
||||
|
||||
System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean());
|
||||
|
||||
if(!m_whileTrue.isFinished())
|
||||
return;
|
||||
|
||||
if(m_condition.getAsBoolean()){
|
||||
m_whileTrue.end(false);
|
||||
m_whileTrue.initialize();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_whileTrue.end(interrupted);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_whileTrue.runsWhenDisabled();
|
||||
}
|
||||
|
||||
@Override
|
||||
public InterruptionBehavior getInterruptionBehavior() {
|
||||
if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
|
||||
return InterruptionBehavior.kCancelSelf;
|
||||
} else {
|
||||
return InterruptionBehavior.kCancelIncoming;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
super.initSendable(builder);
|
||||
builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
|
||||
builder.addStringProperty(
|
||||
"selected",
|
||||
() -> {
|
||||
if (m_whileTrue == null) {
|
||||
return "null";
|
||||
} else {
|
||||
return m_whileTrue.getName();
|
||||
}
|
||||
},
|
||||
null);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,188 @@
|
||||
package frc4388.robot.commands.alignment;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.compute.ReefPositionHelper;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.compute.ReefPositionHelper.Side;
|
||||
import frc4388.utility.structs.Gains;
|
||||
|
||||
public class DriveToReef extends Command {
|
||||
|
||||
|
||||
// private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
|
||||
// private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
|
||||
|
||||
private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
|
||||
private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
|
||||
// private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
|
||||
private Pose2d targetpos;
|
||||
|
||||
SwerveDrive swerveDrive;
|
||||
Vision vision;
|
||||
double distance;
|
||||
Side side;
|
||||
boolean waitVelocityZero;
|
||||
|
||||
/**
|
||||
* Command to drive robot to position.
|
||||
* @param SwerveDrive m_robotSwerveDrive
|
||||
*/
|
||||
|
||||
public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.vision = vision;
|
||||
this.distance = distance;
|
||||
this.side = side;
|
||||
this.waitVelocityZero = waitVelocityZero && false;
|
||||
addRequirements(swerveDrive);
|
||||
}
|
||||
|
||||
|
||||
public static double tagRelativeXError = -1;
|
||||
private static void setTagRelativeXError(double val){
|
||||
tagRelativeXError = val;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
xPID.initialize();
|
||||
yPID.initialize();
|
||||
this.targetpos = ReefPositionHelper.getNearestPosition(
|
||||
this.vision.getPose2d(),
|
||||
side,
|
||||
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
|
||||
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
|
||||
);
|
||||
}
|
||||
|
||||
double xerr;
|
||||
double yerr;
|
||||
double roterr;
|
||||
|
||||
double xoutput;
|
||||
double youtput;
|
||||
double rotoutput;
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
|
||||
yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
|
||||
// xerr = targetpos.getX() - vision.getPose2d().getX();
|
||||
// yerr = targetpos.getX() - vision.getPose2d().getY();
|
||||
|
||||
// roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
|
||||
|
||||
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
|
||||
|
||||
if(roterr > 180){
|
||||
roterr -= 360;
|
||||
}else if(roterr < -180){
|
||||
roterr += 360;
|
||||
}
|
||||
|
||||
// SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
|
||||
// SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
|
||||
// SmartDashboard.putNumber("Rotational PID error", roterr);
|
||||
|
||||
SmartDashboard.putNumber("PID X Error", xerr);
|
||||
SmartDashboard.putNumber("PID Y Error", yerr);
|
||||
SmartDashboard.putNumber("PID Rot Error", roterr);
|
||||
|
||||
xoutput = xPID.update(xerr);
|
||||
youtput = yPID.update(yerr);
|
||||
// rotoutput = rotPID.update(roterr);
|
||||
|
||||
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
|
||||
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
|
||||
// rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
|
||||
|
||||
|
||||
|
||||
Translation2d leftStick = new Translation2d(
|
||||
Math.max(Math.min(-youtput, 1), -1),
|
||||
Math.max(Math.min(-xoutput, 1), -1)
|
||||
// 0,0
|
||||
);
|
||||
|
||||
// Translation2d rightStick = new Translation2d(
|
||||
// Math.max(Math.min(rotoutput, 1), -1),
|
||||
// 0
|
||||
// );
|
||||
|
||||
setTagRelativeXError(
|
||||
new Translation2d(xerr, yerr).getAngle()
|
||||
.rotateBy(targetpos.getRotation())
|
||||
.getCos());
|
||||
|
||||
swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
|
||||
// swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public final boolean isFinished() {
|
||||
boolean finished = (
|
||||
(Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
|
||||
(Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
|
||||
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
|
||||
(!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
|
||||
);
|
||||
// System.out.println(finished);
|
||||
|
||||
if(finished)
|
||||
swerveDrive.softStop();
|
||||
|
||||
return finished;
|
||||
// this statement is a boolean in and of itself
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private class PID {
|
||||
protected Gains gains;
|
||||
private double output = 0;
|
||||
|
||||
|
||||
/** Creates a new PelvicInflammatoryDisease. */
|
||||
public PID(Gains gains, double tolerance) {
|
||||
this.gains = gains;
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
public final void initialize() {
|
||||
output = 0;
|
||||
}
|
||||
|
||||
private double prevError, cumError = 0;
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
public double update(double error) {
|
||||
cumError += error * .02; // 20 ms
|
||||
double delta = error - prevError;
|
||||
|
||||
output = error * gains.kP;
|
||||
output += cumError * gains.kI;
|
||||
output += delta * gains.kD;
|
||||
output += gains.kF;
|
||||
|
||||
return output;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
package frc4388.robot.commands.alignment;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class DriveUntilLiDAR extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final LiDAR m_lidar;
|
||||
private final double mindistance;
|
||||
|
||||
public DriveUntilLiDAR(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
LiDAR lidar,
|
||||
double mindistance) {
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.m_lidar = lidar;
|
||||
this.mindistance = mindistance;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveFine(leftStick, rightStick, 0.3);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (Math.abs(m_lidar.getDistance()) < mindistance) {
|
||||
swerveDrive.softStop();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.alignment;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class LidarAlign extends Command {
|
||||
private SwerveDrive swerveDrive;
|
||||
private LiDAR lidar;
|
||||
|
||||
private int currentFinderTick;
|
||||
// private int tickFoundPipe;
|
||||
private boolean foundReef;
|
||||
private boolean headedRight;
|
||||
private double speed;
|
||||
private int bounces;
|
||||
private double additionalDistance = 0;
|
||||
// private final boolean constructedHeadedRight;
|
||||
|
||||
/** Creates a new LidarAlign. */
|
||||
public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.lidar = lidar;
|
||||
|
||||
addRequirements(swerveDrive, lidar);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
this.currentFinderTick = 0;
|
||||
this.speed = 0.4; // TODO: find good speed for this
|
||||
this.foundReef = false;
|
||||
this.headedRight = (DriveToReef.tagRelativeXError < 0);
|
||||
this.additionalDistance = 0;
|
||||
this.bounces = 0;
|
||||
}
|
||||
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
if (lidar.withinDistance()) {
|
||||
swerveDrive.softStop();
|
||||
foundReef = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
|
||||
headedRight = !headedRight;
|
||||
currentFinderTick *= -1;
|
||||
bounces++;
|
||||
additionalDistance += 5;
|
||||
if (bounces == 5) return;
|
||||
}
|
||||
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
|
||||
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
|
||||
SmartDashboard.putNumber("Rel Angle", relAngle);
|
||||
SmartDashboard.putNumber("heading", currentHeading);
|
||||
if (!headedRight) {
|
||||
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
|
||||
} else {
|
||||
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
|
||||
}
|
||||
|
||||
currentFinderTick++;
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (lidar.getDistance() < 4) {
|
||||
swerveDrive.stopModules();
|
||||
return true;
|
||||
} else if (foundReef && lidar.withinDistance()) { // spot on
|
||||
swerveDrive.stopModules();
|
||||
return true;
|
||||
} else if (foundReef && !lidar.withinDistance()) { // over shot
|
||||
speed = speed / 2;
|
||||
headedRight = !headedRight;
|
||||
currentFinderTick = 0;
|
||||
foundReef = false;
|
||||
return false;
|
||||
} else if (bounces >= 3) {
|
||||
swerveDrive.stopModules();
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitElevatorRefrence extends Command {
|
||||
/** Creates a new waitElevatorRefrence. */
|
||||
private Elevator elevator;
|
||||
public waitElevatorRefrence(Elevator elevator) {
|
||||
this.elevator = elevator;
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return elevator.elevatorAtReference();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitEndefectorRefrence extends Command {
|
||||
/** Creates a new waitElevatorRefrence. */
|
||||
private Elevator elevator;
|
||||
public waitEndefectorRefrence(Elevator elevator) {
|
||||
this.elevator = elevator;
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return elevator.endeffectorAtReference();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitFeedCoral extends Command {
|
||||
/** Creates a new waitElevatorRefrence. */
|
||||
private Elevator elevator;
|
||||
public waitFeedCoral(Elevator elevator) {
|
||||
this.elevator = elevator;
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return elevator.hasCoral();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.wait;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitSupplier extends Command {
|
||||
/** Creates a new waitSupplier. */
|
||||
private final Supplier<Boolean> truth;
|
||||
public waitSupplier(Supplier<Boolean> truth) {
|
||||
this.truth = truth;
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return truth.get();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
package frc4388.robot.constants;
|
||||
|
||||
/**
|
||||
* Automatically generated file containing build version information.
|
||||
*/
|
||||
public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2025RidgeScape";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 173;
|
||||
public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513";
|
||||
public static final String GIT_DATE = "2025-07-17 09:15:19 MDT";
|
||||
public static final String GIT_BRANCH = "advantagekit";
|
||||
public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752774931371L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
}
|
||||
@@ -0,0 +1,146 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.constants;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import frc4388.utility.compute.Trim;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
import frc4388.utility.structs.Gains;
|
||||
import frc4388.utility.structs.LEDPatterns;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
* constants. This class should not be used for any other purpose. All constants should be
|
||||
* declared globally (i.e. public static). Do not put anything functional in this class.
|
||||
*
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final String CANBUS_NAME = "rio";
|
||||
|
||||
public static final class LiDARConstants {
|
||||
public static final int REEF_LIDAR_DIO_CHANNEL = 7;
|
||||
public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
|
||||
|
||||
public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
|
||||
|
||||
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
|
||||
public static final int LIDAR_MICROS_TO_CM = 10;
|
||||
public static final int SECONDS_TO_MICROS = 1000000;
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
|
||||
public static final Gains XY_GAINS = new Gains(8,0,0.0);
|
||||
// public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
|
||||
// public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
|
||||
// public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
|
||||
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
|
||||
|
||||
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
|
||||
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
|
||||
|
||||
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
|
||||
// public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5);
|
||||
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
|
||||
|
||||
public static final double XY_TOLERANCE = 0.07; // Meters
|
||||
public static final double ROT_TOLERANCE = 5; // Degrees
|
||||
|
||||
public static final double MIN_XY_PID_OUTPUT = 0.0;
|
||||
public static final double MIN_ROT_PID_OUTPUT = 0.0;
|
||||
|
||||
public static final double VELOCITY_THRESHHOLD = 0.01;
|
||||
}
|
||||
|
||||
|
||||
public static final class VisionConstants {
|
||||
public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
|
||||
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
|
||||
|
||||
public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
|
||||
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
|
||||
|
||||
public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
|
||||
|
||||
// Photonvision thing
|
||||
// The standard deviations of our vision estimated poses, which affect correction rate
|
||||
// X, Y, Theta
|
||||
// https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
|
||||
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
|
||||
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
|
||||
}
|
||||
|
||||
public static final class FieldConstants {
|
||||
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
|
||||
|
||||
// Test april tag field layout
|
||||
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
|
||||
// Arrays.asList(new AprilTag[] {
|
||||
// new AprilTag(1, new Pose3d(
|
||||
// new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
|
||||
// )),
|
||||
// }), 100, 100);
|
||||
|
||||
}
|
||||
|
||||
public static final class LEDConstants {
|
||||
public static final int LED_SPARK_ID = 9;
|
||||
|
||||
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
|
||||
|
||||
public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
|
||||
public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
|
||||
public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
|
||||
public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
|
||||
|
||||
public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
|
||||
public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final int BUTTONBOX_ID = 2;
|
||||
public static final int XBOX_PROGRAMMER_ID = 3;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Logging constants
|
||||
public static class SimConstants {
|
||||
public static final Mode simMode = Mode.SIM;
|
||||
public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
|
||||
|
||||
public static enum Mode {
|
||||
/** Running on a real robot. */
|
||||
REAL,
|
||||
|
||||
/** Running a physics simulator. */
|
||||
SIM,
|
||||
|
||||
/** Replaying from a log file. */
|
||||
REPLAY
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,112 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.logging.Level;
|
||||
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import frc4388.utility.RobotTime;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class DiffDrive extends Subsystem {
|
||||
// Put methods for controlling this subsystem
|
||||
// here. Call these from Commands.
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private TalonFX m_leftFrontMotor;
|
||||
private TalonFX m_rightFrontMotor;
|
||||
private TalonFX m_leftBackMotor;
|
||||
private TalonFX m_rightBackMotor;
|
||||
private DifferentialDrive m_driveTrain;
|
||||
private RobotGyro m_gyro;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
|
||||
TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
|
||||
|
||||
super();
|
||||
|
||||
m_leftFrontMotor = leftFrontMotor;
|
||||
m_rightFrontMotor = rightFrontMotor;
|
||||
m_leftBackMotor = leftBackMotor;
|
||||
m_rightBackMotor = rightBackMotor;
|
||||
m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
|
||||
m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
|
||||
m_driveTrain = driveTrain;
|
||||
m_gyro = gyro;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
m_gyro.updatePigeonDeltas();
|
||||
|
||||
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
|
||||
updateSmartDashboard();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void driveWithInput(double move, double steer) {
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void tankDriveWithInput(double leftMove, double rightMove) {
|
||||
m_leftFrontMotor.set(leftMove);
|
||||
m_rightFrontMotor.set(rightMove);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
private void updateSmartDashboard() {
|
||||
|
||||
// Examples of the functionality of RobotGyro
|
||||
SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
|
||||
SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
|
||||
SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
|
||||
//SmartDashboard.putData(m_gyro);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Diff Drive";
|
||||
}
|
||||
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
// TODO: Add Stuff
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Log("Diagnostic info for this has not been inplemented!"); //TODO
|
||||
return new Status();
|
||||
}
|
||||
}
|
||||
@@ -7,84 +7,69 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.AddressableLED;
|
||||
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.LEDConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
import frc4388.utility.structs.LEDPatterns;
|
||||
|
||||
/**
|
||||
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
|
||||
* Driver
|
||||
*/
|
||||
public class LED extends SubsystemBase {
|
||||
|
||||
static AddressableLED m_led;
|
||||
static AddressableLEDBuffer m_ledBuffer;
|
||||
static LED m_self;
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
|
||||
public LED(){
|
||||
if(m_self != null)
|
||||
return;
|
||||
m_led = new AddressableLED(9);
|
||||
m_ledBuffer = new AddressableLEDBuffer(10);
|
||||
m_led.setLength(m_ledBuffer.getLength());
|
||||
m_led.setData(m_ledBuffer);
|
||||
m_led.start();
|
||||
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
public class LED extends SubsystemBase implements Queryable {
|
||||
public LED() {
|
||||
FaultReporter.register(this);
|
||||
}
|
||||
public static LED getInstance() {
|
||||
if(m_self == null)
|
||||
m_self = new LED();
|
||||
return m_self;
|
||||
|
||||
private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN;
|
||||
|
||||
public void setMode(LEDPatterns pattern){
|
||||
this.mode = pattern;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic(){
|
||||
//gamermode();
|
||||
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
return;
|
||||
}
|
||||
static int firstcolor = 0;
|
||||
static void gamermode() {
|
||||
for(int i = 0; i < m_ledBuffer.getLength(); i++) {
|
||||
final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
|
||||
setLEDHSV(i, hue, 255, 128);
|
||||
}
|
||||
firstcolor +=3;
|
||||
firstcolor %= 180;
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public static void updateLED (){
|
||||
gamermode();
|
||||
// m_LEDController.set(m_currentPattern.getValue());
|
||||
public void periodic() {
|
||||
update();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public static void setLEDRGB(int lednum, int r, int g, int b){
|
||||
m_ledBuffer.setRGB(lednum, r, g, b);
|
||||
//m_currentPattern = pattern;
|
||||
// m_LEDController.set(m_currentPattern.getValue());
|
||||
public void update() {
|
||||
if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return;
|
||||
|
||||
if(DriverStation.isDisabled()){
|
||||
LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue());
|
||||
}else
|
||||
LEDController.set(mode.getValue());
|
||||
}
|
||||
public static void setLEDHSV(int lednum, int hue, int sat, int val){
|
||||
m_ledBuffer.setRGB(lednum, hue, sat, val);
|
||||
//m_currentPattern = pattern;
|
||||
// m_LEDController.set(m_currentPattern.getValue());
|
||||
|
||||
@AutoLogOutput
|
||||
public String state() {
|
||||
return mode.getClass().toString();
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
* @return
|
||||
*/
|
||||
public AddressableLEDBuffer getBuffer() {
|
||||
return m_ledBuffer;
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "LEDs";
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
if(!LEDController.isAlive())
|
||||
status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
|
||||
|
||||
status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -1,364 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.logging.Level;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants.Conversions;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import frc4388.utility.RobotUnits;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.Report;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
|
||||
public class SwerveDrive extends Subsystem {
|
||||
|
||||
private SwerveModule leftFront;
|
||||
private SwerveModule rightFront;
|
||||
private SwerveModule leftBack;
|
||||
private SwerveModule rightBack;
|
||||
|
||||
private SwerveModule[] modules;
|
||||
|
||||
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
|
||||
|
||||
private RobotGyro gyro;
|
||||
|
||||
private int gear_index;
|
||||
private boolean stopped = false;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
|
||||
super();
|
||||
this.leftFront = leftFront;
|
||||
this.rightFront = rightFront;
|
||||
this.leftBack = leftBack;
|
||||
this.rightBack = rightBack;
|
||||
|
||||
this.gyro = gyro;
|
||||
reset_index();
|
||||
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
||||
}
|
||||
|
||||
public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
|
||||
// double ang = Math.atan2(rightStick.getY(), rightStick.getX());
|
||||
// rightStick.getAngle()
|
||||
double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
|
||||
// System.out.println(ang);
|
||||
// module.go(ang);
|
||||
// Rotation2d rot = Rotation2d.fromRadians(ang);
|
||||
Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
|
||||
SwerveModuleState state = new SwerveModuleState(speed, rot);
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
|
||||
double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
|
||||
SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
|
||||
|
||||
if (fieldRelative) {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot_correction = 0;
|
||||
// rot = rightStick.getX();
|
||||
// SmartDashboard.putBoolean("drift correction", false);
|
||||
stopped = false;
|
||||
} else if(leftStick.getNorm() > 0.05) {
|
||||
if (!stopped) {
|
||||
stopModules();
|
||||
stopped = true;
|
||||
}
|
||||
|
||||
// SmartDashboard.putBoolean("drift correction", true);
|
||||
|
||||
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
}
|
||||
|
||||
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
// chassisSpeeds = chassisSpeeds.
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
|
||||
// SmartDashboard.putBoolean("drift correction", false);
|
||||
stopped = false;
|
||||
} else if(leftStick.getNorm() > 0.05) {
|
||||
if (!stopped) {
|
||||
stopModules();
|
||||
stopped = true;
|
||||
}
|
||||
|
||||
// SmartDashboard.putBoolean("drift correction", true);
|
||||
// double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
|
||||
}
|
||||
|
||||
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
|
||||
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
|
||||
}
|
||||
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
|
||||
// Translation2d rightStick = new Translation2d(-rightX, rightY);
|
||||
double rightX = rightStick.getX();
|
||||
double rightY = rightStick.getY();
|
||||
|
||||
double rot_correction = 0;
|
||||
|
||||
// double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
if(fieldRelative) {
|
||||
double rot = 0;
|
||||
if(rightStick.getNorm() > 0.5) {
|
||||
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
|
||||
|
||||
Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
|
||||
double min = tmp.getDegrees();
|
||||
min = Math.max(Math.abs(min), 2);
|
||||
if(tmp.getDegrees() < 0)
|
||||
min*=-1;
|
||||
tmp = new Rotation2d(min * Math.PI / 180);
|
||||
rot = tmp.getRadians(); // x x - y/x
|
||||
}
|
||||
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1);
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set each module of the swerve drive to the corresponding desired state.
|
||||
* @param desiredStates Array of module states to set.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
for (int i = 0; i < desiredStates.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
}
|
||||
|
||||
public boolean rotateToTarget(double angle) {
|
||||
double currentAngle = getGyroAngle();
|
||||
double error = angle - currentAngle;
|
||||
|
||||
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
|
||||
|
||||
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return -gyro.getAngle();
|
||||
}
|
||||
|
||||
public void add180() {
|
||||
gyro.reset(gyro.getAngle()+180);
|
||||
rotTarget = gyro.getAngle();
|
||||
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
gyro.reset();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroFlip() {
|
||||
gyro.resetFlip();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroRightBlue() {
|
||||
gyro.resetRightSideBlue();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroRightAmp() {
|
||||
gyro.resetAmpSide();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
for (SwerveModule module : this.modules) {
|
||||
module.stop();
|
||||
}
|
||||
}
|
||||
|
||||
public SwerveDriveKinematics getKinematics() {
|
||||
return this.kinematics;
|
||||
}
|
||||
|
||||
public boolean getSpeedState() {
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
}
|
||||
|
||||
private void reset_index() {
|
||||
gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index - 1;
|
||||
if (i == -1) i = 0;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index + 1;
|
||||
if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed;
|
||||
gear_index = -1;
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
|
||||
for(int i=0;i<5;i++){
|
||||
Log("SLOW");
|
||||
}
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
|
||||
for(int i=0;i<5;i++){
|
||||
Log("FAST");
|
||||
}
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
|
||||
for(int i=0;i<5;i++){
|
||||
Log("TURBO");
|
||||
}
|
||||
}
|
||||
|
||||
public void toggleGear(double angle) {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
|
||||
} else {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
|
||||
}
|
||||
}
|
||||
|
||||
public void shiftUpRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
|
||||
public void shiftDownRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Swerve Drive Controller";
|
||||
}
|
||||
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
|
||||
SmartDashboard.putNumber("[" + getSubsystemName() + "] Gyro angle", this.gyro.getAngle());
|
||||
SmartDashboard.putNumber("[" + getSubsystemName() + "] Shift State", this.speedAdjust);
|
||||
|
||||
// this.leftFront.queryStatus();
|
||||
// this.leftBack.queryStatus();
|
||||
// this.rightFront.queryStatus();
|
||||
// this.rightBack.queryStatus();
|
||||
|
||||
//TODO: Add more status things
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
for (SwerveModule module : modules) {
|
||||
for (Report moduleDignostic : module.diagnosticStatus().reports) {
|
||||
status.addReport(moduleDignostic.reportLevel, "[" + module.getSubsystemName() + "] " + moduleDignostic.description);
|
||||
}
|
||||
}
|
||||
|
||||
status.diagnoseHardwareCTRE("Swerve Gyro", gyro.getPigeon());
|
||||
|
||||
return status;
|
||||
}
|
||||
}
|
||||
@@ -1,280 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.logging.Level;
|
||||
|
||||
import com.ctre.phoenix6.StatusSignal;
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.FeedbackConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.controls.ControlRequest;
|
||||
import com.ctre.phoenix6.controls.DutyCycleOut;
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
import com.ctre.phoenix6.signals.MagnetHealthValue;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import com.ctre.phoenix6.signals.SensorDirectionValue;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
// 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.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
|
||||
public class SwerveModule extends Subsystem {
|
||||
private String name = "Null";
|
||||
private TalonFX driveMotor;
|
||||
private TalonFX angleMotor;
|
||||
private CANcoder encoder;
|
||||
// private final StatusSignal<Double> cc_pos;
|
||||
// private final StatusSignal<Double> cc_vel;
|
||||
// private int selfid;
|
||||
// private ConfigurableDouble offsetGetter;
|
||||
private static int swerveId = 0;
|
||||
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
|
||||
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(String name, TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
|
||||
super();
|
||||
this.name = name;
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.encoder = encoder;
|
||||
|
||||
var motorCfg = new TalonFXConfiguration()
|
||||
.withOpenLoopRamps(
|
||||
new OpenLoopRampsConfigs()
|
||||
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
|
||||
).withClosedLoopRamps(
|
||||
new ClosedLoopRampsConfigs()
|
||||
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
|
||||
).withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(100)
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
.withSupplyCurrentLimit(100)
|
||||
.withSupplyCurrentLimitEnable(true)
|
||||
);
|
||||
|
||||
driveMotor.getConfigurator().apply(motorCfg);
|
||||
|
||||
TalonFXConfiguration angleConfig = new TalonFXConfiguration()
|
||||
.withOpenLoopRamps(
|
||||
new OpenLoopRampsConfigs()
|
||||
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
|
||||
).withClosedLoopRamps(
|
||||
new ClosedLoopRampsConfigs()
|
||||
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
|
||||
);
|
||||
|
||||
angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
|
||||
|
||||
angleConfig.Slot0.kP = swerveGains.kP;
|
||||
angleConfig.Slot0.kI = swerveGains.kI;
|
||||
angleConfig.Slot0.kD = swerveGains.kD;
|
||||
|
||||
angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID();
|
||||
angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
|
||||
angleMotor.getConfigurator().apply(angleConfig);
|
||||
|
||||
CANcoderConfiguration canconfig = new CANcoderConfiguration();
|
||||
canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
|
||||
canconfig.MagnetSensor.MagnetOffset = offset;
|
||||
encoder.getConfigurator().apply(canconfig);
|
||||
|
||||
rotateToAngle(0);
|
||||
}
|
||||
|
||||
// public void go(double ang){
|
||||
// // double curang = this.encoder.getAbsolutePosition().getValue();
|
||||
// System.out.println(getAngle().getDegrees());
|
||||
// rotateToAngle(ang);
|
||||
// }
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
//encoder.configMagnetOffset(offsetGetter.get());
|
||||
//SmartDashboard.putString("Error Code: " + selfid, getstuff());
|
||||
// SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
|
||||
// SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
|
||||
// SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
|
||||
// SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
|
||||
}
|
||||
/**
|
||||
* Get the drive motor of the SwerveModule
|
||||
* @return the drive motor of the SwerveModule
|
||||
*/
|
||||
public TalonFX getDriveMotor() {
|
||||
return this.driveMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle motor of the SwerveModule
|
||||
* @return the angle motor of the SwerveModule
|
||||
*/
|
||||
public TalonFX getAngleMotor() {
|
||||
return this.angleMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the CANcoder of the SwerveModule
|
||||
* @return the CANcoder of the SwerveModule
|
||||
*/
|
||||
public CANcoder getEncoder() {
|
||||
return this.encoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle of a SwerveModule as a Rotation2d
|
||||
* @return the angle of a SwerveModule as a Rotation2d
|
||||
*/
|
||||
public Rotation2d getAngle() {
|
||||
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
|
||||
// return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
|
||||
return Rotation2d.fromRotations(encoder.getPosition().getValue().baseUnitMagnitude());
|
||||
}
|
||||
|
||||
public double getAngularVel() {
|
||||
// return this.angleMotor.getSelectedSensorVelocity();
|
||||
return angleMotor.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getDrivePos() {
|
||||
// return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
|
||||
return driveMotor.getPosition().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getDriveVel() {
|
||||
// return this.driveMotor.getSelectedSensorVelocity(0);
|
||||
return driveMotor.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
driveMotor.set(0);
|
||||
angleMotor.set(0);
|
||||
}
|
||||
|
||||
public void rotateToAngle(double angle) {
|
||||
final PositionVoltage m_request = new PositionVoltage(angle);
|
||||
angleMotor.setControl(m_request);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get state of swerve module
|
||||
* @return speed in m/s and angle in degrees
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(
|
||||
Units.inchesToMeters(driveMotor.getVelocity().getValue().baseUnitMagnitude() *
|
||||
SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV *
|
||||
SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV),
|
||||
getAngle()
|
||||
);
|
||||
}
|
||||
|
||||
// private SwerveModuleState optimizeState(SwerveModuleState desiredState) {
|
||||
// Rotation2d curRot = this.getAngle();
|
||||
|
||||
// }
|
||||
|
||||
/**
|
||||
* Returns the current position of the SwerveModule
|
||||
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
|
||||
// */
|
||||
// public SwerveModulePosition getPosition() {
|
||||
// return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
|
||||
// }
|
||||
|
||||
/**
|
||||
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
|
||||
* @param desiredState a SwerveModuleState representing the desired new state of the module
|
||||
// */
|
||||
public void setDesiredState(SwerveModuleState state) {
|
||||
Rotation2d currentRotation = this.getAngle();
|
||||
|
||||
state.optimize(currentRotation);//SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// calculate the difference between our current rotational position and our new rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
|
||||
double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND;
|
||||
|
||||
rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
|
||||
|
||||
driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
|
||||
}
|
||||
|
||||
public void reset() {
|
||||
// encoder.setPosition(0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return this.name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
SmartDashboard.putNumber("[" + getSubsystemName() + "] Drive motor speed", this.driveMotor.get());
|
||||
SmartDashboard.putNumber("[" + getSubsystemName() + "] Angle motor angle", this.angleMotor.getRotorPosition().getValueAsDouble());
|
||||
//TODO: Add more status things
|
||||
}
|
||||
|
||||
public boolean motorsAlive() {
|
||||
return this.driveMotor.isAlive() && this.angleMotor.isAlive();
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
status.diagnoseHardwareCTRE("Drive", this.driveMotor);
|
||||
status.diagnoseHardwareCTRE("Angle", this.angleMotor);
|
||||
status.diagnoseHardwareCTRE("Steer", this.encoder);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
// public double getCurrent() {
|
||||
// return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
|
||||
// }
|
||||
|
||||
// public double getVoltage() {
|
||||
// return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
|
||||
// }
|
||||
|
||||
// public String getstuff() {
|
||||
// encoder.getPosition();
|
||||
// return "" + encoder.getLastError().value;
|
||||
// }
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class LiDAR extends SubsystemBase implements Queryable {
|
||||
LidarIO io;
|
||||
LidarStateAutoLogged state = new LidarStateAutoLogged();
|
||||
|
||||
private String name = "Lidar";
|
||||
|
||||
public LiDAR(LidarIO device, String name) {
|
||||
FaultReporter.register(this);
|
||||
|
||||
this.io = device;
|
||||
this.name = name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
io.updateInputs(state);
|
||||
Logger.processInputs("LiDAR/"+name, state);
|
||||
}
|
||||
|
||||
// @AutoLogOutput(key = "Lidar/{name}")
|
||||
public double getDistance(){
|
||||
return state.distance;
|
||||
}
|
||||
|
||||
public boolean withinDistance(){
|
||||
if(state.distance == -1) return false;
|
||||
return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Lidar " + name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
if(state.distance == -1)
|
||||
s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
|
||||
|
||||
s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance);
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
public interface LidarIO {
|
||||
@AutoLog
|
||||
public class LidarState {
|
||||
public boolean connected;
|
||||
public double distance;
|
||||
}
|
||||
|
||||
public default void updateInputs(LidarState state) {}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import edu.wpi.first.wpilibj.Counter;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
|
||||
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
|
||||
public class LidarReal implements LidarIO {
|
||||
|
||||
|
||||
private Counter LidarPWM;
|
||||
|
||||
public LidarReal(int port) {
|
||||
LidarPWM = new Counter(port);
|
||||
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
|
||||
LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
|
||||
LidarPWM.reset();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(LidarState state) {
|
||||
|
||||
if(LidarPWM.get() < 1)
|
||||
state.distance = -1;
|
||||
else
|
||||
state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,467 @@
|
||||
// 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.robot.subsystems.swerve;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
|
||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
import com.pathplanner.lib.util.PathPlannerLogging;
|
||||
import com.pathplanner.lib.config.PIDConstants;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.config.RobotConfig;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
private SwerveIO io;
|
||||
private SwerveStateAutoLogged state;
|
||||
|
||||
private Vision vision;
|
||||
|
||||
|
||||
|
||||
public int gear_index = SwerveDriveConstants.STARTING_GEAR;
|
||||
public boolean stopped = false;
|
||||
public boolean robotKnowsWhereItIs = false;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
|
||||
// 25%
|
||||
|
||||
public double lastOdomSpeed;
|
||||
|
||||
public Pose2d initalPose2d = new Pose2d();
|
||||
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
|
||||
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
|
||||
// swerveDriveTrain) {
|
||||
FaultReporter.register(this);
|
||||
|
||||
this.io = swerveDriveTrain;
|
||||
this.state = new SwerveStateAutoLogged();
|
||||
|
||||
this.vision = vision;
|
||||
|
||||
RobotConfig config;
|
||||
try {
|
||||
config = RobotConfig.fromGUISettings();
|
||||
} catch (Exception e) {
|
||||
// Handle exception as needed
|
||||
config = null;
|
||||
}
|
||||
// DoubleSupplier a = () -> 1.d;
|
||||
AutoBuilder.configure(
|
||||
() -> {
|
||||
return getPose2d();
|
||||
}, // Robot pose supplier
|
||||
this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
|
||||
// pose)
|
||||
() -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
|
||||
(speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
|
||||
.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
|
||||
// Also optionally outputs individual module feedforwards
|
||||
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
|
||||
// holonomic drive trains
|
||||
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
|
||||
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
|
||||
),
|
||||
config, // The robot configuration
|
||||
() -> {
|
||||
// Boolean supplier that controls when the path will be mirrored for the red
|
||||
// alliance
|
||||
// This will flip the path being followed to the red side of the field.
|
||||
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
|
||||
|
||||
// var alliance = DriverStation.getAlliance();
|
||||
// if (alliance.isPresent()) {
|
||||
// return alliance.get() == DriverStation.Alliance.Red;
|
||||
// }
|
||||
return TimesNegativeOne.isRed;
|
||||
},
|
||||
this // Reference to this subsystem to set requirements
|
||||
);
|
||||
|
||||
PathPlannerLogging.setLogActivePathCallback(
|
||||
(activePath) -> {
|
||||
Logger.recordOutput(
|
||||
"Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
|
||||
});
|
||||
|
||||
PathPlannerLogging.setLogTargetPoseCallback(
|
||||
(targetPose) -> {
|
||||
Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
|
||||
});
|
||||
|
||||
|
||||
|
||||
// // Configure SysId
|
||||
// sysId =
|
||||
// new SysIdRoutine(
|
||||
// new SysIdRoutine.Config(
|
||||
// null,
|
||||
// null,
|
||||
// null,
|
||||
// (state) -> Logger.recordOutput("Drive/SysIdState", toString())),
|
||||
// new SysIdRoutine.Mechanism(
|
||||
// (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
|
||||
|
||||
}
|
||||
|
||||
public void setOdoPose(Pose2d pose) {
|
||||
if (pose == null) return;
|
||||
initalPose2d = pose;
|
||||
io.resetPose(pose);
|
||||
}
|
||||
|
||||
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
|
||||
// Translation2d rightStick){
|
||||
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
|
||||
// // rightStick.getAngle()
|
||||
// double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) +
|
||||
// Math.pow(leftStick.getY(), 2));
|
||||
// // System.out.println(ang);
|
||||
// // module.go(ang);
|
||||
// // Rotation2d rot = Rotation2d.fromRadians(ang);
|
||||
// Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
|
||||
// SwerveModuleState state = new SwerveModuleState(speed, rot);
|
||||
// module.setDesiredState(state);
|
||||
// }
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
stopped = false;
|
||||
if (fieldRelative) {
|
||||
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
|
||||
|
||||
rotTarget = state.currentPose.getRotation().getDegrees();
|
||||
|
||||
io.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
|
||||
// .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
|
||||
SmartDashboard.putBoolean("drift correction", false);
|
||||
} else {
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
|
||||
ctrl.HeadingController.setPID(
|
||||
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
|
||||
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
|
||||
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
|
||||
);
|
||||
io.setControl(ctrl);
|
||||
SmartDashboard.putBoolean("drift correction", true);
|
||||
}
|
||||
|
||||
|
||||
} else { // Create robot-relative speeds.
|
||||
io.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(-leftStick.getY() * speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
}
|
||||
}
|
||||
|
||||
public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
|
||||
stopped = false;
|
||||
// Create robot-relative speeds.
|
||||
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
|
||||
io.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
|
||||
// reason to have a robot
|
||||
// relitive version of
|
||||
// this, and no pre
|
||||
// provided version
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
|
||||
// drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(rightStick.getAngle()));
|
||||
}
|
||||
|
||||
public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
ctrl.HeadingController.setPID(
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
|
||||
);
|
||||
io.setControl(ctrl);
|
||||
}
|
||||
|
||||
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
|
||||
leftStick = leftStick.rotateBy(heading);
|
||||
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
// ctrl.HeadingController.setPID(
|
||||
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
|
||||
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
|
||||
// );
|
||||
io.setControl(ctrl);
|
||||
}
|
||||
|
||||
public void activateLuigiMode() {
|
||||
io.setLimits(20);
|
||||
}
|
||||
|
||||
public void deactivateLuigiMode() {
|
||||
io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
|
||||
}
|
||||
|
||||
public boolean rotateToTarget(double angle) {
|
||||
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(angle)));
|
||||
|
||||
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public boolean isStopped() {
|
||||
return lastOdomSpeed < AutoConstants.STOP_VELOCITY;
|
||||
}
|
||||
|
||||
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
|
||||
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
|
||||
// swerve drive is still going:
|
||||
// stopModules(); // stop the swerve
|
||||
|
||||
// if (leftStick.getNorm() < 0.05) //if no imput
|
||||
// return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * -speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(rot));
|
||||
// double
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return getPose2d().getRotation().getRadians();
|
||||
}
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
if(state.currentPose == null)
|
||||
return initalPose2d;
|
||||
return state.currentPose;
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
io.tareEverything();
|
||||
robotKnowsWhereItIs = false;
|
||||
rotTarget = 0;
|
||||
// vision.resetRotations();
|
||||
}
|
||||
|
||||
|
||||
public void softStop() {
|
||||
stopped = true;
|
||||
io.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0)
|
||||
.withRotationalRate(0)
|
||||
); // stop the modules without breaking
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
// stopped = true;
|
||||
// swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
|
||||
softStop();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
|
||||
io.updateInputs(state);
|
||||
Logger.processInputs("SwerveDrive", state);
|
||||
|
||||
vision.setLastOdomPose(state.currentPose);
|
||||
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
|
||||
|
||||
if (vision.isTag()) {
|
||||
Pose2d pose = vision.getPose2d();
|
||||
if (!robotKnowsWhereItIs) {
|
||||
robotKnowsWhereItIs = true;
|
||||
Pose2d curPose = getPose2d();
|
||||
rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
|
||||
}
|
||||
|
||||
io.addVisionMeasurement(vision.getPosesToAdd());
|
||||
}
|
||||
|
||||
// if(e.isPresent())
|
||||
}
|
||||
|
||||
private void reset_index() {
|
||||
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
|
||||
// robot start in?)
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
|
||||
reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index - 1;
|
||||
if (i == -1)
|
||||
i = 0;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
|
||||
reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index + 1;
|
||||
if (i == SwerveDriveConstants.GEARS.length)
|
||||
i = SwerveDriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
gear_index = -1;
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
|
||||
gear_index = 0;
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
setPercentOutput(SwerveDriveConstants.FAST_SPEED);
|
||||
gear_index = 1;
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
|
||||
gear_index = 2;
|
||||
}
|
||||
|
||||
public void shiftUpRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
|
||||
public void shiftDownRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
|
||||
|
||||
public void startSlowPeriod() {
|
||||
tmp_gear_index = gear_index;
|
||||
setToSlow();
|
||||
}
|
||||
|
||||
public void startTurboPeriod() {
|
||||
tmp_gear_index = gear_index;
|
||||
setToTurbo();
|
||||
}
|
||||
|
||||
public void endSlowPeriod() {
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
|
||||
gear_index = tmp_gear_index;
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){
|
||||
if(curPose != null && lastPose != null){
|
||||
lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
|
||||
}
|
||||
}
|
||||
|
||||
@AutoLogOutput(key="SwerveDrive/speed ")
|
||||
public double getOdometrySpeed() {
|
||||
return lastOdomSpeed;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Swerve Drive Controller";
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
// status.addReport(ReportLevel.INFO,
|
||||
// "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
|
||||
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,246 @@
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
|
||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
import frc4388.utility.structs.Gains;
|
||||
|
||||
|
||||
// No mans land
|
||||
// Beware, there be dragons.
|
||||
public final class SwerveDriveConstants {
|
||||
public static final double MAX_ROT_SPEED = Math.PI * 2;
|
||||
public static final double AUTO_MAX_ROT_SPEED = 1.5;
|
||||
public static final double MIN_ROT_SPEED = 1.0;
|
||||
public static double ROTATION_SPEED = MAX_ROT_SPEED;
|
||||
public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
|
||||
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
|
||||
|
||||
public static final double CORRECTION_MIN = 10;
|
||||
public static final double CORRECTION_MAX = 50;
|
||||
|
||||
public static final double SLOW_SPEED = 0.25;
|
||||
public static final double FAST_SPEED = 0.5;
|
||||
public static final double TURBO_SPEED = 1.0;
|
||||
|
||||
public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED};
|
||||
public static final int STARTING_GEAR = 0;
|
||||
// Dimensions
|
||||
public static final double WIDTH = 18.5; // TODO: validate
|
||||
public static final double HEIGHT = 18.5; // TODO: validate
|
||||
public static final double HALF_WIDTH = WIDTH / 2.d;
|
||||
public static final double HALF_HEIGHT = HEIGHT / 2.d;
|
||||
|
||||
// Mechanics
|
||||
private static final double COUPLE_RATIO = 3; //todo: find
|
||||
private static final double DRIVE_RATIO = 6.12;
|
||||
private static final double STEER_RATIO = (150/7);
|
||||
private static final Distance WHEEL_RADIUS = Inches.of(2);
|
||||
|
||||
public static final double MAX_SPEED_MEETERS_PER_SEC = 6.22; // TODO: Validate
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
|
||||
|
||||
// Operation
|
||||
public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
|
||||
|
||||
public static final boolean DRIFT_CORRECTION_ENABLED = true;
|
||||
public static final boolean INVERT_X = false;
|
||||
public static final boolean INVERT_Y = true;
|
||||
public static final boolean INVERT_ROTATION = false;
|
||||
|
||||
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
|
||||
|
||||
private static final class ModuleSpecificConstants { //2025
|
||||
//Front Left
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
|
||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Front Right
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
|
||||
//Back Left
|
||||
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
|
||||
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Back Right
|
||||
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
|
||||
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
}
|
||||
|
||||
public static final class IDs {
|
||||
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4);
|
||||
public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5);
|
||||
public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11);
|
||||
|
||||
public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 2);
|
||||
public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 3);
|
||||
public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 10);
|
||||
|
||||
public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6);
|
||||
public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7);
|
||||
public static final CanDevice LEFT_BACK_ENCODER = new CanDevice("LEFT_BACK_ENCODER", 12);
|
||||
|
||||
public static final CanDevice RIGHT_BACK_WHEEL = new CanDevice("RIGHT_BACK_WHEEL", 8);
|
||||
public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9);
|
||||
public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13);
|
||||
|
||||
public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14);
|
||||
}
|
||||
|
||||
public static final class PIDConstants {
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0);
|
||||
|
||||
public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs()
|
||||
.withKP(50).withKI(0).withKD(0.32)
|
||||
.withKS(0).withKV(0).withKA(0);
|
||||
|
||||
public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs()
|
||||
.withKP(10).withKI(0).withKD(0.3)
|
||||
.withKS(0).withKV(0).withKA(0);
|
||||
|
||||
public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
|
||||
|
||||
// The steer motor uses any SwerveModule.SteerRequestType control request with the
|
||||
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
|
||||
public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs()
|
||||
.withKP(100).withKI(0).withKD(0.6)
|
||||
.withKS(0.1).withKV(1.91).withKA(0)
|
||||
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
||||
// When using closed-loop control, the drive motor uses the control
|
||||
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
|
||||
public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs()
|
||||
.withKP(0.1).withKI(0).withKD(0)
|
||||
.withKS(0).withKV(0.124);
|
||||
|
||||
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
|
||||
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
|
||||
public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
|
||||
// POWER! (limiting)
|
||||
private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration()
|
||||
.withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
.withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND)
|
||||
).withOpenLoopRamps(
|
||||
new OpenLoopRampsConfigs()
|
||||
.withDutyCycleOpenLoopRampPeriod(OPEN_LOOP_RAMP_RATE)
|
||||
).withClosedLoopRamps(
|
||||
new ClosedLoopRampsConfigs()
|
||||
.withDutyCycleClosedLoopRampPeriod(CLOSED_LOOP_RAMP_RATE)
|
||||
);
|
||||
private static final TalonFXConfiguration STEER_MOTOR_CONFIGS = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
.withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND)
|
||||
// ).withOpenLoopRamps(
|
||||
// new OpenLoopRampsConfigs()
|
||||
// .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
|
||||
// ).withClosedLoopRamps(
|
||||
// new ClosedLoopRampsConfigs()
|
||||
// .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
|
||||
);
|
||||
public static final double SLIP_CURRENT = 60; // TODO: Tune???
|
||||
}
|
||||
|
||||
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
|
||||
.withPigeon2Id(IDs.DRIVE_PIGEON.id);
|
||||
|
||||
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
|
||||
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.
|
||||
.withDriveMotorGearRatio(DRIVE_RATIO)
|
||||
.withSteerMotorGearRatio(STEER_RATIO)
|
||||
.withCouplingGearRatio(COUPLE_RATIO)
|
||||
.withWheelRadius(WHEEL_RADIUS)
|
||||
.withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ?
|
||||
.withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ?
|
||||
.withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage)
|
||||
.withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage)
|
||||
.withSlipCurrent(Configurations.SLIP_CURRENT)
|
||||
.withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC)
|
||||
.withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated)
|
||||
.withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated)
|
||||
.withFeedbackSource(SteerFeedbackType.RemoteCANcoder)
|
||||
.withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS)
|
||||
.withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS);
|
||||
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_LEFT =
|
||||
ConstantCreator.createModuleConstants(
|
||||
IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET,
|
||||
ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS,
|
||||
ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_RIGHT =
|
||||
ConstantCreator.createModuleConstants(
|
||||
IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET,
|
||||
ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS,
|
||||
ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_LEFT =
|
||||
ConstantCreator.createModuleConstants(
|
||||
IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET,
|
||||
ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS,
|
||||
ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_RIGHT =
|
||||
ConstantCreator.createModuleConstants(
|
||||
IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET,
|
||||
ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS,
|
||||
ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED
|
||||
);
|
||||
|
||||
// misc
|
||||
public static final int TIMEOUT_MS = 30;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
|
||||
public interface SwerveIO {
|
||||
@AutoLog
|
||||
public class SwerveState {
|
||||
public Pose2d currentPose = null;
|
||||
public Pose2d lastPose = null;
|
||||
public ChassisSpeeds speeds = null;
|
||||
public double odometryRate = 1;
|
||||
}
|
||||
|
||||
public default void setControl(SwerveRequest ctrl) {}
|
||||
|
||||
public default void setLimits(double limitInAmps) {}
|
||||
|
||||
public default void tareEverything() {}
|
||||
|
||||
public default void resetPose(Pose2d pose) {}
|
||||
|
||||
public default void addVisionMeasurement(List<PoseObservation> poses) {}
|
||||
|
||||
public default void updateInputs(SwerveState state) {}
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
import com.ctre.phoenix6.swerve.SwerveModule;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
|
||||
public class SwerveReal implements SwerveIO {
|
||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
public SwerveReal(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
|
||||
this.swerveDriveTrain = swerveDriveTrain;
|
||||
swerveDriveTrain.getOdometryFrequency();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(SwerveState state) {
|
||||
double time = Vision.getTime();
|
||||
state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency();
|
||||
state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null);
|
||||
state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null);
|
||||
state.speeds = swerveDriveTrain.getState().Speeds;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setControl(SwerveRequest ctrl) {
|
||||
swerveDriveTrain.setControl(ctrl);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void tareEverything() {
|
||||
swerveDriveTrain.tareEverything();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setLimits(double limitInAmps) {
|
||||
for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
|
||||
var talonFXConfigurator = module.getDriveMotor().getConfigurator();
|
||||
var talonFXConfigs = new TalonFXConfiguration();
|
||||
|
||||
talonFXConfigurator.refresh(talonFXConfigs);
|
||||
talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
|
||||
talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
|
||||
talonFXConfigurator.apply(talonFXConfigs);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void addVisionMeasurement(List<PoseObservation> poses) {
|
||||
for(PoseObservation pose : poses) {
|
||||
swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status;
|
||||
|
||||
public class Vision extends SubsystemBase implements Queryable {
|
||||
VisionIO[] io;
|
||||
VisionStateAutoLogged[] state;
|
||||
|
||||
|
||||
public Pose2d lastVisionPose = new Pose2d();
|
||||
public Pose2d lastPhysOdomPose = new Pose2d();
|
||||
|
||||
public Vision(VisionIO... devices) {
|
||||
FaultReporter.register(this);
|
||||
io = devices;
|
||||
state = new VisionStateAutoLogged[io.length];
|
||||
|
||||
for(int i = 0; i < io.length; i++) {
|
||||
state[i] = new VisionStateAutoLogged();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
for(int i = 0; i < io.length; i++) {
|
||||
io[i].updateInputs(state[i]);
|
||||
Logger.processInputs("Vision/Camera" + i , state[i]);
|
||||
}
|
||||
}
|
||||
|
||||
public List<PoseObservation> getPosesToAdd(){
|
||||
List<PoseObservation> poses = new ArrayList<>();
|
||||
for(int i = 0; i < state.length; i++) {
|
||||
if(state[i].lastEstimatedPose != null) {
|
||||
poses.add(state[i].lastEstimatedPose);
|
||||
}
|
||||
}
|
||||
|
||||
return poses;
|
||||
}
|
||||
|
||||
public void setLastOdomPose(Pose2d pose){
|
||||
if(pose != null)
|
||||
lastPhysOdomPose = pose;
|
||||
}
|
||||
|
||||
public boolean isTag(){
|
||||
for(int i = 0; i < state.length; i++){
|
||||
if(state[i].isTagDetected && state[i].isTagProcessed)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@AutoLogOutput
|
||||
public Pose2d getPose2d() {
|
||||
if(lastPhysOdomPose != null)
|
||||
return lastPhysOdomPose;
|
||||
|
||||
// if(lastVisionPose != null)
|
||||
// return lastVisionPose;
|
||||
return new Pose2d();
|
||||
|
||||
}
|
||||
|
||||
public static double getTime() {
|
||||
return Utils.getCurrentTimeSeconds();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
return new Status();
|
||||
// // TODO Auto-generated method stub
|
||||
// throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'");
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
|
||||
public interface VisionIO {
|
||||
@AutoLog
|
||||
public class VisionState {
|
||||
public boolean isTagDetected = false;
|
||||
public boolean isTagProcessed = false;
|
||||
// public double latency = 0;
|
||||
public PoseObservation lastEstimatedPose = null;
|
||||
}
|
||||
|
||||
public static record PoseObservation(
|
||||
Pose2d pose,
|
||||
double timestamp
|
||||
) {}
|
||||
|
||||
public default void updateInputs(VisionState state) {}
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import java.util.Optional;
|
||||
|
||||
import org.photonvision.EstimatedRobotPose;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonPoseEstimator;
|
||||
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import frc4388.robot.constants.Constants.FieldConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
|
||||
public class VisionReal implements VisionIO {
|
||||
// private PhotonCamera[] cameras;
|
||||
// private PhotonPoseEstimator[] estimators;
|
||||
|
||||
private PhotonCamera camera;
|
||||
private PhotonPoseEstimator estimator;
|
||||
|
||||
// public List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
|
||||
|
||||
public VisionReal(PhotonCamera camera, Transform3d position){
|
||||
this.camera = camera;
|
||||
estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
|
||||
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
}
|
||||
|
||||
// private Instant lastVisionTime = null;
|
||||
|
||||
|
||||
public void updateInputs(VisionState state) {
|
||||
state.isTagProcessed = false;
|
||||
state.isTagDetected = false;
|
||||
|
||||
state.lastEstimatedPose = null;
|
||||
|
||||
var results = camera.getAllUnreadResults();
|
||||
|
||||
// If there are no more updates from the camera
|
||||
if (results.size() == 0)
|
||||
return;
|
||||
|
||||
|
||||
var result = results.get(results.size()-1);
|
||||
|
||||
state.isTagDetected = state.isTagDetected | result.hasTargets();
|
||||
|
||||
// If there are no tags
|
||||
if(!result.hasTargets())
|
||||
return;
|
||||
|
||||
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
|
||||
|
||||
// If the tag was failed to be processed
|
||||
if(estimatedRobotPose.isEmpty())
|
||||
return;
|
||||
|
||||
EstimatedRobotPose pose = estimatedRobotPose.get();
|
||||
|
||||
state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds);
|
||||
|
||||
state.isTagProcessed = true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* The latest estimated robot pose on the field from vision data. This may be empty. This should
|
||||
* only be called once per loop.
|
||||
*
|
||||
* <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
|
||||
* {@link getEstimationStdDevs}
|
||||
*
|
||||
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
|
||||
* used for estimation.
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
|
||||
Optional<EstimatedRobotPose> visionEst = Optional.empty();
|
||||
|
||||
var targets = change.getTargets();
|
||||
for(int i = targets.size()-1; i >= 0; i--){
|
||||
Transform3d pos = targets.get(i).getBestCameraToTarget();
|
||||
double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
|
||||
if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
change.targets.remove(i);
|
||||
}
|
||||
}
|
||||
|
||||
if(targets.size() <= 0)
|
||||
return visionEst; // Will be empty
|
||||
|
||||
visionEst = estimator.update(change);
|
||||
// updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
|
||||
|
||||
return visionEst;
|
||||
}
|
||||
|
||||
public String getName() {
|
||||
return camera.getName();
|
||||
}
|
||||
|
||||
|
||||
// /**
|
||||
// * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
|
||||
// * deviations based on number of tags, estimation strategy, and distance from the tags.
|
||||
// *
|
||||
// * @param estimatedPose The estimated pose to guess standard deviations for.
|
||||
// * @param targets All targets in this camera frame
|
||||
// */
|
||||
// private void updateEstimationStdDevs(
|
||||
// Optional<EstimatedRobotPose> estimatedPose,
|
||||
// List<PhotonTrackedTarget> targets,
|
||||
// PhotonPoseEstimator estimator) {
|
||||
// if (estimatedPose.isEmpty()) {
|
||||
// // No pose input. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
|
||||
// } else {
|
||||
// // Pose present. Start running Heuristic
|
||||
// var estStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// int numTags = 0;
|
||||
// double avgDist = 0;
|
||||
|
||||
// // Precalculation - see how many tags we found, and calculate an average-distance metric
|
||||
// for (var tgt : targets) {
|
||||
// var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
// if (tagPose.isEmpty()) continue;
|
||||
|
||||
// double distance = tagPose
|
||||
// .get()
|
||||
// .toPose2d()
|
||||
// .getTranslation()
|
||||
// .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
|
||||
// if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
// numTags++;
|
||||
// avgDist += distance;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (numTags == 0) {
|
||||
// // No tags visible. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// } else {
|
||||
// // One or more tags visible, run the full heuristic.
|
||||
// avgDist /= numTags;
|
||||
// // Decrease std devs if multiple targets are visible
|
||||
// if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
|
||||
// // Increase std devs based on (average) distance
|
||||
// if (numTags == 1 && avgDist > 4)
|
||||
// estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
|
||||
// else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
|
||||
// curStdDevs = estStdDevs;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
}
|
||||
@@ -1,13 +0,0 @@
|
||||
package frc4388.utility;
|
||||
|
||||
// This is a seperate class in case I want to encode rotation or other
|
||||
// information about the tag
|
||||
public class AprilTag {
|
||||
public final double x, y, z;
|
||||
|
||||
public AprilTag(double _x, double _y, double _z) {
|
||||
x = _x;
|
||||
y = _y;
|
||||
z = _z;
|
||||
}
|
||||
}
|
||||
@@ -2,22 +2,40 @@ package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
// Class for running code snippets whenever the robot is enabled.
|
||||
public class DeferredBlock {
|
||||
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
|
||||
private static ArrayList<Runnable> m_blocks_norerun = new ArrayList<>();
|
||||
private static ArrayList<Runnable> m_blocks_rerun = new ArrayList<>();
|
||||
private static boolean m_hasRun = false;
|
||||
|
||||
public DeferredBlock(Runnable block) {
|
||||
m_blocks.add(block);
|
||||
public static void addBlock(Runnable block) {
|
||||
addBlock(block, false);
|
||||
}
|
||||
|
||||
|
||||
public static void addBlock(Runnable block, boolean rerun) {
|
||||
if(rerun) {
|
||||
m_blocks_rerun.add(block);
|
||||
} else {
|
||||
m_blocks_norerun.add(block);
|
||||
}
|
||||
}
|
||||
|
||||
public static void execute() {
|
||||
if (m_hasRun) return;
|
||||
|
||||
for (Runnable block : m_blocks) {
|
||||
// Run blocks that run multiple times.
|
||||
for (Runnable block : m_blocks_rerun) {
|
||||
block.run();
|
||||
}
|
||||
|
||||
m_blocks.clear(); // for garbage collection
|
||||
// Run blocks that only run once
|
||||
if (m_hasRun) return;
|
||||
|
||||
for (Runnable block : m_blocks_norerun) {
|
||||
block.run();
|
||||
}
|
||||
|
||||
m_blocks_norerun.clear(); // for garbage collection
|
||||
m_hasRun = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,269 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.utility;
|
||||
|
||||
// import com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
|
||||
// import edu.wpi.first.wpilibj.GyroBase;
|
||||
// import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
|
||||
/**
|
||||
* Gyro class that allows for interchangeable use between a pigeon and a navX
|
||||
*/
|
||||
public class RobotGyro {
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private Pigeon2 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;
|
||||
|
||||
private double pitchZero = 0;
|
||||
private double rollZero = 0;
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a pigeon
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(Pigeon2 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets yaw, pitch, and roll.
|
||||
*/
|
||||
public void resetZeroValues() {
|
||||
if (!m_isGyroAPigeon) return;
|
||||
|
||||
// pitchZero = m_pigeon.getPitch();
|
||||
// rollZero = m_pigeon.getRoll();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public void calibrate() {
|
||||
return;
|
||||
// if (m_isGyroAPigeon) {
|
||||
// m_pigeon.calibrate();
|
||||
// } else {
|
||||
// m_navX.calibrate();
|
||||
// }
|
||||
}
|
||||
|
||||
public void reset() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(0);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void reset(double val) {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(val);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetFlip() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(180);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetNinety() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(90);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetTwoSeventy() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(270);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetRightSideBlue() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(60);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetAmpSide() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(-60);
|
||||
} 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() {
|
||||
//m_pigeon.getAngle(); // This appeared to not do anything?
|
||||
var rotation = m_pigeon.getRotation3d();
|
||||
|
||||
return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
|
||||
}
|
||||
|
||||
public Rotation2d getRotation2d() {
|
||||
return m_pigeon.getRotation2d();
|
||||
}
|
||||
|
||||
public double getAngle() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return getPigeonAngles()[2];
|
||||
} else {
|
||||
return m_navX.getAngle();
|
||||
}
|
||||
}
|
||||
|
||||
public double getYaw() {
|
||||
return this.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);
|
||||
}
|
||||
}
|
||||
|
||||
public double getRate() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
|
||||
} else {
|
||||
return m_navX.getRate();
|
||||
}
|
||||
}
|
||||
|
||||
public Pigeon2 getPigeon(){
|
||||
return m_pigeon;
|
||||
}
|
||||
|
||||
public AHRS getNavX(){
|
||||
return m_navX;
|
||||
}
|
||||
|
||||
public void close() throws Exception {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public abstract class Subsystem extends SubsystemBase {
|
||||
public static List<Subsystem> subsystems = new ArrayList<>();
|
||||
|
||||
public Subsystem(){
|
||||
subsystems.add(this);
|
||||
}
|
||||
|
||||
public void Log(String str) {
|
||||
System.out.println(getSubsystemName() + " - " + str);
|
||||
}
|
||||
|
||||
// Get name of subsystem, for use in log.
|
||||
public abstract String getSubsystemName();
|
||||
// Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard
|
||||
public abstract void queryStatus();
|
||||
// Proactivly search for any errors in each subsystem
|
||||
public abstract Status diagnosticStatus();
|
||||
}
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
package frc4388.utility;
|
||||
package frc4388.utility.compute;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
|
||||
@@ -0,0 +1,106 @@
|
||||
package frc4388.utility.compute;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.constants.Constants.FieldConstants;
|
||||
|
||||
public class ReefPositionHelper {
|
||||
public enum Side {
|
||||
LEFT,
|
||||
RIGHT,
|
||||
CENTER,
|
||||
FAR_LEFT
|
||||
}
|
||||
|
||||
public static final Pose2d[] RED_TAGS = {
|
||||
FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(11).get().toPose2d()
|
||||
};
|
||||
|
||||
public static final Pose2d[] BLUE_TAGS = {
|
||||
FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(),
|
||||
FieldConstants.kTagLayout.getTagPose(22).get().toPose2d()
|
||||
};
|
||||
|
||||
public static double distanceTo(Pose2d first, Pose2d second){
|
||||
return Math.sqrt(Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Function to loop through a list of tag locations to figure out closest one
|
||||
*/
|
||||
public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){
|
||||
if(locations.length <= 0) return new Pose2d();
|
||||
|
||||
Pose2d minPos = locations[0];
|
||||
double minDistance = distanceTo(position,minPos);
|
||||
|
||||
for(int i = 1; i < locations.length; i++){
|
||||
double dist = distanceTo(locations[i],position);
|
||||
if(dist < minDistance){
|
||||
minPos = locations[i];
|
||||
minDistance = dist;
|
||||
}
|
||||
}
|
||||
|
||||
System.out.println(minPos.getRotation().getDegrees());
|
||||
|
||||
return minPos;
|
||||
}
|
||||
|
||||
/*
|
||||
* Function to find closest tag location based on side
|
||||
*/
|
||||
public static Pose2d getNearestTag(Pose2d position) {
|
||||
|
||||
if(TimesNegativeOne.isRed)
|
||||
return getNearestTag(RED_TAGS, position);
|
||||
else
|
||||
return getNearestTag(BLUE_TAGS, position);
|
||||
}
|
||||
|
||||
public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
|
||||
return offset(getNearestTag(position),
|
||||
getSide(side) + xtrim,
|
||||
ydistance);
|
||||
}
|
||||
|
||||
public static double getSide(Side side){
|
||||
switch(side) {
|
||||
case LEFT:
|
||||
return -(AutoConstants.X_SCORING_POSITION_OFFSET);
|
||||
case FAR_LEFT:
|
||||
return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
|
||||
case RIGHT:
|
||||
return (AutoConstants.X_SCORING_POSITION_OFFSET);
|
||||
case CENTER:
|
||||
return 0;
|
||||
}
|
||||
assert false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
|
||||
Translation2d oldTranslation = oldPose.getTranslation();
|
||||
|
||||
double rot = oldPose.getRotation().getRadians();
|
||||
|
||||
return new Pose2d(new Translation2d(
|
||||
oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
|
||||
oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
|
||||
), oldPose.getRotation().rotateBy(Rotation2d.k180deg));
|
||||
}
|
||||
}
|
||||
+1
-1
@@ -5,7 +5,7 @@
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.utility;
|
||||
package frc4388.utility.compute;
|
||||
|
||||
/**
|
||||
* <p>Keeps track of Robot times like time passed, delta time, etc
|
||||
+1
-1
@@ -2,7 +2,7 @@
|
||||
// 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;
|
||||
package frc4388.utility.compute;
|
||||
|
||||
/** Aarav's good units class (better than WPILib)
|
||||
* @author Aarav Shah */
|
||||
@@ -0,0 +1,51 @@
|
||||
package frc4388.utility.compute;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
||||
|
||||
// Class that holds weather the drivers sticks should be inverted
|
||||
public class TimesNegativeOne {
|
||||
|
||||
public static boolean XAxis = SwerveDriveConstants.INVERT_X;
|
||||
public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
|
||||
public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
|
||||
public static boolean isRed = false;
|
||||
public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
|
||||
|
||||
private static boolean isRed() {
|
||||
Optional<Alliance> alliance = DriverStation.getAlliance();
|
||||
if(alliance.isEmpty()) return false;
|
||||
return (alliance.get() == Alliance.Red);
|
||||
}
|
||||
|
||||
public static void update(){
|
||||
isRed = isRed();
|
||||
XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
|
||||
YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
|
||||
RotAxis = SwerveDriveConstants.INVERT_ROTATION;
|
||||
ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
|
||||
SmartDashboard.putBoolean("Is red alliance", isRed);
|
||||
}
|
||||
|
||||
public static double invert(double num, boolean invert){
|
||||
return invert ? -num : num;
|
||||
}
|
||||
|
||||
public static Translation2d invert(Translation2d stick, boolean invertXY){
|
||||
if(invertXY) return stick.times(-1);
|
||||
else return stick;
|
||||
}
|
||||
|
||||
public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){
|
||||
return new Translation2d(
|
||||
invert(stick.getX(), invertX),
|
||||
invert(stick.getY(), invertY)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
// 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.compute;
|
||||
|
||||
import java.io.FileInputStream;
|
||||
import java.io.FileOutputStream;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
|
||||
/**
|
||||
* Reboot persistant Trims.
|
||||
* @author Zachary Wilke
|
||||
*/
|
||||
public class Trim {
|
||||
private static ArrayList<Trim> trims = new ArrayList<Trim>();
|
||||
private static ShuffleboardTab trimTab = Shuffleboard.getTab("Trims");
|
||||
|
||||
private String trimName;
|
||||
private double upperBound;
|
||||
private double lowerBound;
|
||||
private double step;
|
||||
|
||||
private boolean modified = false;
|
||||
private double currentValue;
|
||||
private boolean persistant = false;
|
||||
|
||||
private GenericEntry trimElement = null;
|
||||
|
||||
/**
|
||||
* Creates a variably Trim with a given name, upper and lower bounds, step size and intial value
|
||||
* @param trimName please keep the trim name without special symbols
|
||||
* @param upperBound the upper limit inclusive
|
||||
* @param lowerBound the lower limit inclusive
|
||||
* @param step the step size
|
||||
* @param inital the inital value, will get overridden if the persistant trim exists on disk.
|
||||
* @param persistnat Weather the trim is persistant or not
|
||||
*/
|
||||
public Trim(String trimName, double upperBound, double lowerBound, double step, double inital, boolean persistant) {
|
||||
this.trimName = trimName;
|
||||
this.upperBound = upperBound;
|
||||
this.lowerBound = lowerBound;
|
||||
this.step = step;
|
||||
this.persistant = persistant;
|
||||
currentValue = inital;
|
||||
load();
|
||||
trimElement = trimTab.add(trimName, currentValue).getEntry();
|
||||
|
||||
trims.add(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a non-Trim with a given name, upper and lower bounds, step size and intial value
|
||||
* @param trimName please keep the trim name without special symbols
|
||||
* @param upperBound the upper limit inclusive
|
||||
* @param lowerBound the lower limit inclusive
|
||||
* @param step the step size
|
||||
* @param inital the inital value, will get overridden if the persistant trim exists on disk.
|
||||
*/
|
||||
public Trim(String trimName, double upperBound, double lowerBound, double step, double inital) {
|
||||
this.trimName = trimName;
|
||||
this.upperBound = upperBound;
|
||||
this.lowerBound = lowerBound;
|
||||
this.step = step;
|
||||
currentValue = inital;
|
||||
load();
|
||||
trimElement = trimTab.add(trimName, currentValue).getEntry();
|
||||
|
||||
trims.add(this);
|
||||
}
|
||||
|
||||
private void clampModify() {
|
||||
currentValue = Math.min(upperBound, Math.max(currentValue, lowerBound));
|
||||
if (trimElement != null)
|
||||
trimElement.setValue(currentValue);
|
||||
modified = true;
|
||||
}
|
||||
|
||||
public void stepUp() {
|
||||
this.currentValue += step;
|
||||
clampModify();
|
||||
}
|
||||
|
||||
public void stepDown() {
|
||||
this.currentValue -= step;
|
||||
clampModify();
|
||||
}
|
||||
|
||||
public void set(double value) {
|
||||
this.currentValue = value;
|
||||
clampModify();
|
||||
}
|
||||
|
||||
public double get() {
|
||||
return this.currentValue;
|
||||
}
|
||||
|
||||
public boolean isModified() {
|
||||
return modified;
|
||||
}
|
||||
|
||||
public boolean load() {
|
||||
if(!persistant)
|
||||
return false;
|
||||
|
||||
try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
|
||||
double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
|
||||
currentValue = fileValue;
|
||||
clampModify();
|
||||
modified = false;
|
||||
if (fileValue != currentValue) {
|
||||
System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
|
||||
modified = true;
|
||||
}
|
||||
return true;
|
||||
} catch (Exception e) {
|
||||
// e.printStackTrace();
|
||||
System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void dump() {
|
||||
try (FileOutputStream stream = new FileOutputStream("/home/lvuser/trims/" + trimName)) {
|
||||
stream.write(DataUtils.doubleToByteArray(currentValue));
|
||||
modified = false;
|
||||
} catch (Exception e) {
|
||||
// e.printStackTrace();
|
||||
System.out.println("TRIMS: Unable to write to trim file `" + trimName + "`!?!");
|
||||
}
|
||||
}
|
||||
|
||||
public static void dumpAll() {
|
||||
for (int i = 0; i < trims.size(); i++) {
|
||||
Trim trim = trims.get(i);
|
||||
if (trim.isModified())
|
||||
trim.dump();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
|
||||
public class ButtonBox extends GenericHID {
|
||||
public static final int White = 1;
|
||||
public static final int One = 2;
|
||||
public static final int Two = 3;
|
||||
public static final int Three = 4;
|
||||
public static final int Four = 5;
|
||||
public static final int Five = 6;
|
||||
public static final int Six = 7;
|
||||
public static final int Seven = 8;
|
||||
public static final int Eight = 9;
|
||||
|
||||
public ButtonBox(int ID){
|
||||
super(ID);
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,7 @@
|
||||
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
import static frc4388.robot.constants.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
@@ -0,0 +1,10 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
import edu.wpi.first.wpilibj.Alert;
|
||||
import edu.wpi.first.wpilibj.Alert.AlertType;
|
||||
import frc4388.robot.RobotContainer;
|
||||
|
||||
// Class to update a series of WPILIB Alerts
|
||||
public class Alerts {
|
||||
public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError);
|
||||
}
|
||||
+7
-11
@@ -1,13 +1,9 @@
|
||||
package frc4388.utility;
|
||||
package frc4388.utility.status;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.hal.CANData;
|
||||
import edu.wpi.first.hal.can.CANJNI;
|
||||
import edu.wpi.first.wpilibj.CAN;
|
||||
import frc4388.utility.Status.Report;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class CanDevice {
|
||||
public static List<CanDevice> devices = new ArrayList<>();
|
||||
@@ -35,13 +31,13 @@ public class CanDevice {
|
||||
System.out.println(getName() + " - " + str);
|
||||
}
|
||||
|
||||
public Status queryStatus() {
|
||||
Status s = new Status();
|
||||
// public Status queryStatus() {
|
||||
// Status s = new Status();
|
||||
|
||||
s.addReport(ReportLevel.INFO, "TODO");
|
||||
// s.addReport(ReportLevel.INFO, "TODO");
|
||||
|
||||
return s;
|
||||
}
|
||||
// return s;
|
||||
// }
|
||||
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
@@ -0,0 +1,76 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
import com.ctre.phoenix6.controls.EmptyControl;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class FaultCANCoder implements Queryable {
|
||||
private String name;
|
||||
private CANcoder cancoder;
|
||||
|
||||
public static void addDevice(CANcoder cancoder, String name) {
|
||||
FaultCANCoder p = new FaultCANCoder();
|
||||
|
||||
p.name = name;
|
||||
p.cancoder = cancoder;
|
||||
|
||||
FaultReporter.register(p);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
boolean debounceBad = !QueryUtils.isDebounceOk(cancoder.getSupplyVoltage());
|
||||
boolean emptyControlBad = cancoder.setControl(new EmptyControl()).value != 0;
|
||||
|
||||
if(debounceBad || emptyControlBad) {
|
||||
s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" +
|
||||
(debounceBad ? " Failed debounce test" : "") +
|
||||
(emptyControlBad ? " Failed empty control test" : "")
|
||||
);
|
||||
}
|
||||
|
||||
// faults
|
||||
if (cancoder.getFault_Hardware().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Hardware fault detected");
|
||||
}
|
||||
if (cancoder.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Device booted while enabled");
|
||||
}
|
||||
if (cancoder.getFault_BadMagnet().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Bad magnet");
|
||||
}
|
||||
if (cancoder.getFault_Undervoltage().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Device supply voltage near brownout");
|
||||
}
|
||||
if (cancoder.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
|
||||
}
|
||||
|
||||
// sticky faults
|
||||
if (cancoder.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Hardware fault detected");
|
||||
}
|
||||
if (cancoder.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled");
|
||||
}
|
||||
if (cancoder.getStickyFault_BadMagnet().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Bad magnet");
|
||||
}
|
||||
if (cancoder.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout");
|
||||
}
|
||||
if (cancoder.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
|
||||
}
|
||||
|
||||
return s;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
import org.photonvision.PhotonCamera;
|
||||
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class FaultPhotonCamera implements Queryable {
|
||||
private String name;
|
||||
private PhotonCamera cam;
|
||||
|
||||
public static void addDevice(PhotonCamera cam, String name) {
|
||||
FaultPhotonCamera p = new FaultPhotonCamera();
|
||||
|
||||
p.name = name;
|
||||
p.cam = cam;
|
||||
|
||||
FaultReporter.register(p);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
if(!cam.isConnected())
|
||||
s.addReport(ReportLevel.ERROR, "Not Connected!");
|
||||
|
||||
return s;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,168 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
import com.ctre.phoenix6.controls.EmptyControl;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class FaultPidgeon2 implements Queryable {
|
||||
private String name;
|
||||
private Pigeon2 pigeon2;
|
||||
|
||||
public static void addDevice(Pigeon2 pigeon2, String name) {
|
||||
FaultPidgeon2 p = new FaultPidgeon2();
|
||||
|
||||
p.name = name;
|
||||
p.pigeon2 = pigeon2;
|
||||
|
||||
FaultReporter.register(p);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
|
||||
|
||||
boolean debounceBad = !QueryUtils.isDebounceOk(pigeon2.getSupplyVoltage());
|
||||
boolean emptyControlBad = pigeon2.setControl(new EmptyControl()).value != 0;
|
||||
|
||||
if(debounceBad || emptyControlBad) {
|
||||
s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" +
|
||||
(debounceBad ? " Failed debounce test" : "") +
|
||||
(emptyControlBad ? " Failed empty control test" : "")
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
s.addReport(ReportLevel.INFO, "Voltage: " + pigeon2.getSupplyVoltage());
|
||||
|
||||
s.addReport(ReportLevel.INFO, "Pitch: " + pigeon2.getPitch());
|
||||
s.addReport(ReportLevel.INFO, "Yaw: " + pigeon2.getYaw());
|
||||
s.addReport(ReportLevel.INFO, "Roll: " + pigeon2.getRoll());
|
||||
|
||||
s.addReport(ReportLevel.INFO, "Acceleration X: " + pigeon2.getAccelerationX());
|
||||
s.addReport(ReportLevel.INFO, "Acceleration Y: " + pigeon2.getAccelerationY());
|
||||
s.addReport(ReportLevel.INFO, "Acceleration Z: " + pigeon2.getAccelerationZ());
|
||||
|
||||
s.addReport(ReportLevel.INFO, "Magnomiter X: " + pigeon2.getMagneticFieldX());
|
||||
s.addReport(ReportLevel.INFO, "Magnomiter Y: " + pigeon2.getMagneticFieldY());
|
||||
s.addReport(ReportLevel.INFO, "Magnomiter Z: " + pigeon2.getMagneticFieldZ());
|
||||
|
||||
|
||||
// faults
|
||||
if (pigeon2.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Device booted while enabled");
|
||||
}
|
||||
if (pigeon2.getFault_BootIntoMotion().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Device booted while in motion");
|
||||
}
|
||||
if (pigeon2.getFault_BootupAccelerometer().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Accelerometer fault detected");
|
||||
}
|
||||
if (pigeon2.getFault_BootupGyroscope().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Gyro fault detected");
|
||||
}
|
||||
if (pigeon2.getFault_BootupMagnetometer().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Magnetometer fault detected");
|
||||
}
|
||||
if (pigeon2.getFault_DataAcquiredLate().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR,
|
||||
"Motion stack data acquisition slower than expected");
|
||||
}
|
||||
if (pigeon2.getFault_Hardware().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Hardware fault detected");
|
||||
}
|
||||
if (pigeon2.getFault_LoopTimeSlow().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR,
|
||||
"Motion stack loop time was slower than expected");
|
||||
}
|
||||
if (pigeon2.getFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "Accelerometer values are saturated");
|
||||
}
|
||||
if (pigeon2.getFault_SaturatedGyroscope().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Gyro values are saturated");
|
||||
}
|
||||
if (pigeon2.getFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "Magnetometer values are saturated");
|
||||
}
|
||||
if (pigeon2.getFault_Undervoltage().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "Device supply voltage near brownout");
|
||||
}
|
||||
if (pigeon2.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
|
||||
}
|
||||
|
||||
// sticky faults
|
||||
if (pigeon2.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "[STICKY] Device booted while enabled");
|
||||
}
|
||||
if (pigeon2.getStickyFault_BootIntoMotion().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "[STICKY] Device booted while in motion");
|
||||
}
|
||||
if (pigeon2.getStickyFault_BootupAccelerometer().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "[STICKY] Accelerometer fault detected");
|
||||
}
|
||||
if (pigeon2.getStickyFault_BootupGyroscope().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Gyro fault detected");
|
||||
}
|
||||
if (pigeon2.getStickyFault_BootupMagnetometer().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "[STICKY] Magnetometer fault detected");
|
||||
}
|
||||
if (pigeon2.getStickyFault_DataAcquiredLate().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR,
|
||||
String.format(
|
||||
"[STICKY] Motion stack data acquisition slower than expected"));
|
||||
}
|
||||
if (pigeon2.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "[STICKY] Hardware fault detected");
|
||||
}
|
||||
if (pigeon2.getStickyFault_LoopTimeSlow().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR,
|
||||
String.format(
|
||||
"[STICKY] Motion stack loop time was slower than expected"));
|
||||
}
|
||||
if (pigeon2.getStickyFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR,
|
||||
"[STICKY] Accelerometer values are saturated");
|
||||
}
|
||||
if (pigeon2.getStickyFault_SaturatedGyroscope().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "[STICKY] Gyro values are saturated");
|
||||
}
|
||||
if (pigeon2.getStickyFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR,
|
||||
"[STICKY] Magnetometer values are saturated");
|
||||
}
|
||||
if (pigeon2.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR,
|
||||
"[STICKY] Device supply voltage near brownout");
|
||||
}
|
||||
if (pigeon2.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
|
||||
s.addReport(
|
||||
ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
|
||||
}
|
||||
|
||||
return s;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
import com.ctre.phoenix6.CANBus.CANBusStatus;
|
||||
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.utility.status.Status.Report;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class FaultReporter {
|
||||
|
||||
private static final String REPORTS_HEADER =
|
||||
"###############\n" + //
|
||||
"#.............#\n" + //
|
||||
"#...Reports...#\n" + //
|
||||
"#.............#\n" + //
|
||||
"###############\n";
|
||||
|
||||
|
||||
private static final String CAN_HEADER =
|
||||
"###############\n" + //
|
||||
"#.............#\n" + //
|
||||
"#....CAN(t)...#\n" + //
|
||||
"#.............#\n" + //
|
||||
"###############\n";
|
||||
|
||||
private static final String ERROR_HEADER =
|
||||
"###############\n" + //
|
||||
"#.............#\n" + //
|
||||
"#....ERRORS...#\n" + //
|
||||
"#.............#\n" + //
|
||||
"###############\n";
|
||||
|
||||
private static List<Queryable> queryables = new ArrayList<>();
|
||||
|
||||
// public static void startThread() {
|
||||
// new Thread() {
|
||||
// public void run() {
|
||||
// try{
|
||||
// while(!this.isInterrupted() && this.isAlive()){
|
||||
// Thread.sleep(500);
|
||||
// for(int i=0;i<queryables.size(); i++){
|
||||
// queryables.get(i).queryStatus();
|
||||
// }
|
||||
|
||||
// // System.out.println("Updated statuses!");
|
||||
|
||||
// }
|
||||
// }catch(Exception e){
|
||||
// e.printStackTrace();
|
||||
// }
|
||||
// }
|
||||
// }.start();
|
||||
// }
|
||||
|
||||
public static void register(Queryable q) {
|
||||
queryables.add(q);
|
||||
}
|
||||
|
||||
private static void Log(Queryable q, String s){
|
||||
System.out.println(q.getName() + " - " + s);
|
||||
}
|
||||
|
||||
public static void printReport() {
|
||||
|
||||
List<String> errors = new ArrayList<>();
|
||||
|
||||
// Subsystems header
|
||||
System.out.println(REPORTS_HEADER);
|
||||
|
||||
for(int i=0;i< queryables.size();i++){
|
||||
|
||||
Queryable q = queryables.get(i);
|
||||
System.out.println("** Subsystem diagnostic report for " + q.getName() + ":");
|
||||
Status status = q.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(a);
|
||||
if(r.reportLevel == ReportLevel.ERROR)
|
||||
errors.add(q.getName() + " - " + r.toString());
|
||||
Log(q, r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// CAN header
|
||||
System.out.println(CAN_HEADER);
|
||||
|
||||
CANBus canBus = new CANBus(Constants.CANBUS_NAME);
|
||||
|
||||
CANBusStatus canInfo = canBus.getStatus();
|
||||
|
||||
System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount);
|
||||
System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization);
|
||||
System.out.println("CANInfo RX Errors count - " + canInfo.REC);
|
||||
System.out.println("CANInfo TX Errors count - " + canInfo.TEC);
|
||||
System.out.println("CANInfo Transmit buffer full count - " + canInfo.TxFullCount);
|
||||
// Broken turniary operator
|
||||
ReportLevel canReportLevel = canInfo.Status.isOK() ? (canInfo.Status.isWarning() ? ReportLevel.WARNING : ReportLevel.ERROR) : ReportLevel.INFO;
|
||||
String canStatus = "CAN " + canReportLevel.name() + " - " + canInfo.Status.getName() + " (" + canInfo.Status.getDescription() + ")";
|
||||
if(canReportLevel == ReportLevel.ERROR) {
|
||||
errors.add(canStatus);
|
||||
}
|
||||
System.out.println(canStatus);
|
||||
|
||||
// for(int i=0;i<CanDevice.devices.size();i++){
|
||||
|
||||
// CanDevice device = CanDevice.devices.get(i);
|
||||
// System.out.println("** CAN diagnostic report for " + device.name + ":");
|
||||
// Status status = device.diagnosticStatus();
|
||||
|
||||
// for(int a=0;a<status.reports.size();a++){
|
||||
// Report r = status.reports.get(a);
|
||||
// if(r.reportLevel == ReportLevel.ERROR)
|
||||
// errors.add(device.getName() + " - " + r.toString());
|
||||
// device.Log(r.toString());
|
||||
// }
|
||||
// }
|
||||
|
||||
// System.out.println("Found CAN devices: " + new DeviceFinder().Find());
|
||||
|
||||
if(errors.size() > 0) {
|
||||
// Errors header
|
||||
System.out.println(ERROR_HEADER);
|
||||
for(int i=0;i<errors.size(); i++){
|
||||
System.out.println(errors.get(i));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,154 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
import com.ctre.phoenix6.controls.EmptyControl;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class FaultTalonFX implements Queryable {
|
||||
private String name;
|
||||
private TalonFX motor;
|
||||
|
||||
public static void addDevice(TalonFX motor, String name) {
|
||||
FaultTalonFX p = new FaultTalonFX();
|
||||
|
||||
p.name = name;
|
||||
p.motor = motor;
|
||||
|
||||
FaultReporter.register(p);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
|
||||
boolean debounceBad = !QueryUtils.isDebounceOk(motor.getSupplyVoltage());
|
||||
boolean emptyControlBad = motor.setControl(new EmptyControl()).value != 0;
|
||||
|
||||
if(debounceBad || emptyControlBad) {
|
||||
s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" +
|
||||
(debounceBad ? " Failed debounce test" : "") +
|
||||
(emptyControlBad ? " Failed empty control test" : "")
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
s.addReport(ReportLevel.INFO, "Voltage: " + motor.getSupplyVoltage());
|
||||
s.addReport(ReportLevel.INFO, "Current: " + motor.getSupplyCurrent());
|
||||
s.addReport(ReportLevel.INFO, "Device temp: " + motor.getDeviceTemp());
|
||||
s.addReport(ReportLevel.INFO, "Processor temp: " + motor.getProcessorTemp());
|
||||
s.addReport(ReportLevel.INFO, "Position: " + motor.getPosition());
|
||||
s.addReport(ReportLevel.INFO, "Velocity: " + motor.getVelocity());
|
||||
s.addReport(ReportLevel.INFO, "Acceleration: " + motor.getAcceleration());
|
||||
|
||||
// faults<
|
||||
if (motor.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Device booted while enabled");
|
||||
}
|
||||
if (motor.getFault_BridgeBrownout().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Bridge was disabled most likely due to supply voltage dropping too low");
|
||||
}
|
||||
if (motor.getFault_DeviceTemp().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Device temperature exceeded limit");
|
||||
}
|
||||
if (motor.getFault_FusedSensorOutOfSync().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Remote sensor is out of sync");
|
||||
}
|
||||
if (motor.getFault_Hardware().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Hardware failure detected");
|
||||
}
|
||||
if (motor.getFault_MissingDifferentialFX().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "The remote Talon used for differential control is not present");
|
||||
}
|
||||
if (motor.getFault_MissingHardLimitRemote().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR,"The remote limit switch device is not present");
|
||||
}
|
||||
if (motor.getFault_MissingSoftLimitRemote().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "The remote soft limit device is not present");
|
||||
}
|
||||
if (motor.getFault_OverSupplyV().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Supply voltage exceeded limit");
|
||||
}
|
||||
if (motor.getFault_ProcTemp().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Processor temperature exceeded limit");
|
||||
}
|
||||
if (motor.getFault_RemoteSensorDataInvalid().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "The remote sensor's data is no longer trusted");
|
||||
}
|
||||
if (motor.getFault_RemoteSensorPosOverflow().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "remote sensor position has overflowed");
|
||||
}
|
||||
if (motor.getFault_RemoteSensorReset().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "The remote sensor has reset");
|
||||
}
|
||||
if (motor.getFault_Undervoltage().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, " Device supply voltage near brownout");
|
||||
}
|
||||
if (motor.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
|
||||
}
|
||||
if (motor.getFault_UnstableSupplyV().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "Supply voltage is unstable");
|
||||
}
|
||||
|
||||
|
||||
// sticky faults
|
||||
if (motor.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled");
|
||||
}
|
||||
if (motor.getStickyFault_BridgeBrownout().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Bridge was disabled most likely due to supply voltage dropping too low");
|
||||
}
|
||||
if (motor.getStickyFault_DeviceTemp().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Device temperature exceeded limit");
|
||||
}
|
||||
if (motor.getStickyFault_FusedSensorOutOfSync().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Remote sensor is out of sync");
|
||||
}
|
||||
if (motor.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Hardware failure detected");
|
||||
}
|
||||
if (motor.getStickyFault_MissingDifferentialFX().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] The remote Talon used for differential control is not present");
|
||||
}
|
||||
if (motor.getStickyFault_MissingHardLimitRemote().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] The remote limit switch device is not present");
|
||||
}
|
||||
if (motor.getStickyFault_MissingSoftLimitRemote().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] The remote soft limit device is not present");
|
||||
}
|
||||
if (motor.getStickyFault_OverSupplyV().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Supply voltage exceeded limit");
|
||||
}
|
||||
if (motor.getStickyFault_ProcTemp().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Processor temperature exceeded limit");
|
||||
}
|
||||
if (motor.getStickyFault_RemoteSensorDataInvalid().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor's data is no longer trusted");
|
||||
}
|
||||
if (motor.getStickyFault_RemoteSensorPosOverflow().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor position has overflowed");
|
||||
}
|
||||
if (motor.getStickyFault_RemoteSensorReset().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor has reset");
|
||||
}
|
||||
if (motor.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout");
|
||||
}
|
||||
if (motor.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
|
||||
}
|
||||
if (motor.getStickyFault_UnstableSupplyV().getValue() == Boolean.TRUE) {
|
||||
s.addReport(ReportLevel.ERROR, "[STICKY] Supply voltage is unstable");
|
||||
}
|
||||
|
||||
|
||||
return s;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
import com.ctre.phoenix6.StatusSignal;
|
||||
|
||||
import edu.wpi.first.math.filter.Debouncer;
|
||||
|
||||
public class QueryUtils {
|
||||
public static boolean isDebounceOk(@SuppressWarnings("rawtypes") StatusSignal status) {
|
||||
Debouncer connectedDebounce = new Debouncer(0.5);
|
||||
status.refresh();
|
||||
return connectedDebounce.calculate(status.getStatus().isOK());
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
public interface Queryable {
|
||||
// Get name of subsystem, for use in log.
|
||||
String getName();
|
||||
// Proactivly search for any errors in each subsystem
|
||||
Status diagnosticStatus();
|
||||
}
|
||||
+5
-1
@@ -1,4 +1,4 @@
|
||||
package frc4388.utility;
|
||||
package frc4388.utility.status;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
@@ -44,6 +44,10 @@ public class Status {
|
||||
}
|
||||
|
||||
public void diagnoseHardwareCTRE(String deviceName, TalonFX motor) {
|
||||
addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: " + (motor.isAlive() ? "Alive." : "Dead!"));
|
||||
|
||||
|
||||
|
||||
if (motor.isAlive()) addReport(ReportLevel.INFO, deviceName + " Motor (TalonFX) Alive?: Alive.");
|
||||
else addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: Dead!");
|
||||
}
|
||||
+1
-1
@@ -2,7 +2,7 @@
|
||||
// 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;
|
||||
package frc4388.utility.structs;
|
||||
|
||||
/** Add your docs here. */
|
||||
public class Gains {
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
package frc4388.utility;
|
||||
package frc4388.utility.structs;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
package frc4388.utility;
|
||||
package frc4388.utility.structs;
|
||||
|
||||
public class UtilityStructs {
|
||||
public static class TimedOutput {
|
||||
@@ -0,0 +1,35 @@
|
||||
{
|
||||
"fileName": "AdvantageKit.json",
|
||||
"name": "AdvantageKit",
|
||||
"version": "4.1.2",
|
||||
"uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
|
||||
"frcYear": "2025",
|
||||
"mavenUrls": [
|
||||
"https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
|
||||
],
|
||||
"jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "org.littletonrobotics.akit",
|
||||
"artifactId": "akit-java",
|
||||
"version": "4.1.2"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "org.littletonrobotics.akit",
|
||||
"artifactId": "akit-wpilibio",
|
||||
"version": "4.1.2",
|
||||
"skipInvalidPlatforms": false,
|
||||
"isJar": false,
|
||||
"validPlatforms": [
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"linuxarm64",
|
||||
"osxuniversal",
|
||||
"windowsx86-64"
|
||||
]
|
||||
}
|
||||
],
|
||||
"cppDependencies": []
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
{
|
||||
"fileName": "PathplannerLib-2025.2.7.json",
|
||||
"name": "PathplannerLib",
|
||||
"version": "2025.2.7",
|
||||
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
||||
"frcYear": "2025",
|
||||
"mavenUrls": [
|
||||
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
|
||||
],
|
||||
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.pathplanner.lib",
|
||||
"artifactId": "PathplannerLib-java",
|
||||
"version": "2025.2.7"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.pathplanner.lib",
|
||||
"artifactId": "PathplannerLib-cpp",
|
||||
"version": "2025.2.7",
|
||||
"libName": "PathplannerLib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"linuxarm64"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,7 +1,7 @@
|
||||
{
|
||||
"fileName": "Phoenix6-25.1.0.json",
|
||||
"fileName": "Phoenix6-frc2025-latest.json",
|
||||
"name": "CTRE-Phoenix (v6)",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"frcYear": "2025",
|
||||
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
|
||||
"mavenUrls": [
|
||||
@@ -19,14 +19,14 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "wpiapi-java",
|
||||
"version": "25.1.0"
|
||||
"version": "25.4.0"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "api-cpp",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -40,7 +40,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "tools",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -54,7 +54,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "api-cpp-sim",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -68,7 +68,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "tools-sim",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -82,7 +82,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simTalonSRX",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -96,7 +96,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simVictorSPX",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -110,7 +110,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simPigeonIMU",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -124,7 +124,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simCANCoder",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -138,7 +138,21 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProTalonFX",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxarm64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProTalonFXS",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -152,7 +166,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANcoder",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -166,7 +180,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProPigeon2",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -180,7 +194,35 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANrange",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxarm64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANdi",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxarm64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANdle",
|
||||
"version": "25.4.0",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -196,7 +238,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "wpiapi-cpp",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_Phoenix6_WPI",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -212,7 +254,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "tools",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_PhoenixTools",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -228,7 +270,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "wpiapi-cpp-sim",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_Phoenix6_WPISim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -244,7 +286,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "tools-sim",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_PhoenixTools_Sim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -260,7 +302,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simTalonSRX",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimTalonSRX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -276,7 +318,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simVictorSPX",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimVictorSPX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -292,7 +334,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simPigeonIMU",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimPigeonIMU",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -308,7 +350,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simCANCoder",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimCANCoder",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -324,7 +366,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProTalonFX",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimProTalonFX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -337,10 +379,26 @@
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProTalonFXS",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimProTalonFXS",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxarm64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANcoder",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimProCANcoder",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -356,7 +414,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProPigeon2",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimProPigeon2",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -372,7 +430,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANrange",
|
||||
"version": "25.1.0",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimProCANrange",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@@ -384,6 +442,38 @@
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANdi",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimProCANdi",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxarm64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANdle",
|
||||
"version": "25.4.0",
|
||||
"libName": "CTRE_SimProCANdle",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxarm64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
{
|
||||
"fileName": "photonlib.json",
|
||||
"name": "photonlib",
|
||||
"version": "v2025.3.1",
|
||||
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
|
||||
"frcYear": "2025",
|
||||
"mavenUrls": [
|
||||
"https://maven.photonvision.org/repository/internal",
|
||||
"https://maven.photonvision.org/repository/snapshots"
|
||||
],
|
||||
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photontargeting-cpp",
|
||||
"version": "v2025.3.1",
|
||||
"skipInvalidPlatforms": true,
|
||||
"isJar": false,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photonlib-cpp",
|
||||
"version": "v2025.3.1",
|
||||
"libName": "photonlib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
},
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photontargeting-cpp",
|
||||
"version": "v2025.3.1",
|
||||
"libName": "photontargeting",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photonlib-java",
|
||||
"version": "v2025.3.1"
|
||||
},
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photontargeting-java",
|
||||
"version": "v2025.3.1"
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user