Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

src/main/java/frc4388/robot/Constants.java

@@ -243,7 +243,8 @@ public final class Constants {
 
     public static final double RANGE = 10;
 
-    public static final double LIMELIGHT_RADIUS = 1.d;
+    public static final double EDGE_TO_CENTER = 20;
+    public static final double LIMELIGHT_RADIUS = 8;
     public static final double SHOOTER_CORRECTION = 1.d;
   }
 }

src/main/java/frc4388/robot/RobotContainer.java

@@ -264,9 +264,9 @@ public class RobotContainer {
     //     .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55)))
     //     .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
       
-    new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //20ft
-        .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000)))
-        .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
+    // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //20ft
+    //     .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000)))
+    //     .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
         .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
@@ -319,9 +319,9 @@ public class RobotContainer {
       //     .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret));
 
        //B > Shoot with Lime
-    // new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-        // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
-        // .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)));
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+        .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
+        .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)));
         // .whileHeld%
 
   

src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java

@@ -19,11 +19,11 @@ public class ExtenderIntakeGroup extends ParallelRaceGroup {
   public ExtenderIntakeGroup(Intake intake, Extender extender) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
-    ExtenderIntakeGroup.direction = 1;
+    ExtenderIntakeGroup.direction = 1; // Does this make sense? It kind of defeats the purpose of making it static, does it work without?
     addCommands(new RunIntakeConditionally(intake), new RunExtender(extender));
   }
 
-  public static void changeDirection() {
+  public static void changeDirection() { // Never implemented?
     ExtenderIntakeGroup.direction *= -1;
   }
 }

src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java

@@ -86,28 +86,29 @@ public class TrackTarget extends CommandBase {
       y_rot += Math.toRadians(VisionConstants.LIME_ANGLE);
 
       double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot);
-      
       DesmosServer.putDouble("distance", distance);
 
       updateRegressionDesmos();
       double regressedDistance = distanceRegression(distance);
+      regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
       DesmosServer.putDouble("distanceReg", regressedDistance);
 
       //Vision odemetry circle fit based pose estimate
       Point targetOffset = m_visionOdometry.getTargetOffset();
       DesmosServer.putPoint("targetOff", targetOffset);
 
+      vel = m_boomBoom.getVelocity(distance);
+      hood = m_boomBoom.getHood(distance);
+      // m_boomBoom.runDrumShooter(vel);
+      m_boomBoom.runDrumShooterVelocityPID(vel);
+      m_hood.runAngleAdjustPID(hood);
+
       // isExecuted = true;
     }
     catch (Exception e){
       e.printStackTrace();
       // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java");
     }
-    // vel = m_boomBoom.getVelocity(distance);
-    // hood = m_boomBoom.getHood(distance);
-    // m_boomBoom.runDrumShooter(vel);
-    // m_boomBoom.runDrumShooterVelocityPID(vel);
-    // m_hood.runAngleAdjustPID(hood);
     // m_turret.runshooterRotatePID(m_targetAngle);
   }
 

src/main/java/frc4388/utility/desmos/DesmosClient.html

@@ -27,7 +27,8 @@
             const EXPRESSION_TYPES = {
                 'expression': ['number', 'point', 'array'],
                 'note': ['note'],
-                'table': ['table']
+                'table': ['table'],
+                'clear': ['all', 'except', 'single']
             };
 
             let status = document.getElementById('status');
@@ -35,6 +36,8 @@
             let elt = document.getElementById('calculator');
             let calculator = Desmos.GraphingCalculator(elt);
 
+            const clearState = calculator.getState();
+
             let helperExpressions = {};
             let variables = [];
 
@@ -72,6 +75,8 @@
                     } else if(EXPRESSION_TYPES['table'].includes(variable['type'])) {
                         let cols = getColumns(variable['value'].split(' '));
                         calculator.setExpression({ type: 'table', id: variable['name'], columns: cols});
+                    } else if(EXPRESSION_TYPES['clear'].includes(variable['type'])) {
+                        clear(variable['value'], variable['type']);
                     } else
                         console.log('Invalid input type : ' + variable['type']);
                 }
