ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2024-12-19 21:27:54 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Home Button and End Behavior Improvments to Wheelbot

Browse Source
This commit is contained in:
avinashc99 2020-08-03 10:09:34 -05:00
parent 2f518da4ba
commit 047d31cba3
10 changed files with 459 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
trick_sims/SIM_wheelbot/README.md
View File

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

18
trick_sims/SIM_wheelbot/RUN_test/input.py
View File

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

21
trick_sims/SIM_wheelbot/models/Control/README.md
View File

@ -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:
<a id=EQ_8_rightMotorSpeedCommand></a>
@ -100,7 +100,7 @@ Here, wheel speed is positive forward, and negative is backwards. For the motor
<a id=EQ_9_leftMotorSpeedCommand></a>
* [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.
<a id=class-PIDController></a>
# 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);

12
trick_sims/SIM_wheelbot/models/Control/include/vehicleController.hh
View File

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

43
trick_sims/SIM_wheelbot/models/Control/src/vehicleController.cpp
View File

@ -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()) {
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();
}
}
}
}
}
}

36
trick_sims/SIM_wheelbot/models/GUIControl1/Makefile Normal file
View File

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

116
trick_sims/SIM_wheelbot/models/GUIControl1/README.md Normal file
View File

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

200
trick_sims/SIM_wheelbot/models/GUIControl1/src/trick/HomeDisplay.java Normal file
View File

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

6
trick_sims/SIM_wheelbot/models/Vehicle/include/vehicleOne.hh
View File

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

22
trick_sims/SIM_wheelbot/models/Vehicle/src/vehicleOne.cpp
View File

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