Branch in Command Group

Technical Java
1

The Context
The autonomous of the robot consists of moving forward, turning, the continuing forward to place a gear. Occasionally, the robot has trouble turning which is a separate issue (likely mechanical :stuck_out_tongue: ) and when this happens, we are unable get the robot to turn in a reasonable amount of time using our PID loop. As we are working through that issue, we would like to be able to stop our autonomous from executing the rest of the sequence (since it will obviously fail miserably if we donā€™t turn with precision).

The Question
Is there an easy way to branch to a different command sequence given the previous command timed out?

Here is pseudo-code of what I would like to do.


addSequential(new DrivetrainDriveToDistance(2.0, 0.05), 4); //(distance, tolerance), timeout
addSequential(new DrivetrainRotateToAngle(180.0, 1.0), 4);

if (RotationCommand did not timeout)
  addSequential(new DrivetrainDriveToDistance(2.0, 0.05), 4);
else
  doOtherStuff()
2

Have you looked at ConditionalCommand?

3

I havenā€™t tested this out, but you can call the .start() method on a command to schedule it. Hereā€™s an example:

    	
CollectorStop command = new CollectorStop();
command.start();
4

maybe DrivetrainRotateToAngle sets a global boolean to false when it initialize then to true if it finished properly. Then create a different DrivetrainDriveToDistanceConditional that checks the global boolean.

I want to loon more into ConditionalCommand. I have not seen or used that yet.

5 
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.ConditionalCommand;

/**
 *
 */
public class myConditionalCommand extends ConditionalCommand {

	public myConditionalCommand(Command onTrue, Command onFalse) {
		super(onTrue, onFalse);
		// TODO Auto-generated constructor stub
	}

	@Override
	protected boolean condition() {
		// TODO Auto-generated method stub
		return false;
	}

   
}

Looks like you may still need that global variable or some way to fill in the return statement.

6

Would another option be the inclusion of an ā€œonFailā€ command to run if your command did not reach its goal in the required amount of time?


   /**
    * Rotate robot to a specified target angle.
    *
    * @param targetAngle - Desired angle to rotate to.
    * @param onFail - Optional command to start if we fail to reach the desired target in time.
    * @param timeoutDur - How long (in seconds) before we give up and start the onFail command.
    */
   public DriveRotateToAngle(double targetAngle, Command onFail, double timeoutDur) {
       this.targetAngle = targetAngle;
       this.runTime = timeoutDur;
       this.onFail = onFail;

       ... rest of construction and requires ...
   }


   protected boolean isFinished() {

     // Done if we reached our target
     if (isOnTarget()) {
       return true;
     }

     // If we didn't get to the target in time and we have a fail
     // command available, start it.

     if (onFail != null && timeSinceInitized() > runTime) {
       onFail.start();

       // Whether we indicate this command is finished at this point is
       // a good question. We can either rely/hope the command we
       // started will interrupt us as needed and continue trying to rotate
       // until interrupted, or we could return true here if we want to risk
       // letting the next step in a sequence occur. I vote that we wait for
       // the interrupt and leave the return commented out.

       // return true;
     }

     return false;
  }
7