From b97f993e7d54cf4ba7540784900fe3b46c67c853 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 15 Mar 2022 14:23:37 -0600 Subject: [PATCH 1/2] desmos shi --- src/main/java/frc4388/robot/Constants.java | 3 +- .../ExtenderIntakeGroup.java | 4 +- .../commands/ShooterCommands/TrackTarget.java | 13 +++-- .../frc4388/utility/desmos/DesmosClient.html | 58 +++++++++++++++++-- .../frc4388/utility/desmos/DesmosServer.java | 38 ++++++++++++ 5 files changed, 103 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c20669b..bd25aab 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; } } diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 2e9748f..7eb3572 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java +++ b/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; } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 1b62d45..4fee5c8 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/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); } diff --git a/src/main/java/frc4388/utility/desmos/DesmosClient.html b/src/main/java/frc4388/utility/desmos/DesmosClient.html index f8a77f3..ca3170c 100644 --- a/src/main/java/frc4388/utility/desmos/DesmosClient.html +++ b/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); } diff --git a/src/main/java/frc4388/utility/desmos/DesmosServer.java b/src/main/java/frc4388/utility/desmos/DesmosServer.java index e3ee55b..cefe858 100644 --- a/src/main/java/frc4388/utility/desmos/DesmosServer.java +++ b/src/main/java/frc4388/utility/desmos/DesmosServer.java @@ -23,6 +23,8 @@ public class DesmosServer extends Thread { private static HashMap desmosQueue = new HashMap<>(); private static HashMap 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++; + } + // --------------------------------------------------------------------- /** From 98a82fb9de3ff008dbc457bdbfd9012b0346c2eb Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 15 Mar 2022 18:27:37 -0600 Subject: [PATCH 2/2] Added buttons --- src/main/java/frc4388/robot/RobotContainer.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cec97f5..8b43872 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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%