Documentation updates for Graphics, Guidance, and Control models. Ref #1011

trick_sims/SIM_wheelbot/Modified_data/pentagon.snackpoints

@@ -0,0 +1,5 @@
+1.0000,0.0,0.0,images/cheese_64x64.png
+0.3093,0.9511,0.0,images/strawberry_64x64.png
+-0.8090,0.5878,0.0,images/cheese_64x64.png
+-0.8090,-0.5878,0.0,images/strawberry_64x64.png
+0.3090,-0.9511,0.0,images/cheese_64x64.png

trick_sims/SIM_wheelbot/Modified_data/pentagon.waypoints

@@ -0,0 +1,5 @@
+1.0000,0.0,0.0,images/wp0.png
+0.3093,0.9511,0.0,images/wp1.png
+-0.8090,0.5878,0.0,images/wp2.png
+-0.8090,-0.5878,0.0,images/wp3.png
+0.3090,-0.9511,0.0,images/wp4.png

trick_sims/SIM_wheelbot/models/Control/README.md

@@ -4,6 +4,7 @@
 
 * [class DifferentialDriveController](#class-DifferentialDriveController)<br>
 * [class VehicleController](#class-VehicleController)<br>
+* [class PIDController](#class-PIDController)<br>
 
 ---
 
@@ -43,7 +44,7 @@ The wheel speed limit, and equations [#1](#EQ_1_heading_rate) and [#2](EQ_2_rang
 First, we'll determine our desired heading rate. We'll make it proportional to the heading error (the difference between our desired heading and our actual heading), and subjected to a heading rate limit.
 
 ```
-    Heading Rate PID controller goes here.
+   desiredHeadingRate = headingctrl.getOutput(headingRateLimit, heading_err);
 ```
 
 | Access  | Member Name        | Type   | Units  | Value  |
@@ -70,7 +71,12 @@ Because half of the difference is above the average, and half is below, half of
 With **availableWheelSpeedForRangeRate** determined, we can figure our range rate. Within a  "slow-down distance" of a destination, we'll make it proportional to the distance error, otherwise it will be equal to our maximum available speed.
 
 ```
-    Range Rate PID controller goes here.
+    if (distance_err > slowDownDistance ) {
+        wheelSpeedForRangeRate = availableWheelSpeedForRangeRate;
+    } else {
+        wheelSpeedForRangeRate = wheelspeedctrl.getOutput(availableWheelSpeedForRangeRate, distance_err);
+    }
+    desiredRangeRate = wheelSpeedForRangeRate * wheelRadius;
 ```
 
 | Access  | Member Name      | Type   | Units  | Value  |
@@ -102,8 +108,10 @@ Here, wheel speed is positive forward, and negative is backwards. For the motor
 
 Finally, we set the commanded speeds for the right and left motors:
 
-    rightMotorController.setCommandedSpeed( rightMotorSpeedCommand);
-    leftMotorController.setCommandedSpeed( leftMotorSpeedCommand);
+```
+rightMotorController.setCommandedSpeed( rightMotorSpeedCommand);
+leftMotorController.setCommandedSpeed( leftMotorSpeedCommand);
+```
 
 | Access  | Member Name           | Type   | Units  | Value  |
 |---------|-----------------------|--------|--------|--------|
@@ -153,21 +161,76 @@ Stop the vehicle.
                       DifferentialDriveController& driveController,
                       double arrival_distance);
 ```
+Initialize the vehicle controller instance.
+
+* ```waypointQueue``` - the queue of waypoints that will determine the path of the vehicle.
+
+* ```navigator``` - pointer to an instance of the vehicle's [Navigator](../Guidance/README.md##class-Navigator).
+
+* ```driveController``` - pointer to an instance of the vehicle's [DifferentialDriveController](#class-DifferentialDriveController).
 
 ### Member Functions
 
 ```
 int getCurrentDestination(Point& currentDestination);
 ```
+Get the destination-[Point](../Guidance/README.md##class-Point) to which the vehicle is currently moving. Return 0 on success, and 1 on failure.
 
 ```
 void setWayPointQueue( std::vector<Point>* waypointQueue );
 ```
+Set the queue of waypoints that will determine the path of the vehicle.
 
 ```
 void printDestination();
 ```
+Print the current destination to ```std::cout```.
 
 ```
 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.
+
+<a id=class-PIDController></a>
+# class PIDController
+
+![PID Controller Diagram](images/PIDController.png)
+
+### Constructor
+
+```
+PIDController::PIDController(
+    double kp,   // proportional gain
+    double ki,   // integral gain
+    double kd,   // derivative gain
+    double omax, // limiter maximum
+    double omin, // limiter minimum
+    double dt,   // sample period
+    double tc    // filter time constant
+    );
+```
+
+
+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);
+```
+This function generates the control command (corresponding to **cmd** in the above diagram.) for the plant. The "plant" is the thing that you are controlling.
+
+For example, the plant could be a lunar lander. The measured value could be altitude, and the command could be motor thrust. The setpoint is the what you want the output of the plant to be. The setpoint could be an altitude of 100 feet. The measured_value is the actual output (the actual altitude) of the lander plus perhaps some noise (because sensors aren't perfect).
+
+### References
+
+The following videos provide some excellent instruction about PID control:
+
+[Understanding PID Control, Part 1: What is PID Control?](https://www.youtube.com/watch?v=wkfEZmsQqiA)
+
+[Understanding PID Control, Part 2: Expanding Beyond a Simple Integral](https://www.youtube.com/watch?v=NVLXCwc8HzM)
+
+[Understanding PID Control, Part 3: Part 3: Expanding Beyond a Simple Derivative](https://www.youtube.com/watch?v=7dUVdrs1e18)
+
+[Understanding PID Control, Part 4: A PID Tuning Guide](https://www.youtube.com/watch?v=sFOEsA0Irjs)

trick_sims/SIM_wheelbot/models/Graphics/README.md

@@ -0,0 +1,160 @@
+# Graphics
+
+**Contents**
+
+* [class Feature](#class-Feature)<br>
+* [class ArenaMap](#class-ArenaMap)<br>
+* [class EVDisplay](#class-EVDisplay)<br>
+
+---
+
+<a id=class-Feature></a>
+## class Feature
+
+### Description
+
+The Feature class represents a graphical feature in an [ArenaMap](#class-ArenaMap). It consists of a 2D position, an orientation, and an image. The position is specified by North, and West.
+
+| Access | Member Name   | Type         | Units  | Value  |
+|--------|---------------|--------------|--------|--------|
+| public | north         |double        |   --   |        |
+| public | west          |double        |   --   |        |
+| public | heading       |double        |   --   |        |
+| public | bufImage      |BufferedImage |   --   |        |
+
+### Constructor
+
+```
+public Feature(double N, double W, double H, String imageFile)
+```
+Create a Feature with position (**N,W**), heading (**H**), and image stored in **imageFile**.
+
+### Member Functions
+
+```
+public void setState(double N, double W, double H)
+```
+
+Set the position (**N,W**), heading (**H**) of the Feature.
+
+---
+
+<a id=class-ArenaMap></a>
+## class ArenaMap 
+extends [JPanel](https://docs.oracle.com/javase/7/docs/api/javax/swing/JPanel.html)
+
+### Description
+The ArenaMap class creates, and manages a graphical display of [Features](#class-Feature) in an arena.
+
+| Access | Member Name     | Type           | Units  | Value  |
+|--------|-----------------|----------------|--------|--------|
+| private | featureList    | List\<Feature\>|   --   | Constructor Arg|
+| private | metersPerPixel | double         |   --   | Constructor Arg|
+| private | groundColor    | Color          |   --   | White  |
+
+### Constructor
+
+```
+public ArenaMap(List<Feature> flist, double mapScale)
+```
+Initialize an ArenaMap object to the given mapscale, and containing the given list of graphical features.
+
+### Member Functions
+
+```
+public void SetScale (double mapScale);
+```
+Set the screen resolution of the Arenamap (meters/pixel).
+
+```
+private void doDrawing(Graphics g);
+```
+
+---
+
+## class EVDisplay 
+extends [JFrame](https://docs.oracle.com/javase/7/docs/api/javax/swing/JFrame.html)
+
+### Description
+This class implements a Trick variable server client application for SIM_wheelbot that displays the vehicle and its waypoints. The position of the vehicle is updated over time.  
+
+#### Running the Client
+```
+java -jar EVDisplay.jar -v <vehicle-image-file> -w <waypoints-file> <port>
+```
+
+* **-v** : specify the image to be used for the vehicle.
+* **-w** : specify the waypoint positions, headings, and images, in the format shown below.
+* **\<port\>** : the port number for the Trick variable server.
+
+#### Example :
+
+```
+java -jar EVDisplay.jar -v "images/twoWheelRover.png" -w "Modified_data/pentagon.waypoints" <port>
+```
+
+##### ```Modified_data/pentagon.waypoints``` :
+
+```
+1.0000,0.0,0.0,images/wp0.png
+0.3093,0.9511,0.0,images/wp1.png
+-0.8090,0.5878,0.0,images/wp2.png
+-0.8090,-0.5878,0.0,images/wp3.png
+0.3090,-0.9511,0.0,images/wp4.png
+```
+
+![Arena](images/Arena.png)
+
+#### A More Fun Example: 
+
+```
+java -jar EVDisplay.jar -v "images/mouse_128x128.png" -w "Modified_data/pentagon.snackpoints" <port>
+```
+
+##### ```Modified_data/pentagon.snackpoints``` :
+
+```
+1.0000,0.0,0.0,images/cheese_64x64.png
+0.3093,0.9511,0.0,images/strawberry_64x64.png
+-0.8090,0.5878,0.0,images/cheese_64x64.png
+-0.8090,-0.5878,0.0,images/strawberry_64x64.png
+0.3090,-0.9511,0.0,images/cheese_64x64.png
+```
+![Arena](images/Arena_alt.png)
+
+### Constructor
+```
+public EVDisplay(ArenaMap arena)
+```
+
+### Member Functions
+
+```
+public void connectToServer(String host, int port ) throws IOException
+```
+Connect to the Trick variable server as specified by **host** and **port**.
+
+```
+public void drawArenaMap()
+```
+Calls the [ArenaMap](#class-ArenaMap)'s repaint() method, that is inherited from the Java-Swing JPanel class. This causes the ArenaMap's doDrawing() method to be called, which draws the arena.
+
+```
+public static void main(String[] args) throws IOException
+```
+
+* Process Args
+* Validate Parameters
+* Read and process the waypoint file:
+   * For each waypoint, create a  [Feature](#class-Feature) and add it to the feature-list.
+* Create a Feature for the vehicle, and add it to the feature-list.
+* Connect to the Trick variable server on the local computer ("localhost") at the port number specified by the applicaton arguments.
+* Request the vehicle position and heading at 10Hz.
+* Enter a continuous while loop that reads, and correspondingly sets the vehicle state.
+
+
+
+
+
+
+

trick_sims/SIM_wheelbot/models/Guidance/README.md

@@ -0,0 +1,262 @@
+# Guidance
+
+**Contents**
+
+* [class Point](#class-Point)<br>
+* [class Arena](#class-Arena)<br>
+* [class Navigator](#class-Navigator)<br>
+* [class GridSquare](#class-GridSquare)<br>
+* [function FindPath](#function-FindPath)<br>
+
+---
+
+<a id=class-Point></a>
+## class Point
+This class simply represents a 2D point.
+
+### Description
+| Access  | Member Name      | Type   | Units  | Value  |
+|---------|------------------|--------|--------|--------|
+| public  | x                | double | --     |        |
+| public  | y                | double | --     |        |
+
+### Constructors
+```
+    Point(): x(0.0),y(0.0) {}
+```
+```
+    Point(double X, double Y): x(X),y(Y) {}
+```
+### Member Functions
+
+
+---
+
+
+<a id=class-Navigator></a>
+## class Navigator
+
+### Description
+
+The Navigator class represents simple navigation state. It is simply a position, and heading on a map. A position on the map is represented by a [Point](#class-Point).
+
+| Access  | Member Name      | Type                | Units  | Value  |
+|---------|------------------|---------------------|--------|--------|
+| public  | heading          |double               |   --   |        |
+| public  | location         |[Point](#class-Point)|   --   |        |
+
+
+#### Coordinate Systems
+
+![](images/CoordSystems.png)
+
+* The **map** coordinate system is simply a 2 dimensional cartesion system on which we can place vehicles, waypoints, and so forth.
+* A vehicle's **platform** coordinate system is the same as the map coordinate system except that its origin is fixed to the center of the vehicle.
+* A vehicle's **body** coordinate system is fixed to the vehicle's location and its heading. It is the same as the vehicle's platform coordinate system except that it's rotated about the vehicle center by its heading angle.
+
+### Constructor
+
+```
+        Navigator(double initial_heading, Point initial_location);
+        
+```
+
+### Member Functions
+```
+        void setHeading(double heading);
+```
+
+```
+        void setLocation(double north, double west);
+```
+
+```
+        double distanceTo(Point& mapPoint);
+```
+
+<!--
+\vec{\delta} = \vec{location} - point_{map}
+{distance} = \sqrt{ \delta_{x}^{2}  + \delta_{y}^{2}}
+-->
+![](images/delta.png)
+
+![](images/distance.png)
+   
+```        
+        double bearingTo(Point& mapPoint);
+```
+<!--
+point_{platform}=\texttt{convertMapToPlatform}(point_{map})
+point_{body}=\texttt{convertPlatformToBody}(point_{platform})
+bearing=\frac{\arcsin(point_{body_x})}{\texttt{distanceTo}(point_{map})}
+-->
+
+```
+        Point convertMapToPlatform(Point& mapPoint);
+```
+<a id=Eq1-map-to-platform></a>
+<!--
+\vec{point_{platform}} = \vec{point_{map}} -  \vec{location}_{map}
+-->
+![](images/EQ1-map-to-platform.png)
+
+```
+        Point convertPlatformToMap(Point& platformPoint);
+```
+<a id=Eq2-platform-to-map></a>
+<!--
+\vec{point_{map}} = \vec{point_{platform}} +  \vec{location}_{map}
+-->
+![](images/EQ2-platform-to-map.png)
+
+```
+        Point convertPlatformToBody(Point& platformPoint);
+```
+
+<a id=Eq3-platform-to-body></a>
+<!--
+\vec{point_{body}} =
+\begin{pmatrix} \cos (heading)&sin(heading)) \\ -\sin(heading)&cos(heading) \end{pmatrix}
+\vec{point_{platform}}
+-->
+![](images/EQ3-platform-to-body.png)
+
+```
+        Point convertBodyToPlatform(Point& bodyPoint);
+```
+
+<a id=Eq4-body-to-platform></a>
+<!--
+\vec{point_{platform}} = 
+\begin{pmatrix} \cos (heading)&-\sin(heading)) \\ sin(heading)&cos(heading) \end{pmatrix}
+\vec{point_{body}}
+-->
+![](images/EQ4-body-to-platform.png)
+
+
+---
+
+
+<a id=class-GridSquare></a>
+## class GridSquare
+
+### Description
+A GridSquare is a component of an [Arena](#class-Arena). It represents the attributes of a specific location within the Arena.
+
+| Access  | Member Name      | Type        | Units  | Value  |
+|---------|------------------|-------------|--------|--------|
+| public  | isBlocked        | bool        | --     |        |
+| public  | mark             | char        | --     |        |
+| public  | parent           | GridSquare* | --     |        |
+| public  | g_score          | int         | --     |        |
+| public  | f_score          | int         | --     |        |
+
+* **isBlocked** means that the grid square cannot be entered, as if because of an obstacle, or a wall.
+* **mark** is simply a single printable character that indicates the status of a gridsquare when printing out the arena.
+
+The remaining three attributes represent a path across the Arena as determined by the [A* Path Finding Algorithm](https://en.wikipedia.org/wiki/A*_search_algorithm).
+
+* **parent** is a pointer to the previous Gridsquare, in the path from an origin Gridsquare to the current Gridsquare. When the A* search arrives at the destination, the path solution is the list of Gridsquares linked by the parent pointers. 
+* **g_score** is the "cost" to travel from the origin to the current Gridsquare.
+* **f_score** is the sum of the g_score and the estimated cost to the path destination (h).
+
+### Constructor
+### Member Functions
+
+
+---
+
+<a id=class-Arena></a>
+## class Arena
+
+An Arena is an 2D array of [GridSquares](#class-GridSquare). It represents a physical environment, with obstacles, in which a vehicle can navigate. 
+
+### Description
+| Access  | Member Name | Type        | Units  | Value  |
+|---------|-------------|-------------|--------|--------|
+| public  | height      | int         |   --   |        |
+| public  | width       | int         |   --   |        |
+| public  | grid        | GridSquare* |   --   |        |
+
+
+### Constructor
+
+```
+        Arena(unsigned int width, unsigned int height);
+```
+
+```
+        Arena(unsigned int width, unsigned int height, unsigned char bits[]);
+```
+
+```
+        ~Arena();
+```
+### Member Functions
+
+
+```
+        int getHeight(){return height;}
+```
+Return the height (number of Gridsquare rows) in the Arena.
+
+```
+        int getWidth(){return width;}
+```
+Return the width (number of Gridsquare columns) in the Arena.
+
+```
+        void block(unsigned int x, unsigned int y);
+```
+Block the Gridsquare at (x,y) , so that it can't be traversed.
+
+```
+        void unblock(unsigned int x, unsigned int y);
+```
+Un-block the Gridsquare at (x,y), so that it can be traversed.
+
+```
+        void mark(unsigned int x, unsigned int y, char c);
+```
+
+```
+        std::vector<GridSquare*> getNeighbors(GridSquare* gridSquarePointer);
+```
+Return a list of non-blocked, immediate neighbors, of the given Gridsquare. The returned list may contain up to eight neighbors.
+
+```
+        GridSquare* getGridSquare(unsigned int x, unsigned int y);
+```
+Return a pointer to the Gridsquare object at (x,y).
+
+```
+        int getGridSquareCoordinates(GridSquare* gridSquarePointer, Point& coords);
+```
+Given a pointer to a Gridsquare object within the Arena, return its (x,y) coordinates.
+
+```
+        int movementCostEstimate(GridSquare* orig, GridSquare* dest, int& cost);
+```
+Calculate a **cost** estimate of the to travel from **orig** to **dest**. Return 0 on success, 1 on failure.
+
+```
+        int distanceBetween(GridSquare* orig, GridSquare* dest, int& distance);
+```
+Calculate the straight-line distance from **orig** to **dest**.
+
+---
+
+
+<a id= function-FindPath></a>
+## function FindPath
+
+```
+std::vector<Point> FindPath( GridSquare* origin, GridSquare* goal, Arena* arena)
+```
+
+### Description
+Using the [A* Path Finding Algorithm](https://en.wikipedia.org/wiki/A*_search_algorithm), return a path (as a vector of [Points](#class-Point)) from **origin** to **goal**, in the given [Arena](#class-Arena).
+
+
+### Constructor
+### Member Functions

trick_sims/SIM_wheelbot/models/README.md

@@ -0,0 +1,11 @@
+# Wheelbot Models
+
+* [Control](Control/README.md)
+
+* [Battery](Battery/README.md)
+* [Electrical](Electrical/README.md)
+* [Guidance](Guidance/README.md)
+* [Control](Control/README.md)
+* [Graphics](Graphics/README.md)
+* [Motor](Motor/README.md)
+* [Vehicle](Vehicle/README.md)