AME 3623: Houvercraft Simulator

This simulator is intended as a near "drop-in" replacement for the Houvercraft hardware. The key features are:

Executes on your Teensy. So, no new software to install (other than new Teensy libraries)
The software interfaces to the simulated Houvercraft are very similar to your physical system. This means that it should be easy to adapt your existing code to the simulator

Preliminaries

Download and install the following libraries:

Configuring Your Project Sketch

Add one include:
```
#include "Houvercraft.h"
```
Note that this will define the SharpSensor enumerated type with two values (SHARP_0 and SHARP_1).

Create a new simulator task:

void hovercraft_step();
PeriodicAction hovercraft_task(5, hovercraft_step);
       
void hovercraft_step()
{
  hover.step();
}

Make sure to add the task to your loop() function!

Create the simulator object as a global variable:
```
 Houvercraft hover(0.0, 90.0);
```
This declaration states that distance sensor zero is mounted on the front of the craft and sensor one is pointed to the right of the craft.

Command/Sensor Interface

The simulator provides the following command/sensor interfaces:

Reading distance from the simulated Sharp sensors:
```
d = hover.read_distance(SHARP_0);
       OR
d = hover.read_distance(SHARP_1);
```
Distances are measured from the center of the houvercraft (not the edge). Units are in cm. The max reported value is 80 cm.
Change the lateral fan magnitudes:
```
hover.set_lateral_fan_magnitudes(magnitude); 
```
magnitude is an array of floats in the following order: left, right, back. Units are N.
Read the compass heading:
```
theta = hover.imu_yaw();
```
Units are degrees in a left-handed coordinate frame.
Read the z-axis rate gyro:
```
dtheta = hover.imu_gz();
```
Units are degrees/s in a left-handed coordinate frame.
Set the central fan PWM:
```
hover.fan_write(val);
```
Values above 50 mean that the craft is hovering.
Read the accumulated motion in the houvercraft coordinate frame:
```
hover.compute_chassis_motion(accumulated_position);  
```
Where accumulated_position is a float array of length 3, corresponding to movement along the houvercraft's X, Y and orientation dimensions.
Note that this function only reports the movement since the last time it was called (similar to how we are handling things in project 7/8).

Debugging Interface

Your code can ask that the true state of the simulated craft be printed on a single line (with no newline):
```
hover.print_state();
```
I call this function in report_step()
Note that your code does not have access to this true state (this is intentional).
Print an ASCII representation of the environmental map. This map shows the walls and the location of the houvercraft:
```
hover.print_map();    
```
This is useful in response to a command from the user.
Set the true state of the houvercraft:
```
hover.set_position(x,y,theta);     
```
Units are m,m,degrees. x must be in the range 0 .. 3; y must be in the range 0 ... 4 (the simulator does not check this yet!).

Simulator Configuration

You can reset the configuration of the craft by calling the following function:
```
hover.reset(); 
```
This is useful to call when the user types a particular command.
By default, the simulator injects noise into the forces that it receives and the sensor values that it returns. However, when you are debugging, it is often helpful to remove this randomness in the behavior. You can do this by inserting the following line into your setup() function:
```
hover.set_stochastic(0);
```
Note that during demos, stochasticity must be turned on.
By default, the simulator is set to execute task zero. This means that when the simulator is reset, the houvercraft is always placed in the lower-right region of the environment: 0.5m, 0.5m, with an orientation of zero degrees. In contrast, task one will place the houvercraft in one of two locations (0.5m, 0.5m, zero degrees OR 0.5m, 3.5m, zero degrees). Your craft is not told which of these two configurations it is placed in. You can switch to task 1 by executing in your setup() function:
```
hover.set_task(1);
hover.reset();       
```

Hints

Be prepared to choose new PD parameters. Go through the same process as we have done over the last few projects:
- Get orientation control working first. Set a small Kv, and adjust Kp so that it completes a 90 degree rotation within a few seconds. Then, start increasing Kv until it is approximately critically damped.
- Go through the same process with lateral control

Sample FSM

Here is a finite state machine that gets started on a task:

