Home Button and End Behavior Improvments to Wheelbot

trick_sims/SIM_wheelbot/README.md

@@ -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

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)

trick_sims/SIM_wheelbot/models/Control/README.md

@@ -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.
+
 <a id=class-PIDController></a>
 # class PIDController
 

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

trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp

@@ -17,6 +17,10 @@ VehicleController::VehicleController( std::vector<Point>* wayPoints,
     waypointQueue = wayPoints;
     destination = waypointQueue->begin();
     printDestination();
+
+    // Initialize homing variables
+    endofSimulation = false;
+    homeCommanded = false;
 }
 
 void VehicleController::setWayPointQueue( std::vector<Point>* 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()) {
+  if (destination == waypointQueue->end() && endofSimulation == false) {
+      if (homeCommanded == false) {
           driveController.update(0.0, 0.0);
-        driveController.stop();
+      }
+      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();
           }
       }
   }
-
-
+}

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 "-------------------------------------------------------------------------------"

trick_sims/SIM_wheelbot/models/GUIControl1/README.md

@@ -0,0 +1,116 @@
+# Graphics
+
+**Contents**
+
+* [class HomeButton](#class-HomeButton)<br>
+* [class HomeDisplay](#class-HomeDisplay)<br>
+* [class TrickSimMode](#class-TrickSimMode)<br>
+
+---
+
+<a id=class-HomeButton></a>
+## 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.
+
+---
+
+<a id=class-TrickSimMode></a>
+## class TrickSimMode
+
+### Description
+
+The TrickSimMode class declares variables for the state of the simulation.
+
+
+<a id=class-HomeDisplay></a>
+## 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 <port>
+```
+
+| 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.
+
+---

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 <port-number>\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

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();

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;