Detect when NavX is not communicating, stop rotation-based auton

[elodotwe]

We witnessed some weird behavior on Saturday while tuning the RotatePID loop where the robot would take off full speed in one direction for a few seconds. The graph for the NavX input went flat, i.e. the NavX was not reporting any movement, despite the fact that we were in a full power spin.

If I recall correctly, the battery was fairly low during this testing. I hypothesize that the NavX lost power, stopped communicating..but since our code doesn't check whether the data stored in AHRS is fresh or stale, our code happily goes into a spin with stale data.

I propose checking ahrs.isConnected() during autonomous.

For DriveTo/DriveUntil, we can continue the command without gyro correction.

For Rotate, I can't think of a good way to recover from complete loss of NavX communication...so maybe we should just halt all autonomous movement?

I have thoughts as to how to accomplish this, but first...thoughts from you guys?

[isaiahkahler]

Well, we could end the command pretty easily, but is ending the command all we have to worry about?

protected boolean isFinished(){
    return turnController.onTarget() || !DriveTrain.sharedInstance().ahrs.isConnected();
}

Auton will run commands sequentially, and only the DriveTo and DriveUntil commands would run, leaving the robot still moving, just not turning. We could add that to each auton command or we could somehow make an autonomousPeriodic call to check a cancel method...

public void autonomousPeriodic() {
    if(cancel()){
        Scheduler.getInstance().disable();
    } else {
        Scheduler.getInstance().run();
    }
}

private boolean cancel(){
    boolean ahrsDisconnected = !DriveTrain.sharedInstance().ahrs.isConnected();
    boolean lowVoltage = pdp.getCurrent(0) < 7.0;
    return ahrsDisconnected || lowVotalge; //keep adding with || statements
}