package org.TexasTorque.TexasTorque2013.subsystem.manipulator;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.TexasTorque.TexasTorque2013.TorqueSubsystem;
import org.TexasTorque.TexasTorque2013.constants.Constants;
import org.TexasTorque.TorqueLib.controlLoop.SimPID;

public class Tilt extends TorqueSubsystem
{
    
    private static Tilt instance;
    
    private SimPID tiltPID;
    
    private double desiredTiltAngle;
    private double tiltMotorSpeed;
    
    public static double lowAngle;
    public static double highAngle;
    public static double feederStationAngle;
    public static double autonomousLowAngle;
    
    public static Tilt getInstance()
    {
        return (instance == null) ? instance = new Tilt() : instance;
    }
    
    public Tilt()
    {
        super();
        
        tiltPID = new SimPID();
        
        desiredTiltAngle = 0.0;
        tiltMotorSpeed = Constants.MOTOR_STOPPED;
    }
    
    public void run()
    {   
        double currentAngle = sensorInput.getTiltAngle();
        tiltMotorSpeed = tiltPID.calcPID(currentAngle);
    }
    
    public void setToRobot()
    {
        robotOutput.setTiltMotor(tiltMotorSpeed);
    }
    
    public void setTiltAngle(double angle)
    {
        if(angle != desiredTiltAngle)
        {
            desiredTiltAngle = angle;
            tiltPID.setDesiredValue(desiredTiltAngle);
        }
    }
    
    public boolean isLocked()
    {
        return tiltPID.isDone();
    }
    
    public String getKeyNames()
    {
        String names = "DesiredTiltAngle,";
        names += "TiltMotorSpeed,";
        names += "ActualTiltAngle,";
        
        return names;
    }
    
    public String logData()
    {
        String data = desiredTiltAngle + ",";
        data += tiltMotorSpeed + ",";
        data += sensorInput.getTiltAngle() + ",";
        
        return data;
    }
    
    public void loadParameters()
    {
        lowAngle = params.getAsDouble("S_ShootLowAngle", 0.0);
        highAngle = params.getAsDouble("S_ShootHighAngle", 0.0);
        feederStationAngle = params.getAsDouble("S_FeederStationAngle", 0.0);
        autonomousLowAngle = params.getAsDouble("A_RearLowAngle", lowAngle);
        
        double p = params.getAsDouble("S_TiltP", 0.0);
        double i = params.getAsDouble("S_TiltI", 0.0);
        double d = params.getAsDouble("S_TiltD", 0.0);
        double e = params.getAsDouble("S_TiltEpsilon", 0.0);
        double r = params.getAsDouble("S_TiltDoneRange", 0.0);
        
        tiltPID.setConstants(p, i, d);
        tiltPID.setErrorEpsilon(e);
        tiltPID.setDoneRange(r);
        tiltPID.resetErrorSum();
        tiltPID.resetPreviousVal();
    }
}

@@ -61,104 +61,8 @@
 r;%0A%0A
-import edu.wpi.first.wpilibj.Timer;%0Aimport edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;%0A
 impo
@@ -1922,25 +1922,25 @@
 etAsDouble(%22
-S
+T
 _ShootLowAng
@@ -1990,17 +1990,17 @@
 Double(%22
-S
+T
 _ShootHi
@@ -2064,17 +2064,17 @@
 Double(%22
-S
+T
 _FeederS
@@ -2214,25 +2214,25 @@
 etAsDouble(%22
-S
+T
 _TiltP%22, 0.0
@@ -2269,25 +2269,25 @@
 etAsDouble(%22
-S
+T
 _TiltI%22, 0.0
@@ -2324,25 +2324,25 @@
 etAsDouble(%22
-S
+T
 _TiltD%22, 0.0
@@ -2379,25 +2379,25 @@
 etAsDouble(%22
-S
+T
 _TiltEpsilon
@@ -2444,17 +2444,17 @@
 Double(%22
-S
+T
 _TiltDon
@@ -2460,32 +2460,99 @@
 neRange%22, 0.0);%0A
+        double maxOutput = params.getAsDouble(%22T_MaxOutput%22, 0.4);%0A
         %0A       
@@ -2717,20 +2717,61 @@
 viousVal();%0A
+        tiltPID.setMaxOutput(maxOutput);%0A
     %7D%0A%7D%0A

put in a motor limitor for the tilt