ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2025-02-20 09:16:20 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Update README and various other tweaks to SIM_wheelbot

Browse Source
This commit is contained in:
John M. Penn 2016-03-03 13:22:37 -06:00
parent 4c471eb32d
commit 4b1b1d451b
11 changed files with 131 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
trick_sims/SIM_sun/RUN_test/input.py
View File

@ -59,10 +59,10 @@ sun_predictor.sun.observer_offset_from_UTC = -6
""" LOCAL TIME """
""" Set local time here. """
""" ======================================== """
sun_predictor.sun.local_time.year = 2006
sun_predictor.sun.local_time.month = 8
sun_predictor.sun.local_time.day = 15
sun_predictor.sun.local_time.hour = 0
sun_predictor.sun.local_time.year = 2016
sun_predictor.sun.local_time.month = 2
sun_predictor.sun.local_time.day = 26
sun_predictor.sun.local_time.hour = 16
sun_predictor.sun.local_time.min = 0
sun_predictor.sun.local_time.sec = 0.0

26
trick_sims/SIM_wheelbot/Modified_data/cross.waypoints
View File

@ -1,13 +1,13 @@
2.0, 0.0, 0.0,images/wp1.png
2.0, 2.0, 0.0,images/wp2.png
0.0, 2.0, 0.0,images/wp3.png
0.0, 4.0, 0.0,images/wp4.png
-2.0, 4.0, 0.0,images/wp5.png
-2.0, 2.0, 0.0,images/wp6.png
-4.0, 2.0, 0.0,images/wp7.png
-4.0, 0.0, 0.0,images/wp8.png
-2.0, 0.0, 0.0,images/wp9.png
-2.0,-2.0, 0.0,images/wp10.png
0.0,-2.0, 0.0,images/wp11.png
0.0, 0.0, 0.0,images/wp0.png
3.5,-3.5, 0.0,images/CompassRose.png
0.5, -0.5, 0.0,images/wp0.png
1.5, -0.5, 0.0,images/wp1.png
1.5, 0.5, 0.0,images/wp2.png
0.5, 0.5, 0.0,images/wp3.png
0.5, 1.5, 0.0,images/wp4.png
-0.5, 1.5, 0.0,images/wp5.png
-0.5, 0.5, 0.0,images/wp6.png
-1.5, 0.5, 0.0,images/wp7.png
-1.5, -0.5, 0.0,images/wp8.png
-0.5, -0.5, 0.0,images/wp9.png
-0.5, -1.5, 0.0,images/wp10.png
0.5, -1.5, 0.0,images/wp11.png
0.0, 0.0, 0.0,images/CompassRose.png

3
trick_sims/SIM_wheelbot/Modified_data/vehicleState.dr
View File

@ -13,13 +13,10 @@ drg[DR_GROUP_ID].set_cycle(0.05)
drg[DR_GROUP_ID].set_single_prec_only(False)
drg[DR_GROUP_ID].add_variable("veh.vehicle.position[0]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.position[1]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.position[2]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.velocity[0]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.velocity[1]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.velocity[2]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.acceleration[0]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.acceleration[1]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.acceleration[2]")
drg[DR_GROUP_ID].add_variable("veh.vehicle.heading")
drg[DR_GROUP_ID].add_variable("veh.vehicle.headingRate")
drg[DR_GROUP_ID].add_variable("veh.vehicle.rightMotorSpeed")

66
trick_sims/SIM_wheelbot/README.md
View File

@ -1,4 +1,66 @@
#SIM\_wheelbot
---
This is a simulation of a two-wheeled robotic electric vehicle.
---
SIM\_wheelbot is a simulation of a two-wheeled electric vehicle that follows a set of waypoints.
![Picture of Vehicle](images/Figure1.png)
---
### Building the Simulation
In the SIM\_wheelbot directory, type **trick-CP** to build the simulation executable. When it's complete, you should see:
```
=== Simulation make complete ===
```
Now **cd** into models/Graphics/ and type **make**. This builds the display client for the simulation.
### Running the Simulation
In the SIM_wheelbot directory:
```
% S_main_*.exe RUN_test/input.py
```
---
### Inputs
Variable | Type | Units | Default Value
-----------------------------------------------|----------------|-------|--------------
veh.vehicle.distanceBetweenWheels | double | m | 0.183
veh.vehicle.wheelRadius | double | m | 0.045
veh.vehicle.wheelSpeedLimit | double | r/s | 8.880
veh.vehicle.headingRateLimit | double | r/s | 𝛑/4
veh.vehicle.wheelDragConstant | double | -- | 1.875
veh.vehicle.corningStiffness | double | -- | 10.0
veh.vehicle.slowDownDistance | double | -- | 0.5
veh.vehicle.arrivalDistance | double | -- | 0.1
![Picture of Vehicle](images/Figure2.png)
#### Adding Waypoints
Waypoints, for the vehicle to follow, are added with a call to
veh.vehicle.add_waypoint( double N, double W )
### Input/Output
Variable | Type | Units
-----------------------------------------------|----------------|-------
veh.vehicle.position | double[2] | m
veh.vehicle.velocity | double[2] | m
veh.vehicle.heading | double | r
veh.vehicle.headingRate | double | r
### Outputs
Variable | Type | Units
----------------------------------------------|----------------|--------
veh.vehicle.acceleration | double[2] | m
veh.vehicle.headingAccel | double | r
veh.vehicle.driveForce | double[2] | N
veh.vehicle.lateralTireForce | double[2] | N
veh.vehicle.rollingResistForce | double[2] | N
veh.vehicle.forceTotal | double[2] | N
veh.vehicle.vehicleZTorque | double | N*m

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

