mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Added desmos server
This commit is contained in:
66945
@@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import frc4388.utility.DesmosServer;
|
||||
import frc4388.utility.RobotTime;
|
||||
import frc4388.utility.VelocityCorrection;
|
||||
|
||||
@@ -46,6 +47,8 @@ public class Robot extends TimedRobot {
|
||||
private Pose2d selectedOdo;
|
||||
private double current;
|
||||
|
||||
private static DesmosServer desmosServer = new DesmosServer(8000);
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be
|
||||
* used for any initialization code.
|
||||
@@ -116,6 +119,8 @@ public class Robot extends TimedRobot {
|
||||
return "Not Running";
|
||||
}
|
||||
});
|
||||
|
||||
desmosServer.start();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -0,0 +1,222 @@
|
||||
<!DOCTYPE html>
|
||||
<html>
|
||||
<head>
|
||||
<script src="https://www.desmos.com/api/v1.6/calculator.js?apiKey=dcb31709b452b1cf9dc26972add0fda6"></script>
|
||||
<style>
|
||||
#status {
|
||||
position: absolute;
|
||||
right: 0;
|
||||
bottom: 0;
|
||||
width: 60pt;
|
||||
height: 20pt;
|
||||
|
||||
background-color: rgba(0, 0, 0, .8);
|
||||
|
||||
font-family: Arial, Helvetica, sans-serif;
|
||||
|
||||
text-align: center;
|
||||
line-height: 20pt;
|
||||
}
|
||||
</style>
|
||||
<title>Desmos Client</title>
|
||||
</head>
|
||||
<body>
|
||||
<div id="calculator" style="width: 100wh; height: 100vh; position: absolute; top: 0; left: 0; right: 0; bottom: 0; overflow: hidden;"></div>
|
||||
<div id="status" style="color: #00FF00;">Active</div>
|
||||
<script>
|
||||
const EXPRESSION_TYPES = {
|
||||
'expression': ['number', 'point', 'array'],
|
||||
'note': ['note'],
|
||||
'table': ['table']
|
||||
};
|
||||
|
||||
let status = document.getElementById('status');
|
||||
|
||||
let elt = document.getElementById('calculator');
|
||||
let calculator = Desmos.GraphingCalculator(elt);
|
||||
|
||||
let helperExpressions = {};
|
||||
let variables = [];
|
||||
|
||||
let url = prompt("Robot URL", "localhost");
|
||||
let port = 8000;
|
||||
|
||||
function requestVariables() {
|
||||
let http = new XMLHttpRequest();
|
||||
http.addEventListener("load", setVariables);
|
||||
http.addEventListener("error", handleError);
|
||||
http.open("POST", `http://${url}:${port}`);
|
||||
|
||||
let json = serverStringify(getVariables());
|
||||
http.send(json);
|
||||
}
|
||||
|
||||
function handleError() {
|
||||
status.style = 'color: #FF0000;';
|
||||
status.innerHTML = 'Offline';
|
||||
|
||||
console.log('Request failed');
|
||||
setTimeout(requestVariables, 0);
|
||||
}
|
||||
|
||||
function setVariables() {
|
||||
status.style = 'color: #00FF00;';
|
||||
status.innerHTML = 'Active';
|
||||
|
||||
variables = JSON.parse(this.responseText);
|
||||
|
||||
for(let variable of variables) {
|
||||
if(EXPRESSION_TYPES['expression'].includes(variable['type'])) {
|
||||
variable['lname'] = latexName(variable['name']);
|
||||
calculator.setExpression({ id: variable['name'], latex: variable['lname'] + '=' + variable['value']});
|
||||
} else if(EXPRESSION_TYPES['table'].includes(variable['type'])) {
|
||||
let cols = getColumns(variable['value'].split(' '));
|
||||
calculator.setExpression({ type: 'table', id: variable['name'], columns: cols});
|
||||
} else
|
||||
console.log('Invalid input type : ' + variable['type']);
|
||||
}
|
||||
|
||||
setTimeout(requestVariables, 0);
|
||||
}
|
||||
|
||||
function getColumns(unparsedCols) {
|
||||
let cols = [];
|
||||
for(let col of unparsedCols) {
|
||||
col = col.split(',');
|
||||
let latexStr = latexName(col[0]);
|
||||
let valuesArr = col.slice(1, col.length);
|
||||
|
||||
cols.push({ latex: latexStr, values: valuesArr });
|
||||
}
|
||||
|
||||
return cols;
|
||||
}
|
||||
|
||||
function getVariables() {
|
||||
let vars = [];
|
||||
let expressions = calculator.getExpressions();
|
||||
|
||||
for(expression of expressions) {
|
||||
if(expression.type === 'expression')
|
||||
vars = readVariable(expression.latex);
|
||||
else if(expression.type === 'table')
|
||||
vars = readTable(expression);
|
||||
else
|
||||
console.log('Invalid output type : ' + expression.type);
|
||||
}
|
||||
|
||||
return vars;
|
||||
}
|
||||
|
||||
function findExpressionType(lvalue) {
|
||||
let raw = lvalue.replace('\\left', '')
|
||||
.replace('\\right', '');
|
||||
|
||||
if(raw.match(/\([a-zA-Z0-9._{}\(\)]*,[a-zA-Z0-9._{}\(\)]*\)/))
|
||||
return 'point';
|
||||
else if(raw.match(/\[[a-zA-Z0-9.,_{}\(\)]*\]/))
|
||||
return 'array';
|
||||
else
|
||||
return 'number';
|
||||
}
|
||||
|
||||
function readVariable(latex) {
|
||||
let vars = [];
|
||||
let lname = latex.split('=')[0];
|
||||
let lvalue = latex.split('=')[1];
|
||||
|
||||
if(lname && lname != '' && lvalue && lvalue != '') {
|
||||
let name = regularName(lname);
|
||||
let value = regularValue(lvalue);
|
||||
|
||||
let type = findExpressionType(lvalue);
|
||||
|
||||
let numericValue, numericValueX, numericValueY, listValue;
|
||||
switch(type) {
|
||||
case 'number':
|
||||
numericValue = getNumericValue(name, lname);
|
||||
break;
|
||||
case 'point':
|
||||
numericValueX = getNumericValue(name + 'x', lname + '.x');
|
||||
numericValueY = getNumericValue(name + 'y', lname + '.y');
|
||||
break;
|
||||
case 'array':
|
||||
listValue = getListValue(name, lname);
|
||||
break;
|
||||
}
|
||||
|
||||
if(numericValue)
|
||||
vars.push({"name": name, "type": "number", "value": numericValue});
|
||||
if(numericValueX && numericValueY)
|
||||
vars.push({"name": name, "type": "point", "value": numericValueX + ',' + numericValueY});
|
||||
if(listValue)
|
||||
vars.push({"name": name, "type": "array", "value": listValue.toString()});
|
||||
}
|
||||
|
||||
return vars;
|
||||
}
|
||||
|
||||
function readTable(expression) {
|
||||
let vars = [];
|
||||
|
||||
for(let column of expression.columns) {
|
||||
let name = regularName(column.latex);
|
||||
let listValue = getListValue(name, column.latex);
|
||||
|
||||
if(listValue)
|
||||
vars.push({"name": name, "type": "array", "value": listValue.toString()});
|
||||
}
|
||||
|
||||
return vars;
|
||||
}
|
||||
|
||||
function getNumericValue(key, lname) {
|
||||
if(!(key in helperExpressions))
|
||||
helperExpressions[key] = calculator.HelperExpression({ latex: lname });
|
||||
else
|
||||
return helperExpressions[key].numericValue;
|
||||
}
|
||||
|
||||
function getListValue(key, lname) {
|
||||
if(!(key in helperExpressions))
|
||||
helperExpressions[key] = calculator.HelperExpression({ latex: lname });
|
||||
else
|
||||
return helperExpressions[key].listValue;
|
||||
}
|
||||
|
||||
function latexName(name) {
|
||||
return name.substring(0, 1) + '_{' + name.substring(1, name.length) + '}';
|
||||
}
|
||||
|
||||
function regularName(lname) {
|
||||
return lname.substring(0, 1) + lname.substring(3, lname.length - 1);
|
||||
}
|
||||
|
||||
function regularValue(lvalue) {
|
||||
let value = lvalue.replace('\\left(', '')
|
||||
.replace('\\right)', '')
|
||||
.replace('\\left[', '')
|
||||
.replace('\\right]', '')
|
||||
.replace('(', '')
|
||||
.replace(')', '')
|
||||
.replace('[', '')
|
||||
.replace(']', '');
|
||||
return value;
|
||||
}
|
||||
|
||||
function serverStringify(vars) {
|
||||
let stringified = '';
|
||||
|
||||
for(let variable of vars) {
|
||||
stringified += variable['name'] + '\t'
|
||||
+ variable['type'] + '\t'
|
||||
+ variable['value'] + '\n';
|
||||
}
|
||||
|
||||
return stringified;
|
||||
}
|
||||
|
||||
setTimeout(requestVariables, 0);
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,314 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.io.BufferedReader;
|
||||
import java.io.IOException;
|
||||
import java.io.InputStreamReader;
|
||||
import java.io.OutputStream;
|
||||
import java.net.ServerSocket;
|
||||
import java.net.Socket;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.HashMap;
|
||||
import java.util.HashSet;
|
||||
import java.util.Set;
|
||||
|
||||
import org.opencv.core.Point;
|
||||
|
||||
/**
|
||||
* A http server that allows the robot to communicate with Desmos Graphing Calculator
|
||||
*
|
||||
* @author Daniel McGrath
|
||||
* */
|
||||
public class DesmosServer extends Thread {
|
||||
private static HashMap<String, String[]> desmosQueue = new HashMap<>();
|
||||
private static HashMap<String, String[]> readVariables = new HashMap<>();
|
||||
|
||||
private static boolean running = false;
|
||||
|
||||
private ServerSocket serverSocket;
|
||||
private int activePort;
|
||||
|
||||
/**
|
||||
* Creates DesmosServer running on port
|
||||
* <p>
|
||||
* Use this for cases when the robot is using the default port
|
||||
*
|
||||
* @param port The port the server will run on
|
||||
* */
|
||||
public DesmosServer(int port) {
|
||||
activePort = port;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates DesmosServer running on port 5500
|
||||
* */
|
||||
public DesmosServer() {
|
||||
activePort = 8000;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
try {
|
||||
runServer(activePort);
|
||||
} catch(IOException err) {
|
||||
err.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs server on port
|
||||
*
|
||||
* @param port The port the server runs on
|
||||
* @throws IOException
|
||||
* */
|
||||
public void runServer(int port) throws IOException {
|
||||
System.out.println("Initializing DesmosServer on port " + port + "...");
|
||||
|
||||
serverSocket = new ServerSocket(port);
|
||||
running = true;
|
||||
|
||||
System.out.println("DesmosServer is active!");
|
||||
|
||||
while(true) {
|
||||
Socket client = serverSocket.accept();
|
||||
handleClient(client);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Handles client requests
|
||||
*
|
||||
* @param client The client connection
|
||||
* @throws IOException
|
||||
* */
|
||||
public void handleClient(Socket client) throws IOException {
|
||||
InputStreamReader clientStream = new InputStreamReader(client.getInputStream());
|
||||
BufferedReader bufferedReader = new BufferedReader(clientStream);
|
||||
|
||||
ArrayList<String> headers = new ArrayList<>();
|
||||
|
||||
String header;
|
||||
while((header = bufferedReader.readLine()).length() != 0) {
|
||||
headers.add(header);
|
||||
}
|
||||
|
||||
String body = "";
|
||||
while(bufferedReader.ready()) {
|
||||
body += (char) bufferedReader.read();
|
||||
}
|
||||
|
||||
readVariables(body);
|
||||
sendResponse(client);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends JSON response with format
|
||||
* <p>
|
||||
* [
|
||||
* {"int": "24"},
|
||||
* {"double": "2.4"},
|
||||
* {"point": "(2,4)"},
|
||||
* {"list": "[2,4]"}
|
||||
* ]
|
||||
*
|
||||
* @param client The client connection
|
||||
* @throws IOException
|
||||
* */
|
||||
public void sendResponse(Socket client) throws IOException {
|
||||
OutputStream clientOutput = client.getOutputStream();
|
||||
|
||||
// Write Headers
|
||||
clientOutput.write("HTTP/1.1 200 OK\r\n".getBytes());
|
||||
clientOutput.write("Access-Control-Allow-Origin: *\r\n".getBytes());
|
||||
clientOutput.write("Keep-Alive: timeout=2, max=100\r\n".getBytes());
|
||||
clientOutput.write("Connection: Keep-Alive\r\n".getBytes());
|
||||
clientOutput.write("Content-Type: application/json\r\n\r\n".getBytes());
|
||||
|
||||
clientOutput.write(getJSONOutput().getBytes());
|
||||
clientOutput.flush();
|
||||
clientOutput.close();
|
||||
}
|
||||
|
||||
/**
|
||||
* Produces JSON output containing Desmos output.
|
||||
*
|
||||
* @return JSON string to be read by Desmos client
|
||||
* */
|
||||
public static String getJSONOutput() {
|
||||
String json = "[";
|
||||
|
||||
if(!desmosQueue.isEmpty()) {
|
||||
Set<String> keySet = new HashSet<>(desmosQueue.keySet());
|
||||
|
||||
for(String key : keySet) {
|
||||
json += "\n\t{"
|
||||
+ "\"name\":\"" + key + "\","
|
||||
+ "\"type\":\"" + desmosQueue.get(key)[0] + "\","
|
||||
+ "\"value\":\"" + desmosQueue.get(key)[1] + "\""
|
||||
+ "},";
|
||||
|
||||
desmosQueue.remove(key);
|
||||
}
|
||||
|
||||
json = json.substring(0, json.length()-1); // remove comma at the end
|
||||
}
|
||||
|
||||
json += "\n]";
|
||||
|
||||
return json;
|
||||
}
|
||||
|
||||
/**
|
||||
* Interpret client request and update variables
|
||||
*
|
||||
* @param requestBody Client request
|
||||
*/
|
||||
public static void readVariables(String requestBody) {
|
||||
for(String variable : requestBody.split("\n")) {
|
||||
if(variable.equals(""))
|
||||
break;
|
||||
|
||||
String[] readVar = variable.split("\t");
|
||||
readVariables.put(readVar[0], new String[] {readVar[1], readVar[2]});
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the server is running
|
||||
*
|
||||
* @return The server status
|
||||
*/
|
||||
public static boolean isRunning() {
|
||||
return running;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* Adds integer to desmos queue
|
||||
*
|
||||
* @param name Name of desmos variable
|
||||
* @param value Integer value
|
||||
* */
|
||||
public static void putInteger(String name, Integer value) {
|
||||
desmosQueue.put(name, new String[] {"number", value.toString()});
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds double to desmos queue
|
||||
*
|
||||
* @param name Name of desmos variable
|
||||
* @param value Double value
|
||||
* */
|
||||
public static void putDouble(String name, Double value) {
|
||||
desmosQueue.put(name, new String[] {"number", value.toString()});
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds point to desmos queue
|
||||
*
|
||||
* @param name Name of desmos variable
|
||||
* @param value Point value
|
||||
* */
|
||||
public static void putPoint(String name, Point point) {
|
||||
desmosQueue.put(name, new String[] {"point", "(" + point.x + "," + point.y + ")"});
|
||||
}
|
||||
|
||||
public static void putArray(String name, double... arr) {
|
||||
desmosQueue.put(name, new String[] {"array", Arrays.toString(arr).replace(" ", "")});
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds table to desmos queue
|
||||
*
|
||||
* @param name ID of table
|
||||
* @param id Column ID
|
||||
* @param column Double array containing column values
|
||||
* @param table Repeat id and column in sequence
|
||||
* */
|
||||
public static void putTable(String name, Object... table) {
|
||||
String tableStr = "";
|
||||
|
||||
for(int i = 0; i < table.length; i += 2) {
|
||||
// Check parameters
|
||||
if(!(table[i] instanceof String)) { return; }
|
||||
if(!(table[i+1] instanceof double[])) { return; }
|
||||
|
||||
tableStr += table[i] + ",";
|
||||
String values = Arrays.toString((double[]) table[i+1]).replace(" ", "");
|
||||
tableStr += values.substring(1, values.length() - 1);
|
||||
tableStr += ' ';
|
||||
}
|
||||
|
||||
tableStr = tableStr.substring(0, tableStr.length()-1); // remove space at the end
|
||||
|
||||
desmosQueue.put(name, new String[] {"table", tableStr});
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* Reads desmos integer
|
||||
*
|
||||
* @param name Desmos variable name
|
||||
* @return Numeric value, if variable is an expression it will be evaluated
|
||||
* <p>if variable is a double it will be cast to int
|
||||
* */
|
||||
public static int readInteger(String name) {
|
||||
if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("number"))
|
||||
return 0;
|
||||
|
||||
return (int) Double.parseDouble(readVariables.get(name)[1]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reads desmos double
|
||||
*
|
||||
* @param name Desmos variable name
|
||||
* @return Numeric value, if variable is an expression it will be evaluated
|
||||
* */
|
||||
public static double readDouble(String name) {
|
||||
if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("number"))
|
||||
return 0;
|
||||
|
||||
return Double.parseDouble(readVariables.get(name)[1]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reads desmos point
|
||||
*
|
||||
* @param name Desmos variable name
|
||||
* @return Point, if variable contains expressions they will be evaluated
|
||||
* */
|
||||
public static Point readPoint(String name) {
|
||||
Point point = new Point();
|
||||
|
||||
if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("point"))
|
||||
return point;
|
||||
|
||||
String pointStr = readVariables.get(name)[1];
|
||||
point.x = Double.parseDouble(pointStr.split(",")[0]);
|
||||
point.y = Double.parseDouble(pointStr.split(",")[1]);
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reads desmos array, including table columns
|
||||
*
|
||||
* @param name Desmos variable name
|
||||
* @returns Array of numeric values, if array contains expressions they will be evaluated
|
||||
* */
|
||||
public static double[] readArray(String name) {
|
||||
if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("array"))
|
||||
return new double[0];
|
||||
|
||||
String[] unparsed = readVariables.get(name)[1].split(",");
|
||||
double[] arr = new double[unparsed.length];
|
||||
|
||||
for(int i = 0; i < arr.length; i++)
|
||||
arr[i] = Integer.parseInt(unparsed[i]);
|
||||
|
||||
return arr;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user