/* 
 * Copyright (c) 2016 RobotsByTheC. All rights reserved.
 *
 * Open Source Software - may be modified and shared by FRC teams. The code must
 * be accompanied by the BSD license file in the root directory of the project.
 */
package org.usfirst.frc.team2084.CMonster2016.commands;

import org.usfirst.frc.team2084.CMonster2016.vision.VisionParameters;

/**
 * Notifies the driver about whether the robot is ready to take a shot. This
 * prints a value on the SmartDashboard.
 * 
 * @author Ben Wolsieffer
 */
public class SetCameraAutoExposure extends ParameterCommand {

    public static final String ENABLED_KEY = "Enabled";

    /**
     * Updates the SmartDashboard indicator based on whether the robot is ready
     * to shoot
     * 
     * @param ready whether the robot is ready
     */
    public SetCameraAutoExposure(boolean enabled) {
        setRunWhenDisabled(true);
        addBooleanParameter(ENABLED_KEY, enabled);
    }

    @Override
    protected void initialize() {
        VisionParameters.setAutoExposure(getBooleanParameter(ENABLED_KEY));
    }

    @Override
    protected void execute() {
    }

    @Override
    protected boolean isFinished() {
        return isTimedOut();
    }

    @Override
    protected void end() {
    }

    @Override
    protected void interrupted() {
    }
}

@@ -626,16 +626,67 @@
 bled%22;%0A%0A
+    public static final double ENABLE_TIME = 0.3;%0A%0A
     /**%0A
@@ -1032,24 +1032,198 @@
 tialize() %7B%0A
+        boolean enable = getBooleanParameter(ENABLED_KEY);%0A%0A        if (enable) %7B%0A            setTimeout(ENABLE_TIME);%0A        %7D else %7B%0A            setTimeout(0);%0A        %7D%0A%0A
         Visi
@@ -1255,40 +1255,14 @@
 ure(
-getBooleanParameter(ENABLED_KEY)
+enable
 );%0A 

Add delay to camera autoexposure enabling.