Can't seem to get the PIDtuned quite right.

Technical Java
1

I am trying to get a PID loop to hold our grabbers position up and down.
Sorry for the bad quality.
Videos: https://drive.google.com/open?id=1jhORFxEaqzu6b0ikCrTuPApjN1v4RxJh

Encoder grabberEncoder = new Encoder(6, 7, false, Encoder.EncodingType.k4X);
	double grabberHoldDistance;
	Spark PIDStore = new Spark(7);
	PIDController grabberPID = new PIDController(.0005, 0.000005, 0, grabberEncoder, grabberAngleController);
@Override
	public void robotInit() {
		grabberPID.setOutputRange(-0.2, 1);
		PIDStore.setDisabled();
		grabberAngleController.setInverted(true);
		// grabberPID.enable();
		rightGrabber.setInverted(true);
	}
//In teleop:
switch (grabberSwitch) {
		case 1:
			if (joy2.getRawButton(8))
				grabberSwitch++;
			if (joy2.getRawButton(3)) {
				grabberPID.disable();
				grabberAngleController.set(-grabberAngleDownSpeed);
				grabberPID.setSetpoint(grabberEncoder.getDistance());
			} else if (joy2.getRawButton(4)) {
				grabberPID.disable();
				grabberAngleController.set(grabberAngleUpSpeed);
				grabberPID.setSetpoint(grabberEncoder.getDistance());
			} else {
				if (grabberEncoder.get() <= 80) {
					grabberPID.disable();
					grabberAngleController.set(0);
				} else {
					grabberPID.enable();
					grabberAngleController.set(grabberPID.get());
				}
			}
			break;

		case 2:
			if(joy2.getRawButton(8))
				grabberSwitch--;
			grabberPID.setSetpoint(270);
			grabberPID.enable();
			grabberAngleController.set(grabberPID.get());
			break;
		}
2

Edit: I wrote the response below before reading your code, and the advice is still good, so I will leave it there. However, there is a glaring problem with your code - you are giving output the PIDController’s motor at the same time that you are enabling PID control. This cannot work; the PID controller works by controlling the output of the motor. This will almost certainly result in weird/unpredictable behavior.

For starters, don’t add anything other than proportional gain until the thing is working reasonably well. It is *much *harder to do a parameter sweep in multiple dimensions than in 1 dimensions.

Oscillatory behavior indicates that your gains are too high. If you find that you cannot make your gains high enough to effectively push your mechanism to the setpoint without oscillation, then you have several options. In code, you can add a feedforward, or mechanically, you can gear the motor down further or add springs to counterbalance the mechanism.

One reason it might seem like the mechanism goes crazy once it reaches a “certain point” is that the mechanics of the thing mean that the voltage required to hold it steady depends on the angle; when it is horizontal, much more voltage is required to hold a steady position than when it is vertical (if it is perfectly vertical, none is required at all) - to see this, simply draw a free-body diagram.

3