package org.usfirst.frc.team3335.robot.subsystems;

import org.usfirst.frc.team3335.robot.RobotMap;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class DoubleUltrasonic extends Subsystem implements LoggableSubsystem {

	// distance in inches the robot wants to stay from an object
	//private static final double kHoldDistance = 12.0;

	// factor to convert sensor values to a distance in inches
	private static final double kValueToInches = 0.125;

	private AnalogInput ultrasonicLeft = new AnalogInput(RobotMap.ANALOG_ULTRASONIC_LEFT);
	private AnalogInput ultrasonicRight = new AnalogInput(RobotMap.ANALOG_ULTRASONIC_RIGHT);

	public double getDistanceLeft() {
		return getDistance(ultrasonicLeft);
	}

	public double getDistanceRight() {
		return getDistance(ultrasonicRight);
	}

	private double getDistance(AnalogInput ultrasonic) {
		return ultrasonic.getValue() * kValueToInches;
	}

	@Override
	protected void initDefaultCommand() {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void log() {
		SmartDashboard.putNumber("Ultrasonic: Left", getDistanceLeft());
		SmartDashboard.putNumber("Ultrasonic: Right", getDistanceRight());
	}

}

@@ -516,16 +516,18 @@
 nches%0D%0A%09
+//
 private 
@@ -569,24 +569,237 @@
 = 0.125;%0D%0A%0D%0A
+%09// factor to convert voltage values to a distance in inches%0D%0A%09// kVoltageToInches = (value V) * (1000mV/V) * (1mm/0.977mV) * (1cm/10mm) * (1in/2.54cm)%0D%0A%09private static final double kVoltageToInches = 40.2969;%0D%0A%0D%0A
 %09private Ana
@@ -1185,24 +1185,26 @@
 sonic) %7B%0D%0A%09%09
+//
 return ultra
@@ -1239,16 +1239,75 @@
 nches;%0D%0A
+%09%09return ultrasonic.getAverageVoltage()*kVoltageToInches;%0D%0A
 %09%7D%0D%0A%0D%0A%09@
@@ -1482,16 +1482,215 @@
 ic: Left
+ Avg Volt%22, ultrasonicLeft.getAverageVoltage());%0D%0A%09%09SmartDashboard.putNumber(%22Ultrasonic: Right Avg Volt%22, ultrasonicRight.getAverageVoltage());%0D%0A%09%09SmartDashboard.putNumber(%22Ultrasonic: Left Distance
 %22, getDi
@@ -1750,16 +1750,25 @@
 c: Right
+ Distance
 %22, getDi

Update DoubleUltrasonic class to use average voltage value, and set proper scaling factor.