From 047d31cba37f742928925552b28e6c25533e5821 Mon Sep 17 00:00:00 2001 From: avinashc99 Date: Mon, 3 Aug 2020 10:09:34 -0500 Subject: [PATCH 1/2] Home Button and End Behavior Improvments to Wheelbot --- trick_sims/SIM_wheelbot/README.md | 4 +- trick_sims/SIM_wheelbot/RUN_test/input.py | 18 ++ .../SIM_wheelbot/models/Control/README.md | 21 +- .../Control/include/vehicleController.hh | 12 ++ .../models/Control/src/vehicleController.cpp | 43 ++-- .../SIM_wheelbot/models/GUIControl1/Makefile | 36 ++++ .../SIM_wheelbot/models/GUIControl1/README.md | 116 ++++++++++ .../GUIControl1/src/trick/HomeDisplay.java | 200 ++++++++++++++++++ .../models/Vehicle/include/vehicleOne.hh | 6 + .../models/Vehicle/src/vehicleOne.cpp | 22 ++ 10 files changed, 459 insertions(+), 19 deletions(-) create mode 100644 trick_sims/SIM_wheelbot/models/GUIControl1/Makefile create mode 100644 trick_sims/SIM_wheelbot/models/GUIControl1/README.md create mode 100644 trick_sims/SIM_wheelbot/models/GUIControl1/src/trick/HomeDisplay.java diff --git a/trick_sims/SIM_wheelbot/README.md b/trick_sims/SIM_wheelbot/README.md index ada5e129..7a37f572 100644 --- a/trick_sims/SIM_wheelbot/README.md +++ b/trick_sims/SIM_wheelbot/README.md @@ -33,7 +33,7 @@ veh.vehicle.wheelSpeedLimit | double | rad/s | 8.880 veh.vehicle.headingRateLimit | double | rad/s | 𝛑/4 veh.vehicle.wheelDragConstant | double | -- | 1.875 veh.vehicle.corningStiffness | double | -- | 10.0 -veh.vehicle.slowDownDistance | double | -- | 0.5 +veh.vehicle.slowDownDistance | double | -- | 0.5 veh.vehicle.arrivalDistance | double | -- | 0.1 ![Picture of Vehicle](images/Figure2.png) @@ -43,6 +43,8 @@ Waypoints, for the vehicle to follow, are added with a call to veh.vehicle.add_waypoint( double N, double W ) +#### Adding Home point +A home point is designated by the last waypoint in the waypoint file. ### Input/Output diff --git a/trick_sims/SIM_wheelbot/RUN_test/input.py b/trick_sims/SIM_wheelbot/RUN_test/input.py index a416a67c..dd6875d7 100644 --- a/trick_sims/SIM_wheelbot/RUN_test/input.py +++ b/trick_sims/SIM_wheelbot/RUN_test/input.py @@ -21,6 +21,7 @@ veh.vehicle.arrivalDistance = 0.1 #========================================== # Add the waypoints to the SIM. +# Set a home point by adding it as the last waypoint. #========================================== waypoints_path = "Modified_data/cross.waypoints" fp = open(waypoints_path, "r") @@ -47,4 +48,21 @@ else : print('EVDisplay needs to be built. Please \"cd\" into models/Graphics and type \"make\".') print('==================================================================================') +#========================================== +# Start the display VarServer Client +#========================================== +varServerPort = trick.var_server_get_port(); +HomeDisplay_path = "models/GUIControl1/dist/HomeDisplay.jar" + +if (os.path.isfile(HomeDisplay_path)) : + HomeDisplay_cmd = "java -jar " \ + + HomeDisplay_path \ + + " " + str(varServerPort) + " &" ; + print(HomeDisplay_cmd) + os.system( HomeDisplay_cmd); +else : + print('==================================================================================') + print('HomeDisplay needs to be built. Please \"cd\" into models/GUIControl1 and type \"make\".') + print('==================================================================================') + trick.stop(100) diff --git a/trick_sims/SIM_wheelbot/models/Control/README.md b/trick_sims/SIM_wheelbot/models/Control/README.md index dce3a38a..7651936b 100644 --- a/trick_sims/SIM_wheelbot/models/Control/README.md +++ b/trick_sims/SIM_wheelbot/models/Control/README.md @@ -87,11 +87,11 @@ With **availableWheelSpeedForRangeRate** determined, we can figure our range rat #### Calculating the Required Motor Speeds Now that we've apportioned the available wheel speed to turning and moving, we can figure our right and left wheel speeds: - + * [Eq#6] **desiredRightWheelSpeed** = wheelSpeedForRangeRate + wheelSpeedForHeadingRate - + * [Eq#7] **desiredLeftWheelSpeed** = wheelSpeedForRangeRate - wheelSpeedForHeadingRate - + Here, wheel speed is positive forward, and negative is backwards. For the motor models positive is counter clockwise, and negative is clockwise. So, we have to change sign for the right motor: @@ -100,7 +100,7 @@ Here, wheel speed is positive forward, and negative is backwards. For the motor * [Eq#9] **leftMotorSpeedCommand** = desiredLeftWheelSpeed - + | Access | Member Name | Type | Units | Value | |---------|-----------------------|--------|--------|--------| | private | rightMotorSpeedCommand| double | rad/s |[Eq#8](#EQ_8_rightMotorSpeedCommand)| @@ -192,6 +192,17 @@ void update(); Depending on the vehicles current destination, and its distance from that destination, call the DifferentialDriveController update() method with the current distance-error, and heading-error to drive, and steer the vehicle. +``` +void gohome(); +``` +Depending on the vehicle's homeCommanded variable, the gohome() function is called when the car needs to be homed. It sets the iterator to the +last value in the waypoints list (which is set to be the home point). + +``` +bool getStatus(); +``` +Returns the status of the simulation, if it is at end with no destination then the vehicle stops moving. Used to improve end time behavior. + # class PIDController @@ -214,7 +225,7 @@ PIDController::PIDController( For no filtering, set tc to the value of dt. To filter, set tc a value higher than dt. - + ``` double PIDController::getOutput( double setpoint_value, double measured_value); diff --git a/trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh b/trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh index 5191f7a9..0f190dd0 100644 --- a/trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh +++ b/trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh @@ -25,6 +25,12 @@ class VehicleController { void printDestination(); void update(); + // Homing Functions + // Commands wheelbot to navigate to home + void gohome(); + // Returns the value of the variable endofSimulation + bool getStatus(); + private: // Do not allow the default constructor to be used. VehicleController(); @@ -35,6 +41,12 @@ class VehicleController { Navigator& navigator; DifferentialDriveController& driveController; + // Homing variables + // Records if end of simulation + bool endofSimulation; + // Records if told to go home + bool homeCommanded; + double arrivalDistance; }; #endif diff --git a/trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp b/trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp index 51d9324d..5899ab5d 100644 --- a/trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp +++ b/trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp @@ -17,6 +17,10 @@ VehicleController::VehicleController( std::vector* wayPoints, waypointQueue = wayPoints; destination = waypointQueue->begin(); printDestination(); + + // Initialize homing variables + endofSimulation = false; + homeCommanded = false; } void VehicleController::setWayPointQueue( std::vector* wayPoints ) { @@ -33,6 +37,12 @@ int VehicleController::getCurrentDestination(Point& currentDestination) { return 1; } +// Commands wheelbot to navigate to home +void VehicleController::gohome() { + destination = waypointQueue->end()-1; + homeCommanded = true; +} + void VehicleController::printDestination() { if (destination != waypointQueue->end()) { std::cout << "Destination = (" << destination->x << "," << destination->y << ")." << std::endl; @@ -41,22 +51,29 @@ void VehicleController::printDestination() { } } +// Returns the value of the variable endofSimulation +bool VehicleController::getStatus() { + return endofSimulation; +} + void VehicleController::update() { - if (destination == waypointQueue->end()) { - driveController.update(0.0, 0.0); - driveController.stop(); - } else { - double distance_err = navigator.distanceTo(*destination); - if ( distance_err > arrivalDistance) { - double heading_err = navigator.bearingTo(*destination); - driveController.update(distance_err, heading_err); - } else { + if (destination == waypointQueue->end() && endofSimulation == false) { + if (homeCommanded == false) { + driveController.update(0.0, 0.0); + } + endofSimulation = true; + } else { + double distance_err = navigator.distanceTo(*destination); + if ( distance_err > arrivalDistance) { + double heading_err = navigator.bearingTo(*destination); + driveController.update(distance_err, heading_err); + } else { + if (endofSimulation != true) { std::cout << "Arrived at Destination." << std::endl; destination ++; printDestination(); - } - } + } + } + } } - - diff --git a/trick_sims/SIM_wheelbot/models/GUIControl1/Makefile b/trick_sims/SIM_wheelbot/models/GUIControl1/Makefile new file mode 100644 index 00000000..4b1b4921 --- /dev/null +++ b/trick_sims/SIM_wheelbot/models/GUIControl1/Makefile @@ -0,0 +1,36 @@ +SHELL = /bin/sh + +PROJECT_NAME = HomeDisplay +SRC_DIR = src +BUILD_DIR = build +CLASSES_DIR = $(BUILD_DIR)/classes +JAR_DIR = dist +MAIN_CLASS = trick.HomeDisplay + +all: jar + +clean: + rm -rf $(BUILD_DIR) + rm -f manifest + +spotless: clean + rm -rf dist + +$(CLASSES_DIR): + @ mkdir -p $(CLASSES_DIR) + +compile: | $(CLASSES_DIR) + javac -sourcepath $(SRC_DIR) -d $(CLASSES_DIR) $(SRC_DIR)/trick/HomeDisplay.java + +manifest: + @ echo "Main-Class: $(MAIN_CLASS)" > $@ + +$(JAR_DIR): + @ mkdir -p $(JAR_DIR) + +jar: compile manifest | $(JAR_DIR) + jar cvfm $(JAR_DIR)/$(PROJECT_NAME).jar manifest -C $(CLASSES_DIR) . + @ echo "-------------------------------------------------------------------------------" + @ echo " BUILD COMPLETE" + @ echo "The Java jar file (the Java Executable) is located at: $(JAR_DIR)/$(PROJECT_NAME).jar" + @ echo "-------------------------------------------------------------------------------" diff --git a/trick_sims/SIM_wheelbot/models/GUIControl1/README.md b/trick_sims/SIM_wheelbot/models/GUIControl1/README.md new file mode 100644 index 00000000..38d76b9a --- /dev/null +++ b/trick_sims/SIM_wheelbot/models/GUIControl1/README.md @@ -0,0 +1,116 @@ +# Graphics + +**Contents** + +* [class HomeButton](#class-HomeButton)
+* [class HomeDisplay](#class-HomeDisplay)
+* [class TrickSimMode](#class-TrickSimMode)
+ +--- + + +## class homeButton +extends [JPanel](https://docs.oracle.com/javase/7/docs/api/javax/swing/JPanel.html) +implements [ActionListener](https://docs.oracle.com/javase/10/docs/api/java/awt/event/ActionListener.html) + +### Description + +The HomeButton class represents a graphical button in an [HomeDisplay](#class-HomeDisplay). It utilizes ActionListener to record user input to the button. + +| Access | Member Name | Type | Units | Value | +|---------|---------------|--------------|--------|--------| +| private | homeButton1 |JButton | -- | | +| private | homeCommand |boolean | -- | | +| private | label |JLabel | -- | | + +### Constructor + +``` +public HomeButton(); +``` +Constructs the button with the appropriate label and adds it to the JPanel. + +### Member Functions + +``` +public void actionPerformed(ActionEvent e); +``` +Utilizes the ActionListener and record the user input in the form of a boolean variable homeCommand. + +``` +public void resetHomeCommand(); +``` +Resets the homeCommand variable to false. + +``` +public void getHomeCommand(); +``` +Returns the value of homeCommand. + +--- + + +## class TrickSimMode + +### Description + +The TrickSimMode class declares variables for the state of the simulation. + + + +## class HomeDisplay +extends [JFrame](https://docs.oracle.com/en/java/javase/11/docs/api/java.desktop/javax/swing/JFrame.html) + +### Description +This class implements a Trick variable server client application for SIM_wheelbot that displays the homing button. + +#### Running the Client +``` +java -jar HomeDisplay.jar +``` + +| Access | Member Name | Type | Units | Value | +|--------|-----------------|----------------- |--------|--------| +| private | buttonPanel | HomeButton | -- | | +| private | in | BufferedReader | -- | | +| private | out | DataOutputStream | -- | | + +### Constructor + +``` +public HomeDisplay(); +``` +Initialize an JFrame with the HomeButton class. + +### Member Functions + +``` +public void resetHomeCommand(); +``` +Extension of the resetHomeCommand function found in the HomeButton class. + +``` +public void getHomeCommand(); +``` +Extension of the getHomeCommand function found in the HomeButton class. + +``` +public void connectToServer(String host, int port ); +``` +Connect to the Trick variable server as specified by **host** and **port**. + +``` +private static void printHelpText(); +``` + +``` +public static void main(String[] args); +``` +* Process Args +* Validate Parameters +* Initialize HomeDisplay GUI +* Connect to the Trick variable server on the local computer ("localhost") at the port number specified by the applicaton arguments. +* Request the vehicle home variable and simulation mode. +* Enter a continuous while loop that reads, and correspondingly sets the vehicle state. + +--- diff --git a/trick_sims/SIM_wheelbot/models/GUIControl1/src/trick/HomeDisplay.java b/trick_sims/SIM_wheelbot/models/GUIControl1/src/trick/HomeDisplay.java new file mode 100644 index 00000000..02c26bb1 --- /dev/null +++ b/trick_sims/SIM_wheelbot/models/GUIControl1/src/trick/HomeDisplay.java @@ -0,0 +1,200 @@ +/* + * Homing Varible Server Client +*/ + +package trick; + +import javax.swing.*; +import java.awt.*; +import java.awt.event.ActionEvent; +import java.awt.event.ActionListener; +import javax.swing.JButton; +import javax.swing.JLabel; +import javax.swing.JPanel; +import javax.swing.JFrame; +import java.io.BufferedOutputStream; +import java.io.BufferedReader; +import java.io.DataOutputStream; +import java.io.IOException; +import java.io.InputStreamReader; +import java.net.Socket; +import java.util.*; +import java.io.*; + +// Create the HomeButton class by extending JPanel and ActionListener +class HomeButton extends JPanel implements ActionListener { + + // Private variables + private boolean homeCommand; + private JButton homeButton1; + private JLabel label; + public HomeButton() { + createButton(); + } + + // Constructor + public void createButton() { + homeCommand = false; + + setBorder(BorderFactory.createEmptyBorder(30, 30, 10, 30)); + setLayout(new GridLayout(0, 1)); + + homeButton1 = new JButton("GO TO HOME"); + homeButton1.addActionListener(this); + homeButton1.setActionCommand("home"); + add(homeButton1); + + label = new JLabel("Status : Deactivated"); + add(label); + } + + // Action event function + public void actionPerformed(ActionEvent e) { + String s = e.getActionCommand(); + switch (s) { + case "home" : + homeCommand = true; + label.setText("Status : Activated"); + break; + default: + System.out.println("Unknown Action Command: " + s); + break; + } + } + + // HomeCommand variable functions + public void resetHomeCommand() { + homeCommand = false; + } + + public boolean getHomeCommand() { + return homeCommand; + } +} + +// Delcare trick sim modes +class TrickSimMode { + public static final int INIT = 0; + public static final int FREEZE = 1; + public static final int RUN = 5; +} + +// Main display class extending the JFrame +public class HomeDisplay extends JFrame { + + // Private variables + private HomeButton buttonPanel; + private BufferedReader in; + private DataOutputStream out; + + // Constructor + public HomeDisplay() { + setTitle("WheelBot Control"); + + buttonPanel = new HomeButton(); + add(buttonPanel, BorderLayout.CENTER); + setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + } + + // Extention functions for HomeCommand + public void resetHomeCommand() { + buttonPanel.resetHomeCommand(); + } + + public boolean getHomeCommand() { + return buttonPanel.getHomeCommand(); + } + + // Connect to the Trick variable server + public void connectToServer(String host, int port ) throws IOException { + Socket socket = new Socket(host, port); + in = new BufferedReader( new InputStreamReader( socket.getInputStream())); + out = new DataOutputStream(new BufferedOutputStream(socket.getOutputStream())); + } + + private static void printHelpText() { + System.out.println( + "----------------------------------------------------------------------\n" + + "usage: java jar HomeDisplay.jar \n" + + "----------------------------------------------------------------------\n" + ); + } + + // Main: Displays the button and uses the variable server client to update veh.vehicle.homeCommanded + public static void main(String[] args) throws IOException { + + String host = "localHost"; + int port = 0; + + int ii = 0; + while (ii < args.length) { + switch (args[ii]) { + case "-help" : + case "--help" : { + printHelpText(); + System.exit(0); + } break; + default : { + port = (Integer.parseInt(args[ii])); + } break; + } + ++ii; + } + + // Display the GUI + HomeDisplay displayGUI = new HomeDisplay(); + int CommandedHome = 0; + boolean go = true; + int simMode = 0; + + displayGUI.pack(); + displayGUI.setVisible(true); + + // Check for valid port + if (port == 0) { + System.out.println("No variable server port specified."); + printHelpText(); + System.exit(0); + } + + // Connect to Trick variable server + System.out.println("Connecting to: " + host + ":" + port); + displayGUI.connectToServer(host, port); + + displayGUI.out.writeBytes("trick.var_set_client_tag(\"Home Button\") \n"); + displayGUI.out.flush(); + + // Cycically read the simulation mode and the homeCommanded value + displayGUI.out.writeBytes("trick.var_pause() \n" + + "trick.var_add(\"trick_sys.sched.mode\")\n" + + "trick.var_add(\"veh.vehicle.homeCommanded\") \n" + + "trick.var_ascii() \n" + + "trick.var_cycle(0.1) \n" + + "trick.var_unpause() \n" ); + + displayGUI.out.flush(); + + // Loop for simulation + while (go) { + try { + String line; + String field[]; + line = displayGUI.in.readLine(); + field = line.split("\t"); + simMode = Integer.parseInt( field[1]); + CommandedHome = Integer.parseInt( field[2]); + } catch (IOException | NullPointerException e) { + go = false; + } + + // Check if homeCommanded is true (button pressed) and update veh.vehicle.homeCommanded + if (simMode == TrickSimMode.RUN) { + if (displayGUI.getHomeCommand()) { + displayGUI.out.writeBytes("veh.vehicle.homeCommanded = 1 ;\n"); + displayGUI.out.flush(); + displayGUI.resetHomeCommand(); + } + } + } // while + } // main +} // class diff --git a/trick_sims/SIM_wheelbot/models/Vehicle/include/vehicleOne.hh b/trick_sims/SIM_wheelbot/models/Vehicle/include/vehicleOne.hh index f1290742..321f6c89 100644 --- a/trick_sims/SIM_wheelbot/models/Vehicle/include/vehicleOne.hh +++ b/trick_sims/SIM_wheelbot/models/Vehicle/include/vehicleOne.hh @@ -57,6 +57,12 @@ class VehicleOne { double batteryVoltage; + // Homing Button Variables + // Get input from Trick server client for homing + int homeCommanded; + // If Wheelbot was homed and end of simulation + bool endofHoming; + void add_waypoint(double x, double y); int default_data(); diff --git a/trick_sims/SIM_wheelbot/models/Vehicle/src/vehicleOne.cpp b/trick_sims/SIM_wheelbot/models/Vehicle/src/vehicleOne.cpp index 319ceaba..ea3a41ea 100644 --- a/trick_sims/SIM_wheelbot/models/Vehicle/src/vehicleOne.cpp +++ b/trick_sims/SIM_wheelbot/models/Vehicle/src/vehicleOne.cpp @@ -61,6 +61,10 @@ int VehicleOne::default_data() { batteryVoltage = 5.0; + // Initialize homing variables + homeCommanded = 0; + endofHoming = false; + return 0; } @@ -107,6 +111,12 @@ void VehicleOne::control() { navigator->setHeading(heading); navigator->setLocation(position[0], position[1]); + // Check to see if the variable server client input for homeCommanded has been activated + // if so, go home and declare end of simulation + if (homeCommanded == 1 && endofHoming == false) { + vehicleController->gohome(); + endofHoming = true; + } vehicleController->update(); } @@ -169,6 +179,18 @@ int VehicleOne::state_deriv() { // Body Rotational Acceleration headingAccel = vehicleZTorque / ZAxisMomentofInertia; + // If the simulation is at the end, the vehicle stops moving + if (vehicleController->getStatus() == true) { + forceTotal[0] = 0; + forceTotal[1] = 0; + rightMotorSpeed = 0; + leftMotorSpeed = 0; + velocity[0] = 0; + velocity[1] = 0; + headingRate = 0; + headingAccel = 0; + } + // Body Linear Acceleration acceleration[0] = forceTotal[0] / vehicleMass; acceleration[1] = forceTotal[1] / vehicleMass; From 5638a62a22c04466946da1d0fa60f6e9a87ec677 Mon Sep 17 00:00:00 2001 From: avinashc99 Date: Mon, 3 Aug 2020 10:44:52 -0500 Subject: [PATCH 2/2] Variable Name Update --- .../models/Control/include/vehicleController.hh | 4 ++-- .../models/Control/src/vehicleController.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh b/trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh index 0f190dd0..cf3624cd 100644 --- a/trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh +++ b/trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh @@ -28,7 +28,7 @@ class VehicleController { // Homing Functions // Commands wheelbot to navigate to home void gohome(); - // Returns the value of the variable endofSimulation + // Returns the value of the variable endofWaypoints bool getStatus(); private: @@ -43,7 +43,7 @@ class VehicleController { // Homing variables // Records if end of simulation - bool endofSimulation; + bool endofWaypoints; // Records if told to go home bool homeCommanded; diff --git a/trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp b/trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp index 5899ab5d..189cb573 100644 --- a/trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp +++ b/trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp @@ -19,7 +19,7 @@ VehicleController::VehicleController( std::vector* wayPoints, printDestination(); // Initialize homing variables - endofSimulation = false; + endofWaypoints = false; homeCommanded = false; } @@ -51,25 +51,25 @@ void VehicleController::printDestination() { } } -// Returns the value of the variable endofSimulation +// Returns the value of the variable endofWaypoints bool VehicleController::getStatus() { - return endofSimulation; + return endofWaypoints; } void VehicleController::update() { - if (destination == waypointQueue->end() && endofSimulation == false) { + if (destination == waypointQueue->end() && endofWaypoints == false) { if (homeCommanded == false) { driveController.update(0.0, 0.0); } - endofSimulation = true; + endofWaypoints = true; } else { double distance_err = navigator.distanceTo(*destination); if ( distance_err > arrivalDistance) { double heading_err = navigator.bearingTo(*destination); driveController.update(distance_err, heading_err); } else { - if (endofSimulation != true) { + if (endofWaypoints != true) { std::cout << "Arrived at Destination." << std::endl; destination ++; printDestination();