fixed adv kit errors

Team3128 · Sep 30, 2024 · 354741f · 354741f
1 parent 6a0a9ec
commit 354741f
Show file tree

Hide file tree

Showing 11 changed files with 62 additions and 259 deletions.
diff --git a/src/main/java/common/core/misc/NAR_Robot.java b/src/main/java/common/core/misc/NAR_Robot.java
@@ -4,7 +4,6 @@
 import java.lang.management.ManagementFactory;
 import java.lang.reflect.Method;
 import java.time.LocalDateTime;
-import java.util.LinkedList;
 import java.util.List;
 import java.util.PriorityQueue;
 
@@ -22,14 +21,26 @@
 import edu.wpi.first.wpilibj.Timer;
 
 /**
- * Team 3128's Robot class that includes advantageScope and addPeriodic
+ * Team 3128's Custom Robot class
  * 
+ * <p> Includes AdvantageKit and addPeriodic()
+ * 
+ * <p>NOTES:
+ * <ul>
+ * <li> TimedRobot implements the IterativeRobotBase robot program framework.
+ * <li> The TimedRobot class is intended to be subclassed by a user creating a robot program.
+ * <li> Periodic() functions from the base class are called on an interval by a Notifier instance.
+ * <li> AdvantageKit logging is disabled by default. 
+ * </ul>
+ * 
+ * <p> ----------------------------------------------
+ *  
  * @since 2023 Charged Up
- * @author Mason Lam
+ * @author Mason Lam, Wallim
  */
 public class NAR_Robot extends IterativeRobotBase {
 
-    public static boolean logWithAdvantageKit = true;
+    public static boolean logWithAdvantageKit = false;
 
     @SuppressWarnings("MemberName")
     static class Callback implements Comparable<Callback> {
@@ -87,9 +98,9 @@ public int compareTo(Callback rhs) {
 
     private static final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
 
-    private final Method periodicAfterUser0;
+    private Method periodicAfterUser0 = null;
 
-    private final GcStatsCollector gcStatsCollector = new GcStatsCollector();
+    private GcStatsCollector gcStatsCollector = new GcStatsCollector();
 
     /** Constructor for TimedRobot. */
     protected NAR_Robot() {
@@ -105,33 +116,40 @@ protected NAR_Robot(double period) {
         super(period);
         m_startTime = Timer.getFPGATimestamp();
 
-        Method periodicBeforeUser = null; // Method to get the periodicBeforeUser method from Logger
-        Method periodicAfterUser = null; // Method to get the periodicAfterUser method from Logger
-        try {
-            periodicBeforeUser = Logger.class.getDeclaredMethod("periodicBeforeUser");
-            periodicAfterUser = Logger.class.getDeclaredMethod("periodicAfterUser", long.class, long.class);
-        } catch (NoSuchMethodException | SecurityException e) {
-            e.printStackTrace();
-        }
+        if(logWithAdvantageKit) {
+            Method periodicBeforeUser = null; // Method to get the periodicBeforeUser method from Logger
+            Method periodicAfterUser = null; // Method to get the periodicAfterUser method from Logger
+            try {
+                periodicBeforeUser = Logger.class.getDeclaredMethod("periodicBeforeUser");
+                periodicAfterUser = Logger.class.getDeclaredMethod("periodicAfterUser", long.class, long.class);
+            } catch (NoSuchMethodException | SecurityException e) {
+                e.printStackTrace();
+            }
 
-        periodicBeforeUser.setAccessible(true); // set the method to be accessible
-        periodicAfterUser.setAccessible(true); // set the method to be accessible
+            periodicBeforeUser.setAccessible(true); // set the method to be accessible
+            periodicAfterUser.setAccessible(true); // set the method to be accessible
+
+            Method periodicBeforeUser0 = periodicBeforeUser;
+            periodicAfterUser0 = periodicAfterUser;
+
+            addPeriodic(() -> {
+                try {
+                    long loopCycleStart = Logger.getRealTimestamp();
+                    if (logWithAdvantageKit) periodicBeforeUser0.invoke(null);
+                    long userCodeStart = Logger.getRealTimestamp();
+                    loopFunc();
+                    long loopCycleEnd = Logger.getRealTimestamp();
+                    if (logWithAdvantageKit) gcStatsCollector.update();
+                    if (logWithAdvantageKit) periodicAfterUser0.invoke(null, loopCycleEnd - userCodeStart, userCodeStart - loopCycleStart);
+                } catch (Exception e) {
+                }
+            }, period);
 
-        final Method periodicBeforeUser0 = periodicBeforeUser;
-        periodicAfterUser0 = periodicAfterUser;
+        } else {
+            addPeriodic(this::loopFunc, period);
+        }
 
-        addPeriodic(() -> {
-            try {
-                long loopCycleStart = Logger.getRealTimestamp();
-                if (logWithAdvantageKit) periodicBeforeUser0.invoke(null);
-                long userCodeStart = Logger.getRealTimestamp();
-                loopFunc();
-                long loopCycleEnd = Logger.getRealTimestamp();
-                if (logWithAdvantageKit) gcStatsCollector.update();
-                if (logWithAdvantageKit) periodicAfterUser0.invoke(null, loopCycleEnd - userCodeStart, userCodeStart - loopCycleStart);
-            } catch (Exception e) {
-            }
-        }, period);
+
         NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
 
         HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed);
@@ -146,14 +164,17 @@ public void close() {
     /** Provide an alternate "main loop" via startCompetition(). */
     @Override
     public void startCompetition() {
-        long initStart = Logger.getRealTimestamp();
+        long initStart = 0;
+        if(logWithAdvantageKit) initStart = Logger.getRealTimestamp();
+
         robotInit();
 
         if (isSimulation()) {
             simulationInit();
         }
 
-        long initEnd = Logger.getRealTimestamp();
+        long initEnd = 0;
+        if(logWithAdvantageKit) initEnd = Logger.getRealTimestamp();
 
         if (logWithAdvantageKit) {
             try {

diff --git a/src/main/java/common/core/misc/NAR_Robot2.java b/src/main/java/common/core/misc/NAR_Robot2.java
diff --git a/src/main/java/common/core/subsystems/ElevatorTemplate.java b/src/main/java/common/core/subsystems/ElevatorTemplate.java
@@ -78,6 +78,11 @@ public Command runElevator(double power){
      * @return Command that resets the elevator position.
      */
     public Command reset(double position) {
-        return runOnce(()-> motors[0].resetPosition(position));
+        return runOnce(()-> {
+
+        for(NAR_Motor motor : motors){
+            motor.resetPosition(position);
+        }
+    });
     }
 }
diff --git a/src/main/java/common/core/subsystems/NAR_PIDSubsystem.java b/src/main/java/common/core/subsystems/NAR_PIDSubsystem.java
@@ -12,7 +12,6 @@
 import common.utility.tester.Tester.SystemsTest;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 

diff --git a/src/main/java/common/core/swerve/Dep-SwerveModuleConstants.java b/src/main/java/common/core/swerve/Dep-SwerveModuleConstants.java
diff --git a/src/main/java/common/core/swerve/SwerveBase.java b/src/main/java/common/core/swerve/SwerveBase.java
@@ -21,9 +21,6 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-/**
- * To do fix this garbage
- */
 public abstract class SwerveBase extends SubsystemBase {
 
     public boolean chassisVelocityCorrection = true;
@@ -87,7 +84,7 @@ public void drive(ChassisSpeeds velocity) {
                                         twistVel.dtheta / dtConstant);
         }
         setModuleStates(kinematics.toSwerveModuleStates(velocity));
-        Logger.recordOutput("Swerve/DesiredModuleStates", kinematics.toSwerveModuleStates(velocity));
+        if(NAR_Robot.logWithAdvantageKit) Logger.recordOutput("Swerve/DesiredModuleStates", kinematics.toSwerveModuleStates(velocity));
     }
 
     /**

diff --git a/src/main/java/common/hardware/camera/Camera.java b/src/main/java/common/hardware/camera/Camera.java
@@ -5,7 +5,6 @@
 import java.util.function.BiConsumer;
 import java.util.function.Supplier;
 import java.util.ArrayList;
-import java.util.HashMap;
 
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;