// 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.subsystems;
import org.usfirst.frc330.Beachbot2013Java.RobotMap;
import org.usfirst.frc330.Beachbot2013Java.commands.*;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc330.Beachbot2013Java.Robot;
/*
 * $Log: FrisbeePickup.java,v $
 * Revision 1.13  2013-03-17 20:26:34  jross
 * count frisbees
 *
 * Revision 1.12  2013-03-17 17:15:32  jross
 * fix logic for PickupDown timer
 *
 * Revision 1.11  2013-03-17 01:57:22  jdavid
 * Added pickup sensor
 *
 * Revision 1.10  2013-03-15 02:51:28  echan
 * added cvs log comments
 *
 */
 
/**
 *
 */
public class FrisbeePickup extends Subsystem {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    SpeedController frisbeePickupController = RobotMap.frisbeePickupFrisbeePickupController;
    DoubleSolenoid pickupSolenoid = RobotMap.frisbeePickupPickupSolenoid;
    DigitalInput pickupDiscSensor = RobotMap.frisbeePickupPickupDiscSensor;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public FrisbeePickup() {
        InitializeSlowFrisbeePickup();
        InitializeFrisbeePickup();
        sensorState = pickupDiscSensor.get();
    }
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	
        // Set the default command for a subsystem here.
//        setDefaultCommand(new PickupUp());
    }
    
    public double getFrisbeePickup() {
        if (!Preferences.getInstance().containsKey("FrisbeePickupSetpoint"))
        {
            Preferences.getInstance().putDouble("FrisbeePickupSetpoint", 0.0);
            Preferences.getInstance().save();
        }
        return Preferences.getInstance().getDouble("FrisbeePickupSetpoint", 0.0);
    }
    
    public void InitializeFrisbeePickup()
    {
        if (!Preferences.getInstance().containsKey("FrisbeePickupMotorOutput"))
        {
            Preferences.getInstance().putDouble("FrisbeePickupMotorOutput", 0.6);
            Preferences.getInstance().save();
        }
    }
    
    public void InitializeSlowFrisbeePickup()
    {
        if (!Preferences.getInstance().containsKey("SlowFrisbeePickupMotorOutput"))
        {
            Preferences.getInstance().putDouble("SlowFrisbeePickupMotorOutput", 0.3);
            Preferences.getInstance().save();
        }
    }
    public void toggleFrisbeePickup()
    {
        if (isPickupDown())
            setFrisbeePickupUp();
        else
            setFrisbeePickupDown();
    }
    
    public void setFrisbeePickupUp()
    {
      pickupSolenoid.set(DoubleSolenoid.Value.kForward);
//        System.err.println("setFrisbeePickupUp");
    }
    private double pickupDownTime;
    public void setFrisbeePickupDown()
    {
        if (!isPickupDown())
            pickupDownTime = Timer.getFPGATimestamp();
        pickupSolenoid.set(DoubleSolenoid.Value.kReverse);
//        System.err.println("setFrisbeePickupDown");
    }
    
    public boolean isPickupDown()
    {
        if (pickupSolenoid.get() == DoubleSolenoid.Value.kReverse)
        {
            return true;
        }
        else
            return false;
    }
    
    public double getPickupDownTime()
    {
        return pickupDownTime;
    }
    
    public void setFrisbeePickupMotorStop()
    {
        frisbeePickupController.set(0);
//        System.err.println("setFrisbeePickupMotorStop");
    }
    
    public void setFrisbeePickupMotor(double speed)
    {
        frisbeePickupController.set(speed);
//        System.err.println("setFrisbeePickupMotor: " + speed);
    }
    
    public void setFrisbeePickupMotorPickup()
    {
        frisbeePickupController.set(Preferences.getInstance().getDouble("FrisbeePickupMotorOutput", 0.6));
//        System.err.println("setFrisbeePickupMotorPickup");
    }
    
    public void setSlowFrisbeePickupMotorPickup()
    {
        frisbeePickupController.set(Preferences.getInstance().getDouble("SlowFrisbeePickupMotorOutput", 0.3));
//        System.err.println("setSlowFrisbeePickupMotorPickup");
    }
    
    public void setFrisbeePickupMotorReverse()
    {
        frisbeePickupController.set(-Preferences.getInstance().getDouble("FrisbeePickupMotorOutput", 0.6));
//        System.err.println("setFrisbeePickupMotorReverse");
    }
    
    public void calcPeriodic()
    {
        countFrisbees();
    }
    
    boolean sensorState;
    int frisbeeCount = 0;
    private void countFrisbees()
    {
        if (pickupDiscSensor.get() == false && sensorState == true)
        {
            frisbeeCount++;
            SmartDashboard.putNumber("FrisbeeCount", frisbeeCount);
        }
        sensorState = pickupDiscSensor.get();
    }
    
    public void decrementFrisbees()
    {
        if (frisbeeCount > 0)
            frisbeeCount--;
        SmartDashboard.putNumber("FrisbeeCount", frisbeeCount);
    }
}

@@ -868,16 +868,96 @@
 ava,v $%0A
+ * Revision 1.14  2013-03-20 06:06:43  dstation%0A * add pickup toggle command%0A *%0A
  * Revis
@@ -1810,38 +1810,17 @@
 State = 
-pickupDiscSensor.get()
+0
 ;%0A    %7D%0A
@@ -5171,23 +5171,19 @@
    %0A    
-boolean
+int
  sensorS
@@ -5186,16 +5186,20 @@
 sorState
+ = 0
 ;%0A    in
@@ -5319,20 +5319,17 @@
 tate == 
-true
+5
 )%0A      
@@ -5360,16 +5360,43 @@
 ount++;%0A
+            sensorState++;%0A
         
@@ -5473,30 +5473,25 @@
         
-sensorState = 
+else if (
 pickupDi
@@ -5504,17 +5504,135 @@
 or.get()
-;
+ == false)%0A        %7B%0A            sensorState++;%0A        %7D%0A        else%0A        %7B%0A            sensorState = 0;%0A        %7D
 %0A    %7D%0A 

add a filter to need 5 consecutive samples of frisbee detected