Robot Restart and Reset Issues

[Jared]

We're having a very strange problem with our autonomous code and we really need some help fixing it. Our code is here:
https://github.com/dicarlo236/2015-Robot

Our autonomous code generates several arrays of data during the robotInit method. This procedure takes around 400-500 ms, and uses 10 MB of RAM. These arrays are then read by two threads to control the wheels of our robot as we enter autonomous mode. This process is very sensitive to timing, as we are actively controlling position, velocity, and acceleration of the two wheels independently.

If we power on the robot and run our autonomous mode, we drive too quickly and crash into things. The drive movement is jerky and uncontrolled in autonomous mode.

If we power on the robot, wait for the code to start, and push the reset button on the roboRIO, the autonomous code works very well and we pick up three totes.

If we power on the robot, wait for the code to start, and click the reboot roboRIO or restart robot code button on the driver station, autonomous mode works perfectly.

If we power on the robot, compile and download new software, it works perfectly too.

I have replicated this issue many times, and I am sure that the cold boot of the roboRIO is causing our problem.

This weekend, we missed doing the reset trick once during one of our elimination matches and autonomous mode went wrong.

Next time we get to robot, I'll try running our auto setup code in a static initialization block (we're using Java) in the main robot class, or having it initialize auto mode 5 seconds after the user code starts running, but I do not know if this will fix the problem.

We also experience high CPU usage and a bit of lag with the controls before we've reset the robot code. The FTA's at the event could not explain the problem, and kindly allowed us to restart the code and waited for us, but we aren't happy with this solution.

I'm fairly convinced it's a bug with the framework as there should be no difference between a cold boot and a reset button boot.

Any suggestions?