// 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.command.Subsystem;
import org.usfirst.frc330.wpilibj.BeachbotPrefSendablePIDController;
import org.usfirst.frc330.Beachbot2013Java.Robot;
/**
 *
 */
public class Arm extends Subsystem {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    SpeedController armSpeedController = RobotMap.armArmSpeedController;
    AnalogChannel potentiometer = RobotMap.armPotentiometer;
    DoubleSolenoid brakeArmSolenoid = RobotMap.armBrakeArmSolenoid;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    private BeachbotPrefSendablePIDController armPID;
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
        setDefaultCommand(new ManualArm());
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    public Arm(){
        armPID = new BeachbotPrefSendablePIDController(0,0,0,potentiometer, armSpeedController, "armPID");
        armPID.setAbsoluteTolerance(0.1);
        Preferences.getInstance().putDouble("ArmAbsoluteTolerance", 0.1);
    }
    
    public void holdArmOn() {
        brakeArmSolenoid.set(DoubleSolenoid.Value.kForward);
        armSpeedController.set(0);
    }
    
    public void holdArmOff() {
        brakeArmSolenoid.set(DoubleSolenoid.Value.kReverse);
    }
    
    public double getArmSetpoint() {
        if (!Preferences.getInstance().containsKey("ArmPositionSetpoint"))
        {
            Preferences.getInstance().putDouble("ArmPositionSetpoint", 1.0);
            Preferences.getInstance().save();
        }
        return Preferences.getInstance().getDouble("ArmPositionSetpoint", 1.0);
    }
    public double getArmZero()
    {
        if (!Preferences.getInstance().containsKey("ArmPositionZero"))
        {
            Preferences.getInstance().putDouble("ArmPositionZero", 0.0);
            Preferences.getInstance().save();
        }
        return Preferences.getInstance().getDouble("ArmPositionZero", 0.0);
    }
    
    public void manualArm() {
        armSpeedController.set(Robot.oi.operatorJoystick.getY());
    }
    
    public void armSetPoint(double setpoint) {
        armPID.setSetpoint(setpoint + getArmZero());
    }
    
    public void armSetPointHighShooting() {
        armSetPoint(Preferences.getInstance().
                getDouble("armSetpointHighShooting", 1));
    }
    
    public void armSetPointLowShooting() {
        armSetPoint(Preferences.getInstance().
                getDouble("armSetpointLowShooting", 1));
    }
    
    public void armSetPointClimbing() {
        armSetPoint(Preferences.getInstance().
                getDouble("armSetpointClimbing", 1));
    }
    
    public synchronized double getSetpoint() {
        return armPID.getSetpoint();
    }
    public synchronized boolean onTarget() {
        return armPID.onTarget();
    }
    public synchronized boolean isEnable() {
        return armPID.isEnable();
    }
    public synchronized void enable() {
        armPID.enable();
    }
    public synchronized void disable() {
        armPID.disable();
    }
}

@@ -723,16 +723,76 @@
 system;%0A
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;%0A
 import o
@@ -852,16 +852,16 @@
 roller;%0A
-
 import o
@@ -948,16 +948,36 @@
 bsystem 
+implements PIDSource
 %7B%0A    //
@@ -1882,29 +1882,20 @@
 r(0,0,0,
-potentiometer
+this
 , armSpe
@@ -2031,24 +2031,74 @@
 nce%22, 0.1);%0A
+        SmartDashboard.putData(%22ArmPID%22, armPID);%0A
     %7D%0A    %0A 
@@ -3110,32 +3110,147 @@
 ());%0A    %7D%0A    %0A
+    public double getArmPosition()%0A    %7B%0A        return potentiometer.getAverageVoltage()-getArmZero();%0A    %7D%0A    %0A
     public void 
@@ -4178,32 +4178,32 @@
 oid disable() %7B%0A
-
         armPID.d
@@ -4217,11 +4217,80 @@
 );%0A    %7D
+%0A%0A    public double pidGet() %7B%0A        return getArmPosition();%0A    %7D
 %0A%7D%0A

subtract zero and add get position