// 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.9  2013-03-15 04:23:25  echan
 * Added the lookup table
 *
 * 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()
    {
        if (!Preferences.getInstance().containsKey("LEDoffAngle"))
        {
            Preferences.getInstance().putDouble("LEDoffAngle", 45);
            Preferences.getInstance().save();
        }
        return Preferences.getInstance().getInt("LEDoffAngle", 45);
    }
    
    public double turnOnLEDAngle()
    {       
        if (!Preferences.getInstance().containsKey("LEDonAngle"))
        {
            Preferences.getInstance().putDouble("LEDonAngle", 35);
            Preferences.getInstance().save();
        }
        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,102 @@
 ava,v $%0A
+ * Revision 1.10  2013-03-16 01:16:33  jross%0A * check if LED preferences key exist%0A *%0A
  * Revis
@@ -3456,27 +3456,30 @@
 stance().get
-Int
+Double
 (%22LEDoffAngl
@@ -3793,11 +3793,14 @@
 .get
-Int
+Double
 (%22LE

fix preferences angle to get doubles instead of ints