// 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.buttons.NetworkButton;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/*
 * $Log: Vision.java,v $
 * Revision 1.8  2013-03-15 02:51:40  echan
 * added cvs log comments
 *
 */
 
/**
 *
 */
public class Vision extends Subsystem {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    DigitalOutput highShooterLED = RobotMap.visionHighShooterLED;
    DigitalOutput lowShooterLED = RobotMap.visionLowShooterLED;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    boolean highLEDstate, lowLEDstate;
    double[][]aP =   {{10, 15, 20, 25, 30, 35, 40, 45, 50},
                    {0, .25, .5, .75, 1, 1.25, 1.5, 1.75, 2}};
    double leftx = 0, rightx = 0, lefty = 0, righty = 0, dx, dy, m, y;
    
    public Vision() {
        SmartDashboard.putBoolean("LEDEnable", false);
        SmartDashboard.putBoolean("LEDOverride", false);
        highLEDstate = false;
        lowLEDstate = false;
    }
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
        setDefaultCommand(new ControlLEDs());
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    public void turnOnHighShooterLED()
    {
        highShooterLED.set(true);
        highLEDstate = true;
    }
    
    public void turnOffHighShooterLED()
    {
        highShooterLED.set(false);
        highLEDstate = false;
    }
    
    public void turnOnLowShooterLED()
    {
        lowShooterLED.set(true);
        lowLEDstate = true;
    }
    
    public void turnOffLowShooterLED()
    {
        lowShooterLED.set(false);
        lowLEDstate = false;
    }
    
    public boolean getLEDEnable()
    {
        return SmartDashboard.getBoolean("LEDEnable", false);
    }
    
    public boolean getLEDOverride()
    {
        return SmartDashboard.getBoolean("LEDOverride", false);
    }
    
    public boolean getHighLEDState()
    {
        return highLEDstate;
    }
    
    public boolean getLowLEDState()
    {
        return lowLEDstate;
    }
    
    public double turnOffLEDAngle()
    {
        return Preferences.getInstance().getInt("LEDoffAngle", 45);
    }
    
    public double turnOnLEDAngle()
    {
        return Preferences.getInstance().getInt("LEDonAngle", 35);
    }
    
    public double armLookupTable(double x)
    {
        if (x != -1)
        {
        for (int i = 0; aP[0][i] < x; i++)
        {
            leftx = aP[0][i];
            lefty = aP[1][i];
        }
        
        for (int i = aP.length; aP[0][i] > x; i--)
        {
            rightx = aP[0][i];
            righty = aP[1][i];
        }
        
        dx = rightx - leftx;
        dy = righty - lefty; 
        m = dy/dx;
        y = m*(x-leftx) + lefty;
        return y;
        }
        
        else
        {
            return aP[1][0];
        }
    }
    
    public double getDistance()
    {
        return SmartDashboard.getNumber("DistanceToCenter",-1);
    }
    
    public double getAngle()
    {
        return SmartDashboard.getNumber("AngleToCenter",0);
    }
}

@@ -863,16 +863,89 @@
 ava,v $%0A
+ * Revision 1.9  2013-03-15 04:23:25  echan%0A * Added the lookup table%0A *%0A
  * Revis
@@ -3141,119 +3141,526 @@
     
-return Preferences.getInstance().getInt(%22LEDoffAngle%22, 45);%0A    %7D%0A    %0A    public double turnOnLEDAngle()%0A    %7B
+if (!Preferences.getInstance().containsKey(%22LEDoffAngle%22))%0A        %7B%0A            Preferences.getInstance().putDouble(%22LEDoffAngle%22, 45);%0A            Preferences.getInstance().save();%0A        %7D%0A        return Preferences.getInstance().getInt(%22LEDoffAngle%22, 45);%0A    %7D%0A    %0A    public double turnOnLEDAngle()%0A    %7B       %0A        if (!Preferences.getInstance().containsKey(%22LEDonAngle%22))%0A        %7B%0A            Preferences.getInstance().putDouble(%22LEDonAngle%22, 35);%0A            Preferences.getInstance().save();%0A        %7D
 %0A   

check if LED preferences key exist