/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.edinarobotics.zephyr;


import com.edinarobotics.utils.gamepad.Gamepad;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SimpleRobot;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Zephyr extends SimpleRobot {
    private double leftDrive = 0;
    private double rightDrive = 0;
    private double shooterSpeed = 0;
    private boolean ballLoaderUp = false;
    
    private double lastManualSpeed = 0;
    
    private final double SHOOTER_SPEED_STEP = 0.0005;
    
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        Gamepad gamepad1 = new Gamepad(1);
        Gamepad gamepad2 = new Gamepad(2);
        Components components = Components.getInstance();
        while(this.isOperatorControl()&&this.isEnabled()){
           leftDrive = gamepad1.getLeftY();
           rightDrive = gamepad1.getRightY()*-1;
           if(gamepad1.getRawButton(Gamepad.RIGHT_BUMPER)){
               //Step speed of shooter up.
               shooterSpeed -= SHOOTER_SPEED_STEP;
               if(shooterSpeed<=-1){
                   shooterSpeed = -1;//Max speed is reverse 1 (-1).
               }
               lastManualSpeed = shooterSpeed;
           }
           else if(gamepad1.getRawButton(Gamepad.LEFT_BUMPER)){
               //Step speed of shooter down.
               shooterSpeed += SHOOTER_SPEED_STEP;
               if(shooterSpeed>=0){
                   shooterSpeed = 0;
               }
               lastManualSpeed = shooterSpeed;
           }
           if(gamepad1.getRawButton(Gamepad.BUTTON_1)){
               //Jump shooter speed to max.
               shooterSpeed = -1; //Max is -1
           }
           else if(gamepad1.getRawButton(Gamepad.BUTTON_2)){
               //Jump shooter speed to min.
               shooterSpeed = 0;
           }
           else if(gamepad1.getRawButton(Gamepad.BUTTON_3)){
               shooterSpeed = lastManualSpeed;
           }
           ballLoaderUp = gamepad1.getRawButton(Gamepad.RIGHT_TRIGGER);
           mechanismSet();
        }
        stop();
    }
    
    private void mechanismSet(){
        Components robotParts = Components.getInstance();
        robotParts.driveControl.tankDrive(leftDrive, rightDrive);
        robotParts.shooterJaguar.set(shooterSpeed);
        robotParts.ballLoadPiston.set((ballLoaderUp ? Relay.Value.kReverse :
                                                      Relay.Value.kForward));
        String shooterPowerString = "Shooter: "+shooterSpeed;
        robotParts.textOutput.println(DriverStationLCD.Line.kUser2, 1, shooterPowerString);
        robotParts.textOutput.updateLCD();
    }
    
    private void stop(){
        leftDrive = 0;
        rightDrive = 0;
        shooterSpeed = 0;
        ballLoaderUp = false;
        mechanismSet();
    }
}

@@ -555,23 +555,252 @@
 gamepad.
-Gamepad
+FilterSet;%0Aimport com.edinarobotics.utils.gamepad.Gamepad;%0Aimport com.edinarobotics.utils.gamepad.GamepadResult;%0Aimport com.edinarobotics.utils.gamepad.filters.DeadzoneFilter;%0Aimport com.edinarobotics.utils.gamepad.filters.ScalingFilter
 ;%0Aimport
@@ -1815,24 +1815,169 @@
 Control() %7B%0A
+        FilterSet filters = new FilterSet();%0A        filters.addFilter(new DeadzoneFilter(.05));%0A        filters.addFilter(new ScalingFilter());%0A
         Game
@@ -2007,16 +2007,16 @@
 pad(1);%0A
-
         
@@ -2182,28 +2182,105 @@
     
-leftDrive = gamepad1
+GamepadResult joystick = filters.filter(gamepad1.getJoysticks());%0A           leftDrive = joystick
 .get
@@ -2312,24 +2312,24 @@
 Drive = 
-gamepad1
+joystick
 .getRigh

Add DeadzoneFilter and ScalingFilter filters to operatorControl.