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

import org.usfirst.frc.team4795.robot.RobotMap;
import org.usfirst.frc.team4795.robot.commands.HoldArm;
import org.usfirst.frc.team4795.robot.commands.ManualArm;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.CANJaguar.JaguarControlMode;
import edu.wpi.first.wpilibj.CANJaguar.LimitMode;
import edu.wpi.first.wpilibj.CANJaguar.NeutralMode;
import edu.wpi.first.wpilibj.command.Subsystem;

public class Arm extends Subsystem {
    
    private final CANJaguar motor;
    
    public Arm() {
        motor = new CANJaguar(RobotMap.ARM_MOTOR.value);
        motor.disableControl();
        motor.configEncoderCodesPerRev(497);
        motor.configMaxOutputVoltage(12);
        motor.configLimitMode(LimitMode.SwitchInputsOnly);
        //motor.setControlMode(JaguarControlMode.Position.getValue());
        //motor.setPositionMode(CANJaguar.kQuadEncoder, 497, 0.0, 0.0, 0.0);
        //motor.set(0.0);
        motor.setControlMode(JaguarControlMode.Position.getValue());
        motor.setPercentMode(CANJaguar.kQuadEncoder, 497);
        motor.configNeutralMode(NeutralMode.Brake);
        motor.enableControl();
    }
    
    public double getArmDegrees() {
        return motor.getPosition() * 360.0;
    }
    
    public void setPID(double P, double I, double D) {
        motor.setPID(P, I, D);
    }
    
    public void startSpeedMode(double p, double i, double d) {
        motor.disableControl();
        motor.setSpeedMode(CANJaguar.kQuadEncoder, 2048, p, i, d);
        motor.enableControl();
      }
    public void setArmDegrees(double angle, double P, double I, double D) {
        motor.setPID(P, I, D);
        motor.set(angle / 360.0);
    }
    
    @Override
    protected void initDefaultCommand() {
        setDefaultCommand(new HoldArm());
    }
    
    public void setControlMode(int mode){
    	motor.setControlMode(mode);	
    }
    public void setRaw(double value) {
    	motor.set(value);
    }
    public boolean getForwardLimit(){
    	return motor.getForwardLimitOK();
    }
    
    public boolean getBackLimit(){
    	return motor.getReverseLimitOK();
    }
    
}

@@ -503,16 +503,18 @@
    %0A    
+//
 private 
@@ -563,24 +563,26 @@
 () %7B%0A       
+//
  motor = new
@@ -619,32 +619,34 @@
 value);%0A        
+//
 motor.disableCon
@@ -652,32 +652,34 @@
 ntrol();%0A       
+//
  motor.configEnc
@@ -700,32 +700,34 @@
 v(497);%0A        
+//
 motor.configMaxO
@@ -744,32 +744,34 @@
 ge(12);%0A        
+//
 motor.configLimi
@@ -979,32 +979,34 @@
 t(0.0);%0A        
+//
 motor.setControl
@@ -1050,32 +1050,34 @@
 lue());%0A        
+//
 motor.setPercent
@@ -1115,24 +1115,26 @@
 7);%0A        
+//
 motor.config
@@ -1165,32 +1165,34 @@
 Brake);%0A        
+//
 motor.enableCont
@@ -1256,24 +1256,29 @@
      return 
+1; //
 motor.getPos
@@ -1360,32 +1360,34 @@
 le D) %7B%0A        
+//
 motor.setPID(P, 
@@ -1471,24 +1471,26 @@
 ) %7B%0A        
+//
 motor.disabl
@@ -1501,32 +1501,34 @@
 trol();%0A        
+//
 motor.setSpeedMo
@@ -1574,24 +1574,26 @@
 d);%0A        
+//
 motor.enable
@@ -1687,32 +1687,34 @@
 le D) %7B%0A        
+//
 motor.setPID(P, 
@@ -1728,16 +1728,18 @@
         
+//
 motor.se
@@ -1919,24 +1919,26 @@
 mode)%7B%0A    %09
+//
 motor.setCon
@@ -2004,16 +2004,18 @@
  %7B%0A    %09
+//
 motor.se
@@ -2072,32 +2072,39 @@
 ()%7B%0A    %09return 
+true;//
 motor.getForward
@@ -2172,16 +2172,23 @@
 %09return 
+true;//
 motor.ge

REMOVE THIS