@ -5,14 +5,16 @@ trick.TMM_reduced_checkpoint(0)
veh_integloop.getIntegrator(trick.Runge_Kutta_4, 8)
veh.vehicle.position[0] = 0.0
veh.vehicle.position[1] = 0.0
veh.vehicle.heading = 0.0;
#==========================================
# Configure the Vehicle.
#==========================================
veh.vehicle.position = [0.0, 0.0]
veh.vehicle.heading = 0.0
veh.vehicle.distanceBetweenWheels = 0.183
veh.vehicle.wheelRadius = 0.045
veh.vehicle.wheelSpeedLimit = 8.880
veh.vehicle.headingRateLimit = 3.14159/3
veh.vehicle.headingRateLimit = 3.14159/4
veh.vehicle.slowDownDistance = 0.5
veh.vehicle.arrivalDistance = 0.1
@ -23,7 +25,7 @@ veh.vehicle.arrivalDistance = 0.1
waypoints_path = "Modified_data/cross.waypoints"
fp = open(waypoints_path, "r")
for line in fp:
fields = line.split(r",")
fields = line.split(",")
veh.vehicle.add_waypoint( float(fields[0]), float(fields[1]))
#==========================================
@ -35,14 +37,14 @@ EVDisplay_path = "models/Graphics/dist/EVDisplay.jar"
if (os.path.isfile(EVDisplay_path)) :
EVDisplay_cmd = "java -jar " \
+ EVDisplay_path \
+ " -v images/twoWheelRover.png " \
+ " -v images/twoWheelRover.png" \
+ " -w " + waypoints_path \
+ " " + str(varServerPort) + " &" ;
print(EVDisplay_cmd)
os.system( EVDisplay_cmd);
else :
print('============================')
print('EVDisplay needs to be built.')
print('============================')
print('==================================================================================')
print('EVDisplay needs to be built. Please \"cd\" into models/Graphics and type \"make\".')
print('==================================================================================')
trick.stop(100)

BIN
trick_sims/SIM_wheelbot/images/Figure1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 20 KiB

BIN
trick_sims/SIM_wheelbot/images/Figure2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 36 KiB

17
trick_sims/SIM_wheelbot/models/Control/src/differentialDriveController.cpp
View File