void fsm_step()
{
  static State state = STATE_START;
  static int counter = 0;
  const int DELAY = 10*20;
  
  switch(state)
  {
    case STATE_START:
      if(toggle == 1)
      {
        state = STATE_HOVER;
        counter = 0;
        toggle = 0;         // This is set by a user command
        hover.fan_write(70);
        Serial.println("Lifting off");
      }
      break;
      
    case STATE_HOVER:
      ++counter;
      if(counter >= DELAY){
        state = STATE_TURNING1;
        orientation_goal = -10.0;
        Serial.println("Start turning");
      }
      break;
      
    case STATE_TURNING1:
      if(fabs(orientation_error) < 5.0)
      {
        Serial.println("Done turning");
        state = STATE_HOVER2;
        counter = 0;
      }
      break;

    case STATE_HOVER2:
      ++counter;
      if(counter >= DELAY)
      {
        Serial.println("Translating +X");
        state = STATE_TRANSLATE1;
        velocity_goal[0] = 0.05;
      }
      break;

Sample Run

Here is a sample run (the above FSM is used with additional states). Each row corresponds to 0.5 seconds of time.

The switch is toggled by the user and the user requests that maps be printed from time to time.

X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 50.2523
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 50.2534
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 51.0332
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 49.1193
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 50.1135
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 50.0805
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 51.2786
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 48.4402
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 50.5196
   |-----------------------|-----------------------|
   |                       |                       |
   |                       |                       |
   |                       |                       |
   |                       |                       |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |                       |           |
   |           |                       |           |
   |           |                       |     o     |
   |           |                       |           |
   |-----------|-----------------------|-----------|
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 48.3852
X = 0.5000 (0.0000) 	Y = 0.5000 (0.0000) 	theta = 0.0000 (0.0000)	collisions =  0	# DIST: 80.0000 52.3633
Switch pressed
Lifting off
X = 0.5014 (0.0076) 	Y = 0.4987 (-0.0089) 	theta = -0.1430 (-1.0946)	collisions =  0	# DIST: 80.0000 52.0946
X = 0.5013 (-0.0082) 	Y = 0.4975 (0.0029) 	theta = -0.4101 (-0.2865)	collisions =  0	# DIST: 80.0000 51.4500
X = 0.4961 (-0.0131) 	Y = 0.5013 (0.0138) 	theta = -0.5754 (-0.1616)	collisions =  0	# DIST: 80.0000 51.9587
X = 0.4935 (-0.0012) 	Y = 0.5028 (-0.0053) 	theta = -0.5008 (0.5054)	collisions =  0	# DIST: 80.0000 52.3881
X = 0.4934 (0.0009) 	Y = 0.4991 (-0.0079) 	theta = -0.4433 (0.0262)	collisions =  0	# DIST: 80.0000 49.7753
X = 0.4942 (0.0034) 	Y = 0.4984 (-0.0003) 	theta = -0.3946 (0.6731)	collisions =  0	# DIST: 80.0000 50.8592
X = 0.4935 (-0.0026) 	Y = 0.4985 (-0.0012) 	theta = 0.0401 (0.8226)	collisions =  0	# DIST: 80.0000 51.5828
X = 0.4905 (-0.0025) 	Y = 0.4971 (0.0020) 	theta = 0.1473 (0.1643)	collisions =  0	# DIST: 80.0000 49.4846
X = 0.4894 (-0.0051) 	Y = 0.4975 (0.0034) 	theta = 0.4001 (0.8385)	collisions =  0	# DIST: 80.0000 52.2019
X = 0.4867 (-0.0037) 	Y = 0.5038 (0.0162) 	theta = 0.4270 (-0.5429)	collisions =  0	# DIST: 80.0000 50.4252
X = 0.4842 (-0.0111) 	Y = 0.5122 (0.0106) 	theta = 0.0138 (-0.6692)	collisions =  0	# DIST: 80.0000 51.9530
X = 0.4795 (-0.0040) 	Y = 0.5170 (0.0107) 	theta = -0.3644 (-0.5632)	collisions =  0	# DIST: 80.0000 49.8661
X = 0.4766 (-0.0058) 	Y = 0.5184 (0.0007) 	theta = -0.5695 (-0.3181)	collisions =  0	# DIST: 80.0000 50.1370
X = 0.4748 (0.0014) 	Y = 0.5183 (0.0063) 	theta = -0.6337 (-0.3320)	collisions =  0	# DIST: 80.0000 50.8728
X = 0.4747 (-0.0053) 	Y = 0.5208 (0.0057) 	theta = -0.8008 (-0.4021)	collisions =  0	# DIST: 80.0000 51.9970
X = 0.4753 (0.0019) 	Y = 0.5219 (-0.0017) 	theta = -0.8593 (-0.1260)	collisions =  0	# DIST: 80.0000 52.4076
X = 0.4765 (0.0017) 	Y = 0.5195 (-0.0019) 	theta = -1.0216 (-0.9061)	collisions =  0	# DIST: 80.0000 50.5457
X = 0.4705 (-0.0110) 	Y = 0.5193 (0.0012) 	theta = -1.6285 (-0.6196)	collisions =  0	# DIST: 80.0000 50.0926
X = 0.4655 (-0.0153) 	Y = 0.5198 (-0.0016) 	theta = -1.6207 (0.6578)	collisions =  0	# DIST: 80.0000 50.3633
X = 0.4570 (-0.0202) 	Y = 0.5183 (-0.0015) 	theta = -1.2141 (0.5867)	collisions =  0	# DIST: 80.0000 51.9149
Start turning
X = 0.4523 (0.0006) 	Y = 0.5141 (-0.0067) 	theta = -1.3733 (-1.1458)	collisions =  0	# DIST: 80.0000 50.4219
X = 0.4532 (0.0050) 	Y = 0.5108 (-0.0036) 	theta = -2.1756 (-2.1528)	collisions =  0	# DIST: 80.0000 51.7383
X = 0.4565 (0.0073) 	Y = 0.5092 (-0.0081) 	theta = -3.3058 (-2.1825)	collisions =  0	# DIST: 80.0000 48.7627
X = 0.4580 (0.0034) 	Y = 0.5047 (-0.0016) 	theta = -4.3172 (-1.4508)	collisions =  0	# DIST: 80.0000 52.1994
X = 0.4573 (0.0003) 	Y = 0.5048 (-0.0012) 	theta = -5.1658 (-1.6539)	collisions =  0	# DIST: 80.0000 48.2794
Done turning
X = 0.4564 (-0.0066) 	Y = 0.5017 (-0.0085) 	theta = -5.8862 (-2.2690)	collisions =  0	# DIST: 80.0000 51.2893
X = 0.4516 (-0.0119) 	Y = 0.4988 (0.0006) 	theta = -6.8389 (-1.5158)	collisions =  0	# DIST: 80.0000 50.4124
X = 0.4499 (-0.0015) 	Y = 0.4992 (-0.0002) 	theta = -7.4611 (-0.8930)	collisions =  0	# DIST: 80.0000 50.5426
X = 0.4521 (0.0025) 	Y = 0.4995 (0.0034) 	theta = -7.6909 (-0.0699)	collisions =  0	# DIST: 80.0000 50.1839
X = 0.4531 (-0.0001) 	Y = 0.5019 (0.0064) 	theta = -7.8228 (-0.2059)	collisions =  0	# DIST: 80.0000 51.2468
X = 0.4538 (0.0007) 	Y = 0.5051 (0.0080) 	theta = -7.8533 (-0.3924)	collisions =  0	# DIST: 80.0000 50.3334
X = 0.4539 (0.0018) 	Y = 0.5100 (0.0112) 	theta = -7.9629 (0.2565)	collisions =  0	# DIST: 80.0000 49.8837
X = 0.4546 (0.0011) 	Y = 0.5135 (-0.0002) 	theta = -7.9930 (-0.5439)	collisions =  0	# DIST: 80.0000 51.8997
X = 0.4541 (-0.0008) 	Y = 0.5175 (0.0100) 	theta = -8.4055 (-0.9513)	collisions =  0	# DIST: 80.0000 53.2518
X = 0.4554 (-0.0020) 	Y = 0.5230 (0.0095) 	theta = -8.9624 (-0.5946)	collisions =  0	# DIST: 80.0000 53.6916
X = 0.4530 (-0.0071) 	Y = 0.5247 (-0.0034) 	theta = -8.7940 (0.8538)	collisions =  0	# DIST: 80.0000 54.4223
X = 0.4502 (-0.0052) 	Y = 0.5238 (0.0002) 	theta = -8.2226 (1.4327)	collisions =  0	# DIST: 80.0000 55.2871
X = 0.4493 (-0.0010) 	Y = 0.5254 (0.0007) 	theta = -7.5896 (1.0071)	collisions =  0	# DIST: 80.0000 52.4162
X = 0.4492 (0.0002) 	Y = 0.5300 (0.0120) 	theta = -7.3994 (0.1107)	collisions =  0	# DIST: 80.0000 52.1823
X = 0.4482 (-0.0042) 	Y = 0.5344 (0.0051) 	theta = -7.4841 (-0.4500)	collisions =  0	# DIST: 80.0000 52.7726
X = 0.4455 (-0.0115) 	Y = 0.5366 (-0.0001) 	theta = -7.6668 (-0.3087)	collisions =  0	# DIST: 80.0000 53.9774
X = 0.4431 (0.0013) 	Y = 0.5374 (-0.0034) 	theta = -7.6166 (-0.1281)	collisions =  0	# DIST: 80.0000 51.9880
X = 0.4441 (0.0051) 	Y = 0.5390 (0.0022) 	theta = -7.8492 (-0.1963)	collisions =  0	# DIST: 80.0000 53.7986
X = 0.4460 (0.0038) 	Y = 0.5386 (0.0024) 	theta = -7.5544 (0.9872)	collisions =  0	# DIST: 80.0000 56.0274
X = 0.4437 (-0.0098) 	Y = 0.5389 (-0.0056) 	theta = -6.9175 (1.9600)	collisions =  0	# DIST: 80.0000 56.1328
Translating +X
X = 0.4497 (0.0236) 	Y = 0.5349 (-0.0038) 	theta = -6.2303 (0.7546)	collisions =  0	# DIST: 80.0000 53.3013
X = 0.4663 (0.0450) 	Y = 0.5355 (0.0023) 	theta = -6.0605 (-0.1431)	collisions =  0	# DIST: 80.0000 52.5638
X = 0.4905 (0.0492) 	Y = 0.5358 (0.0040) 	theta = -6.1279 (0.1012)	collisions =  0	# DIST: 80.0000 56.3128
X = 0.5143 (0.0483) 	Y = 0.5337 (-0.0109) 	theta = -6.0177 (0.2875)	collisions =  0	# DIST: 80.0000 54.4022
X = 0.5423 (0.0640) 	Y = 0.5320 (0.0021) 	theta = -6.0751 (-0.7540)	collisions =  0	# DIST: 80.0000 52.3532
X = 0.5723 (0.0606) 	Y = 0.5300 (-0.0025) 	theta = -6.2689 (-0.1922)	collisions =  0	# DIST: 80.0000 54.5143
X = 0.5991 (0.0484) 	Y = 0.5291 (-0.0035) 	theta = -6.3985 (-0.0508)	collisions =  0	# DIST: 80.0000 52.5419
X = 0.6255 (0.0513) 	Y = 0.5263 (-0.0011) 	theta = -6.6294 (-0.5883)	collisions =  0	# DIST: 80.0000 54.3363
X = 0.6507 (0.0534) 	Y = 0.5291 (0.0056) 	theta = -6.8592 (-0.1193)	collisions =  0	# DIST: 80.0000 51.3356
X = 0.6782 (0.0598) 	Y = 0.5317 (0.0089) 	theta = -7.0549 (-0.5921)	collisions =  0	# DIST: 80.0000 54.6370
X = 0.7056 (0.0513) 	Y = 0.5329 (0.0062) 	theta = -7.6565 (-0.9653)	collisions =  0	# DIST: 80.0000 53.2549
X = 0.7322 (0.0550) 	Y = 0.5349 (0.0040) 	theta = -8.1818 (-1.4688)	collisions =  0	# DIST: 80.0000 55.3257
X = 0.7621 (0.0546) 	Y = 0.5355 (-0.0004) 	theta = -8.5644 (-0.3481)	collisions =  0	# DIST: 80.0000 53.6372
X = 0.7884 (0.0529) 	Y = 0.5374 (0.0074) 	theta = -8.5165 (0.3062)	collisions =  0	# DIST: 80.0000 51.9758
X = 0.8129 (0.0430) 	Y = 0.5407 (0.0090) 	theta = -8.7939 (-0.3553)	collisions =  0	# DIST: 80.0000 54.8093
X = 0.8352 (0.0487) 	Y = 0.5436 (0.0073) 	theta = -8.8180 (0.6097)	collisions =  0	# DIST: 80.0000 52.7637
X = 0.8614 (0.0505) 	Y = 0.5486 (0.0115) 	theta = -8.4911 (0.2938)	collisions =  0	# DIST: 80.0000 56.1124
X = 0.8875 (0.0477) 	Y = 0.5571 (0.0170) 	theta = -8.7482 (-0.4783)	collisions =  0	# DIST: 80.0000 55.8726
X = 0.9136 (0.0537) 	Y = 0.5640 (0.0039) 	theta = -9.0803 (-0.4188)	collisions =  0	# DIST: 80.0000 55.6906
X = 0.9386 (0.0471) 	Y = 0.5657 (0.0043) 	theta = -9.3996 (-0.7853)	collisions =  0	# DIST: 80.0000 57.4264
   |-----------------------|-----------------------|
   |                       |                       |
   |                       |                       |
   |                       |                       |
   |                       |                       |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |                       |     @     |
   |           |                       |           |
   |           |                       |           |
   |           |                       |           |
   |-----------|-----------------------|-----------|
X = 0.9629 (0.0556) 	Y = 0.5693 (0.0053) 	theta = -9.6196 (-0.9315)	collisions =  0	# DIST: 80.0000 56.2571
X = 0.9894 (0.0562) 	Y = 0.5734 (0.0012) 	theta = -10.1205 (-1.1920)	collisions =  0	# DIST: 80.0000 57.1450
X = 1.0219 (0.0643) 	Y = 0.5752 (0.0021) 	theta = -10.1875 (0.1722)	collisions =  0	# DIST: 80.0000 60.1881
X = 1.0520 (0.0586) 	Y = 0.5774 (0.0129) 	theta = -9.9617 (0.6745)	collisions =  0	# DIST: 80.0000 56.9258
X = 1.0807 (0.0566) 	Y = 0.5825 (0.0081) 	theta = -9.6902 (0.5045)	collisions =  0	# DIST: 80.0000 57.8899
X = 1.1070 (0.0498) 	Y = 0.5823 (0.0048) 	theta = -9.6250 (-0.3174)	collisions =  0	# DIST: 80.0000 58.3450
X = 1.1334 (0.0540) 	Y = 0.5879 (0.0210) 	theta = -9.5885 (0.7997)	collisions =  0	# DIST: 80.0000 59.4368
X = 1.1598 (0.0505) 	Y = 0.5961 (0.0144) 	theta = -9.2727 (-0.0834)	collisions =  0	# DIST: 80.0000 58.2445
X = 1.1865 (0.0587) 	Y = 0.6040 (0.0205) 	theta = -9.2928 (-0.0119)	collisions =  0	# DIST: 80.0000 61.1637
X = 1.2130 (0.0491) 	Y = 0.6149 (0.0136) 	theta = -9.1748 (-0.7678)	collisions =  0	# DIST: 80.0000 59.9566
X = 1.2386 (0.0520) 	Y = 0.6217 (0.0218) 	theta = -9.4892 (-0.8368)	collisions =  0	# DIST: 80.0000 61.4406
X = 1.2686 (0.0625) 	Y = 0.6298 (0.0129) 	theta = -9.8277 (-0.3551)	collisions =  0	# DIST: 80.0000 61.9333
X = 1.3001 (0.0701) 	Y = 0.6353 (0.0147) 	theta = -10.1852 (-0.9549)	collisions =  0	# DIST: 80.0000 64.6351
X = 1.3306 (0.0567) 	Y = 0.6412 (0.0108) 	theta = -10.5161 (-0.0843)	collisions =  0	# DIST: 80.0000 62.9331
X = 1.3579 (0.0500) 	Y = 0.6464 (0.0129) 	theta = -10.2483 (0.6232)	collisions =  0	# DIST: 80.0000 67.8289
X = 1.3833 (0.0505) 	Y = 0.6534 (0.0146) 	theta = -9.7080 (1.4640)	collisions =  0	# DIST: 80.0000 67.1460
X = 1.4077 (0.0485) 	Y = 0.6604 (0.0155) 	theta = -9.2482 (0.4490)	collisions =  0	# DIST: 80.0000 65.9057
X = 1.4325 (0.0529) 	Y = 0.6642 (0.0110) 	theta = -8.9292 (0.4580)	collisions =  0	# DIST: 80.0000 66.1669
X = 1.4569 (0.0472) 	Y = 0.6723 (0.0151) 	theta = -9.1201 (-0.8796)	collisions =  0	# DIST: 80.0000 68.3814
X = 1.4778 (0.0447) 	Y = 0.6775 (0.0045) 	theta = -9.3900 (-0.1143)	collisions =  0	# DIST: 80.0000 69.9365
X = 1.5011 (0.0472) 	Y = 0.6809 (0.0063) 	theta = -9.1560 (0.2329)	collisions =  0	# DIST: 80.0000 70.9882
X = 1.5213 (0.0392) 	Y = 0.6844 (0.0047) 	theta = -9.0487 (0.4288)	collisions =  0	# DIST: 80.0000 71.2984
X = 1.5416 (0.0441) 	Y = 0.6884 (0.0067) 	theta = -8.9013 (0.1062)	collisions =  0	# DIST: 80.0000 71.4511
X = 1.5642 (0.0458) 	Y = 0.6920 (0.0070) 	theta = -8.7799 (-0.2923)	collisions =  0	# DIST: 80.0000 69.9452
X = 1.5853 (0.0516) 	Y = 0.6945 (-0.0019) 	theta = -8.9097 (-0.2452)	collisions =  0	# DIST: 80.0000 71.9590
X = 1.6120 (0.0628) 	Y = 0.6967 (0.0080) 	theta = -9.1554 (-0.5697)	collisions =  0	# DIST: 80.0000 68.9189
X = 1.6417 (0.0509) 	Y = 0.6998 (0.0093) 	theta = -9.2152 (0.4242)	collisions =  0	# DIST: 80.0000 71.8315
X = 1.6649 (0.0471) 	Y = 0.7063 (0.0120) 	theta = -8.8895 (0.2440)	collisions =  0	# DIST: 80.0000 69.0273
X = 1.6891 (0.0466) 	Y = 0.7123 (0.0143) 	theta = -8.9027 (0.4727)	collisions =  0	# DIST: 80.0000 74.4968
X = 1.7145 (0.0530) 	Y = 0.7198 (0.0092) 	theta = -8.4681 (1.2743)	collisions =  0	# DIST: 80.0000 74.5541
X = 1.7428 (0.0608) 	Y = 0.7274 (0.0096) 	theta = -7.8039 (1.6630)	collisions =  0	# DIST: 80.0000 72.5486
X = 1.7708 (0.0564) 	Y = 0.7287 (0.0064) 	theta = -7.3685 (0.6183)	collisions =  0	# DIST: 80.0000 73.4088
X = 1.7981 (0.0439) 	Y = 0.7321 (0.0021) 	theta = -7.2640 (-0.0160)	collisions =  0	# DIST: 80.0000 74.6760
X = 1.8228 (0.0537) 	Y = 0.7347 (0.0103) 	theta = -7.3529 (0.1256)	collisions =  0	# DIST: 80.0000 75.9884
X = 1.8491 (0.0509) 	Y = 0.7372 (-0.0016) 	theta = -7.5569 (-0.1051)	collisions =  0	# DIST: 80.0000 73.6044
X = 1.8759 (0.0523) 	Y = 0.7417 (0.0120) 	theta = -7.6679 (-0.0468)	collisions =  0	# DIST: 80.0000 75.8379
X = 1.9015 (0.0496) 	Y = 0.7484 (0.0125) 	theta = -7.6380 (0.6216)	collisions =  0	# DIST: 80.0000 73.4424
X = 1.9297 (0.0549) 	Y = 0.7544 (0.0082) 	theta = -7.5073 (-0.0555)	collisions =  0	# DIST: 80.0000 76.6556
X = 1.9560 (0.0507) 	Y = 0.7556 (0.0047) 	theta = -7.4594 (0.7351)	collisions =  0	# DIST: 80.0000 76.1940
X = 1.9781 (0.0427) 	Y = 0.7569 (-0.0038) 	theta = -7.2174 (0.0692)	collisions =  0	# DIST: 80.0000 75.4847
X = 2.0003 (0.0451) 	Y = 0.7546 (-0.0001) 	theta = -7.6221 (-1.3289)	collisions =  0	# DIST: 80.0000 74.2169
X = 2.0266 (0.0592) 	Y = 0.7543 (-0.0089) 	theta = -7.8613 (0.6356)	collisions =  0	# DIST: 80.0000 73.6495
X = 2.0568 (0.0531) 	Y = 0.7510 (-0.0027) 	theta = -7.8191 (-0.5305)	collisions =  0	# DIST: 80.0000 74.8559
X = 2.0835 (0.0499) 	Y = 0.7527 (0.0111) 	theta = -8.0534 (-0.7346)	collisions =  0	# DIST: 80.0000 76.2431
X = 2.1086 (0.0510) 	Y = 0.7537 (0.0047) 	theta = -8.5465 (-1.7528)	collisions =  0	# DIST: 80.0000 75.2301
X = 2.1327 (0.0486) 	Y = 0.7570 (0.0091) 	theta = -9.0229 (-0.3701)	collisions =  0	# DIST: 80.0000 76.5031
X = 2.1599 (0.0597) 	Y = 0.7600 (0.0028) 	theta = -9.0578 (0.1819)	collisions =  0	# DIST: 80.0000 75.3048
X = 2.1905 (0.0560) 	Y = 0.7621 (-0.0018) 	theta = -8.8737 (0.5468)	collisions =  0	# DIST: 80.0000 78.3152
X = 2.2175 (0.0472) 	Y = 0.7645 (0.0028) 	theta = -8.4169 (1.8156)	collisions =  0	# DIST: 78.6204 75.4194
X = 2.2400 (0.0421) 	Y = 0.7693 (0.0089) 	theta = -7.6913 (1.8573)	collisions =  0	# DIST: 77.0197 76.3492
X = 2.2646 (0.0493) 	Y = 0.7752 (0.0142) 	theta = -7.1486 (0.5660)	collisions =  0	# DIST: 73.0747 77.3337
X = 2.2909 (0.0637) 	Y = 0.7833 (0.0128) 	theta = -6.8261 (0.9865)	collisions =  0	# DIST: 71.0439 77.0567
X = 2.3206 (0.0599) 	Y = 0.7904 (0.0173) 	theta = -6.5295 (0.0795)	collisions =  0	# DIST: 70.2499 80.0000
X = 2.3475 (0.0485) 	Y = 0.7948 (-0.0018) 	theta = -6.4773 (0.4152)	collisions =  0	# DIST: 65.8979 80.0000
X = 2.3714 (0.0521) 	Y = 0.7930 (-0.0013) 	theta = -6.2057 (0.6443)	collisions =  0	# DIST: 63.2520 79.3509
X = 2.3989 (0.0507) 	Y = 0.7940 (0.0008) 	theta = -5.8837 (0.2185)	collisions =  0	# DIST: 60.8913 80.0000
   |-----------------------|-----------------------|
   |                       |                       |
   |                       |                       |
   |                       |                       |
   |                       |              @        |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |                       |           |
   |           |                       |           |
   |           |                       |           |
   |           |                       |           |
   |-----------|-----------------------|-----------|
X = 2.4249 (0.0490) 	Y = 0.7960 (0.0049) 	theta = -5.7147 (0.8375)	collisions =  0	# DIST: 59.9211 80.0000
X = 2.4499 (0.0518) 	Y = 0.7978 (0.0028) 	theta = -5.3361 (0.7536)	collisions =  0	# DIST: 55.8628 80.0000
X = 2.4747 (0.0527) 	Y = 0.8009 (0.0060) 	theta = -4.9680 (0.5612)	collisions =  0	# DIST: 50.8902 80.0000
X = 2.5021 (0.0550) 	Y = 0.8041 (0.0076) 	theta = -4.8658 (-0.2646)	collisions =  0	# DIST: 47.6450 80.0000
Done translating
X = 2.5244 (0.0358) 	Y = 0.8093 (0.0064) 	theta = -4.7928 (0.2508)	collisions =  0	# DIST: 47.6695 80.0000
X = 2.5367 (0.0196) 	Y = 0.8114 (0.0089) 	theta = -4.6054 (-0.0077)	collisions =  0	# DIST: 47.5416 80.0000
X = 2.5412 (0.0041) 	Y = 0.8158 (0.0038) 	theta = -4.5019 (-0.3018)	collisions =  0	# DIST: 43.8816 80.0000
X = 2.5423 (0.0018) 	Y = 0.8206 (0.0123) 	theta = -4.9981 (-1.1845)	collisions =  0	# DIST: 45.2539 80.0000
X = 2.5407 (-0.0090) 	Y = 0.8243 (0.0040) 	theta = -5.7489 (-1.3555)	collisions =  0	# DIST: 48.0715 80.0000
X = 2.5355 (-0.0100) 	Y = 0.8272 (0.0036) 	theta = -6.3125 (-0.7011)	collisions =  0	# DIST: 45.1886 80.0000
X = 2.5315 (0.0027) 	Y = 0.8295 (-0.0050) 	theta = -6.6881 (-0.9149)	collisions =  0	# DIST: 47.1646 80.0000
X = 2.5306 (-0.0031) 	Y = 0.8258 (-0.0075) 	theta = -6.8794 (0.0956)	collisions =  0	# DIST: 48.8601 80.0000
X = 2.5275 (-0.0029) 	Y = 0.8242 (-0.0059) 	theta = -7.0698 (-0.9131)	collisions =  0	# DIST: 48.3763 80.0000
X = 2.5263 (0.0018) 	Y = 0.8204 (-0.0067) 	theta = -7.2450 (-0.4616)	collisions =  0	# DIST: 45.3455 80.0000
X = 2.5258 (-0.0003) 	Y = 0.8196 (0.0075) 	theta = -7.2285 (1.0112)	collisions =  0	# DIST: 49.0039 80.0000
X = 2.5258 (-0.0038) 	Y = 0.8226 (0.0041) 	theta = -6.8250 (0.6264)	collisions =  0	# DIST: 46.2002 80.0000
X = 2.5234 (-0.0011) 	Y = 0.8220 (-0.0015) 	theta = -6.8334 (-0.1677)	collisions =  0	# DIST: 46.7117 80.0000
X = 2.5253 (0.0045) 	Y = 0.8188 (-0.0070) 	theta = -6.8014 (0.1894)	collisions =  0	# DIST: 48.1353 80.0000
X = 2.5253 (0.0017) 	Y = 0.8133 (-0.0085) 	theta = -6.4890 (0.5672)	collisions =  0	# DIST: 46.1821 80.0000
X = 2.5265 (-0.0020) 	Y = 0.8104 (-0.0101) 	theta = -6.3435 (0.2042)	collisions =  0	# DIST: 48.5167 80.0000
X = 2.5227 (-0.0033) 	Y = 0.8071 (-0.0106) 	theta = -6.2459 (-0.1963)	collisions =  0	# DIST: 48.1389 80.0000
X = 2.5225 (0.0000) 	Y = 0.8000 (-0.0084) 	theta = -6.4158 (-0.4074)	collisions =  0	# DIST: 48.4427 80.0000
X = 2.5246 (0.0058) 	Y = 0.7955 (-0.0058) 	theta = -6.8081 (-0.6524)	collisions =  0	# DIST: 48.7240 80.0000
X = 2.5289 (0.0064) 	Y = 0.7960 (0.0037) 	theta = -7.2656 (-0.7989)	collisions =  0	# DIST: 46.9865 80.0000
Translating +Y
X = 2.5288 (-0.0102) 	Y = 0.7999 (0.0153) 	theta = -7.6672 (-0.1386)	collisions =  0	# DIST: 46.7867 80.0000
X = 2.5213 (-0.0157) 	Y = 0.8167 (0.0500) 	theta = -7.7147 (0.0324)	collisions =  0	# DIST: 47.2858 80.0000
X = 2.5140 (-0.0094) 	Y = 0.8439 (0.0587) 	theta = -8.1373 (-0.6857)	collisions =  0	# DIST: 49.7321 80.0000
X = 2.5093 (-0.0127) 	Y = 0.8718 (0.0579) 	theta = -8.4937 (0.0669)	collisions =  0	# DIST: 51.2088 80.0000
X = 2.5028 (-0.0158) 	Y = 0.8974 (0.0559) 	theta = -8.2660 (0.9465)	collisions =  0	# DIST: 50.2769 80.0000
X = 2.5002 (-0.0014) 	Y = 0.9235 (0.0534) 	theta = -7.7456 (1.5785)	collisions =  0	# DIST: 49.4370 80.0000
X = 2.4955 (-0.0126) 	Y = 0.9527 (0.0597) 	theta = -6.8497 (1.2148)	collisions =  0	# DIST: 53.2212 80.0000
X = 2.4895 (-0.0132) 	Y = 0.9813 (0.0563) 	theta = -6.7289 (-0.0583)	collisions =  0	# DIST: 50.0684 80.0000
X = 2.4859 (-0.0049) 	Y = 1.0094 (0.0601) 	theta = -6.8631 (-0.1501)	collisions =  0	# DIST: 51.8577 80.0000
X = 2.4829 (-0.0080) 	Y = 1.0400 (0.0574) 	theta = -6.7089 (0.2386)	collisions =  0	# DIST: 52.1140 80.0000
X = 2.4760 (-0.0169) 	Y = 1.0671 (0.0549) 	theta = -6.6827 (0.5540)	collisions =  0	# DIST: 51.0751 80.0000
X = 2.4674 (-0.0170) 	Y = 1.0950 (0.0574) 	theta = -6.3032 (1.0138)	collisions =  0	# DIST: 54.0658 80.0000
X = 2.4627 (0.0011) 	Y = 1.1285 (0.0725) 	theta = -5.9830 (0.2186)	collisions =  0	# DIST: 53.4167 80.0000
X = 2.4619 (-0.0069) 	Y = 1.1643 (0.0689) 	theta = -5.9453 (0.2413)	collisions =  0	# DIST: 55.4736 80.0000
X = 2.4590 (-0.0000) 	Y = 1.1970 (0.0618) 	theta = -5.8176 (-0.1119)	collisions =  0	# DIST: 52.2171 80.0000
X = 2.4559 (-0.0112) 	Y = 1.2258 (0.0475) 	theta = -5.8542 (0.0953)	collisions =  0	# DIST: 54.2746 80.0000
X = 2.4507 (-0.0106) 	Y = 1.2508 (0.0508) 	theta = -5.7051 (-0.0036)	collisions =  0	# DIST: 55.2367 80.0000
X = 2.4493 (0.0017) 	Y = 1.2792 (0.0530) 	theta = -5.9056 (-0.3431)	collisions =  0	# DIST: 57.0694 80.0000
X = 2.4497 (-0.0034) 	Y = 1.3036 (0.0494) 	theta = -6.0630 (0.3715)	collisions =  0	# DIST: 53.3085 80.0000
X = 2.4478 (-0.0045) 	Y = 1.3295 (0.0522) 	theta = -6.0625 (-0.4173)	collisions =  0	# DIST: 54.4668 80.0000
Done translating
X = 2.4442 (-0.0086) 	Y = 1.3543 (0.0410) 	theta = -6.2359 (0.0596)	collisions =  0	# DIST: 58.3660 80.0000
X = 2.4406 (-0.0055) 	Y = 1.3661 (0.0152) 	theta = -6.4004 (-0.1664)	collisions =  0	# DIST: 55.1311 80.0000
X = 2.4380 (-0.0033) 	Y = 1.3685 (0.0010) 	theta = -6.8175 (-0.5065)	collisions =  0	# DIST: 58.9106 80.0000
X = 2.4368 (-0.0070) 	Y = 1.3665 (-0.0104) 	theta = -6.6712 (1.1173)	collisions =  0	# DIST: 54.5325 80.0000
X = 2.4338 (-0.0004) 	Y = 1.3636 (-0.0018) 	theta = -6.3852 (0.4953)	collisions =  0	# DIST: 54.8843 80.0000
X = 2.4329 (0.0054) 	Y = 1.3652 (0.0077) 	theta = -6.4156 (0.0016)	collisions =  0	# DIST: 54.7444 80.0000
X = 2.4326 (-0.0010) 	Y = 1.3680 (0.0021) 	theta = -6.7081 (-1.0025)	collisions =  0	# DIST: 57.8025 80.0000
X = 2.4333 (0.0054) 	Y = 1.3697 (0.0032) 	theta = -7.0360 (-0.8059)	collisions =  0	# DIST: 58.0608 80.0000
X = 2.4331 (-0.0012) 	Y = 1.3691 (-0.0095) 	theta = -7.6105 (-1.1227)	collisions =  0	# DIST: 58.2732 80.0000
X = 2.4322 (-0.0072) 	Y = 1.3670 (0.0012) 	theta = -7.9522 (-0.2559)	collisions =  0	# DIST: 55.2461 80.0000
X = 2.4296 (-0.0056) 	Y = 1.3648 (-0.0107) 	theta = -7.9016 (-0.2657)	collisions =  0	# DIST: 58.3491 80.0000
X = 2.4263 (-0.0138) 	Y = 1.3607 (-0.0074) 	theta = -7.9554 (-0.0612)	collisions =  0	# DIST: 58.5625 80.0000
X = 2.4237 (-0.0009) 	Y = 1.3577 (-0.0007) 	theta = -8.1194 (-0.4910)	collisions =  0	# DIST: 60.3127 80.0000
X = 2.4197 (-0.0110) 	Y = 1.3593 (0.0045) 	theta = -8.3646 (-0.7161)	collisions =  0	# DIST: 60.6376 80.0000
X = 2.4168 (0.0014) 	Y = 1.3613 (0.0045) 	theta = -8.6506 (-0.0102)	collisions =  0	# DIST: 56.6920 80.0000
X = 2.4169 (-0.0118) 	Y = 1.3636 (0.0034) 	theta = -8.7959 (-0.5440)	collisions =  0	# DIST: 60.6628 80.0000
X = 2.4108 (-0.0111) 	Y = 1.3650 (-0.0010) 	theta = -9.0501 (-0.0048)	collisions =  0	# DIST: 57.7684 80.0000
X = 2.4064 (-0.0070) 	Y = 1.3628 (-0.0079) 	theta = -9.1514 (-0.4356)	collisions =  0	# DIST: 62.4624 80.0000
X = 2.4064 (0.0014) 	Y = 1.3571 (-0.0079) 	theta = -9.5334 (-1.1233)	collisions =  0	# DIST: 62.4468 80.0000
X = 2.4047 (-0.0101) 	Y = 1.3536 (-0.0108) 	theta = -9.6399 (0.3057)	collisions =  0	# DIST: 61.7349 80.0000
Turning back
X = 2.4019 (-0.0050) 	Y = 1.3469 (-0.0121) 	theta = -9.4021 (0.4497)	collisions =  0	# DIST: 62.9382 80.0000
X = 2.4013 (-0.0052) 	Y = 1.3421 (-0.0078) 	theta = -8.7141 (0.9226)	collisions =  0	# DIST: 61.1692 80.0000
X = 2.4009 (0.0054) 	Y = 1.3381 (-0.0092) 	theta = -8.2912 (0.9221)	collisions =  0	# DIST: 62.2272 80.0000
X = 2.4055 (0.0054) 	Y = 1.3354 (-0.0096) 	theta = -7.8004 (1.3181)	collisions =  0	# DIST: 62.3275 80.0000
X = 2.4084 (0.0065) 	Y = 1.3310 (-0.0062) 	theta = -7.1803 (1.7403)	collisions =  0	# DIST: 61.1413 80.0000
X = 2.4117 (0.0043) 	Y = 1.3294 (0.0025) 	theta = -6.2285 (2.2410)	collisions =  0	# DIST: 61.2093 80.0000
Done turning
X = 2.4116 (-0.0054) 	Y = 1.3289 (-0.0046) 	theta = -5.0926 (2.5309)	collisions =  0	# DIST: 61.4496 80.0000
X = 2.4101 (-0.0060) 	Y = 1.3259 (-0.0049) 	theta = -4.1581 (1.2955)	collisions =  0	# DIST: 61.1938 80.0000
X = 2.4083 (-0.0009) 	Y = 1.3246 (0.0036) 	theta = -3.6164 (0.4304)	collisions =  0	# DIST: 58.6792 80.0000
X = 2.4095 (-0.0014) 	Y = 1.3276 (0.0076) 	theta = -3.0421 (1.4582)	collisions =  0	# DIST: 59.4903 80.0000
X = 2.4095 (0.0023) 	Y = 1.3289 (-0.0018) 	theta = -2.7903 (-0.8620)	collisions =  0	# DIST: 56.7643 80.0000
X = 2.4113 (-0.0033) 	Y = 1.3295 (0.0009) 	theta = -3.0392 (-0.4319)	collisions =  0	# DIST: 59.9246 80.0000
X = 2.4129 (0.0029) 	Y = 1.3315 (0.0060) 	theta = -3.0223 (-0.0918)	collisions =  0	# DIST: 56.8432 80.0000
X = 2.4123 (-0.0038) 	Y = 1.3326 (0.0016) 	theta = -3.2844 (-0.5174)	collisions =  0	# DIST: 61.1841 80.0000
X = 2.4127 (0.0044) 	Y = 1.3349 (0.0064) 	theta = -3.6260 (-0.4657)	collisions =  0	# DIST: 58.3629 80.0000
X = 2.4135 (0.0014) 	Y = 1.3382 (0.0068) 	theta = -3.6842 (0.1035)	collisions =  0	# DIST: 57.4748 80.0000
X = 2.4172 (0.0078) 	Y = 1.3391 (-0.0059) 	theta = -3.5224 (0.3966)	collisions =  0	# DIST: 59.3404 80.0000
X = 2.4205 (0.0036) 	Y = 1.3367 (0.0020) 	theta = -3.2347 (0.7886)	collisions =  0	# DIST: 57.3127 80.0000
X = 2.4245 (0.0064) 	Y = 1.3379 (-0.0002) 	theta = -3.0711 (0.3956)	collisions =  0	# DIST: 55.7508 80.0000
X = 2.4245 (-0.0036) 	Y = 1.3371 (0.0006) 	theta = -3.3099 (-0.9051)	collisions =  0	# DIST: 56.1429 80.0000
X = 2.4224 (-0.0036) 	Y = 1.3360 (-0.0032) 	theta = -3.7429 (-0.5887)	collisions =  0	# DIST: 58.1605 80.0000
X = 2.4205 (-0.0051) 	Y = 1.3338 (-0.0037) 	theta = -3.9346 (-0.3232)	collisions =  0	# DIST: 57.2746 80.0000
X = 2.4187 (-0.0017) 	Y = 1.3294 (-0.0123) 	theta = -4.1054 (-0.5182)	collisions =  0	# DIST: 59.5858 80.0000
X = 2.4197 (0.0027) 	Y = 1.3278 (0.0044) 	theta = -4.4890 (-1.4337)	collisions =  0	# DIST: 58.2135 80.0000
X = 2.4240 (0.0147) 	Y = 1.3294 (0.0052) 	theta = -5.2877 (-1.3109)	collisions =  0	# DIST: 56.6091 80.0000
X = 2.4305 (0.0070) 	Y = 1.3303 (-0.0040) 	theta = -5.8261 (-1.3652)	collisions =  0	# DIST: 58.1770 80.0000
SHUTTING DOWN
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 54.7031 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 55.4521 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 56.1862 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 59.0266 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 56.8308 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 56.7133 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 56.6732 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 55.7747 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 59.0714 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 57.6567 80.0000
   |-----------------------|-----------------------|
   |                       |                       |
   |                       |                       |
   |                       |        o              |
   |                       |                       |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |           |           |           |
   |           |                       |           |
   |           |                       |           |
   |           |                       |           |
   |           |                       |           |
   |-----------|-----------------------|-----------|
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 56.2941 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 56.8196 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 57.9870 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 58.7856 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 58.3439 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 57.4213 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 59.0090 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 57.4484 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 57.9894 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 54.8897 80.0000
Ready to start
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 58.4869 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 56.0949 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 58.6131 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 55.2276 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 58.3649 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 57.7005 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 56.2917 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 58.7105 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 57.0795 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 58.7526 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 59.0631 80.0000
X = 2.4317 (0.0000) 	Y = 1.3296 (0.0000) 	theta = -6.0163 (0.0000)	collisions =  0	# DIST: 58.8284 80.0000

andrewhfagg at gmail.com

Last modified: Wed Apr 15 23:22:39 2020