Merge pull request #163 from SouthwestRoboticsProgramming/neo_drive

Neo drive (again for some reason)

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

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);

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,31 +242,32 @@ 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()));
     }
 
     /**
      * @param newRotation New gyro rotation, CCW +
      */
     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<Double> 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;

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()));
     }
 

Robot/src/main/java/com/swrobotics/robot/subsystems/intake/IntakeSubsystem.java

@@ -5,20 +5,23 @@
 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 =
             (NTDouble) new NTDouble("Intake/Current Speed", 0).setTemporary();
     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;

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();
     }

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();

ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/data/nt/NetworkTablesConnection.java

@@ -75,15 +75,17 @@ 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;
         isNt4 = null;
         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