Information about Project
There is a dropbox on the computers with all the project files. The dropbox account is delvecchiodropbox@gmail.com with password delvecchio.
The paths in this project are stored in fig8A.txt and fig8B.txt. (Note: there may be different versions of these; the correct one should have 1327 2307 as the first line in fig8A.txt.) Also, I have been using fig8C.txt, which is fig8B.txt backwards, because there are issues with the positioning system switching between cameras in one direction but not the next. Supposedly, the corrected data in myStances[] is used for the path follower for better individual accuracy to follow the paths, while carStances[] contains the "official" positions of all the cars, so that they all use the same data in their path-following algorithm.
In the current code, the cars start moving along the two paths. At each normal time-step, if the projected positions of the cars are safe, then the velocity controller sustains the car's speed at the speed measured throughout the past time-step. Note that this does not effectively keep the car's speed constant throughout the run, but it keeps the normal acceleration close to 0, which is all that is needed. Also in each time-step, the positions of the cars are projected 1 sec. forward, and the code checks whether the projected positions are in the capture set. If so, the supervisor finds an acceleration for the current positions. If the supervisor cannot find a solution for the current positions either (which shouldn't happen in theory in an ideal situation, but during a real experiment the position of the cars can jump suddenly into the capture set), then the supervisor uses the previously found solution.
Still to be worked on: any better way to handle situations in which the cars suddenly jump into capture set? Currently, it seems that the capture set changes rapidly, because the measurement of speed is not precise and jumps a lot.
Software
To run the code
Currently the supervisor works only on the two working cars, Car 1 and Car 2. Car 1 carries Car 6's symbol, whose white background makes it easier for the positioning system to track. There should be a copy of the source code on the cars in project/Kevin/source. To run the code, first start the CPS on the three camera computers. For this project, use Kevin CPS on the camera computers. Just click the shortcuts to start the CPS.
A copy of the code should also be in the dropbox on the computers (under the folder CA and CPS), as well as on the SVN under folder "Kevin" (under the folders OFFICIAL_CA_CODE- and OFFICIAL_CPS_CODE-). Note that they differ in car_fns.h. Each car has a separate copy of car_fns.h, with respective car-dependent constants. The car_fns.h in the dropbox and the SVN have CODE_IN_CAR set to 0; this disables functions that require brainstem libraries and allow the code to be compiled without the brainstem library code.
Then on the cars, go to project/Kevin and type "make clean" followed by "make new" to compile the code into the directory i686. There is a symbolic link to i686 in the home directory; go there and type ./ca <path filename> to run the code. The file is fig8A.txt for Car 1 and fig8B.txt for Car 2. The cars need to start on their respective paths (they can start anywhere on the path, but they need to be in the right direction: see diagram). They should trudge along until the supervisor decides that they will soon enter the capture set, in which case they will start applying the respective accelerations that allow them to pass the intersection without collision.
See diagram: the top left corner is (0, 0). The center intersection is (3000, 3000). fig8A.txt is the figure 8 that goes from the top left to the bottom right; fig8B.txt goes from top right to bottom left. The total length of the path is 12984mm. The starting point (0 mm) are shown in the diagram. The intersection is at 4680mm along the path and 4680 + 6492mm along the path (since the path intersects twice). The collision zone is assumed to be every point within 600mm of the intersection, or from 4080mm - 5280mm. Note that the cars need to be going in the correct directions.
IMPORTANT! The tracking system seems to have trouble keeping track of Car 2 when going in the direction shown in the diagram. Therefore, there is another text file called fig8C.txt that is the same path as fig8B.txt, but going in the other direction opposite from that shown in the diagram. This works better for the tracking.
C Programs
ca.c
Contains the main method starting point for the program.
Flags -
KEYBOARD_INPUT: set to 1 to receive input from keyboard. No reason to set to 0. Note: if switched off, the program must be force-quit with Ctrl-C and the brainstem will not be properly closed, so the program will fail on the next run.
GET_POSITIONS: set to 1 to receive positioning data from camera computers. If on, the CPS on the computers must be running. This can be set off when debugging the cars when they do not need positioning data, since that way the CPS on the computers does not have to be started.
SUPERVISOR: runs the supervisor functions to determine the speed profiles of the cars to not collide with each other. The desired acceleration the supervisor gives is stored in desiredInput.
MAINTAIN_VELOCITY: runs a PI controller to sustain the car at the speed stored in the variable desiredSpeed.
PATHPLAN_ON: runs the path follower to keep the car on the path from the text file input by the user.
DEBUG: turns on writing useful information to a file "data.txt".
Main Variables -
Stance carStances[NUM_CARS]: contains the basic information (a Stance consists of a position, speed, 1D position on path, and heading) about each car; sent by the camera computers.
Position carPositions[NUM_CARS]: when SUPERVISOR is set to 1, the 1D positions and speed of each car are stored into carPositions and passed to the supervisor functions.
Stance myStance: contains exactly the same information as carStances[CAR_NUM - 1]. Simply used for convenience, to avoid having to type carStances[CAR_NUM - 1].<something>
int meas_count: the number of times that the camera computer has sent a position
int startTime: absolute seconds at start of program; other times marked relative to this one
struct timeval tp: current time
float dT: time between last measurement received and current measured received, in seconds
Stance myStances[DATAPOINTS]: contains all the stances stored in myStance since the beginning of the program. For example, to access the speed of the car at the previous timestep, use myStance[meas_count - 1].speed
float times[DATAPOINTS]: contains the absolute times that the measurements in myStances were taken
float steerVals[DATAPOINTS]: contains the steering values that the car used at each corresponding point
float torqueVal: at the end of each loop, this value is used to set the throttle given by the equation throttle = clip(ZERO_THROTTLE + torqueVal * THROTTLE_DEVIATION, MIN_THROTTLE, MAX_THROTTLE)
float turnVal: at the end of each loop, this value is used to set the steering, given by the equation steer = clip(turnVal - ZERO_STEER * STEER_FACTOR, -120 * STEER_FACTOR, 120 * STEER_FACTOR)
Properties myProperties: contains u_min, u_max, v_min, v_max, and which road (path) the car is on. Also stored in carProperties[CAR_NUM - 1]; again this is used for convenience.
Program -
First, several things are initialized - initializeBrainstem() is called, the startTime is initialized, the brake & steer are set to 0, and the path controller & velocity controller are loaded. Then the main loop is entered:
Main loop:
- gets the time of day and calculates dT, the timestep since the previous run of the loop
- If GET_POSITIONS is set to 1, calls getCarStances(), which receives data of the positions of all the cars from the camera computers
- If KEYBOARD_INPUT is set to 1, checks if a key was entered and performs the corresponding instructions. This is implemented as a large switch statement.
- If SUPERVISOR is set to 1, calls the supervisor methods to decide whether to override ithe input
- If MAINTAIN_VELOCITY is set to 1, sets a torque value based on the desired speed variable
- If PATHPLAN_ON is set to 1, sets a turning value to keep the car on the path
- Sets the actual throttle and steering value based on a simple linear formula in terms of the torque value and turning value found in 5) and 6), respectively. Note: If 5 or 6 are not performed, the torque value and turning value can still be set with corresponding keyboard commands.
Keyboard Commands -
- Space bar - stops program.
- m - "mark" print a marker in the console; useful if you want to go back over the output to see when something happened
- g - "go" starts the car
- h - "halt" slows the car
- b - "brake" brakes the car; there might not be much difference between h and b.
- i, j, k, l - something like arrow keys; i increases speed, k decreases speed, j moves toward left, l moves toward right
add additional commands if desired.
Notes -
In the project, almost always the units are in millimeters and seconds (e.g. speed is in mm/s, acceleration is in mm/s^2) I think the only exception are some variables on the CPS program on the camera computers, which express time in milliseconds.
The values in myStances[] may differ from the measured values sent by the computer that are stored in carStances[], because accept_data() may correct for bad measurements. However, myStance will always contain the same information as carStances[]; that variable is strictly for convenience to carStances[MY_CAR_NUM - 1]. Supposedly, the corrected data in myStances[] is used for the path follower for better individual accuracy to follow the paths, while carStances[] contains the "official" positions of all the cars, so that they all use the same data in their path-following algorithm.
Note: the values in myStances[] may differ from the measured values sent by the computer that are stored in carStances[], because accept_data() may correct for bad measurements. However, myStance will always contain the same information as carStances[]; that variable is strictly for convenience and equal to carStances[MY_CAR_NUM - 1].
car_fns.h
CODE_IN_CAR should be set to 1 in the cars and 0 on other computers without access to brainstem libraries. All parts of the code that require brainstem libraries (initializer.c, setget.c) are only compiled when CODE_IN_CAR is set to 1.
Contains variables and functions specific to each car, for example, each car's number (1-6) and throttle range. Any code specific to the cars (especially attributes that differ between the cars) should be placed here.
struct Stance: this contains the x, y position, 1D position, heading, and speed. The camera computers constantly send information about the 6 cars to each car as a string, and the car code function getCarStances[] parses the string into an array consisting of 6 stances.
struct Properties: properties of the car used for the supervisor algorithm: the parameters v_min, v_max, u_min, u_max, and which road the car is on.
ext_math.c
Contains various math functions. Should contain functions essentially unrelated to the cars, brainstem, or the experiments, for example, calculating intersection between two lines.
Functions that were in previous versions of ext_math are signum, min, and max. These are pretty straightforward; signum(x) returns either -1, 0, or 1 depending on the sign of x; min(x, y) and max(x, y) returns the minimum and maximum of x and y, respectively.
Additional functions I added:
clip(x, a, b): returns the value of x, saturated in the interval [a, b]. So the minimum possible value returned by this function is a, and the maximum possible value returned is b. Useful for things like making sure the velocity and acceleration are within the allowed bounds.
solve2Eq(A, B, C, D, E, F, *x, *y): solves the system of 2 equations Ax + By = C & Dx + Ey = F in x and y, using Cramer's Rule. Used to find intersection between two lines.
dist(x0, y0, x1, y1): finds the Euclidean distance between the points (x0, y0) and (x1, y1) using standard distance formula.
dotProduct(vx1, vy1, vx2, vy2): finds the dot product of the vectors <vx1, vy1> and <vx2, vy2> using the formula for a dot product in a standard orthonormal basis: dot product = vx1 * vx2 + vy1 * vy2
crossProduct(vx1, vy1, vx2, vy2): finds the magnitude of the cross product of the vectors <vx1, vy1> and <vx2, vy2>, using the formula that the magnitude of the cross product is the determinant of the matrix with these two vectors as columns.
standardForm(x1, y1, x2, y2, *A, *B, *C): finds A, B, C such that the line Ax + By + C = 0 passes through (x1, y1) and (x2, y2)
distToLine(x, y, x1, y1, x2, y2): finds the distance of the point (x, y) to the line passing through (x1, y1) and (x2, y2) by using the standard formula for distance to a line.
distToSegment(x, y, x1, y1, x2, y2): finds the distance of the point (x, y) to the line segment from (x1, y1) and (x2, y2); might be longer than distToLine() called with the same arguments, if the car is far from the line.
signedDistToSegment(x, y, x1, y1, x2, y2): returns a number that has the same magnitude as distToSegment() called with the same arguments, but with a sign depending on which "side" of the segment (x, y) is on.
Is positive when (x, y) is on the "right" of the line segment, when looking from (x1, y1) to (x2, y2). This is called by the path follower to determine the error of distance from the path.
projectToLine(x, y, x1, y1, x2, y2, *x_proj, *y_proj): Projects the point (x, y) onto the line passing through (x1, y1) and (x2, y2)
projectToSegment(x, y, x1, y1, x2, y2, *x_proj, *y_proj):Projects the point (x, y) onto the line segment from (x1, y1) and (x2, y2)
If the projected point is off the segment, will return the closest end-point of the segment. This is used by the CPS to project the car's position onto the path.
angleDiff(angle1, angle2): Returns the difference between the two angles, in degrees. Keeps track that this may wrap around from 0 degrees to 360 degrees.
headingTo(x0, y0, x1, y1): Finds heading (in degrees counterclockwise from positive x-axis) from (x0, y0) to (x1, y1)
getPermutations(n, *freqs): returns an array consisting of arrays, each representing a permutation with the given frequencies.
* The last element of the array is a null pointer.
* Note: all int pointers in the returned value must be freed after usage.
* Ex. getPermutations({2, 2}) returns 0,0,1,1},{0,1,0,1},{0,1,1,0},{1,0,0,1},{1,0,1,0},{1,1,0,0
This is used for the supervisor algorithm to consider all the possible orderings.
initializer.c
This code used to be in various places inside ca.c, but I moved all this code that was dependent on brainstem and system libraries into this file. Now to set up brainstem and keyboard input, only calls to the corresponding functions are required (and don't forget to close them at the end of the program).
Contains two functions; one that sets up brainstem before the car begins moving, and one that closes brainstem after the program is completed. Also contains two functions relating to keyboard input; one that loads keyboard input, and one that returns the value of the pressed key.
1. Mainly: initializeBrainstem() needs to be called in the beginning of the program; closeBrainstem() once it is done. Note: if closeBrainstem() is not called, the program will fail on the next run. (It will say ERROR: brainstem)
setget.c
Contains two functions, set() and get(), which allows the user to respectively set or get properties of the car, such as its throttle value or steering value. Old code also required a parameter of an aStemRef, but this requires the brainstem libraries directly so I now have it hidden inside initializer and setget. Now only high-level calls to get and set variables are needed.
1. To get an attribute: call get(<attribute name>) where
<attribute name> is one of SPEED, PWM, TIMESTEP, THROTTLE, STEER, VOLTAGE, BRAKE (all defined as character constants that brainstem recognizes)
SPEED gives speed measured by brainstem in cm/s (NOTE: the program measures all speeds in mm/s; easiest to change this to mm/s by multiplying by 10 immediately after reading!)
PWM gives pulse-width-modulation (used in new cars; correlated to speed)
TIMESTEP (?)
others are pretty straightforward.
2. To set an attribute: call set(<attribute name>, <new value>)
<attribute name> is one of STEER, THROTTLE, PWM, BRAKE, SWITCH
SWITCH needs to be set to 0 before and after the program
PWM can only be set on new cars; THROTTLE must be set to -1000 for this to work
others are pretty straightforward.
positioning.c
Gets a message from the camera computer that contains information about all the cars in the system. I wrote getCarStances(), which receives one package of data from Camera Computer 0. This is the only communication related to the cars (the computers still communicate amongst themselves).
1. initializeComputerConnection(CAMERA_DATA) needs to be called first.
2. Call getCarStances() (with a parameter sData which is what was returned by initializeComputerConnection()) to get all the information stored into carStances[])
3. flush_socket() when done with program.
To use positioning.c, first call initializeComputerConnection(). This function takes one integer parameter which should be either CAMERA DATA or WHEEL DATA (both defined in positioning.h), that correspond respectively to getting positioning data from the camera computers and getting input data from the manual wheel. Currently getting data from the wheel is not supported.
To get the positions of the cars, call getCarStances() with an array consisting of Stances (a struct defined in positioning.h; there must be at least as many Stances as there are cars). The function will receive a message from Camera Computer 0 consisting of the `official' stances of each car. It will write Car 1's stance into Stance 0, Car 2's stance into Stance 1, and so on. Each stance consists of:
1. x, y: integers representing the coordinates of the center of the car (in mm), which is calculated directly from the computer vision program
2. heading: an integer representing the orientation of the car, in degrees counterclockwise from the positive x axis, which is calculated from the previous position of the car that is at least a certain distance (30 cm) away
3. speed: a float representing the speed of the car (in mm/s), also calculated based on the previous position of the car at least a certain distance away.
To access one of these properties, simply call something of the form myStances[2].heading, to get the heading of Car 3, for example.
Finally, at the end of the program, flush socket() should be called.
path_fns.c
Contains functions to maintain the car at a velocity and steer the car along a specied path. This file was written essentially from scratch.
See PDF File "Lab Report 1" on the SVN repository for details and equations for the velocity controller and path follower.
supervisor.c
Contains functions to supervise the cars; given positions, speeds, and desired accelerations of all the cars, determines whether it needs to override their input accelerations.
The two important functions are:
1. float critical_x(float pos1D): takes the 1D parameter of the position of the car on the path and outputs a 1D parameter to be used for the supervisor. In general this could be the same. However, in the case of the figure 8, there are two lobes, and critical_x maps both lobes to the same output values, as they are identical. The positions given to safeInput() should be set to the output of this function.
2. float safeInput(Position carPositions[]): the most important method that takes the positions and speeds of the cars and outputs a single number representing the acceleration value for this car, or NaN = -9999.0 if no solution exists to keep the cars from venturing into the bad set.
Helper functions:
1. tryPermutation(): Given a certain ordering that the cars should pass the intersection in, tries to find a solution using that ordering.
2. getPath(): Solves the following problem: if the car before this current car, on the same road, has a path given by "above", and the car before this current car, not on the same road, is able to pass the intersection at time T, then what is the path of the current car that allows this car to leave the intersection at the earliest possible time? Note: this returns a PathData, which contains the path as well as the calculated earliest possible time. Thus, tryPermutation() merely has to iterate through the cars, and for each car find its optimal trajectory given the data of the car before it not on the same road and the car before it, on the same road, to try to successfully find a permutation.
3. getInput(): does the same thing, except without the constraint of the car before it on the same road. Note this is used for when no two cars are on the same road.
To Test supervisor.c Approximate Polynomial Algorithm
supervisor.c has two functions, safeInput() and safeInputApproximate(). To call the approximate algorithm, you can just change the corresponding line that calls the supervisor function in ca.c. This is based off of the scheduling problem and details can be read in the paper 'Efficient Algorithms for Collision Avoidance at Intersections'.
To Test supervisor2.c
**Note: There are 2 versions of supervisor.h and supervisor.c. The ones named supervisor.h and supervisor.c work for 2 cars, one on each figure 8. There is also a separate supervisor2.h and supervisor2.c which contains code that should work for multiple cars per path, and these programs require helper functions in supervisor_util.h and supervisor_util.c. These should be stored in the repository under "Supervisor 2". This contains the original code for 2 cars, plus additional code, which takes care of the case if a car has to take into account, additionally, cars on its same road. This additional code was transferred from the Java code simulation, which should work, but the transferred C code has not been tested.
Comment out all of supervisor.h and supervisor.c, and uncomment all of supervisor2.h, supervisor2.c, supervisor_util.h, and supervisor_util.c (you can easily toggle this in eclipse using Ctrl+A to select all and then Ctrl+/ to toggle between commented and uncommented). Then in ca.c, change #include supervisor.h to #include supervisor2.h.
How the supervisor works
In every iteration of the main loop in ca.c, the cars are projected forward 0.3 s. They are assumed that they will have roughly the same speed 0.3 s later, and their 1D position along the path is assumed to be incremented by speed * 0.3. This new position and speed for all the cars are sent to the supervisor function safeInput(), which figures out whether these projected positions lie in the capture set.
If not, "safe" is printed, and the velocity controller is given the current speed as the desired speed, so there should be roughly zero acceleration. Otherwise, however, the current positions are sent to safeInput(), which finds whether there is a solution given the current positions. If so, the two cars are supposed to accelerate at the accelerations given by the supervisor's solution. This is implemented by feeding the desired velocity (equal to current velocity + desired acceleration * dT, where dT is the previous timestep and is assumed to be relatively constant from step to step) into the velocity controller with very high gain. The reason for the high gain is because the timesteps are very short, so a high gain is necessary to make the car react enough.
If there is no solution even for the current positions, which should not happen in a theoretical situation when the cars are being supervised every timestep, but may occur during the experiment because the speeds of the cars are very jumpy, then the code uses the previous solution found by the supervisor. This is just a quick fix for the problem of jumpiness in the positions/speeds of the cars.
Changes to Old Car Code (ca2 code)
Mainly, the three main parts of the code (properties of the car, code that sets up brainstem, and higher level code that calculates how the car should move) have been cleanly separated. The bulk of code that initializes brainstem and the keyboard input but has no other interaction with the rest of the program has been moved separately to the new le initializer.c to avoid clutter in ca.c. All properties specific to the cars and how they respond to various values of throttle and steering input have been moved to the new file car fns, so that it can theoretically be the only file that needs to be changed from car to car. ext _math has the same name as an old file, but it consists mostly of new methods used especially by the path follower. setget has also been cleaned so that no outside references to aStemLib stemRef are necessary; that variable is set up by initializer.c and passed straight to setget.
Changes to Tea Code
I also made a change to the tea code in the brainstem. The tea code is stored on the laptop, in OldDesktopStuff/brainstem backup/aUser. I made a few changes to the function setSteer() in RoboCar_Lib.tea. The old code locked the steering value at -100 to 100; I changed it to -120 and 120, and changed the gain from 200 to 250. This allowed the cars to turn more severely, which was necessary for them to make the sharp turns of the figure 8.
To make further changes to the tea code, edit the various tea files in that folder. Then connect to a car and run Console.exe (in brainstem backup folder; also a shortcut in Kevin folder on laptop Desktop). Type steep "<filename>.tea" to compile and load "<filename>.cup" to load onto the car. Note: if loading a new version of RoboCar_Lib, make sure to change the SLIM constant depending on the car.
Java Code
The entire Java simulation is also stored in the repository, under Kevin/Java Simulation. This was mainly for testing in ideal situations, and is otherwise separate from the actual project. The main method is in Supervisor.java, and there are two test simulations, test1() and test2(). There are also two copies of findSolution(); the first is for one car per road, the second is for multiple cars per road.
Most of the main calculations (finding the optimal trajectory for each car given cars before it) are in Vehicle.java. Supervisor.java iterates through all orderings and checks them using the functions in Vehicle.java.
The executable PathVideo.jar takes a file output by the cars into data.txt in the i686 folder on the cars when the DEBUG flag in ca.c is set to 1. It outputs a sequence of screen-shots of the positions of the cars and the supervisor trajectory, if one exists. Note: to use this, make sure to move data.txt output by the cars into the same directory as PathVideo.jar. The source code is in Java Simulation/gui/PathVideo.java, which can be edited as necessary.
CPS
I've changed this so that the CPS camera computer 0 (the one on the very left) sends the Stances of all the cars in one package to each car. Note: this is not compatible with previous ca2.c programs, and the current ca.c program is not compatible with the new version of CPS! The new version of CPS is currently named Kevin CPS. There is also a Kevin CPS (Branch) on camera computer 0 which has new experimental things that only computer 0 does; this is not on the other computers. These are currently being updated.
The variable PROJECT_ONTO_PATH in CPS projects the cars onto the paths hardcoded in the beginning of main.cpp in order to calculate the velocities. The original positions are still sent; this variable only changes the sent velocities.
Calculating velocity: if PROJECT_ONTO_PATH is set to 1, then the positions of the cars tracked by the cameras are first projected onto the corresponding paths given by the textfile names at the top of main.cpp. (looks like char *path_filenames[] = { "fig8A.txt", "fig8C.txt", ... })
At each timestep, the program finds the latest position which is at least some distance (currently 5 cm) away from the previous stored position. It measures a velocity and heading based on the displacement and elapsed time between those two positions.
Note: currently only Cars 1 and 2 work on the paths, but Car 6's symbol is more easily track-able than Car 1's. So a small segment of code on the CPS switches Car 1 and Car 6's data, so it correctly sets the position of Car 6's symbol as the position of Car 1.
Example
Data Run.xlsx contains a bit of data collected from Data Run 7. This was a particularly good situation where the two cars evaded each other well. The trajectory of the cars is shown in the graph below. The cars initially are heading straight for the bad set (outlined in red). At the point (2788, 3234), the supervisor realizes that the cars cannot evade the bad set any more if they continue with zero acceleration, so it forces Car 1 to slow down at -200 mm/s^2 and Car 2 to speed up at 100 mm/s^2. The path deviates and just misses the bad set.
Here is a graph of the velocities. Dark blue and dark red represent the actual (quite chaotic, because of difficulties in measuring the exact velocity accurately) velocities of Car 1 and Car 2 respectively. The lighter blue and red represent their corresponding desired accelerations according to the velocity profile found by the supervisor, which is a straight line because they each apply a constant (extreme) acceleration. Note that Car 1 does gradually slow down and Car 2 gradually speeds up in this case, although the actual speed changes are quite hectic.
