package org.usfirst.frc.team5806.robot;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {
	DriveTrain train;
	Joystick stick;
	
	AnalogInput leftDistance;
	AnalogInput rightDistance;
	
	Counter gearEncoder;
	Victor spinnyMotor;
	NeoMagic neoMagic;

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	@Override
	public void robotInit() {
		train = new DriveTrain();
		stick = new Joystick(0);
		//gearEncoder = new Counter(new DigitalInput(0));
		//leftDistance = new AnalogInput(0);
		//rightDistance = new AnalogInput(1);
		neoMagic = new NeoMagic();
		spinnyMotor = new Victor(2);
	}
	
	public double[] getDistance(){
		//triggerDistance.set(false);
		return new double[]{leftDistance.getVoltage()/0.0049, rightDistance.getVoltage()*1024};	
	}
	/**
	 * This function is run once each time the robot enters autonomous mode
	 */
	@Override
	public void autonomousInit() {
	}

	/**
	 * This function is called periodically during autonomous
	 */
	@Override
	public void autonomousPeriodic() {
	}

	/**
	 * This function is called once each time the robot enters tele-operated
	 * mode
	 */
	@Override
	public void teleopInit() {
		train.lEncoder.reset();
		train.rEncoder.reset();
		train.ahrs.reset();
		
		//train.turn(0.5, 89.75, 40);
		//train.turn(0.5, 89.75, 40);
		
		/*train.driveFoward(0.5, 6.5*12);
		train.turn(0.5, 89, 30);
		train.turn(0.5, 89, 30);
		train.driveFoward(0.5, 2*12+6.5);*/

		//gearEncoder.reset();
		
		neoMagic.setAll(new NeoMagic.Pixel(0, 255, 0));
	}

	
	@Override
	public void teleopPeriodic() {
		//train.setSpeeds(1, 1);
		//train.setSpeeds(-stick.getRawAxis(1)*0.5, -stick.getRawAxis(5)*0.5);
		//train.getDistance();
		//train.updateDashboard();
		//spinnyMotor.set(-stick.getRawAxis(1)*0.4);
		//SmartDashboard.putNumber("touchlessInch", enc.get() / 20.0);
		//SmartDashboard.putNumber("LeftDistance", getDistance()[0]);
	}

	/**
	 * This function is called periodically during test mode
	 */
	@Override
	public void testPeriodic() {
		LiveWindow.run();
	}
}
@@ -473,24 +473,65 @@
 libj.Timer;%0A
+import edu.wpi.first.wpilibj.Ultrasonic;%0A
 import edu.w
@@ -1102,98 +1102,87 @@
  %7B%0A%09
-DriveTrain train;%0A%09Joystick stick;%0A%09%0A%09AnalogInput leftDistance;%0A%09AnalogInput rightDistance
+static final int trigPin = 6, echoPin = 5;%0A%09%0A%09DriveTrain train;%0A%09Joystick stick
 ;%0A%09%0A
@@ -1243,16 +1243,35 @@
 eoMagic;
+%0A%09Ultrasonic sonar;
 %0A%0A%09/**%0A%09
@@ -1675,170 +1675,56 @@
 );%0A%09
-%7D%0A%09%0A%09public double%5B%5D getDistance()%7B%0A%09%09//triggerDistance.set(false);%0A%09%09return new double%5B%5D%7BleftDistance.getVoltage()/0.0049, rightDistance.getVoltage()*1024%7D;%09
+%09sonar = new Ultrasonic(trigPin, echoPin);
 %0A%09%7D%0A
+%09%0A
 %09/**
@@ -2452,16 +2452,48 @@
 5, 0));%0A
+%09%09sonar.setAutomaticMode(true);%0A
 %09%7D%0A%0A%09%0A%09@
@@ -2833,20 +2833,16 @@
 er(%22
-Left
 Distance
 %22, g
@@ -2841,27 +2841,40 @@
 ance
-%22, getDistance()%5B0%5D
+ (in): %22, sonar.getRangeInches()
 );%0A%09

Update for new sonar