@ -37,17 +37,18 @@ int DifferentialDriveController::update( double distance_err, // m
rightMotorSpeedCommand = 0.0;
leftMotorSpeedCommand = 0.0;
// double K = (2.0 * wheelRadius / distanceBetweenWheels);
if ( cos(heading_err) > 0.999 ) {
desiredHeadingRate = 0.0;
} else if (heading_err > 0.0 ) {
desiredHeadingRate = headingRateLimit;
// If the vehicle heading is within 2 degrees of the target, then the
// heading desired heading rate is proportional to the heading error.
if ( cos(heading_err) > cos(2.0 * (PI/180.0))) {
desiredHeadingRate = heading_err/(2.0*(PI/180.0)) * headingRateLimit;
} else {
desiredHeadingRate = -headingRateLimit;
if (heading_err > 0.0) {
desiredHeadingRate = headingRateLimit;
} else {
desiredHeadingRate = -headingRateLimit;
}
}
// double wheelSpeedForHeadingRate = desiredHeadingRate / K;
double wheelSpeedForHeadingRate = (desiredHeadingRate * distanceBetweenWheels) / (2.0 * wheelRadius);
double availableWheelSpeedForRangeRate = wheelSpeedLimit - fabs(wheelSpeedForHeadingRate);

39
trick_sims/SIM_wheelbot/models/Graphics/src/trick/EVDisplay.java
View File

@ -22,6 +22,7 @@ import java.util.*;
import javax.swing.ImageIcon;
import javax.swing.JFrame;
import javax.swing.JPanel;
import java.awt.Color;
/**
*
@ -58,11 +59,13 @@ class ArenaMap extends JPanel {
private List<Feature> featureList;
private double metersPerPixel;
private Color groundColor;
public ArenaMap(List<Feature> flist, double mapScale) {
featureList = flist;
metersPerPixel = mapScale;
SetScale(mapScale);
groundColor = new Color(255,255,255);
}
public void SetScale (double mapScale) {
@ -79,6 +82,9 @@ class ArenaMap extends JPanel {
int width = getWidth();
int height = getHeight();
g2d.setPaint(groundColor);
g2d.fillRect(0, 0, width, height);
// Translate map origin to the center of the panel.
Graphics2D gCenter = (Graphics2D)g2d.create();
gCenter.translate(width/2, height/2);
@ -181,23 +187,23 @@ public class EVDisplay extends JFrame {
++ii;
}
if (port == 0) {
System.out.println("No variable server port specified.");
printHelpText();
System.exit(0);
}
if (port == 0) {
System.out.println("No variable server port specified.");
printHelpText();
System.exit(0);
}
if (wayPointsFile == null) {
System.out.println("No waypoints file specified. Use the -w option to specify a waypoints file.");
printHelpText();
System.exit(0);
}
if (wayPointsFile == null) {
System.out.println("No waypoints file specified. Use the -w option to specify a waypoints file.");
printHelpText();
System.exit(0);
}
if (vehicleImageFile == null) {
System.out.println("No vehicle image file specified. Use the -v option to specify the vehicle image file.");
printHelpText();
System.exit(0);
}
if (vehicleImageFile == null) {
System.out.println("No vehicle image file specified. Use the -v option to specify the vehicle image file.");
printHelpText();
System.exit(0);
}
List<Feature> featureList = new ArrayList<>();
@ -216,7 +222,8 @@ public class EVDisplay extends JFrame {
Feature vehicle = new Feature(0, 0, Math.toRadians(0), vehicleImageFile);
featureList.add(vehicle);
EVDisplay evd = new EVDisplay( new ArenaMap( featureList, 0.01));
double mapScale = 0.005; // 5 millimeters per pixel
EVDisplay evd = new EVDisplay( new ArenaMap( featureList, mapScale));
evd.setVisible(true);
System.out.println("Connecting to: " + host + ":" + port);

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

@ -30,17 +30,16 @@ class VehicleOne {
double arrivalDistance; /* m */
double wheelSpeedLimit; /* r/s */
double headingRateLimit; /* r/s */
double wheelDragConstant; /* */
double corningStiffness; /* */
// DCMotor Parameters
double DCMotorInternalResistance; /* ohm */
double DCMotorTorqueConstant; /* N*m/amp */
double wheelDragConstant;
double corningStiffness;
double position[3]; /* m */
double velocity[3]; /* m/s */
double acceleration[3]; /* m/s2 */
double position[2]; /* m */
double velocity[2]; /* m/s */
double acceleration[2]; /* m/s2 */
double heading; /* r */
double headingRate; /* r/s */

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

@ -25,10 +25,10 @@ int VehicleOne::default_data() {
ZAxisMomentofInertia = 0.5 * vehicleMass * axelRadius * axelRadius;
// Vehicle Controller Parameters
slowDownDistance = 0.2;
slowDownDistance = 0.5;
arrivalDistance = 0.1;
wheelSpeedLimit = 8.880;
headingRateLimit = PI/3;
headingRateLimit = PI/4;
// DCMotor Parameters
// At 5v the following parameters will result in a current of
@ -47,15 +47,12 @@ int VehicleOne::default_data() {
// Dynamic State
position[0] = 0.0;
position[1] = 0.0;
position[2] = 0.0;
velocity[0] = 0.0;
velocity[1] = 0.0;
velocity[2] = 0.0;
acceleration[0] = 0.0;
acceleration[1] = 0.0;
acceleration[2] = 0.0;
heading = 0.0;
headingRate = 0.0;
@ -192,10 +189,8 @@ int VehicleOne::state_integ() {
&headingRate,
&position[0],
&position[1],
&position[2],
&velocity[0],
&velocity[1],
&velocity[2],
(double*)0
);
@ -204,10 +199,8 @@ int VehicleOne::state_integ() {
&headingAccel,
&velocity[0],
&velocity[1],
&velocity[2],
&acceleration[0],
&acceleration[1],
&acceleration[2],
(double*)0
);
@ -218,10 +211,8 @@ int VehicleOne::state_integ() {
&headingRate,
&position[0],
&position[1],
&position[2],
&velocity[0],
&velocity[1],
&velocity[2],
(double*)0
);