diff --git a/.github/linters/checkstyle.xml b/.github/linters/checkstyle.xml index 9ed851b5..2cefdd3b 100644 --- a/.github/linters/checkstyle.xml +++ b/.github/linters/checkstyle.xml @@ -3,212 +3,191 @@ "-//Checkstyle//DTD Checkstyle Configuration 1.3//EN" "https://checkstyle.org/dtds/configuration_1_3.dtd"> - + - - + + - + - + - + - - + + - + - + - + - + - + - + - + - + - + - - - - + + + + - - + + - - - - + + + + - + - - - + + + - - - - - - - - - - - + + + + + + + + + + + - - - + + + - + - + - - + + - + - - - - - - - - - + + + + + + + + + - - - - - + + + + + - + - - + + - + - - - - - + + + + + - + - - + + - + - - - - - - - - - + + + + + + + + + - + - + - - - + + + - + - - - - + + + + - - + + - - - - - + + + + + - \ No newline at end of file + diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 0640fdd1..468ab529 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -4,4 +4,4 @@ Nicholas Chang Adrian Deutscher-Bishop Rajit Ghosh Debajit Ghosh -Max Spier \ No newline at end of file +Max Spier diff --git a/build.gradle b/build.gradle index 16195267..24a2d9c0 100644 --- a/build.gradle +++ b/build.gradle @@ -183,7 +183,7 @@ allprojects { } spotless { - ratchetFrom 'origin/main' // only check files that have changed relative to main + ratchetFrom 'origin/master' // only check files that have changed relative to main java { target fileTree('.') { include '**/*.java' diff --git a/docs/sample.md b/docs/sample.md index e04c21cd..e0ec5576 100644 --- a/docs/sample.md +++ b/docs/sample.md @@ -9,7 +9,7 @@ * Point 3 > Multiple -> +> > Line > Blockquote @@ -19,8 +19,8 @@ Block of Code: public class Mechanism { - // ... - } + // ... + } ### Image ![Image](https://imgs.xkcd.com/comics/compiling.png) diff --git a/docs/template.md b/docs/template.md index 499979d9..d233ac8f 100644 --- a/docs/template.md +++ b/docs/template.md @@ -28,7 +28,7 @@ Explain the initialization of the mechanism and its parameters. ### Running mechanism -How to use the mechanism? What procedures do what? +How to use the mechanism? What procedures do what? ### Integrating mechanism with other code diff --git a/src/main/java/com/team766/controllers/PIDController.java b/src/main/java/com/team766/controllers/PIDController.java index c4b2b626..a1a678e1 100755 --- a/src/main/java/com/team766/controllers/PIDController.java +++ b/src/main/java/com/team766/controllers/PIDController.java @@ -27,308 +27,342 @@ * @author Blevenson * */ - public class PIDController { - private int printCounter = 0; - private boolean print = false; - - private final ValueProvider Kp; - private final ValueProvider Ki; - private final ValueProvider Kd; - private final ValueProvider Kff; - private final ValueProvider maxoutput_low; - private final ValueProvider maxoutput_high; - private final ValueProvider endthreshold; - - private double setpoint = Double.NaN; - - private boolean needsUpdate = true; - - private double cur_error = 0; - private double prev_error = 0; - private double error_rate = 0; - private double total_error = 0; - - private double output_value = 0; - - TimeProviderI timeProvider; - private double lastTime; - - public static PIDController loadFromConfig(String configPrefix) { - if (!configPrefix.endsWith(".")) { - configPrefix += "."; - } - return new PIDController(ConfigFileReader.getInstance().getDouble(configPrefix + "pGain"), - ConfigFileReader.getInstance().getDouble(configPrefix + "iGain"), - ConfigFileReader.getInstance().getDouble(configPrefix + "dGain"), - ConfigFileReader.getInstance().getDouble(configPrefix + "ffGain"), - ConfigFileReader.getInstance().getDouble(configPrefix + "outputMaxLow"), - ConfigFileReader.getInstance().getDouble(configPrefix + "outputMaxHigh"), - ConfigFileReader.getInstance().getDouble(configPrefix + "threshold")); - } - - /** - * - * @param P P constant - * @param I I constant - * @param D D constant - * @param outputmax_low Largest output in the negative direction - * @param outputmax_high Largest output in the positive direction - * @param threshold threshold for declaring the PID 'done' - */ - public PIDController(final double P, final double I, final double D, final double outputmax_low, - final double outputmax_high, final double threshold) { - Kp = new SetValueProvider(P); - Ki = new SetValueProvider(I); - Kd = new SetValueProvider(D); - Kff = new SetValueProvider(); - maxoutput_low = new SetValueProvider(outputmax_low); - maxoutput_high = new SetValueProvider(outputmax_high); - endthreshold = new SetValueProvider(threshold); - setTimeProvider(RobotProvider.getTimeProvider()); - } - - public PIDController(final double P, final double I, final double D, final double FF, - final double outputmax_low, final double outputmax_high, final double threshold) { - this(P, I, D, outputmax_low, outputmax_high, threshold); - ((SetValueProvider) Kff).set(FF); - } - - private void setTimeProvider(final TimeProviderI timeProvider_) { - this.timeProvider = timeProvider_; - lastTime = timeProvider.get(); - } - - public PIDController(final ValueProvider P, final ValueProvider I, - final ValueProvider D, final ValueProvider FF, - final ValueProvider outputmax_low, final ValueProvider outputmax_high, - final ValueProvider threshold) { - Kp = P; - Ki = I; - Kd = D; - Kff = FF; - maxoutput_low = outputmax_low; - maxoutput_high = outputmax_high; - endthreshold = threshold; - setTimeProvider(RobotProvider.getTimeProvider()); - } - - /** - * Constructs a PID controller, with the specified P,I,D values, along with the end threshold. - * - * @param P Proportional value used in the PID controller - * @param I Integral value used in the PID controller - * @param D Derivative value used in the PID controller - * @param threshold the end threshold for declaring the PID 'done' - * @param timeProvider - */ - public PIDController(final double P, final double I, final double D, final double threshold, - final TimeProviderI timeProvider_) { - Kp = new SetValueProvider(P); - Ki = new SetValueProvider(I); - Kd = new SetValueProvider(D); - Kff = new SetValueProvider(); - maxoutput_low = new SetValueProvider(); - maxoutput_high = new SetValueProvider(); - endthreshold = new SetValueProvider(threshold); - setTimeProvider(timeProvider_); - } - - public PIDController(final double P, final double I, final double D, final double FF, - final double threshold, final TimeProviderI timeProvider_) { - Kp = new SetValueProvider(P); - Ki = new SetValueProvider(I); - Kd = new SetValueProvider(D); - Kff = new SetValueProvider(FF); - maxoutput_low = new SetValueProvider(); - maxoutput_high = new SetValueProvider(); - endthreshold = new SetValueProvider(threshold); - setTimeProvider(timeProvider_); - } - - /** - * We may want to use same PID object, but with different setpoints, so this is separated from - * constructor - * - * @param set Target point to match - */ - public void setSetpoint(final double set) { - setpoint = set; - needsUpdate = true; - } - - public void disable() { - setpoint = Double.NaN; - needsUpdate = true; - reset(); - } - - /** - * If we want to set values, such as with SmartDash - * - * @param P Proportional value used in the PID controller - * @param I Integral value used in the PID controller - * @param D Derivative value used in the PID controller - */ - public void setConstants(final double P, final double I, final double D) { - ((SettableValueProvider) Kp).set(P); - ((SettableValueProvider) Ki).set(I); - ((SettableValueProvider) Kd).set(D); - needsUpdate = true; - } - - public void setP(final double P) { - ((SettableValueProvider) Kp).set(P); - needsUpdate = true; - } - - public void setI(final double I) { - ((SettableValueProvider) Ki).set(I); - needsUpdate = true; - } - - public void setD(final double D) { - ((SettableValueProvider) Kd).set(D); - needsUpdate = true; - } - - public void setFF(final double FF) { - ((SettableValueProvider) Kff).set(FF); - needsUpdate = true; - } - - /** - * Same as calculate() except that it prints debugging information - * - * @param cur_input The current input to be plugged into the PID controller - */ - public void calculateDebug(final double cur_input) { - print = true; - calculate(cur_input); - } - - /** - * Calculate PID value. Run only once per loop. Use getOutput to get output. - * - * @param cur_input Input value from sensor - */ - public void calculate(final double cur_input) { - if (Double.isNaN(setpoint)) { - // Setpoint has not been set yet. - output_value = 0; - return; - } - - cur_error = (setpoint - cur_input); - /* - * if (isDone()) { output_value = 0; pr("pid done"); return; } - */ - - double delta_time = timeProvider.get() - lastTime; - - if (delta_time > 0) { // This condition basically only false in the simulator - error_rate = (cur_error - prev_error) / delta_time; - } - - total_error += cur_error * delta_time; - - double ki = Ki.valueOr(0.0); - if ((total_error * ki) > 1) { - total_error = 1 / ki; - } else if ((total_error * ki) < -1) { - total_error = -1 / ki; - } - - double out = Kp.valueOr(0.0) * cur_error + Ki.valueOr(0.0) * total_error - + Kd.valueOr(0.0) * error_rate + Kff.valueOr(0.0) * setpoint; - prev_error = cur_error; - - pr("Pre-clip output: " + out); - - output_value = clip(out); - - needsUpdate = false; - - lastTime = timeProvider.get(); - - pr(" Total Error: " + total_error + " Current Error: " + cur_error - + " Output: " + output_value + " Setpoint: " + setpoint); - } - - public double getOutput() { - return output_value; - } - - public boolean isDone() { - final double TIME_HORIZON = 0.5; - return !needsUpdate && Math.abs(cur_error) < endthreshold.get() - && Math.abs(cur_error + error_rate * TIME_HORIZON) < endthreshold.get(); - } - - /** - * Reset all accumulated errors - */ - public void reset() { - cur_error = 0; - prev_error = 0; - error_rate = 0; - total_error = 0; - lastTime = timeProvider.get(); - needsUpdate = true; - } - - /** - * Clips value for sending to motor controllers. This deals with if you don't want to run an arm - * or wheels at full speed under PID. - * - * @param clipped - * @return clipped value, safe for setting to controllers - */ - private double clip(final double clipped) { - double out = clipped; - if (maxoutput_high.hasValue() && out > maxoutput_high.get()) { - out = maxoutput_high.get(); - } - if (maxoutput_low.hasValue() && out < maxoutput_low.get()) { - out = maxoutput_low.get(); - } - return out; - } - - public double getError() { - return total_error; - } - - public double getCurrentError() { - return cur_error; - } - - public void setMaxoutputHigh(final Double in) { - if (in == null) { - ((SettableValueProvider) maxoutput_high).clear(); - } else { - ((SettableValueProvider) maxoutput_high).set(in); - } - } - - public void setMaxoutputLow(final Double in) { - if (in == null) { - ((SettableValueProvider) maxoutput_low).clear(); - } else { - ((SettableValueProvider) maxoutput_low).set(in); - } - } - - public double getSetpoint() { - return setpoint; - } - - private void pr(final Object text) { - if (print && printCounter > 0) { - Logger.get(Category.PID_CONTROLLER).logRaw(Severity.DEBUG, "PID: " + text); - printCounter = 0; - } - printCounter++; - } - + private int printCounter = 0; + private boolean print = false; + + private final ValueProvider Kp; + private final ValueProvider Ki; + private final ValueProvider Kd; + private final ValueProvider Kff; + private final ValueProvider maxoutput_low; + private final ValueProvider maxoutput_high; + private final ValueProvider endthreshold; + + private double setpoint = Double.NaN; + + private boolean needsUpdate = true; + + private double cur_error = 0; + private double prev_error = 0; + private double error_rate = 0; + private double total_error = 0; + + private double output_value = 0; + + TimeProviderI timeProvider; + private double lastTime; + + public static PIDController loadFromConfig(String configPrefix) { + if (!configPrefix.endsWith(".")) { + configPrefix += "."; + } + return new PIDController( + ConfigFileReader.getInstance().getDouble(configPrefix + "pGain"), + ConfigFileReader.getInstance().getDouble(configPrefix + "iGain"), + ConfigFileReader.getInstance().getDouble(configPrefix + "dGain"), + ConfigFileReader.getInstance().getDouble(configPrefix + "ffGain"), + ConfigFileReader.getInstance().getDouble(configPrefix + "outputMaxLow"), + ConfigFileReader.getInstance().getDouble(configPrefix + "outputMaxHigh"), + ConfigFileReader.getInstance().getDouble(configPrefix + "threshold")); + } + + /** + * + * @param P P constant + * @param I I constant + * @param D D constant + * @param outputmax_low Largest output in the negative direction + * @param outputmax_high Largest output in the positive direction + * @param threshold threshold for declaring the PID 'done' + */ + public PIDController( + final double P, + final double I, + final double D, + final double outputmax_low, + final double outputmax_high, + final double threshold) { + Kp = new SetValueProvider(P); + Ki = new SetValueProvider(I); + Kd = new SetValueProvider(D); + Kff = new SetValueProvider(); + maxoutput_low = new SetValueProvider(outputmax_low); + maxoutput_high = new SetValueProvider(outputmax_high); + endthreshold = new SetValueProvider(threshold); + setTimeProvider(RobotProvider.getTimeProvider()); + } + + public PIDController( + final double P, + final double I, + final double D, + final double FF, + final double outputmax_low, + final double outputmax_high, + final double threshold) { + this(P, I, D, outputmax_low, outputmax_high, threshold); + ((SetValueProvider) Kff).set(FF); + } + + private void setTimeProvider(final TimeProviderI timeProvider_) { + this.timeProvider = timeProvider_; + lastTime = timeProvider.get(); + } + + public PIDController( + final ValueProvider P, + final ValueProvider I, + final ValueProvider D, + final ValueProvider FF, + final ValueProvider outputmax_low, + final ValueProvider outputmax_high, + final ValueProvider threshold) { + Kp = P; + Ki = I; + Kd = D; + Kff = FF; + maxoutput_low = outputmax_low; + maxoutput_high = outputmax_high; + endthreshold = threshold; + setTimeProvider(RobotProvider.getTimeProvider()); + } + + /** + * Constructs a PID controller, with the specified P,I,D values, along with the end threshold. + * + * @param P Proportional value used in the PID controller + * @param I Integral value used in the PID controller + * @param D Derivative value used in the PID controller + * @param threshold the end threshold for declaring the PID 'done' + * @param timeProvider + */ + public PIDController( + final double P, + final double I, + final double D, + final double threshold, + final TimeProviderI timeProvider_) { + Kp = new SetValueProvider(P); + Ki = new SetValueProvider(I); + Kd = new SetValueProvider(D); + Kff = new SetValueProvider(); + maxoutput_low = new SetValueProvider(); + maxoutput_high = new SetValueProvider(); + endthreshold = new SetValueProvider(threshold); + setTimeProvider(timeProvider_); + } + + public PIDController( + final double P, + final double I, + final double D, + final double FF, + final double threshold, + final TimeProviderI timeProvider_) { + Kp = new SetValueProvider(P); + Ki = new SetValueProvider(I); + Kd = new SetValueProvider(D); + Kff = new SetValueProvider(FF); + maxoutput_low = new SetValueProvider(); + maxoutput_high = new SetValueProvider(); + endthreshold = new SetValueProvider(threshold); + setTimeProvider(timeProvider_); + } + + /** + * We may want to use same PID object, but with different setpoints, so this is separated from + * constructor + * + * @param set Target point to match + */ + public void setSetpoint(final double set) { + setpoint = set; + needsUpdate = true; + } + + public void disable() { + setpoint = Double.NaN; + needsUpdate = true; + reset(); + } + + /** + * If we want to set values, such as with SmartDash + * + * @param P Proportional value used in the PID controller + * @param I Integral value used in the PID controller + * @param D Derivative value used in the PID controller + */ + public void setConstants(final double P, final double I, final double D) { + ((SettableValueProvider) Kp).set(P); + ((SettableValueProvider) Ki).set(I); + ((SettableValueProvider) Kd).set(D); + needsUpdate = true; + } + + public void setP(final double P) { + ((SettableValueProvider) Kp).set(P); + needsUpdate = true; + } + + public void setI(final double I) { + ((SettableValueProvider) Ki).set(I); + needsUpdate = true; + } + + public void setD(final double D) { + ((SettableValueProvider) Kd).set(D); + needsUpdate = true; + } + + public void setFF(final double FF) { + ((SettableValueProvider) Kff).set(FF); + needsUpdate = true; + } + + /** + * Same as calculate() except that it prints debugging information + * + * @param cur_input The current input to be plugged into the PID controller + */ + public void calculateDebug(final double cur_input) { + print = true; + calculate(cur_input); + } + + /** + * Calculate PID value. Run only once per loop. Use getOutput to get output. + * + * @param cur_input Input value from sensor + */ + public void calculate(final double cur_input) { + if (Double.isNaN(setpoint)) { + // Setpoint has not been set yet. + output_value = 0; + return; + } + + cur_error = (setpoint - cur_input); + /* + * if (isDone()) { output_value = 0; pr("pid done"); return; } + */ + + double delta_time = timeProvider.get() - lastTime; + + if (delta_time > 0) { // This condition basically only false in the simulator + error_rate = (cur_error - prev_error) / delta_time; + } + + total_error += cur_error * delta_time; + + double ki = Ki.valueOr(0.0); + if ((total_error * ki) > 1) { + total_error = 1 / ki; + } else if ((total_error * ki) < -1) { + total_error = -1 / ki; + } + + double out = + Kp.valueOr(0.0) * cur_error + + Ki.valueOr(0.0) * total_error + + Kd.valueOr(0.0) * error_rate + + Kff.valueOr(0.0) * setpoint; + prev_error = cur_error; + + pr("Pre-clip output: " + out); + + output_value = clip(out); + + needsUpdate = false; + + lastTime = timeProvider.get(); + + pr( + " Total Error: " + + total_error + + " Current Error: " + + cur_error + + " Output: " + + output_value + + " Setpoint: " + + setpoint); + } + + public double getOutput() { + return output_value; + } + + public boolean isDone() { + final double TIME_HORIZON = 0.5; + return !needsUpdate + && Math.abs(cur_error) < endthreshold.get() + && Math.abs(cur_error + error_rate * TIME_HORIZON) < endthreshold.get(); + } + + /** + * Reset all accumulated errors + */ + public void reset() { + cur_error = 0; + prev_error = 0; + error_rate = 0; + total_error = 0; + lastTime = timeProvider.get(); + needsUpdate = true; + } + + /** + * Clips value for sending to motor controllers. This deals with if you don't want to run an arm + * or wheels at full speed under PID. + * + * @param clipped + * @return clipped value, safe for setting to controllers + */ + private double clip(final double clipped) { + double out = clipped; + if (maxoutput_high.hasValue() && out > maxoutput_high.get()) { + out = maxoutput_high.get(); + } + if (maxoutput_low.hasValue() && out < maxoutput_low.get()) { + out = maxoutput_low.get(); + } + return out; + } + + public double getError() { + return total_error; + } + + public double getCurrentError() { + return cur_error; + } + + public void setMaxoutputHigh(final Double in) { + if (in == null) { + ((SettableValueProvider) maxoutput_high).clear(); + } else { + ((SettableValueProvider) maxoutput_high).set(in); + } + } + + public void setMaxoutputLow(final Double in) { + if (in == null) { + ((SettableValueProvider) maxoutput_low).clear(); + } else { + ((SettableValueProvider) maxoutput_low).set(in); + } + } + + public double getSetpoint() { + return setpoint; + } + + private void pr(final Object text) { + if (print && printCounter > 0) { + Logger.get(Category.PID_CONTROLLER).logRaw(Severity.DEBUG, "PID: " + text); + printCounter = 0; + } + printCounter++; + } } diff --git a/src/main/java/com/team766/hal/CanivPoller.java b/src/main/java/com/team766/hal/CanivPoller.java index 27048eae..8b36c193 100644 --- a/src/main/java/com/team766/hal/CanivPoller.java +++ b/src/main/java/com/team766/hal/CanivPoller.java @@ -1,81 +1,82 @@ package com.team766.hal; -import java.io.BufferedReader; -import java.io.IOException; -import java.io.InputStream; -import java.io.InputStreamReader; -import java.util.HashMap; -import java.util.Map; -import java.util.concurrent.atomic.AtomicBoolean; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.Severity; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringPublisher; +import java.io.BufferedReader; +import java.io.InputStream; +import java.io.InputStreamReader; +import java.util.HashMap; +import java.util.Map; +import java.util.concurrent.atomic.AtomicBoolean; public class CanivPoller implements Runnable { - private static final String CANIV_PATH = "/usr/bin"; - public static final String CANIV_BIN = CANIV_PATH + "/" + "caniv"; - private static final String CANIV_ARGS[] = { CANIV_BIN, "-a", "-i" }; // because, AI + private static final String CANIV_PATH = "/usr/bin"; + public static final String CANIV_BIN = CANIV_PATH + "/" + "caniv"; + private static final String CANIV_ARGS[] = {CANIV_BIN, "-a", "-i"}; // because, AI - private final Logger logger = Logger.get(Category.DRIVE); + private final Logger logger = Logger.get(Category.DRIVE); - private final long periodMillis; - private final ProcessBuilder processBuilder; - private final AtomicBoolean done = new AtomicBoolean(false); - private final NetworkTableInstance nti = NetworkTableInstance.getDefault(); - private final NetworkTable table = nti.getTable("caniv"); - private final Map publishers = new HashMap(); + private final long periodMillis; + private final ProcessBuilder processBuilder; + private final AtomicBoolean done = new AtomicBoolean(false); + private final NetworkTableInstance nti = NetworkTableInstance.getDefault(); + private final NetworkTable table = nti.getTable("caniv"); + private final Map publishers = new HashMap(); - public CanivPoller(long periodMillis) { - this.periodMillis = periodMillis; - this.processBuilder = new ProcessBuilder(CANIV_ARGS); - } + public CanivPoller(long periodMillis) { + this.periodMillis = periodMillis; + this.processBuilder = new ProcessBuilder(CANIV_ARGS); + } - @Override - public void run() { - while (!done.get()) { - try { - Process process = processBuilder.start(); - InputStream response = process.getInputStream(); - BufferedReader reader = new BufferedReader(new InputStreamReader(response)); - logger.logRaw(Severity.INFO, "caniv"); - String line = null; - while ((line = reader.readLine()) != null) { - if (line.isEmpty()) continue; - logger.logRaw(Severity.INFO, line); - String[] keyValue = line.split(":", 2); - if (keyValue.length != 2) continue; - keyValue[0] = keyValue[0].trim(); - keyValue[1] = keyValue[1].trim(); + @Override + public void run() { + while (!done.get()) { + try { + Process process = processBuilder.start(); + InputStream response = process.getInputStream(); + BufferedReader reader = new BufferedReader(new InputStreamReader(response)); + logger.logRaw(Severity.INFO, "caniv"); + String line = null; + while ((line = reader.readLine()) != null) { + if (line.isEmpty()) continue; + logger.logRaw(Severity.INFO, line); + String[] keyValue = line.split(":", 2); + if (keyValue.length != 2) continue; + keyValue[0] = keyValue[0].trim(); + keyValue[1] = keyValue[1].trim(); - if (!publishers.containsKey(keyValue[0])) { - publishers.put(keyValue[0], table.getStringTopic(keyValue[0]).publish()); - } - publishers.get(keyValue[0]).set(keyValue[1]); - } - response.close(); + if (!publishers.containsKey(keyValue[0])) { + publishers.put(keyValue[0], table.getStringTopic(keyValue[0]).publish()); + } + publishers.get(keyValue[0]).set(keyValue[1]); + } + response.close(); - process.destroy(); + process.destroy(); - } catch (Exception e) { - logger.logRaw(Severity.ERROR, "Exception caught trying to execute or parse output from caniv: " - + e.getMessage()); - e.printStackTrace(); - } + } catch (Exception e) { + logger.logRaw( + Severity.ERROR, + "Exception caught trying to execute or parse output from caniv: " + + e.getMessage()); + e.printStackTrace(); + } - try { - Thread.sleep(periodMillis); // TODO: measure execution time, adjust sleep. - } catch (Exception e) { - logger.logRaw(Severity.ERROR, "Exception caught trying to sleep: " - + e.getMessage()); - e.printStackTrace(); - } - } - } + try { + Thread.sleep(periodMillis); // TODO: measure execution time, adjust sleep. + } catch (Exception e) { + logger.logRaw( + Severity.ERROR, "Exception caught trying to sleep: " + e.getMessage()); + e.printStackTrace(); + } + } + } - public void setDone(boolean done) { - this.done.set(done); - } + public void setDone(boolean done) { + this.done.set(done); + } } diff --git a/src/main/java/com/team766/hal/GenericRobotMain.java b/src/main/java/com/team766/hal/GenericRobotMain.java index 8e76425d..c48a72a5 100755 --- a/src/main/java/com/team766/hal/GenericRobotMain.java +++ b/src/main/java/com/team766/hal/GenericRobotMain.java @@ -1,10 +1,10 @@ package com.team766.hal; -import com.team766.framework.Scheduler; import com.team766.framework.AutonomousMode; import com.team766.framework.LaunchedContext; import com.team766.framework.Procedure; // import com.team766.hal.GenericRobotMain; +import com.team766.framework.Scheduler; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.Severity; @@ -22,146 +22,150 @@ // Team 766 - Robot Interface Base class public final class GenericRobotMain { - private OI m_oi; - - private WebServer m_webServer; - private AutonomousSelector m_autonSelector; - private AutonomousMode m_autonMode = null; - private LaunchedContext m_autonomous = null; - private LaunchedContext m_oiContext = null; - - // Reset the autonomous routine if the robot is disabled for more than this - // number of seconds. - private static final double RESET_IN_DISABLED_PERIOD = 10.0; - private double m_disabledModeStartTime; - - private boolean faultInRobotInit = false; - private boolean faultInAutoInit = false; - private boolean faultInTeleopInit = false; - - public GenericRobotMain() { - Scheduler.getInstance().reset(); - - m_autonSelector = new AutonomousSelector(AutonomousModes.AUTONOMOUS_MODES); - m_webServer = new WebServer(); - m_webServer.addHandler(new Dashboard()); - m_webServer.addHandler(new DriverInterface(m_autonSelector)); - m_webServer.addHandler(new ConfigUI()); - m_webServer.addHandler(new LogViewer()); - m_webServer.addHandler(new ReadLogs()); - m_webServer.addHandler(m_autonSelector); - m_webServer.start(); - } - - public void robotInit() { - try { - Robot.robotInit(); - - m_oi = new OI(); - } catch (Throwable ex) { - faultInRobotInit = true; - throw ex; - } - faultInRobotInit = false; - } - - public void disabledInit() { - m_disabledModeStartTime = RobotProvider.instance.getClock().getTime(); - } - - public void disabledPeriodic() { - if (faultInRobotInit) return; - - // The robot can enter disabled mode for two reasons: - // - The field control system set the robots to disabled. - // - The robot loses communication with the driver station. - // In the former case, we want to reset the autonomous routine, as there - // may have been a field fault, which would mean the match is going to - // be replayed (and thus we would want to run the autonomous routine - // from the beginning). In the latter case, we don't want to reset the - // autonomous routine because the communication drop was likely caused - // by some short-lived (less than a second long, or so) interference; - // when the communications are restored, we want to continue executing - // the routine that was interrupted, since it has knowledge of where the - // robot is on the field, the state of the robot's mechanisms, etc. - // Thus, we set a threshold on the amount of time spent in autonomous of - // 10 seconds. It is almost certain that it will take longer than 10 - // seconds to reset the field if a match is to be replayed, but it is - // also almost certain that a communication drop will be much shorter - // than 10 seconds. - double timeInState = - RobotProvider.instance.getClock().getTime() - m_disabledModeStartTime; - if (timeInState > RESET_IN_DISABLED_PERIOD) { - resetAutonomousMode("time in disabled mode"); - } - Scheduler.getInstance().run(); - } - - public void resetAutonomousMode(final String reason) { - if (m_autonomous != null) { - m_autonomous.stop(); - m_autonomous = null; - m_autonMode = null; - Logger.get(Category.AUTONOMOUS).logRaw( - Severity.INFO, "Resetting autonomus procedure from " + reason); - } - } - - public void autonomousInit() { - faultInAutoInit = true; - - if (m_oiContext != null) { - m_oiContext.stop(); - m_oiContext = null; - } - - Robot.gyro.resetGyro180(); -// Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - - if (m_autonomous != null) { - Logger.get(Category.AUTONOMOUS).logRaw(Severity.INFO, "Continuing previous autonomus procedure " + m_autonomous.getContextName()); - } else if (m_autonSelector.getSelectedAutonMode() == null) { - Logger.get(Category.AUTONOMOUS).logRaw(Severity.WARNING, "No autonomous mode selected"); - } - faultInAutoInit = false; - } - - public void autonomousPeriodic() { - if (faultInRobotInit || faultInAutoInit) return; - - final AutonomousMode autonomousMode = m_autonSelector.getSelectedAutonMode(); - if (autonomousMode != null && m_autonMode != autonomousMode) { - final Procedure autonProcedure = autonomousMode.instantiate(); - m_autonomous = Scheduler.getInstance().startAsync(autonProcedure); - m_autonMode = autonomousMode; - Logger.get(Category.AUTONOMOUS).logRaw(Severity.INFO, "Starting new autonomus procedure " + autonProcedure.getName()); - } - Scheduler.getInstance().run(); - } - - public void teleopInit() { - faultInTeleopInit = true; - - if (m_autonomous != null) { - m_autonomous.stop(); - m_autonomous = null; - m_autonMode = null; - } - - if (m_oiContext == null && m_oi != null) { - m_oiContext = Scheduler.getInstance().startAsync(m_oi); - } - - faultInTeleopInit = false; - } - - public void teleopPeriodic() { - if (faultInRobotInit || faultInTeleopInit) return; - - if (m_oiContext != null && m_oiContext.isDone()) { - m_oiContext = Scheduler.getInstance().startAsync(m_oi); - Logger.get(Category.OPERATOR_INTERFACE).logRaw(Severity.WARNING, "Restarting OI context"); - } - Scheduler.getInstance().run(); - } + private OI m_oi; + + private WebServer m_webServer; + private AutonomousSelector m_autonSelector; + private AutonomousMode m_autonMode = null; + private LaunchedContext m_autonomous = null; + private LaunchedContext m_oiContext = null; + + // Reset the autonomous routine if the robot is disabled for more than this + // number of seconds. + private static final double RESET_IN_DISABLED_PERIOD = 10.0; + private double m_disabledModeStartTime; + + private boolean faultInRobotInit = false; + private boolean faultInAutoInit = false; + private boolean faultInTeleopInit = false; + + public GenericRobotMain() { + Scheduler.getInstance().reset(); + + m_autonSelector = new AutonomousSelector(AutonomousModes.AUTONOMOUS_MODES); + m_webServer = new WebServer(); + m_webServer.addHandler(new Dashboard()); + m_webServer.addHandler(new DriverInterface(m_autonSelector)); + m_webServer.addHandler(new ConfigUI()); + m_webServer.addHandler(new LogViewer()); + m_webServer.addHandler(new ReadLogs()); + m_webServer.addHandler(m_autonSelector); + m_webServer.start(); + } + + public void robotInit() { + try { + Robot.robotInit(); + + m_oi = new OI(); + } catch (Throwable ex) { + faultInRobotInit = true; + throw ex; + } + faultInRobotInit = false; + } + + public void disabledInit() { + m_disabledModeStartTime = RobotProvider.instance.getClock().getTime(); + } + + public void disabledPeriodic() { + if (faultInRobotInit) return; + + // The robot can enter disabled mode for two reasons: + // - The field control system set the robots to disabled. + // - The robot loses communication with the driver station. + // In the former case, we want to reset the autonomous routine, as there + // may have been a field fault, which would mean the match is going to + // be replayed (and thus we would want to run the autonomous routine + // from the beginning). In the latter case, we don't want to reset the + // autonomous routine because the communication drop was likely caused + // by some short-lived (less than a second long, or so) interference; + // when the communications are restored, we want to continue executing + // the routine that was interrupted, since it has knowledge of where the + // robot is on the field, the state of the robot's mechanisms, etc. + // Thus, we set a threshold on the amount of time spent in autonomous of + // 10 seconds. It is almost certain that it will take longer than 10 + // seconds to reset the field if a match is to be replayed, but it is + // also almost certain that a communication drop will be much shorter + // than 10 seconds. + double timeInState = RobotProvider.instance.getClock().getTime() - m_disabledModeStartTime; + if (timeInState > RESET_IN_DISABLED_PERIOD) { + resetAutonomousMode("time in disabled mode"); + } + Scheduler.getInstance().run(); + } + + public void resetAutonomousMode(final String reason) { + if (m_autonomous != null) { + m_autonomous.stop(); + m_autonomous = null; + m_autonMode = null; + Logger.get(Category.AUTONOMOUS) + .logRaw(Severity.INFO, "Resetting autonomus procedure from " + reason); + } + } + + public void autonomousInit() { + faultInAutoInit = true; + + if (m_oiContext != null) { + m_oiContext.stop(); + m_oiContext = null; + } + + if (m_autonomous != null) { + Logger.get(Category.AUTONOMOUS) + .logRaw( + Severity.INFO, + "Continuing previous autonomus procedure " + + m_autonomous.getContextName()); + } else if (m_autonSelector.getSelectedAutonMode() == null) { + Logger.get(Category.AUTONOMOUS).logRaw(Severity.WARNING, "No autonomous mode selected"); + } + faultInAutoInit = false; + } + + public void autonomousPeriodic() { + if (faultInRobotInit || faultInAutoInit) return; + + final AutonomousMode autonomousMode = m_autonSelector.getSelectedAutonMode(); + if (autonomousMode != null && m_autonMode != autonomousMode) { + final Procedure autonProcedure = autonomousMode.instantiate(); + m_autonomous = Scheduler.getInstance().startAsync(autonProcedure); + m_autonMode = autonomousMode; + Logger.get(Category.AUTONOMOUS) + .logRaw( + Severity.INFO, + "Starting new autonomus procedure " + autonProcedure.getName()); + } + Scheduler.getInstance().run(); + } + + public void teleopInit() { + faultInTeleopInit = true; + + if (m_autonomous != null) { + m_autonomous.stop(); + m_autonomous = null; + m_autonMode = null; + } + + if (m_oiContext == null && m_oi != null) { + m_oiContext = Scheduler.getInstance().startAsync(m_oi); + } + + faultInTeleopInit = false; + } + + public void teleopPeriodic() { + if (faultInRobotInit || faultInTeleopInit) return; + + if (m_oiContext != null && m_oiContext.isDone()) { + m_oiContext = Scheduler.getInstance().startAsync(m_oi); + Logger.get(Category.OPERATOR_INTERFACE) + .logRaw(Severity.WARNING, "Restarting OI context"); + } + Scheduler.getInstance().run(); + } } diff --git a/src/main/java/com/team766/hal/RobotProvider.java b/src/main/java/com/team766/hal/RobotProvider.java index 74dec9f7..03256b48 100755 --- a/src/main/java/com/team766/hal/RobotProvider.java +++ b/src/main/java/com/team766/hal/RobotProvider.java @@ -1,267 +1,312 @@ package com.team766.hal; -import java.util.Arrays; -import java.util.HashMap; -import java.io.StringWriter; -import java.io.PrintWriter; - import com.team766.config.ConfigFileReader; import com.team766.controllers.TimeProviderI; import com.team766.hal.mock.MockAnalogInput; import com.team766.hal.mock.MockDigitalInput; import com.team766.hal.mock.MockEncoder; import com.team766.hal.mock.MockGyro; -import com.team766.hal.mock.MockRelay; import com.team766.hal.mock.MockMotorController; +import com.team766.hal.mock.MockRelay; import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; +import java.util.Arrays; +import java.util.HashMap; public abstract class RobotProvider { - public static RobotProvider instance; - - protected EncoderReader[] encoders = new EncoderReader[20]; - protected SolenoidController[] solenoids = new SolenoidController[20]; - protected GyroReader[] gyros = new GyroReader[13]; - protected HashMap cams = new HashMap(); - protected JoystickReader[] joysticks = new JoystickReader[8]; - protected DigitalInputReader[] digInputs = new DigitalInputReader[8]; - protected AnalogInputReader[] angInputs = new AnalogInputReader[5]; - protected RelayOutput[] relays = new RelayOutput[5]; - protected PositionReader positionSensor = null; - protected BeaconReader beaconSensor = null; - - private HashMap motorDeviceIdNames = new HashMap(); - private HashMap motorPortNames = new HashMap(); - private HashMap digitalIoNames = new HashMap(); - private HashMap analogInputNames = new HashMap(); - private HashMap relayNames = new HashMap(); - private HashMap solenoidNames = new HashMap(); - private HashMap gyroNames = new HashMap(); - - //HAL - protected abstract MotorController getMotor(int index, String configPrefix, MotorController.Type type, ControlInputReader localSensor); - - public abstract EncoderReader getEncoder(int index1, int index2); - - public abstract DigitalInputReader getDigitalInput(int index); - - public abstract AnalogInputReader getAnalogInput(int index); - - public abstract RelayOutput getRelay(int index); - - public abstract SolenoidController getSolenoid(int index); - - public abstract GyroReader getGyro(int index); - - public abstract CameraReader getCamera(String id, String value); - - public abstract PositionReader getPositionSensor(); - - public abstract BeaconReader getBeaconSensor(); - - public static TimeProviderI getTimeProvider() { - return () -> instance.getClock().getTime(); - } - - // Config-driven methods - - private void checkDeviceName(final String deviceType, - final HashMap deviceNames, final Integer portId, - final String configName) { - String previousName = deviceNames.putIfAbsent(portId, configName); - if (previousName != null && previousName != configName) { - Logger.get(Category.CONFIGURATION).logRaw(Severity.ERROR, "Multiple " + deviceType - + " devices for port ID " + portId + ": " + previousName + ", " + configName); - } - } - - public MotorController getMotor(final String configName) { - final String encoderConfigName = configName + ".encoder"; - final String analogInputConfigName = configName + ".analogInput"; - final ControlInputReader sensor = - ConfigFileReader.getInstance().containsKey(encoderConfigName) - ? getEncoder(encoderConfigName) - : ConfigFileReader.getInstance().containsKey(analogInputConfigName) - ? getAnalogInput(analogInputConfigName) - : null; - - try { - ValueProvider deviceId = - ConfigFileReader.getInstance().getInt(configName + ".deviceId"); - final ValueProvider port = - ConfigFileReader.getInstance().getInt(configName + ".port"); - final ValueProvider sensorScaleConfig = - ConfigFileReader.getInstance().getDouble(configName + ".sensorScale"); - final ValueProvider invertedConfig = - ConfigFileReader.getInstance().getBoolean(configName + ".inverted"); - final ValueProvider sensorInvertedConfig = - ConfigFileReader.getInstance().getBoolean(configName + ".sensorInverted"); - final ValueProvider type = ConfigFileReader.getInstance() - .getEnum(MotorController.Type.class, configName + ".type"); - - if (deviceId.hasValue() && port.hasValue()) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, - "Motor %s configuration should have only one of `deviceId` or `port`", - configName); - } - - MotorController.Type defaultType = MotorController.Type.TalonSRX; - if (!deviceId.hasValue()) { - deviceId = port; - defaultType = MotorController.Type.VictorSP; - checkDeviceName("PWM motor controller", motorPortNames, port.get(), configName); - } else { - checkDeviceName("CAN motor controller", motorDeviceIdNames, deviceId.get(), - configName); - } - - var motor = getMotor(deviceId.get(), configName, type.valueOr(defaultType), sensor); - if (sensorScaleConfig.hasValue()) { - motor = new MotorControllerWithSensorScale(motor, sensorScaleConfig.get()); - } - if (invertedConfig.valueOr(false)) { - motor.setInverted(true); - } - if (sensorInvertedConfig.valueOr(false)) { - motor.setSensorInverted(true); - } - return motor; - } catch (IllegalArgumentException ex) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, - "Error getting configuration for motor %s from config file, using mock motor instead.\nDetailed error: %s", - configName, LoggerExceptionUtils.exceptionToString(ex)); - return new LocalMotorController(configName, new MockMotorController(0), sensor); - } - } - - public EncoderReader getEncoder(final String configName) { - try { - final ValueProvider ports = - ConfigFileReader.getInstance().getInts(configName + ".ports"); - final ValueProvider distancePerPulseConfig = - ConfigFileReader.getInstance().getDouble(configName + ".distancePerPulse"); - - final var portsValue = ports.get(); - if (portsValue.length != 2) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, - "Encoder %s has %d config values, but expected 2", configName, - portsValue.length); - return new MockEncoder(0, 0); - } - checkDeviceName("encoder/digital input", digitalIoNames, portsValue[0], configName); - checkDeviceName("encoder/digital input", digitalIoNames, portsValue[1], configName); - final EncoderReader reader = getEncoder(portsValue[0], portsValue[1]); - if (distancePerPulseConfig.hasValue()) { - reader.setDistancePerPulse(distancePerPulseConfig.get()); - } - return reader; - } catch (IllegalArgumentException ex) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, - "Encoder %s not found in config file, using mock encoder instead", configName); - return new MockEncoder(0, 0); - } - } - - public DigitalInputReader getDigitalInput(final String configName) { - try { - ValueProvider port = - ConfigFileReader.getInstance().getInt(configName + ".port"); - checkDeviceName("encoder/digital input", digitalIoNames, port.get(), configName); - - return getDigitalInput(port.get()); - } catch (IllegalArgumentException ex) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, - "Digital input %s not found in config file, using mock digital input instead", - configName); - return new MockDigitalInput(); - } - } - - public AnalogInputReader getAnalogInput(final String configName) { - try { - ValueProvider port = - ConfigFileReader.getInstance().getInt(configName + ".port"); - checkDeviceName("analog input", analogInputNames, port.get(), configName); - - return getAnalogInput(port.get()); - } catch (IllegalArgumentException ex) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, - "Analog input %s not found in config file, using mock analog input instead", - configName); - return new MockAnalogInput(); - } - } - - public RelayOutput getRelay(final String configName) { - try { - ValueProvider port = - ConfigFileReader.getInstance().getInt(configName + ".port"); - checkDeviceName("relay", relayNames, port.get(), configName); - - return getRelay(port.get()); - } catch (IllegalArgumentException ex) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, - "Relay %s not found in config file, using mock relay instead", configName); - return new MockRelay(0); - } - } - - public DoubleSolenoid getSolenoid(final String configName) { - try { - final String legacyConfigKey = configName + ".port"; - ValueProvider forwardPorts = - ConfigFileReader.getInstance().containsKey(legacyConfigKey) - ? ConfigFileReader.getInstance().getInts(legacyConfigKey) - : ConfigFileReader.getInstance().getInts(configName + ".forwardPort"); - ValueProvider reversePorts = - ConfigFileReader.getInstance().getInts(configName + ".reversePort"); - - for (Integer port : forwardPorts.valueOr(new Integer[0])) { - checkDeviceName("solenoid", solenoidNames, port, configName); - } - for (Integer port : reversePorts.valueOr(new Integer[0])) { - checkDeviceName("solenoid", solenoidNames, port, configName); - } - - SolenoidController forwardSolenoids = new MultiSolenoid(Arrays - .stream(forwardPorts.valueOr(new Integer[0])) - .map(this::getSolenoid).toArray(SolenoidController[]::new)); - SolenoidController reverseSolenoids = new MultiSolenoid(Arrays - .stream(reversePorts.valueOr(new Integer[0])) - .map(this::getSolenoid).toArray(SolenoidController[]::new)); - return new DoubleSolenoid(forwardSolenoids, reverseSolenoids); - } catch (IllegalArgumentException ex) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, "Solenoid %s not found in config file, using mock solenoid instead %s", configName, ex.toString()); - return new DoubleSolenoid(null, null); - } - } - - public GyroReader getGyro(final String configName) { - try { - ValueProvider port = ConfigFileReader.getInstance().getInt(configName + ".port"); - checkDeviceName("gyro", gyroNames, port.get(), configName); - - return getGyro(port.get()); - } catch (IllegalArgumentException ex) { - Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, "Gyro %s not found in config file, using mock gyro instead", configName); - return new MockGyro(); - } - } - - //Operator Devices - public abstract JoystickReader getJoystick(int index); - - public abstract CameraInterface getCamServer(); - - public abstract Clock getClock(); - - public abstract double getBatteryVoltage(); - - public abstract void refreshDriverStationData(); - - public abstract boolean hasNewDriverStationData(); + public static RobotProvider instance; + + protected EncoderReader[] encoders = new EncoderReader[20]; + protected SolenoidController[] solenoids = new SolenoidController[20]; + protected GyroReader[] gyros = new GyroReader[13]; + protected HashMap cams = new HashMap(); + protected JoystickReader[] joysticks = new JoystickReader[8]; + protected DigitalInputReader[] digInputs = new DigitalInputReader[8]; + protected AnalogInputReader[] angInputs = new AnalogInputReader[5]; + protected RelayOutput[] relays = new RelayOutput[5]; + protected PositionReader positionSensor = null; + protected BeaconReader beaconSensor = null; + + private HashMap motorDeviceIdNames = new HashMap(); + private HashMap motorPortNames = new HashMap(); + private HashMap digitalIoNames = new HashMap(); + private HashMap analogInputNames = new HashMap(); + private HashMap relayNames = new HashMap(); + private HashMap solenoidNames = new HashMap(); + private HashMap gyroNames = new HashMap(); + + // HAL + protected abstract MotorController getMotor( + int index, + String configPrefix, + MotorController.Type type, + ControlInputReader localSensor); + + public abstract EncoderReader getEncoder(int index1, int index2); + + public abstract DigitalInputReader getDigitalInput(int index); + + public abstract AnalogInputReader getAnalogInput(int index); + + public abstract RelayOutput getRelay(int index); + + public abstract SolenoidController getSolenoid(int index); + + public abstract GyroReader getGyro(int index); + + public abstract CameraReader getCamera(String id, String value); + + public abstract PositionReader getPositionSensor(); + + public abstract BeaconReader getBeaconSensor(); + + public static TimeProviderI getTimeProvider() { + return () -> instance.getClock().getTime(); + } + + // Config-driven methods + + private void checkDeviceName( + final String deviceType, + final HashMap deviceNames, + final Integer portId, + final String configName) { + String previousName = deviceNames.putIfAbsent(portId, configName); + if (previousName != null && previousName != configName) { + Logger.get(Category.CONFIGURATION) + .logRaw( + Severity.ERROR, + "Multiple " + + deviceType + + " devices for port ID " + + portId + + ": " + + previousName + + ", " + + configName); + } + } + + public MotorController getMotor(final String configName) { + final String encoderConfigName = configName + ".encoder"; + final String analogInputConfigName = configName + ".analogInput"; + final ControlInputReader sensor = + ConfigFileReader.getInstance().containsKey(encoderConfigName) + ? getEncoder(encoderConfigName) + : ConfigFileReader.getInstance().containsKey(analogInputConfigName) + ? getAnalogInput(analogInputConfigName) + : null; + + try { + ValueProvider deviceId = + ConfigFileReader.getInstance().getInt(configName + ".deviceId"); + final ValueProvider port = + ConfigFileReader.getInstance().getInt(configName + ".port"); + final ValueProvider sensorScaleConfig = + ConfigFileReader.getInstance().getDouble(configName + ".sensorScale"); + final ValueProvider invertedConfig = + ConfigFileReader.getInstance().getBoolean(configName + ".inverted"); + final ValueProvider sensorInvertedConfig = + ConfigFileReader.getInstance().getBoolean(configName + ".sensorInverted"); + final ValueProvider type = + ConfigFileReader.getInstance() + .getEnum(MotorController.Type.class, configName + ".type"); + + if (deviceId.hasValue() && port.hasValue()) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Motor %s configuration should have only one of `deviceId` or `port`", + configName); + } + + MotorController.Type defaultType = MotorController.Type.TalonSRX; + if (!deviceId.hasValue()) { + deviceId = port; + defaultType = MotorController.Type.VictorSP; + checkDeviceName("PWM motor controller", motorPortNames, port.get(), configName); + } else { + checkDeviceName( + "CAN motor controller", motorDeviceIdNames, deviceId.get(), configName); + } + + var motor = getMotor(deviceId.get(), configName, type.valueOr(defaultType), sensor); + if (sensorScaleConfig.hasValue()) { + motor = new MotorControllerWithSensorScale(motor, sensorScaleConfig.get()); + } + if (invertedConfig.valueOr(false)) { + motor.setInverted(true); + } + if (sensorInvertedConfig.valueOr(false)) { + motor.setSensorInverted(true); + } + return motor; + } catch (IllegalArgumentException ex) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Error getting configuration for motor %s from config file, using mock motor instead.\nDetailed error: %s", + configName, + LoggerExceptionUtils.exceptionToString(ex)); + return new LocalMotorController(configName, new MockMotorController(0), sensor); + } + } + + public EncoderReader getEncoder(final String configName) { + try { + final ValueProvider ports = + ConfigFileReader.getInstance().getInts(configName + ".ports"); + final ValueProvider distancePerPulseConfig = + ConfigFileReader.getInstance().getDouble(configName + ".distancePerPulse"); + + final var portsValue = ports.get(); + if (portsValue.length != 2) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Encoder %s has %d config values, but expected 2", + configName, + portsValue.length); + return new MockEncoder(0, 0); + } + checkDeviceName("encoder/digital input", digitalIoNames, portsValue[0], configName); + checkDeviceName("encoder/digital input", digitalIoNames, portsValue[1], configName); + final EncoderReader reader = getEncoder(portsValue[0], portsValue[1]); + if (distancePerPulseConfig.hasValue()) { + reader.setDistancePerPulse(distancePerPulseConfig.get()); + } + return reader; + } catch (IllegalArgumentException ex) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Encoder %s not found in config file, using mock encoder instead", + configName); + return new MockEncoder(0, 0); + } + } + + public DigitalInputReader getDigitalInput(final String configName) { + try { + ValueProvider port = + ConfigFileReader.getInstance().getInt(configName + ".port"); + checkDeviceName("encoder/digital input", digitalIoNames, port.get(), configName); + + return getDigitalInput(port.get()); + } catch (IllegalArgumentException ex) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Digital input %s not found in config file, using mock digital input instead", + configName); + return new MockDigitalInput(); + } + } + + public AnalogInputReader getAnalogInput(final String configName) { + try { + ValueProvider port = + ConfigFileReader.getInstance().getInt(configName + ".port"); + checkDeviceName("analog input", analogInputNames, port.get(), configName); + + return getAnalogInput(port.get()); + } catch (IllegalArgumentException ex) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Analog input %s not found in config file, using mock analog input instead", + configName); + return new MockAnalogInput(); + } + } + + public RelayOutput getRelay(final String configName) { + try { + ValueProvider port = + ConfigFileReader.getInstance().getInt(configName + ".port"); + checkDeviceName("relay", relayNames, port.get(), configName); + + return getRelay(port.get()); + } catch (IllegalArgumentException ex) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Relay %s not found in config file, using mock relay instead", + configName); + return new MockRelay(0); + } + } + + public DoubleSolenoid getSolenoid(final String configName) { + try { + final String legacyConfigKey = configName + ".port"; + ValueProvider forwardPorts = + ConfigFileReader.getInstance().containsKey(legacyConfigKey) + ? ConfigFileReader.getInstance().getInts(legacyConfigKey) + : ConfigFileReader.getInstance().getInts(configName + ".forwardPort"); + ValueProvider reversePorts = + ConfigFileReader.getInstance().getInts(configName + ".reversePort"); + + for (Integer port : forwardPorts.valueOr(new Integer[0])) { + checkDeviceName("solenoid", solenoidNames, port, configName); + } + for (Integer port : reversePorts.valueOr(new Integer[0])) { + checkDeviceName("solenoid", solenoidNames, port, configName); + } + + SolenoidController forwardSolenoids = + new MultiSolenoid( + Arrays.stream(forwardPorts.valueOr(new Integer[0])) + .map(this::getSolenoid) + .toArray(SolenoidController[]::new)); + SolenoidController reverseSolenoids = + new MultiSolenoid( + Arrays.stream(reversePorts.valueOr(new Integer[0])) + .map(this::getSolenoid) + .toArray(SolenoidController[]::new)); + return new DoubleSolenoid(forwardSolenoids, reverseSolenoids); + } catch (IllegalArgumentException ex) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Solenoid %s not found in config file, using mock solenoid instead %s", + configName, + ex.toString()); + return new DoubleSolenoid(null, null); + } + } + + public GyroReader getGyro(final String configName) { + try { + ValueProvider port = + ConfigFileReader.getInstance().getInt(configName + ".port"); + checkDeviceName("gyro", gyroNames, port.get(), configName); + + return getGyro(port.get()); + } catch (IllegalArgumentException ex) { + Logger.get(Category.CONFIGURATION) + .logData( + Severity.ERROR, + "Gyro %s not found in config file, using mock gyro instead", + configName); + return new MockGyro(); + } + } + + // Operator Devices + public abstract JoystickReader getJoystick(int index); + + public abstract CameraInterface getCamServer(); + + public abstract Clock getClock(); + + public abstract double getBatteryVoltage(); + + public abstract void refreshDriverStationData(); + + public abstract boolean hasNewDriverStationData(); } diff --git a/src/main/java/com/team766/hal/simulator/RobotMain.java b/src/main/java/com/team766/hal/simulator/RobotMain.java index cae35009..eda13057 100755 --- a/src/main/java/com/team766/hal/simulator/RobotMain.java +++ b/src/main/java/com/team766/hal/simulator/RobotMain.java @@ -1,127 +1,132 @@ package com.team766.hal.simulator; -import java.io.IOException; import com.team766.config.ConfigFileReader; +import com.team766.framework.Scheduler; import com.team766.hal.GenericRobotMain; import com.team766.hal.RobotProvider; import com.team766.logging.LoggerExceptionUtils; import com.team766.simulator.Program; import com.team766.simulator.ProgramInterface; import com.team766.simulator.Simulator; +import java.io.IOException; public class RobotMain { - enum Mode { - MaroonSim, - VrConnector, - } - - private GenericRobotMain robot; - private Runnable simulator; + enum Mode { + MaroonSim, + VrConnector, + } - public RobotMain(final Mode mode) { - try { - // TODO: update this to come from deploy directory? - ConfigFileReader.instance = new ConfigFileReader("simConfig.txt"); - RobotProvider.instance = new SimulationRobotProvider(); + private GenericRobotMain robot; + private Runnable simulator; - Scheduler.getInstance().reset(); + public RobotMain(final Mode mode) { + try { + // TODO: update this to come from deploy directory? + ConfigFileReader.instance = new ConfigFileReader("simConfig.txt"); + RobotProvider.instance = new SimulationRobotProvider(); - robot = new GenericRobotMain(); + Scheduler.getInstance().reset(); - robot.robotInit(); + robot = new GenericRobotMain(); - ProgramInterface.program = new Program() { - ProgramInterface.RobotMode prevRobotMode = null; + robot.robotInit(); - @Override - public void step(double dt) { - switch (ProgramInterface.robotMode) { - case DISABLED: - if (prevRobotMode != ProgramInterface.RobotMode.DISABLED) { - robot.disabledInit(); - prevRobotMode = ProgramInterface.RobotMode.DISABLED; - } - robot.disabledPeriodic(); - break; - case AUTON: - if (prevRobotMode != ProgramInterface.RobotMode.AUTON) { - robot.autonomousInit(); - prevRobotMode = ProgramInterface.RobotMode.AUTON; - } - robot.autonomousPeriodic(); - break; - case TELEOP: - if (prevRobotMode != ProgramInterface.RobotMode.TELEOP) { - robot.teleopInit(); - prevRobotMode = ProgramInterface.RobotMode.TELEOP; - } - robot.teleopPeriodic(); - break; - default: - LoggerExceptionUtils.logException(new IllegalArgumentException("Value of ProgramInterface.robotMode invalid. Provided value: " + ProgramInterface.robotMode)); - break; - } - } + ProgramInterface.program = + new Program() { + ProgramInterface.RobotMode prevRobotMode = null; - @Override - public void reset() { - robot.resetAutonomousMode("simulation reset"); - } - }; - } catch (Exception exc) { - exc.printStackTrace(); - LoggerExceptionUtils.logException(exc); - } + @Override + public void step(double dt) { + switch (ProgramInterface.robotMode) { + case DISABLED: + if (prevRobotMode != ProgramInterface.RobotMode.DISABLED) { + robot.disabledInit(); + prevRobotMode = ProgramInterface.RobotMode.DISABLED; + } + robot.disabledPeriodic(); + break; + case AUTON: + if (prevRobotMode != ProgramInterface.RobotMode.AUTON) { + robot.autonomousInit(); + prevRobotMode = ProgramInterface.RobotMode.AUTON; + } + robot.autonomousPeriodic(); + break; + case TELEOP: + if (prevRobotMode != ProgramInterface.RobotMode.TELEOP) { + robot.teleopInit(); + prevRobotMode = ProgramInterface.RobotMode.TELEOP; + } + robot.teleopPeriodic(); + break; + default: + LoggerExceptionUtils.logException( + new IllegalArgumentException( + "Value of ProgramInterface.robotMode invalid. Provided value: " + + ProgramInterface.robotMode)); + break; + } + } - switch (mode) { - case MaroonSim: - simulator = new Simulator(); - break; - case VrConnector: - ProgramInterface.robotMode = ProgramInterface.RobotMode.DISABLED; - try { - simulator = new VrConnector(); - } catch (IOException ex) { - throw new RuntimeException("Error initializing communication with 3d Simulator", - ex); - } - break; - default: - LoggerExceptionUtils.logException( - new IllegalArgumentException("Unknown simulator mode. ProgramInterface.robotMode: " + ProgramInterface.robotMode) - ); - break; - } + @Override + public void reset() { + robot.resetAutonomousMode("simulation reset"); + } + }; + } catch (Exception exc) { + exc.printStackTrace(); + LoggerExceptionUtils.logException(exc); + } - } + switch (mode) { + case MaroonSim: + simulator = new Simulator(); + break; + case VrConnector: + ProgramInterface.robotMode = ProgramInterface.RobotMode.DISABLED; + try { + simulator = new VrConnector(); + } catch (IOException ex) { + throw new RuntimeException( + "Error initializing communication with 3d Simulator", ex); + } + break; + default: + LoggerExceptionUtils.logException( + new IllegalArgumentException( + "Unknown simulator mode. ProgramInterface.robotMode: " + + ProgramInterface.robotMode)); + break; + } + } - public void run() { - try { - simulator.run(); - } catch (Exception exc) { - exc.printStackTrace(); - LoggerExceptionUtils.logException(exc); - } - } + public void run() { + try { + simulator.run(); + } catch (Exception exc) { + exc.printStackTrace(); + LoggerExceptionUtils.logException(exc); + } + } - public static void main(final String[] args) { - if (args.length != 1) { - System.err.println("Needs -maroon_sim or -vr_connector"); - System.exit(1); - } - Mode mode; - switch (args[0]) { - case "-maroon_sim": - mode = Mode.MaroonSim; - break; - case "-vr_connector": - mode = Mode.VrConnector; - break; - default: - System.err.println("Needs -maroon_sim or -vr_connector"); - System.exit(1); - return; - } - new RobotMain(mode).run(); - } + public static void main(final String[] args) { + if (args.length != 1) { + System.err.println("Needs -maroon_sim or -vr_connector"); + System.exit(1); + } + Mode mode; + switch (args[0]) { + case "-maroon_sim": + mode = Mode.MaroonSim; + break; + case "-vr_connector": + mode = Mode.VrConnector; + break; + default: + System.err.println("Needs -maroon_sim or -vr_connector"); + System.exit(1); + return; + } + new RobotMain(mode).run(); + } } diff --git a/src/main/java/com/team766/hal/simulator/VrConnector.java b/src/main/java/com/team766/hal/simulator/VrConnector.java index 7cd8c3d5..e9ba551f 100644 --- a/src/main/java/com/team766/hal/simulator/VrConnector.java +++ b/src/main/java/com/team766/hal/simulator/VrConnector.java @@ -2,6 +2,8 @@ import static com.team766.math.Math.normalizeAngleDegrees; +import com.team766.logging.LoggerExceptionUtils; +import com.team766.simulator.ProgramInterface; import java.io.IOException; import java.net.InetAddress; import java.net.InetSocketAddress; @@ -13,355 +15,388 @@ import java.util.Arrays; import java.util.List; import java.util.Map; -import com.team766.logging.LoggerExceptionUtils; -import com.team766.simulator.ProgramInterface; public class VrConnector implements Runnable { - private static class PortMapping { - public final int messageDataIndex; - public final int robotPortIndex; - - PortMapping(final int messageIndex, final int robotIndex) { - this.messageDataIndex = messageIndex; - this.robotPortIndex = robotIndex; - } - } - - private static class CANPortMapping { - public final int canId; - public final int motorCommandMessageDataIndex; - public final int sensorFeedbackMessageDataIndex; - - CANPortMapping(final int canId_, final int motorCommandMessageDataIndex_, - final int sensorFeedbackMessageDataIndex_) { - this.canId = canId_; - this.motorCommandMessageDataIndex = motorCommandMessageDataIndex_; - this.sensorFeedbackMessageDataIndex = sensorFeedbackMessageDataIndex_; - } - } - - /// Command indexes - - private static final int MAX_COMMANDS = 100; - - private static final int RESET_SIM_CHANNEL = 0; - - private static final List PWM_CHANNELS = Arrays.asList( - // new PortMapping(10, 6), // Left motor - // new PortMapping(11, 4), // Right motor - // new PortMapping(14, 1), // Auxiliary / Center motor - // new PortMapping(12, 0) // Intake - ); - - //CHECKSTYLE:OFF - private static final List SOLENOID_CHANNELS = Arrays.asList( - new PortMapping(15, 0), // Intake arm - new PortMapping(13, 1) // Catapult launch - ); - //CHECKSTYLE:ON - - private static final List RELAY_CHANNELS = Arrays.asList(); - - //CHECKSTYLE:OFF - private static final List CAN_MOTOR_CHANNELS = Arrays.asList( - new CANPortMapping(6, 10, 10), // Left motor - new CANPortMapping(4, 11, 11), // Right motor - new CANPortMapping(10, 12, 13), // Intake - new CANPortMapping(12, 14, 0), // Aux/center motor - new CANPortMapping(14, 16, 0), // Aux2 motor - - new CANPortMapping(84, 84, 84), // FLD motor - new CANPortMapping(85, 85, 85), // BLD motor - new CANPortMapping(86, 86, 86), // FRD motor - new CANPortMapping(87, 87, 87), // BRD motor - new CANPortMapping(88, 88, 88), // FLS motor - new CANPortMapping(89, 89, 89), // BLS motor - new CANPortMapping(90, 90, 90), // FRS motor - new CANPortMapping(91, 91, 91) // BRS motor - ); - //CHECKSTYLE:ON - - /// Feedback indexes - - private static final int TIMESTAMP_LSW_CHANNEL = 5; - private static final int TIMESTAMP_MSW_CHANNEL = 4; - - private static final int RESET_COUNTER_CHANNEL = 6; - - private static final int ROBOT_MODE_CHANNEL = 3; - private static final Map ROBOT_MODES = - Map.of(0, ProgramInterface.RobotMode.DISABLED, 1, ProgramInterface.RobotMode.AUTON, 2, - ProgramInterface.RobotMode.TELEOP); - - private static final int ROBOT_X_CHANNEL = 8; - private static final int ROBOT_Y_CHANNEL = 9; - - private static final int BEACON_SENSOR_START = 120; - private static final int BEACON_SENSOR_STRIDE = 6; // (x, y, z, yaw, pitch, roll) - - //CHECKSTYLE:OFF - private static final List ENCODER_CHANNELS = Arrays.asList( - new PortMapping(10, 0), // Left encoder - new PortMapping(11, 2), // Right encoder - new PortMapping(13, 4) // Mechanism encoder - ); - //CHECKSTYLE:ON - - private static final int GYRO_CHANNEL = 15; - private static final int GYRO_RATE_CHANNEL = 16; - private static final int GYRO_PITCH_CHANNEL = 80; - private static final int GYRO_ROLL_CHANNEL = 81; - - //CHECKSTYLE:OFF - private static final List DIGITAL_CHANNELS = Arrays.asList( - new PortMapping(13, 0), // Intake state - new PortMapping(14, 1), // Ball presence - new PortMapping(17, 4), // Line Sensor 1 - new PortMapping(18, 5), // Line Sensor 2 - new PortMapping(19, 6) // Line Sensor 3 - ); - //CHECKSTYLE:ON - - private static final List ANALOG_CHANNELS = Arrays.asList(); - - private static final int NUM_JOYSTICK = 4; - private static final int BASE_AXIS_START = 20; - private static final int BASE_AXES_PER_JOYSTICK = 4; - private static final int ADDITIONAL_AXIS_START = 100; - private static final int ADDITIONAL_AXES_PER_JOYSTICK = 4; - private static final int JOYSTICK_BUTTON_START = 72; - private static final int BUTTONS_PER_JOYSTICK = 20; - - /// Socket Communication - - private static final int commandsPort = 7661; - private static final int feedbackPort = 7662; - private static final int BUF_SZ = 1024; - - private long startTime; - private boolean started = false; - - private Selector selector; - private InetSocketAddress sendAddr; - private ByteBuffer feedback = ByteBuffer.allocate(BUF_SZ); - private ByteBuffer commands = ByteBuffer.allocate(BUF_SZ); - private int resetCounter = 0; - - private int lastResetCounter = 0; - private double lastGyroValue = Double.NaN; - private long[] lastEncoderValue = new long[ProgramInterface.encoderChannels.length]; - private long[] lastCANSensorValue = - new long[ProgramInterface.canMotorControllerChannels.length]; - - private int getFeedback(final int index) { - return feedback.getInt(index * 4); - } - - private static long assembleLong(final int msw, final int lsw) { - return ((long) msw << 32) | (lsw & 0xffffffffL); - } - - private void putCommand(final int index, final int value) { - commands.putInt(index * 4, value); - } - - private void putCommandFloat(final int index, final double value) { - putCommand(index, (int) (value * 512.0)); - } - - private void putCommandTristate(final int index, final int value) { - if (value == 0) { - putCommand(index, 0); - } else if (value > 0) { - putCommand(index, 511); - } else { - putCommand(index, -512); - } - } - - private void putCommandBool(final int index, final boolean value) { - putCommand(index, value ? 511 : -512); - } - - public VrConnector() throws IOException { - selector = Selector.open(); - DatagramChannel channel = DatagramChannel.open(); - InetSocketAddress receiveAddr = new InetSocketAddress(feedbackPort); - channel.bind(receiveAddr); - sendAddr = new InetSocketAddress(InetAddress.getLoopbackAddress(), commandsPort); - channel.configureBlocking(false); - channel.register(selector, SelectionKey.OP_READ); - commands.limit(MAX_COMMANDS * 4); - commands.order(ByteOrder.LITTLE_ENDIAN); - feedback.order(ByteOrder.LITTLE_ENDIAN); - startTime = System.currentTimeMillis(); - } - - private boolean process() throws IOException { - for (PortMapping m : PWM_CHANNELS) { - putCommandFloat(m.messageDataIndex, ProgramInterface.pwmChannels[m.robotPortIndex]); - } - for (PortMapping m : SOLENOID_CHANNELS) { - putCommandBool(m.messageDataIndex, ProgramInterface.solenoidChannels[m.robotPortIndex]); - } - for (PortMapping m : RELAY_CHANNELS) { - putCommandTristate(m.messageDataIndex, ProgramInterface.relayChannels[m.robotPortIndex]); - } - for (CANPortMapping m : CAN_MOTOR_CHANNELS) { - putCommandFloat( - m.motorCommandMessageDataIndex, - ProgramInterface.canMotorControllerChannels[m.canId].command.output); - } - - selector.selectedKeys().clear(); - selector.select(); - boolean newData = false; - for (SelectionKey key : selector.selectedKeys()) { - if (!key.isValid()) { - continue; - } - - DatagramChannel chan = (DatagramChannel) key.channel(); - if (key.isReadable()) { - feedback.clear(); - chan.receive(feedback); - newData = true; - key.interestOps(SelectionKey.OP_WRITE); - } - if (key.isWritable()) { - if (started) { - chan.send(commands.duplicate(), sendAddr); - putCommand(RESET_SIM_CHANNEL, 0); - } - key.interestOps(SelectionKey.OP_READ); - } - } - - if (newData) { - double prevSimTime = ProgramInterface.simulationTime; - // Time is sent in milliseconds - ProgramInterface.simulationTime = assembleLong( - getFeedback(TIMESTAMP_MSW_CHANNEL), getFeedback(TIMESTAMP_LSW_CHANNEL)) * 0.001; - - resetCounter = getFeedback(RESET_COUNTER_CHANNEL); - - ProgramInterface.robotMode = ROBOT_MODES.get(getFeedback(ROBOT_MODE_CHANNEL)); - - final double gyroValue = getFeedback(GYRO_CHANNEL) / 10.0; - if (Double.isNaN(lastGyroValue)) { - lastGyroValue = gyroValue; - } - ProgramInterface.gyro.angle += gyroValue - lastGyroValue; - lastGyroValue = gyroValue; - - ProgramInterface.robotPosition.x = getFeedback(ROBOT_X_CHANNEL) / 1000.0; - ProgramInterface.robotPosition.y = getFeedback(ROBOT_Y_CHANNEL) / 1000.0; - ProgramInterface.robotPosition.heading = gyroValue; - - ProgramInterface.gyro.rate = getFeedback(GYRO_RATE_CHANNEL) / 100.0; - ProgramInterface.gyro.pitch = normalizeAngleDegrees(getFeedback(GYRO_PITCH_CHANNEL) / 10.0); - ProgramInterface.gyro.roll = normalizeAngleDegrees(getFeedback(GYRO_ROLL_CHANNEL) / 10.0); - - for (int i = 0; i < ProgramInterface.NUM_BEACONS; ++i) { - ProgramInterface.beacons[i].x = getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 0) / 1000.0; - ProgramInterface.beacons[i].y = getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 1) / 1000.0; - ProgramInterface.beacons[i].z = getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 2) / 1000.0; - ProgramInterface.beacons[i].yaw = getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 3) / 1000.0; - ProgramInterface.beacons[i].pitch = getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 4) / 1000.0; - ProgramInterface.beacons[i].roll = getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 5) / 1000.0; - } - - for (PortMapping m : ENCODER_CHANNELS) { - final long value = getFeedback(m.messageDataIndex); - final long delta = value - lastEncoderValue[m.robotPortIndex]; - lastEncoderValue[m.robotPortIndex] = value; - - ProgramInterface.encoderChannels[m.robotPortIndex].distance += delta; - if (ProgramInterface.simulationTime > prevSimTime) { - ProgramInterface.encoderChannels[m.robotPortIndex].rate = delta / (ProgramInterface.simulationTime - prevSimTime); - } - } - for (CANPortMapping m : CAN_MOTOR_CHANNELS) { - var status = ProgramInterface.canMotorControllerChannels[m.canId].status; - - long value = getFeedback(m.sensorFeedbackMessageDataIndex); - long delta = value - lastCANSensorValue[m.canId]; - lastCANSensorValue[m.canId] = value; - - status.sensorPosition += delta; - if (ProgramInterface.simulationTime > prevSimTime) { - status.sensorVelocity = delta / (ProgramInterface.simulationTime - prevSimTime); - } - } - for (PortMapping m : DIGITAL_CHANNELS) { - ProgramInterface.digitalChannels[m.robotPortIndex] = getFeedback(m.messageDataIndex) > 0; - } - for (PortMapping m : ANALOG_CHANNELS) { - ProgramInterface.analogChannels[m.robotPortIndex] = getFeedback(m.messageDataIndex) * 5.0 / 1024.0; - } - for (int j = 0; j < NUM_JOYSTICK; ++j) { - for (int a = 0; a < BASE_AXES_PER_JOYSTICK; ++a) { - ProgramInterface.joystickChannels[j].setAxisValue(a, getFeedback(j * BASE_AXES_PER_JOYSTICK + a + BASE_AXIS_START) / 100.0); - } - for (int a = 0; a < ADDITIONAL_AXES_PER_JOYSTICK; ++a) { - ProgramInterface.joystickChannels[j].setAxisValue(a + BASE_AXES_PER_JOYSTICK, getFeedback(j * ADDITIONAL_AXES_PER_JOYSTICK + a + ADDITIONAL_AXIS_START) / 100.0); - } - int denseButtonState = getFeedback(j + JOYSTICK_BUTTON_START); - for (int b = 0; b < BUTTONS_PER_JOYSTICK; ++b) { - ProgramInterface.joystickChannels[j].setButton(b + 1, ((denseButtonState >> b) & 1) != 0); - } - } - - ++ProgramInterface.driverStationUpdateNumber; - } - - return newData; - } - - public void run() { - double prevSimTime = 0; - while (true) { - boolean newData = false; - try { - newData = process(); - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - try { - Thread.sleep(10); - } catch (InterruptedException e1) { - } - } - if (ProgramInterface.simulationTime == 0) { - // Wait for a connection to the simulator before starting to run the robot code. - startTime = System.currentTimeMillis(); - continue; - } - if (resetCounter != lastResetCounter) { - lastResetCounter = resetCounter; - ProgramInterface.program.reset(); - } - if (!newData) { - continue; - } - if (!started) { - // When the simulator has already been running, we seem to need to allow the socket - // service loop to run for a bit to allow buffers in the sockets that communicate - // with the simulator to flush, otherwise the messages queue up which results in a - // significant control latency. - if (System.currentTimeMillis() - startTime > 1000) { - System.out.println("Starting simulation"); - started = true; - } else { - continue; - } - prevSimTime = ProgramInterface.simulationTime; - } - if (ProgramInterface.program != null) { - final double time = ProgramInterface.simulationTime; - ProgramInterface.program.step(time - prevSimTime); - prevSimTime = time; - } - } - } + private static class PortMapping { + public final int messageDataIndex; + public final int robotPortIndex; + + PortMapping(final int messageIndex, final int robotIndex) { + this.messageDataIndex = messageIndex; + this.robotPortIndex = robotIndex; + } + } + + private static class CANPortMapping { + public final int canId; + public final int motorCommandMessageDataIndex; + public final int sensorFeedbackMessageDataIndex; + + CANPortMapping( + final int canId_, + final int motorCommandMessageDataIndex_, + final int sensorFeedbackMessageDataIndex_) { + this.canId = canId_; + this.motorCommandMessageDataIndex = motorCommandMessageDataIndex_; + this.sensorFeedbackMessageDataIndex = sensorFeedbackMessageDataIndex_; + } + } + + /// Command indexes + + private static final int MAX_COMMANDS = 100; + + private static final int RESET_SIM_CHANNEL = 0; + + private static final List PWM_CHANNELS = + Arrays.asList( + // new PortMapping(10, 6), // Left motor + // new PortMapping(11, 4), // Right motor + // new PortMapping(14, 1), // Auxiliary / Center motor + // new PortMapping(12, 0) // Intake + ); + + // CHECKSTYLE:OFF + private static final List SOLENOID_CHANNELS = + Arrays.asList( + new PortMapping(15, 0), // Intake arm + new PortMapping(13, 1) // Catapult launch + ); + // CHECKSTYLE:ON + + private static final List RELAY_CHANNELS = Arrays.asList(); + + // CHECKSTYLE:OFF + private static final List CAN_MOTOR_CHANNELS = + Arrays.asList( + new CANPortMapping(6, 10, 10), // Left motor + new CANPortMapping(4, 11, 11), // Right motor + new CANPortMapping(10, 12, 13), // Intake + new CANPortMapping(12, 14, 0), // Aux/center motor + new CANPortMapping(14, 16, 0), // Aux2 motor + new CANPortMapping(84, 84, 84), // FLD motor + new CANPortMapping(85, 85, 85), // BLD motor + new CANPortMapping(86, 86, 86), // FRD motor + new CANPortMapping(87, 87, 87), // BRD motor + new CANPortMapping(88, 88, 88), // FLS motor + new CANPortMapping(89, 89, 89), // BLS motor + new CANPortMapping(90, 90, 90), // FRS motor + new CANPortMapping(91, 91, 91) // BRS motor + ); + // CHECKSTYLE:ON + + /// Feedback indexes + + private static final int TIMESTAMP_LSW_CHANNEL = 5; + private static final int TIMESTAMP_MSW_CHANNEL = 4; + + private static final int RESET_COUNTER_CHANNEL = 6; + + private static final int ROBOT_MODE_CHANNEL = 3; + private static final Map ROBOT_MODES = + Map.of( + 0, + ProgramInterface.RobotMode.DISABLED, + 1, + ProgramInterface.RobotMode.AUTON, + 2, + ProgramInterface.RobotMode.TELEOP); + + private static final int ROBOT_X_CHANNEL = 8; + private static final int ROBOT_Y_CHANNEL = 9; + + private static final int BEACON_SENSOR_START = 120; + private static final int BEACON_SENSOR_STRIDE = 6; // (x, y, z, yaw, pitch, roll) + + // CHECKSTYLE:OFF + private static final List ENCODER_CHANNELS = + Arrays.asList( + new PortMapping(10, 0), // Left encoder + new PortMapping(11, 2), // Right encoder + new PortMapping(13, 4) // Mechanism encoder + ); + // CHECKSTYLE:ON + + private static final int GYRO_CHANNEL = 15; + private static final int GYRO_RATE_CHANNEL = 16; + private static final int GYRO_PITCH_CHANNEL = 80; + private static final int GYRO_ROLL_CHANNEL = 81; + + // CHECKSTYLE:OFF + private static final List DIGITAL_CHANNELS = + Arrays.asList( + new PortMapping(13, 0), // Intake state + new PortMapping(14, 1), // Ball presence + new PortMapping(17, 4), // Line Sensor 1 + new PortMapping(18, 5), // Line Sensor 2 + new PortMapping(19, 6) // Line Sensor 3 + ); + // CHECKSTYLE:ON + + private static final List ANALOG_CHANNELS = Arrays.asList(); + + private static final int NUM_JOYSTICK = 4; + private static final int BASE_AXIS_START = 20; + private static final int BASE_AXES_PER_JOYSTICK = 4; + private static final int ADDITIONAL_AXIS_START = 100; + private static final int ADDITIONAL_AXES_PER_JOYSTICK = 4; + private static final int JOYSTICK_BUTTON_START = 72; + private static final int BUTTONS_PER_JOYSTICK = 20; + + /// Socket Communication + + private static final int commandsPort = 7661; + private static final int feedbackPort = 7662; + private static final int BUF_SZ = 1024; + + private long startTime; + private boolean started = false; + + private Selector selector; + private InetSocketAddress sendAddr; + private ByteBuffer feedback = ByteBuffer.allocate(BUF_SZ); + private ByteBuffer commands = ByteBuffer.allocate(BUF_SZ); + private int resetCounter = 0; + + private int lastResetCounter = 0; + private double lastGyroValue = Double.NaN; + private long[] lastEncoderValue = new long[ProgramInterface.encoderChannels.length]; + private long[] lastCANSensorValue = + new long[ProgramInterface.canMotorControllerChannels.length]; + + private int getFeedback(final int index) { + return feedback.getInt(index * 4); + } + + private static long assembleLong(final int msw, final int lsw) { + return ((long) msw << 32) | (lsw & 0xffffffffL); + } + + private void putCommand(final int index, final int value) { + commands.putInt(index * 4, value); + } + + private void putCommandFloat(final int index, final double value) { + putCommand(index, (int) (value * 512.0)); + } + + private void putCommandTristate(final int index, final int value) { + if (value == 0) { + putCommand(index, 0); + } else if (value > 0) { + putCommand(index, 511); + } else { + putCommand(index, -512); + } + } + + private void putCommandBool(final int index, final boolean value) { + putCommand(index, value ? 511 : -512); + } + + public VrConnector() throws IOException { + selector = Selector.open(); + DatagramChannel channel = DatagramChannel.open(); + InetSocketAddress receiveAddr = new InetSocketAddress(feedbackPort); + channel.bind(receiveAddr); + sendAddr = new InetSocketAddress(InetAddress.getLoopbackAddress(), commandsPort); + channel.configureBlocking(false); + channel.register(selector, SelectionKey.OP_READ); + commands.limit(MAX_COMMANDS * 4); + commands.order(ByteOrder.LITTLE_ENDIAN); + feedback.order(ByteOrder.LITTLE_ENDIAN); + startTime = System.currentTimeMillis(); + } + + private boolean process() throws IOException { + for (PortMapping m : PWM_CHANNELS) { + putCommandFloat(m.messageDataIndex, ProgramInterface.pwmChannels[m.robotPortIndex]); + } + for (PortMapping m : SOLENOID_CHANNELS) { + putCommandBool(m.messageDataIndex, ProgramInterface.solenoidChannels[m.robotPortIndex]); + } + for (PortMapping m : RELAY_CHANNELS) { + putCommandTristate( + m.messageDataIndex, ProgramInterface.relayChannels[m.robotPortIndex]); + } + for (CANPortMapping m : CAN_MOTOR_CHANNELS) { + putCommandFloat( + m.motorCommandMessageDataIndex, + ProgramInterface.canMotorControllerChannels[m.canId].command.output); + } + + selector.selectedKeys().clear(); + selector.select(); + boolean newData = false; + for (SelectionKey key : selector.selectedKeys()) { + if (!key.isValid()) { + continue; + } + + DatagramChannel chan = (DatagramChannel) key.channel(); + if (key.isReadable()) { + feedback.clear(); + chan.receive(feedback); + newData = true; + key.interestOps(SelectionKey.OP_WRITE); + } + if (key.isWritable()) { + if (started) { + chan.send(commands.duplicate(), sendAddr); + putCommand(RESET_SIM_CHANNEL, 0); + } + key.interestOps(SelectionKey.OP_READ); + } + } + + if (newData) { + double prevSimTime = ProgramInterface.simulationTime; + // Time is sent in milliseconds + ProgramInterface.simulationTime = + assembleLong( + getFeedback(TIMESTAMP_MSW_CHANNEL), + getFeedback(TIMESTAMP_LSW_CHANNEL)) + * 0.001; + + resetCounter = getFeedback(RESET_COUNTER_CHANNEL); + + ProgramInterface.robotMode = ROBOT_MODES.get(getFeedback(ROBOT_MODE_CHANNEL)); + + final double gyroValue = getFeedback(GYRO_CHANNEL) / 10.0; + if (Double.isNaN(lastGyroValue)) { + lastGyroValue = gyroValue; + } + ProgramInterface.gyro.angle += gyroValue - lastGyroValue; + lastGyroValue = gyroValue; + + ProgramInterface.robotPosition.x = getFeedback(ROBOT_X_CHANNEL) / 1000.0; + ProgramInterface.robotPosition.y = getFeedback(ROBOT_Y_CHANNEL) / 1000.0; + ProgramInterface.robotPosition.heading = gyroValue; + + ProgramInterface.gyro.rate = getFeedback(GYRO_RATE_CHANNEL) / 100.0; + ProgramInterface.gyro.pitch = + normalizeAngleDegrees(getFeedback(GYRO_PITCH_CHANNEL) / 10.0); + ProgramInterface.gyro.roll = + normalizeAngleDegrees(getFeedback(GYRO_ROLL_CHANNEL) / 10.0); + + for (int i = 0; i < ProgramInterface.NUM_BEACONS; ++i) { + ProgramInterface.beacons[i].x = + getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 0) / 1000.0; + ProgramInterface.beacons[i].y = + getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 1) / 1000.0; + ProgramInterface.beacons[i].z = + getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 2) / 1000.0; + ProgramInterface.beacons[i].yaw = + getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 3) / 1000.0; + ProgramInterface.beacons[i].pitch = + getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 4) / 1000.0; + ProgramInterface.beacons[i].roll = + getFeedback(BEACON_SENSOR_START + i * BEACON_SENSOR_STRIDE + 5) / 1000.0; + } + + for (PortMapping m : ENCODER_CHANNELS) { + final long value = getFeedback(m.messageDataIndex); + final long delta = value - lastEncoderValue[m.robotPortIndex]; + lastEncoderValue[m.robotPortIndex] = value; + + ProgramInterface.encoderChannels[m.robotPortIndex].distance += delta; + if (ProgramInterface.simulationTime > prevSimTime) { + ProgramInterface.encoderChannels[m.robotPortIndex].rate = + delta / (ProgramInterface.simulationTime - prevSimTime); + } + } + for (CANPortMapping m : CAN_MOTOR_CHANNELS) { + var status = ProgramInterface.canMotorControllerChannels[m.canId].status; + + long value = getFeedback(m.sensorFeedbackMessageDataIndex); + long delta = value - lastCANSensorValue[m.canId]; + lastCANSensorValue[m.canId] = value; + + status.sensorPosition += delta; + if (ProgramInterface.simulationTime > prevSimTime) { + status.sensorVelocity = delta / (ProgramInterface.simulationTime - prevSimTime); + } + } + for (PortMapping m : DIGITAL_CHANNELS) { + ProgramInterface.digitalChannels[m.robotPortIndex] = + getFeedback(m.messageDataIndex) > 0; + } + for (PortMapping m : ANALOG_CHANNELS) { + ProgramInterface.analogChannels[m.robotPortIndex] = + getFeedback(m.messageDataIndex) * 5.0 / 1024.0; + } + for (int j = 0; j < NUM_JOYSTICK; ++j) { + for (int a = 0; a < BASE_AXES_PER_JOYSTICK; ++a) { + ProgramInterface.joystickChannels[j].setAxisValue( + a, + getFeedback(j * BASE_AXES_PER_JOYSTICK + a + BASE_AXIS_START) / 100.0); + } + for (int a = 0; a < ADDITIONAL_AXES_PER_JOYSTICK; ++a) { + ProgramInterface.joystickChannels[j].setAxisValue( + a + BASE_AXES_PER_JOYSTICK, + getFeedback( + j * ADDITIONAL_AXES_PER_JOYSTICK + + a + + ADDITIONAL_AXIS_START) + / 100.0); + } + int denseButtonState = getFeedback(j + JOYSTICK_BUTTON_START); + for (int b = 0; b < BUTTONS_PER_JOYSTICK; ++b) { + ProgramInterface.joystickChannels[j].setButton( + b + 1, ((denseButtonState >> b) & 1) != 0); + } + } + + ++ProgramInterface.driverStationUpdateNumber; + } + + return newData; + } + + public void run() { + double prevSimTime = 0; + while (true) { + boolean newData = false; + try { + newData = process(); + } catch (Exception e) { + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + try { + Thread.sleep(10); + } catch (InterruptedException e1) { + } + } + if (ProgramInterface.simulationTime == 0) { + // Wait for a connection to the simulator before starting to run the robot code. + startTime = System.currentTimeMillis(); + continue; + } + if (resetCounter != lastResetCounter) { + lastResetCounter = resetCounter; + ProgramInterface.program.reset(); + } + if (!newData) { + continue; + } + if (!started) { + // When the simulator has already been running, we seem to need to allow the socket + // service loop to run for a bit to allow buffers in the sockets that communicate + // with the simulator to flush, otherwise the messages queue up which results in a + // significant control latency. + if (System.currentTimeMillis() - startTime > 1000) { + System.out.println("Starting simulation"); + started = true; + } else { + continue; + } + prevSimTime = ProgramInterface.simulationTime; + } + if (ProgramInterface.program != null) { + final double time = ProgramInterface.simulationTime; + ProgramInterface.program.step(time - prevSimTime); + prevSimTime = time; + } + } + } } diff --git a/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java b/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java index 489c30c4..56b2b0a1 100644 --- a/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java @@ -1,7 +1,5 @@ package com.team766.hal.wpilib; -import java.util.function.Function; -import java.util.function.Supplier; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; @@ -11,240 +9,249 @@ import com.team766.hal.MotorController; import com.team766.hal.MotorControllerCommandFailedException; import com.team766.logging.LoggerExceptionUtils; +import java.util.function.Function; +import java.util.function.Supplier; public class CANSparkMaxMotorController extends CANSparkMax implements MotorController { - private Supplier sensorPositionSupplier; - private Supplier sensorVelocitySupplier; - private Function sensorPositionSetter; - private Function sensorInvertedSetter; - private boolean sensorInverted = false; - - public CANSparkMaxMotorController(final int deviceId) { - super(deviceId, MotorType.kBrushless); - - // Set default feedback device. This ensures that our implementations of - // getSensorPosition/getSensorVelocity return values that match what the - // device's PID controller is using. - setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - } - - private enum ExceptionTarget { - THROW, - LOG, - } - - private static void revErrorToException(final ExceptionTarget throwEx, final REVLibError err) { - if (err == REVLibError.kOk) { - return; - } - var ex = new MotorControllerCommandFailedException(err.toString()); - switch (throwEx) { - case THROW: - throw ex; - default: case LOG: - LoggerExceptionUtils.logException(ex); - break; - } - } - - @Override - public double getSensorPosition() { - return sensorPositionSupplier.get(); - } - - @Override - public double getSensorVelocity() { - return sensorVelocitySupplier.get(); - } - - @Override - public void set(final ControlMode mode, final double value) { - switch (mode) { - case Current: - getPIDController().setReference(value, CANSparkMax.ControlType.kCurrent); - break; - case Disabled: - disable(); - break; - case Follower: - throw new IllegalArgumentException("Use follow() method instead"); - case MotionMagic: - throw new IllegalArgumentException("SparkMax does not support MotionMagic"); - case MotionProfile: - throw new IllegalArgumentException("SparkMax does not support MotionProfile"); - case MotionProfileArc: - throw new IllegalArgumentException("SparkMax does not support MotionProfileArc"); - case PercentOutput: - getPIDController().setReference(value, CANSparkMax.ControlType.kDutyCycle); - break; - case Position: - getPIDController().setReference(value, CANSparkMax.ControlType.kPosition); - break; - case Velocity: - getPIDController().setReference(value, CANSparkMax.ControlType.kVelocity); - break; - case Voltage: - getPIDController().setReference(value, CANSparkMax.ControlType.kVoltage); - default: - throw new IllegalArgumentException("Unsupported control mode " + mode); - } - } - - @Override - public void setSensorPosition(final double position) { - revErrorToException(ExceptionTarget.THROW, sensorPositionSetter.apply(position)); - } - - @Override - public void follow(final MotorController leader) { - try { - revErrorToException(ExceptionTarget.LOG, super.follow((CANSparkMax) leader)); - } catch (ClassCastException ex) { - LoggerExceptionUtils.logException(new IllegalArgumentException( - "Spark Max can only follow another Spark Max", ex)); - } - } - - @Override - public void setNeutralMode(final NeutralMode neutralMode) { - switch (neutralMode) { - case Brake: - revErrorToException(ExceptionTarget.LOG, setIdleMode(IdleMode.kBrake)); - break; - case Coast: - revErrorToException(ExceptionTarget.LOG, setIdleMode(IdleMode.kCoast)); - break; - default: - LoggerExceptionUtils.logException(new IllegalArgumentException("Unsupported neutral mode " + neutralMode)); - break; - } - } - - @Override - public void setP(final double value) { - revErrorToException(ExceptionTarget.LOG, getPIDController().setP(value)); - } - - @Override - public void setI(final double value) { - revErrorToException(ExceptionTarget.LOG, getPIDController().setI(value)); - } - - @Override - public void setD(final double value) { - revErrorToException(ExceptionTarget.LOG, getPIDController().setD(value)); - } - - @Override - public void setFF(final double value) { - revErrorToException(ExceptionTarget.LOG, getPIDController().setFF(value)); - } - - @Override - public void setSelectedFeedbackSensor(final FeedbackDevice feedbackDevice) { - switch (feedbackDevice) { - case Analog: { - SparkMaxAnalogSensor analog = getAnalog(SparkMaxAnalogSensor.Mode.kAbsolute); - revErrorToException(ExceptionTarget.LOG, analog.setInverted(sensorInverted)); - sensorPositionSupplier = analog::getPosition; - sensorVelocitySupplier = analog::getVelocity; - sensorPositionSetter = (pos) -> REVLibError.kOk; - sensorInvertedSetter = analog::setInverted; - revErrorToException(ExceptionTarget.LOG, - getPIDController().setFeedbackDevice(analog)); - return; - } - case CTRE_MagEncoder_Absolute: - LoggerExceptionUtils.logException( - new IllegalArgumentException("SparkMax does not support CTRE Mag Encoder")); - case CTRE_MagEncoder_Relative: - LoggerExceptionUtils.logException( - new IllegalArgumentException("SparkMax does not support CTRE Mag Encoder")); - case IntegratedSensor: { - RelativeEncoder encoder = getEncoder(); - // NOTE(rcahoon, 2022-04-19): Don't call this. Trying to call setInverted on the - // integrated sensor returns an error. - // revErrorToException(ExceptionTarget.LOG, encoder.setInverted(sensorInverted)); - sensorPositionSupplier = encoder::getPosition; - sensorVelocitySupplier = encoder::getVelocity; - sensorPositionSetter = encoder::setPosition; - // NOTE(rcahoon, 2022-04-19): Don't call this. Trying to call setInverted on the - // integrated sensor returns an error. - // sensorInvertedSetter = encoder::setInverted; - sensorInvertedSetter = (inverted) -> REVLibError.kOk; - revErrorToException(ExceptionTarget.LOG, - getPIDController().setFeedbackDevice(encoder)); - return; - } - case None: - return; - case PulseWidthEncodedPosition: - LoggerExceptionUtils.logException( - new IllegalArgumentException("SparkMax does not support PWM sensors")); - case QuadEncoder: - // TODO: should we pass a real counts-per-rev scale here? - RelativeEncoder encoder = getAlternateEncoder(1); - revErrorToException(ExceptionTarget.LOG, encoder.setInverted(sensorInverted)); - sensorPositionSupplier = encoder::getPosition; - sensorVelocitySupplier = encoder::getVelocity; - sensorPositionSetter = encoder::setPosition; - sensorInvertedSetter = encoder::setInverted; - revErrorToException(ExceptionTarget.LOG, - getPIDController().setFeedbackDevice(encoder)); - return; - case RemoteSensor0: - LoggerExceptionUtils.logException( - new IllegalArgumentException("SparkMax does not support remote sensors")); - case RemoteSensor1: - LoggerExceptionUtils.logException( - new IllegalArgumentException("SparkMax does not support remote sensors")); - case SensorDifference: - LoggerExceptionUtils.logException( - new IllegalArgumentException("SparkMax does not support SensorDifference")); - case SensorSum: - LoggerExceptionUtils.logException( - new IllegalArgumentException("SparkMax does not support SensorSum")); - case SoftwareEmulatedSensor: - LoggerExceptionUtils.logException(new IllegalArgumentException( - "SparkMax does not support SoftwareEmulatedSensor")); - case Tachometer: - LoggerExceptionUtils.logException( - new IllegalArgumentException("SparkMax does not support Tachometer")); - default: - LoggerExceptionUtils.logException( - new IllegalArgumentException("Unsupported sensor type " + feedbackDevice)); - } - } - - @Override - public void setSensorInverted(final boolean inverted) { - sensorInverted = inverted; - revErrorToException(ExceptionTarget.LOG, sensorInvertedSetter.apply(inverted)); - } - - @Override - public void setOutputRange(final double minOutput, final double maxOutput) { - revErrorToException(ExceptionTarget.LOG, - getPIDController().setOutputRange(minOutput, maxOutput)); - } - - public void setCurrentLimit(final double ampsLimit) { - revErrorToException(ExceptionTarget.LOG, setSmartCurrentLimit((int) ampsLimit)); - } - - @Override - public void restoreFactoryDefault() { - revErrorToException(ExceptionTarget.LOG, restoreFactoryDefaults()); - } - - @Override - public void setOpenLoopRamp(final double secondsFromNeutralToFull) { - revErrorToException(ExceptionTarget.LOG, setOpenLoopRampRate(secondsFromNeutralToFull)); - } - - @Override - public void setClosedLoopRamp(final double secondsFromNeutralToFull) { - revErrorToException(ExceptionTarget.LOG, setClosedLoopRampRate(secondsFromNeutralToFull)); - } + private Supplier sensorPositionSupplier; + private Supplier sensorVelocitySupplier; + private Function sensorPositionSetter; + private Function sensorInvertedSetter; + private boolean sensorInverted = false; + + public CANSparkMaxMotorController(final int deviceId) { + super(deviceId, MotorType.kBrushless); + + // Set default feedback device. This ensures that our implementations of + // getSensorPosition/getSensorVelocity return values that match what the + // device's PID controller is using. + setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + } + + private enum ExceptionTarget { + THROW, + LOG, + } + + private static void revErrorToException(final ExceptionTarget throwEx, final REVLibError err) { + if (err == REVLibError.kOk) { + return; + } + var ex = new MotorControllerCommandFailedException(err.toString()); + switch (throwEx) { + case THROW: + throw ex; + default: + case LOG: + LoggerExceptionUtils.logException(ex); + break; + } + } + + @Override + public double getSensorPosition() { + return sensorPositionSupplier.get(); + } + + @Override + public double getSensorVelocity() { + return sensorVelocitySupplier.get(); + } + + @Override + public void set(final ControlMode mode, final double value) { + switch (mode) { + case Current: + getPIDController().setReference(value, CANSparkMax.ControlType.kCurrent); + break; + case Disabled: + disable(); + break; + case Follower: + throw new IllegalArgumentException("Use follow() method instead"); + case MotionMagic: + throw new IllegalArgumentException("SparkMax does not support MotionMagic"); + case MotionProfile: + throw new IllegalArgumentException("SparkMax does not support MotionProfile"); + case MotionProfileArc: + throw new IllegalArgumentException("SparkMax does not support MotionProfileArc"); + case PercentOutput: + getPIDController().setReference(value, CANSparkMax.ControlType.kDutyCycle); + break; + case Position: + getPIDController().setReference(value, CANSparkMax.ControlType.kPosition); + break; + case Velocity: + getPIDController().setReference(value, CANSparkMax.ControlType.kVelocity); + break; + case Voltage: + getPIDController().setReference(value, CANSparkMax.ControlType.kVoltage); + default: + throw new IllegalArgumentException("Unsupported control mode " + mode); + } + } + + @Override + public void setSensorPosition(final double position) { + revErrorToException(ExceptionTarget.THROW, sensorPositionSetter.apply(position)); + } + + @Override + public void follow(final MotorController leader) { + try { + revErrorToException(ExceptionTarget.LOG, super.follow((CANSparkMax) leader)); + } catch (ClassCastException ex) { + LoggerExceptionUtils.logException( + new IllegalArgumentException( + "Spark Max can only follow another Spark Max", ex)); + } + } + + @Override + public void setNeutralMode(final NeutralMode neutralMode) { + switch (neutralMode) { + case Brake: + revErrorToException(ExceptionTarget.LOG, setIdleMode(IdleMode.kBrake)); + break; + case Coast: + revErrorToException(ExceptionTarget.LOG, setIdleMode(IdleMode.kCoast)); + break; + default: + LoggerExceptionUtils.logException( + new IllegalArgumentException("Unsupported neutral mode " + neutralMode)); + break; + } + } + + @Override + public void setP(final double value) { + revErrorToException(ExceptionTarget.LOG, getPIDController().setP(value)); + } + + @Override + public void setI(final double value) { + revErrorToException(ExceptionTarget.LOG, getPIDController().setI(value)); + } + + @Override + public void setD(final double value) { + revErrorToException(ExceptionTarget.LOG, getPIDController().setD(value)); + } + + @Override + public void setFF(final double value) { + revErrorToException(ExceptionTarget.LOG, getPIDController().setFF(value)); + } + + @Override + public void setSelectedFeedbackSensor(final FeedbackDevice feedbackDevice) { + switch (feedbackDevice) { + case Analog: + { + SparkMaxAnalogSensor analog = getAnalog(SparkMaxAnalogSensor.Mode.kAbsolute); + revErrorToException(ExceptionTarget.LOG, analog.setInverted(sensorInverted)); + sensorPositionSupplier = analog::getPosition; + sensorVelocitySupplier = analog::getVelocity; + sensorPositionSetter = (pos) -> REVLibError.kOk; + sensorInvertedSetter = analog::setInverted; + revErrorToException( + ExceptionTarget.LOG, getPIDController().setFeedbackDevice(analog)); + return; + } + case CTRE_MagEncoder_Absolute: + LoggerExceptionUtils.logException( + new IllegalArgumentException("SparkMax does not support CTRE Mag Encoder")); + case CTRE_MagEncoder_Relative: + LoggerExceptionUtils.logException( + new IllegalArgumentException("SparkMax does not support CTRE Mag Encoder")); + case IntegratedSensor: + { + RelativeEncoder encoder = getEncoder(); + // NOTE(rcahoon, 2022-04-19): Don't call this. Trying to call setInverted on the + // integrated sensor returns an error. + // revErrorToException(ExceptionTarget.LOG, + // encoder.setInverted(sensorInverted)); + sensorPositionSupplier = encoder::getPosition; + sensorVelocitySupplier = encoder::getVelocity; + sensorPositionSetter = encoder::setPosition; + // NOTE(rcahoon, 2022-04-19): Don't call this. Trying to call setInverted on the + // integrated sensor returns an error. + // sensorInvertedSetter = encoder::setInverted; + sensorInvertedSetter = (inverted) -> REVLibError.kOk; + revErrorToException( + ExceptionTarget.LOG, getPIDController().setFeedbackDevice(encoder)); + return; + } + case None: + return; + case PulseWidthEncodedPosition: + LoggerExceptionUtils.logException( + new IllegalArgumentException("SparkMax does not support PWM sensors")); + case QuadEncoder: + // TODO: should we pass a real counts-per-rev scale here? + RelativeEncoder encoder = getAlternateEncoder(1); + revErrorToException(ExceptionTarget.LOG, encoder.setInverted(sensorInverted)); + sensorPositionSupplier = encoder::getPosition; + sensorVelocitySupplier = encoder::getVelocity; + sensorPositionSetter = encoder::setPosition; + sensorInvertedSetter = encoder::setInverted; + revErrorToException( + ExceptionTarget.LOG, getPIDController().setFeedbackDevice(encoder)); + return; + case RemoteSensor0: + LoggerExceptionUtils.logException( + new IllegalArgumentException("SparkMax does not support remote sensors")); + case RemoteSensor1: + LoggerExceptionUtils.logException( + new IllegalArgumentException("SparkMax does not support remote sensors")); + case SensorDifference: + LoggerExceptionUtils.logException( + new IllegalArgumentException("SparkMax does not support SensorDifference")); + case SensorSum: + LoggerExceptionUtils.logException( + new IllegalArgumentException("SparkMax does not support SensorSum")); + case SoftwareEmulatedSensor: + LoggerExceptionUtils.logException( + new IllegalArgumentException( + "SparkMax does not support SoftwareEmulatedSensor")); + case Tachometer: + LoggerExceptionUtils.logException( + new IllegalArgumentException("SparkMax does not support Tachometer")); + default: + LoggerExceptionUtils.logException( + new IllegalArgumentException("Unsupported sensor type " + feedbackDevice)); + } + } + + @Override + public void setSensorInverted(final boolean inverted) { + sensorInverted = inverted; + revErrorToException(ExceptionTarget.LOG, sensorInvertedSetter.apply(inverted)); + } + + @Override + public void setOutputRange(final double minOutput, final double maxOutput) { + revErrorToException( + ExceptionTarget.LOG, getPIDController().setOutputRange(minOutput, maxOutput)); + } + + public void setCurrentLimit(final double ampsLimit) { + revErrorToException(ExceptionTarget.LOG, setSmartCurrentLimit((int) ampsLimit)); + } + + @Override + public void restoreFactoryDefault() { + revErrorToException(ExceptionTarget.LOG, restoreFactoryDefaults()); + } + + @Override + public void setOpenLoopRamp(final double secondsFromNeutralToFull) { + revErrorToException(ExceptionTarget.LOG, setOpenLoopRampRate(secondsFromNeutralToFull)); + } + + @Override + public void setClosedLoopRamp(final double secondsFromNeutralToFull) { + revErrorToException(ExceptionTarget.LOG, setClosedLoopRampRate(secondsFromNeutralToFull)); + } } diff --git a/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java b/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java index 148901a7..f1dd05f3 100644 --- a/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java @@ -14,188 +14,194 @@ public class CANTalonFxMotorController extends BaseCTREMotorController implements MotorController { - private WPI_TalonFX m_device; - private double m_feedForward = 0.0; - - public CANTalonFxMotorController(int deviceNumber, String CANBus) { - m_device = new WPI_TalonFX(deviceNumber, CANBus); - } - - public CANTalonFxMotorController(final int deviceNumber) { - m_device = new WPI_TalonFX(deviceNumber); - } - - @Override - public void set(final ControlMode mode, double value) { - com.ctre.phoenix.motorcontrol.ControlMode ctre_mode = null; - boolean useFourTermSet = true; - switch (mode) { - case PercentOutput: - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput; - useFourTermSet = false; - break; - case Position: - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Position; - break; - case Velocity: - // Sensor velocity is measured in units per 100ms. - value /= 10.0; - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Velocity; - break; - case Current: - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Current; - break; - case Follower: - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Follower; - useFourTermSet = false; - break; - case MotionProfile: - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.MotionProfile; - break; - case MotionMagic: - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.MotionMagic; - break; - case MotionProfileArc: - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.MotionProfileArc; - break; - case Voltage: - m_device.setVoltage(value); - return; - case Disabled: - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Disabled; - useFourTermSet = false; - break; - default: - LoggerExceptionUtils.logException(new UnsupportedOperationException("invalid mode provided. provided value: " + mode)); - break; - } - if (ctre_mode == null) { - Logger.get(Category.HAL).logRaw(Severity.ERROR, - "CAN ControlMode is not translatable: " + mode); - ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Disabled; - } - if (useFourTermSet) { - m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, m_feedForward); - } else { - m_device.set(ctre_mode, value); - } - } - - @Override - public void stopMotor() { - m_device.set(com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput, 0); - } - - @Override - public double getSensorPosition() { - return m_device.getSelectedSensorPosition(0); - } - - @Override - public double getSensorVelocity() { - // Sensor velocity is returned in units per 100ms. - return m_device.getSelectedSensorVelocity(0) * 10.0; - } - - @Override - public void setSensorPosition(final double position) { - errorCodeToException(ExceptionTarget.THROW, - m_device.setSelectedSensorPosition(position, 0, 0)); - } - - @Override - public void follow(final MotorController leader) { - try { - m_device.follow((IMotorController) leader); - } catch (ClassCastException ex) { - LoggerExceptionUtils.logException(new IllegalArgumentException( - "Talon can only follow another CTRE motor controller", ex)); - } - } - - @Override - public void setOpenLoopRamp(final double secondsFromNeutralToFull) { - errorCodeToException(ExceptionTarget.LOG, - m_device.configOpenloopRamp(secondsFromNeutralToFull, TIMEOUT_MS)); - } - - @Override - public void setClosedLoopRamp(final double secondsFromNeutralToFull) { - errorCodeToException(ExceptionTarget.LOG, - m_device.configClosedloopRamp(secondsFromNeutralToFull, TIMEOUT_MS)); - } - - @Override - public void setFF(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kF(0, value, TIMEOUT_MS)); - } - - @Override - public void setP(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kP(0, value)); - } - - @Override - public void setI(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kI(0, value)); - } - - @Override - public void setD(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kD(0, value)); - } - - @Override - public void setSelectedFeedbackSensor(final FeedbackDevice feedbackDevice) { - errorCodeToException(ExceptionTarget.LOG, - m_device.configSelectedFeedbackSensor(feedbackDevice)); - } - - @Override - public void setSensorInverted(final boolean inverted) { - m_device.setSensorPhase(inverted); - } - - @Override - public void setOutputRange(final double minOutput, final double maxOutput) { - errorCodeToException(ExceptionTarget.LOG, m_device.configPeakOutputReverse(minOutput)); - errorCodeToException(ExceptionTarget.LOG, m_device.configPeakOutputForward(maxOutput)); - } - - @Override - public void setCurrentLimit(final double ampsLimit) { - errorCodeToException(ExceptionTarget.LOG, m_device.configSupplyCurrentLimit( - new SupplyCurrentLimitConfiguration(true, ampsLimit, 0, 0.01))); - } - - @Override - public void restoreFactoryDefault() { - errorCodeToException(ExceptionTarget.LOG, m_device.configFactoryDefault()); - } - - @Override - public double get() { - return m_device.get(); - } - - @Override - public void set(final double power) { - m_device.set(power); - } - - @Override - public void setInverted(final boolean isInverted) { - m_device.setInverted(isInverted); - } - - @Override - public boolean getInverted() { - return m_device.getInverted(); - } - - @Override - public void setNeutralMode(final NeutralMode neutralMode) { - m_device.setNeutralMode(neutralMode); - } - + private WPI_TalonFX m_device; + private double m_feedForward = 0.0; + + public CANTalonFxMotorController(int deviceNumber, String CANBus) { + m_device = new WPI_TalonFX(deviceNumber, CANBus); + } + + public CANTalonFxMotorController(final int deviceNumber) { + m_device = new WPI_TalonFX(deviceNumber); + } + + @Override + public void set(final ControlMode mode, double value) { + com.ctre.phoenix.motorcontrol.ControlMode ctre_mode = null; + boolean useFourTermSet = true; + switch (mode) { + case PercentOutput: + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput; + useFourTermSet = false; + break; + case Position: + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Position; + break; + case Velocity: + // Sensor velocity is measured in units per 100ms. + value /= 10.0; + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Velocity; + break; + case Current: + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Current; + break; + case Follower: + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Follower; + useFourTermSet = false; + break; + case MotionProfile: + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.MotionProfile; + break; + case MotionMagic: + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.MotionMagic; + break; + case MotionProfileArc: + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.MotionProfileArc; + break; + case Voltage: + m_device.setVoltage(value); + return; + case Disabled: + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Disabled; + useFourTermSet = false; + break; + default: + LoggerExceptionUtils.logException( + new UnsupportedOperationException( + "invalid mode provided. provided value: " + mode)); + break; + } + if (ctre_mode == null) { + Logger.get(Category.HAL) + .logRaw(Severity.ERROR, "CAN ControlMode is not translatable: " + mode); + ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Disabled; + } + if (useFourTermSet) { + m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, m_feedForward); + } else { + m_device.set(ctre_mode, value); + } + } + + @Override + public void stopMotor() { + m_device.set(com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput, 0); + } + + @Override + public double getSensorPosition() { + return m_device.getSelectedSensorPosition(0); + } + + @Override + public double getSensorVelocity() { + // Sensor velocity is returned in units per 100ms. + return m_device.getSelectedSensorVelocity(0) * 10.0; + } + + @Override + public void setSensorPosition(final double position) { + errorCodeToException( + ExceptionTarget.THROW, m_device.setSelectedSensorPosition(position, 0, 0)); + } + + @Override + public void follow(final MotorController leader) { + try { + m_device.follow((IMotorController) leader); + } catch (ClassCastException ex) { + LoggerExceptionUtils.logException( + new IllegalArgumentException( + "Talon can only follow another CTRE motor controller", ex)); + } + } + + @Override + public void setOpenLoopRamp(final double secondsFromNeutralToFull) { + errorCodeToException( + ExceptionTarget.LOG, + m_device.configOpenloopRamp(secondsFromNeutralToFull, TIMEOUT_MS)); + } + + @Override + public void setClosedLoopRamp(final double secondsFromNeutralToFull) { + errorCodeToException( + ExceptionTarget.LOG, + m_device.configClosedloopRamp(secondsFromNeutralToFull, TIMEOUT_MS)); + } + + @Override + public void setFF(final double value) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kF(0, value, TIMEOUT_MS)); + } + + @Override + public void setP(final double value) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kP(0, value)); + } + + @Override + public void setI(final double value) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kI(0, value)); + } + + @Override + public void setD(final double value) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kD(0, value)); + } + + @Override + public void setSelectedFeedbackSensor(final FeedbackDevice feedbackDevice) { + errorCodeToException( + ExceptionTarget.LOG, m_device.configSelectedFeedbackSensor(feedbackDevice)); + } + + @Override + public void setSensorInverted(final boolean inverted) { + m_device.setSensorPhase(inverted); + } + + @Override + public void setOutputRange(final double minOutput, final double maxOutput) { + errorCodeToException(ExceptionTarget.LOG, m_device.configPeakOutputReverse(minOutput)); + errorCodeToException(ExceptionTarget.LOG, m_device.configPeakOutputForward(maxOutput)); + } + + @Override + public void setCurrentLimit(final double ampsLimit) { + errorCodeToException( + ExceptionTarget.LOG, + m_device.configSupplyCurrentLimit( + new SupplyCurrentLimitConfiguration(true, ampsLimit, 0, 0.01))); + } + + @Override + public void restoreFactoryDefault() { + errorCodeToException(ExceptionTarget.LOG, m_device.configFactoryDefault()); + } + + @Override + public double get() { + return m_device.get(); + } + + @Override + public void set(final double power) { + m_device.set(power); + } + + @Override + public void setInverted(final boolean isInverted) { + m_device.setInverted(isInverted); + } + + @Override + public boolean getInverted() { + return m_device.getInverted(); + } + + @Override + public void setNeutralMode(final NeutralMode neutralMode) { + m_device.setNeutralMode(neutralMode); + } } diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index f275f81e..f3421679 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -1,10 +1,7 @@ package com.team766.hal.wpilib; -import java.io.File; -//import java.nio.file.Files; -import java.nio.file.Path; -import java.util.function.Supplier; import com.team766.config.ConfigFileReader; +import com.team766.framework.Scheduler; import com.team766.hal.CanivPoller; import com.team766.hal.GenericRobotMain; import com.team766.hal.RobotProvider; @@ -12,155 +9,160 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.TimedRobot; +import java.io.File; +// import java.nio.file.Files; +import java.nio.file.Path; +import java.util.function.Supplier; public class RobotMain extends TimedRobot { - // this file, if present, will be a symlink to one of several config files in the deploy directory. - // this allows for the same code to be deployed to multiple physical robots, each with their own - // config file with CAN bus port mappings, etc, with the actual file used for a specific robot - // to be "selected" via this symlink to the actual file. - private static final String SELECTED_CONFIG_FILE = "/home/lvuser/selectedConfig.txt"; - - // if the symlink (above) is not present, back off to this file in the deploy directory. - private static final String DEFAULT_CONFIG_FILE = "configs/defaultRobotConfig.txt"; - - // for backwards compatibility, back off to the previous config file location if the above are - // not - // found in the deploy directory. - private static final String LEGACY_CONFIG_FILE = "/home/lvuser/robotConfig.txt"; - - private GenericRobotMain robot; - - public static void main(final String... args) { - Supplier supplier = new Supplier() { - RobotMain instance; - - @Override - public RobotMain get() { - if (instance == null) { - instance = new RobotMain(); - } - return instance; - } - }; - - // periodically poll "caniv" in the background, if present - CanivPoller canivPoller = null; - if (new File(CanivPoller.CANIV_BIN).exists()) { - canivPoller = new CanivPoller(10*1000 /* millis */); - new Thread(canivPoller, "caniv poller").start(); - } - - try { - RobotBase.startRobot(supplier); - } catch (Throwable ex) { - ex.printStackTrace(); - LoggerExceptionUtils.logException(ex); - } - - if (canivPoller != null) { - canivPoller.setDone(true); - } - } - - public RobotMain() { - super(0.005); - } - - private static String checkForAndReturnPathToConfigFile(final String file) { - Path configPath = Filesystem.getDeployDirectory().toPath().resolve(file); - File configFile = configPath.toFile(); - if (configFile.exists()) { - return configFile.getPath(); - } - return null; - } - - @Override - public void robotInit() { - try { - String filename = null; - filename = checkForAndReturnPathToConfigFile(SELECTED_CONFIG_FILE); - - if (filename == null) { - filename = checkForAndReturnPathToConfigFile(DEFAULT_CONFIG_FILE); - } - - if (filename == null) { - filename = LEGACY_CONFIG_FILE; - } - - ConfigFileReader.instance = new ConfigFileReader(filename); - RobotProvider.instance = new WPIRobotProvider(); - robot = new GenericRobotMain(); - - robot.robotInit(); - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - } - } - - @Override - public void disabledInit() { - try { - robot.disabledInit(); - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - } - } - - @Override - public void autonomousInit() { - try { - robot.autonomousInit(); - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - } - } - - @Override - public void teleopInit() { - try { - robot.teleopInit(); - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - } - } - - @Override - public void disabledPeriodic() { - try { - robot.disabledPeriodic(); - }catch (Exception e){ - Scheduler.getInstance().run(); - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - } - } - - @Override - public void autonomousPeriodic() { - try { - robot.autonomousPeriodic(); - Scheduler.getInstance().run(); - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - } - } - - @Override - public void teleopPeriodic() { - try { - robot.teleopPeriodic(); - Scheduler.getInstance().run(); - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - } - } + // this file, if present, will be a symlink to one of several config files in the deploy + // directory. + // this allows for the same code to be deployed to multiple physical robots, each with their own + // config file with CAN bus port mappings, etc, with the actual file used for a specific robot + // to be "selected" via this symlink to the actual file. + private static final String SELECTED_CONFIG_FILE = "/home/lvuser/selectedConfig.txt"; + + // if the symlink (above) is not present, back off to this file in the deploy directory. + private static final String DEFAULT_CONFIG_FILE = "configs/defaultRobotConfig.txt"; + + // for backwards compatibility, back off to the previous config file location if the above are + // not + // found in the deploy directory. + private static final String LEGACY_CONFIG_FILE = "/home/lvuser/robotConfig.txt"; + + private GenericRobotMain robot; + + public static void main(final String... args) { + Supplier supplier = + new Supplier() { + RobotMain instance; + + @Override + public RobotMain get() { + if (instance == null) { + instance = new RobotMain(); + } + return instance; + } + }; + + // periodically poll "caniv" in the background, if present + CanivPoller canivPoller = null; + if (new File(CanivPoller.CANIV_BIN).exists()) { + canivPoller = new CanivPoller(10 * 1000 /* millis */); + new Thread(canivPoller, "caniv poller").start(); + } + + try { + RobotBase.startRobot(supplier); + } catch (Throwable ex) { + ex.printStackTrace(); + LoggerExceptionUtils.logException(ex); + } + + if (canivPoller != null) { + canivPoller.setDone(true); + } + } + + public RobotMain() { + super(0.005); + } + + private static String checkForAndReturnPathToConfigFile(final String file) { + Path configPath = Filesystem.getDeployDirectory().toPath().resolve(file); + File configFile = configPath.toFile(); + if (configFile.exists()) { + return configFile.getPath(); + } + return null; + } + + @Override + public void robotInit() { + try { + String filename = null; + filename = checkForAndReturnPathToConfigFile(SELECTED_CONFIG_FILE); + + if (filename == null) { + filename = checkForAndReturnPathToConfigFile(DEFAULT_CONFIG_FILE); + } + + if (filename == null) { + filename = LEGACY_CONFIG_FILE; + } + + ConfigFileReader.instance = new ConfigFileReader(filename); + RobotProvider.instance = new WPIRobotProvider(); + robot = new GenericRobotMain(); + + robot.robotInit(); + } catch (Exception e) { + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + } + } + + @Override + public void disabledInit() { + try { + robot.disabledInit(); + } catch (Exception e) { + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + } + } + + @Override + public void autonomousInit() { + try { + robot.autonomousInit(); + } catch (Exception e) { + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + } + } + + @Override + public void teleopInit() { + try { + robot.teleopInit(); + } catch (Exception e) { + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + } + } + + @Override + public void disabledPeriodic() { + try { + robot.disabledPeriodic(); + } catch (Exception e) { + Scheduler.getInstance().run(); + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + } + } + + @Override + public void autonomousPeriodic() { + try { + robot.autonomousPeriodic(); + Scheduler.getInstance().run(); + } catch (Exception e) { + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + } + } + + @Override + public void teleopPeriodic() { + try { + robot.teleopPeriodic(); + Scheduler.getInstance().run(); + } catch (Exception e) { + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + } + } } diff --git a/src/main/java/com/team766/hal/wpilib/Solenoid.java b/src/main/java/com/team766/hal/wpilib/Solenoid.java index 582f01ff..b1b28255 100755 --- a/src/main/java/com/team766/hal/wpilib/Solenoid.java +++ b/src/main/java/com/team766/hal/wpilib/Solenoid.java @@ -1,15 +1,14 @@ package com.team766.hal.wpilib; import com.team766.hal.SolenoidController; - import edu.wpi.first.wpilibj.PneumaticsModuleType; public class Solenoid extends edu.wpi.first.wpilibj.Solenoid implements SolenoidController { - public Solenoid(int channel) { - super(PneumaticsModuleType.REVPH, channel); - } + public Solenoid(int channel) { + super(PneumaticsModuleType.REVPH, channel); + } - public Solenoid(int channel, PneumaticsModuleType type) { - super(type, channel); - } + public Solenoid(int channel, PneumaticsModuleType type) { + super(type, channel); + } } diff --git a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java index 227ca1f6..c5241879 100755 --- a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java +++ b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java @@ -1,7 +1,5 @@ package com.team766.hal.wpilib; -import java.util.concurrent.atomic.AtomicBoolean; -import java.util.concurrent.atomic.AtomicInteger; import com.team766.config.ConfigFileReader; import com.team766.hal.AnalogInputReader; import com.team766.hal.BeaconReader; @@ -14,16 +12,16 @@ import com.team766.hal.GyroReader; import com.team766.hal.JoystickReader; import com.team766.hal.LocalMotorController; +import com.team766.hal.MotorController; import com.team766.hal.PositionReader; import com.team766.hal.RelayOutput; import com.team766.hal.RobotProvider; import com.team766.hal.SolenoidController; -import com.team766.hal.MotorController; import com.team766.hal.mock.MockBeaconSensor; import com.team766.hal.mock.MockGyro; +import com.team766.hal.mock.MockMotorController; import com.team766.hal.mock.MockPositionSensor; import com.team766.library.ValueProvider; -import com.team766.hal.mock.MockMotorController; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; @@ -36,251 +34,266 @@ import edu.wpi.first.wpilibj.PneumaticsControlModule; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SPI; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; public class WPIRobotProvider extends RobotProvider { - /** - * Runnable that counts the number of times we receive new data from the driver station. Used as - * part of impl of {@link #hasNewDriverStationData()}. - */ - private static class DataRefreshRunnable implements Runnable { - private final AtomicBoolean m_keepAlive = new AtomicBoolean(); - private final AtomicInteger m_dataCount = new AtomicInteger(); + /** + * Runnable that counts the number of times we receive new data from the driver station. Used as + * part of impl of {@link #hasNewDriverStationData()}. + */ + private static class DataRefreshRunnable implements Runnable { + private final AtomicBoolean m_keepAlive = new AtomicBoolean(); + private final AtomicInteger m_dataCount = new AtomicInteger(); + + DataRefreshRunnable() { + m_keepAlive.set(true); + } - DataRefreshRunnable() { - m_keepAlive.set(true); - } + public void cancel() { + m_keepAlive.set(false); + } - public void cancel() { - m_keepAlive.set(false); - } + @Override + public void run() { + // create and register a handle that gets notified whenever there's new DS data. + int handle = WPIUtilJNI.createEvent(false, false); + DriverStationJNI.provideNewDataEventHandle(handle); - @Override - public void run() { - // create and register a handle that gets notified whenever there's new DS data. - int handle = WPIUtilJNI.createEvent(false, false); - DriverStationJNI.provideNewDataEventHandle(handle); + while (m_keepAlive.get()) { + try { + // wait for new data or timeout + // (timeout returns true) + if (!WPIUtilJNI.waitForObjectTimeout(handle, 0.1)) { + m_dataCount.incrementAndGet(); + } + } catch (InterruptedException e) { + // should only happen during failures + LoggerExceptionUtils.logException(e); - while (m_keepAlive.get()) { - try { - // wait for new data or timeout - // (timeout returns true) - if (!WPIUtilJNI.waitForObjectTimeout(handle, 0.1)) { - m_dataCount.incrementAndGet(); - } - } catch (InterruptedException e) { - // should only happen during failures - LoggerExceptionUtils.logException(e); + // clean up handle + DriverStationJNI.removeNewDataEventHandle(handle); + WPIUtilJNI.destroyEvent(handle); - // clean up handle - DriverStationJNI.removeNewDataEventHandle(handle); - WPIUtilJNI.destroyEvent(handle); + // re-register handle in an attempt to keep data flowing. + handle = WPIUtilJNI.createEvent(false, false); + DriverStationJNI.provideNewDataEventHandle(handle); + } + } - // re-register handle in an attempt to keep data flowing. - handle = WPIUtilJNI.createEvent(false, false); - DriverStationJNI.provideNewDataEventHandle(handle); - } - } + DriverStationJNI.removeNewDataEventHandle(handle); + WPIUtilJNI.destroyEvent(handle); + } + } - DriverStationJNI.removeNewDataEventHandle(handle); - WPIUtilJNI.destroyEvent(handle); - } - } + private DataRefreshRunnable m_DataRefreshRunnable = new DataRefreshRunnable(); + private Thread m_dataRefreshThread; + private int m_lastDataCount = 0; - private DataRefreshRunnable m_DataRefreshRunnable = new DataRefreshRunnable(); - private Thread m_dataRefreshThread; - private int m_lastDataCount = 0; + public WPIRobotProvider() { + m_dataRefreshThread = new Thread(m_DataRefreshRunnable, "DataRefreshThread"); + m_dataRefreshThread.start(); + try { + ph.enableCompressorAnalog(90, 115); + } catch (Exception ex) { + LoggerExceptionUtils.logException(ex); + } + } - public WPIRobotProvider() { - m_dataRefreshThread = new Thread(m_DataRefreshRunnable, "DataRefreshThread"); - m_dataRefreshThread.start(); - try { - ph.enableCompressorAnalog(90, 115); - } catch (Exception ex) { - LoggerExceptionUtils.logException(ex); - } - } + private MotorController[][] motors = + new MotorController[MotorController.Type.values().length][64]; - private MotorController[][] motors = - new MotorController[MotorController.Type.values().length][64]; + // The presence of this object allows the compressor to run before we've declared any solenoids. + @SuppressWarnings("unused") + private PneumaticsControlModule pcm = new PneumaticsControlModule(); - // The presence of this object allows the compressor to run before we've declared any solenoids. - @SuppressWarnings("unused") - private PneumaticsControlModule pcm = new PneumaticsControlModule(); - private PneumaticHub ph = new PneumaticHub(); + private PneumaticHub ph = new PneumaticHub(); - @Override - public MotorController getMotor(final int index, final String configPrefix, - final MotorController.Type type, ControlInputReader localSensor) { - if (motors[type.ordinal()][index] != null) { - return motors[type.ordinal()][index]; - } - MotorController motor = null; - switch (type) { - case SparkMax: - try { - motor = new CANSparkMaxMotorController(index); - } catch (Exception ex) { - LoggerExceptionUtils.logException(ex); - motor = new LocalMotorController(configPrefix, new MockMotorController(index), - localSensor); - localSensor = null; - } - break; - case TalonSRX: - motor = new CANTalonMotorController(index); - break; - case VictorSPX: - motor = new CANVictorMotorController(index); - break; - case TalonFX: - final ValueProvider CANBus = ConfigFileReader.getInstance().getString(configPrefix + ".CANBus"); - motor = new CANTalonFxMotorController(index, CANBus.get()); - break; - case VictorSP: - motor = new LocalMotorController(configPrefix, new PWMVictorSP(index), localSensor); - localSensor = null; - break; - default: - break; - } - if (motor == null) { - LoggerExceptionUtils - .logException(new IllegalArgumentException("Unsupported motor type " + type)); - motor = new LocalMotorController(configPrefix, new MockMotorController(index), - localSensor); - localSensor = null; - } - if (localSensor != null) { - motor = new LocalMotorController(configPrefix, motor, localSensor); - } - motors[type.ordinal()][index] = motor; - return motor; - } + @Override + public MotorController getMotor( + final int index, + final String configPrefix, + final MotorController.Type type, + ControlInputReader localSensor) { + if (motors[type.ordinal()][index] != null) { + return motors[type.ordinal()][index]; + } + MotorController motor = null; + switch (type) { + case SparkMax: + try { + motor = new CANSparkMaxMotorController(index); + } catch (Exception ex) { + LoggerExceptionUtils.logException(ex); + motor = + new LocalMotorController( + configPrefix, new MockMotorController(index), localSensor); + localSensor = null; + } + break; + case TalonSRX: + motor = new CANTalonMotorController(index); + break; + case VictorSPX: + motor = new CANVictorMotorController(index); + break; + case TalonFX: + final ValueProvider CANBus = + ConfigFileReader.getInstance().getString(configPrefix + ".CANBus"); + motor = new CANTalonFxMotorController(index, CANBus.get()); + break; + case VictorSP: + motor = new LocalMotorController(configPrefix, new PWMVictorSP(index), localSensor); + localSensor = null; + break; + default: + break; + } + if (motor == null) { + LoggerExceptionUtils.logException( + new IllegalArgumentException("Unsupported motor type " + type)); + motor = + new LocalMotorController( + configPrefix, new MockMotorController(index), localSensor); + localSensor = null; + } + if (localSensor != null) { + motor = new LocalMotorController(configPrefix, motor, localSensor); + } + motors[type.ordinal()][index] = motor; + return motor; + } - @Override - public EncoderReader getEncoder(int index1, int index2) { - if (encoders[index1] == null) { - encoders[index1] = new Encoder(index1, index2); - } - return encoders[index1]; - } + @Override + public EncoderReader getEncoder(int index1, int index2) { + if (encoders[index1] == null) { + encoders[index1] = new Encoder(index1, index2); + } + return encoders[index1]; + } - @Override - public SolenoidController getSolenoid(int index) { - if(solenoids[index] == null) { - solenoids[index] = new Solenoid(index); - } - return solenoids[index]; - } + @Override + public SolenoidController getSolenoid(int index) { + if (solenoids[index] == null) { + solenoids[index] = new Solenoid(index); + } + return solenoids[index]; + } - @Override - // Gyro index values: - // -1 = Spartan Gyro - // 0+ = Analog Gyro on port index - public GyroReader getGyro(final int index) { - if (gyros[index + 2] == null) { - if (index < -2) { - Logger.get(Category.CONFIGURATION).logRaw(Severity.ERROR, "Invalid gyro port " - + index + ". Must be -2, -1, or a non-negative integer"); - return new MockGyro(); - } else if (index == -2) { - gyros[index + 2] = new NavXGyro(I2C.Port.kOnboard); - } else if (index == -1) { - gyros[index + 2] = new ADXRS450_Gyro(SPI.Port.kOnboardCS0); - } else { - gyros[index + 2] = new AnalogGyro(index); - } - } - return gyros[index + 2]; - } + @Override + // Gyro index values: + // -1 = Spartan Gyro + // 0+ = Analog Gyro on port index + public GyroReader getGyro(final int index) { + if (gyros[index + 2] == null) { + if (index < -2) { + Logger.get(Category.CONFIGURATION) + .logRaw( + Severity.ERROR, + "Invalid gyro port " + + index + + ". Must be -2, -1, or a non-negative integer"); + return new MockGyro(); + } else if (index == -2) { + gyros[index + 2] = new NavXGyro(I2C.Port.kOnboard); + } else if (index == -1) { + gyros[index + 2] = new ADXRS450_Gyro(SPI.Port.kOnboardCS0); + } else { + gyros[index + 2] = new AnalogGyro(index); + } + } + return gyros[index + 2]; + } - @Override - public CameraReader getCamera(final String id, final String value) { - System.err.println("Camera support not yet avaible"); - return null; - } + @Override + public CameraReader getCamera(final String id, final String value) { + System.err.println("Camera support not yet avaible"); + return null; + } - @Override - public JoystickReader getJoystick(int index) { - if (joysticks[index] == null) { - joysticks[index] = new Joystick(index); - } - return joysticks[index]; - } + @Override + public JoystickReader getJoystick(int index) { + if (joysticks[index] == null) { + joysticks[index] = new Joystick(index); + } + return joysticks[index]; + } - @Override - public CameraInterface getCamServer() { - return new com.team766.hal.wpilib.CameraInterface(); - } + @Override + public CameraInterface getCamServer() { + return new com.team766.hal.wpilib.CameraInterface(); + } - @Override - public DigitalInputReader getDigitalInput(final int index) { - if (digInputs[index] == null) { - digInputs[index] = new DigitalInput(index); - } - return digInputs[index]; - } + @Override + public DigitalInputReader getDigitalInput(final int index) { + if (digInputs[index] == null) { + digInputs[index] = new DigitalInput(index); + } + return digInputs[index]; + } - @Override - public AnalogInputReader getAnalogInput(int index) { - if (angInputs[index] == null) { - angInputs[index] = new AnalogInput(index); - } - return angInputs[index]; - } + @Override + public AnalogInputReader getAnalogInput(int index) { + if (angInputs[index] == null) { + angInputs[index] = new AnalogInput(index); + } + return angInputs[index]; + } - @Override - public RelayOutput getRelay(int index) { - if (relays[index] == null) { - relays[index] = new Relay(index); - } - return relays[index]; - } + @Override + public RelayOutput getRelay(int index) { + if (relays[index] == null) { + relays[index] = new Relay(index); + } + return relays[index]; + } - @Override - public PositionReader getPositionSensor() { - if (positionSensor == null) { - positionSensor = new MockPositionSensor(); - Logger.get(Category.CONFIGURATION).logRaw(Severity.ERROR, - "Position sensor does not exist on real robots. Using mock position sensor instead - it will always return a position of 0"); - } - return positionSensor; - } + @Override + public PositionReader getPositionSensor() { + if (positionSensor == null) { + positionSensor = new MockPositionSensor(); + Logger.get(Category.CONFIGURATION) + .logRaw( + Severity.ERROR, + "Position sensor does not exist on real robots. Using mock position sensor instead - it will always return a position of 0"); + } + return positionSensor; + } - @Override - public BeaconReader getBeaconSensor() { - if (beaconSensor == null) { - beaconSensor = new MockBeaconSensor(); - Logger.get(Category.CONFIGURATION).logRaw( - Severity.ERROR, - "Beacon sensor does not exist on real robots. Using mock beacon sensor instead - it will always return no beacons" - ); - } - return beaconSensor; - } + @Override + public BeaconReader getBeaconSensor() { + if (beaconSensor == null) { + beaconSensor = new MockBeaconSensor(); + Logger.get(Category.CONFIGURATION) + .logRaw( + Severity.ERROR, + "Beacon sensor does not exist on real robots. Using mock beacon sensor instead - it will always return no beacons"); + } + return beaconSensor; + } - @Override - public Clock getClock() { - return SystemClock.instance; - } + @Override + public Clock getClock() { + return SystemClock.instance; + } - @Override - public void refreshDriverStationData() { - DriverStation.refreshData(); - } + @Override + public void refreshDriverStationData() { + DriverStation.refreshData(); + } - @Override - public boolean hasNewDriverStationData() { - // see if the thread has counted more data changes than the last time this method was called - int currentDataRefreshThreadCount = m_DataRefreshRunnable.m_dataCount.get(); - boolean dataChanged = m_lastDataCount != currentDataRefreshThreadCount; - m_lastDataCount = currentDataRefreshThreadCount; - return dataChanged; - } + @Override + public boolean hasNewDriverStationData() { + // see if the thread has counted more data changes than the last time this method was called + int currentDataRefreshThreadCount = m_DataRefreshRunnable.m_dataCount.get(); + boolean dataChanged = m_lastDataCount != currentDataRefreshThreadCount; + m_lastDataCount = currentDataRefreshThreadCount; + return dataChanged; + } - @Override - public double getBatteryVoltage() { - return RobotController.getBatteryVoltage(); - } + @Override + public double getBatteryVoltage() { + return RobotController.getBatteryVoltage(); + } } diff --git a/src/main/java/com/team766/library/RateLimiter.java b/src/main/java/com/team766/library/RateLimiter.java index 18384280..9152331d 100644 --- a/src/main/java/com/team766/library/RateLimiter.java +++ b/src/main/java/com/team766/library/RateLimiter.java @@ -10,10 +10,6 @@ public RateLimiter(final double periodSeconds_) { this.periodSeconds = periodSeconds_; } - public void reset() { - nextTime = 0; - } - public boolean next() { final double now = RobotProvider.getTimeProvider().get(); if (now > nextTime) { diff --git a/src/main/java/com/team766/logging/Logger.java b/src/main/java/com/team766/logging/Logger.java index dcef58e4..0d99de9c 100644 --- a/src/main/java/com/team766/logging/Logger.java +++ b/src/main/java/com/team766/logging/Logger.java @@ -1,5 +1,7 @@ package com.team766.logging; +import com.team766.config.ConfigFileReader; +import com.team766.library.CircularBuffer; import java.io.File; import java.text.SimpleDateFormat; import java.util.Collection; @@ -7,124 +9,132 @@ import java.util.Date; import java.util.EnumMap; -import com.team766.config.ConfigFileReader; -import com.team766.library.CircularBuffer; - public final class Logger { - private static class LogUncaughtException implements Thread.UncaughtExceptionHandler { - public void uncaughtException(final Thread t, final Throwable e) { - e.printStackTrace(); - - LoggerExceptionUtils.logException(e); - - if (m_logWriter != null) { - try { - m_logWriter.close(); - } catch (Exception e1) { - e1.printStackTrace(); - } - } - - System.exit(1); - } - } - - private static final int MAX_NUM_RECENT_ENTRIES = 100; - - private static EnumMap m_loggers = - new EnumMap(Category.class); - private static LogWriter m_logWriter = null; - private CircularBuffer m_recentEntries = - new CircularBuffer(MAX_NUM_RECENT_ENTRIES); - private static Object s_lastWriteTimeSync = new Object(); - private static long s_lastWriteTime = 0L; - - public static String logFilePathBase = null; - - static { - for (Category category : Category.values()) { - m_loggers.put(category, new Logger(category)); - } - try { - ConfigFileReader config_file = ConfigFileReader.getInstance(); - if (config_file != null) { - logFilePathBase = config_file.getString("logFilePath").get(); - new File(logFilePathBase).mkdirs(); - final String timestamp = - new SimpleDateFormat("yyyyMMdd'T'HHmmss").format(new Date()); - final String logFilePath = new File(logFilePathBase, timestamp).getAbsolutePath(); - m_logWriter = new LogWriter(logFilePath); - get(Category.CONFIGURATION).logRaw(Severity.INFO, "Logging to " + logFilePath); - } else { - get(Category.CONFIGURATION).logRaw(Severity.ERROR, - "Config file is not available. Logs will only be in-memory and will be lost when the robot is turned off."); - } - } catch (Exception e) { - e.printStackTrace(); - LoggerExceptionUtils.logException(e); - } - - Thread.setDefaultUncaughtExceptionHandler(new LogUncaughtException()); - } - - public static Logger get(final Category category) { - return m_loggers.get(category); - } - - static long getTime() { - long nowNanosec = new Date().getTime() * 1000000; - synchronized (s_lastWriteTimeSync) { - // Ensure that log entries' timestamps are unique. This is important - // because the log viewer uses an entry's timestamp as a unique ID, - // and we don't want two different log entries to accidentally - // compare as equal. - nowNanosec = s_lastWriteTime = Math.max(nowNanosec, s_lastWriteTime); - } - return nowNanosec; - } - - private final Category m_category; - - private Logger(final Category category) { - m_category = category; - } - - public Collection recentEntries() { - return Collections.unmodifiableCollection(m_recentEntries); - } - - public void logData(final Severity severity, final String format, final Object... args) { - var entry = LogEntry.newBuilder().setTime(getTime()).setSeverity(severity) - .setCategory(m_category); - entry.setMessageStr(String.format(format, args)); - m_recentEntries.add(entry.build()); - entry.setMessageStr(format); - for (Object arg : args) { - var logValue = LogValue.newBuilder(); - SerializationUtils.valueToProto(arg, logValue); - entry.addArg(logValue.build()); - } - if (m_logWriter != null) { - m_logWriter.logStoredFormat(entry); - } - } - - public void logRaw(final Severity severity, final String message) { - var entry = LogEntry.newBuilder().setTime(getTime()).setSeverity(severity) - .setCategory(m_category).setMessageStr(message).build(); - m_recentEntries.add(entry); - if (m_logWriter != null) { - m_logWriter.log(entry); - } - } - - void logOnlyInMemory(final Severity severity, final String message) { - var entry = LogEntry.newBuilder() - .setTime(getTime()) - .setSeverity(severity) - .setCategory(m_category) - .setMessageStr(message) - .build(); - m_recentEntries.add(entry); - } + private static class LogUncaughtException implements Thread.UncaughtExceptionHandler { + public void uncaughtException(final Thread t, final Throwable e) { + e.printStackTrace(); + + LoggerExceptionUtils.logException(e); + + if (m_logWriter != null) { + try { + m_logWriter.close(); + } catch (Exception e1) { + e1.printStackTrace(); + } + } + + System.exit(1); + } + } + + private static final int MAX_NUM_RECENT_ENTRIES = 100; + + private static EnumMap m_loggers = + new EnumMap(Category.class); + private static LogWriter m_logWriter = null; + private CircularBuffer m_recentEntries = + new CircularBuffer(MAX_NUM_RECENT_ENTRIES); + private static Object s_lastWriteTimeSync = new Object(); + private static long s_lastWriteTime = 0L; + + public static String logFilePathBase = null; + + static { + for (Category category : Category.values()) { + m_loggers.put(category, new Logger(category)); + } + try { + ConfigFileReader config_file = ConfigFileReader.getInstance(); + if (config_file != null) { + logFilePathBase = config_file.getString("logFilePath").get(); + new File(logFilePathBase).mkdirs(); + final String timestamp = + new SimpleDateFormat("yyyyMMdd'T'HHmmss").format(new Date()); + final String logFilePath = new File(logFilePathBase, timestamp).getAbsolutePath(); + m_logWriter = new LogWriter(logFilePath); + get(Category.CONFIGURATION).logRaw(Severity.INFO, "Logging to " + logFilePath); + } else { + get(Category.CONFIGURATION) + .logRaw( + Severity.ERROR, + "Config file is not available. Logs will only be in-memory and will be lost when the robot is turned off."); + } + } catch (Exception e) { + e.printStackTrace(); + LoggerExceptionUtils.logException(e); + } + + Thread.setDefaultUncaughtExceptionHandler(new LogUncaughtException()); + } + + public static Logger get(final Category category) { + return m_loggers.get(category); + } + + static long getTime() { + long nowNanosec = new Date().getTime() * 1000000; + synchronized (s_lastWriteTimeSync) { + // Ensure that log entries' timestamps are unique. This is important + // because the log viewer uses an entry's timestamp as a unique ID, + // and we don't want two different log entries to accidentally + // compare as equal. + nowNanosec = s_lastWriteTime = Math.max(nowNanosec, s_lastWriteTime); + } + return nowNanosec; + } + + private final Category m_category; + + private Logger(final Category category) { + m_category = category; + } + + public Collection recentEntries() { + return Collections.unmodifiableCollection(m_recentEntries); + } + + public void logData(final Severity severity, final String format, final Object... args) { + var entry = + LogEntry.newBuilder() + .setTime(getTime()) + .setSeverity(severity) + .setCategory(m_category); + entry.setMessageStr(String.format(format, args)); + m_recentEntries.add(entry.build()); + entry.setMessageStr(format); + for (Object arg : args) { + var logValue = LogValue.newBuilder(); + SerializationUtils.valueToProto(arg, logValue); + entry.addArg(logValue.build()); + } + if (m_logWriter != null) { + m_logWriter.logStoredFormat(entry); + } + } + + public void logRaw(final Severity severity, final String message) { + var entry = + LogEntry.newBuilder() + .setTime(getTime()) + .setSeverity(severity) + .setCategory(m_category) + .setMessageStr(message) + .build(); + m_recentEntries.add(entry); + if (m_logWriter != null) { + m_logWriter.log(entry); + } + } + + void logOnlyInMemory(final Severity severity, final String message) { + var entry = + LogEntry.newBuilder() + .setTime(getTime()) + .setSeverity(severity) + .setCategory(m_category) + .setMessageStr(message) + .build(); + m_recentEntries.add(entry); + } } diff --git a/src/main/java/com/team766/robot/constants/ExampleInputConstants.java b/src/main/java/com/team766/robot/constants/ExampleInputConstants.java index aa7cb28b..1ec1734c 100644 --- a/src/main/java/com/team766/robot/constants/ExampleInputConstants.java +++ b/src/main/java/com/team766/robot/constants/ExampleInputConstants.java @@ -2,39 +2,38 @@ /** * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc. - * + * */ public final class ExampleInputConstants { - // "joysticks" - public static final int LEFT_JOYSTICK = 0; - public static final int RIGHT_JOYSTICK = 1; - public static final int CONTROL_PANEL = 2; + // "joysticks" + public static final int LEFT_JOYSTICK = 0; + public static final int RIGHT_JOYSTICK = 1; + public static final int CONTROL_PANEL = 2; - // navigation - public static final int AXIS_LEFT_RIGHT = 0; - public static final int AXIS_FORWARD_BACKWARD = 1; + // navigation + public static final int AXIS_LEFT_RIGHT = 0; + public static final int AXIS_FORWARD_BACKWARD = 1; + public static final int AXIS_SHOOTER_DIAL = 3; - public static final int AXIS_SHOOTER_DIAL = 3; + public static final int CONTROL_PANEL_SHOOTER_SWITCH = 1; + public static final int CONTROL_PANEL_INTAKE_BUTTON = 2; - public static final int CONTROL_PANEL_SHOOTER_SWITCH = 1; - public static final int CONTROL_PANEL_INTAKE_BUTTON = 2; + public static final int CONTROL_PANEL_BELTS_BUTTON = 3; - public static final int CONTROL_PANEL_BELTS_BUTTON = 3; + public static final int CONTROL_PANEL_SPITBALL_BUTTON = 14; - public static final int CONTROL_PANEL_SPITBALL_BUTTON = 14; - - public static final int CONTROL_PANEL_AUTOCLIMB_BUTTON = 9; - public static final int CONTROL_PANEL_ARMS_SWITCH = 13; - public static final int CONTROL_PANEL_ELEVATOR_UP_BUTTON = 7; - public static final int CONTROL_PANEL_ELEVATOR_DOWN_BUTTON = 8; - public static final int CONTROL_PANEL_ELEVATOR_TOP_BUTTON = 5; - public static final int CONTROL_PANEL_ELEVATOR_BOTTOM_BUTTON = 6; - public static final int CONTROL_PANEL_AUTO_SHOOT = 10; - public static final int JOYSTICK_TRIGGER = 1; - public static final int E_STOP = 11; - public static final int JOYSTICK_ELEVATOR_RESET_BUTTON = 2; - public static final int JOYSTICK_CLIMB_RUNG_BUTTON = 4; - public static final int JOYSTICK_CLIMB_FIRST_RUNG_BUTTON = 5; + public static final int CONTROL_PANEL_AUTOCLIMB_BUTTON = 9; + public static final int CONTROL_PANEL_ARMS_SWITCH = 13; + public static final int CONTROL_PANEL_ELEVATOR_UP_BUTTON = 7; + public static final int CONTROL_PANEL_ELEVATOR_DOWN_BUTTON = 8; + public static final int CONTROL_PANEL_ELEVATOR_TOP_BUTTON = 5; + public static final int CONTROL_PANEL_ELEVATOR_BOTTOM_BUTTON = 6; + public static final int CONTROL_PANEL_AUTO_SHOOT = 10; + public static final int JOYSTICK_TRIGGER = 1; + public static final int E_STOP = 11; + public static final int JOYSTICK_ELEVATOR_RESET_BUTTON = 2; + public static final int JOYSTICK_CLIMB_RUNG_BUTTON = 4; + public static final int JOYSTICK_CLIMB_FIRST_RUNG_BUTTON = 5; } diff --git a/src/main/java/com/team766/simulator/ElectricalSystem.java b/src/main/java/com/team766/simulator/ElectricalSystem.java index 8c08ef8e..7c318d01 100644 --- a/src/main/java/com/team766/simulator/ElectricalSystem.java +++ b/src/main/java/com/team766/simulator/ElectricalSystem.java @@ -1,53 +1,55 @@ package com.team766.simulator; +import com.team766.simulator.interfaces.ElectricalDevice; import java.util.ArrayList; import java.util.LinkedHashMap; -import com.team766.simulator.interfaces.ElectricalDevice; public class ElectricalSystem { - private double nominalVoltage = Parameters.BATTERY_VOLTAGE; - private double primaryResistance = Parameters.PRIMARY_ELECTRICAL_RESISTANCE; - - public class BranchInfo { - public final ElectricalDevice device; - public ElectricalDevice.Output flow; - - public BranchInfo(ElectricalDevice device) { - this.device = device; - this.flow = new ElectricalDevice.Output(0); - } - } - - private ArrayList branchCircuits = new ArrayList(); - - private ElectricalDevice.Input systemState; - - public ElectricalSystem() { - systemState = new ElectricalDevice.Input(nominalVoltage); - } - - public void addDevice(ElectricalDevice device) { - branchCircuits.add(new BranchInfo(device)); - } - - public void step(double dt) { - double current = 0.0; - for (BranchInfo branch : branchCircuits) { - branch.flow = branch.device.step(systemState, dt); - current += branch.flow.current; - } - systemState = new ElectricalDevice.Input(Math.max(0, nominalVoltage - primaryResistance * current)); - } - - public double getSystemVoltage() { - return systemState.voltage; - } - - public LinkedHashMap getBranchCurrents() { - var currents = new LinkedHashMap(); - for (BranchInfo branch : branchCircuits) { - currents.put(branch.device.name(), branch.flow.current); - } - return currents; - } + private double nominalVoltage = Parameters.BATTERY_VOLTAGE; + private double primaryResistance = Parameters.PRIMARY_ELECTRICAL_RESISTANCE; + + public class BranchInfo { + public final ElectricalDevice device; + public ElectricalDevice.Output flow; + + public BranchInfo(ElectricalDevice device) { + this.device = device; + this.flow = new ElectricalDevice.Output(0); + } + } + + private ArrayList branchCircuits = new ArrayList(); + + private ElectricalDevice.Input systemState; + + public ElectricalSystem() { + systemState = new ElectricalDevice.Input(nominalVoltage); + } + + public void addDevice(ElectricalDevice device) { + branchCircuits.add(new BranchInfo(device)); + } + + public void step(double dt) { + double current = 0.0; + for (BranchInfo branch : branchCircuits) { + branch.flow = branch.device.step(systemState, dt); + current += branch.flow.current; + } + systemState = + new ElectricalDevice.Input( + Math.max(0, nominalVoltage - primaryResistance * current)); + } + + public double getSystemVoltage() { + return systemState.voltage; + } + + public LinkedHashMap getBranchCurrents() { + var currents = new LinkedHashMap(); + for (BranchInfo branch : branchCircuits) { + currents.put(branch.device.name(), branch.flow.current); + } + return currents; + } } diff --git a/src/main/java/com/team766/simulator/Metrics.java b/src/main/java/com/team766/simulator/Metrics.java index 82e82cd8..4a0874d2 100644 --- a/src/main/java/com/team766/simulator/Metrics.java +++ b/src/main/java/com/team766/simulator/Metrics.java @@ -8,100 +8,101 @@ * Stores a set of parallel time series (i.e. each series has data at the same points in time). */ public class Metrics { - /** - * This is a handle to a particular time series. - */ - public class Series { - private final int index; + /** + * This is a handle to a particular time series. + */ + public class Series { + private final int index; - private Series(int index) { - this.index = index; - } + private Series(int index) { + this.index = index; + } - private void check(Metrics m) { - if (m != Metrics.this) { - throw new IllegalArgumentException(); - } - } - } + private void check(Metrics m) { + if (m != Metrics.this) { + throw new IllegalArgumentException(); + } + } + } - /** - * This is a view on a point in time, with the data for all of the time series at that point. - */ - public class Point { - private final Double[] data; + /** + * This is a view on a point in time, with the data for all of the time series at that point. + */ + public class Point { + private final Double[] data; - private Point(Double[] data) { - this.data = data; - } + private Point(Double[] data) { + this.data = data; + } - /** - * Set the value in a particular series at this point in time. - */ - public Point set(Series series, double value) { - series.check(Metrics.this); - if (series.index >= 0) { - data[series.index] = value; - } - return this; - } - } + /** + * Set the value in a particular series at this point in time. + */ + public Point set(Series series, double value) { + series.check(Metrics.this); + if (series.index >= 0) { + data[series.index] = value; + } + return this; + } + } - // Each element of this list should be an array of the same length. - // In each element, index 0 is time and subsequent elements are values. - private ArrayList metrics = new ArrayList<>(); - // The labels for the series. - private ArrayList seriesLabels = new ArrayList<>(); + // Each element of this list should be an array of the same length. + // In each element, index 0 is time and subsequent elements are values. + private ArrayList metrics = new ArrayList<>(); + // The labels for the series. + private ArrayList seriesLabels = new ArrayList<>(); - /** - * Add a new data series. - * - * This can only be called before data is added. - * - * @param name The label for the series. - * @param enabled If false, data from this series will be ignored. This allows for an easy, - * centralized way of selecting which data should be processed/visualized. - * @return A handle to this series, which should be passed when adding data via a Point. - * @throws IllegalStateException if called after data has been added (via {@link #add()}) - */ - public Series addSeries(String name, boolean enabled) { - if (metrics.size() != 0) { - throw new IllegalStateException(); - } - if (!enabled) { - return new Series(-1); - } - seriesLabels.add(name); - return new Series(seriesLabels.size()); - } - public Series addSeries(String name) { - return addSeries(name, true); - } + /** + * Add a new data series. + * + * This can only be called before data is added. + * + * @param name The label for the series. + * @param enabled If false, data from this series will be ignored. This allows for an easy, + * centralized way of selecting which data should be processed/visualized. + * @return A handle to this series, which should be passed when adding data via a Point. + * @throws IllegalStateException if called after data has been added (via {@link #add()}) + */ + public Series addSeries(String name, boolean enabled) { + if (metrics.size() != 0) { + throw new IllegalStateException(); + } + if (!enabled) { + return new Series(-1); + } + seriesLabels.add(name); + return new Series(seriesLabels.size()); + } - /** - * Add a new data point. - * - * Data points should be added in order of increasing time. - */ - public Point add(double time) { - var data = new Double[seriesLabels.size() + 1]; - data[0] = time; - metrics.add(data); - return new Point(data); - } + public Series addSeries(String name) { + return addSeries(name, true); + } - /** - * Remove all data points (Series are not removed) - */ - public void clear() { - metrics.clear(); - } + /** + * Add a new data point. + * + * Data points should be added in order of increasing time. + */ + public Point add(double time) { + var data = new Double[seriesLabels.size() + 1]; + data[0] = time; + metrics.add(data); + return new Point(data); + } - public List getMetrics() { - return Collections.unmodifiableList(metrics); - } + /** + * Remove all data points (Series are not removed) + */ + public void clear() { + metrics.clear(); + } - public List getLabels() { - return Collections.unmodifiableList(seriesLabels); - } + public List getMetrics() { + return Collections.unmodifiableList(metrics); + } + + public List getLabels() { + return Collections.unmodifiableList(seriesLabels); + } } diff --git a/src/main/java/com/team766/simulator/Parameters.java b/src/main/java/com/team766/simulator/Parameters.java index 8c8a84d2..6d3814cd 100644 --- a/src/main/java/com/team766/simulator/Parameters.java +++ b/src/main/java/com/team766/simulator/Parameters.java @@ -1,30 +1,33 @@ package com.team766.simulator; public class Parameters { - public static final double TIME_STEP = 0.0001; // seconds - public static final double DURATION = 10.0; // seconds + public static final double TIME_STEP = 0.0001; // seconds + public static final double DURATION = 10.0; // seconds - public static final double LOGGING_PERIOD = 0.005; // seconds + public static final double LOGGING_PERIOD = 0.005; // seconds - // Robot mode to run in the simulator - public static final ProgramInterface.RobotMode INITIAL_ROBOT_MODE = ProgramInterface.RobotMode.AUTON; + // Robot mode to run in the simulator + public static final ProgramInterface.RobotMode INITIAL_ROBOT_MODE = + ProgramInterface.RobotMode.AUTON; - public static final double BATTERY_VOLTAGE = 12.6; // volts - public static final double PRIMARY_ELECTRICAL_RESISTANCE = 0.018 + 0.01; // ohms + public static final double BATTERY_VOLTAGE = 12.6; // volts + public static final double PRIMARY_ELECTRICAL_RESISTANCE = 0.018 + 0.01; // ohms - public static final double STARTING_PRESSURE = 0; // pascals (relative); 120 psi = 827370.875 pascals + public static final double STARTING_PRESSURE = + 0; // pascals (relative); 120 psi = 827370.875 pascals - public static final double DRIVE_WHEEL_DIAMETER = 0.1524; // 6 inches in meters - public static final double DRIVE_GEAR_RATIO = 8.0; - public static final int NUM_LOADED_WHEELS = 2; - public static final int ENCODER_TICKS_PER_REVOLUTION = 256; + public static final double DRIVE_WHEEL_DIAMETER = 0.1524; // 6 inches in meters + public static final double DRIVE_GEAR_RATIO = 8.0; + public static final int NUM_LOADED_WHEELS = 2; + public static final int ENCODER_TICKS_PER_REVOLUTION = 256; - public static final double ROBOT_MASS = 66; // approx. 145 lbs in kg - public static final double ROBOT_LENGTH = 1.0; // meters - public static final double ROBOT_WIDTH = 0.8; // meters - public static final double ROBOT_MOMENT_OF_INERTIA = 1.0 / 12.0 * ROBOT_MASS * (ROBOT_WIDTH * ROBOT_WIDTH + ROBOT_LENGTH * ROBOT_LENGTH); + public static final double ROBOT_MASS = 66; // approx. 145 lbs in kg + public static final double ROBOT_LENGTH = 1.0; // meters + public static final double ROBOT_WIDTH = 0.8; // meters + public static final double ROBOT_MOMENT_OF_INERTIA = + 1.0 / 12.0 * ROBOT_MASS * (ROBOT_WIDTH * ROBOT_WIDTH + ROBOT_LENGTH * ROBOT_LENGTH); - public static final double WHEEL_COEFFICIENT_OF_FRICTION = 1.1; - public static final double ROLLING_RESISTANCE = 0.09; - public static final double TURNING_RESISTANCE_FACTOR = 0.15; + public static final double WHEEL_COEFFICIENT_OF_FRICTION = 1.1; + public static final double ROLLING_RESISTANCE = 0.09; + public static final double TURNING_RESISTANCE_FACTOR = 0.15; } diff --git a/src/main/java/com/team766/simulator/PneumaticsSystem.java b/src/main/java/com/team766/simulator/PneumaticsSystem.java index 54f63029..092a7b0c 100644 --- a/src/main/java/com/team766/simulator/PneumaticsSystem.java +++ b/src/main/java/com/team766/simulator/PneumaticsSystem.java @@ -1,61 +1,67 @@ package com.team766.simulator; -import java.util.ArrayList; - import com.team766.simulator.interfaces.PneumaticDevice; +import java.util.ArrayList; public class PneumaticsSystem { - public static final double PSI_TO_PASCALS = 6894.75729; - - private static class BranchCircuit { - public PneumaticDevice device; - public double regulatedPressure; - } - - private ArrayList branchCircuits = new ArrayList(); - - private double systemPressure = Parameters.STARTING_PRESSURE; - private double compressedAirVolume = 0.0; - private boolean initialized = false; - - public void addDevice(final PneumaticDevice device, final double regulatedPressure) { - BranchCircuit circuit = new BranchCircuit(); - circuit.device = device; - circuit.regulatedPressure = regulatedPressure; - branchCircuits.add(circuit); - } - - public void step(double dt) { - double flowVolume = 0.0; - double systemVolume = 0.0; - for (BranchCircuit circuit : branchCircuits) { - double devicePressure = Math.min(circuit.regulatedPressure, systemPressure); - PneumaticDevice.Input inputState = new PneumaticDevice.Input(devicePressure); - PneumaticDevice.Output deviceState = circuit.device.step(inputState, dt); - // TODO: implement relieving pressure regulator (make sure device pressure doesn't exceed - // circuit.regulatedPressure, even when including flow volume) - flowVolume += deviceState.flowVolume; - systemVolume += deviceState.deviceVolume; - } - compressedAirVolume += flowVolume; - if (systemVolume == 0.) { - throw new RuntimeException("Your pneumatics system has no storage volume"); - } - if (!initialized) { - compressedAirVolume = systemVolume * (systemPressure + PhysicalConstants.ATMOSPHERIC_PRESSURE) / PhysicalConstants.ATMOSPHERIC_PRESSURE; - initialized = true; - } - systemPressure = compressedAirVolume / systemVolume * PhysicalConstants.ATMOSPHERIC_PRESSURE - PhysicalConstants.ATMOSPHERIC_PRESSURE; - } - - public double getSystemPressure() { - return systemPressure; - } - - // Simulate the system venting all of its compressed air (e.g. someone opened the release valve; - // to simulate the pneumatics system becoming compromised, call this method on every simulation tick) - public void ventPressure() { - systemPressure = 0; - initialized = false; - } + public static final double PSI_TO_PASCALS = 6894.75729; + + private static class BranchCircuit { + public PneumaticDevice device; + public double regulatedPressure; + } + + private ArrayList branchCircuits = new ArrayList(); + + private double systemPressure = Parameters.STARTING_PRESSURE; + private double compressedAirVolume = 0.0; + private boolean initialized = false; + + public void addDevice(final PneumaticDevice device, final double regulatedPressure) { + BranchCircuit circuit = new BranchCircuit(); + circuit.device = device; + circuit.regulatedPressure = regulatedPressure; + branchCircuits.add(circuit); + } + + public void step(double dt) { + double flowVolume = 0.0; + double systemVolume = 0.0; + for (BranchCircuit circuit : branchCircuits) { + double devicePressure = Math.min(circuit.regulatedPressure, systemPressure); + PneumaticDevice.Input inputState = new PneumaticDevice.Input(devicePressure); + PneumaticDevice.Output deviceState = circuit.device.step(inputState, dt); + // TODO: implement relieving pressure regulator (make sure device pressure doesn't + // exceed + // circuit.regulatedPressure, even when including flow volume) + flowVolume += deviceState.flowVolume; + systemVolume += deviceState.deviceVolume; + } + compressedAirVolume += flowVolume; + if (systemVolume == 0.) { + throw new RuntimeException("Your pneumatics system has no storage volume"); + } + if (!initialized) { + compressedAirVolume = + systemVolume + * (systemPressure + PhysicalConstants.ATMOSPHERIC_PRESSURE) + / PhysicalConstants.ATMOSPHERIC_PRESSURE; + initialized = true; + } + systemPressure = + compressedAirVolume / systemVolume * PhysicalConstants.ATMOSPHERIC_PRESSURE + - PhysicalConstants.ATMOSPHERIC_PRESSURE; + } + + public double getSystemPressure() { + return systemPressure; + } + + // Simulate the system venting all of its compressed air (e.g. someone opened the release valve; + // to simulate the pneumatics system becoming compromised, call this method on every simulation + // tick) + public void ventPressure() { + systemPressure = 0; + initialized = false; + } } diff --git a/src/main/java/com/team766/simulator/Program.java b/src/main/java/com/team766/simulator/Program.java index 4c1e779a..c6753fde 100644 --- a/src/main/java/com/team766/simulator/Program.java +++ b/src/main/java/com/team766/simulator/Program.java @@ -1,7 +1,7 @@ package com.team766.simulator; public interface Program { - public void step(double dt); + public void step(double dt); - void reset(); + void reset(); } diff --git a/src/main/java/com/team766/simulator/Simulator.java b/src/main/java/com/team766/simulator/Simulator.java index 0d5e313c..05ab17bd 100644 --- a/src/main/java/com/team766/simulator/Simulator.java +++ b/src/main/java/com/team766/simulator/Simulator.java @@ -1,127 +1,143 @@ package com.team766.simulator; +import com.team766.simulator.elements.*; +import com.team766.simulator.mechanisms.*; +import com.team766.simulator.ui.*; import java.util.ArrayList; import java.util.HashMap; import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; -import com.team766.simulator.elements.*; -import com.team766.simulator.mechanisms.*; -import com.team766.simulator.ui.*; - public class Simulator implements Runnable { - private ElectricalSystem electricalSystem = new ElectricalSystem(); - private PneumaticsSystem pneumaticsSystem = new PneumaticsSystem(); - private WestCoastDrive drive = new WestCoastDrive(electricalSystem); - private AirCompressor compressor = new AirCompressor(); - private DoubleJointedArm arm = new DoubleJointedArm(electricalSystem); - - private double time; - private double nextLogTime; - - private final Metrics metrics = new Metrics(); - private final Metrics.Series xPositionSeries = metrics.addSeries("X Position (m)", false); - private final Metrics.Series yPositionSeries = metrics.addSeries("Y Position (m)", false); - private final Metrics.Series rotationSeries = metrics.addSeries("Rotation (deg)", false); - private final Metrics.Series velocitySeries = metrics.addSeries("Velocity (m/s)", false); - private final Metrics.Series accelerationSeries = metrics.addSeries("Acceleration (m/s^2)", false); - private final Metrics.Series armAngle1Series = metrics.addSeries("Arm angle 1"); - private final Metrics.Series armAngle2Series = metrics.addSeries("Arm angle 2"); - private final Metrics.Series armVelocity1Series = metrics.addSeries("Arm velocity 1"); - private final Metrics.Series armVelocity2Series = metrics.addSeries("Arm velocity 2"); - private final Metrics.Series systemVoltageSeries = metrics.addSeries("System Voltage (V)", false); - private final Metrics.Series systemPressureSeries = metrics.addSeries("System Pressure (PSI)", false); - private final HashMap branchCurrentSeries = new HashMap<>(); - - private ArrayList driveTrajectory = new ArrayList(); - private ArrayList armTrajectory = new ArrayList(); - - public Simulator() { - // NOTE(rcahoon, 2023-02-09): Disabled the compressor so it won't affect the battery voltage - // available to the arm's motors. This should be re-enabled if we want to simulate - // pneumatics again. - //electricalSystem.addDevice(compressor); - pneumaticsSystem.addDevice(compressor, 120 * PneumaticsSystem.PSI_TO_PASCALS); - pneumaticsSystem.addDevice(new AirReservoir(0.000574), 120 * PneumaticsSystem.PSI_TO_PASCALS); // 574 mL - pneumaticsSystem.addDevice(new AirReservoir(0.000574), 120 * PneumaticsSystem.PSI_TO_PASCALS); // 574 mL - - for (String branchName : electricalSystem.getBranchCurrents().keySet()) { - branchCurrentSeries.put(branchName, metrics.addSeries(branchName, false)); - } - } - - public void step() { - double dt = Parameters.TIME_STEP; - time += dt; - ProgramInterface.simulationTime = time; - - electricalSystem.step(dt); - pneumaticsSystem.step(dt); - drive.step(dt); - arm.step(dt); - - if (ProgramInterface.program != null) { - ProgramInterface.program.step(dt); - } - - if (nextLogTime <= time) { - nextLogTime += Parameters.LOGGING_PERIOD; - - var metricsPoint = metrics.add(time); - metricsPoint.set(xPositionSeries, drive.getPosition().getX()); - metricsPoint.set(yPositionSeries, drive.getPosition().getY()); - metricsPoint.set(rotationSeries, Math.toDegrees(drive.getRotation().getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[2])); - metricsPoint.set(velocitySeries, drive.getLinearVelocity().getX()); - metricsPoint.set(accelerationSeries, drive.getLinearAcceleration().getX()); - metricsPoint.set(armAngle1Series, arm.getJ1Angle()); - metricsPoint.set(armAngle2Series, arm.getJ2Angle()); - metricsPoint.set(armVelocity1Series, arm.getJ1Velocity()); - metricsPoint.set(armVelocity2Series, arm.getJ2Velocity()); - metricsPoint.set(systemVoltageSeries, electricalSystem.getSystemVoltage()); - metricsPoint.set(systemPressureSeries, pneumaticsSystem.getSystemPressure() / PneumaticsSystem.PSI_TO_PASCALS); - for (var branch : electricalSystem.getBranchCurrents().entrySet()) { - metricsPoint.set(branchCurrentSeries.get(branch.getKey()), branch.getValue()); - } - - driveTrajectory.add(new Double[] { - time, - drive.getPosition().getX(), - drive.getPosition().getY(), - drive.getRotation().getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[2], - drive.getLinearVelocity().getX(), - drive.getLinearVelocity().getY(), - }); - - armTrajectory.add(new Double[] { - time, - arm.getJ1Position().get(0, 0), - arm.getJ1Position().get(1, 0), - arm.getJ2Position().get(0, 0), - arm.getJ2Position().get(1, 0) - }); - } - } - - public void run() { - metrics.clear(); - time = 0.0; - nextLogTime = 0.0; - while (time <= Parameters.DURATION) { - step(); - if (Math.abs(time - 3.0) < Parameters.TIME_STEP) { - pneumaticsSystem.ventPressure(); - } - } - - var playbackTimer = new PlaybackTimer(time); - - // var trajectoryPanel = new Trajectory(driveTrajectory, playbackTimer); - // new UIFrame("Trajectory", trajectoryPanel); - - var armTrajectoryPanel = new ArmTrajectory(armTrajectory, playbackTimer); - new UIFrame("ArmTrajectory", armTrajectoryPanel); - - var metricsPanel = new MetricsPlot(metrics, playbackTimer); - new UIFrame("Metrics", metricsPanel); - } + private ElectricalSystem electricalSystem = new ElectricalSystem(); + private PneumaticsSystem pneumaticsSystem = new PneumaticsSystem(); + private WestCoastDrive drive = new WestCoastDrive(electricalSystem); + private AirCompressor compressor = new AirCompressor(); + private DoubleJointedArm arm = new DoubleJointedArm(electricalSystem); + + private double time; + private double nextLogTime; + + private final Metrics metrics = new Metrics(); + private final Metrics.Series xPositionSeries = metrics.addSeries("X Position (m)", false); + private final Metrics.Series yPositionSeries = metrics.addSeries("Y Position (m)", false); + private final Metrics.Series rotationSeries = metrics.addSeries("Rotation (deg)", false); + private final Metrics.Series velocitySeries = metrics.addSeries("Velocity (m/s)", false); + private final Metrics.Series accelerationSeries = + metrics.addSeries("Acceleration (m/s^2)", false); + private final Metrics.Series armAngle1Series = metrics.addSeries("Arm angle 1"); + private final Metrics.Series armAngle2Series = metrics.addSeries("Arm angle 2"); + private final Metrics.Series armVelocity1Series = metrics.addSeries("Arm velocity 1"); + private final Metrics.Series armVelocity2Series = metrics.addSeries("Arm velocity 2"); + private final Metrics.Series systemVoltageSeries = + metrics.addSeries("System Voltage (V)", false); + private final Metrics.Series systemPressureSeries = + metrics.addSeries("System Pressure (PSI)", false); + private final HashMap branchCurrentSeries = new HashMap<>(); + + private ArrayList driveTrajectory = new ArrayList(); + private ArrayList armTrajectory = new ArrayList(); + + public Simulator() { + // NOTE(rcahoon, 2023-02-09): Disabled the compressor so it won't affect the battery voltage + // available to the arm's motors. This should be re-enabled if we want to simulate + // pneumatics again. + // electricalSystem.addDevice(compressor); + pneumaticsSystem.addDevice(compressor, 120 * PneumaticsSystem.PSI_TO_PASCALS); + pneumaticsSystem.addDevice( + new AirReservoir(0.000574), 120 * PneumaticsSystem.PSI_TO_PASCALS); // 574 mL + pneumaticsSystem.addDevice( + new AirReservoir(0.000574), 120 * PneumaticsSystem.PSI_TO_PASCALS); // 574 mL + + for (String branchName : electricalSystem.getBranchCurrents().keySet()) { + branchCurrentSeries.put(branchName, metrics.addSeries(branchName, false)); + } + } + + public void step() { + double dt = Parameters.TIME_STEP; + time += dt; + ProgramInterface.simulationTime = time; + + electricalSystem.step(dt); + pneumaticsSystem.step(dt); + drive.step(dt); + arm.step(dt); + + if (ProgramInterface.program != null) { + ProgramInterface.program.step(dt); + } + + if (nextLogTime <= time) { + nextLogTime += Parameters.LOGGING_PERIOD; + + var metricsPoint = metrics.add(time); + metricsPoint.set(xPositionSeries, drive.getPosition().getX()); + metricsPoint.set(yPositionSeries, drive.getPosition().getY()); + metricsPoint.set( + rotationSeries, + Math.toDegrees( + drive.getRotation() + .getAngles( + RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[ + 2])); + metricsPoint.set(velocitySeries, drive.getLinearVelocity().getX()); + metricsPoint.set(accelerationSeries, drive.getLinearAcceleration().getX()); + metricsPoint.set(armAngle1Series, arm.getJ1Angle()); + metricsPoint.set(armAngle2Series, arm.getJ2Angle()); + metricsPoint.set(armVelocity1Series, arm.getJ1Velocity()); + metricsPoint.set(armVelocity2Series, arm.getJ2Velocity()); + metricsPoint.set(systemVoltageSeries, electricalSystem.getSystemVoltage()); + metricsPoint.set( + systemPressureSeries, + pneumaticsSystem.getSystemPressure() / PneumaticsSystem.PSI_TO_PASCALS); + for (var branch : electricalSystem.getBranchCurrents().entrySet()) { + metricsPoint.set(branchCurrentSeries.get(branch.getKey()), branch.getValue()); + } + + driveTrajectory.add( + new Double[] { + time, + drive.getPosition().getX(), + drive.getPosition().getY(), + drive.getRotation() + .getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[ + 2], + drive.getLinearVelocity().getX(), + drive.getLinearVelocity().getY(), + }); + + armTrajectory.add( + new Double[] { + time, + arm.getJ1Position().get(0, 0), + arm.getJ1Position().get(1, 0), + arm.getJ2Position().get(0, 0), + arm.getJ2Position().get(1, 0) + }); + } + } + + public void run() { + metrics.clear(); + time = 0.0; + nextLogTime = 0.0; + while (time <= Parameters.DURATION) { + step(); + if (Math.abs(time - 3.0) < Parameters.TIME_STEP) { + pneumaticsSystem.ventPressure(); + } + } + + var playbackTimer = new PlaybackTimer(time); + + // var trajectoryPanel = new Trajectory(driveTrajectory, playbackTimer); + // new UIFrame("Trajectory", trajectoryPanel); + + var armTrajectoryPanel = new ArmTrajectory(armTrajectory, playbackTimer); + new UIFrame("ArmTrajectory", armTrajectoryPanel); + + var metricsPanel = new MetricsPlot(metrics, playbackTimer); + new UIFrame("Metrics", metricsPanel); + } } diff --git a/src/main/java/com/team766/simulator/elements/AirCompressor.java b/src/main/java/com/team766/simulator/elements/AirCompressor.java index 275c1519..a3367021 100644 --- a/src/main/java/com/team766/simulator/elements/AirCompressor.java +++ b/src/main/java/com/team766/simulator/elements/AirCompressor.java @@ -1,60 +1,68 @@ package com.team766.simulator.elements; -import java.util.Arrays; - -import org.apache.commons.math3.analysis.interpolation.LinearInterpolator; -import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; - import com.team766.simulator.PneumaticsSystem; import com.team766.simulator.interfaces.ElectricalDevice; import com.team766.simulator.interfaces.PneumaticDevice; +import java.util.Arrays; +import org.apache.commons.math3.analysis.interpolation.LinearInterpolator; +import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; public class AirCompressor implements ElectricalDevice, PneumaticDevice { - private static final double CFM_TO_M3_PER_SEC = 0.000471947443; - - // Values for http://www.andymark.com/product-p/am-2005.htm - private static final double NOMINAL_VOLTAGE = 12; - private static final double[] PRESSURES_PSI = {0., 10., 20., 30., 40., 50., 60., 70., 80., 90., 100., 110.}; - private static final double[] FLOW_RATES_CFM = {0.88, 0.50, 0.43, 0.36, 0.30, 0.27, 0.25, 0.24, 0.24, 0.23, 0.22, 0.22}; - private static final double[] CURRENTS = {7., 8., 8., 9., 9., 9., 10., 10., 10., 11., 11., 10.}; - private static final double[] PRESSURES = Arrays.stream(PRESSURES_PSI).map(psi -> psi * PneumaticsSystem.PSI_TO_PASCALS).toArray(); - private static final double[] FLOW_RATES = Arrays.stream(FLOW_RATES_CFM).map(cfm -> cfm * CFM_TO_M3_PER_SEC).toArray(); - - PolynomialSplineFunction currentFunction = new LinearInterpolator().interpolate(PRESSURES, CURRENTS); - PolynomialSplineFunction flowRateFunction = new LinearInterpolator().interpolate(PRESSURES, FLOW_RATES); - - private boolean isOn = true; - - private ElectricalDevice.Input electricalState = new ElectricalDevice.Input(0); - private PneumaticDevice.Input pneumaticState = new PneumaticDevice.Input(0); - - public void setIsOn(final boolean on) { - isOn = on; - } - - @Override - public ElectricalDevice.Output step(ElectricalDevice.Input input, double dt) { - electricalState = input; - if (isOn) { - double nominalCurrent = currentFunction.value(pneumaticState.pressure); - double adjustedCurrent = nominalCurrent * electricalState.voltage / NOMINAL_VOLTAGE; - return new ElectricalDevice.Output(adjustedCurrent); - } else { - return new ElectricalDevice.Output(0); - } - } - - @Override - public PneumaticDevice.Output step(PneumaticDevice.Input input, double dt) { - pneumaticState = input; - double nominalFlowRate = flowRateFunction.value(pneumaticState.pressure); - double adjustedFlowRate = nominalFlowRate * electricalState.voltage / NOMINAL_VOLTAGE; - double flowVolume = adjustedFlowRate * dt; - return new PneumaticDevice.Output(flowVolume, 0); - } - - @Override - public String name() { - return "AirCompressor"; - } + private static final double CFM_TO_M3_PER_SEC = 0.000471947443; + + // Values for http://www.andymark.com/product-p/am-2005.htm + private static final double NOMINAL_VOLTAGE = 12; + private static final double[] PRESSURES_PSI = { + 0., 10., 20., 30., 40., 50., 60., 70., 80., 90., 100., 110. + }; + private static final double[] FLOW_RATES_CFM = { + 0.88, 0.50, 0.43, 0.36, 0.30, 0.27, 0.25, 0.24, 0.24, 0.23, 0.22, 0.22 + }; + private static final double[] CURRENTS = {7., 8., 8., 9., 9., 9., 10., 10., 10., 11., 11., 10.}; + private static final double[] PRESSURES = + Arrays.stream(PRESSURES_PSI) + .map(psi -> psi * PneumaticsSystem.PSI_TO_PASCALS) + .toArray(); + private static final double[] FLOW_RATES = + Arrays.stream(FLOW_RATES_CFM).map(cfm -> cfm * CFM_TO_M3_PER_SEC).toArray(); + + PolynomialSplineFunction currentFunction = + new LinearInterpolator().interpolate(PRESSURES, CURRENTS); + PolynomialSplineFunction flowRateFunction = + new LinearInterpolator().interpolate(PRESSURES, FLOW_RATES); + + private boolean isOn = true; + + private ElectricalDevice.Input electricalState = new ElectricalDevice.Input(0); + private PneumaticDevice.Input pneumaticState = new PneumaticDevice.Input(0); + + public void setIsOn(final boolean on) { + isOn = on; + } + + @Override + public ElectricalDevice.Output step(ElectricalDevice.Input input, double dt) { + electricalState = input; + if (isOn) { + double nominalCurrent = currentFunction.value(pneumaticState.pressure); + double adjustedCurrent = nominalCurrent * electricalState.voltage / NOMINAL_VOLTAGE; + return new ElectricalDevice.Output(adjustedCurrent); + } else { + return new ElectricalDevice.Output(0); + } + } + + @Override + public PneumaticDevice.Output step(PneumaticDevice.Input input, double dt) { + pneumaticState = input; + double nominalFlowRate = flowRateFunction.value(pneumaticState.pressure); + double adjustedFlowRate = nominalFlowRate * electricalState.voltage / NOMINAL_VOLTAGE; + double flowVolume = adjustedFlowRate * dt; + return new PneumaticDevice.Output(flowVolume, 0); + } + + @Override + public String name() { + return "AirCompressor"; + } } diff --git a/src/main/java/com/team766/simulator/elements/AirReservoir.java b/src/main/java/com/team766/simulator/elements/AirReservoir.java index a96ef54d..5d60b2c9 100644 --- a/src/main/java/com/team766/simulator/elements/AirReservoir.java +++ b/src/main/java/com/team766/simulator/elements/AirReservoir.java @@ -4,14 +4,14 @@ public class AirReservoir implements PneumaticDevice { - private double volume; + private double volume; - public AirReservoir(final double volume_) { - this.volume = volume_; - } + public AirReservoir(final double volume_) { + this.volume = volume_; + } - @Override - public PneumaticDevice.Output step(PneumaticDevice.Input input, double dt) { - return new PneumaticDevice.Output(0, volume); - } + @Override + public PneumaticDevice.Output step(PneumaticDevice.Input input, double dt) { + return new PneumaticDevice.Output(0, volume); + } } diff --git a/src/main/java/com/team766/simulator/elements/CanMotorController.java b/src/main/java/com/team766/simulator/elements/CanMotorController.java index 00d8a99c..d424df78 100644 --- a/src/main/java/com/team766/simulator/elements/CanMotorController.java +++ b/src/main/java/com/team766/simulator/elements/CanMotorController.java @@ -5,24 +5,23 @@ public class CanMotorController extends MotorController { - private int address; + private int address; - public CanMotorController(final int address_, final ElectricalDevice downstream) { - super(downstream); - this.address = address_; - } + public CanMotorController(final int address_, final ElectricalDevice downstream) { + super(downstream); + this.address = address_; + } - @Override - protected double getCommand() { - return ProgramInterface.canMotorControllerChannels[address].command.output; - } + @Override + protected double getCommand() { + return ProgramInterface.canMotorControllerChannels[address].command.output; + } - public void setSensorPosition(double position) { - ProgramInterface.canMotorControllerChannels[address].status.sensorPosition = position; - } - - public void setSensorVelocity(double velocity) { - ProgramInterface.canMotorControllerChannels[address].status.sensorVelocity = velocity; - } + public void setSensorPosition(double position) { + ProgramInterface.canMotorControllerChannels[address].status.sensorPosition = position; + } + public void setSensorVelocity(double velocity) { + ProgramInterface.canMotorControllerChannels[address].status.sensorVelocity = velocity; + } } diff --git a/src/main/java/com/team766/simulator/elements/DCMotor.java b/src/main/java/com/team766/simulator/elements/DCMotor.java index 98512ba3..08830762 100644 --- a/src/main/java/com/team766/simulator/elements/DCMotor.java +++ b/src/main/java/com/team766/simulator/elements/DCMotor.java @@ -4,65 +4,82 @@ import com.team766.simulator.interfaces.MechanicalAngularDevice; public class DCMotor implements ElectricalDevice, MechanicalAngularDevice { - // TODO: Add rotor inertia - // TODO: Add thermal effects - - // Motor data from https://motors.vex.com/ - public static DCMotor makeCIM(String name) { - return new DCMotor(name, 12, 5330, 2.7, 2.41, 131); - } - public static DCMotor make775Pro(String name) { - return new DCMotor(name, 12, 18730, 0.7, 0.71, 134); - } - public static DCMotor makeFalcon500(String name) { - return new DCMotor(name, 12, 6380, 1.5, 4.69, 257); - } - public static DCMotor makeNeo(String name) { - return new DCMotor(name, 12, 5880, 1.3, 3.36, 166); - } - public static DCMotor makeNeo550(String name) { - return new DCMotor(name, 12, 11710, 1.1, 1.08, 111); - } - - // Motor velocity constant in radian/second/volt - // (motor velocity) = kV * (motor voltage) - private final double kV; - - // Motor torque constant in newton-meter/ampere - // (motor torque) = kT * (motor current) - private final double kT; - - // Motor resistance is calculated as 12V / (stall current at 12V) - private final double motorResistance; - - private ElectricalDevice.Output electricalState = new ElectricalDevice.Output(0); - private MechanicalAngularDevice.Input mechanicalState = new MechanicalAngularDevice.Input(0); - - private final String m_name; - - public DCMotor(String name, double referenceVoltage, double freeSpeedRpm, double freeCurrentAmps, double stallTorqueNewtonMeters, double stallCurrentAmps) { - m_name = name; - - this.motorResistance = referenceVoltage / stallCurrentAmps; - this.kV = freeSpeedRpm / 60.0 * 2 * Math.PI / (referenceVoltage - motorResistance * freeCurrentAmps); - this.kT = stallTorqueNewtonMeters / stallCurrentAmps; - } - - @Override - public MechanicalAngularDevice.Output step(MechanicalAngularDevice.Input input, double dt) { - mechanicalState = input; - - return new MechanicalAngularDevice.Output(kT * electricalState.current); - } - - @Override - public ElectricalDevice.Output step(ElectricalDevice.Input input, double dt) { - electricalState = new ElectricalDevice.Output((input.voltage - mechanicalState.angularVelocity / kV) / motorResistance); - return electricalState; - } - - @Override - public String name() { - return m_name; - } + // TODO: Add rotor inertia + // TODO: Add thermal effects + + // Motor data from https://motors.vex.com/ + public static DCMotor makeCIM(String name) { + return new DCMotor(name, 12, 5330, 2.7, 2.41, 131); + } + + public static DCMotor make775Pro(String name) { + return new DCMotor(name, 12, 18730, 0.7, 0.71, 134); + } + + public static DCMotor makeFalcon500(String name) { + return new DCMotor(name, 12, 6380, 1.5, 4.69, 257); + } + + public static DCMotor makeNeo(String name) { + return new DCMotor(name, 12, 5880, 1.3, 3.36, 166); + } + + public static DCMotor makeNeo550(String name) { + return new DCMotor(name, 12, 11710, 1.1, 1.08, 111); + } + + // Motor velocity constant in radian/second/volt + // (motor velocity) = kV * (motor voltage) + private final double kV; + + // Motor torque constant in newton-meter/ampere + // (motor torque) = kT * (motor current) + private final double kT; + + // Motor resistance is calculated as 12V / (stall current at 12V) + private final double motorResistance; + + private ElectricalDevice.Output electricalState = new ElectricalDevice.Output(0); + private MechanicalAngularDevice.Input mechanicalState = new MechanicalAngularDevice.Input(0); + + private final String m_name; + + public DCMotor( + String name, + double referenceVoltage, + double freeSpeedRpm, + double freeCurrentAmps, + double stallTorqueNewtonMeters, + double stallCurrentAmps) { + m_name = name; + + this.motorResistance = referenceVoltage / stallCurrentAmps; + this.kV = + freeSpeedRpm + / 60.0 + * 2 + * Math.PI + / (referenceVoltage - motorResistance * freeCurrentAmps); + this.kT = stallTorqueNewtonMeters / stallCurrentAmps; + } + + @Override + public MechanicalAngularDevice.Output step(MechanicalAngularDevice.Input input, double dt) { + mechanicalState = input; + + return new MechanicalAngularDevice.Output(kT * electricalState.current); + } + + @Override + public ElectricalDevice.Output step(ElectricalDevice.Input input, double dt) { + electricalState = + new ElectricalDevice.Output( + (input.voltage - mechanicalState.angularVelocity / kV) / motorResistance); + return electricalState; + } + + @Override + public String name() { + return m_name; + } } diff --git a/src/main/java/com/team766/simulator/elements/DoubleActingPneumaticCylinder.java b/src/main/java/com/team766/simulator/elements/DoubleActingPneumaticCylinder.java index 71fb7d4e..b85d5e39 100644 --- a/src/main/java/com/team766/simulator/elements/DoubleActingPneumaticCylinder.java +++ b/src/main/java/com/team766/simulator/elements/DoubleActingPneumaticCylinder.java @@ -1,55 +1,57 @@ package com.team766.simulator.elements; -import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; - import com.team766.simulator.PhysicalConstants; import com.team766.simulator.interfaces.MechanicalDevice; import com.team766.simulator.interfaces.PneumaticDevice; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; public class DoubleActingPneumaticCylinder implements PneumaticDevice, MechanicalDevice { - private static final Vector3D FORWARD = new Vector3D(1, 0, 0); - - private final double boreDiameter; - private final double stroke; - - private boolean isExtended = false; - private boolean commandExtended = false; - - private PneumaticDevice.Input pneumaticState = new PneumaticDevice.Input(0); - - public DoubleActingPneumaticCylinder(final double boreDiameter, final double stroke) { - this.boreDiameter = boreDiameter; - this.stroke = stroke; - } - - public void setExtended(final boolean state) { - commandExtended = state; - } - - @Override - public PneumaticDevice.Output step(PneumaticDevice.Input input, double dt) { - pneumaticState = input; - PneumaticDevice.Output output; - double deviceVolume = boreArea() * stroke; - if (commandExtended != isExtended) { - output = new PneumaticDevice.Output( - -deviceVolume * (input.pressure + PhysicalConstants.ATMOSPHERIC_PRESSURE) - / PhysicalConstants.ATMOSPHERIC_PRESSURE, - deviceVolume); - } else { - output = new PneumaticDevice.Output(0, deviceVolume); - } - isExtended = commandExtended; - return output; - } - - @Override - public MechanicalDevice.Output step(MechanicalDevice.Input input, double dt) { - Vector3D direction = isExtended ? FORWARD : FORWARD.negate(); - return new MechanicalDevice.Output(direction.scalarMultiply(boreArea() * pneumaticState.pressure)); - } - - private double boreArea() { - return Math.PI * Math.pow(boreDiameter / 2., 2); - } + private static final Vector3D FORWARD = new Vector3D(1, 0, 0); + + private final double boreDiameter; + private final double stroke; + + private boolean isExtended = false; + private boolean commandExtended = false; + + private PneumaticDevice.Input pneumaticState = new PneumaticDevice.Input(0); + + public DoubleActingPneumaticCylinder(final double boreDiameter, final double stroke) { + this.boreDiameter = boreDiameter; + this.stroke = stroke; + } + + public void setExtended(final boolean state) { + commandExtended = state; + } + + @Override + public PneumaticDevice.Output step(PneumaticDevice.Input input, double dt) { + pneumaticState = input; + PneumaticDevice.Output output; + double deviceVolume = boreArea() * stroke; + if (commandExtended != isExtended) { + output = + new PneumaticDevice.Output( + -deviceVolume + * (input.pressure + PhysicalConstants.ATMOSPHERIC_PRESSURE) + / PhysicalConstants.ATMOSPHERIC_PRESSURE, + deviceVolume); + } else { + output = new PneumaticDevice.Output(0, deviceVolume); + } + isExtended = commandExtended; + return output; + } + + @Override + public MechanicalDevice.Output step(MechanicalDevice.Input input, double dt) { + Vector3D direction = isExtended ? FORWARD : FORWARD.negate(); + return new MechanicalDevice.Output( + direction.scalarMultiply(boreArea() * pneumaticState.pressure)); + } + + private double boreArea() { + return Math.PI * Math.pow(boreDiameter / 2., 2); + } } diff --git a/src/main/java/com/team766/simulator/elements/ElectricalResistance.java b/src/main/java/com/team766/simulator/elements/ElectricalResistance.java index 17edde92..879affc1 100644 --- a/src/main/java/com/team766/simulator/elements/ElectricalResistance.java +++ b/src/main/java/com/team766/simulator/elements/ElectricalResistance.java @@ -3,39 +3,44 @@ import com.team766.simulator.interfaces.ElectricalDevice; public class ElectricalResistance implements ElectricalDevice { - // Wire AWG -> Ohms/m (copper) - // http://www.daycounter.com/Calculators/AWG.phtml - private static double[] WIRE_RESISTANCE_PER_M = new double[] { - 0.000323, 0.000407, 0.000513, 0.000647, 0.000815, 0.00103, 0.00130, 0.00163, 0.00206, 0.00260, // 0-9 - 0.00328, 0.00413, 0.00521, 0.00657, 0.00829, 0.0104, 0.0132, 0.0166, 0.0210, 0.0264, // 10-19 - 0.0333, 0.0420, 0.0530, 0.0668, 0.0842, 0.106, 0.134, 0.169, 0.213, 0.268, // 20-29 - 0.339, 0.427, 0.538, 0.679, 0.856, 1.08, 1.36, 1.72, 2.16, 2.73, 3.44, // 30-40 - }; - public static ElectricalResistance makeWires(final int awg, final double length, - final ElectricalDevice downstream) { - return new ElectricalResistance(WIRE_RESISTANCE_PER_M[awg] * length / 1000., downstream); - } - - private final double resistance; - - private ElectricalDevice downstream; - - private ElectricalDevice.Output state; - - public ElectricalResistance(final double resistance_, final ElectricalDevice downstream_) { - this.resistance = resistance_; - this.downstream = downstream_; - } - - @Override - public ElectricalDevice.Output step(ElectricalDevice.Input input, double dt) { - ElectricalDevice.Input downstreamInput = new ElectricalDevice.Input(input.voltage - resistance * state.current); - state = downstream.step(downstreamInput, dt); - return state; - } - - @Override - public String name() { - return downstream.name() + "+resistance"; - } + // Wire AWG -> Ohms/m (copper) + // http://www.daycounter.com/Calculators/AWG.phtml + private static double[] WIRE_RESISTANCE_PER_M = + new double[] { + 0.000323, 0.000407, 0.000513, 0.000647, 0.000815, 0.00103, 0.00130, 0.00163, + 0.00206, 0.00260, // 0-9 + 0.00328, 0.00413, 0.00521, 0.00657, 0.00829, 0.0104, 0.0132, 0.0166, 0.0210, + 0.0264, // 10-19 + 0.0333, 0.0420, 0.0530, 0.0668, 0.0842, 0.106, 0.134, 0.169, 0.213, 0.268, // 20-29 + 0.339, 0.427, 0.538, 0.679, 0.856, 1.08, 1.36, 1.72, 2.16, 2.73, 3.44, // 30-40 + }; + + public static ElectricalResistance makeWires( + final int awg, final double length, final ElectricalDevice downstream) { + return new ElectricalResistance(WIRE_RESISTANCE_PER_M[awg] * length / 1000., downstream); + } + + private final double resistance; + + private ElectricalDevice downstream; + + private ElectricalDevice.Output state; + + public ElectricalResistance(final double resistance_, final ElectricalDevice downstream_) { + this.resistance = resistance_; + this.downstream = downstream_; + } + + @Override + public ElectricalDevice.Output step(ElectricalDevice.Input input, double dt) { + ElectricalDevice.Input downstreamInput = + new ElectricalDevice.Input(input.voltage - resistance * state.current); + state = downstream.step(downstreamInput, dt); + return state; + } + + @Override + public String name() { + return downstream.name() + "+resistance"; + } } diff --git a/src/main/java/com/team766/simulator/elements/Gears.java b/src/main/java/com/team766/simulator/elements/Gears.java index 51e9f969..48462b6d 100644 --- a/src/main/java/com/team766/simulator/elements/Gears.java +++ b/src/main/java/com/team766/simulator/elements/Gears.java @@ -3,24 +3,24 @@ import com.team766.simulator.interfaces.MechanicalAngularDevice; public class Gears implements MechanicalAngularDevice { - // TODO: Add rotational inertia - // TODO: Add losses + // TODO: Add rotational inertia + // TODO: Add losses - // Torque ratio (output / input) - private final double torqueRatio; + // Torque ratio (output / input) + private final double torqueRatio; - private MechanicalAngularDevice upstream; + private MechanicalAngularDevice upstream; - public Gears(final double torqueRatio_, final MechanicalAngularDevice upstream_) { - this.torqueRatio = torqueRatio_; - this.upstream = upstream_; - } + public Gears(final double torqueRatio_, final MechanicalAngularDevice upstream_) { + this.torqueRatio = torqueRatio_; + this.upstream = upstream_; + } - @Override - public MechanicalAngularDevice.Output step(MechanicalAngularDevice.Input input, double dt) { - MechanicalAngularDevice.Input upstreamInput = - new MechanicalAngularDevice.Input(input.angularVelocity * torqueRatio); - MechanicalAngularDevice.Output upstreamOutput = upstream.step(upstreamInput, dt); - return new MechanicalAngularDevice.Output(upstreamOutput.torque * torqueRatio); - } + @Override + public MechanicalAngularDevice.Output step(MechanicalAngularDevice.Input input, double dt) { + MechanicalAngularDevice.Input upstreamInput = + new MechanicalAngularDevice.Input(input.angularVelocity * torqueRatio); + MechanicalAngularDevice.Output upstreamOutput = upstream.step(upstreamInput, dt); + return new MechanicalAngularDevice.Output(upstreamOutput.torque * torqueRatio); + } } diff --git a/src/main/java/com/team766/simulator/elements/MotorController.java b/src/main/java/com/team766/simulator/elements/MotorController.java index 8889dda8..aa1d98ec 100644 --- a/src/main/java/com/team766/simulator/elements/MotorController.java +++ b/src/main/java/com/team766/simulator/elements/MotorController.java @@ -3,25 +3,26 @@ import com.team766.simulator.interfaces.ElectricalDevice; public abstract class MotorController implements ElectricalDevice { - private ElectricalDevice downstream; + private ElectricalDevice downstream; - public MotorController(final ElectricalDevice downstream_) { - this.downstream = downstream_; - } + public MotorController(final ElectricalDevice downstream_) { + this.downstream = downstream_; + } - // [-1, 1] representing the command sent from the application processor - protected abstract double getCommand(); + // [-1, 1] representing the command sent from the application processor + protected abstract double getCommand(); - @Override - public ElectricalDevice.Output step(ElectricalDevice.Input input, double dt) { - double command = getCommand(); - ElectricalDevice.Input downstreamInput = new ElectricalDevice.Input(input.voltage * command); - ElectricalDevice.Output downstreamOutput = downstream.step(downstreamInput, dt); - return new Output(Math.max(0, downstreamOutput.current * Math.signum(command))); - } + @Override + public ElectricalDevice.Output step(ElectricalDevice.Input input, double dt) { + double command = getCommand(); + ElectricalDevice.Input downstreamInput = + new ElectricalDevice.Input(input.voltage * command); + ElectricalDevice.Output downstreamOutput = downstream.step(downstreamInput, dt); + return new Output(Math.max(0, downstreamOutput.current * Math.signum(command))); + } - @Override - public String name() { - return "MotorController:" + downstream.name(); - } + @Override + public String name() { + return "MotorController:" + downstream.name(); + } } diff --git a/src/main/java/com/team766/simulator/elements/SingleActingPneumaticCylinder.java b/src/main/java/com/team766/simulator/elements/SingleActingPneumaticCylinder.java index 5deb5c8e..23101267 100644 --- a/src/main/java/com/team766/simulator/elements/SingleActingPneumaticCylinder.java +++ b/src/main/java/com/team766/simulator/elements/SingleActingPneumaticCylinder.java @@ -1,59 +1,60 @@ package com.team766.simulator.elements; -import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; - import com.team766.simulator.PhysicalConstants; import com.team766.simulator.interfaces.MechanicalDevice; import com.team766.simulator.interfaces.PneumaticDevice; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; public class SingleActingPneumaticCylinder implements PneumaticDevice, MechanicalDevice { - private static final Vector3D FORWARD = new Vector3D(1, 0, 0); - - private final double boreDiameter; - private final double stroke; - private final double returnSpringForce; - - private boolean isExtended = false; - private boolean commandExtended = false; - - private PneumaticDevice.Input pneumaticState = new PneumaticDevice.Input(0); - - public SingleActingPneumaticCylinder(final double boreDiameter_, final double stroke_, - final double returnSpringForce_) { - this.boreDiameter = boreDiameter_; - this.stroke = stroke_; - this.returnSpringForce = returnSpringForce_; - } - - public void setExtended(final boolean state) { - commandExtended = state; - } - - @Override - public PneumaticDevice.Output step(PneumaticDevice.Input input, double dt) { - pneumaticState = input; - PneumaticDevice.Output output; - double deviceVolume = isExtended ? boreArea() * stroke : 0; - if (isExtended && !commandExtended) { - output = new PneumaticDevice.Output( - -deviceVolume * (input.pressure + PhysicalConstants.ATMOSPHERIC_PRESSURE) - / PhysicalConstants.ATMOSPHERIC_PRESSURE, - deviceVolume); - } else { - output = new PneumaticDevice.Output(0, deviceVolume); - } - isExtended = commandExtended; - return output; - } - - @Override - public MechanicalDevice.Output step(MechanicalDevice.Input input, double dt) { - Vector3D direction = isExtended ? FORWARD : FORWARD.negate(); - double force = isExtended ? boreArea() * pneumaticState.pressure : returnSpringForce; - return new MechanicalDevice.Output(direction.scalarMultiply(force)); - } - - private double boreArea() { - return Math.PI * Math.pow(boreDiameter / 2., 2); - } + private static final Vector3D FORWARD = new Vector3D(1, 0, 0); + + private final double boreDiameter; + private final double stroke; + private final double returnSpringForce; + + private boolean isExtended = false; + private boolean commandExtended = false; + + private PneumaticDevice.Input pneumaticState = new PneumaticDevice.Input(0); + + public SingleActingPneumaticCylinder( + final double boreDiameter_, final double stroke_, final double returnSpringForce_) { + this.boreDiameter = boreDiameter_; + this.stroke = stroke_; + this.returnSpringForce = returnSpringForce_; + } + + public void setExtended(final boolean state) { + commandExtended = state; + } + + @Override + public PneumaticDevice.Output step(PneumaticDevice.Input input, double dt) { + pneumaticState = input; + PneumaticDevice.Output output; + double deviceVolume = isExtended ? boreArea() * stroke : 0; + if (isExtended && !commandExtended) { + output = + new PneumaticDevice.Output( + -deviceVolume + * (input.pressure + PhysicalConstants.ATMOSPHERIC_PRESSURE) + / PhysicalConstants.ATMOSPHERIC_PRESSURE, + deviceVolume); + } else { + output = new PneumaticDevice.Output(0, deviceVolume); + } + isExtended = commandExtended; + return output; + } + + @Override + public MechanicalDevice.Output step(MechanicalDevice.Input input, double dt) { + Vector3D direction = isExtended ? FORWARD : FORWARD.negate(); + double force = isExtended ? boreArea() * pneumaticState.pressure : returnSpringForce; + return new MechanicalDevice.Output(direction.scalarMultiply(force)); + } + + private double boreArea() { + return Math.PI * Math.pow(boreDiameter / 2., 2); + } } diff --git a/src/main/java/com/team766/simulator/elements/Wheel.java b/src/main/java/com/team766/simulator/elements/Wheel.java index 4ae82857..432c9fbc 100644 --- a/src/main/java/com/team766/simulator/elements/Wheel.java +++ b/src/main/java/com/team766/simulator/elements/Wheel.java @@ -1,40 +1,44 @@ package com.team766.simulator.elements; -import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; - import com.team766.simulator.Parameters; import com.team766.simulator.PhysicalConstants; import com.team766.simulator.interfaces.MechanicalAngularDevice; import com.team766.simulator.interfaces.MechanicalDevice; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; // Simulate a wheel revolving around the positive Y axis. public class Wheel implements MechanicalDevice { - // TODO: Add transverse friction - // TODO: Add traction limit/wheel slip (static vs kinetic friction) - // TODO: Add rotational inertia - - private static final Vector3D FORWARD = new Vector3D(-1, 0, 0); - - // Diameter of the wheel in meters - private final double diameter; - - private MechanicalAngularDevice upstream; - - public Wheel(final double diameter_, final MechanicalAngularDevice upstream_) { - this.diameter = diameter_; - this.upstream = upstream_; - } - - @Override - public MechanicalDevice.Output step(MechanicalDevice.Input input, double dt) { - MechanicalAngularDevice.Input upstreamInput = - new MechanicalAngularDevice.Input(FORWARD.dotProduct(input.velocity) * 2. / diameter); - MechanicalAngularDevice.Output upstreamOutput = upstream.step(upstreamInput, dt); - double appliedForce = upstreamOutput.torque * 2. / diameter; - double maxFriction = Parameters.WHEEL_COEFFICIENT_OF_FRICTION * Parameters.ROBOT_MASS * PhysicalConstants.GRAVITY_ACCELERATION / Parameters.NUM_LOADED_WHEELS; - if (Math.abs(appliedForce) > maxFriction) { - appliedForce = Math.signum(appliedForce) * maxFriction; - } - return new MechanicalDevice.Output(FORWARD.scalarMultiply(appliedForce)); - } + // TODO: Add transverse friction + // TODO: Add traction limit/wheel slip (static vs kinetic friction) + // TODO: Add rotational inertia + + private static final Vector3D FORWARD = new Vector3D(-1, 0, 0); + + // Diameter of the wheel in meters + private final double diameter; + + private MechanicalAngularDevice upstream; + + public Wheel(final double diameter_, final MechanicalAngularDevice upstream_) { + this.diameter = diameter_; + this.upstream = upstream_; + } + + @Override + public MechanicalDevice.Output step(MechanicalDevice.Input input, double dt) { + MechanicalAngularDevice.Input upstreamInput = + new MechanicalAngularDevice.Input( + FORWARD.dotProduct(input.velocity) * 2. / diameter); + MechanicalAngularDevice.Output upstreamOutput = upstream.step(upstreamInput, dt); + double appliedForce = upstreamOutput.torque * 2. / diameter; + double maxFriction = + Parameters.WHEEL_COEFFICIENT_OF_FRICTION + * Parameters.ROBOT_MASS + * PhysicalConstants.GRAVITY_ACCELERATION + / Parameters.NUM_LOADED_WHEELS; + if (Math.abs(appliedForce) > maxFriction) { + appliedForce = Math.signum(appliedForce) * maxFriction; + } + return new MechanicalDevice.Output(FORWARD.scalarMultiply(appliedForce)); + } } diff --git a/src/main/java/com/team766/simulator/interfaces/ElectricalDevice.java b/src/main/java/com/team766/simulator/interfaces/ElectricalDevice.java index dabf49e1..3fa0c9f0 100644 --- a/src/main/java/com/team766/simulator/interfaces/ElectricalDevice.java +++ b/src/main/java/com/team766/simulator/interfaces/ElectricalDevice.java @@ -1,31 +1,31 @@ package com.team766.simulator.interfaces; public interface ElectricalDevice { - class Input { - public Input(final double voltage_) { - this.voltage = voltage_; - } + class Input { + public Input(final double voltage_) { + this.voltage = voltage_; + } - public Input(final Input other) { - voltage = other.voltage; - } + public Input(final Input other) { + voltage = other.voltage; + } - public final double voltage; - } + public final double voltage; + } - class Output { - public Output(final double current_) { - this.current = current_; - } + class Output { + public Output(final double current_) { + this.current = current_; + } - public Output(final Output other) { - current = other.current; - } + public Output(final Output other) { + current = other.current; + } - public final double current; - } + public final double current; + } - public String name(); - - public Output step(Input input, double dt); + public String name(); + + public Output step(Input input, double dt); } diff --git a/src/main/java/com/team766/simulator/interfaces/MechanicalAngularDevice.java b/src/main/java/com/team766/simulator/interfaces/MechanicalAngularDevice.java index d2f6a150..fbe0acbb 100644 --- a/src/main/java/com/team766/simulator/interfaces/MechanicalAngularDevice.java +++ b/src/main/java/com/team766/simulator/interfaces/MechanicalAngularDevice.java @@ -1,29 +1,29 @@ package com.team766.simulator.interfaces; public interface MechanicalAngularDevice { - class Input { - public Input(final double angularVelocity_) { - this.angularVelocity = angularVelocity_; - } + class Input { + public Input(final double angularVelocity_) { + this.angularVelocity = angularVelocity_; + } - public Input(final Input other) { - this.angularVelocity = other.angularVelocity; - } + public Input(final Input other) { + this.angularVelocity = other.angularVelocity; + } - public final double angularVelocity; - } + public final double angularVelocity; + } - class Output { - public Output(final double torque_) { - this.torque = torque_; - } + class Output { + public Output(final double torque_) { + this.torque = torque_; + } - public Output(final Output other) { - this.torque = other.torque; - } + public Output(final Output other) { + this.torque = other.torque; + } - public final double torque; - } - - public Output step(Input input, double dt); + public final double torque; + } + + public Output step(Input input, double dt); } diff --git a/src/main/java/com/team766/simulator/interfaces/MechanicalDevice.java b/src/main/java/com/team766/simulator/interfaces/MechanicalDevice.java index 85b375f7..8388761d 100644 --- a/src/main/java/com/team766/simulator/interfaces/MechanicalDevice.java +++ b/src/main/java/com/team766/simulator/interfaces/MechanicalDevice.java @@ -3,32 +3,32 @@ import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; public interface MechanicalDevice { - class Input { - public Input(final Vector3D position_, final Vector3D velocity_) { - this.position = position_; - this.velocity = velocity_; - } - - public Input(final Input other) { - position = other.position; - velocity = other.velocity; - } - - public final Vector3D position; - public final Vector3D velocity; - } - - class Output { - public Output(final Vector3D force_) { - this.force = force_; - } - - public Output(final Output other) { - force = other.force; - } - - public final Vector3D force; - } - - public Output step(Input input, double dt); + class Input { + public Input(final Vector3D position_, final Vector3D velocity_) { + this.position = position_; + this.velocity = velocity_; + } + + public Input(final Input other) { + position = other.position; + velocity = other.velocity; + } + + public final Vector3D position; + public final Vector3D velocity; + } + + class Output { + public Output(final Vector3D force_) { + this.force = force_; + } + + public Output(final Output other) { + force = other.force; + } + + public final Vector3D force; + } + + public Output step(Input input, double dt); } diff --git a/src/main/java/com/team766/simulator/interfaces/PneumaticDevice.java b/src/main/java/com/team766/simulator/interfaces/PneumaticDevice.java index fef20273..4b4ffd30 100644 --- a/src/main/java/com/team766/simulator/interfaces/PneumaticDevice.java +++ b/src/main/java/com/team766/simulator/interfaces/PneumaticDevice.java @@ -1,42 +1,42 @@ package com.team766.simulator.interfaces; public interface PneumaticDevice { - class Input { - public Input(final double pressure_) { - this.pressure = pressure_; - } - - public Input(final Input other) { - pressure = other.pressure; - } - - // Pascals (relative pressure) - public final double pressure; - } - - class Output { - public Output(final double flowVolume_, final double deviceVolume_) { - this.flowVolume = flowVolume_; - this.deviceVolume = deviceVolume_; - } - - public Output(final Output other) { - flowVolume = other.flowVolume; - deviceVolume = other.deviceVolume; - } - - // Volumetric flow (delta m^3 at atmospheric pressure) - // Positive flow is into the system, e.g. from a compressor - // Negative flow is out of the system, e.g. from venting to atmosphere - public final double flowVolume; - - // Volume of air that the device contains (m^3) - public final double deviceVolume; - - // Note that an expanding volume (such as a cylinder expanding) - // should increase volume, but have 0 flow volume because no - // pressurized air is actually leaving the system. - } - - public Output step(Input input, double dt); + class Input { + public Input(final double pressure_) { + this.pressure = pressure_; + } + + public Input(final Input other) { + pressure = other.pressure; + } + + // Pascals (relative pressure) + public final double pressure; + } + + class Output { + public Output(final double flowVolume_, final double deviceVolume_) { + this.flowVolume = flowVolume_; + this.deviceVolume = deviceVolume_; + } + + public Output(final Output other) { + flowVolume = other.flowVolume; + deviceVolume = other.deviceVolume; + } + + // Volumetric flow (delta m^3 at atmospheric pressure) + // Positive flow is into the system, e.g. from a compressor + // Negative flow is out of the system, e.g. from venting to atmosphere + public final double flowVolume; + + // Volume of air that the device contains (m^3) + public final double deviceVolume; + + // Note that an expanding volume (such as a cylinder expanding) + // should increase volume, but have 0 flow volume because no + // pressurized air is actually leaving the system. + } + + public Output step(Input input, double dt); } diff --git a/src/main/java/com/team766/simulator/mechanisms/DoubleJointedArm.java b/src/main/java/com/team766/simulator/mechanisms/DoubleJointedArm.java index dad28752..d36ddc51 100644 --- a/src/main/java/com/team766/simulator/mechanisms/DoubleJointedArm.java +++ b/src/main/java/com/team766/simulator/mechanisms/DoubleJointedArm.java @@ -16,280 +16,352 @@ * "virtual linkage". */ public class DoubleJointedArm { - // General nomenclature: - // J1 refers to the first (shoulder) joint, as well as the first link of the arm. - // J2 refers to the second (elbow) joint, as well as the second link of the arm. - - private static final double J1_INITIAL_ANGLE = Math.toRadians(202.); // radians, relative to "down" - private static final double J2_INITIAL_ANGLE = Math.toRadians(32.); // radians, relative to "down" - - // Joint limits - private static final double J1_MIN = Math.toRadians(90.); // radians, relative to "down" - private static final double J1_MAX = Math.toRadians(205.); // radians, relative to "down" - private static final double J2_MIN = Math.toRadians(-170); // radians, relative to J1 - private static final double J2_MAX = Math.toRadians(170); // radians, relative to J1 - - // Physical characterisics of the arm links. - // NOTE(rcahoon, 2023-02-09): We could determine these from CAD data, but using guessed/approximate values for now. - private static final double J1_LENGTH = 1; // m - private static final double J2_LENGTH = 1; // m - private static final double J1_MASS = 2; // kg - private static final double J2_MASS = 5; // kg - private static final double J1_FIRST_MOMENT_OF_MASS = J1_MASS * J1_LENGTH; // kg m, with the origin/axis at the first joint - private static final double J1_SECOND_MOMENT_OF_MASS = J1_MASS * J1_LENGTH * J1_LENGTH; // kg m^2, with the origin/axis at the first joint - aka moment of inertia - private static final double J2_FIRST_MOMENT_OF_MASS = J2_MASS * J2_LENGTH; // kg m, with the origin/axis at the second joint - private static final double J2_SECOND_MOMENT_OF_MASS = J2_MASS * J2_LENGTH * J2_LENGTH; // kg m^2, with the origin/axis at the second joint - aka moment of inertia - - private static final double J1_GEAR_RATIO = 4. * 4. * 3. * (58. / 14.); - private static final double J2_GEAR_RATIO = 4. * 4. * 3. * (58. / 14.); - - // Simulated CAN Bus IDs for the two motors. - private static final int J1_CAN_CHANNEL = 98; - private static final int J2_CAN_CHANNEL = 99; - - private DCMotor j1Motor = DCMotor.makeNeo("ArmJoint1"); - private DCMotor j2Motor = DCMotor.makeNeo("ArmJoint2"); - - private CanMotorController j1Controller = new CanMotorController(J1_CAN_CHANNEL, j1Motor); - private CanMotorController j2Controller = new CanMotorController(J2_CAN_CHANNEL, j2Motor); - - private Gears j1Gears = new Gears(J1_GEAR_RATIO, j1Motor); - private Gears j2Gears = new Gears(J2_GEAR_RATIO, j2Motor); - - private Matrix state = VecBuilder.fill( - J1_INITIAL_ANGLE, // j1 angle, radians, relative to "down" - J2_INITIAL_ANGLE, // j2 angle, radians, relative to "down" - 0, // j1 velocity, radians/sec - 0 // j2 velocity, radians/sec - ); - - // Derivation of the equations of motion via Lagrangian mechanics. - // This system is often referred to as a "double pendulum", and is a commonly studied problem, - // so literature is available. See this series of videos for an explanation: - // https://youtu.be/tc2ah-KnDXw, https://youtu.be/eBg8gof1RBg, https://youtu.be/QE1_H2vtHLU - // The formulation given below is slightly more generalized to allow arbitrary mass - // distributions along the arm links. - // - // Kinematics: - // x1 = J1_FIRST_MOMENT_OF_MASS / J1_MASS * sin(theta1) - // y1 = -J1_FIRST_MOMENT_OF_MASS / J1_MASS * cos(theta1) - // x2 = J1_LENGTH * sin(theta1) + J2_FIRST_MOMENT_OF_MASS / J2_MASS * sin(theta2) - // y2 = -J1_LENGTH * cos(theta1) - J2_FIRST_MOMENT_OF_MASS / J2_MASS * cos(theta2) - // - // Lagrangian: - // L = T - V - // = 1/2 * J1_MASS * (x1'^2 + y1'^2) - // + 1/2 * (J1_SECOND_MOMENT_OF_MASS - J1_FIRST_MOMENT_OF_MASS^2 / J1_MASS) * theta1'^2 - // + 1/2 * J2_MASS * (x2'^2 + y2'^2) - // + 1/2 * (J2_SECOND_MOMENT_OF_MASS - J2_FIRST_MOMENT_OF_MASS^2 / J2_MASS) * theta2'^2 - // - J1_MASS * y1 * PhysicalConstants.GRAVITY_ACCELERATION - // - J2_MASS * y2 * PhysicalConstants.GRAVITY_ACCELERATION - // = 1/2 * J1_SECOND_MOMENT_OF_MASS * theta1'^2 - // + 1/2 * J2_MASS * ((J1_LENGTH * cos(theta1) * theta1' + J2_FIRST_MOMENT_OF_MASS / J2_MASS * cos(theta2) * theta2')^2 + (J1_LENGTH * sin(theta1) * theta1' + J2_FIRST_MOMENT_OF_MASS / J2_MASS * sin(theta2) * theta2')^2) - // + 1/2 * (J2_SECOND_MOMENT_OF_MASS - J2_FIRST_MOMENT_OF_MASS^2 / J2_MASS) * theta2'^2 - // + J1_FIRST_MOMENT_OF_MASS * cos(theta1) * PhysicalConstants.GRAVITY_ACCELERATION - // + (J2_MASS * J1_LENGTH * cos(theta1) + J2_FIRST_MOMENT_OF_MASS * cos(theta2)) * PhysicalConstants.GRAVITY_ACCELERATION - // = 1/2 * J1_SECOND_MOMENT_OF_MASS * theta1'^2 - // + 1/2 * (J2_MASS * J1_LENGTH^2 * theta1'^2 + 2 * J1_LENGTH * cos(theta1 - theta2) * theta1' * J2_FIRST_MOMENT_OF_MASS * theta2' + J2_FIRST_MOMENT_OF_MASS^2 / J2_MASS * theta2'^2) - // + 1/2 * (J2_SECOND_MOMENT_OF_MASS - J2_FIRST_MOMENT_OF_MASS^2 / J2_MASS) * theta2'^2 - // + J1_FIRST_MOMENT_OF_MASS * cos(theta1) * PhysicalConstants.GRAVITY_ACCELERATION - // + (J2_MASS * J1_LENGTH * cos(theta1) + J2_FIRST_MOMENT_OF_MASS * cos(theta2)) * PhysicalConstants.GRAVITY_ACCELERATION - // = 1/2 * (J1_SECOND_MOMENT_OF_MASS + J2_MASS * J1_LENGTH^2) * theta1'^2 - // + 1/2 * J2_SECOND_MOMENT_OF_MASS * theta2'^2 - // + J2_FIRST_MOMENT_OF_MASS * J1_LENGTH * cos(theta1 - theta2) * theta1' * theta2' - // + (J1_FIRST_MOMENT_OF_MASS + J2_MASS * J1_LENGTH) * PhysicalConstants.GRAVITY_ACCELERATION * cos(theta1) - // + J2_FIRST_MOMENT_OF_MASS * PhysicalConstants.GRAVITY_ACCELERATION * cos(theta2) - // - // Define some constants to simplify the following equations - // I1 = J1_SECOND_MOMENT_OF_MASS + J2_MASS * J1_LENGTH^2 - // I2 = J2_SECOND_MOMENT_OF_MASS - // Ix = J2_FIRST_MOMENT_OF_MASS * J1_LENGTH - // g1 = (J1_FIRST_MOMENT_OF_MASS + J2_MASS * J1_LENGTH) * PhysicalConstants.GRAVITY_ACCELERATION - // g2 = J2_FIRST_MOMENT_OF_MASS * PhysicalConstants.GRAVITY_ACCELERATION - // - // L = 1/2 * I1 * theta1'^2 + 1/2 * I2 * theta2'^2 + Ix * cos(theta1 - theta2) * theta1' * theta2' + g1 * cos(theta1) + g2 * cos(theta2) - // - // Euler-Lagrangian: - // d/dt [Partial]L/[Partial]theta' - [Partial]L/[Partial]theta = tau - // - // d/dt [Partial]L/[Partial]theta1' - [Partial]L/[Partial]theta1 = I1 * theta1'' + Ix * cos(theta1 - theta2) * theta2'' - Ix * sin(theta1 - theta2) * (theta1' - theta2') * theta2' - // + Ix * sin(theta1 - theta2) * theta1' * theta2' + g1 * sin(theta1) - // = I1 * theta1'' + Ix * cos(theta1 - theta2) * theta2'' + Ix * sin(theta1 - theta2) * theta2'^2 + g1 * sin(theta1) - // = tau1 - // - // d/dt [Partial]L/[Partial]theta2' - [Partial]L/[Partial]theta2 = I2 * theta2'' + Ix * cos(theta1 - theta2) * theta1'' - Ix * sin(theta1 - theta2) * (theta1' - theta2') * theta1' - // - Ix * sin(theta1 - theta2) * theta1' * theta2' + g2 * sin(theta2)) - // = I2 * theta2'' + Ix * cos(theta1 - theta2) * theta1'' - Ix * sin(theta1 - theta2) * theta1'^2 + g2 * sin(theta2) - // = tau2 - // - // Rewrite as a matrix equation: - // M * theta'' + G = tau - // theta'' = M^-1 * (tau - G) - // - // M = [ I1, Ix * cos(theta1 - theta2) ] - // = [ Ix * cos(theta1 - theta2), I2 ] - // G = [ Ix * sin(theta1 - theta2) * theta1' * theta2' + g1 * sin(theta1) ] - // [ -Ix * sin(theta1 - theta2) * theta1' * theta2' + g2 * sin(theta2) ] - - public DoubleJointedArm(ElectricalSystem electricalSystem) { - electricalSystem.addDevice(j1Controller); - electricalSystem.addDevice(j2Controller); - } - - /** - * Advance the simulation model by the given amount of time. - */ - public void step(double dt) { - final MechanicalAngularDevice.Output j1Drive = j1Gears.step(new MechanicalAngularDevice.Input(getJ1Velocity()), dt); - final MechanicalAngularDevice.Output j2Drive = j2Gears.step(new MechanicalAngularDevice.Input(getJ2Velocity()), dt); - - double j1Limit = 0.; - if (state.get(0, 0) <= J1_MIN) { - state.set(0, 0, J1_MIN); - state.set(2, 0, 0.0); - j1Limit = 1.; - } - if (state.get(0, 0) >= J1_MAX) { - state.set(0, 0, J1_MAX); - state.set(2, 0, 0.0); - j1Limit = -1.; - } - double j2Limit = 0.; - if (state.get(1, 0) - state.get(0, 0) <= J2_MIN) { - state.set(1, 0, J2_MIN + state.get(0, 0)); - j2Limit = 1.; - } - if (state.get(1, 0) - state.get(0, 0) >= J2_MAX) { - state.set(1, 0, J2_MAX + state.get(0, 0)); - j2Limit = -1.; - } - - if (state.get(2, 0) * j1Limit < 0) { - state.set(2, 0, 0.0); - } - if (state.get(3, 0) * j2Limit < 0) { - state.set(3, 0, 0.0); - } - - final var u = VecBuilder.fill(j1Drive.torque, j2Drive.torque, j1Limit, j2Limit); - final var stateDot = S(state, u); - state = state.plus(stateDot.times(dt)); - - j1Controller.setSensorPosition(state.get(0, 0)); - j2Controller.setSensorPosition(state.get(1, 0)); - j1Controller.setSensorVelocity(state.get(2, 0)); - j2Controller.setSensorVelocity(state.get(3, 0)); - } - - /** - * State space model X'(t) = S(X(t), u(t)) of the system - */ - // Defined as a static method to make sure we don't accidentally refer to any of the state - // stored in class member variables when we should be referring to the state the integrator - // gave us. - private static Vector S(Matrix state, Matrix u) { - Vector motorTorques = VecBuilder.fill(u.get(0, 0), u.get(1, 0)); - Vector jointLimits = VecBuilder.fill(u.get(2, 0), u.get(3, 0)); - Vector velocity = VecBuilder.fill(state.get(2, 0), state.get(3, 0)); - - final double Mx = J2_FIRST_MOMENT_OF_MASS * J1_LENGTH * Math.cos(state.get(0, 0) - state.get(1, 0)); - var M = Matrix.mat(N2.instance, N2.instance).fill( - J1_SECOND_MOMENT_OF_MASS + J2_MASS * J1_LENGTH * J1_LENGTH, Mx, - Mx, J2_SECOND_MOMENT_OF_MASS - ); - - final double C = J2_FIRST_MOMENT_OF_MASS * J1_LENGTH * Math.sin(state.get(0, 0) - state.get(1, 0)) * state.get(2, 0) * state.get(3, 0); - final var G = VecBuilder.fill( - C + (J1_FIRST_MOMENT_OF_MASS + J2_MASS * J1_LENGTH) * PhysicalConstants.GRAVITY_ACCELERATION * Math.sin(state.get(0, 0)), - -C + J2_FIRST_MOMENT_OF_MASS * PhysicalConstants.GRAVITY_ACCELERATION * Math.sin(state.get(1, 0)) - ); - var T = motorTorques.minus(G); - - if (T.get(0, 0) * jointLimits.get(0, 0) < 0) { - M.set(0, 0, 1.0); - M.set(1, 0, 0.0); - M.set(0, 1, 0.0); - T.set(0, 0, 0.0); - } - if (T.get(1, 0) * jointLimits.get(1, 0) < 0) { - M.set(1, 1, 1.0); - M.set(1, 0, 0.0); - M.set(0, 1, 0.0); - T.set(1, 0, 0.0); - } - - final var acceleration = M.inv().times(T); - - return VecBuilder.fill(velocity.get(0, 0), velocity.get(1, 0), acceleration.get(0, 0), acceleration.get(1, 0)); - } - - /** - * Return the angle of the first joint. - */ - public double getJ1Angle() { - return state.get(0, 0); - } - - /** - * Return the angle of the second joint. - */ - public double getJ2Angle() { - return state.get(1, 0); - } - - /** - * Return the velocity of the first joint. - */ - public double getJ1Velocity() { - return state.get(2, 0); - } - - /** - * Return the velocity of the second joint. - */ - public double getJ2Velocity() { - return state.get(3, 0); - } - - /** - * Calculate the total kinetic and potential energy in the arm. - * - * If no energy is added or removed (e.g. by motors or friction), this quantity should remain - * constant (i.e. conservation of energy), so plotting it over time makes a good sanity check of - * the model. - */ - public double getEnergy() { - double y1 = -J1_FIRST_MOMENT_OF_MASS / J1_MASS * Math.cos(getJ1Angle()); - double y2 = -J1_LENGTH * Math.cos(getJ1Angle()) - J2_FIRST_MOMENT_OF_MASS / J2_MASS * Math.cos(getJ2Angle()); - double dx1 = J1_FIRST_MOMENT_OF_MASS / J1_MASS * Math.cos(getJ1Angle()) * getJ1Velocity(); - double dy1 = J1_FIRST_MOMENT_OF_MASS / J1_MASS * Math.sin(getJ1Angle()) * getJ1Velocity(); - double dx2 = J1_LENGTH * Math.cos(getJ1Angle()) * getJ1Velocity() + J2_FIRST_MOMENT_OF_MASS / J2_MASS * Math.cos(getJ2Angle()) * getJ2Velocity(); - double dy2 = J1_LENGTH * Math.sin(getJ1Angle()) * getJ1Velocity() + J2_FIRST_MOMENT_OF_MASS / J2_MASS * Math.sin(getJ2Angle()) * getJ2Velocity(); - - double energy = 0.5 * J1_MASS * (dx1*dx1 + dy1*dy1) - + 0.5 * (J1_SECOND_MOMENT_OF_MASS - J1_FIRST_MOMENT_OF_MASS * J1_FIRST_MOMENT_OF_MASS / J1_MASS) * getJ1Velocity() * getJ1Velocity() - + 0.5 * J2_MASS * (dx2*dx2 + dy2*dy2) - + 0.5 * (J2_SECOND_MOMENT_OF_MASS - J2_FIRST_MOMENT_OF_MASS * J2_FIRST_MOMENT_OF_MASS / J2_MASS) * getJ2Velocity() * getJ2Velocity() - + J1_MASS * y1 * PhysicalConstants.GRAVITY_ACCELERATION - + J2_MASS * y2 * PhysicalConstants.GRAVITY_ACCELERATION; - return energy; - } - - /** - * Calculuate the cartesian coordinates of the end of the first link. - */ - public Vector getJ1Position() { - double x1 = J1_LENGTH * Math.sin(getJ1Angle()); - double y1 = -J1_LENGTH * Math.cos(getJ1Angle()); - return VecBuilder.fill(x1, y1); - } - - /** - * Calculuate the cartesian coordinates of the end of the second link. - */ - public Vector getJ2Position() { - double x2 = J1_LENGTH * Math.sin(getJ1Angle()) + J2_LENGTH * Math.sin(getJ2Angle()); - double y2 = -J1_LENGTH * Math.cos(getJ1Angle()) - J2_LENGTH * Math.cos(getJ2Angle()); - return VecBuilder.fill(x2, y2); - } + // General nomenclature: + // J1 refers to the first (shoulder) joint, as well as the first link of the arm. + // J2 refers to the second (elbow) joint, as well as the second link of the arm. + + private static final double J1_INITIAL_ANGLE = + Math.toRadians(202.); // radians, relative to "down" + private static final double J2_INITIAL_ANGLE = + Math.toRadians(32.); // radians, relative to "down" + + // Joint limits + private static final double J1_MIN = Math.toRadians(90.); // radians, relative to "down" + private static final double J1_MAX = Math.toRadians(205.); // radians, relative to "down" + private static final double J2_MIN = Math.toRadians(-170); // radians, relative to J1 + private static final double J2_MAX = Math.toRadians(170); // radians, relative to J1 + + // Physical characterisics of the arm links. + // NOTE(rcahoon, 2023-02-09): We could determine these from CAD data, but using + // guessed/approximate values for now. + private static final double J1_LENGTH = 1; // m + private static final double J2_LENGTH = 1; // m + private static final double J1_MASS = 2; // kg + private static final double J2_MASS = 5; // kg + private static final double J1_FIRST_MOMENT_OF_MASS = + J1_MASS * J1_LENGTH; // kg m, with the origin/axis at the first joint + private static final double J1_SECOND_MOMENT_OF_MASS = + J1_MASS * J1_LENGTH + * J1_LENGTH; // kg m^2, with the origin/axis at the first joint - aka moment of + // inertia + private static final double J2_FIRST_MOMENT_OF_MASS = + J2_MASS * J2_LENGTH; // kg m, with the origin/axis at the second joint + private static final double J2_SECOND_MOMENT_OF_MASS = + J2_MASS * J2_LENGTH + * J2_LENGTH; // kg m^2, with the origin/axis at the second joint - aka moment of + // inertia + + private static final double J1_GEAR_RATIO = 4. * 4. * 3. * (58. / 14.); + private static final double J2_GEAR_RATIO = 4. * 4. * 3. * (58. / 14.); + + // Simulated CAN Bus IDs for the two motors. + private static final int J1_CAN_CHANNEL = 98; + private static final int J2_CAN_CHANNEL = 99; + + private DCMotor j1Motor = DCMotor.makeNeo("ArmJoint1"); + private DCMotor j2Motor = DCMotor.makeNeo("ArmJoint2"); + + private CanMotorController j1Controller = new CanMotorController(J1_CAN_CHANNEL, j1Motor); + private CanMotorController j2Controller = new CanMotorController(J2_CAN_CHANNEL, j2Motor); + + private Gears j1Gears = new Gears(J1_GEAR_RATIO, j1Motor); + private Gears j2Gears = new Gears(J2_GEAR_RATIO, j2Motor); + + private Matrix state = + VecBuilder.fill( + J1_INITIAL_ANGLE, // j1 angle, radians, relative to "down" + J2_INITIAL_ANGLE, // j2 angle, radians, relative to "down" + 0, // j1 velocity, radians/sec + 0 // j2 velocity, radians/sec + ); + + // Derivation of the equations of motion via Lagrangian mechanics. + // This system is often referred to as a "double pendulum", and is a commonly studied problem, + // so literature is available. See this series of videos for an explanation: + // https://youtu.be/tc2ah-KnDXw, https://youtu.be/eBg8gof1RBg, https://youtu.be/QE1_H2vtHLU + // The formulation given below is slightly more generalized to allow arbitrary mass + // distributions along the arm links. + // + // Kinematics: + // x1 = J1_FIRST_MOMENT_OF_MASS / J1_MASS * sin(theta1) + // y1 = -J1_FIRST_MOMENT_OF_MASS / J1_MASS * cos(theta1) + // x2 = J1_LENGTH * sin(theta1) + J2_FIRST_MOMENT_OF_MASS / J2_MASS * sin(theta2) + // y2 = -J1_LENGTH * cos(theta1) - J2_FIRST_MOMENT_OF_MASS / J2_MASS * cos(theta2) + // + // Lagrangian: + // L = T - V + // = 1/2 * J1_MASS * (x1'^2 + y1'^2) + // + 1/2 * (J1_SECOND_MOMENT_OF_MASS - J1_FIRST_MOMENT_OF_MASS^2 / J1_MASS) * theta1'^2 + // + 1/2 * J2_MASS * (x2'^2 + y2'^2) + // + 1/2 * (J2_SECOND_MOMENT_OF_MASS - J2_FIRST_MOMENT_OF_MASS^2 / J2_MASS) * theta2'^2 + // - J1_MASS * y1 * PhysicalConstants.GRAVITY_ACCELERATION + // - J2_MASS * y2 * PhysicalConstants.GRAVITY_ACCELERATION + // = 1/2 * J1_SECOND_MOMENT_OF_MASS * theta1'^2 + // + 1/2 * J2_MASS * ((J1_LENGTH * cos(theta1) * theta1' + J2_FIRST_MOMENT_OF_MASS / J2_MASS + // * cos(theta2) * theta2')^2 + (J1_LENGTH * sin(theta1) * theta1' + J2_FIRST_MOMENT_OF_MASS / + // J2_MASS * sin(theta2) * theta2')^2) + // + 1/2 * (J2_SECOND_MOMENT_OF_MASS - J2_FIRST_MOMENT_OF_MASS^2 / J2_MASS) * theta2'^2 + // + J1_FIRST_MOMENT_OF_MASS * cos(theta1) * PhysicalConstants.GRAVITY_ACCELERATION + // + (J2_MASS * J1_LENGTH * cos(theta1) + J2_FIRST_MOMENT_OF_MASS * cos(theta2)) * + // PhysicalConstants.GRAVITY_ACCELERATION + // = 1/2 * J1_SECOND_MOMENT_OF_MASS * theta1'^2 + // + 1/2 * (J2_MASS * J1_LENGTH^2 * theta1'^2 + 2 * J1_LENGTH * cos(theta1 - theta2) * + // theta1' * J2_FIRST_MOMENT_OF_MASS * theta2' + J2_FIRST_MOMENT_OF_MASS^2 / J2_MASS * + // theta2'^2) + // + 1/2 * (J2_SECOND_MOMENT_OF_MASS - J2_FIRST_MOMENT_OF_MASS^2 / J2_MASS) * theta2'^2 + // + J1_FIRST_MOMENT_OF_MASS * cos(theta1) * PhysicalConstants.GRAVITY_ACCELERATION + // + (J2_MASS * J1_LENGTH * cos(theta1) + J2_FIRST_MOMENT_OF_MASS * cos(theta2)) * + // PhysicalConstants.GRAVITY_ACCELERATION + // = 1/2 * (J1_SECOND_MOMENT_OF_MASS + J2_MASS * J1_LENGTH^2) * theta1'^2 + // + 1/2 * J2_SECOND_MOMENT_OF_MASS * theta2'^2 + // + J2_FIRST_MOMENT_OF_MASS * J1_LENGTH * cos(theta1 - theta2) * theta1' * theta2' + // + (J1_FIRST_MOMENT_OF_MASS + J2_MASS * J1_LENGTH) * + // PhysicalConstants.GRAVITY_ACCELERATION * cos(theta1) + // + J2_FIRST_MOMENT_OF_MASS * PhysicalConstants.GRAVITY_ACCELERATION * cos(theta2) + // + // Define some constants to simplify the following equations + // I1 = J1_SECOND_MOMENT_OF_MASS + J2_MASS * J1_LENGTH^2 + // I2 = J2_SECOND_MOMENT_OF_MASS + // Ix = J2_FIRST_MOMENT_OF_MASS * J1_LENGTH + // g1 = (J1_FIRST_MOMENT_OF_MASS + J2_MASS * J1_LENGTH) * PhysicalConstants.GRAVITY_ACCELERATION + // g2 = J2_FIRST_MOMENT_OF_MASS * PhysicalConstants.GRAVITY_ACCELERATION + // + // L = 1/2 * I1 * theta1'^2 + 1/2 * I2 * theta2'^2 + Ix * cos(theta1 - theta2) * theta1' * + // theta2' + g1 * cos(theta1) + g2 * cos(theta2) + // + // Euler-Lagrangian: + // d/dt [Partial]L/[Partial]theta' - [Partial]L/[Partial]theta = tau + // + // d/dt [Partial]L/[Partial]theta1' - [Partial]L/[Partial]theta1 = I1 * theta1'' + Ix * + // cos(theta1 - theta2) * theta2'' - Ix * sin(theta1 - theta2) * (theta1' - theta2') * theta2' + // + Ix * sin(theta1 - theta2) * theta1' * theta2' + g1 * + // sin(theta1) + // = I1 * theta1'' + Ix * cos(theta1 - theta2) * theta2'' + Ix * + // sin(theta1 - theta2) * theta2'^2 + g1 * sin(theta1) + // = tau1 + // + // d/dt [Partial]L/[Partial]theta2' - [Partial]L/[Partial]theta2 = I2 * theta2'' + Ix * + // cos(theta1 - theta2) * theta1'' - Ix * sin(theta1 - theta2) * (theta1' - theta2') * theta1' + // - Ix * sin(theta1 - theta2) * theta1' * theta2' + g2 * + // sin(theta2)) + // = I2 * theta2'' + Ix * cos(theta1 - theta2) * theta1'' - Ix * + // sin(theta1 - theta2) * theta1'^2 + g2 * sin(theta2) + // = tau2 + // + // Rewrite as a matrix equation: + // M * theta'' + G = tau + // theta'' = M^-1 * (tau - G) + // + // M = [ I1, Ix * cos(theta1 - theta2) ] + // = [ Ix * cos(theta1 - theta2), I2 ] + // G = [ Ix * sin(theta1 - theta2) * theta1' * theta2' + g1 * sin(theta1) ] + // [ -Ix * sin(theta1 - theta2) * theta1' * theta2' + g2 * sin(theta2) ] + + public DoubleJointedArm(ElectricalSystem electricalSystem) { + electricalSystem.addDevice(j1Controller); + electricalSystem.addDevice(j2Controller); + } + + /** + * Advance the simulation model by the given amount of time. + */ + public void step(double dt) { + final MechanicalAngularDevice.Output j1Drive = + j1Gears.step(new MechanicalAngularDevice.Input(getJ1Velocity()), dt); + final MechanicalAngularDevice.Output j2Drive = + j2Gears.step(new MechanicalAngularDevice.Input(getJ2Velocity()), dt); + + double j1Limit = 0.; + if (state.get(0, 0) <= J1_MIN) { + state.set(0, 0, J1_MIN); + state.set(2, 0, 0.0); + j1Limit = 1.; + } + if (state.get(0, 0) >= J1_MAX) { + state.set(0, 0, J1_MAX); + state.set(2, 0, 0.0); + j1Limit = -1.; + } + double j2Limit = 0.; + if (state.get(1, 0) - state.get(0, 0) <= J2_MIN) { + state.set(1, 0, J2_MIN + state.get(0, 0)); + j2Limit = 1.; + } + if (state.get(1, 0) - state.get(0, 0) >= J2_MAX) { + state.set(1, 0, J2_MAX + state.get(0, 0)); + j2Limit = -1.; + } + + if (state.get(2, 0) * j1Limit < 0) { + state.set(2, 0, 0.0); + } + if (state.get(3, 0) * j2Limit < 0) { + state.set(3, 0, 0.0); + } + + final var u = VecBuilder.fill(j1Drive.torque, j2Drive.torque, j1Limit, j2Limit); + final var stateDot = S(state, u); + state = state.plus(stateDot.times(dt)); + + j1Controller.setSensorPosition(state.get(0, 0)); + j2Controller.setSensorPosition(state.get(1, 0)); + j1Controller.setSensorVelocity(state.get(2, 0)); + j2Controller.setSensorVelocity(state.get(3, 0)); + } + + /** + * State space model X'(t) = S(X(t), u(t)) of the system + */ + // Defined as a static method to make sure we don't accidentally refer to any of the state + // stored in class member variables when we should be referring to the state the integrator + // gave us. + private static Vector S(Matrix state, Matrix u) { + Vector motorTorques = VecBuilder.fill(u.get(0, 0), u.get(1, 0)); + Vector jointLimits = VecBuilder.fill(u.get(2, 0), u.get(3, 0)); + Vector velocity = VecBuilder.fill(state.get(2, 0), state.get(3, 0)); + + final double Mx = + J2_FIRST_MOMENT_OF_MASS * J1_LENGTH * Math.cos(state.get(0, 0) - state.get(1, 0)); + var M = + Matrix.mat(N2.instance, N2.instance) + .fill( + J1_SECOND_MOMENT_OF_MASS + J2_MASS * J1_LENGTH * J1_LENGTH, + Mx, + Mx, + J2_SECOND_MOMENT_OF_MASS); + + final double C = + J2_FIRST_MOMENT_OF_MASS + * J1_LENGTH + * Math.sin(state.get(0, 0) - state.get(1, 0)) + * state.get(2, 0) + * state.get(3, 0); + final var G = + VecBuilder.fill( + C + + (J1_FIRST_MOMENT_OF_MASS + J2_MASS * J1_LENGTH) + * PhysicalConstants.GRAVITY_ACCELERATION + * Math.sin(state.get(0, 0)), + -C + + J2_FIRST_MOMENT_OF_MASS + * PhysicalConstants.GRAVITY_ACCELERATION + * Math.sin(state.get(1, 0))); + var T = motorTorques.minus(G); + + if (T.get(0, 0) * jointLimits.get(0, 0) < 0) { + M.set(0, 0, 1.0); + M.set(1, 0, 0.0); + M.set(0, 1, 0.0); + T.set(0, 0, 0.0); + } + if (T.get(1, 0) * jointLimits.get(1, 0) < 0) { + M.set(1, 1, 1.0); + M.set(1, 0, 0.0); + M.set(0, 1, 0.0); + T.set(1, 0, 0.0); + } + + final var acceleration = M.inv().times(T); + + return VecBuilder.fill( + velocity.get(0, 0), + velocity.get(1, 0), + acceleration.get(0, 0), + acceleration.get(1, 0)); + } + + /** + * Return the angle of the first joint. + */ + public double getJ1Angle() { + return state.get(0, 0); + } + + /** + * Return the angle of the second joint. + */ + public double getJ2Angle() { + return state.get(1, 0); + } + + /** + * Return the velocity of the first joint. + */ + public double getJ1Velocity() { + return state.get(2, 0); + } + + /** + * Return the velocity of the second joint. + */ + public double getJ2Velocity() { + return state.get(3, 0); + } + + /** + * Calculate the total kinetic and potential energy in the arm. + * + * If no energy is added or removed (e.g. by motors or friction), this quantity should remain + * constant (i.e. conservation of energy), so plotting it over time makes a good sanity check of + * the model. + */ + public double getEnergy() { + double y1 = -J1_FIRST_MOMENT_OF_MASS / J1_MASS * Math.cos(getJ1Angle()); + double y2 = + -J1_LENGTH * Math.cos(getJ1Angle()) + - J2_FIRST_MOMENT_OF_MASS / J2_MASS * Math.cos(getJ2Angle()); + double dx1 = J1_FIRST_MOMENT_OF_MASS / J1_MASS * Math.cos(getJ1Angle()) * getJ1Velocity(); + double dy1 = J1_FIRST_MOMENT_OF_MASS / J1_MASS * Math.sin(getJ1Angle()) * getJ1Velocity(); + double dx2 = + J1_LENGTH * Math.cos(getJ1Angle()) * getJ1Velocity() + + J2_FIRST_MOMENT_OF_MASS + / J2_MASS + * Math.cos(getJ2Angle()) + * getJ2Velocity(); + double dy2 = + J1_LENGTH * Math.sin(getJ1Angle()) * getJ1Velocity() + + J2_FIRST_MOMENT_OF_MASS + / J2_MASS + * Math.sin(getJ2Angle()) + * getJ2Velocity(); + + double energy = + 0.5 * J1_MASS * (dx1 * dx1 + dy1 * dy1) + + 0.5 + * (J1_SECOND_MOMENT_OF_MASS + - J1_FIRST_MOMENT_OF_MASS + * J1_FIRST_MOMENT_OF_MASS + / J1_MASS) + * getJ1Velocity() + * getJ1Velocity() + + 0.5 * J2_MASS * (dx2 * dx2 + dy2 * dy2) + + 0.5 + * (J2_SECOND_MOMENT_OF_MASS + - J2_FIRST_MOMENT_OF_MASS + * J2_FIRST_MOMENT_OF_MASS + / J2_MASS) + * getJ2Velocity() + * getJ2Velocity() + + J1_MASS * y1 * PhysicalConstants.GRAVITY_ACCELERATION + + J2_MASS * y2 * PhysicalConstants.GRAVITY_ACCELERATION; + return energy; + } + + /** + * Calculuate the cartesian coordinates of the end of the first link. + */ + public Vector getJ1Position() { + double x1 = J1_LENGTH * Math.sin(getJ1Angle()); + double y1 = -J1_LENGTH * Math.cos(getJ1Angle()); + return VecBuilder.fill(x1, y1); + } + + /** + * Calculuate the cartesian coordinates of the end of the second link. + */ + public Vector getJ2Position() { + double x2 = J1_LENGTH * Math.sin(getJ1Angle()) + J2_LENGTH * Math.sin(getJ2Angle()); + double y2 = -J1_LENGTH * Math.cos(getJ1Angle()) - J2_LENGTH * Math.cos(getJ2Angle()); + return VecBuilder.fill(x2, y2); + } } diff --git a/src/main/java/com/team766/simulator/mechanisms/WestCoastDrive.java b/src/main/java/com/team766/simulator/mechanisms/WestCoastDrive.java index 8f96fbbb..458281b3 100644 --- a/src/main/java/com/team766/simulator/mechanisms/WestCoastDrive.java +++ b/src/main/java/com/team766/simulator/mechanisms/WestCoastDrive.java @@ -1,10 +1,5 @@ package com.team766.simulator.mechanisms; -import org.apache.commons.math3.geometry.euclidean.threed.Rotation; -import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; -import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; -import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; - import com.team766.simulator.ElectricalSystem; import com.team766.simulator.Parameters; import com.team766.simulator.PhysicalConstants; @@ -12,136 +7,166 @@ import com.team766.simulator.elements.DCMotor; import com.team766.simulator.elements.DriveBase; import com.team766.simulator.elements.Gears; -import com.team766.simulator.elements.PwmMotorController; import com.team766.simulator.elements.MotorController; +import com.team766.simulator.elements.PwmMotorController; import com.team766.simulator.elements.Wheel; import com.team766.simulator.interfaces.MechanicalDevice; +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; +import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; public class WestCoastDrive extends DriveBase { - private DCMotor leftMotor = DCMotor.makeCIM("DriveLeftMotor"); - private DCMotor rightMotor = DCMotor.makeCIM("DriveRightMotor"); - - private MotorController leftController = new PwmMotorController(6, leftMotor); - private MotorController rightController = new PwmMotorController(4, rightMotor); - - private Gears leftGears = new Gears(Parameters.DRIVE_GEAR_RATIO, leftMotor); - private Gears rightGears = new Gears(Parameters.DRIVE_GEAR_RATIO, rightMotor); - - private Wheel leftWheels = new Wheel(Parameters.DRIVE_WHEEL_DIAMETER, leftGears); - private static final Vector3D LEFT_WHEEL_POSITION = new Vector3D(0., 0.3302, 0.); - private Wheel rightWheels = new Wheel(Parameters.DRIVE_WHEEL_DIAMETER, rightGears); - private static final Vector3D RIGHT_WHEEL_POSITION = new Vector3D(0., -0.3302, 0.); - private static final double WHEEL_BASE = 0.585; - private static final double ENCODER_TICKS_PER_METER = Parameters.ENCODER_TICKS_PER_REVOLUTION / (Parameters.DRIVE_WHEEL_DIAMETER * Math.PI); - - private Vector3D robotPosition = Vector3D.ZERO; - private Rotation robotRotation = Rotation.IDENTITY; - private Vector3D linearVelocity = Vector3D.ZERO; - private Vector3D angularVelocity = Vector3D.ZERO; - private Vector3D linearAcceleration = Vector3D.ZERO; - private Vector3D angularAcceleration = Vector3D.ZERO; - - private double leftEncoderResidual = 0; - private double rightEncoderResidual = 0; - - public WestCoastDrive(final ElectricalSystem electricalSystem) { - electricalSystem.addDevice(leftController); - electricalSystem.addDevice(rightController); - } - - private static double softSignum(double x) { - x /= 0.01; - if (x > 1.0) { - x = 1.0; - } else if (x < -1.0) { - x = -1.0; - } - return x; - } - - public void step(double dt) { - Vector3D wheelForce; - Vector3D netForce = Vector3D.ZERO; - Vector3D netTorque = Vector3D.ZERO; - MechanicalDevice.Input leftWheelInput = new MechanicalDevice.Input( - LEFT_WHEEL_POSITION, - linearVelocity.scalarMultiply(-1.)); - wheelForce = leftWheels.step(leftWheelInput, dt).force.scalarMultiply(-1.0); - netForce = netForce.add(wheelForce); - netTorque = netTorque.add(Vector3D.crossProduct(LEFT_WHEEL_POSITION, wheelForce)); - MechanicalDevice.Input rightWheelInput = new MechanicalDevice.Input( - RIGHT_WHEEL_POSITION, - linearVelocity.scalarMultiply(-1.)); - wheelForce = rightWheels.step(rightWheelInput, dt).force.scalarMultiply(-1.0); - netForce = netForce.add(wheelForce); - netTorque = netTorque.add(Vector3D.crossProduct(RIGHT_WHEEL_POSITION, wheelForce)); - - Vector3D ego_velocity = robotRotation.applyInverseTo(linearVelocity); - - double rateLeft = ENCODER_TICKS_PER_METER * (ego_velocity.getX() - angularVelocity.getZ() * LEFT_WHEEL_POSITION.getNorm()); - double rateRight = ENCODER_TICKS_PER_METER * (ego_velocity.getX() + angularVelocity.getZ() * RIGHT_WHEEL_POSITION.getNorm()); - leftEncoderResidual += rateLeft * dt; - rightEncoderResidual += rateRight * dt; - ProgramInterface.encoderChannels[0].distance += (long)leftEncoderResidual; - ProgramInterface.encoderChannels[0].rate = rateLeft; - ProgramInterface.encoderChannels[2].distance += (long) rightEncoderResidual; - ProgramInterface.encoderChannels[2].rate = rateRight; - leftEncoderResidual %= 1; - rightEncoderResidual %= 1; - - ProgramInterface.gyro.angle = Math.toDegrees(robotRotation.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[2]); - ProgramInterface.gyro.rate = Math.toDegrees(angularVelocity.getZ()); - - Vector3D rollingResistance = new Vector3D(-softSignum(ego_velocity.getX()), 0.0, 0.0).scalarMultiply( - Parameters.ROBOT_MASS * PhysicalConstants.GRAVITY_ACCELERATION * Parameters.ROLLING_RESISTANCE); - netForce = netForce.add(rollingResistance); - - Vector3D transverseFriction = new Vector3D(0., -softSignum(ego_velocity.getY()), 0.).scalarMultiply( - Parameters.WHEEL_COEFFICIENT_OF_FRICTION * Parameters.ROBOT_MASS * PhysicalConstants.GRAVITY_ACCELERATION); - netForce = netForce.add(transverseFriction); - - double maxFriction = Parameters.WHEEL_COEFFICIENT_OF_FRICTION * Parameters.ROBOT_MASS * PhysicalConstants.GRAVITY_ACCELERATION * Parameters.TURNING_RESISTANCE_FACTOR; - netTorque = netTorque.add( - new Vector3D(0, 0, -softSignum(angularVelocity.getZ())).scalarMultiply( - maxFriction * WHEEL_BASE / 2)); - - linearAcceleration = robotRotation.applyTo(netForce).scalarMultiply(1.0 / Parameters.ROBOT_MASS); - linearVelocity = linearVelocity.add(linearAcceleration.scalarMultiply(dt)); - robotPosition = robotPosition.add(linearVelocity.scalarMultiply(dt)); - - angularAcceleration = netTorque.scalarMultiply(1.0 / Parameters.ROBOT_MOMENT_OF_INERTIA); - angularVelocity = angularVelocity.add(angularAcceleration.scalarMultiply(dt)); - Vector3D angularDelta = angularVelocity.scalarMultiply(dt); - robotRotation = robotRotation.compose( - new Rotation(RotationOrder.XYZ, - RotationConvention.VECTOR_OPERATOR, - angularDelta.getX(), - angularDelta.getY(), - angularDelta.getZ()), - RotationConvention.VECTOR_OPERATOR); - } - - public Vector3D getPosition() { - return robotPosition; - } - - public Rotation getRotation() { - return robotRotation; - } - - public Vector3D getLinearVelocity() { - return linearVelocity; - } - - public Vector3D getAngularVelocity() { - return angularVelocity; - } - - public Vector3D getLinearAcceleration() { - return linearAcceleration; - } - - public Vector3D getAngularAcceleration() { - return angularAcceleration; - } + private DCMotor leftMotor = DCMotor.makeCIM("DriveLeftMotor"); + private DCMotor rightMotor = DCMotor.makeCIM("DriveRightMotor"); + + private MotorController leftController = new PwmMotorController(6, leftMotor); + private MotorController rightController = new PwmMotorController(4, rightMotor); + + private Gears leftGears = new Gears(Parameters.DRIVE_GEAR_RATIO, leftMotor); + private Gears rightGears = new Gears(Parameters.DRIVE_GEAR_RATIO, rightMotor); + + private Wheel leftWheels = new Wheel(Parameters.DRIVE_WHEEL_DIAMETER, leftGears); + private static final Vector3D LEFT_WHEEL_POSITION = new Vector3D(0., 0.3302, 0.); + private Wheel rightWheels = new Wheel(Parameters.DRIVE_WHEEL_DIAMETER, rightGears); + private static final Vector3D RIGHT_WHEEL_POSITION = new Vector3D(0., -0.3302, 0.); + private static final double WHEEL_BASE = 0.585; + private static final double ENCODER_TICKS_PER_METER = + Parameters.ENCODER_TICKS_PER_REVOLUTION / (Parameters.DRIVE_WHEEL_DIAMETER * Math.PI); + + private Vector3D robotPosition = Vector3D.ZERO; + private Rotation robotRotation = Rotation.IDENTITY; + private Vector3D linearVelocity = Vector3D.ZERO; + private Vector3D angularVelocity = Vector3D.ZERO; + private Vector3D linearAcceleration = Vector3D.ZERO; + private Vector3D angularAcceleration = Vector3D.ZERO; + + private double leftEncoderResidual = 0; + private double rightEncoderResidual = 0; + + public WestCoastDrive(final ElectricalSystem electricalSystem) { + electricalSystem.addDevice(leftController); + electricalSystem.addDevice(rightController); + } + + private static double softSignum(double x) { + x /= 0.01; + if (x > 1.0) { + x = 1.0; + } else if (x < -1.0) { + x = -1.0; + } + return x; + } + + public void step(double dt) { + Vector3D wheelForce; + Vector3D netForce = Vector3D.ZERO; + Vector3D netTorque = Vector3D.ZERO; + MechanicalDevice.Input leftWheelInput = + new MechanicalDevice.Input(LEFT_WHEEL_POSITION, linearVelocity.scalarMultiply(-1.)); + wheelForce = leftWheels.step(leftWheelInput, dt).force.scalarMultiply(-1.0); + netForce = netForce.add(wheelForce); + netTorque = netTorque.add(Vector3D.crossProduct(LEFT_WHEEL_POSITION, wheelForce)); + MechanicalDevice.Input rightWheelInput = + new MechanicalDevice.Input( + RIGHT_WHEEL_POSITION, linearVelocity.scalarMultiply(-1.)); + wheelForce = rightWheels.step(rightWheelInput, dt).force.scalarMultiply(-1.0); + netForce = netForce.add(wheelForce); + netTorque = netTorque.add(Vector3D.crossProduct(RIGHT_WHEEL_POSITION, wheelForce)); + + Vector3D ego_velocity = robotRotation.applyInverseTo(linearVelocity); + + double rateLeft = + ENCODER_TICKS_PER_METER + * (ego_velocity.getX() + - angularVelocity.getZ() * LEFT_WHEEL_POSITION.getNorm()); + double rateRight = + ENCODER_TICKS_PER_METER + * (ego_velocity.getX() + + angularVelocity.getZ() * RIGHT_WHEEL_POSITION.getNorm()); + leftEncoderResidual += rateLeft * dt; + rightEncoderResidual += rateRight * dt; + ProgramInterface.encoderChannels[0].distance += (long) leftEncoderResidual; + ProgramInterface.encoderChannels[0].rate = rateLeft; + ProgramInterface.encoderChannels[2].distance += (long) rightEncoderResidual; + ProgramInterface.encoderChannels[2].rate = rateRight; + leftEncoderResidual %= 1; + rightEncoderResidual %= 1; + + ProgramInterface.gyro.angle = + Math.toDegrees( + robotRotation + .getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[ + 2]); + ProgramInterface.gyro.rate = Math.toDegrees(angularVelocity.getZ()); + + Vector3D rollingResistance = + new Vector3D(-softSignum(ego_velocity.getX()), 0.0, 0.0) + .scalarMultiply( + Parameters.ROBOT_MASS + * PhysicalConstants.GRAVITY_ACCELERATION + * Parameters.ROLLING_RESISTANCE); + netForce = netForce.add(rollingResistance); + + Vector3D transverseFriction = + new Vector3D(0., -softSignum(ego_velocity.getY()), 0.) + .scalarMultiply( + Parameters.WHEEL_COEFFICIENT_OF_FRICTION + * Parameters.ROBOT_MASS + * PhysicalConstants.GRAVITY_ACCELERATION); + netForce = netForce.add(transverseFriction); + + double maxFriction = + Parameters.WHEEL_COEFFICIENT_OF_FRICTION + * Parameters.ROBOT_MASS + * PhysicalConstants.GRAVITY_ACCELERATION + * Parameters.TURNING_RESISTANCE_FACTOR; + netTorque = + netTorque.add( + new Vector3D(0, 0, -softSignum(angularVelocity.getZ())) + .scalarMultiply(maxFriction * WHEEL_BASE / 2)); + + linearAcceleration = + robotRotation.applyTo(netForce).scalarMultiply(1.0 / Parameters.ROBOT_MASS); + linearVelocity = linearVelocity.add(linearAcceleration.scalarMultiply(dt)); + robotPosition = robotPosition.add(linearVelocity.scalarMultiply(dt)); + + angularAcceleration = netTorque.scalarMultiply(1.0 / Parameters.ROBOT_MOMENT_OF_INERTIA); + angularVelocity = angularVelocity.add(angularAcceleration.scalarMultiply(dt)); + Vector3D angularDelta = angularVelocity.scalarMultiply(dt); + robotRotation = + robotRotation.compose( + new Rotation( + RotationOrder.XYZ, + RotationConvention.VECTOR_OPERATOR, + angularDelta.getX(), + angularDelta.getY(), + angularDelta.getZ()), + RotationConvention.VECTOR_OPERATOR); + } + + public Vector3D getPosition() { + return robotPosition; + } + + public Rotation getRotation() { + return robotRotation; + } + + public Vector3D getLinearVelocity() { + return linearVelocity; + } + + public Vector3D getAngularVelocity() { + return angularVelocity; + } + + public Vector3D getLinearAcceleration() { + return linearAcceleration; + } + + public Vector3D getAngularAcceleration() { + return angularAcceleration; + } } diff --git a/src/main/java/com/team766/simulator/ui/ArmTrajectory.java b/src/main/java/com/team766/simulator/ui/ArmTrajectory.java index 7ed2d893..3d7d52fc 100644 --- a/src/main/java/com/team766/simulator/ui/ArmTrajectory.java +++ b/src/main/java/com/team766/simulator/ui/ArmTrajectory.java @@ -1,103 +1,102 @@ package com.team766.simulator.ui; +import de.erichseifert.gral.data.DataSeries; +import de.erichseifert.gral.data.DataTable; +import de.erichseifert.gral.plots.XYPlot; +import de.erichseifert.gral.plots.lines.DefaultLineRenderer2D; +import de.erichseifert.gral.plots.lines.LineRenderer; +import de.erichseifert.gral.ui.InteractivePanel; import java.awt.BasicStroke; import java.awt.BorderLayout; import java.awt.Color; import java.awt.Graphics; import java.awt.Graphics2D; import java.awt.Point; - import javax.swing.JPanel; -import de.erichseifert.gral.data.DataSeries; -import de.erichseifert.gral.data.DataTable; -import de.erichseifert.gral.plots.XYPlot; -import de.erichseifert.gral.plots.lines.DefaultLineRenderer2D; -import de.erichseifert.gral.plots.lines.LineRenderer; -import de.erichseifert.gral.ui.InteractivePanel; - /** * UI Window that displays the movements that the robot's arm made. */ public class ArmTrajectory extends JPanel { - private final XYPlot plot; - private final DataTable data; - // The current playback time. - private double time; + private final XYPlot plot; + private final DataTable data; + // The current playback time. + private double time; + + /** + * @param series The time series data defining the robot's trajectory. + * Each element should be an array with 5 values: + * [0]: Time + * [1]: X Position of the end of the first link + * [2]: Y Position of the end of the first link + * [3]: X Position of the end of the second link + * [4]: Y Position of the end of the second link + * @param playbackTimer The timer (shared between windows) which controls playback time. + */ + public ArmTrajectory(Iterable series, PlaybackTimer playbackTimer) { + // Create an X-Y plot to display the trajectories + data = PlotUtils.makeDataTable(series); + var firstTrajectory = new DataSeries("First link", data, 1, 2); + var secondTrajectory = new DataSeries("Second link", data, 3, 4); + plot = new XYPlot(firstTrajectory, secondTrajectory); + plot.getAxis(XYPlot.AXIS_X).setRange(-2.5, 2.5); + plot.getAxis(XYPlot.AXIS_Y).setRange(-2.5, 2.5); - /** - * @param series The time series data defining the robot's trajectory. - * Each element should be an array with 5 values: - * [0]: Time - * [1]: X Position of the end of the first link - * [2]: Y Position of the end of the first link - * [3]: X Position of the end of the second link - * [4]: Y Position of the end of the second link - * @param playbackTimer The timer (shared between windows) which controls playback time. - */ - public ArmTrajectory(Iterable series, PlaybackTimer playbackTimer) { - // Create an X-Y plot to display the trajectories - data = PlotUtils.makeDataTable(series); - var firstTrajectory = new DataSeries("First link", data, 1, 2); - var secondTrajectory = new DataSeries("Second link", data, 3, 4); - plot = new XYPlot(firstTrajectory, secondTrajectory); - plot.getAxis(XYPlot.AXIS_X).setRange(-2.5, 2.5); - plot.getAxis(XYPlot.AXIS_Y).setRange(-2.5, 2.5); + // Show the trajectories for the arm's links using different colors. + { + LineRenderer lines = new DefaultLineRenderer2D(); + lines.setColor(new Color(0, 0, 128)); + plot.setLineRenderers(firstTrajectory, lines); + plot.setPointRenderers(firstTrajectory, null); + } + { + LineRenderer lines = new DefaultLineRenderer2D(); + lines.setColor(new Color(128, 0, 0)); + plot.setLineRenderers(secondTrajectory, lines); + plot.setPointRenderers(secondTrajectory, null); + } - // Show the trajectories for the arm's links using different colors. - { - LineRenderer lines = new DefaultLineRenderer2D(); - lines.setColor(new Color(0, 0, 128)); - plot.setLineRenderers(firstTrajectory, lines); - plot.setPointRenderers(firstTrajectory, null); - } - { - LineRenderer lines = new DefaultLineRenderer2D(); - lines.setColor(new Color(128, 0, 0)); - plot.setLineRenderers(secondTrajectory, lines); - plot.setPointRenderers(secondTrajectory, null); - } - - InteractivePanel panel = new InteractivePanel(plot); - setLayout(new BorderLayout()); - add(panel, BorderLayout.CENTER); + InteractivePanel panel = new InteractivePanel(plot); + setLayout(new BorderLayout()); + add(panel, BorderLayout.CENTER); - // Add the standard time slider and play/pause button. - add(new PlaybackControls(playbackTimer), BorderLayout.SOUTH); + // Add the standard time slider and play/pause button. + add(new PlaybackControls(playbackTimer), BorderLayout.SOUTH); - // Add the callback that will update this window when playback time progresses. - playbackTimer.addListener(event -> { - this.time = (Double)event.getNewValue(); - this.repaint(); - }); - } + // Add the callback that will update this window when playback time progresses. + playbackTimer.addListener( + event -> { + this.time = (Double) event.getNewValue(); + this.repaint(); + }); + } - /** - * Draw overlays on the plot - */ - @Override - public void paint(Graphics g) { - super.paint(g); + /** + * Draw overlays on the plot + */ + @Override + public void paint(Graphics g) { + super.paint(g); - Graphics2D g2d = (Graphics2D)g; + Graphics2D g2d = (Graphics2D) g; - int index = PlotUtils.findIndex(data.getColumn(0), time); + int index = PlotUtils.findIndex(data.getColumn(0), time); - double j1_x = (Double)data.get(1, index); - double j1_y = (Double)data.get(2, index); - double j2_x = (Double)data.get(3, index); - double j2_y = (Double)data.get(4, index); + double j1_x = (Double) data.get(1, index); + double j1_y = (Double) data.get(2, index); + double j2_x = (Double) data.get(3, index); + double j2_y = (Double) data.get(4, index); - // Show the current position of the arm by drawing a line for each of the links. + // Show the current position of the arm by drawing a line for each of the links. - Point j0_p = PlotUtils.getPixelCoords(plot, 0.0, 0.0); - Point j1_p = PlotUtils.getPixelCoords(plot, j1_x, j1_y); - Point j2_p = PlotUtils.getPixelCoords(plot, j2_x, j2_y); + Point j0_p = PlotUtils.getPixelCoords(plot, 0.0, 0.0); + Point j1_p = PlotUtils.getPixelCoords(plot, j1_x, j1_y); + Point j2_p = PlotUtils.getPixelCoords(plot, j2_x, j2_y); - g2d.setStroke(new BasicStroke(10)); - g2d.setColor(new Color(128, 128, 255)); - g2d.drawLine(j0_p.x, j0_p.y, j1_p.x, j1_p.y); - g2d.setColor(Color.red); - g2d.drawLine(j1_p.x, j1_p.y, j2_p.x, j2_p.y); - } -} \ No newline at end of file + g2d.setStroke(new BasicStroke(10)); + g2d.setColor(new Color(128, 128, 255)); + g2d.drawLine(j0_p.x, j0_p.y, j1_p.x, j1_p.y); + g2d.setColor(Color.red); + g2d.drawLine(j1_p.x, j1_p.y, j2_p.x, j2_p.y); + } +} diff --git a/src/main/java/com/team766/simulator/ui/MetricsPlot.java b/src/main/java/com/team766/simulator/ui/MetricsPlot.java index 9e7e324b..b98b0ea0 100644 --- a/src/main/java/com/team766/simulator/ui/MetricsPlot.java +++ b/src/main/java/com/team766/simulator/ui/MetricsPlot.java @@ -1,5 +1,13 @@ package com.team766.simulator.ui; +import com.team766.simulator.Metrics; +import de.erichseifert.gral.data.DataSeries; +import de.erichseifert.gral.data.DataSource; +import de.erichseifert.gral.data.DataTable; +import de.erichseifert.gral.plots.XYPlot; +import de.erichseifert.gral.plots.lines.DefaultLineRenderer2D; +import de.erichseifert.gral.plots.lines.LineRenderer; +import de.erichseifert.gral.ui.InteractivePanel; import java.awt.BasicStroke; import java.awt.BorderLayout; import java.awt.Color; @@ -12,171 +20,183 @@ import java.util.List; import javax.swing.JPanel; import javax.swing.SwingUtilities; -import com.team766.simulator.Metrics; -import de.erichseifert.gral.data.DataSeries; -import de.erichseifert.gral.data.DataSource; -import de.erichseifert.gral.data.DataTable; -import de.erichseifert.gral.plots.XYPlot; -import de.erichseifert.gral.plots.lines.DefaultLineRenderer2D; -import de.erichseifert.gral.plots.lines.LineRenderer; -import de.erichseifert.gral.ui.InteractivePanel; /** * UI Window that displays plots of time-series data. */ public class MetricsPlot extends JPanel { - // Color palette from http://www.mulinblog.com/a-color-palette-optimized-for-data-visualization/ - private static final String[] COLORS = { - "#4D4D4D", // gray - "#5DA5DA", // blue - "#FAA43A", // orange - "#60BD68", // green - "#F17CB0", // pink - "#B2912F", // brown - "#B276B2", // purple - "#DECF3F", // yellow - "#F15854", // red - }; - - private static class DataSeriesWithMutableName extends DataSeries { - public DataSeriesWithMutableName(String name, DataSource data, int... cols) { - super(name, data, cols); - } - - // Override to make this public - @Override - public void setName(String name) { - super.setName(name); - } - } - - private final XYPlot plot; - // The data store. Column 0 is time, the other columns are the metrics values. - private final DataTable data; - // A view into data for each of the series. - private final DataSeriesWithMutableName[] sources; - // A name for each of the series. - private final List labels; - // The current playback time. - private double time; - - /** - * @param metrics The time series data to display - * @param playbackTimer The timer (shared between windows) which controls playback time. - */ - public MetricsPlot(Metrics metrics, PlaybackTimer playbackTimer) { - // Create the plot - data = PlotUtils.makeDataTable(metrics.getMetrics()); - labels = metrics.getLabels(); - sources = new DataSeriesWithMutableName[labels.size()]; - for (int i = 0; i < labels.size(); ++i) { - sources[i] = new DataSeriesWithMutableName(labels.get(i), data, 0, i + 1); - } - plot = new XYPlot(sources); - - // Add a little margin on all sides of the plot for better readability. Also make sure - // that the axes are visible so we can see the scale of the data. - var xAxis = plot.getAxis(XYPlot.AXIS_X); - xAxis.setMin(-0.05 * xAxis.getRange()); - xAxis.setMax(xAxis.getMax().doubleValue() + 0.05 * xAxis.getRange()); - var yAxis = plot.getAxis(XYPlot.AXIS_Y); - yAxis.setMin(Math.min(yAxis.getMin().doubleValue() - 0.05 * yAxis.getRange(), -0.05 * yAxis.getRange())); - yAxis.setMax(Math.max(yAxis.getMax().doubleValue() + 0.05 * yAxis.getRange(), 0.05 * yAxis.getRange())); - - // Assign a different color to each data series. - int colorIndex = 0; - for (DataSource source : sources) { - LineRenderer lines = new DefaultLineRenderer2D(); - Color color = Color.decode(COLORS[colorIndex++ % COLORS.length]); - lines.setColor(color); - plot.setLineRenderers(source, lines); - plot.setPointRenderers(source, null); - } - - // Setup the legend so it's visible and placed on the top-right - updateLegend(); - plot.getLegend().setAlignmentX(1.0); - plot.getLegend().setAlignmentY(0.1); - plot.setLegendVisible(true); - - InteractivePanel panel = new InteractivePanel(plot); - setLayout(new BorderLayout()); - add(panel, BorderLayout.CENTER); - - // Remove listener installed by InteractivePanel that conflicts with our MouseListener. - for (var l : panel.getMouseListeners()) { - // We're looking to remove "MouseZoomListener", which is a private inner class, and Java - // doesn't seem to have its proper name stored in reflection (the stored name is "a"). - // We instead find it because it's the only one of the listeners which directly - // implements MouseWheelListener (note that other of the listeners extend MouseAdapter, - // meaning they indirectly implement MouseWheelListener, so we can't just use instanceof). - if (Arrays.asList(l.getClass().getInterfaces()).contains(MouseWheelListener.class)) { - panel.removeMouseListener(l); - } - } - // Add a mouse listener that will set playback time if the plot is double-clicked. - panel.addMouseListener(new MouseAdapter() { - @Override - public void mouseClicked(MouseEvent e) { - if (!SwingUtilities.isLeftMouseButton(e) || e.getClickCount() < 2) { - return; - } - - double x = e.getX() - plot.getPlotArea().getX(); - double selectedTime = plot.getAxisRenderer(XYPlot.AXIS_X).viewToWorld(plot.getAxis(XYPlot.AXIS_X), x, false).doubleValue(); - - playbackTimer.setTime(selectedTime); - - // final int index = PlotUtils.findIndex(data.getColumn(0), time); - // System.out.println("At time=" + selectedTime + ":"); - // for (int i = 0; i < labels.size(); ++i) { - // System.out.println(" " + labels.get(i) + ": " + data.get(i + 1, index)); - // } - // System.out.println(); - - e.consume(); - } - }); - - // Add the standard time slider and play/pause button. - add(new PlaybackControls(playbackTimer), BorderLayout.SOUTH); - - // Add the callback that will update this window when playback time progresses. - playbackTimer.addListener(event -> { - this.time = (Double)event.getNewValue(); - updateLegend(); - this.repaint(); - }); - } - - /** - * Update the legend so that it displays the current value for each of the series. - */ - private void updateLegend() { - final int index = PlotUtils.findIndex(data.getColumn(0), time); - final var legend = plot.getLegend(); - legend.clear(); - for (int i = 0; i < sources.length; ++i) { - sources[i].setName(String.format("%s: %.4f", labels.get(i), data.get(i + 1, index))); - legend.add(sources[i]); - } - } - - /** - * Draw overlays on the plot - */ - @Override - public void paint(Graphics g) { - super.paint(g); - - Graphics2D g2d = (Graphics2D)g; - - // Draw a dashed vertical line to trace the current time on the plot. - final int lineX = PlotUtils.getPixelCoords(plot, time, 0.0).x; - final double plotAreaTop = plot.getPlotArea().getY(); - final double plotAreaBottom = plotAreaTop + plot.getPlotArea().getHeight(); - g2d.setStroke(new BasicStroke(2, BasicStroke.CAP_SQUARE, BasicStroke.JOIN_MITER, 10.f, new float[]{10.0f}, 0.f)); - g2d.setColor(Color.black); - g2d.drawLine(lineX, (int)plotAreaTop, lineX, (int)plotAreaBottom); - } -} \ No newline at end of file + // Color palette from http://www.mulinblog.com/a-color-palette-optimized-for-data-visualization/ + private static final String[] COLORS = { + "#4D4D4D", // gray + "#5DA5DA", // blue + "#FAA43A", // orange + "#60BD68", // green + "#F17CB0", // pink + "#B2912F", // brown + "#B276B2", // purple + "#DECF3F", // yellow + "#F15854", // red + }; + + private static class DataSeriesWithMutableName extends DataSeries { + public DataSeriesWithMutableName(String name, DataSource data, int... cols) { + super(name, data, cols); + } + + // Override to make this public + @Override + public void setName(String name) { + super.setName(name); + } + } + + private final XYPlot plot; + // The data store. Column 0 is time, the other columns are the metrics values. + private final DataTable data; + // A view into data for each of the series. + private final DataSeriesWithMutableName[] sources; + // A name for each of the series. + private final List labels; + // The current playback time. + private double time; + + /** + * @param metrics The time series data to display + * @param playbackTimer The timer (shared between windows) which controls playback time. + */ + public MetricsPlot(Metrics metrics, PlaybackTimer playbackTimer) { + // Create the plot + data = PlotUtils.makeDataTable(metrics.getMetrics()); + labels = metrics.getLabels(); + sources = new DataSeriesWithMutableName[labels.size()]; + for (int i = 0; i < labels.size(); ++i) { + sources[i] = new DataSeriesWithMutableName(labels.get(i), data, 0, i + 1); + } + plot = new XYPlot(sources); + + // Add a little margin on all sides of the plot for better readability. Also make sure + // that the axes are visible so we can see the scale of the data. + var xAxis = plot.getAxis(XYPlot.AXIS_X); + xAxis.setMin(-0.05 * xAxis.getRange()); + xAxis.setMax(xAxis.getMax().doubleValue() + 0.05 * xAxis.getRange()); + var yAxis = plot.getAxis(XYPlot.AXIS_Y); + yAxis.setMin( + Math.min( + yAxis.getMin().doubleValue() - 0.05 * yAxis.getRange(), + -0.05 * yAxis.getRange())); + yAxis.setMax( + Math.max( + yAxis.getMax().doubleValue() + 0.05 * yAxis.getRange(), + 0.05 * yAxis.getRange())); + + // Assign a different color to each data series. + int colorIndex = 0; + for (DataSource source : sources) { + LineRenderer lines = new DefaultLineRenderer2D(); + Color color = Color.decode(COLORS[colorIndex++ % COLORS.length]); + lines.setColor(color); + plot.setLineRenderers(source, lines); + plot.setPointRenderers(source, null); + } + + // Setup the legend so it's visible and placed on the top-right + updateLegend(); + plot.getLegend().setAlignmentX(1.0); + plot.getLegend().setAlignmentY(0.1); + plot.setLegendVisible(true); + + InteractivePanel panel = new InteractivePanel(plot); + setLayout(new BorderLayout()); + add(panel, BorderLayout.CENTER); + + // Remove listener installed by InteractivePanel that conflicts with our MouseListener. + for (var l : panel.getMouseListeners()) { + // We're looking to remove "MouseZoomListener", which is a private inner class, and Java + // doesn't seem to have its proper name stored in reflection (the stored name is "a"). + // We instead find it because it's the only one of the listeners which directly + // implements MouseWheelListener (note that other of the listeners extend MouseAdapter, + // meaning they indirectly implement MouseWheelListener, so we can't just use + // instanceof). + if (Arrays.asList(l.getClass().getInterfaces()).contains(MouseWheelListener.class)) { + panel.removeMouseListener(l); + } + } + // Add a mouse listener that will set playback time if the plot is double-clicked. + panel.addMouseListener( + new MouseAdapter() { + @Override + public void mouseClicked(MouseEvent e) { + if (!SwingUtilities.isLeftMouseButton(e) || e.getClickCount() < 2) { + return; + } + + double x = e.getX() - plot.getPlotArea().getX(); + double selectedTime = + plot.getAxisRenderer(XYPlot.AXIS_X) + .viewToWorld(plot.getAxis(XYPlot.AXIS_X), x, false) + .doubleValue(); + + playbackTimer.setTime(selectedTime); + + // final int index = PlotUtils.findIndex(data.getColumn(0), time); + // System.out.println("At time=" + selectedTime + ":"); + // for (int i = 0; i < labels.size(); ++i) { + // System.out.println(" " + labels.get(i) + ": " + data.get(i + 1, + // index)); + // } + // System.out.println(); + + e.consume(); + } + }); + + // Add the standard time slider and play/pause button. + add(new PlaybackControls(playbackTimer), BorderLayout.SOUTH); + + // Add the callback that will update this window when playback time progresses. + playbackTimer.addListener( + event -> { + this.time = (Double) event.getNewValue(); + updateLegend(); + this.repaint(); + }); + } + + /** + * Update the legend so that it displays the current value for each of the series. + */ + private void updateLegend() { + final int index = PlotUtils.findIndex(data.getColumn(0), time); + final var legend = plot.getLegend(); + legend.clear(); + for (int i = 0; i < sources.length; ++i) { + sources[i].setName(String.format("%s: %.4f", labels.get(i), data.get(i + 1, index))); + legend.add(sources[i]); + } + } + + /** + * Draw overlays on the plot + */ + @Override + public void paint(Graphics g) { + super.paint(g); + + Graphics2D g2d = (Graphics2D) g; + + // Draw a dashed vertical line to trace the current time on the plot. + final int lineX = PlotUtils.getPixelCoords(plot, time, 0.0).x; + final double plotAreaTop = plot.getPlotArea().getY(); + final double plotAreaBottom = plotAreaTop + plot.getPlotArea().getHeight(); + g2d.setStroke( + new BasicStroke( + 2, + BasicStroke.CAP_SQUARE, + BasicStroke.JOIN_MITER, + 10.f, + new float[] {10.0f}, + 0.f)); + g2d.setColor(Color.black); + g2d.drawLine(lineX, (int) plotAreaTop, lineX, (int) plotAreaBottom); + } +} diff --git a/src/main/java/com/team766/simulator/ui/PlaybackControls.java b/src/main/java/com/team766/simulator/ui/PlaybackControls.java index 2c032c81..6cc96c46 100644 --- a/src/main/java/com/team766/simulator/ui/PlaybackControls.java +++ b/src/main/java/com/team766/simulator/ui/PlaybackControls.java @@ -15,56 +15,60 @@ * playback. */ public class PlaybackControls extends JPanel { - // How large one step of the slider is, in seconds. - private static final double SLIDER_RESOLUTION = 0.020; + // How large one step of the slider is, in seconds. + private static final double SLIDER_RESOLUTION = 0.020; - // There's a circular dependency between the PlaybackTimer and the slider: both have the ability - // to change time, and both are updated when the other changes time, triggering callbacks to - // listeners. - // This flag is set to true during the callback that is executed when the PlaybackTimer updates - // its time, so that when it updates the position of the slider, the slider's callback will not - // try to update timer, which would lead to an infinite loop of callbacks. - private boolean inTimerTick = false; + // There's a circular dependency between the PlaybackTimer and the slider: both have the ability + // to change time, and both are updated when the other changes time, triggering callbacks to + // listeners. + // This flag is set to true during the callback that is executed when the PlaybackTimer updates + // its time, so that when it updates the position of the slider, the slider's callback will not + // try to update timer, which would lead to an infinite loop of callbacks. + private boolean inTimerTick = false; - /** - * @param playbackTimer The timer (shared between windows) which controls playback time. - */ - public PlaybackControls(PlaybackTimer playbackTimer) { - setLayout(new BoxLayout(this, BoxLayout.LINE_AXIS)); + /** + * @param playbackTimer The timer (shared between windows) which controls playback time. + */ + public PlaybackControls(PlaybackTimer playbackTimer) { + setLayout(new BoxLayout(this, BoxLayout.LINE_AXIS)); - // Add a slider that shows the progression of time and also allows for seeking to a - // different time. - var slider = new JSlider(0, (int)Math.ceil(playbackTimer.endTime() / SLIDER_RESOLUTION)); - slider.setValue(0); - slider.addChangeListener(new ChangeListener() { - public void stateChanged(ChangeEvent e) { - if (inTimerTick) { - return; - } - playbackTimer.setTime(slider.getValue() * SLIDER_RESOLUTION); - } - }); - add(slider); + // Add a slider that shows the progression of time and also allows for seeking to a + // different time. + var slider = new JSlider(0, (int) Math.ceil(playbackTimer.endTime() / SLIDER_RESOLUTION)); + slider.setValue(0); + slider.addChangeListener( + new ChangeListener() { + public void stateChanged(ChangeEvent e) { + if (inTimerTick) { + return; + } + playbackTimer.setTime(slider.getValue() * SLIDER_RESOLUTION); + } + }); + add(slider); - // Add a button that can start and stop playback. - JButton playButton = new JButton(">"); - playButton.addActionListener(new ActionListener() { - @Override - public void actionPerformed(ActionEvent e) { - if (playbackTimer.isRunning()) { - playbackTimer.stop(); - } else { - playbackTimer.start(); - } - } - }); - add(playButton); + // Add a button that can start and stop playback. + JButton playButton = new JButton(">"); + playButton.addActionListener( + new ActionListener() { + @Override + public void actionPerformed(ActionEvent e) { + if (playbackTimer.isRunning()) { + playbackTimer.stop(); + } else { + playbackTimer.start(); + } + } + }); + add(playButton); - // Add the callback that will update this panel when playback time progresses. - playbackTimer.addListener((PropertyChangeEvent event) -> { - inTimerTick = true; - slider.setValue((int)Math.ceil((Double)event.getNewValue() / SLIDER_RESOLUTION)); - inTimerTick = false; - }); - } + // Add the callback that will update this panel when playback time progresses. + playbackTimer.addListener( + (PropertyChangeEvent event) -> { + inTimerTick = true; + slider.setValue( + (int) Math.ceil((Double) event.getNewValue() / SLIDER_RESOLUTION)); + inTimerTick = false; + }); + } } diff --git a/src/main/java/com/team766/simulator/ui/PlaybackTimer.java b/src/main/java/com/team766/simulator/ui/PlaybackTimer.java index 5f800dbf..1a097839 100644 --- a/src/main/java/com/team766/simulator/ui/PlaybackTimer.java +++ b/src/main/java/com/team766/simulator/ui/PlaybackTimer.java @@ -11,128 +11,129 @@ * Supports starting realtime playback, stopping playback, and seeking to a particular time. */ public class PlaybackTimer { - // Minimum period between notifications to listeners when playback is running (in milliseconds). - // Actual notification period may be longer than this, depending on how long it takes listeners - // to run their callbacks. - private static final int TIMER_PERIOD_MS = 50; + // Minimum period between notifications to listeners when playback is running (in milliseconds). + // Actual notification period may be longer than this, depending on how long it takes listeners + // to run their callbacks. + private static final int TIMER_PERIOD_MS = 50; - // The property name used in the PropertyChangeEvents that this sends to listeners. - public static final String PROPERTY_NAME = "playbackTime"; + // The property name used in the PropertyChangeEvents that this sends to listeners. + public static final String PROPERTY_NAME = "playbackTime"; - private final Timer timer; + private final Timer timer; - private final PropertyChangeSupport listeners = new PropertyChangeSupport(this); - - // The wall-clock time corresponding to t=0 of playback time. - // Only valid when playback is running. - private double startTime; - // The playback time, when it was most recently updated. - private double playTime = 0.0; - // The playback time of the previous update. - private double prevTime = 0.0; - // The playback time at which playback should automatically stop. - private final double endTime; - // Monitor that synchronizes access to these *Time variables. - private final Object timeLock = new Object(); + private final PropertyChangeSupport listeners = new PropertyChangeSupport(this); - /** - * @param endTime The playback time at which playback should automatically stop. - */ - public PlaybackTimer(double endTime) { - this.endTime = endTime; - timer = new Timer(TIMER_PERIOD_MS, null) { - @Override - protected void fireActionPerformed(ActionEvent event) { - synchronized (timeLock) { - playTime = System.currentTimeMillis() * 0.001 - startTime; - if (playTime >= endTime) { - playTime = endTime; - timer.stop(); - } - super.fireActionPerformed(event); - listeners.firePropertyChange(PROPERTY_NAME, prevTime, playTime); - prevTime = playTime; - } - } - }; - timer.setRepeats(true); - timer.setCoalesce(true); - } + // The wall-clock time corresponding to t=0 of playback time. + // Only valid when playback is running. + private double startTime; + // The playback time, when it was most recently updated. + private double playTime = 0.0; + // The playback time of the previous update. + private double prevTime = 0.0; + // The playback time at which playback should automatically stop. + private final double endTime; + // Monitor that synchronizes access to these *Time variables. + private final Object timeLock = new Object(); - /** - * Register the given listener to receive callbacks when playback time updates. - */ - public void addListener(PropertyChangeListener listener) { - listeners.addPropertyChangeListener(listener); - } + /** + * @param endTime The playback time at which playback should automatically stop. + */ + public PlaybackTimer(double endTime) { + this.endTime = endTime; + timer = + new Timer(TIMER_PERIOD_MS, null) { + @Override + protected void fireActionPerformed(ActionEvent event) { + synchronized (timeLock) { + playTime = System.currentTimeMillis() * 0.001 - startTime; + if (playTime >= endTime) { + playTime = endTime; + timer.stop(); + } + super.fireActionPerformed(event); + listeners.firePropertyChange(PROPERTY_NAME, prevTime, playTime); + prevTime = playTime; + } + } + }; + timer.setRepeats(true); + timer.setCoalesce(true); + } - /** - * Returns whether playback is currently running. - */ - public boolean isRunning() { - return timer.isRunning(); - } + /** + * Register the given listener to receive callbacks when playback time updates. + */ + public void addListener(PropertyChangeListener listener) { + listeners.addPropertyChangeListener(listener); + } - /** - * Returns the current playback time, as of the most recent update. - */ - public double currentTime() { - return playTime; - } + /** + * Returns whether playback is currently running. + */ + public boolean isRunning() { + return timer.isRunning(); + } - /** - * Returns the playback time at which playback will automatically stop. - */ - public double endTime() { - return endTime; - } + /** + * Returns the current playback time, as of the most recent update. + */ + public double currentTime() { + return playTime; + } - /** - * Start playback, if it is not already running. - * - * If playback had previously reached endTime, which will automatically start again from the - * beginning. - */ - public void start() { - if (isRunning()) { - return; - } - synchronized(timeLock) { - if (playTime >= endTime) { - setTime(0.0); - } - startTime = System.currentTimeMillis() * 0.001 - playTime; - } - timer.restart(); - } + /** + * Returns the playback time at which playback will automatically stop. + */ + public double endTime() { + return endTime; + } - /** - * Seek to the given playback time. - */ - public void setTime(double t) { - if (t < 0.) { - t = 0.; - } - synchronized(timeLock) { - playTime = t; - startTime = System.currentTimeMillis() * 0.001 - t; - if (!isRunning()) { - listeners.firePropertyChange(PROPERTY_NAME, prevTime, playTime); - } - prevTime = t; - } - } + /** + * Start playback, if it is not already running. + * + * If playback had previously reached endTime, which will automatically start again from the + * beginning. + */ + public void start() { + if (isRunning()) { + return; + } + synchronized (timeLock) { + if (playTime >= endTime) { + setTime(0.0); + } + startTime = System.currentTimeMillis() * 0.001 - playTime; + } + timer.restart(); + } - /** - * Stop playback. - */ - public void stop() { - if (!isRunning()) { - return; - } - timer.stop(); - synchronized(timeLock) { - playTime = System.currentTimeMillis() * 0.001 - startTime; - } - } + /** + * Seek to the given playback time. + */ + public void setTime(double t) { + if (t < 0.) { + t = 0.; + } + synchronized (timeLock) { + playTime = t; + startTime = System.currentTimeMillis() * 0.001 - t; + if (!isRunning()) { + listeners.firePropertyChange(PROPERTY_NAME, prevTime, playTime); + } + prevTime = t; + } + } + + /** + * Stop playback. + */ + public void stop() { + if (!isRunning()) { + return; + } + timer.stop(); + synchronized (timeLock) { + playTime = System.currentTimeMillis() * 0.001 - startTime; + } + } } diff --git a/src/main/java/com/team766/simulator/ui/PlotUtils.java b/src/main/java/com/team766/simulator/ui/PlotUtils.java index d27195be..b8e2b371 100644 --- a/src/main/java/com/team766/simulator/ui/PlotUtils.java +++ b/src/main/java/com/team766/simulator/ui/PlotUtils.java @@ -1,72 +1,76 @@ package com.team766.simulator.ui; -import java.awt.Point; -import java.util.Arrays; import de.erichseifert.gral.data.Column; import de.erichseifert.gral.data.DataTable; import de.erichseifert.gral.plots.XYPlot; +import java.awt.Point; +import java.util.Arrays; /** * Various utility methods for dealing with the Gral plotting library. */ class PlotUtils { - /** - * Return a DataTable containing the data in the given series of multivariate data. - * - * @throws IllegalArgumentException if all elements of `series` are not of the same length. - */ - public static DataTable makeDataTable(Iterable series) { - Double[] first = series.iterator().next(); - @SuppressWarnings("unchecked") - Class[] types = new Class[first.length]; - Arrays.fill(types, Double.class); - DataTable data = new DataTable(types); - for (Double[] values : series) { - if (first.length != values.length) { - throw new IllegalArgumentException("Data values must be the same length"); - } - data.add(values); - } - return data; - } + /** + * Return a DataTable containing the data in the given series of multivariate data. + * + * @throws IllegalArgumentException if all elements of `series` are not of the same length. + */ + public static DataTable makeDataTable(Iterable series) { + Double[] first = series.iterator().next(); + @SuppressWarnings("unchecked") + Class[] types = new Class[first.length]; + Arrays.fill(types, Double.class); + DataTable data = new DataTable(types); + for (Double[] values : series) { + if (first.length != values.length) { + throw new IllegalArgumentException("Data values must be the same length"); + } + data.add(values); + } + return data; + } - /** - * Return the index of the first element which is greater than or equal to `value`. - * If no such element is found, returns the index of the last element. - */ - public static int findIndex(Column column, Comparable value) { - // TODO: We use this to search sorted data, so we ought to be able to use a binary search, - // but Column is not compatible with either Arrays.binarySearch or Collections.binarySearch. - // NOTE: Calling column.toArray() and then Arrays.binarySearch would be more expensive than - // this linear scan. - int index = 0; - for (Object v : column) { - @SuppressWarnings("unchecked") - var t_v = (T)v; - if (value.compareTo(t_v) < 0) { - return index; - } - ++index; - } - return index - 1; - } + /** + * Return the index of the first element which is greater than or equal to `value`. + * If no such element is found, returns the index of the last element. + */ + public static int findIndex(Column column, Comparable value) { + // TODO: We use this to search sorted data, so we ought to be able to use a binary search, + // but Column is not compatible with either Arrays.binarySearch or Collections.binarySearch. + // NOTE: Calling column.toArray() and then Arrays.binarySearch would be more expensive than + // this linear scan. + int index = 0; + for (Object v : column) { + @SuppressWarnings("unchecked") + var t_v = (T) v; + if (value.compareTo(t_v) < 0) { + return index; + } + ++index; + } + return index - 1; + } - /** - * Convert a (x, y) coordinate pair from data values to UI coordinates. - * - * This is useful for drawing overlays onto plots. - */ - public static Point getPixelCoords(XYPlot plot, double x, double y) { - double pixelX = plot.getAxisRenderer(XYPlot.AXIS_X).worldToView(plot.getAxis(XYPlot.AXIS_X), x, false); - double pixelY = plot.getAxisRenderer(XYPlot.AXIS_Y).worldToView(plot.getAxis(XYPlot.AXIS_Y), y, false); + /** + * Convert a (x, y) coordinate pair from data values to UI coordinates. + * + * This is useful for drawing overlays onto plots. + */ + public static Point getPixelCoords(XYPlot plot, double x, double y) { + double pixelX = + plot.getAxisRenderer(XYPlot.AXIS_X) + .worldToView(plot.getAxis(XYPlot.AXIS_X), x, false); + double pixelY = + plot.getAxisRenderer(XYPlot.AXIS_Y) + .worldToView(plot.getAxis(XYPlot.AXIS_Y), y, false); - double plotAreaHeight = plot.getPlotArea().getHeight(); - double plotAreaX = plot.getPlotArea().getX(); - double plotAreaY = plot.getPlotArea().getY(); + double plotAreaHeight = plot.getPlotArea().getHeight(); + double plotAreaX = plot.getPlotArea().getX(); + double plotAreaY = plot.getPlotArea().getY(); - pixelX = plotAreaX + pixelX; - pixelY = plotAreaY + plotAreaHeight - pixelY; + pixelX = plotAreaX + pixelX; + pixelY = plotAreaY + plotAreaHeight - pixelY; - return new Point((int)pixelX, (int)pixelY); - } + return new Point((int) pixelX, (int) pixelY); + } } diff --git a/src/main/java/com/team766/simulator/ui/Trajectory.java b/src/main/java/com/team766/simulator/ui/Trajectory.java index fc27a231..2248d92e 100644 --- a/src/main/java/com/team766/simulator/ui/Trajectory.java +++ b/src/main/java/com/team766/simulator/ui/Trajectory.java @@ -1,91 +1,91 @@ package com.team766.simulator.ui; +import de.erichseifert.gral.data.DataSeries; +import de.erichseifert.gral.data.DataTable; +import de.erichseifert.gral.plots.XYPlot; +import de.erichseifert.gral.ui.InteractivePanel; import java.awt.BorderLayout; import java.awt.Color; import java.awt.Graphics; import java.awt.Graphics2D; import java.awt.Point; import java.awt.geom.AffineTransform; - import javax.swing.JPanel; -import de.erichseifert.gral.data.DataSeries; -import de.erichseifert.gral.data.DataTable; -import de.erichseifert.gral.plots.XYPlot; -import de.erichseifert.gral.ui.InteractivePanel; - /** * UI Window that displays the path that the robot drove. */ public class Trajectory extends JPanel { - private final XYPlot plot; - private final DataTable data; - // The current playback time. - private double time; + private final XYPlot plot; + private final DataTable data; + // The current playback time. + private double time; + + /** + * @param series The time series data defining the robot's trajectory. + * Each element should be an array with 6 values: + * [0]: Time + * [1]: X Position + * [2]: Y Position + * [3]: Orientation + * [4]: X Velocity + * [5]: Y Velocity + * @param playbackTimer The timer (shared between windows) which controls playback time. + */ + public Trajectory(Iterable series, PlaybackTimer playbackTimer) { + // Create an X-Y plot to display the trajectory + data = PlotUtils.makeDataTable(series); + var source = new DataSeries("Trajectory", data, 1, 2); + plot = new XYPlot(source); + plot.getAxis(XYPlot.AXIS_X).setRange(-10.0, 10.0); + plot.getAxis(XYPlot.AXIS_Y).setRange(-10.0, 10.0); + plot.setPointRenderers(source, null); - /** - * @param series The time series data defining the robot's trajectory. - * Each element should be an array with 6 values: - * [0]: Time - * [1]: X Position - * [2]: Y Position - * [3]: Orientation - * [4]: X Velocity - * [5]: Y Velocity - * @param playbackTimer The timer (shared between windows) which controls playback time. - */ - public Trajectory(Iterable series, PlaybackTimer playbackTimer) { - // Create an X-Y plot to display the trajectory - data = PlotUtils.makeDataTable(series); - var source = new DataSeries("Trajectory", data, 1, 2); - plot = new XYPlot(source); - plot.getAxis(XYPlot.AXIS_X).setRange(-10.0, 10.0); - plot.getAxis(XYPlot.AXIS_Y).setRange(-10.0, 10.0); - plot.setPointRenderers(source, null); - - InteractivePanel panel = new InteractivePanel(plot); - setLayout(new BorderLayout()); - add(panel, BorderLayout.CENTER); + InteractivePanel panel = new InteractivePanel(plot); + setLayout(new BorderLayout()); + add(panel, BorderLayout.CENTER); - // Add the standard time slider and play/pause button. - add(new PlaybackControls(playbackTimer), BorderLayout.SOUTH); + // Add the standard time slider and play/pause button. + add(new PlaybackControls(playbackTimer), BorderLayout.SOUTH); - // Add the callback that will update this window when playback time progresses. - playbackTimer.addListener(event -> { - this.time = (Double)event.getNewValue(); - this.repaint(); - }); - } + // Add the callback that will update this window when playback time progresses. + playbackTimer.addListener( + event -> { + this.time = (Double) event.getNewValue(); + this.repaint(); + }); + } - /** - * Draw overlays on the plot - */ - @Override - public void paint(final Graphics g) { - super.paint(g); + /** + * Draw overlays on the plot + */ + @Override + public void paint(final Graphics g) { + super.paint(g); - Graphics2D g2d = (Graphics2D)g; + Graphics2D g2d = (Graphics2D) g; - int index = PlotUtils.findIndex(data.getColumn(0), time); + int index = PlotUtils.findIndex(data.getColumn(0), time); - double x = (Double)data.get(1, index); - double y = (Double)data.get(2, index); - double orientation = (Double)data.get(3, index); - double vel_x = (Double)data.get(4, index); - double vel_y = (Double)data.get(5, index); + double x = (Double) data.get(1, index); + double y = (Double) data.get(2, index); + double orientation = (Double) data.get(3, index); + double vel_x = (Double) data.get(4, index); + double vel_y = (Double) data.get(5, index); - // Show the robot's current pose by drawing a rectangle. - Point pixelPos = PlotUtils.getPixelCoords(plot, x, y); - final int SIZE_X = 30; - final int SIZE_Y = 20; - g2d.setColor(new Color(128, 128, 255)); - AffineTransform saveXf = g2d.getTransform(); - g2d.rotate(-orientation, pixelPos.x, pixelPos.y); - g2d.fillRect((int)pixelPos.x - SIZE_X / 2, (int)pixelPos.y - SIZE_Y / 2, SIZE_X, SIZE_Y); - g2d.setTransform(saveXf); + // Show the robot's current pose by drawing a rectangle. + Point pixelPos = PlotUtils.getPixelCoords(plot, x, y); + final int SIZE_X = 30; + final int SIZE_Y = 20; + g2d.setColor(new Color(128, 128, 255)); + AffineTransform saveXf = g2d.getTransform(); + g2d.rotate(-orientation, pixelPos.x, pixelPos.y); + g2d.fillRect((int) pixelPos.x - SIZE_X / 2, (int) pixelPos.y - SIZE_Y / 2, SIZE_X, SIZE_Y); + g2d.setTransform(saveXf); - // Show the robot's current velocity by drawing a line extending from the robot's position. - //g2d.setColor(Color.red); - //g2d.drawLine((int)pixelPos.x, (int)pixelPos.y, (int)(pixelPos.x + vel_x * 20), (int)(pixelPos.y - vel_y * 20)); - } -} \ No newline at end of file + // Show the robot's current velocity by drawing a line extending from the robot's position. + // g2d.setColor(Color.red); + // g2d.drawLine((int)pixelPos.x, (int)pixelPos.y, (int)(pixelPos.x + vel_x * 20), + // (int)(pixelPos.y - vel_y * 20)); + } +} diff --git a/src/main/java/com/team766/simulator/ui/UIFrame.java b/src/main/java/com/team766/simulator/ui/UIFrame.java index 10afff93..e3da83fb 100644 --- a/src/main/java/com/team766/simulator/ui/UIFrame.java +++ b/src/main/java/com/team766/simulator/ui/UIFrame.java @@ -10,62 +10,63 @@ /** * A subclass of JFrame that adds automatic saving/restoring of the window's position and size. - * + * * We open several windows to display plots and trajectories, and it's preferable to not have to * rearrange them everytime the simulation is restarted. */ public class UIFrame extends JFrame { - private Preferences prefs; + private Preferences prefs; - public UIFrame(String frameUniqueId, Container content) { - super.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - super.setSize(800, 600); - super.setContentPane(content); - super.setTitle(frameUniqueId); - super.setVisible(true); + public UIFrame(String frameUniqueId, Container content) { + super.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + super.setSize(800, 600); + super.setContentPane(content); + super.setTitle(frameUniqueId); + super.setVisible(true); - prefs = Preferences.userNodeForPackage(UIFrame.class).node(frameUniqueId); + prefs = Preferences.userNodeForPackage(UIFrame.class).node(frameUniqueId); - restoreFrameLocation(); - restoreFrameSize(); + restoreFrameLocation(); + restoreFrameSize(); - super.addComponentListener(new ComponentAdapter() { - @Override - public void componentResized(ComponentEvent e) { - updatePref(); - } - - @Override - public void componentMoved(ComponentEvent e) { - updatePref(); - } - }); - } - - private void updatePref() { - Point location = super.getLocation(); - prefs.putInt("x", location.x); - prefs.putInt("y", location.y); - Dimension size = super.getSize(); - prefs.putInt("w", size.width); - prefs.putInt("h", size.height); - } - - private void restoreFrameSize() { - int w = prefs.getInt("w", -1); - int h = prefs.getInt("h", -1); - if (w < 0 || h < 0) { - return; - } - super.setSize(new Dimension(w, h)); - } - - private void restoreFrameLocation() { - int x = prefs.getInt("x", -1); - int y = prefs.getInt("y", -1); - if (x < 0 || y < 0) { - return; - } - super.setLocation(new Point(x, y)); - } + super.addComponentListener( + new ComponentAdapter() { + @Override + public void componentResized(ComponentEvent e) { + updatePref(); + } + + @Override + public void componentMoved(ComponentEvent e) { + updatePref(); + } + }); + } + + private void updatePref() { + Point location = super.getLocation(); + prefs.putInt("x", location.x); + prefs.putInt("y", location.y); + Dimension size = super.getSize(); + prefs.putInt("w", size.width); + prefs.putInt("h", size.height); + } + + private void restoreFrameSize() { + int w = prefs.getInt("w", -1); + int h = prefs.getInt("h", -1); + if (w < 0 || h < 0) { + return; + } + super.setSize(new Dimension(w, h)); + } + + private void restoreFrameLocation() { + int x = prefs.getInt("x", -1); + int y = prefs.getInt("y", -1); + if (x < 0 || y < 0) { + return; + } + super.setLocation(new Point(x, y)); + } } diff --git a/src/main/java/com/team766/web/ConfigUI.java b/src/main/java/com/team766/web/ConfigUI.java index a4768c9b..c1b56fb8 100644 --- a/src/main/java/com/team766/web/ConfigUI.java +++ b/src/main/java/com/team766/web/ConfigUI.java @@ -1,89 +1,100 @@ package com.team766.web; -import java.util.Map; -import java.util.ArrayList; import com.team766.config.ConfigFileReader; import com.team766.config.ConfigValueParseException; +import java.util.ArrayList; +import java.util.Map; public class ConfigUI implements WebServer.Handler { - @Override - public String endpoint() { - return "/config"; - } + @Override + public String endpoint() { + return "/config"; + } + + @Override + public String handle(Map params) { + String r = "

Configuration

"; + + r += "
\n"; + if (params.containsKey("configJson")) { + String configJsonString = (String) params.get("configJson"); + ArrayList validationErrors = new ArrayList(); + try { + ConfigFileReader.getInstance().reloadFromJson(configJsonString); + } catch (ConfigValueParseException ex) { + validationErrors.add(ex.toString()); + } catch (Exception ex) { + validationErrors.add("Failed to parse config json: " + ex); + } + if (validationErrors.isEmpty()) { + r += + "

New configuration (v" + + ConfigFileReader.getInstance().getGeneration() + + ") has been applied

"; + r += + "

Remember to click Restart Robot Code in the driver station if you have changed any motor/sensor settings

"; + if (params.containsKey("saveToFile")) { + try { + ConfigFileReader.getInstance().saveFile(configJsonString); + } catch (Exception ex) { + validationErrors.add("Could not save config file: " + ex.toString()); + } + } + } + if (validationErrors.size() > 0) { + r += "

Errors:\n"; + r += "

    \n"; + for (String error : validationErrors) { + r += "
  • " + error + "
    • \n"; + } + r += "

\n"; + } + } + r += "
\n"; - @Override - public String handle(Map params) { - String r = "

Configuration

"; + r += + String.join( + "\n", + new String[] { + "", + }); - r += "
\n"; - if (params.containsKey("configJson")) { - String configJsonString = (String)params.get("configJson"); - ArrayList validationErrors = new ArrayList(); - try { - ConfigFileReader.getInstance().reloadFromJson(configJsonString); - } catch (ConfigValueParseException ex) { - validationErrors.add(ex.toString()); - } catch (Exception ex) { - validationErrors.add("Failed to parse config json: " + ex); - } - if (validationErrors.isEmpty()) { - r += "

New configuration (v" + ConfigFileReader.getInstance().getGeneration() + ") has been applied

"; - r += "

Remember to click Restart Robot Code in the driver station if you have changed any motor/sensor settings

"; - if (params.containsKey("saveToFile")) { - try { - ConfigFileReader.getInstance().saveFile(configJsonString); - } catch (Exception ex) { - validationErrors.add("Could not save config file: " + ex.toString()); - } - } - } - if (validationErrors.size() > 0) { - r += "

Errors:\n"; - r += "

    \n"; - for (String error : validationErrors) { - r += "
  • " + error + "
    • \n"; - } - r += "

\n"; - } - } - r += "
\n"; + r += + "
\n"; + r += + "\n"; + r += "

Save to config file on robot?

\n"; + r += "

\n"; - r += String.join("\n", new String[]{ - "", - }); - - r += "
\n"; - r += "\n"; - r += "

Save to config file on robot?

\n"; - r += "

\n"; - - return r; - } + return r; + } - @Override - public String title() { - return "Config Values"; - } + @Override + public String title() { + return "Config Values"; + } } diff --git a/src/main/java/com/team766/web/LogViewer.java b/src/main/java/com/team766/web/LogViewer.java index 011fcd73..cd612c69 100644 --- a/src/main/java/com/team766/web/LogViewer.java +++ b/src/main/java/com/team766/web/LogViewer.java @@ -1,127 +1,126 @@ package com.team766.web; +import com.team766.logging.Category; +import com.team766.logging.LogEntry; +import com.team766.logging.LogEntryRenderer; +import com.team766.logging.Logger; +import com.team766.logging.Severity; import java.text.SimpleDateFormat; import java.util.Arrays; import java.util.Date; import java.util.Map; import java.util.stream.Stream; -import com.team766.logging.Category; -import com.team766.logging.Logger; -import com.team766.logging.LogEntry; -import com.team766.logging.LogEntryRenderer; -import com.team766.logging.Severity; public class LogViewer implements WebServer.Handler { - private static final String ENDPOINT = "/logs"; - private static final String ALL_ERRORS_NAME = "All Errors"; - private static final String ALL_MESSAGES_NAME = "All Messages"; - - private static final SimpleDateFormat timeFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss"); + private static final String ENDPOINT = "/logs"; + private static final String ALL_ERRORS_NAME = "All Errors"; + private static final String ALL_MESSAGES_NAME = "All Messages"; - private static String makeLogEntriesTable(final Iterable entries) { - String r = "\n"; - for (LogEntry entry : entries) { - r += String.format( - "\n", - entry.getCategory(), timeFormat.format(new Date(entry.getTime() / 1000000)), - entry.getSeverity(), LogEntryRenderer.renderLogEntry(entry, null)); - } - r += "
%s%s%s%s
"; - return r; - } + private static final SimpleDateFormat timeFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss"); - private static String makePage(String categoryName, Iterable entries) { - return String.join("\n", new String[]{ - "

Log: " + categoryName + "

", - "

", - HtmlElements.buildDropDown( - "category", - categoryName, - Stream.concat( - Stream.of(ALL_ERRORS_NAME, ALL_MESSAGES_NAME), - Arrays.stream(Category.values()).map(Category::name) - ).toArray(String[]::new)), - "", - "

", - makeLogEntriesTable(entries), - "", - "", - "", - "", - }); - } + private static String makeLogEntriesTable(final Iterable entries) { + String r = "\n"; + for (LogEntry entry : entries) { + r += + String.format( + "\n", + entry.getCategory(), + timeFormat.format(new Date(entry.getTime() / 1000000)), + entry.getSeverity(), + LogEntryRenderer.renderLogEntry(entry, null)); + } + r += "
%s%s%s%s
"; + return r; + } - static String makeAllErrorsPage() { - return makePage(ALL_ERRORS_NAME, - Arrays.stream(Category.values()) - .flatMap(category -> Logger.get(category).recentEntries().stream()) - .filter(entry -> entry.getSeverity() == Severity.ERROR)::iterator); - } + private static String makePage(String categoryName, Iterable entries) { + return String.join( + "\n", + new String[] { + "

Log: " + categoryName + "

", + "

", + HtmlElements.buildDropDown( + "category", + categoryName, + Stream.concat( + Stream.of(ALL_ERRORS_NAME, ALL_MESSAGES_NAME), + Arrays.stream(Category.values()).map(Category::name)) + .toArray(String[]::new)), + "", + "

", + makeLogEntriesTable(entries), + "", + "", + "", + "", + }); + } - static String makeAllMessagesPage() { - return makePage( - ALL_MESSAGES_NAME, - Arrays.stream(Category.values()) - .flatMap(category -> Logger.get(category).recentEntries().stream()) - ::iterator); - } + static String makeAllErrorsPage() { + return makePage( + ALL_ERRORS_NAME, + Arrays.stream(Category.values()) + .flatMap(category -> Logger.get(category).recentEntries().stream()) + .filter(entry -> entry.getSeverity() == Severity.ERROR) + ::iterator); + } - static String makeAllMessagesPage() { - return makePage( - ALL_MESSAGES_NAME, - Arrays.stream(Category.values()) - .flatMap(category -> Logger.get(category).recentEntries().stream()) - ::iterator); - } + static String makeAllMessagesPage() { + return makePage( + ALL_MESSAGES_NAME, + Arrays.stream(Category.values()) + .flatMap(category -> Logger.get(category).recentEntries().stream()) + ::iterator); + } - @Override - public String endpoint() { - return ENDPOINT; - } + @Override + public String endpoint() { + return ENDPOINT; + } - @Override - public String handle(final Map params) { - String categoryName = (String) params.get("category"); - if (categoryName == null || categoryName.equals(ALL_ERRORS_NAME)) { - return makeAllErrorsPage(); - } else if (categoryName.equals(ALL_MESSAGES_NAME)) { - return makeAllMessagesPage(); - } else { - Category category = Enum.valueOf(Category.class, categoryName); - return makePage(category.name(), Logger.get(category).recentEntries()); - } - } + @Override + public String handle(final Map params) { + String categoryName = (String) params.get("category"); + if (categoryName == null || categoryName.equals(ALL_ERRORS_NAME)) { + return makeAllErrorsPage(); + } else if (categoryName.equals(ALL_MESSAGES_NAME)) { + return makeAllMessagesPage(); + } else { + Category category = Enum.valueOf(Category.class, categoryName); + return makePage(category.name(), Logger.get(category).recentEntries()); + } + } - @Override - public String title() { - return "Log Viewer"; - } + @Override + public String title() { + return "Log Viewer"; + } } diff --git a/src/test/java/com/team766/simulator/MetricsTest.java b/src/test/java/com/team766/simulator/MetricsTest.java index 844cd31c..a5fe8412 100644 --- a/src/test/java/com/team766/simulator/MetricsTest.java +++ b/src/test/java/com/team766/simulator/MetricsTest.java @@ -5,16 +5,18 @@ import org.junit.jupiter.api.Test; public class MetricsTest { - @Test - public void test() { - var metrics = new Metrics(); - var series1 = metrics.addSeries("Series1"); - var series2 = metrics.addSeries("Series2"); + @Test + public void test() { + var metrics = new Metrics(); + var series1 = metrics.addSeries("Series1"); + var series2 = metrics.addSeries("Series2"); - metrics.add(0).set(series1, 42).set(series2, 24); - metrics.add(1).set(series1, 83).set(series2, 38); - - Assertions.assertIterableEquals(metrics.getLabels(), List.of("Series1", "Series2")); - Assertions.assertArrayEquals(metrics.getMetrics().toArray(), new Double[][]{new Double[]{0., 42., 24.}, new Double[]{1., 83., 38.}}); - } -} \ No newline at end of file + metrics.add(0).set(series1, 42).set(series2, 24); + metrics.add(1).set(series1, 83).set(series2, 38); + + Assertions.assertIterableEquals(metrics.getLabels(), List.of("Series1", "Series2")); + Assertions.assertArrayEquals( + metrics.getMetrics().toArray(), + new Double[][] {new Double[] {0., 42., 24.}, new Double[] {1., 83., 38.}}); + } +}