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

import org.usfirst.frc.team4373.robot.Robot;
import org.usfirst.frc.team4373.robot.RobotMap;
import org.usfirst.frc.team4373.robot.commands.CommandBase;
import org.usfirst.frc.team4373.robot.commands.MoveForklift;
import org.usfirst.frc.team4373.robot.commands.RooIntakeCommand;

import util.RooMath;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.command.Subsystem;

public class RooForklift extends Subsystem {
	private Talon motorPair;
	private boolean reversed = false;
	public static final int p1=10,p2=2,p3=3,p4=4;
	private static final double deadZone = 1;
	private static final double power = 0.5;
	
	public RooForklift() {
		motorPair = new Talon(RobotMap.forkliftMotorPairPort);
	}
	public void moveToPosition(int position) {
		int distanceFromTarget = CommandBase.getOI().getSchmencoderPosition() - position;
		if (Math.abs(distanceFromTarget) > deadZone) {
			double motorPower = -power * (RooMath.getSign(distanceFromTarget));
			CommandBase.getOI().rd.putNumber("Forklift move to position power: ", motorPower);
			CommandBase.getOI().rd.putNumber("Forklift distance from dest: ", distanceFromTarget);
			motorPair.set(motorPower);
		}
		else {
			CommandBase.getOI().rd.putNumber("FOrklift move to position power: ", 0.0D);
			motorPair.set(0.0D);
		}
			
	}
	@Override
	protected void initDefaultCommand() {
		setDefaultCommand(new MoveForklift());
		
	}
	public void set(double val) {
		motorPair.set(val);
	}
	
	public int getDirection(){
		return RooMath.getSign(motorPair.get());
	}
}

@@ -43,19 +43,17 @@
 systems;
-%0D%0A%0D
+%0A
 %0Aimport 
@@ -89,17 +89,16 @@
 t.Robot;
-%0D
 %0Aimport 
@@ -137,17 +137,16 @@
 obotMap;
-%0D
 %0Aimport 
@@ -197,17 +197,16 @@
 andBase;
-%0D
 %0Aimport 
@@ -258,17 +258,16 @@
 orklift;
-%0D
 %0Aimport 
@@ -323,19 +323,17 @@
 Command;
-%0D%0A%0D
+%0A
 %0Aimport 
@@ -345,17 +345,16 @@
 RooMath;
-%0D
 %0Aimport 
@@ -381,17 +381,16 @@
 j.Talon;
-%0D
 %0Aimport 
@@ -433,11 +433,9 @@
 tem;
-%0D%0A%0D
+%0A
 %0Apub
@@ -475,17 +475,16 @@
 system %7B
-%0D
 %0A%09privat
@@ -501,17 +501,16 @@
 torPair;
-%0D
 %0A%09privat
@@ -536,17 +536,16 @@
 = false;
-%0D
 %0A%09public
@@ -566,17 +566,16 @@
 int p1=1
-0
 ,p2=2,p3
@@ -582,17 +582,16 @@
 =3,p4=4;
-%0D
 %0A%09privat
@@ -625,17 +625,16 @@
 one = 1;
-%0D
 %0A%09privat
@@ -667,20 +667,18 @@
 r = 0.5;
-%0D
 %0A%09
-%0D
 %0A%09public
@@ -693,17 +693,16 @@
 lift() %7B
-%0D
 %0A%09%09motor
@@ -750,21 +750,19 @@
 irPort);
-%0D
 %0A%09%7D
-%0D
 %0A%09public
@@ -797,17 +797,16 @@
 ition) %7B
-%0D
 %0A%09%09int d
@@ -881,17 +881,16 @@
 osition;
-%0D
 %0A%09%09if (M
@@ -932,41 +932,36 @@
 one)
- %7B%0D
 %0A%09%09%09
-double motorP
+motorPair.set(p
 ower 
-= -power
+* -1
  * (
@@ -1000,354 +1000,44 @@
 et))
-;%0D%0A%09%09%09CommandBase.getOI().rd.putNumber(%22Forklift move to position power: %22, motorPower);%0D%0A%09%09%09CommandBase.getOI().rd.putNumber(%22Forklift distance from dest: %22, distanceFromTarget);%0D%0A%09%09%09motorPair.set(motorPower);%0D%0A%09%09%7D%0D%0A%09%09else %7B%0D%0A%09%09%09CommandBase.getOI().rd.putNumber(%22FOrklift move to position power: %22, 
+);%0A%09%09else%0A%09%09%09motorPair.set(
 0.0D);
-%0D
 %0A%09
-%09%09motorPair.set(0.0D);%0D%0A%09%09%7D%0D%0A%09%09%09%0D%0A%09%7D%0D
+%7D
 %0A%09@O
@@ -1043,17 +1043,16 @@
 Override
-%0D
 %0A%09protec
@@ -1082,17 +1082,16 @@
 mand() %7B
-%0D
 %0A%09%09setDe
@@ -1127,17 +1127,14 @@
 ());
-%0D
 %0A%09%09
-%0D
 %0A%09%7D
-%0D
 %0A%09pu
@@ -1160,17 +1160,16 @@
 e val) %7B
-%0D
 %0A%09%09motor
@@ -1186,16 +1186,13 @@
 al);
-%0D
 %0A%09%7D
-%0D
 %0A%09
-%0D
 %0A%09pu
@@ -1215,17 +1215,16 @@
 ction()%7B
-%0D
 %0A%09%09retur
@@ -1225,32 +1225,22 @@
 %09return 
-RooMath.getSign(
+(int) 
 motorPai
@@ -1250,15 +1250,10 @@
 et()
-);%0D
+;
 %0A%09%7D
-%0D
 %0A%7D
-%0D%0A

revert revert!