@@ -92,6 +97,47 @@
                 return cols;
             }
 
+            function clear(lname, type) {
+                let state = calculator.getState();
+                let expressions = state.expressions.list;
+
+                if(expressions.length === 0)
+                    return;
+
+                let lnames = lname.split(',');
+
+                switch(type) {
+                    case 'all':
+                        state = clearState;
+                        break;
+                    case 'except':
+                        for(let i = 0; i < expressions.length; i++) {
+                            if(!expressions[i].latex.includes('='))
+                                return;
+                            
+                            let expressionName = regularName(expressions[i].latex.split('=')[0]);
+                            if(!lnames.includes(expressionName)) {
+                                expressions.splice(i, 1);
+                                i--;
+                            }
+                        }
+                        break;
+                    case 'single':
+                        for(let i = 0; i < expressions.length; i++) {
+                            if(!expressions[i].latex.includes('='))
+                                return;
+
+                            if(lnames.includes(regularName(expressions[i].split('=')[0]))) {
+                                expressions.splice(i, 1);
+                                i--;
+                            }
+                        }
+                        break;
+                }
+
+                calculator.setState(state);
+            }
+
             function getVariables() {
                 let vars = [];
                 let expressions = calculator.getExpressions();
@@ -116,13 +162,14 @@
                     return 'point';
                 else if(raw.match(/\[[a-zA-Z0-9.,_{}\(\)]*\]/))
                     return 'array';
-                else if(raw.includes('='))
-                    return 'number';
                 else
-                    return 'none';
+                    return 'number';
             }
 
             function readVariable(latex) {
+                if(!latex.includes('='))
+                    return [];
+
                 let vars = [];
                 let lname = latex.split('=')[0];
                 let lvalue = latex.split('=')[1];
@@ -191,6 +238,9 @@
             }
 
             function regularName(lname) {
+                if(lname.length == 1)
+                    return lname;
+
                 return lname.substring(0, 1) + lname.substring(3, lname.length - 1);
             }
 

src/main/java/frc4388/utility/desmos/DesmosServer.java

@@ -23,6 +23,8 @@ public class DesmosServer extends Thread {
     private static HashMap<String, String[]> desmosQueue = new HashMap<>();
     private static HashMap<String, String[]> readVariables = new HashMap<>();
 
+    private static int clearCount = 0;
+
     private static boolean running = false;
     
     private ServerSocket serverSocket;
@@ -127,6 +129,8 @@ public class DesmosServer extends Thread {
         clientOutput.write(getJSONOutput().getBytes());
         clientOutput.flush();
         clientOutput.close();
+
+        clearCount = 0;
     }
 
     /**
@@ -245,6 +249,40 @@ public class DesmosServer extends Thread {
         desmosQueue.put(name, new String[] {"table", tableStr});
     }
 
+    /**
+     * Clears entire expression panel
+     */
+    public static void clearAll() {
+        desmosQueue.put("clear" + clearCount, new String[] {"all", "n/a"});
+        clearCount++;
+    }
+
+    /**
+     * Clears entire expression panel except for expressions in {@code names}
+     * 
+     * @param names Names which should be preserved in clear
+     */
+    public static void clearExcept(String... names) {
+        String namesStr = Arrays.toString(names).replace(" ", "");
+        namesStr = namesStr.substring(1, namesStr.length()-1);
+
+        desmosQueue.put("clear" + clearCount, new String[] {"except", namesStr});
+        clearCount++;
+    }
+
+    /**
+     * Removes expressions in {@code names}
+     * 
+     * @param names Names which should be removed
+     */
+    public static void clearSpecific(String... names) {
+        String namesStr = Arrays.toString(names).replace(" ", "");
+        namesStr = namesStr.substring(1, namesStr.length()-1);
+        
+        desmosQueue.put("clear" + clearCount, new String[] {"specific", namesStr});
+        clearCount++;
+    }
+
     // ---------------------------------------------------------------------
 
     /**