Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Ryan Manley
2022-03-13 17:58:06 -06:00
parent cfdac7f069 8e13b5c69f
commit df7b577852
3 changed files with 541 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+5
View File
@@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.utility.DesmosServer;
import frc4388.utility.RobotTime; import frc4388.utility.RobotTime;
import frc4388.utility.VelocityCorrection; import frc4388.utility.VelocityCorrection;
@@ -46,6 +47,8 @@ public class Robot extends TimedRobot {
private Pose2d selectedOdo; private Pose2d selectedOdo;
private double current; private double current;
private static DesmosServer desmosServer = new DesmosServer(8000);
/** /**
* This function is run when the robot is first started up and should be * This function is run when the robot is first started up and should be
* used for any initialization code. * used for any initialization code.
@@ -116,6 +119,8 @@ public class Robot extends TimedRobot {
return "Not Running"; return "Not Running";
} }
}); });
desmosServer.start();
} }
/** /**
src/main/java/frc4388/utility/DesmosClient.html
+222
View File
@@ -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>
src/main/java/frc4388/utility/DesmosServer.java
+314
View File
@@ -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;
}
}