// RobotBuilder Version: 0.0.2
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in th future.
package org.usfirst.frc330.Beachbot2013Java.commands;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.AutoSpreadsheetCommand;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc330.Beachbot2013Java.Robot;
/*
 * $Log: LaunchFrisbee.java,v $
 * Revision 1.17  2013-03-29 03:55:43  jross
 * keep shooting if trigger is pressed
 *
 * Revision 1.16  2013-03-26 05:40:09  jross
 * add action item
 *
 * Revision 1.15  2013-03-24 18:12:05  jross
 * if shooter wheel isn't up to speed, wait 1.5 seconds then launch it anyway
 *
 * Revision 1.14  2013-03-21 02:08:29  echan
 * removed the todo
 *
 * Revision 1.13  2013-03-18 06:48:06  jross
 * check arm angle and speed before shooting
 *
 * Revision 1.12  2013-03-17 20:26:34  jross
 * count frisbees
 *
 * Revision 1.11  2013-03-17 17:14:22  jross
 * don't require shooter, because it may stop the shooter spinning
 *
 * Revision 1.10  2013-03-17 01:57:22  jdavid
 * Added pickup sensor
 *
 * Revision 1.9  2013-03-16 21:17:47  jross
 * add javadocs
 *
 * Revision 1.8  2013-03-15 02:58:19  echan
 * robotbuilder update
 *
 * Revision 1.7  2013-03-15 02:50:55  echan
 * added cvs log comments
 *
 */
/**
 * Shoot a frisbee. Actuate the cylinder immediately and then wait until
 * retracting it. The retract time is the Preference "solenoidOffTime".
 */
public class  LaunchFrisbee extends Command implements AutoSpreadsheetCommand {
    public LaunchFrisbee() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
	
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        SmartDashboard.putNumber("ShooterLowRPM", 3300);
    }
    private double endTime;
    private double solenoidOffTime;
    final static int waitingForSpeed = 0;
    final static int solenoidOn = 1;
    final static int solenoidOff = 2;
    final static int finish = 3;
    int state;
    int speedCounter = 0;
    double timeSinceStart = 0;
    // Called just before this Command runs the first time
    protected void initialize() {
        state = waitingForSpeed;
        timeSinceStart = Timer.getFPGATimestamp();
        if (Robot.shooterLow.getSpeed() == 0)
        {
            state = finish;
        }
    }
    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        
        switch (state)
        {
            case waitingForSpeed:
                if (Math.abs(Robot.shooterLow.getSpeed() - Robot.shooterLow.getShootLowSetpoint()) < 50 )
                {
                    speedCounter++;
                }
                else
                    speedCounter = 0;
                
                if ((speedCounter > 25 || Timer.getFPGATimestamp() > timeSinceStart + 1.5) && Robot.arm.onTarget())
                {
                    state = solenoidOn;
                    solenoidOffTime = Robot.shooterLow.launchFrisbeeSolenoidOffTime() + Timer.getFPGATimestamp();
                }
                break;
            case solenoidOn:
                Robot.shooterLow.armLoadShooterOn();
                if (Timer.getFPGATimestamp() > solenoidOffTime)
                {
                    state = solenoidOff;
                    endTime = Robot.shooterLow.launchFrisbeeSolenoidOffTime() + Timer.getFPGATimestamp();
                }
                SmartDashboard.putBoolean("TakeScreenshot", true);
                break;
            case solenoidOff:    
                Robot.shooterLow.armLoadShooterOff();
                if (Timer.getFPGATimestamp() > endTime)
                {
                    if (Robot.oi.shootButton.get() == true)
                    {
                        initialize();
                    }
                    else
                    {
                        state = finish;
                    }
                }
                break;
        }
    }
    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        if (state == finish || isTimedOut()) 
        {
            return true;
        }
        return false;
    }
    // Called once after isFinished returns true
    protected void end() {
//        Robot.shooterLow.armLoadShooterOff();
        if (Robot.shooterLow.getSpeed() > 0)
            Robot.frisbeePickup.decrementFrisbees();
        System.out.println("Launch Frisbee Finished");
    }
    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
    /**
     * Not Used
     * @param param1 
     */
    public void setParam1(double param1) {
    }
    /**
     * Not Used
     * @param param2 
     */
    public void setParam2(double param2) {
    }
    /**
     * Not Used
     * @param param3 
     */
    public void setParam3(double param3) {
    }
    /**
     * Not Used
     * @param stopAtEnd 
     */
    public void setStopAtEnd(boolean stopAtEnd) {
    }
    public Command copy() {
        return new LaunchFrisbee();
    }
}

@@ -863,16 +863,148 @@
 ava,v $%0A
+ * Revision 1.18  2013-03-30 01:45:58  jross%0A * change speedCounter check to 25 (0.5 seconds), to give time for frisbees to drop%0A *%0A
  * Revis
@@ -3006,32 +3006,59 @@
 nish;%0A        %7D%0A
+        speedCounter = 0; %0A
     %7D%0A    // Cal

set speedCounter to 0 in initialize so it can't phantom shoot