Skip to content

Commit

Permalink
Updating to the start of C19
Browse files Browse the repository at this point in the history
  • Loading branch information
@madhephaestus
madhephaestus committed Jan 11, 2019
1 parent 8c6d0fa commit c04238a
Show file tree
Hide file tree
Showing 7 changed files with 699 additions and 152 deletions.
Binary file removed src/.DS_Store
View file
Binary file not shown.
Binary file removed src/main/.DS_Store
View file
Binary file not shown.
319 changes: 282 additions & 37 deletions src/main/java/edu/wpi/rbe/rbe2001/fieldsimulator/gui/InterfaceController.java
View file
Original file line number Diff line number Diff line change
@@ -1,30 +1,89 @@
package edu.wpi.rbe.rbe2001.fieldsimulator.gui;

import edu.wpi.rbe.rbe2001.fieldsimulator.robot.WarehouseRobot;
import java.text.DecimalFormat;
import java.util.ArrayList;
import edu.wpi.rbe.rbe2001.fieldsimulator.robot.RBE2001Robot;
import edu.wpi.rbe.rbe2001.fieldsimulator.robot.WarehouseRobotStatus;
import javafx.application.Platform;
import javafx.collections.FXCollections;
import javafx.collections.ObservableList;
import javafx.fxml.FXML;
import javafx.scene.chart.LineChart;
import javafx.scene.control.Button;
import javafx.scene.control.ChoiceBox;
import javafx.scene.control.Label;
import javafx.scene.control.RadioButton;
import javafx.scene.control.Tab;
import javafx.scene.control.TextArea;
import javafx.scene.control.TextField;
import javafx.scene.chart.XYChart;
import javafx.scene.chart.XYChart.Series;

