diff --git a/Robot/src/main/java/com/swrobotics/robot/input/Input.java b/Robot/src/main/java/com/swrobotics/robot/input/Input.java index 9f20b1b9..b6c954bd 100644 --- a/Robot/src/main/java/com/swrobotics/robot/input/Input.java +++ b/Robot/src/main/java/com/swrobotics/robot/input/Input.java @@ -72,7 +72,7 @@ public enum IntakeMode { private static final double NUDGE_PER_PERIODIC = 0.25 * 0.02; - private static final NTBoolean L_IS_CONE = new NTBoolean("Is Cone", false); + private static final NTDouble L_IS_CONE = new NTDouble("Random stuffs/Is Cone v2", 0); private final RobotContainer robot; @@ -105,13 +105,13 @@ public Input(RobotContainer robot) { () -> { gamePiece = GamePiece.CUBE; robot.messenger.prepare("Robot:GamePiece").addBoolean(false).send(); - L_IS_CONE.set(false); + L_IS_CONE.set(0.0); }); manipulator.rightBumper.onRising( () -> { gamePiece = GamePiece.CONE; robot.messenger.prepare("Robot:GamePiece").addBoolean(true).send(); - L_IS_CONE.set(true); + L_IS_CONE.set(10000.0); }); snapDriveCmd = new PathfindToPointCommand(robot, null); @@ -315,6 +315,8 @@ private void snapToAngle(Rotation2d angle) { setCommandEnabled(snapTurnCmd, angleDiff > SNAP_TURN_TOL); } + private long prevTime = System.currentTimeMillis(); + private void manipulatorPeriodic() { if (getGamePiece() == GamePiece.CONE) robot.lights.set(Lights.Color.YELLOW); else robot.lights.set(Lights.Color.BLUE); @@ -357,6 +359,8 @@ private void manipulatorPeriodic() { Translation2d armTarget = ntArmTarget == null ? robot.arm.getHomeTarget() : ntArmTarget.getTranslation(); + boolean isDefault = ntArmTarget == ArmPositions.DEFAULT; + // If it is moving to a new target if (!armTarget.equals(prevArmTarget) && (isGrid || prevWasGrid)) { armNudge = new Translation2d(0, 0); @@ -378,6 +382,14 @@ private void manipulatorPeriodic() { } robot.arm.setTargetPosition(armTarget); + + if (isDefault && robot.arm.isInTolerance()) { + long now = System.currentTimeMillis(); + if (now - prevTime >= 1000) { + robot.arm.calibrateAbsolute(); + prevTime = now; + } + } } @Override diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/arm/ArmSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/arm/ArmSubsystem.java index 16d71407..5f23f27d 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/arm/ArmSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/arm/ArmSubsystem.java @@ -158,6 +158,11 @@ public ArmSubsystem(MessengerClient msg) { }); } + // MUST only be called when arm is near home! Otherwise arm will probably break itself! + public void calibrateAbsolute() { + calibrateHome(new ArmPose(HOME_BOTTOM.get(), HOME_TOP.get())); + } + private void calibrateHome(ArmPose homePose) { bottomJoint.calibrateHome(homePose.bottomAngle); topJoint.calibrateHome(homePose.topAngle); diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/drive/DrivetrainSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/drive/DrivetrainSubsystem.java index 2f0ce2de..0b79deae 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/drive/DrivetrainSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/drive/DrivetrainSubsystem.java @@ -1,9 +1,12 @@ package com.swrobotics.robot.subsystems.drive; +import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.auto.PIDConstants; import com.pathplanner.lib.auto.SwerveAutoBuilder; import com.swrobotics.lib.net.NTBoolean; +import com.swrobotics.lib.net.NTDouble; +import com.swrobotics.lib.net.NTEntry; import com.swrobotics.mathlib.Angle; import com.swrobotics.mathlib.CCWAngle; import com.swrobotics.mathlib.CWAngle; @@ -85,10 +88,10 @@ public enum StopPosition { /* Modules that could be hot-swapped into a location on the swerve drive */ private static final SwerveModuleInfo[] SELECTABLE_MODULES = new SwerveModuleInfo[] { - new SwerveModuleInfo("Module 0", 9, 5, 1, 38.41), // Default front left - new SwerveModuleInfo("Module 1", 10, 6, 2, 185.45), // Default front right - new SwerveModuleInfo("Module 2", 11, 7, 3, 132.63), // Default back left - new SwerveModuleInfo("Module 3", 12, 8, 4, 78.93) // Default back right + new SwerveModuleInfo("Module 0", 9, 5, 1, 44.121094), // Default front left + new SwerveModuleInfo("Module 1", 10, 6, 2, 219.111328), // Default front right + new SwerveModuleInfo("Module 2", 11, 7, 3, 3.515625), // Default back left + new SwerveModuleInfo("Module 3", 12, 8, 4, 76.201172) // Default back right }; // Currently, no fifth module is built (not enough falcons) @@ -147,9 +150,9 @@ public enum StopPosition { -DRIVETRAIN_WHEELBASE_METERS / 2.0)); // Initialize a NavX over MXP port - private final ADIS16448_IMU gyro = new ADIS16448_IMU( - ADIS16448_IMU.IMUAxis.kZ, Port.kMXP, ADIS16448_IMU.CalibrationTime._4s); -// private final AHRS gyro = new AHRS(Port.kMXP); +// private final ADIS16448_IMU gyro = new ADIS16448_IMU( +// ADIS16448_IMU.IMUAxis.kZ, Port.kMXP, ADIS16448_IMU.CalibrationTime._4s); + private final AHRS gyro = new AHRS(Port.kMXP); private Rotation2d gyroOffset = new Rotation2d(0); // Subtracted to get angle // Create a field sim to view where the odometry thinks we are @@ -224,7 +227,7 @@ public DrivetrainSubsystem() { printEncoderOffsets(); } - gyro.calibrate(); +// gyro.calibrate(); // FIXME: Change back to getGyroscopeRotation odometry = @@ -239,23 +242,25 @@ public DrivetrainSubsystem() { // Keep this private - use getPose().getRotation() instead private Rotation2d getGyroscopeRotation() { - return getRawGyroscopeRotation().minus(gyroOffset); -// return gyro.getRotation2d().plus(gyroOffset); +// return Rotation2d.fromDegrees(gyro.getAngle()).plus(gyroOffset); + return gyro.getRotation2d().plus(gyroOffset); } public Translation2d getTiltAsTranslation() { // FIXME: probably wrong - return new Translation2d(gyro.getGyroAngleX(), gyro.getGyroAngleY()); -// return new Translation2d(gyro.getPitch(), -gyro.getRoll()); +// return new Translation2d(gyro.getGyroAngleX(), gyro.getGyroAngleY()); + return new Translation2d(gyro.getPitch(), -gyro.getRoll()); } private Rotation2d getRawGyroscopeRotation() { - // return gyro.getRotation2d(); - return Rotation2d.fromDegrees(gyro.getAngle()); + return gyro.getRotation2d(); +// return Rotation2d.fromDegrees(-gyro.getAngle()); } public void zeroGyroscope() { - setGyroscopeRotation(new Rotation2d()); + Pose2d currentPose = getPose(); + resetPose( + new Pose2d(currentPose.getTranslation(), getAllianceForward().ccw().rotation2d())); } /** @@ -263,7 +268,6 @@ public void zeroGyroscope() { */ private void setGyroscopeRotation(Rotation2d newRotation) { gyroOffset = getRawGyroscopeRotation().plus(newRotation); - // resetPose(new Pose2d(getPose().getTranslation(), getGyroscopeRotation())); } public void setChassisSpeeds(ChassisSpeeds speeds) { @@ -498,8 +502,19 @@ protected void onDisable() { } } + NTEntry LOG_ANGLE = new NTDouble("Log/Gyro Degrees CCW", 0).setTemporary(); + + long prevCalibTime = System.currentTimeMillis(); + @Override public void periodic() { + long now = System.currentTimeMillis(); + if (now - prevCalibTime >= 1000) { + for (SwerveModule module : modules) { + module.calibrateWithAbsoluteEncoder(); + } + prevCalibTime = now; + } // Check if it should use auto for some or all of the movement if (translation.getNorm() != 0.0) { @@ -511,6 +526,8 @@ public void periodic() { speeds.omegaRadiansPerSecond = rotation.getRadians(); } + LOG_ANGLE.set(getPose().getRotation().getDegrees()); + SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, 4.0); double vx = speeds.vxMetersPerSecond; diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/drive/SwerveModule.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/drive/SwerveModule.java index 692bd026..b3b63dd9 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/drive/SwerveModule.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/drive/SwerveModule.java @@ -63,6 +63,12 @@ public class SwerveModule { public SwerveModule( SwerveModuleInfo moduleInfo, Translation2d position, double positionalOffset) { + try { + Thread.sleep(1000); + } catch (InterruptedException e) { + e.printStackTrace(); + } + this.position = position; this.positionalOffset = positionalOffset; @@ -111,6 +117,12 @@ public SwerveModule( setState(new SwerveModuleState()); + try { + Thread.sleep(1000); + } catch (InterruptedException e) { + e.printStackTrace(); + } + turnEncoderToAngle = ENCODER_TICKS_PER_ROTATION * TURN_MOTOR_GEAR_RATIO / (Math.PI * 2); // driveEncoderVelocityToMPS = ((600.0 / 2048.0) / DRIVE_MOTOR_GEAR_RATIO * @@ -210,7 +222,7 @@ public double getDriveVelocity() { return fromNativeDriveVelocityUnits(driveEncoder.getVelocity()); } - private void calibrateWithAbsoluteEncoder() { + public void calibrateWithAbsoluteEncoder() { turnEncoder.setPosition(toNativeTurnUnits(getAbsoluteAngle())); } diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/intake/IntakeSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/intake/IntakeSubsystem.java index d3049dde..a1c1ab43 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/intake/IntakeSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/intake/IntakeSubsystem.java @@ -5,6 +5,7 @@ import com.swrobotics.robot.subsystems.SwitchableSubsystemBase; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.motorcontrol.PWMTalonFX; public final class IntakeSubsystem extends SwitchableSubsystemBase { private static final NTDouble L_INTAKE_SPEED = @@ -12,13 +13,15 @@ public final class IntakeSubsystem extends SwitchableSubsystemBase { private static final NTDouble CONE_HOLD = new NTDouble("Intake/Cone Hold", 0.1); private static final NTDouble CUBE_HOLD = new NTDouble("Intake/Cube Hold", -0.1); - private final PWMSparkMax motor; +// private final PWMSparkMax motor; + private final PWMTalonFX motor; private GamePiece expectedPiece; private boolean running; public IntakeSubsystem() { - motor = new PWMSparkMax(RIOPorts.INTAKE_PWM); +// motor = new PWMSparkMax(RIOPorts.INTAKE_PWM); + motor = new PWMTalonFX(RIOPorts.INTAKE_PWM); expectedPiece = GamePiece.CUBE; running = false; diff --git a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/ShuffleLog.java b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/ShuffleLog.java index 4de572a7..2e4ca6b4 100644 --- a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/ShuffleLog.java +++ b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/ShuffleLog.java @@ -16,6 +16,7 @@ import com.swrobotics.shufflelog.tool.taskmanager.TaskManagerTool; import edu.wpi.first.math.WPIMathJNI; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTablesJNI; import edu.wpi.first.util.CombinedRuntimeLoader; import edu.wpi.first.util.WPIUtilJNI; @@ -139,14 +140,25 @@ public void setup() { tools.add(msg); tools.add(new ShuffleLogProfilerTool(this)); DataLogTool dataLog = new DataLogTool(this); + ConeOrCubeTool coneOrCubeTool = new ConeOrCubeTool(messenger); tools.add(dataLog); - tools.add(new NetworkTablesTool(threadPool)); + tools.add(new NetworkTablesTool(threadPool, new NetworkTablesTool.AAAAAAA() { + @Override + public void onInit(NetworkTableInstance instance) { + coneOrCubeTool.updateNT(instance); + } + + @Override + public void onStop() { + coneOrCubeTool.stopNT(); + } + })); tools.add(new TaskManagerTool(this, "TaskManager")); tools.add(new RoboRIOFilesTool(this)); tools.add(new FieldViewTool(this)); tools.add(new ArmDebugTool(this, messenger)); tools.add(new PreMatchChecklistTool(msg)); - tools.add(new ConeOrCubeTool(messenger)); + tools.add(coneOrCubeTool); startTime = System.currentTimeMillis(); } diff --git a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/ConeOrCubeTool.java b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/ConeOrCubeTool.java index aa6e8407..e3c6c44d 100644 --- a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/ConeOrCubeTool.java +++ b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/ConeOrCubeTool.java @@ -2,42 +2,64 @@ import com.swrobotics.messenger.client.MessengerClient; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; import imgui.ImGui; import imgui.ImVec2; import imgui.flag.ImGuiColorEditFlags; public final class ConeOrCubeTool implements Tool { + // FIXME: VERY BAD + public static double VALUE = -1000; + private static final String MESSAGE = "Robot:GamePiece"; private static final int CONE_COLOR = ImGui.getColorU32(1, 1, 0, 1); private static final int CUBE_COLOR = ImGui.getColorU32(0.25f, 0.25f, 1, 1); // Null is not connected - private Boolean isCone; +// private Boolean isCone; + private NetworkTableEntry isConeEnt; public ConeOrCubeTool(MessengerClient msg) { - isCone = null; + isConeEnt = null; + - msg.addHandler( - MESSAGE, - (type, data) -> { - isCone = data.readBoolean(); - }); +// msg.addHandler( +// MESSAGE, +// (type, data) -> { +// isCone = data.readBoolean(); +// }); +// +// msg.addDisconnectHandler(() -> isCone = null); + } + + public void updateNT(NetworkTableInstance instance) { +// isConeSub = instance.getBooleanTopic("Is Cone").subscribe(false); + isConeEnt = instance.getEntry("Random stuffs/Is Cone v2"); + System.out.println("UPDATE NTTTT"); + } - msg.addDisconnectHandler(() -> isCone = null); + public void stopNT() { + isConeEnt = null; + System.out.println("STOP NTTTTTTTTTTT"); } @Override public void process() { if (ImGui.begin("Cone or Cube")) { - if (isCone == null) { + double v; + if (isConeEnt == null || (v = VALUE) < 0) { ImGui.textDisabled("Unknown"); ImGui.end(); return; } float[] color; - if (isCone) color = new float[] {1, 1, 0, 1}; +// isConeEnt.setBoolean(Math.random() > 0.5); + if (v > 0) color = new float[] {1, 1, 0, 1}; else color = new float[] {0.25f, 0.25f, 1, 1}; ImVec2 size = ImGui.getContentRegionAvail(); diff --git a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesConnection.java b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesConnection.java index 5fc10bae..bc73ded0 100644 --- a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesConnection.java +++ b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesConnection.java @@ -75,8 +75,9 @@ public int getColor() { private NetworkTableRepr rootTable; private final AtomicInteger activeInstances; + private final NetworkTablesTool.AAAAAAA aaaaaaa; - public NetworkTablesConnection(ExecutorService threadPool) { + public NetworkTablesConnection(ExecutorService threadPool, NetworkTablesTool.AAAAAAA aaaaaaa) { this.threadPool = threadPool; instance = null; stopFuture = null; @@ -84,6 +85,7 @@ public NetworkTablesConnection(ExecutorService threadPool) { params = null; activeInstances = new AtomicInteger(0); + this.aaaaaaa = aaaaaaa; } public void setServerParams(boolean isNt4, Params params) { @@ -102,6 +104,8 @@ public void setServerParams(boolean isNt4, Params params) { rootTable = new NetworkTableRepr(instance.getTable("/")); this.params = params; + + aaaaaaa.onInit(instance); } // If the current client already satisfies the desired parameters, @@ -119,6 +123,7 @@ public void setServerParams(boolean isNt4, Params params) { // Save a reference to the current instance and clear instance variable so // the instance can't be used after closing NetworkTableInstance savedInstance = instance; + aaaaaaa.onStop(); instance = null; // Stop on other thread because stopping the client can take around diff --git a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesTool.java b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesTool.java index 1c2ee363..8489f748 100644 --- a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesTool.java +++ b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesTool.java @@ -1,5 +1,6 @@ package com.swrobotics.shufflelog.tool.data.nt; +import com.swrobotics.shufflelog.tool.ConeOrCubeTool; import com.swrobotics.shufflelog.tool.Tool; import com.swrobotics.shufflelog.tool.data.DataLogTool; import com.swrobotics.shufflelog.tool.data.PlotDef; @@ -62,14 +63,19 @@ public final class NetworkTablesTool implements Tool { private final ImDouble tempDouble = new ImDouble(); private final ImString tempString = new ImString(1024); - public NetworkTablesTool(ExecutorService threadPool) { + public interface AAAAAAA { + void onInit(NetworkTableInstance instance); + void onStop(); + } + + public NetworkTablesTool(ExecutorService threadPool, AAAAAAA aaaaaaa) { version = new ImInt(DEFAULT_VERSION); connectionMode = new ImInt(DEFAULT_CONN_MODE); host = new ImString(64); host.set(DEFAULT_HOST); portOrTeamNumber = new ImInt(getDefaultPortOrTeamNumber()); - connection = new NetworkTablesConnection(threadPool); + connection = new NetworkTablesConnection(threadPool, aaaaaaa); } // --- Server connection --- @@ -548,8 +554,23 @@ private void showData() { } } + // FIXME: This is horrendously terrible + private void findIsCone(NetworkTableRepr repr) { + for (NetworkTableValueRepr value : repr.getValues()) { + if (value.getName().equals("Is Cone v2")) { + ConeOrCubeTool.VALUE = value.sub.getDouble(-1000); + } + } + + for (NetworkTableRepr subtable : repr.getSubtables()) { + findIsCone(subtable); + } + } + @Override public void process() { + if (connection.getRootTable() != null) + findIsCone(connection.getRootTable()); if (ImGui.begin(TITLE)) { ImGui.text("Instances: " + connection.getActiveInstances()); showConnectionInfo();