package org.texastorque.texastorque2015.input;

import org.texastorque.torquelib.util.GenericController;

public class DriverInput extends Input {

    GenericController driver;
    GenericController operator;

    public DriverInput() {
        driver = new GenericController(0, GenericController.TYPE_XBOX, 0.2);
        operator = new GenericController(1, GenericController.TYPE_XBOX, 0.2);

        override = false;
        armOpen = false;
        punchOut = false;
        tiltUp = false;
    }

    @Override
    public void run() {
        if (operator.getLeftCenterButton()) {
            override = true;
        } else if (operator.getRightCenterButton()) {
            override = false;
        }
        
        if (override) {
            calcOverride();
        } else {
            calcElevator();
        }
        
        calcIntake();
        calcDrivebase();
    }
    
    //Drivebase
    private void calcDrivebase() {
        leftSpeed = -1 * driver.getLeftYAxis() + driver.getRightXAxis();
        rightSpeed = -1 * driver.getLeftYAxis() - driver.getRightXAxis();
        strafeSpeed = driver.getLeftXAxis();

        if (driver.getLeftBumper()) {
            leftSpeed = leftSpeed / 2;
            rightSpeed = rightSpeed / 2;
        }
    }
    
    private void calcElevator() {
        
    }
    
    private void calcOverride() {
        overrideElevatorMotorSpeed = -1 * operator.getLeftYAxis();
    }
    
    private void calcIntake() {
        if (operator.getRightBumper()) {
            leftIntakeSpeed = 1.0 - operator.getRightXAxis() / 2;
            rightIntakeSpeed = 1.0 + operator.getRightXAxis() / 2;
            
            if (operator.getRightYAxis() > 0.75) {
                intakeIn = true;
            } else {
                intakeIn = false;
            }
        } else {
            leftIntakeSpeed = 0.0;
            rightIntakeSpeed = 0.0;
            intakeIn = false;
        }
    }
}

@@ -97,16 +97,68 @@
 troller;
+%0Aimport org.texastorque.torquelib.util.TorqueFilter;
 %0A%0Apublic
@@ -193,24 +193,32 @@
 nput %7B%0A%0A    
+private 
 GenericContr
@@ -235,16 +235,24 @@
 er;%0A    
+private 
 GenericC
@@ -272,16 +272,102 @@
 rator;%0A%0A
+    private TorqueFilter driveAccelFilter;%0A    private TorqueFilter turnAccelFilter;%0A%0A
     publ
@@ -538,24 +538,122 @@
 BOX, 0.2);%0A%0A
+        driveAccelFilter = new TorqueFilter(50);%0A        turnAccelFilter = new TorqueFilter(50);%0A%0A
         over
@@ -951,32 +951,24 @@
 ;%0A        %7D%0A
-        
 %0A        if 
@@ -1063,24 +1063,16 @@
       %7D%0A
-        
 %0A       
@@ -1113,28 +1113,24 @@
 se();%0A    %7D%0A
-    
 %0A    //Drive
@@ -1181,24 +1181,21 @@
     
-left
+strafe
 Speed =
- -1 *
  dri
@@ -1209,92 +1209,146 @@
 Left
-Y
+X
 Axis()
- + driver.getRightXAxis();%0A        rightSpeed = -1 * driver.getLeftYAxis() - 
+;%0A%0A        if (driver.getLeftBumper()) %7B%0A            driveAccelFilter.add(-driver.getLeftYAxis());%0A            turnAccelFilter.add(
 driv
@@ -1361,25 +1361,27 @@
 RightXAxis()
-;
+);%0A
 %0A        str
@@ -1373,38 +1373,40 @@
 );%0A%0A        
-strafe
+    left
 Speed = 
 driver.getLe
@@ -1397,148 +1397,427 @@
 d = 
+(
 drive
-r.getLeftXAxis();%0A%0A        if (driver.getLeftBumper()) %7B%0A            leftSpeed = leftSpeed / 2;%0A            rightSpeed = rightSpeed / 2
+AccelFilter.getAverage() + turnAccelFilter.getAverage()) / 2;%0A            rightSpeed = (driveAccelFilter.getAverage() - turnAccelFilter.getAverage()) / 2;%0A        %7D else %7B%0A            driveAccelFilter.reset();%0A            turnAccelFilter.reset();%0A            %0A            leftSpeed = -1 * driver.getLeftYAxis() + driver.getRightXAxis();%0A            rightSpeed = -1 * driver.getLeftYAxis() - driver.getRightXAxis()
 ;%0A  
@@ -1826,28 +1826,24 @@
     %7D%0A    %7D%0A
-    
 %0A    private
@@ -1869,27 +1869,15 @@
 ) %7B%0A
-        
 %0A    %7D%0A
-    
 %0A   
@@ -1980,20 +1980,16 @@
 ;%0A    %7D%0A
-    
 %0A    pri
@@ -2106,36 +2106,32 @@
 .getRightXAxis()
- / 2
 ;%0A            ri
@@ -2177,34 +2177,18 @@
 tXAxis()
- / 2;%0A            
+;%0A
 %0A       
@@ -2185,35 +2185,42 @@
 ;%0A%0A            i
-f (
+ntakeIn = 
 operator.getRigh
@@ -2238,113 +2238,9 @@
 0.75
-) %7B%0A                intakeIn = true;%0A            %7D else %7B%0A                intakeIn = false;%0A            %7D
+;
 %0A   

Change intake and drive controls