public class InterfaceController {

private static WarehouseRobot fieldSim;
@FXML
private Tab connectTab;

@FXML
private TextField teamName;

private WarehouseRobotStatus status = WarehouseRobotStatus.Fault_E_Stop_pressed;
private ObservableList<String> weights = FXCollections.observableArrayList("Aluminum", "Plastic");
private ObservableList<String> sides = FXCollections.observableArrayList("25", "45");
private ObservableList<String> pos = FXCollections.observableArrayList("1", "2");
@FXML
private Button connectToDevice;

@FXML
private Button start;
private Label robotName;

@FXML
private Tab pidVelTab;
@FXML
private Tab tab2001Field;
@FXML
private TextField kpVel;

@FXML
private TextField kdVel;

@FXML
private Button pidConstUpdateVelocity;

@FXML
private ChoiceBox<?> pidChannelVelocity;

@FXML
private TextField setpointVelocity;

@FXML
private Button setSetpointVelocity;

@FXML
private Label velocityVal;

@FXML
private Tab pidTab;

@FXML
private TextField kp;

@FXML
private TextField ki;

@FXML
private TextField kd;

@FXML
private Button pidConstUpdate;

@FXML
private TextField setpoint;

@FXML
private Button setSetpoint;

@FXML
private Label position;

@FXML
private ChoiceBox<Integer> pidChannel;

@FXML
private Button stop;
Expand All @@ -46,28 +105,73 @@ public class InterfaceController {

@FXML
private ChoiceBox<String> choiceBoxPos;
@FXML
private TextField teamName;

@FXML
private Button approveButton;

@FXML // fx:id="setDuration"
private TextField setDuration; // Value injected by FXMLLoader

@FXML // fx:id="setType"
private ChoiceBox<String> setType; // Value injected by FXMLLoader

@FXML
private LineChart<Double, Double> pidGraph;
@FXML
private LineChart<Double, Double> pidGraphVel;
private ArrayList<XYChart.Series> pidGraphSeriesVel = new ArrayList<>();
private ArrayList<XYChart.Series> pidGraphSeries = new ArrayList<>();
private WarehouseRobotStatus status = WarehouseRobotStatus.Fault_E_Stop_pressed;
private ObservableList<String> weights = FXCollections.observableArrayList("Aluminum", "Plastic");
private ObservableList<String> sides = FXCollections.observableArrayList("25", "45");
private ObservableList<String> pos = FXCollections.observableArrayList("1", "2");
private double pidConfig[] = null;

private DecimalFormat formatter = new DecimalFormat();
private double start = ((double) System.currentTimeMillis()) / 1000.0;
private long lastPos;
private long lastSet;
static InterfaceController me;
private static RBE2001Robot fieldSim;
private int numPIDControllers = -1;
private int currentIndex = 0;
private static final int numPIDControllersOnDevice = 3;

@FXML
private void initialize() {
assert connectToDevice != null : "fx:id=\"connectToDevice\" was not injected: check your FXML file 'MainScreen"
+ ".fxml'.";
assert start != null : "fx:id=\"start\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert stop != null : "fx:id=\"stop\" was not injected: check your FXML file 'MainScreen.fxml'.";
me = this;
formatter.setMaximumFractionDigits(6);

assert connectTab != null : "fx:id=\"connectTab\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert teamName != null : "fx:id=\"teamName\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert response != null : "fx:id=\"response\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert send != null : "fx:id=\"send\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert heartBeat != null : "fx:id=\"heartBeat\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert choiceBoxWeight != null : "fx:id=\"choiceBoxWeight\" was not injected: check your FXML file 'MainScreen"
+ ".fxml'.";
assert choiceBoxSide != null : "fx:id=\"choiceBoxSide\" was not injected: check your FXML file 'MainScreen"
+ ".fxml'.";
assert choiceBoxPos != null : "fx:id=\"choiceBoxPos\" was not injected: check your FXML file 'MainScreen"
+ ".fxml'.";
assert connectToDevice != null : "fx:id=\"connectToDevice\" was not injected: check your FXML file 'MainScreen.fxml'.";

assert pidTab != null : "fx:id=\"pidTab\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert pidGraph != null : "fx:id=\"pidGraph\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert kp != null : "fx:id=\"kp\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert ki != null : "fx:id=\"ki\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert kd != null : "fx:id=\"kd\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert pidConstUpdate != null : "fx:id=\"pidConstUpdate\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert pidChannel != null : "fx:id=\"pidChannel\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert setpoint != null : "fx:id=\"setpoint\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert setSetpoint != null : "fx:id=\"setSetpoint\" was not injected: check your FXML file 'MainScreen.fxml'.";
assert position != null : "fx:id=\"position\" was not injected: check your FXML file 'MainScreen.fxml'.";
teamName.setText("IMU-Team21");

for (int i = 0; i < numPIDControllersOnDevice; i++) {
Series e = new XYChart.Series();

pidGraphSeries.add(i, e);
pidGraph.getData().add(e);
}
pidGraphVel.getXAxis().autoRangingProperty().set(true);
for (int i = 0; i < numPIDControllersOnDevice; i++) {
Series e = new XYChart.Series();

pidGraphSeriesVel.add(i, e);
pidGraphVel.getData().add(e);
}
pidGraphVel.getXAxis().autoRangingProperty().set(true);
choiceBoxWeight.setValue(weights.get(0));
choiceBoxWeight.setItems(weights);
choiceBoxSide.setValue("25");
Expand All @@ -77,7 +181,6 @@ private void initialize() {

choiceBoxWeight.getSelectionModel().select(weights.get(0));

start.setDisable(true);
stop.setDisable(true);
// PLE.setDisable(true);
// RHE.setDisable(true);
Expand All @@ -90,7 +193,26 @@ private void connectToDevice() {
connectToDevice.setDisable(true);
new Thread(() -> {
try {
setFieldSim(WarehouseRobot.get(teamName.getText()).get(0));
setFieldSim(RBE2001Robot.get(teamName.getText()).get(0));
Thread.sleep(1000);
// getFieldSim().setReadTimeout(1000);
if (getRobot() != null) {
Platform.runLater(() -> {
robotName.setText(getRobot().getName());
pidTab.setDisable(false);
//pidVelTab.setDisable(false);
tab2001Field.setDisable(false);
});
}
} catch (Exception ex) {
// ex.printStackTrace();
Platform.runLater(() -> robotName.setText(teamName.getText() + " Not Found!"));
}
if (getRobot() == null) {
Platform.runLater(() -> connectToDevice.setDisable(false));
}

try {
// getFieldSim().setReadTimeout(1000);
if (getRobot() != null) {
getRobot().addEvent(2012, () -> {
Expand All @@ -114,7 +236,6 @@ private void connectToDevice() {
});

Platform.runLater(() -> {
start.setDisable(false);
stop.setDisable(false);
// PLE.setDisable(false);
// RHE.setDisable(false);
Expand All @@ -132,6 +253,142 @@ private void connectToDevice() {
}
}

@FXML
void onConnect() {
System.out.println("onConnect");
connectToDevice();
}

@FXML
void onSetGains() {
double kpv = Double.parseDouble(kp.getText());
double kiv = Double.parseDouble(ki.getText());
double kdv = Double.parseDouble(kd.getText());
for (int i = 0; i < numPIDControllers; i++)
fieldSim.setPidGains(i, kpv, kiv, kdv);
}

@FXML
void onSetSetpoint() {
fieldSim.setPidSetpoint(Integer.parseInt(setDuration.getText()),
setType.getSelectionModel().getSelectedItem().equals("LIN") ? 0 : 1,
currentIndex,
Double.parseDouble(setpoint.getText()));

}

public RBE2001Robot getRobot() {
return fieldSim;
}

private void setFieldSim(RBE2001Robot fieldSim) {
fieldSim.setReadTimeout(1000);
InterfaceController.fieldSim = fieldSim;

fieldSim.addEvent(1910, () -> {
try {

if (numPIDControllers != fieldSim.getMyNumPid()) {
numPIDControllers = fieldSim.getMyNumPid();
setUpPid();
}
double pos = fieldSim.getPidPosition(currentIndex);
double set = fieldSim.getPidSetpoint(currentIndex);

String positionVal = formatter.format(pos);
// System.out.println(positionVal+"
// "+DoubleStream.of(piddata).boxed().collect(Collectors.toCollection(ArrayList::new)));
;
Platform.runLater(() -> position.setText(positionVal));
Platform.runLater(() -> updateGraph(pos, set));

} catch (Exception ex) {
ex.printStackTrace();
}
});

fieldSim.addEvent(1857, () -> {
try {
if (pidConfig == null)
pidConfig = new double[3 * numPIDControllersOnDevice];
fieldSim.readFloats(1857, pidConfig);

// System.out.println("
// "+DoubleStream.of(pidConfig).boxed().collect(Collectors.toCollection(ArrayList::new)));

Platform.runLater(() -> kp.setText(formatter.format(pidConfig[currentIndex * 3 + 0])));

Platform.runLater(() -> ki.setText(formatter.format(pidConfig[currentIndex * 3 + 1])));

Platform.runLater(() -> kd.setText(formatter.format(pidConfig[currentIndex * 3 + 2])));

} catch (Exception ex) {
ex.printStackTrace();
}
});
fieldSim.updatConfig();

}

@SuppressWarnings("unchecked")
private void updateGraph(double pos, double set) {
if (pidGraphSeries.size() == 0)
return;
double now = ((double) System.currentTimeMillis()) / 1000.0 - start;
long thispos = (long) pos;
long thisSet = (long) set;
if (thispos != lastPos || thisSet != lastSet) {
pidGraphSeries.get(0).getData().add(new XYChart.Data(now - 0.0001, lastPos));
pidGraphSeries.get(1).getData().add(new XYChart.Data(now - 0.0001, lastSet));
lastSet = thisSet;
lastPos = thispos;
pidGraphSeries.get(0).getData().add(new XYChart.Data(now, pos));
pidGraphSeries.get(1).getData().add(new XYChart.Data(now, set));
}
for (Series s : pidGraphSeries) {
while (s.getData().size() > 200) {
s.getData().remove(0);
}
}
}

private void setUpPid() {
System.out.println("PID controller has " + fieldSim.getNumPid() + " controllers");
if (fieldSim.getNumPid() > 0) {
for (int i = 0; i < fieldSim.getNumPid(); i++) {
int index = i;
Platform.runLater(() -> pidChannel.getItems().add(index));
}
pidChannel.getSelectionModel().selectedIndexProperty().addListener((obs, old, newVal) -> {
System.out.println("Set to channel " + newVal);
currentIndex = newVal.intValue();
fieldSim.updatConfig();
for (Series s : pidGraphSeries) {
s.getData().clear();

}
});
Platform.runLater(() -> pidChannel.setValue(0));
Platform.runLater(() -> setType.getItems().add("LIN"));
Platform.runLater(() -> setType.getItems().add("SIN"));
Platform.runLater(() -> setType.setValue("LIN"));
}
}

public static void disconnect() {
if (me.getRobot() != null)
me.getRobot().disconnect();
}

@FXML
void onSetVelocity() {

}

@FXML
void onSetGainsVelocity() {

}
@FXML
void onApprove() {
System.out.println("approve");
Expand Down Expand Up @@ -172,18 +429,6 @@ void stop() {
}
}

@FXML
void onConnect() {
System.out.println("onConnect");
connectToDevice();
}

public static WarehouseRobot getRobot() {
return fieldSim;
}

private static void setFieldSim(WarehouseRobot fieldSim) {
fieldSim.setReadTimeout(1000);
InterfaceController.fieldSim = fieldSim;
}
}
Loading

0 comments on commit c04238a

Please sign in to comment.