package com.saintsrobotics.frc.commands.autonomous;

import com.saintsrobotics.frc.commands.CommandBase;

/**
 * Move into the alliance's zone.
 * @author Saints Robotics
 */
public class MoveIntoZone extends CommandBase {
    private static final int FULL_SPEED_MOVE = 1;
    private static final int FULL_SPEED_ROTATE = 0;
    private static final double DRIVE_SECONDS = 1;
    
    private boolean isFinished;
    
    public MoveIntoZone() {
        requires(driveTrain);
    }
    
    /**
     * Called just before this Command runs the first time.
     */
    protected void initialize() {}

    /**
     * Called repeatedly when this Command is scheduled to run.
     */
    protected void execute() {
        double moveValue = FULL_SPEED_MOVE;
        double rotateValue = FULL_SPEED_ROTATE;
        
        driveTrain.arcadeDrive(moveValue, rotateValue);
        setTimeout(DRIVE_SECONDS);
        
        driveTrain.stop();
        
        isFinished = true;
    }

    /**
     * Returns whether this command is finished.
     * @return whether this command is finished
     */
    protected boolean isFinished() {
        return isFinished;
    }

    /**
     * Called once after isFinished returns true.
     */
    protected void end() {
        driveTrain.stop();
    }

    /**
     * Called when another command which requires one or more of the same
     * subsystems is scheduled to run.
     */
    protected void interrupted() {
        end();
    }
}

@@ -97,16 +97,52 @@
 andBase;
+%0Aimport edu.wpi.first.wpilibj.Timer;
 %0A%0A/**%0A *
@@ -438,24 +438,54 @@
 isFinished;%0A
+    private double startTime;%0A
     %0A    pub
@@ -657,121 +657,8 @@
 ze()
- %7B%7D%0A%0A    /**%0A     * Called repeatedly when this Command is scheduled to run.%0A     */%0A    protected void execute()
  %7B%0A 
@@ -825,44 +825,394 @@
     
-setTimeout(DRIVE_SECONDS);%0A        %0A
+%0A        // Record starting time of drive%0A        startTime = Timer.getFPGATimestamp();%0A    %7D%0A%0A    /**%0A     * Called repeatedly when this Command is scheduled to run.%0A     */%0A    protected void execute() %7B%0A        double currentTime = Timer.getFPGATimestamp();%0A        %0A        // Stop if more than DRIVE_SECONDS have elapsed%0A        if (currentTime - startTime %3E= DRIVE_SECONDS) %7B%0A    
     
@@ -1242,25 +1242,20 @@
         
-%0A   
     
- 
 isFinish
@@ -1265,16 +1265,26 @@
 = true;%0A
+        %7D%0A
     %7D%0A%0A 

Fix autonomous drive